技术领域本发明实施例涉及无人艇运动控制技术领域，尤其涉及一种无人艇航向运动控制方法。
背景技术：
PID控制器是一种常规设计，由于在设计中,将被控系统的参数考虑为常数，它们只能在一定范围内有效，它们的最大缺点是闭环控制系统不具有鲁棒性，实际船舶系统常具有不确定性、非线性、非稳定性和复杂性，很难建立精确的模型方程，甚至不能直接进行分析和表示，因而就不能得到预期的控制效果。而人工操作者通过他们对所遇情况的处理经验和智能理解与解释，就能有效地控制船舶航行。因此，人们很自然地开始寻找类似于人工操作的智能控制方法。其中模糊控制算法面对复杂的和模型不清楚的系统能进行简单有效的控制，但是简单的模糊控制器不具备积分环节，即当模糊控制器的输入e和ec处于零或者零附近时，其输出也同样为零，因此在模糊控制的系统中很难完全消除稳态误差，而且在变量分级不够多的情况下，常常在平衡点附近会有小的振荡现象。
技术实现要素：
本发明实施例提供一种无人艇航向运动控制方法，以克服上述技术问题。本发明无人艇航向运动控制方法，包括：接收传感器模块采集的无人艇实时航向角，所述传感器模块包括：陀螺仪、加速度计以及磁场强度传感器；对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率；采用模糊PID控制算法根据所述航向偏差与所述航向偏差率确定指令舵角，所述指令舵角包括打舵方向和打舵速度；将所述指令舵角发送至电机驱动器；所述电机驱动器根据所述打舵方向和所述打舵速度控制无人艇运动。进一步地，所述对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率之前，还包括：接收陀螺仪检测加速度计三个轴的角速度，磁场强度传感器采集无人艇的航向角，加速度计采集无人艇的横摇角、纵摇角；根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角；将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角。进一步地，所述根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角，包括：根据姿态传感器模块中的陀螺仪采集的角速率偏差、角速率构造系统的状态方程为αβk=1-Δt01αβk-1+Δt0ωk-1+wgΔt0---(1)]]>量测方程为Z(k)=[10]αβ+wa---(2)]]>其中，α表示姿态角，所述姿态角包括横摇角、纵摇角，β表示陀螺仪输出的角速率偏差，Δt表示采样周期，ωk-1表示k-1(k＝1,2,…,n)时刻陀螺仪检测的角速率，wg表示陀螺仪输出的白噪声，wa为加速度计输出白噪声，所述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)AT+Q(4)计算预测误差的协方差，其中，P(k|k-1)为k时刻预测误差的协方差，P(k-1|k-1)为k-1时刻最优估计值的协方差，Q为系统噪声协方差，AT为系统矩阵的转置矩阵；根据所述预测误差的协方差，采用公式Kg(k)＝P(k|k-1)HT[HP(k|k-1)HT+R]-1(5)计算卡尔曼增益，其中，所述Kg为卡尔曼滤波器增益，H为观测矩阵，R为测量噪声协方差矩阵，所述HT为观测矩阵的转置矩阵；根据所述卡尔曼增益，采用公式X(k|k)＝X(k|k-1)+Kg(k)[Z(k)-HX(k|k-1)](6)修正k时刻的最优姿态角，其中，所述Z(k)为加速度计的测量值。进一步地，所述将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角，包括：根据修正后的横摇角、纵摇角采用公式Mh=CbhMb---(7)]]>将所述航向角由传感器坐标系转换到水平坐标系内的磁场强度，其中，Mb为对应传感器坐标系的磁场强度，Mh为对应水平坐标系的磁场强度，为转换矩阵；根据水平坐标系内的磁场强度的分量计算无人艇艇体平面的磁航向角；根据所述磁航向角与磁偏角确定精确航向角。进一步地，所述采用模糊PID控制算法根据所述航向偏差确定指令舵角，包括：采用三角形函数f(x,a,b,c)=0,x≤ax-ab-a,a≤x≤bc-xc-b,b≤x≤c0,c≤x---(8)]]>将航向偏差、航向偏差率的精确值进行模糊化处理，得到模糊输出集合，其中，x为航向偏差或航向偏差率，a,b,c指定三角形函数的形状，且要求a≤b≤c；根据所述模糊输出集合确定模糊整定表，并根据所述模糊整定表整定所述模糊输出集合确定PID控制器的比例系数、积分系数以及微分系数；采用加权平均法对所述比例系数、积分系数以及微分系数惊醒模糊判决，获得所述比例系数、积分系数以及微分系数的精确值；将所述比例系数、积分系数以及微分系数的精确值输入所述PID控制器获取指令舵角。进一步地，所述模糊整定表为：其中，e为航向偏差，ec为航向偏差率，NB、NM、NS、Z、PS、PM、PB、PM为模糊子集，kp为比例系数；其中，ki为积分系数；其中，kd为微分系数。本发明解决了船舶自动舵在航向控制中稳态性能和动态性能差，控制精度低、调节时间长、鲁棒性差的问题。附图说明为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。图1为本发明无人艇航向运动控制方法流程图；图2为本发明绘图工具存储的姿态数据示意图；图3为本发明磁场强度传感器的相关角度及坐标关系示意图；图4为本发明变量的模糊论域和隶属度函数示意图；图5为本发明模糊PID的控制流程图；图6为本发明模糊PID控制系统原理图。具体实施方式为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。图1为本发明无人艇航向运动控制方法流程图，如图1所示，本实施例方法，包括：步骤101、接收传感器模块采集的无人艇实时航向角，所述传感器模块包括：陀螺仪、加速度计以及磁场强度传感器；步骤102、对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率；步骤103、采用模糊PID控制算法根据所述航向偏差与所述航向偏差率确定指令舵角，所述指令舵角包括打舵方向和打舵速度；步骤104、将所述指令舵角发送至电机驱动器；步骤105、所述电机驱动器根据所述打舵方向和所述打舵速度控制无人艇运动。进一步地，所述对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率之前，还包括：接收陀螺仪检测加速度计三个轴的角速度，磁场强度传感器采集无人艇的航向角，加速度计采集无人艇的横摇角、纵摇角；根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角；将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角。具体来说，无人艇的姿态角：横摇角、纵摇角都是通过姿态传感器GY-86在航行控制过程中准确并及时地获取，是对无人艇进行精确控制的关键。姿态传感器模块中的三轴加速度计和三轴陀螺仪都能够独自进行姿态角度的测量，但是这两种传感器的任意一个单独使用都不能保证测量的准确性，并且两种传感器各有不同的特点。首先我们对加速度计传感器进行分析，加速度计获取姿态角的原理概述为：加速度计能够检测到重力加速度在各个轴向的分量，当载体姿态发生改变时，加速度计三个轴向的检测值也会发生改变，根据各个轴向的检测值就可以计算得到姿态数据。其次，无人艇航行过程中的震动干扰会严重影响加速度计检测的准确性。同样对于陀螺仪而言，它能够检测到对应三个轴的角速度，然后同采样周期相乘就可以得到一个采样周期所转过的角度，但是通过这种方式获取的姿态角短时间内比较准确，长时间就会由于漂移的作用导致测量误差越来越大。为有效解决震动干扰和漂移带来的误差问题，我们采用卡尔曼(Kalman)滤波，对来自加速度计和陀螺仪的采集信号进行融合。卡尔曼滤波是一种高性能的递归滤波器，最大的特点是可以在一个不完全、甚至包含噪声的测量时间序列里估计出动态系统的状态。卡尔曼滤波以最小均方差为估计的最佳准则，再结合一套递推估计的算法，进而实现状态的预测。卡尔曼滤波对数据存储量和运算量的要求较小，因此它适合实时处理和单片机运算。进一步地，所述根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角，包括：根据姿态传感器模块中的陀螺仪采集的角速率偏差、角速率构造系统的状态方程为αβk=1-Δt01αβk-1+Δt0ωk-1+wgΔt0---(1)]]>量测方程为Z(k)=[10]αβ+wa---(2)]]>其中，α表示姿态角，所述姿态角包括横摇角、纵摇角，β表示陀螺仪输出的角速率偏差，Δt表示采样周期，ωk-1表示k-1(k＝1,2,…,n)时刻陀螺仪检测的角速率，wg表示陀螺仪输出的白噪声，wa为加速度计输出白噪声，所述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)AT+Q(4)计算预测误差的协方差，其中，P(k|k-1)为k时刻预测误差的协方差，P(k-1|k-1)为k-1时刻最优估计值的协方差，Q为系统噪声协方差，AT为系统矩阵的转置矩阵；根据所述预测误差的协方差，采用公式Kg(k)＝P(k|k-1)HT[HP(k|k-1)HT+R]-1(5)计算卡尔曼增益，其中，所述Kg为卡尔曼滤波器增益，H为观测矩阵，R为测量噪声协方差矩阵，所述HT为观测矩阵的转置矩阵；根据所述卡尔曼增益，采用公式X(k|k)＝X(k|k-1)+Kg(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-KgH]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(0)=00,P(0|0)=1001,Q=0.001000.003,R=0.69---(8)]]>卡尔曼滤波是一种高性能的递归滤波器,最大的特点是可以在一个不完全、甚至包含噪声的测量时间序列里估计出动态系统的状态。卡尔曼滤波相对于其他滤波如维纳Wiener滤波的优点，是不需要之前时刻对应的全部的测量值，卡尔曼滤波是根据前一个估计值和最近一个测量值来估计信号的当前值，它是用状态方程和递推估计算法进行估计的，因此卡尔曼滤波不要求信号的平稳性和时不变性。卡尔曼滤波以最小均方差为估计的最佳准则，再结合一套递推估计的算法，进而实现状态的预测。它的基本思路是：采用信号和噪声的状态空间模型，通过对前一时刻的估计值和当前时刻的测量值完成对状态变量估计的更新，得出当前时刻的估计值。用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态。为验证卡尔曼滤波的效果，针对加速度计的震动干扰和陀螺仪的漂移现象做了如下试验：将姿态传感器模块固定在无人艇上，打开无人艇发动机产生震动效果，使用微控制器STM32以20ms为周期通过IIC总线对加速度计和陀螺仪进行采样，然后通过卡尔曼滤波算法对两种传感器的姿态数据进行处理，再将处理前与处理后的姿态信息通过串口传送到上位机显示保存，最后采用Matlab绘图功能将存储的姿态数据如图2所示，由图可知卡尔曼滤波能够有效地消除加速度计的干扰和陀螺仪的零漂影响，得出相对准确的姿态信息。经过卡尔曼滤波方法的计算，可较好地修正横摇角、纵摇角。进一步地，所述将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角，包括：根据修正后的横摇角、纵摇角采用公式Mh=CbhMb---(9)]]>将所述航向角由传感器坐标系转换到水平坐标系内的磁场强度，其中，Mb为对应传感器坐标系的磁场强度，Mh为对应水平坐标系的磁场强度，为转换矩阵；根据水平坐标系内的磁场强度的分量计算无人艇艇体平面的磁航向角；根据所述磁航向角与磁偏角确定精确航向角。具体来说，航向角主要是通过磁场强度传感器得出，在水平状态下，磁场强度传感器经过基于椭圆假设的罗差补偿之后就可以获得相对精确的航向值，当磁场强度传感器不处于水平状态时，就需要根据上述所求的横摇角和纵摇角与磁场强度传感器采集的数据进行融合，求取航向角。如图3所示，Hx、Hy、Hz分别表示磁场强度传感器本体坐标系下三轴的磁场强度，其中Hx指向载体的前进方向。HX、HY、HZ为任意姿态下Hx、Hy、Hz在水平坐标系下的投影。为纵摇角，θ为横摇角，NS为地理南北极的轴线，N'S'为磁南北极的轴线。由图3所示，可知航向角的解析必须将三轴的磁场强度转换到水平坐标系下求得，并且满足，磁航向角angle+磁偏角β＝地理航向角，所以可先计算传感器数据得磁航向角，加上当地磁偏角，从而得到地理航向角。磁航向角angle的算法分以下两种情况：第一种情况，当磁场强度传感器水平放置时，即传感器的坐标系与水平坐标系夹角为零，此时，HZ轴的磁场矢量为零，则：angle=arctanHYHX---(10)]]>第二种情况，当磁场强度传感器和水平面存在夹角时，说明载体出现纵摇、横摇的情况，则须通过加速度传感器和陀螺仪传感器测得纵摇角和横摇角θ。然后将传感器坐标系测量的磁场信息转换到水平坐标系内，求解载体的磁航向角。由传感器坐标系到水平坐标系的转换矩阵为：Mb为对应传感器坐标系的磁场强度，Mh为对应水平坐标系的磁场强度。坐标系的转换公式为：公式(10)的分量形式为：其中，Mbx、MbY、MbZ为磁场强度在传感器坐标系三轴上的分量，MhX、MhY、MhZ为磁场强度在水平坐标系三轴上的分量。将上式的变量转换到坐标系中所对应的变量，即可得到磁感应强度在水平坐标系的分量：从而可以计算出艇体平面的磁航向角angle为：angle＝arctan(HY/HX)(15)求得磁航向角angle之后，查得当地磁偏角β，可得地理航向角α为α＝angle+β(16)航向角预处理后，克服了由于船体纵摇、横摇、磁场强度变化带来的航向角测量上的不准确性，通过数据融合，得出船体准确的航向角。进一步地，所述采用模糊PID控制算法根据所述航向偏差确定指令舵角，包括：采用三角形函数f(x,a,b,c)=0,x≤ax-ab-a,a≤x≤bc-xc-b,b≤x≤c0,c≤x---(17)]]>将航向偏差、航向偏差率的精确值进行模糊化处理，得到模糊输出集合，其中，x为航向偏差或航向偏差率，a,b,c指定三角形函数的形状，且要求a≤b≤c；根据所述模糊输出集合确定模糊整定表，并根据所述模糊整定表整定所述模糊输出集合确定PID控制器的比例系数、积分系数以及微分系数；采用加权平均法对所述比例系数、积分系数以及微分系数惊醒模糊判决，获得所述比例系数、积分系数以及微分系数的精确值；将所述比例系数、积分系数以及微分系数的精确值输入所述PID控制器获取指令舵角。具体来说，对于把模糊技术和PID算法结合起来，常见的有如下两种：一是利用模糊控制器来给PID控制器在线自整定PID参数，组成模糊自整定参数PID控制器；另一种是采用不同的偏差阀值构建多模分段控制器，根据不同的条件和要求分段用不同模态进行控制，使不同的算法在不同的控制阶段充分发挥各自的优势，例如P-FUZZY-PI多模态分段控制器。本实施例采用第一种控制方式，如图5所示，在PID航向控制器的基础上，以航向偏差e和航向的偏差变化率ec作为输入，采用模糊控制对PID参数Kp，Ki和Kd进行在线自整定，以满足不同的控制阶段对控制器参数的不同要求，从而使被控对象具有良好的动态和静态性能。模糊PID控制系统是由模糊控制器与常规PID控制器两部分构成的闭环控制系统。r表示给定设定量，y表示的是系统的输出量，e表示系统误差，即设定值与实际输出值的差值，ec表示系统误差的变化率，E和Ec分别表示e和ec经过模糊化处理之后所得到的模糊量，Kp、Ki和Kd代表模糊控制器输出的对应常规PID控制三个参数的调整值，u指的是常规PID控制器作用在被控对象上的输出量。模糊糊PID控制系统的控制流程如图6所示，首先通过系统设定值和系统输出值的对比得出系统误差e，误差变化率ec，然后进入模糊控制器将输入变量进行模糊化处理，其次，模糊化处理之后的输入量根据模糊整定表会得到一个模糊输出集合，再对这个模糊输出集合进行解模糊判决，得到一个精确输出值。其实质是在一定的输出范围内找到一个最合适的输出控制值。模糊控制器的最终输出变量分别为比例系数Kp、积分系数Ki和微分系数Kd的调整量。最后，利用应用模糊控制的输出量对PID控制器参数进行整定，其整定的计算式为：Kp=Kp0+ΔKpKi=Ki0+ΔKiKd=Kd0+ΔKd---(18)]]>其中，Kp0，Ki0和Kd0为参数调整的基准值，ΔKp，ΔKi和ΔKd是有模糊算法得到的整定量。通常模糊控制器有以下三部分组成：输入变量的模糊化及隶属度的选择、模糊控制整定表以及输出变量的解模糊。在本实施例模糊PID控制器中有两个输入：航向误差e和航向误差变化率ec，当控制结构确定以后由于在控制系统中，输入量都是清晰值，为了使这些清晰值能够与语言表述的模糊整定表相适配，进行近似推理，必须将它们变换成模糊量，本实施例是将变量量化为7个等级，即{负大，负中，负小，零，正小，正中，正大