CN105785999A - 无人艇航向运动控制方法 - Google Patents

Abstract

本发明提供一种无人艇航向运动控制方法，包括：接收传感器模块采集的无人艇实时航向角，所述传感器模块包括：陀螺仪、加速度计以及磁场强度传感器；对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率；采用模糊PID控制算法根据所述航向偏差与所述航向偏差率确定指令舵角，所述指令舵角包括打舵方向和打舵速度；将所述指令舵角发送至电机驱动器；所述电机驱动器根据所述打舵方向和所述打舵速度控制无人艇运动。本发明实现了无人艇的航向控制，提高了无人艇航向运动控制的稳态性能、动态性性能和控制精度，减少了航向控制的调节时间。

Description

无人艇航向运动控制方法

技术领域

本发明实施例涉及无人艇运动控制技术领域，尤其涉及一种无人艇航向运动控制方法。

背景技术

PID控制器是一种常规设计，由于在设计中,将被控***的参数考虑为常数，它们只能在一定范围内有效，它们的最大缺点是闭环控制***不具有鲁棒性，实际船舶***常具有不确定性、非线性、非稳定性和复杂性，很难建立精确的模型方程，甚至不能直接进行分析和表示，因而就不能得到预期的控制效果。而人工操作者通过他们对所遇情况的处理经验和智能理解与解释，就能有效地控制船舶航行。因此，人们很自然地开始寻找类似于人工操作的智能控制方法。其中模糊控制算法面对复杂的和模型不清楚的***能进行简单有效的控制，但是简单的模糊控制器不具备积分环节，即当模糊控制器的输入e和e_c处于零或者零附近时，其输出也同样为零，因此在模糊控制的***中很难完全消除稳态误差，而且在变量分级不够多的情况下，常常在平衡点附近会有小的振荡现象。

发明内容

本发明实施例提供一种无人艇航向运动控制方法，以克服上述技术问题。

本发明无人艇航向运动控制方法，包括：

接收传感器模块采集的无人艇实时航向角，所述传感器模块包括：陀螺仪、加速度计以及磁场强度传感器；

对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率；

采用模糊PID控制算法根据所述航向偏差与所述航向偏差率确定指令舵角，所述指令舵角包括打舵方向和打舵速度；

将所述指令舵角发送至电机驱动器；

所述电机驱动器根据所述打舵方向和所述打舵速度控制无人艇运动。

进一步地，所述对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率之前，还包括：

接收陀螺仪检测加速度计三个轴的角速度，磁场强度传感器采集无人艇的航向角，加速度计采集无人艇的横摇角、纵摇角；

根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角；

将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角。

进一步地，所述根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角，包括：

根据姿态传感器模块中的陀螺仪采集的角速率偏差、角速率构造***的状态方程为

${\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_k=\left[\begin{array}{cc}1& -\mathrm{\Δ}t\\ 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_{k-1}+\left[\begin{array}{c}\mathrm{\Δ}t\\ 0\end{array}\right]{\mathrm{\ω}}_{k-1}+\left[\begin{array}{c}{w}_g\mathrm{\Δ}t\\ 0\end{array}\right]---\left(1\right)$

量测方程为

$Z\left(k\right)=\[\begin{array}{cc}1& 0\end{array}\]\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]+w_a---\left(2\right)$

其中，α表示姿态角，所述姿态角包括横摇角、纵摇角，β表示陀螺仪输出的角速率偏差，Δt表示采样周期，ω_k-1表示k-1(k＝1,2,…,n)时刻陀螺仪检测的角速率，w_g表示陀螺仪输出的白噪声，w_a为加速度计输出白噪声，所述Z(k)为加速度计的测量值；

根据状态方程和量测方程结合公式

X(k|k-1)＝AX(k-1|k-1)+BU(k-1)(3)

得到k-1时刻的最优姿态角，由k-1时刻的最优姿态角预测当前时刻的姿态角，其中,X(k-1|k-1)为k-1时刻的最优姿态角，X(k|k-1)为k时刻的初步估计姿态角，U(k)为当前时刻的控制量，所述A为***矩阵，所述B为控制输入矩阵；

根据公式

P(k|k-1)＝AP(k-1|k-1)A^T+Q(4)

计算预测误差的协方差，其中，P(k|k-1)为k时刻预测误差的协方差，P(k-1|k-1)为k-1时刻最优估计值的协方差，Q为***噪声协方差，A^T为***矩阵的转置矩阵；

根据所述预测误差的协方差，采用公式

K_g(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R]^-1(5)

计算卡尔曼增益，其中，所述Kg为卡尔曼滤波器增益，H为观测矩阵，R为测量噪声协方差矩阵，所述H^T为观测矩阵的转置矩阵；

根据所述卡尔曼增益，采用公式

X(k|k)＝X(k|k-1)+K_g(k)[Z(k)-HX(k|k-1)](6)

修正k时刻的最优姿态角，其中，所述Z(k)为加速度计的测量值。

进一步地，所述将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角，包括：

根据修正后的横摇角、纵摇角采用公式

$M^h=C_b^h{M}^b---\left(7\right)$

将所述航向角由传感器坐标系转换到水平坐标系内的磁场强度，其中，M^b为对应传感器坐标系的磁场强度，M^h为对应水平坐标系的磁场强度，为转换矩阵；

根据水平坐标系内的磁场强度的分量计算无人艇艇体平面的磁航向角；

根据所述磁航向角与磁偏角确定精确航向角。

进一步地，所述采用模糊PID控制算法根据所述航向偏差确定指令舵角，包括：

采用三角形函数

$f(x,a,b,c)=\left\{\begin{array}{cc}0,& x\≤a\\ \frac{x-a}{b-a},& a\≤x\≤b\\ \frac{c-x}{c-b},& b\≤x\≤c\\ 0,& c\≤x\end{array}\right.---\left(8\right)$

将航向偏差、航向偏差率的精确值进行模糊化处理，得到模糊输出集合，其中，x为航向偏差或航向偏差率，a,b,c指定三角形函数的形状，且要求a≤b≤c；

根据所述模糊输出集合确定模糊整定表，并根据所述模糊整定表整定所述模糊输出集合确定PID控制器的比例系数、积分系数以及微分系数；

采用加权平均法对所述比例系数、积分系数以及微分系数惊醒模糊判决，获得所述比例系数、积分系数以及微分系数的精确值；

将所述比例系数、积分系数以及微分系数的精确值输入所述PID控制器获取指令舵角。

进一步地，所述模糊整定表为：

其中，e为航向偏差，e_c为航向偏差率，NB、NM、NS、Z、PS、PM、PB、PM为模糊子集，k_p为比例系数；

其中，k_i为积分系数；

其中，k_d为微分系数。

本发明解决了船舶自动舵在航向控制中稳态性能和动态性能差，控制精度低、调节时间长、鲁棒性差的问题。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明无人艇航向运动控制方法流程图；

图2为本发明绘图工具存储的姿态数据示意图；

图3为本发明磁场强度传感器的相关角度及坐标关系示意图；

图4为本发明变量的模糊论域和隶属度函数示意图；

图5为本发明模糊PID的控制流程图；

图6为本发明模糊PID控制***原理图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明无人艇航向运动控制方法流程图，如图1所示，本实施例方法，包括：

步骤101、接收传感器模块采集的无人艇实时航向角，所述传感器模块包括：陀螺仪、加速度计以及磁场强度传感器；

步骤102、对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率；

步骤103、采用模糊PID控制算法根据所述航向偏差与所述航向偏差率确定指令舵角，所述指令舵角包括打舵方向和打舵速度；

步骤104、将所述指令舵角发送至电机驱动器；

步骤105、所述电机驱动器根据所述打舵方向和所述打舵速度控制无人艇运动。

进一步地，所述对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率之前，还包括：

接收陀螺仪检测加速度计三个轴的角速度，磁场强度传感器采集无人艇的航向角，加速度计采集无人艇的横摇角、纵摇角；

根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角；

将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角。

具体来说，无人艇的姿态角：横摇角、纵摇角都是通过姿态传感器GY-86在航行控制过程中准确并及时地获取，是对无人艇进行精确控制的关键。姿态传感器模块中的三轴加速度计和三轴陀螺仪都能够独自进行姿态角度的测量，但是这两种传感器的任意一个单独使用都不能保证测量的准确性，并且两种传感器各有不同的特点。首先我们对加速度计传感器进行分析，加速度计获取姿态角的原理概述为：加速度计能够检测到重力加速度在各个轴向的分量，当载体姿态发生改变时，加速度计三个轴向的检测值也会发生改变，根据各个轴向的检测值就可以计算得到姿态数据。其次，无人艇航行过程中的震动干扰会严重影响加速度计检测的准确性。同样对于陀螺仪而言，它能够检测到对应三个轴的角速度，然后同采样周期相乘就可以得到一个采样周期所转过的角度，但是通过这种方式获取的姿态角短时间内比较准确，长时间就会由于漂移的作用导致测量误差越来越大。为有效解决震动干扰和漂移带来的误差问题，我们采用卡尔曼(Kalman)滤波，对来自加速度计和陀螺仪的采集信号进行融合。卡尔曼滤波是一种高性能的递归滤波器，最大的特点是可以在一个不完全、甚至包含噪声的测量时间序列里估计出动态***的状态。卡尔曼滤波以最小均方差为估计的最佳准则，再结合一套递推估计的算法，进而实现状态的预测。卡尔曼滤波对数据存储量和运算量的要求较小，因此它适合实时处理和单片机运算。

进一步地，所述根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角，包括：

根据姿态传感器模块中的陀螺仪采集的角速率偏差、角速率构造***的状态方程为

${\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_k=\left[\begin{array}{cc}1& -\mathrm{\Δ}t\\ 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_{k-1}+\left[\begin{array}{c}\mathrm{\Δ}t\\ 0\end{array}\right]{\mathrm{\ω}}_{k-1}+\left[\begin{array}{c}{w}_g\mathrm{\Δ}t\\ 0\end{array}\right]---\left(1\right)$

量测方程为

$Z\left(k\right)=\[\begin{array}{cc}1& 0\end{array}\]\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]+w_a---\left(2\right)$

其中，α表示姿态角，所述姿态角包括横摇角、纵摇角，β表示陀螺仪输出的角速率偏差，Δt表示采样周期，ω_k-1表示k-1(k＝1,2,…,n)时刻陀螺仪检测的角速率，w_g表示陀螺仪输出的白噪声，w_a为加速度计输出白噪声，所述Z(k)为加速度计的测量值；

根据状态方程和量测方程结合公式

X(k|k-1)＝AX(k-1|k-1)+BU(k-1)(3)

得到k-1时刻的最优姿态角，由k-1时刻的最优姿态角预测当前时刻的姿态角，其中,X(k-1|k-1)为k-1时刻的最优姿态角，X(k|k-1)为k时刻的初步估计姿态角，U(k)为当前时刻的控制量，所述A为***矩阵，所述B为控制输入矩阵；

根据公式

P(k|k-1)＝AP(k-1|k-1)A^T+Q(4)

计算预测误差的协方差，其中，P(k|k-1)为k时刻预测误差的协方差，P(k-1|k-1)为k-1时刻最优估计值的协方差，Q为***噪声协方差，A^T为***矩阵的转置矩阵；

根据所述预测误差的协方差，采用公式

K_g(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R]^-1(5)

计算卡尔曼增益，其中，所述Kg为卡尔曼滤波器增益，H为观测矩阵，R为测量噪声协方差矩阵，所述H^T为观测矩阵的转置矩阵；

根据所述卡尔曼增益，采用公式

X(k|k)＝X(k|k-1)+K_g(k)[Z(k)-HX(k|k-1)](6)

修正k时刻的最优姿态角，其中，所述Z(k)为加速度计的测量值。

具体来说，本实施例通过陀螺仪的测量数据来预测姿态，然后通过加速度计的测量值进行修正。状态方程和量测方程为公式(1)和公式(2)，由状态方程和量测方程结合卡尔曼理论可以得到数值计算的迭代的过程。首先可以得到卡尔曼滤波的两个时间更新方程公式(3)和公式(4)。公式(3)由k-1时刻的最优估计值预测当前时刻的状态值，公式(4)根据k-1时刻的协方差计算得到预测误差的协方差。在完成时间更新之后还要实现状态更新，卡尔曼滤波的三个状态更新方程如公式(5)和公式(6)，其作用主要是获取卡尔曼增益，进而求取k时刻的最优估计值。根据

P(k|k)＝[I-K_gH]P(k|k-1)(7)

计算得到当前时刻最优估计值的协方差，为下一个时刻的迭代运算做准备。其中，I为1的矩阵，对于单模型单测量，I＝1。当***进入k+1状态时，P(k|k)就是式子7式的P(k-1|k-1)，该式作用为将k-1时刻的最优估计协方差更新为k时刻的最优估计协方差，进行卡尔曼滤波计算所涉及到的***变量初始值X(0)、预测误差协方差初始值P(0|0)、***噪声协方差Q、量测噪声协方差R取值如下所示：

$X\left(0\right)=\left[\begin{array}{c}0\\ 0\end{array}\right],P\left(0\right|0)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],Q=\left[\begin{array}{cc}0.001& 0\\ 0& 0.003\end{array}\right],R=0.69---\left(8\right)$

卡尔曼滤波是一种高性能的递归滤波器,最大的特点是可以在一个不完全、甚至包含噪声的测量时间序列里估计出动态***的状态。卡尔曼滤波相对于其他滤波如维纳Wiener滤波的优点，是不需要之前时刻对应的全部的测量值，卡尔曼滤波是根据前一个估计值和最近一个测量值来估计信号的当前值，它是用状态方程和递推估计算法进行估计的，因此卡尔曼滤波不要求信号的平稳性和时不变性。卡尔曼滤波以最小均方差为估计的最佳准则，再结合一套递推估计的算法，进而实现状态的预测。它的基本思路是：采用信号和噪声的状态空间模型，通过对前一时刻的估计值和当前时刻的测量值完成对状态变量估计的更新，得出当前时刻的估计值。用***的过程模型，来预测下一状态的***。假设现在的***状态是k，根据***的模型，可以基于***的上一状态而预测出现在状态。

为验证卡尔曼滤波的效果，针对加速度计的震动干扰和陀螺仪的漂移现象做了如下试验：将姿态传感器模块固定在无人艇上，打开无人艇发动机产生震动效果，使用微控制器STM32以20ms为周期通过IIC总线对加速度计和陀螺仪进行采样，然后通过卡尔曼滤波算法对两种传感器的姿态数据进行处理，再将处理前与处理后的姿态信息通过串口传送到上位机显示保存，最后采用Matlab绘图功能将存储的姿态数据如图2所示，由图可知卡尔曼滤波能够有效地消除加速度计的干扰和陀螺仪的零漂影响，得出相对准确的姿态信息。经过卡尔曼滤波方法的计算，可较好地修正横摇角、纵摇角。

进一步地，所述将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角，包括：

根据修正后的横摇角、纵摇角采用公式

$M^h=C_b^h{M}^b---\left(9\right)$

将所述航向角由传感器坐标系转换到水平坐标系内的磁场强度，其中，M^b为对应传感器坐标系的磁场强度，M^h为对应水平坐标系的磁场强度，为转换矩阵；

根据水平坐标系内的磁场强度的分量计算无人艇艇体平面的磁航向角；

根据所述磁航向角与磁偏角确定精确航向角。

具体来说，航向角主要是通过磁场强度传感器得出，在水平状态下，磁场强度传感器经过基于椭圆假设的罗差补偿之后就可以获得相对精确的航向值，当磁场强度传感器不处于水平状态时，就需要根据上述所求的横摇角和纵摇角与磁场强度传感器采集的数据进行融合，求取航向角。如图3所示，H_x、H_y、H_z分别表示磁场强度传感器本体坐标系下三轴的磁场强度，其中H_x指向载体的前进方向。H_X、H_Y、H_Z为任意姿态下H_x、H_y、H_z在水平坐标系下的投影。为纵摇角，θ为横摇角，NS为地理南北极的轴线，N'S'为磁南北极的轴线。

由图3所示，可知航向角的解析必须将三轴的磁场强度转换到水平坐标系下求得，并且满足，磁航向角angle+磁偏角β＝地理航向角，所以可先计算传感器数据得磁航向角，加上当地磁偏角，从而得到地理航向角。磁航向角angle的算法分以下两种情况：

第一种情况，当磁场强度传感器水平放置时，即传感器的坐标系与水平坐标系夹角为零，此时，H_Z轴的磁场矢量为零，则：

$angle=\arctan \frac{H_Y}{H_X}---\left(10\right)$

第二种情况，当磁场强度传感器和水平面存在夹角时，说明载体出现纵摇、横摇的情况，则须通过加速度传感器和陀螺仪传感器测得纵摇角和横摇角θ。然后将传感器坐标系测量的磁场信息转换到水平坐标系内，求解载体的磁航向角。由传感器坐标系到水平坐标系的转换矩阵为：

M^b为对应传感器坐标系的磁场强度，M^h为对应水平坐标系的磁场强度。坐标系的转换公式为：

公式(10)的分量形式为：

其中，M^b _x、M^b _Y、M^b _Z为磁场强度在传感器坐标系三轴上的分量，M^h _X、M^h _Y、M^h _Z为磁场强度在水平坐标系三轴上的分量。将上式的变量转换到坐标系中所对应的变量，即可得到磁感应强度在水平坐标系的分量：

从而可以计算出艇体平面的磁航向角angle为：

angle＝arctan(H_Y/H_X)(15)

求得磁航向角angle之后，查得当地磁偏角β，可得地理航向角α为

α＝angle+β(16)

航向角预处理后，克服了由于船体纵摇、横摇、磁场强度变化带来的航向角测量上的不准确性，通过数据融合，得出船体准确的航向角。

进一步地，所述采用模糊PID控制算法根据所述航向偏差确定指令舵角，包括：

采用三角形函数

$f(x,a,b,c)=\left\{\begin{array}{cc}0,& x\≤a\\ \frac{x-a}{b-a},& a\≤x\≤b\\ \frac{c-x}{c-b},& b\≤x\≤c\\ 0,& c\≤x\end{array}\right.---\left(17\right)$

将航向偏差、航向偏差率的精确值进行模糊化处理，得到模糊输出集合，其中，x为航向偏差或航向偏差率，a,b,c指定三角形函数的形状，且要求a≤b≤c；

根据所述模糊输出集合确定模糊整定表，并根据所述模糊整定表整定所述模糊输出集合确定PID控制器的比例系数、积分系数以及微分系数；

采用加权平均法对所述比例系数、积分系数以及微分系数惊醒模糊判决，获得所述比例系数、积分系数以及微分系数的精确值；

将所述比例系数、积分系数以及微分系数的精确值输入所述PID控制器获取指令舵角。

具体来说，对于把模糊技术和PID算法结合起来，常见的有如下两种：一是利用模糊控制器来给PID控制器在线自整定PID参数，组成模糊自整定参数PID控制器；另一种是采用不同的偏差阀值构建多模分段控制器，根据不同的条件和要求分段用不同模态进行控制，使不同的算法在不同的控制阶段充分发挥各自的优势，例如P-FUZZY-PI多模态分段控制器。本实施例采用第一种控制方式，如图5所示，在PID航向控制器的基础上，以航向偏差e和航向的偏差变化率e_c作为输入，采用模糊控制对PID参数K_p，K_i和K_d进行在线自整定，以满足不同的控制阶段对控制器参数的不同要求，从而使被控对象具有良好的动态和静态性能。

模糊PID控制***是由模糊控制器与常规PID控制器两部分构成的闭环控制***。r表示给定设定量，y表示的是***的输出量，e表示***误差，即设定值与实际输出值的差值，e_c表示***误差的变化率，E和E_c分别表示e和e_c经过模糊化处理之后所得到的模糊量，K_p、K_i和K_d代表模糊控制器输出的对应常规PID控制三个参数的调整值，u指的是常规PID控制器作用在被控对象上的输出量。

模糊糊PID控制***的控制流程如图6所示，首先通过***设定值和***输出值的对比得出***误差e，误差变化率e_c，然后进入模糊控制器将输入变量进行模糊化处理，其次，模糊化处理之后的输入量根据模糊整定表会得到一个模糊输出集合，再对这个模糊输出集合进行解模糊判决，得到一个精确输出值。其实质是在一定的输出范围内找到一个最合适的输出控制值。模糊控制器的最终输出变量分别为比例系数K_p、积分系数K_i和微分系数K_d的调整量。最后，利用应用模糊控制的输出量对PID控制器参数进行整定，其整定的计算式为：

$\left\{\begin{array}{c}{K}_p=K_{p0}+{\mathrm{\ΔK}}_p\\ K_i=K_{i0}+{\mathrm{\ΔK}}_i\\ K_d=K_{d0}+{\mathrm{\ΔK}}_d\end{array}\right.---\left(18\right)$

其中，K_p0，K_i0和K_d0为参数调整的基准值，ΔK_p，ΔK_i和ΔK_d是有模糊算法得到的整定量。通常模糊控制器有以下三部分组成：输入变量的模糊化及隶属度的选择、模糊控制整定表以及输出变量的解模糊。

在本实施例模糊PID控制器中有两个输入：航向误差e和航向误差变化率e_c，当控制结构确定以后由于在控制***中，输入量都是清晰值，为了使这些清晰值能够与语言表述的模糊整定表相适配，进行近似推理，必须将它们变换成模糊量，本实施例是将变量量化为7个等级，即{负大，负中，负小，零，正小，正中，正大}，简记为{NB，NM，NS，Z，PS，PM，PB}。

隶属函数是模糊集合应用于实际问题的基础，正确构造隶属函数是能否用好，模糊集合的关键，然而到目前为止，还没有一个成熟有效且统一的方法。在实际的应用中，确定隶属函数仍然依靠经验来解决，然后再通过实验或者计算机模拟得到的反馈信息进行修正。现在常用的隶属度函数包括Z函数，S函数，梯形函数，钟型函数，三角形函数以及高斯型函数等，综合考虑控制的灵敏度和运算速度，本实施例中各变量的隶属度函数均采用三角形函数公式(15)，该三角形函数的作用是将精确值转化为模糊值。所有变量的模糊论域和隶属度函数如图4所示：

进一步地，所述模糊整定表为：

其中，e为航向偏差，e_c为航向偏差率，NB、NM、NS、Z、PS、PM、PB、PM为模糊子集，k_p为比例系数；

其中，k_i为积分系数；

其中，k_d为微分系数。

具体来说，通过上述模糊整定表得到结果是一个模糊集合，但在实际应用中需要模糊控制最终输出一个确定的值。在模糊集合中取一个相对能够代表这个模糊集合的单值的过程被称作模糊决策。反模糊有多种方法，不同的方法所得到的结果也是不同的。理论上采用重心法最为合理，但是该方法计算比较复杂，所以在实时性要求较高的***中一般不采用这种方法。最简单的反模糊方法是最大隶属度法，这种方法取所有模糊集合或者隶属函数中隶属度最大的那个值作为输出，但是这种方法没有将隶属度较小的值考虑在内，代表性不好，所以只能用于较为简单的***。介于两者之间的还有各种平均方法，本实施例采用的加权平均法。其计算公式为：

$u^{*}=\frac{\underset{i}{\Σ}\mathrm{\μ}\left(u_i\right)\·u_i}{\underset{i}{\Σ}\mathrm{\μ}\left(u_i\right)}---\left(19\right)$

其中，μ(u_i)为解模糊输出的精确值，u_i是输出模糊论域的子集，μ(u_i)为输出子集所对应的隶属度。

在本实施例中将航向误差e的基本论域设定为[-45，45]，航向误差变化率e_c的基本论域设定为[-5°/s,5°/s]，将e、e_c、ΔK_p、ΔK_i、ΔK_d在模糊集上的论域设定为{-6，-4，-2，0，2，4，6}，所以量化因子分别为:K_e＝6/45＝0.133，K_ec＝6/5＝1.2。所有变量量化为7个等级，即{负大，负中，负小，零，正小，正中，正大}。

模糊控制最重要的部分就是比例系数K_p、积分系数K_i和微分系数K_d的模糊整定表。K_p、K_i和K_d的模糊整定表分别表1至表3。

当模糊整定表建立好后，根据K_p、K_i和K_d的模糊整定表内容确定模糊关系，计算反映控制量变化的模糊集合，然后采用加权平均法进行模糊判决，获得K_p、K_i和K_d调整量的精确值。

本发明提高了无人艇航向运动控制的稳态性能、动态性性能和控制精度，减少了航向控制的调节时间。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (6)

1.一种无人艇航向运动控制方法，其特征在于，包括：

接收传感器模块采集的无人艇实时航向角，所述传感器模块包括：陀螺仪、加速度计以及磁场强度传感器；

对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率；

采用模糊PID控制算法根据所述航向偏差与所述航向偏差率确定指令舵角，所述指令舵角包括打舵方向和打舵速度；

将所述指令舵角发送至电机驱动器；

所述电机驱动器根据所述打舵方向和所述打舵速度控制无人艇运动。

2.根据权利要求1所述的方法，其特征在于，所述对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率之前，还包括：

接收陀螺仪检测加速度计三个轴的角速度，磁场强度传感器采集无人艇的航向角，加速度计采集无人艇的横摇角、纵摇角；

根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角；

将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角。

3.根据权利要求2所述的方法，其特征在于，所述根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角，包括：

根据姿态传感器模块中的陀螺仪采集的角速率偏差、角速率构造***的状态方程为

${\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_k=\left[\begin{array}{cc}1& -\mathrm{\Δ}t\\ 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]}_{k-1}+\left[\begin{array}{c}\mathrm{\Δ}t\\ 0\end{array}\right]{\mathrm{\ω}}_{k-1}+\left[\begin{array}{c}{w}_g\mathrm{\Δ}t\\ 0\end{array}\right]---\left(1\right)$

量测方程为

$Z\left(k\right)=\[\begin{array}{cc}1& 0\end{array}\]\left[\begin{array}{c}\mathrm{\α}\\ \mathrm{\β}\end{array}\right]+w_a---\left(2\right)$

其中，α表示姿态角，所述姿态角包括横摇角、纵摇角，β表示陀螺仪输出的角速率偏差，Δt表示采样周期，ω_k-1表示k-1(k＝1,2,…,n)时刻陀螺仪检测的角速率，w_g表示陀螺仪输出的白噪声，w_a为加速度计输出白噪声，所述Z(k)为加速度计的测量值；

根据状态方程和量测方程结合公式

X(k|k-1)＝AX(k-1|k-1)+BU(k-1)(3)

得到k-1时刻的最优姿态角，由k-1时刻的最优姿态角预测当前时刻的姿态角，其中,X(k-1|k-1)为k-1时刻的最优姿态角，X(k|k-1)为k时刻的初步估计姿态角，U(k)为当前时刻的控制量，所述A为***矩阵，所述B为控制输入矩阵；

根据公式

P(k|k-1)＝AP(k-1|k-1)A^T+Q(4)

计算预测误差的协方差，其中，P(k|k-1)为k时刻预测误差的协方差，P(k-1|k-1)为k-1时刻最优估计值的协方差，Q为***噪声协方差，A^T为***矩阵的转置矩阵；

根据所述预测误差的协方差，采用公式

K_g(k)＝P(k|k-1)H^T[HP(k|k-1)H^T+R]^-1(5)

计算卡尔曼增益，其中，所述Kg为卡尔曼滤波器增益，H为观测矩阵，R为测量噪声协方差矩阵，所述H^T为观测矩阵的转置矩阵；

根据所述卡尔曼增益，采用公式

X(k|k)＝X(k|k-1)+K_g(k)[Z(k)-HX(k|k-1)](6)

修正k时刻的最优姿态角，其中，所述Z(k)为加速度计的测量值。

4.根据权利要求3所述的方法，其特征在于，所述将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角，包括：

根据修正后的横摇角、纵摇角采用公式

$M^h=C_b^h{M}^b---\left(7\right)$

将所述航向角由传感器坐标系转换到水平坐标系内的磁场强度，其中，M^b为对应传感器坐标系的磁场强度，M^h为对应水平坐标系的磁场强度，为转换矩阵；

根据水平坐标系内的磁场强度的分量计算无人艇艇体平面的磁航向角；

根据所述磁航向角与磁偏角确定精确航向角。

5.根据权利要求1所述的方法，其特征在于，所述采用模糊PID控制算法根据所述航向偏差确定指令舵角，包括：

采用三角形函数

$f(x,a,b,c)=\left\{\begin{array}{c}0,x\≤a\\ \frac{x-a}{b-a},a\≤x\≤b\\ \frac{c-x}{c-b},b\≤x\≤c\\ 0,c\≤x\end{array}\right.---\left(8\right)$

将航向偏差、航向偏差率的精确值进行模糊化处理，得到模糊输出集合，其中，x为航向偏差或航向偏差率，a,b,c指定三角形函数的形状，且要求a≤b≤c；

根据所述模糊输出集合确定模糊整定表，并根据所述模糊整定表整定所述模糊输出集合确定PID控制器的比例系数、积分系数以及微分系数；

采用加权平均法对所述比例系数、积分系数以及微分系数惊醒模糊判决，获得所述比例系数、积分系数以及微分系数的精确值；

将所述比例系数、积分系数以及微分系数的精确值输入所述PID控制器获取指令舵角。

6.根据权利要求5所述的方法，其特征在于，所述模糊整定表为：

其中，e为航向偏差，e_c为航向偏差率，NB、NM、NS、Z、PS、PM、PB、PM为模糊子集，k_p为比例系数；

其中，k_i为积分系数；

其中，k_d为微分系数。