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

摘要

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

主权项

1.一种无人艇航向运动控制方法，其特征在于，包括：接收传感器模块采集的无人艇实时航向角，所述传感器模块包括：陀螺仪、加速度计以及磁场强度传感器；对比所述实时航向角与设定航向角得到无人艇航向偏差、航向偏差率；采用模糊PID控制算法根据所述航向偏差与所述航向偏差率确定指令舵角，所述指令舵角包括打舵方向和打舵速度；将所述指令舵角发送至电机驱动器；所述电机驱动器根据所述打舵方向和所述打舵速度控制无人艇运动；所述对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率之前，还包括：接收陀螺仪检测加速度计三个轴的角速度，磁场强度传感器采集无人艇的航向角，加速度计采集无人艇的横摇角、纵摇角；根据所述角速度采用卡尔曼滤波修正所述加速度计采集的无人艇的横摇角、纵摇角；所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角。所述根据所述角速度采用卡尔曼滤波修正所述加速度计采集的无人艇的横摇角、纵摇角，包括：根据传感器模块中的陀螺仪采集的角速度偏差、角速度构造系统的状态方程为量测方程为其中，α表示姿态角，所述姿态角包括横摇角、纵摇角，β表示陀螺仪输出的角速度偏差，Δ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(k)为卡尔曼滤波器增益，H为观测矩阵，R为测量噪声协方差矩阵，所述HT为观测矩阵的转置矩阵；根据所述卡尔曼增益，采用公式X(k|k)＝X(k|k‑1)+Kg(k)[Z(k)‑HX(k|k‑1)]  (6)修正k时刻的最优姿态角，其中，所述Z(k)为加速度计的测量值。