[发明专利]一种基于智能预测控制的水面无人船路径跟踪方法

权利要求书

1.一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，具体的实现步骤为：

步骤1.下位机接收北斗、惯性导航传感器的数据，并进行解析，得到无人船的当前经纬度坐标、航速、艏向角等信息，设置阈值增量约束改进互补滤波算法，对艏向角进行滤波处理；将滤波后的数据将进行转化，得到北东地坐标系下的状态信息；

步骤2.按照无人船作业任务预先规划出航行路径点，然后连接各个航行路径点形成一条期望路径曲线作为跟踪的路径，根据无人船初始状态，计算出每个路径点的自动接纳圆半径；

步骤3.根据状态信息进行航向规划并利用自适应视线法解算出期望航向角；

步骤4.进行路径段的更新，在当前路径点时，若无人船在自适应的接纳圆内，则跟踪下一路径点；

步骤5.在航速给定的情况下，将当前艏向角与期望航向角的误差作为模型预测控制器的输入，设计基于模型预测控制的路径跟踪控制器，通过构建预测模型，以滚动优化确定当前的最优控制策略，使未来一段时间内被控变量与期望值偏差最小，再进行反馈校正，解算出对应推进器的转速指令来调整动作，来缩小并消除误差；

步骤6.本发明引入人工鱼群算法，把目标函数倒数作为人工鱼群算法的适应度函数，并对该算法进行改进，在觅食阶段，将视野范围扩大后，在新的视野范围内，再次进行搜索，寻找适应度高的人工鱼，并向该方向前进一步；

步骤7.通过改进的人工鱼群算法解算出最优控制增量序列中的第一个控制增量分量，将其作用到无人船，在下一时刻，根据导航传感器的实时状态，判断无人船是否到达终点，如果没有到达，将其信息与下一时刻期望路径点的状态信息的差值作为模型预测控制器的输入，形成反馈，构成闭环控制。

2.根据权利要求1所述的一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，所述步骤1对艏向角的融合滤波算法具体步骤为：

步骤1.1：下位机通过RS232通信协议接收北斗、惯性导航传感器的数据，并进行解析；

步骤1.2：初始化姿态参数；

步骤1.3：获取加速度计实测值，并进行归一化处理，转换成单位向量；

步骤1.4：利用陀螺仪更新方向余弦矩阵：

式中，为加速度分量，为四元素分量；

步骤1.5：将标准重力加速度向量与加速度测量值向量进行叉乘，得到如下的误差向量：

式中，为误差分量，为角速度分量，为加速度分量；

步骤1.6：通过PID控制器对误差进行消除，得到当前时刻无人船艏向角的滤波值，控制器公式如下：

式中，e_z(t)为艏向角误差，Δe_z(t)为艏向角补偿量，k_p为比例系数，T_i为积分系数，T_d为微分系数；

步骤1.7：采用一阶龙格库塔法进行四元素更新：

式中，为四元素分量，为角速度分量，T为采样时间；

步骤1.8：保留上一时刻的滤波值；

步骤1.9：本发明对互补滤波算法进行了改进：设置阈值增量约束将当前时刻和上一时刻滤波差值与阈值增量约束进行比较，若满足则输出结果，否则重新执行，判定公式如下：

|x_now-x_last|≤β (5)

式中，x_now为当前滤波值，x_last为上一时刻滤波值，β为阈值增量约束,选取范围为[0,1]；

步骤1.10：将机体坐标系下的当前状态信息进行转化处理，得到北东地坐标系下的状态信息。