基于二阶波浪扰动预测的无人船路径跟踪控制方法及系统

本发明属于无人船运动控制、海洋环境扰动补偿和非线性模型预测控制，尤其涉及基于二阶波浪扰动预测的无人船路径跟踪控制方法及系统。

背景技术：

1、无人船在海洋测绘、巡逻警戒、环境监测和自主作业中，通常需要沿预设路线航行，这个过程中会持续受到风、浪、流等环境载荷影响。其中，波浪引起的低频二阶波浪漂移力和漂移力矩会持续作用于船体水平面运动，使无人船的纵荡速度、横荡速度和艏摇角速度发生变化，进而造成横向偏离、航向滞后和控制输入频繁调整等问题。

2、对于平面运动控制，通常采用纵荡、横荡和艏摇三自由度模型描述无人船状态，其中纵荡速度记为u，横荡速度记为v，艏摇角速度记为r。对于欠驱动无人船，一般仅能通过推进器产生纵荡推力、通过舵或差速推进产生艏摇力矩，而不能直接产生独立的横荡控制力。因此，横荡方向的波浪扰动往往首先表现为侧漂和路径横向误差，不能简单地按施加一个反向横荡力进行直接补偿。

3、现有无人船路径跟踪系统通常包括导引层和控制层。导引层根据当前位置与参考路径之间的几何关系生成期望艏向，常采用视线导引（los）或扩展视线导引（elos）方法；控制层根据期望航速、期望艏向以及实测状态输出推进和转艏控制指令，常采用比例积分微分控制、反步控制、滑模控制或模型预测控制。对于侧漂和外界扰动，现有技术还常设置侧滑观测器、扰动观测器或扩张状态观测器，根据已发生的状态误差估计未知扰动，再将估计结果用于控制补偿。

4、现有技术已经分别公开了二阶波浪漂移力建模、侧滑补偿导引、扰动观测、波浪预测和模型预测控制等技术手段。但是，针对控制器已获得与控制时刻对齐的纵荡、横荡和艏摇三自由度二阶波浪扰动预测序列的情形，现有方案仍存在以下问题。

5、现有技术的主要不足不在于缺少波浪建模、扰动观测或预测控制模块，而在于缺少针对已获得三自由度二阶波浪扰动预测序列的明确控制器级处理机制。具体而言，未能解决以下问题：如何将横荡方向预测扰动转换为适合欠驱动无人船的侧漂导引先验；如何在已利用横荡扰动先验后，使eso只估计剩余扰动而非重复估计已知扰动；如何将纵荡和艏摇方向预测扰动按预测步稳定输入nmpc预测模型；如何使导引修正、残余扰动估计和nmpc滚动优化在时间索引、坐标方向和控制作用上保持一致；如何在不重新构建波浪预测器的前提下，直接利用上游模块已经提供的预测序列，输出满足执行器约束的纵荡推力和艏摇力矩控制指令。

技术实现思路

1、为克服相关技术中存在的问题，本发明提供了基于二阶波浪扰动预测的无人船路径跟踪控制方法及系统，尤其涉及一种将预生成的三自由度二阶波浪扰动预测序列，按照其在欠驱动无人船上的物理作用方向，分别用于横荡先验导引和纵荡—艏摇预测控制的方法。本发明目的在于解决现有技术中波浪预测信息、侧漂导引、扰动观测和预测控制之间作用重复或时序错位的问题，从而输出适用于欠驱动无人船的纵荡推力和艏摇力矩路径跟踪控制指令。该方法不涉及二阶波浪扰动的反演或在线预测，而是在控制器已获得与控制时刻对齐的纵荡、横荡和艏摇三自由度二阶波浪扰动预测序列后，对不同方向的扰动信息进行差异化利用。其中，将横荡方向预测扰动转换为侧漂先验，用于修正扩展视线导引的期望艏向，并由扩张状态观测器估计先验修正后仍存在的残余扰动；将纵荡和艏摇方向预测扰动按预测步输入非线性模型预测控制器，在推力、艏摇力矩及其变化率约束下滚动求解控制指令。

2、为实现上述目的，本发明采用如下技术方案：基于二阶波浪扰动预测的无人船路径跟踪控制方法，包括以下步骤：

3、s1、获取当前控制时刻的无人船航行状态、参考路径、期望航速以及外部提供的三自由度二阶波浪扰动预测序列；所述三自由度二阶波浪扰动预测序列包括纵荡预测力、横荡预测力和艏摇预测力矩，且各预测样本与当前控制时刻及预测控制器的预测步对应；

4、s2、基于所述横荡预测力获得横荡有效预测扰动，并转换为有界的侧漂先验补偿角；

5、s3、根据所述无人船航行状态和所述参考路径，求解基础视线期望艏向；在残余扩张状态观测器的名义横向误差动态中，引入由所述侧漂先验补偿角确定的横向速度投影项，得到残余侧漂补偿角；通过所述侧漂先验补偿角和所述残余侧漂补偿角修正所述基础视线期望艏向，得到修正期望艏向；

6、s4、针对非线性模型预测控制器的每一预测步，按所述预测步读取所述纵荡预测力和所述艏摇预测力矩对应的预测样本，获得经预测域局部平滑和通道标定的预测模型外生扰动向量；所述预测模型外生扰动向量的横荡分量固定为零；

7、s5、将所述修正期望艏向、无人船航行状态和所述预测模型外生扰动向量输入保留横荡速度状态递推的三自由度非线性模型预测控制器nmpc，在纵荡推力、艏摇力矩及变化率约束下滚动求解控制序列；以及执行所述控制序列的当前时刻纵荡推力和艏摇力矩，并在下一采样时刻更新所述无人船航行状态和所述三自由度二阶波浪扰动预测序列的读取起点。

8、在步骤s1中，所述获取当前控制时刻的无人船航行状态、参考路径、期望航速以及外部提供的三自由度二阶波浪扰动预测序列包括：

9、s101、获取状态、路径和预测扰动序列；在第个采样时刻，获取当前无人船状态、参考路径、期望航速和预测扰动序列；参考路径由连续的路径点或折线段表示；对当前路径段计算船舶投影点、路径切向角、横向误差和前视点；

10、s102、按物理作用方向分配三自由度二阶波浪扰动预测序列；所述三自由度二阶波浪扰动预测序列以船体坐标系表示，纵荡预测力、横荡预测力和艏摇预测力矩预测样本分别与无人船的纵荡、横荡和艏摇运动方向对应；所述预测样本的采样周期与所述非线性模型预测控制器的控制采样周期相同，或者经时间插值后与所述控制采样周期对齐。

11、在步骤s2中，所述基于所述横荡预测力获得横荡有效预测扰动，并转换为有界的侧漂先验补偿角包括：

12、s201、所述横荡有效预测扰动通过对当前横荡预测力进行一阶低通处理并进行死区处理获得；

13、对当前横荡预测力进行一阶低通处理，表达式为：

14、；

15、式中，为第个采样时刻的滤波后的横荡预测力，为第个采样时刻的滤波后的横荡预测力，表示上一采样时刻的滤波结果；为一阶低通滤波系数，为第个采样时刻的横荡预测力，若小于预设死区阈值，则将所述横荡有效预测扰动置为零；否则保留；为采样周期，为横荡先验滤波时间常数；

16、s202、将所述横荡有效预测扰动转换为侧漂先验补偿角；利用横荡线性阻尼参数和期望航速将换算为侧漂先验补偿角，表达式为：

17、；

18、式中，为第个采样时刻的侧漂先验补偿角，为侧漂先验补偿角限幅函数，为允许的最大绝对值，为横荡方向符号系数，为横荡预测力到侧漂先验补偿角的标定系数，为横荡线性阻尼参数的正值，为期望航速，为低航速保护值。

19、在步骤s3中，所述得到修正期望艏向包括：

20、首先，由路径切向角、横向误差和前视距离形成几何los艏向：

21、；

22、式中，为几何艏向，为根据纵、横分量计算角度的双参数反正切函数；

23、其次，构造仅针对剩余扰动的eso；合速度为：

24、；

25、式中，为第个采样时刻无人船在船体坐标系水平面内的合速度，为时刻的纵荡速度，为时刻的横荡速度，为取较大值，为合速度的下限保护值；

26、未包含侧漂补偿时的名义横向误差变化率为：

27、；

28、残余扩张状态观测器eso的两个状态按下式更新：

29、；

30、式中，为横向误差观测状态，为横荡先验补偿后残余横向误差变化率的观测状态，为的变化率，为的变化率，为由当前时刻经低通平滑和死区处理后的横荡二阶波浪预测力，为未包含侧漂补偿时的名义横向误差变化率，为合速度，均为观测器增益，为横向误差；

31、所述残余侧漂补偿角满足：

32、；

33、式中，为残余侧漂补偿角，为反正弦函数，为把截断至，为反正弦函数输入限幅值，为残余侧漂补偿角的最大绝对值；

34、最后，将所述侧漂先验补偿角和残余补偿角共同作用于艏向，得到修正期望艏向：

35、；

36、式中，为先验与残余共同修正后的期望艏向，上标在该式中表示期望参考值；为角度规整函数，为基础视线期望艏向，为第个采样时刻的由横荡预测力形成的侧漂先验补偿角，为第个采样时刻的残余侧漂补偿角。

37、在步骤s4中，获得经预测域局部平滑和通道标定的预测模型外生扰动向量包括：针对第个预测步，采用分通道标定向量形成预测模型的外生扰动向量：

38、；

39、式中，为在当前采样时刻基于已获取信息构造的、第个预测步对应的预测模型外生扰动向量，单位为；，其中，为预测域长度；符号表示对未来时刻的量在时刻作出的预测；为第个预测步的纵荡二阶波浪扰动力预测值；为第个预测步的艏摇二阶波浪扰动力矩预测值，和分别为纵荡通道和艏摇通道的无量纲标定系数，下标表示纵荡自由度，下标表示艏摇自由度；上横线表示经预处理后的预测扰动量；向量中的第二个分量为零，表示横荡预测力不作为非线性模型预测控制器的直接外生扰动输入；上标表示转置。

40、进一步，所述纵荡通道和艏摇通道的无量纲标定系数在控制运行前，根据历史预测扰动样本与对应历史测量扰动样本计算，并在当前控制任务的运行期间保持不变；对于通道，通道标定系数满足：

41、；

42、式中，为通道的无量纲离线标定系数，为将运算结果强制截断限制在0到1的区间内，超出范围的数值直接映射为边界值；为第个采样时刻的纵荡通道或艏摇通道的预测力，为纵荡通道或艏摇通道的预测力，为第时刻的纵荡通道或艏摇通道的预测力，为滤波后的纵荡通道或艏摇通道的预测力，分别为纵荡通道、艏摇通道。

43、在步骤s5中，所述滚动求解包括对预测横向误差、艏向误差、航速误差、横荡速度、艏摇角速度、控制输入和控制输入增量进行加权，并对所述纵荡推力和所述艏摇力矩施加幅值约束及相邻控制时刻的变化率约束；每次滚动求解后，将满足加权项和约束条件的控制序列中的第一个控制输入发送至推进和转艏执行机构。

44、本发明的另一目的在于提供一种基于二阶波浪扰动预测的无人船路径跟踪控制系统，该系统实施所述的基于二阶波浪扰动预测的无人船路径跟踪控制方法，该系统包括：

45、三自由度二阶波浪扰动预测序列获取模块，用于获取当前控制时刻的无人船航行状态、参考路径、期望航速以及外部提供的三自由度二阶波浪扰动预测序列；所述三自由度二阶波浪扰动预测序列包括纵荡预测力、横荡预测力和艏摇预测力矩，且各预测样本与当前控制时刻及预测控制器的预测步对应；

46、侧漂先验补偿角转换模块，基于所述横荡预测力获得横荡有效预测扰动，并转换为有界的侧漂先验补偿角；

47、修正期望艏向得到模块，用于根据所述无人船航行状态和所述参考路径，求解基础视线期望艏向；在残余扩张状态观测器的名义横向误差动态中，引入由所述侧漂先验补偿角确定的横向速度投影项，得到残余侧漂补偿角；通过所述侧漂先验补偿角和所述残余侧漂补偿角修正所述基础视线期望艏向，得到修正期望艏向；

48、预测模型外生扰动向量获得模块，用于针对非线性模型预测控制器的每一预测步，按所述预测步读取所述纵荡预测力和所述艏摇预测力矩对应的预测样本，获得经预测域局部平滑和通道标定的预测模型外生扰动向量；所述预测模型外生扰动向量的横荡分量固定为零；

49、滚动求解模块，用于将所述修正期望艏向、无人船航行状态和所述预测模型外生扰动向量输入保留横荡速度状态递推的三自由度非线性模型预测控制器nmpc，在纵荡推力、艏摇力矩及变化率约束下滚动求解控制序列；以及执行所述控制序列的当前时刻纵荡推力和艏摇力矩，并在下一采样时刻更新所述无人船航行状态和所述三自由度二阶波浪扰动预测序列的读取起点。

50、结合上述的所有技术方案，本发明所具备的有益效果为：

51、第一、本发明将横荡预测扰动转换为侧漂先验补偿角并修正期望艏向，可在横向误差明显累积前提前调整船首方向，适合无独立横荡执行器的欠驱动无人船。残余eso在已包含横荡先验补偿的名义模型上工作，仅估计预测误差、模型误差及其他未建模扰动，减少已知横荡信息被重复观测、重复补偿造成的控制作用重叠。将纵荡预测力和艏摇预测力矩按预测步与nmpc预测域对齐输入，可避免仅采用当前扰动值保持不变所导致的未来扰动时序失配，使控制器能够在预测域内预先评估波浪扰动对航速和艏向的影响。在nmpc中同时设置推力、艏摇力矩及其变化率约束，并仅执行每次优化序列的首个控制量，可降低控制指令突变和超出执行器能力范围的风险；优化异常时保持上一有效控制量，增强控制过程的连续性和安全性。在两个不重叠的90°横浪留出时间窗中，采用同一离线标定的纵荡和艏摇预测通道系数后，横向误差rmse分别降低34.78%和34.63%，艏向误差rmse分别降低38.66%和38.55%，艏摇力矩总变差分别降低39.23%和39.34%；平均单次求解时间仍小于0.5s采样周期，且未出现求解失败。该结果说明，利用本发明方案时间对齐的纵荡和艏摇扰动预测序列构造nmpc预测域外生扰动，并按预测可靠性进行通道标定，有助于在不增加艏摇控制波动的条件下提高给定工况下的路径跟踪性能。

52、第二、本发明可作为船载或岸基控制软件功能接入已有波浪预测接口，无需在控制器内重新部署波浪测量、反演或预测模块。适合测绘、巡逻、环境监测和海上设施巡检无人船的软件升级。当前行业虽已有波浪力建模、侧滑补偿、扰动观测和预测控制等单点技术，但缺少面向欠驱动无人船、能够将三自由度二阶波浪预测信息按物理作用方向进行协同分配的控制器级方案。本发明提出横荡用于导引、纵荡与艏摇用于预测控制的分层协同架构，形成从预测扰动输入到推力和艏摇力矩输出的完整闭环。本发明解决了已有多自由度预测扰动混合进入控制器带来的误差问题。若统一处理三分量扰动，容易产生横荡先验与观测器重复作用、或与纵荡/艏摇控制通道错位。本发明从机理出发，通过横荡先验导引、残余eso和纵荡—艏摇预测域nmpc的顺序协同，使各预测量具有明确且不重复的控制作用。