一种面向于弱机动海洋机器人改进引力型LOS路径跟踪方法、程序、设备及存储介质

本发明属于海洋机器人路径跟踪，具体涉及一种面向于弱机动海洋机器人改进引力型los路径跟踪方法、程序、设备及存储介质。

背景技术：

1、海洋机器人的路径跟踪控制指使得海洋机器人按照期望的路径进行航行，仅将航迹与期望路径间的横向距离误差作为评价控制器的指标，对航行时间或航行速度没有要求。在海洋机器人的实际应用中，海洋环境监测、区域巡逻等多种任务在控制系统设计上均可视作海洋机器人的路径跟踪控制问题。在海洋航行器的控制器设计中，通常将路径跟踪控制过程分解为制导环节与艏向控制环节。在制导环节根据海洋机器人当前位置给出合适的浮体期望艏向，之后由艏向控制器完成艏向控制过程。

2、视线法(line-of-sight，los)是一种经典的制导算法。它具有结构简单，设计参数少，物理意义明确等众多优点，在水面无人艇、水下机器人和无人机等路径跟踪控制控制器设计中获得了广泛应用。但对于一些受海浪流影响较大的弱机动海洋机器人，如欠驱动的无人艇，波浪滑翔器等，由于其机动能力较弱，海流速度与其航速大小通常是相当的，海流对这些弱机动海洋机器人的路径跟踪的影响将造成一些传统航行器中通常难以出现独特现象。

3、例如波浪滑翔器有时将会在强海流的影响下发生较大位移，当海流影响不再剧烈时波浪滑翔器已距离期望路径过远，传统的制导律可能无法使波浪滑翔器重新返回期望路径。而对于含积分过程的引力型los制导律，波浪滑翔器可能由于海流影响下迟迟不能收敛至期望路径造成控制器中误差积分过大，进而损害控制效果。

4、专利cn114115297b中采用了视线导引法对auv的路径进行跟踪，但是这种导引方法更适用于推进器完备以及受海浪影响较小的航向器，对于受海浪影响较大的航向器会容易产生较大的横向稳定误差，导致无法准确跟踪。

技术实现思路

1、本发明的目的在于解决当前已有的引力向量型los制导策略中容易出现海洋机器人越过期望路径出现控制超调的问题，提供一种面向于弱机动海洋机器人改进引力型los路径跟踪方法。

2、一种面向于弱机动海洋机器人改进引力型los路径跟踪方法，包括以下步骤：

3、步骤1：获取海洋机器人当前时刻位置pwg、当前期望路径的起点pk和终点pk+1，获取引力和

4、从海洋机器人当前时刻位置pwg向期望路径pkpk+1作垂线，垂足为pe；点pk+1和pe处分别存在引力场，点pk+1处的引力用于引导海洋机器人向期望路径点pk+1航行；点pe处存在两个不同的引力，分别为和用于当海洋机器人偏离期望路径时引导海洋机器人返回期望路径；用于消除路径跟随中的稳态误差；

5、步骤2：在当前期望路径的终点pk+1处，设置一个以pk+1为圆心，半径为r的接纳圆；判断当前时刻海洋机器人是否处于接纳圆的范围内；若当前时刻海洋机器人处于接纳圆的范围内，则判定当前期望路径pkpk+1的跟踪任务，将下一段期望路径pk+1pk+2设为当前期望路径，并将引力初始化为返回步骤1；否则，直接执行步骤3；

6、步骤3：判断海洋机器人当前位置与期望路径的横向误差的绝对值|e|是否超过设定的最大误差阈值emax；即海洋机器人当前位置与期望路径的垂直距离；

7、若|e|>emax，则将海洋机器人的期望艏向取为方向，并将进行初始化为且不再更新直至横向误差绝对值|e|小于最大误差阈值；

8、若|e|≤emax，则更新引力将海洋机器人的期望艏向取为海洋机器人所受的引力总和方向，

9、步骤4：海洋机器人根据期望艏向航行，更新海洋机器人当前时刻位置；

10、步骤5：判断当前期望路径pkpk+1是否跟踪完毕，若未完成跟踪，则返回步骤1；若已完成当前期望路径pkpk+1跟踪，则执行步骤6；

11、步骤6：判断路径跟踪任务是否完成，若未完成，则将下一段期望路径pk+1pk+2设为当前期望路径，返回步骤1；若已完成路径跟踪任务，则停止计算。

12、进一步地，所述点pe的坐标(xpe,ype)为：

13、

14、

15、其中，(xt,yt)为海洋机器人当前时刻位置pwg的坐标；(xk,yk)为当前期望路径的起点pk的坐标；(xk+1,yk+1)为当前期望路径的终点pk+1的坐标。

16、进一步地，所述引力向量的坐标(xfp,yfp)为：

17、

18、

19、其中，k1为设计参数；(xt,yt)为海洋机器人当前时刻位置pwg的坐标；(xk,yk)为当前期望路径的起点pk的坐标；(xk+1,yk+1)为当前期望路径的终点pk+1的坐标。

20、进一步地，所述引力向量的坐标为：

21、

22、其中，k2为设计参数；(xt,yt)为海洋机器人当前时刻位置pwg的坐标；(xpe,ype)为点pe的坐标。

23、进一步地，所述步骤3中若|e|≤emax，则更新引力的方法具体为：

24、

25、其中，表示更新后的引力同时为下一时刻的初值；表示当前时刻的初值；k3为设计参数；λ为放大因子，λ＞1；表示海洋机器人穿越期望路径pkpk+1，发生控制超调。

26、一种计算机装置/设备/系统，包括存储器、处理器及存储在存储器上的计算机程序，所述处理器执行所述计算机程序以实现上述一种面向于弱机动海洋机器人改进引力型los路径跟踪方法的步骤。

27、一种计算机可读存储介质，其上存储有计算机程序/指令，该计算机程序/指令被处理器执行时实现上述一种面向于弱机动海洋机器人改进引力型los路径跟踪方法的步骤。

28、一种计算机程序产品，包括计算机程序/指令，该计算机程序/指令被处理器执行时实现上述一种面向于弱机动海洋机器人改进引力型los路径跟踪方法的步骤。

29、本发明的有益效果在于：

30、本发明针对海流影响下的弱机动路径跟踪控制问题，提出了改进的引力向量型los制导策略，在期望路径的几个特殊位置处设置引力向量，用于指引期望路径终点方向，缩小横向跟踪误差以及克服海流影响带来的稳态误差。将期望艏向设置为几个引力的合力方向，同时设定了横向误差判断器以及根据是否发生超调采取不同的期望艏向角计算公式，使得既可以准确的跟踪期望路径，又可以在大偏移下返回目标路径点附近，继续完成路径跟踪任务。本发明相比于传统的los制导策略可以良好的解决由于海流影响带来的稳态误差问题，同时也解决了传统的aflos算法可能导致的超调过程大，调节时间长的问题。

技术特征：

1.一种面向于弱机动海洋机器人改进引力型los路径跟踪方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种面向于弱机动海洋机器人改进引力型los路径跟踪方法，其特征在于：所述点pe的坐标(xpe,ype)为：

3.根据权利要求1所述的一种面向于弱机动海洋机器人改进引力型los路径跟踪方法，其特征在于：所述引力向量的坐标(xfp,yfp)为：

4.根据权利要求1所述的一种面向于弱机动海洋机器人改进引力型los路径跟踪方法，其特征在于：所述引力向量的坐标为：

5.根据权利要求1所述的一种面向于弱机动海洋机器人改进引力型los路径跟踪方法，其特征在于：所述步骤3中若|e|≤emax，则更新引力的方法具体为：

6.一种计算机装置/设备/系统，包括存储器、处理器及存储在存储器上的计算机程序，其特征在于：所述处理器执行所述计算机程序以实现权利要求1至5中任一项所述方法的步骤。

7.一种计算机可读存储介质，其上存储有计算机程序/指令，其特征在于：该计算机程序/指令被处理器执行时实现权利要求1至5中任一项所述方法的步骤。

8.一种计算机程序产品，包括计算机程序/指令，其特征在于：该计算机程序/指令被处理器执行时实现权利要求1至5中任一项所述方法的步骤。

技术总结本发明属于海洋机器人路径跟踪技术领域，具体涉及一种面向于弱机动海洋机器人改进引力型LOS路径跟踪方法、程序、设备及存储介质。本发明在期望路径的几个特殊位置处设置引力向量，用于指引期望路径终点方向，缩小横向跟踪误差以及克服海流影响带来的稳态误差；将期望艏向设置为几个引力的合力方向，同时设定了横向误差判断器以及根据是否发生超调采取不同的期望艏向角计算公式，使得既可以准确的跟踪期望路径，又可以在大偏移下返回目标路径点附近，继续完成路径跟踪任务。本发明相比于传统的LOS制导策略可以良好的解决由于海流影响带来的稳态误差问题，同时也解决了传统的AFLOS算法可能导致的超调过程大，调节时间长的问题。技术研发人员：廖煜雷,潘恺文,魏天宇,李晔,刘竟成,史健,张拓圣,张家华受保护的技术使用者：哈尔滨工程大学技术研发日：技术公布日：2024/6/20