[发明专利]无人艇航向运动控制方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

量测方程为

其中，α表示姿态角，所述姿态角包括横摇角、纵摇角，β表示陀螺仪输出的角速度偏差，Δ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)

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

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

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

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

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

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

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

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

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