一种面向船舶轨迹重现任务的高精度路径跟踪控制方法

本发明涉及船舶运动控制，尤其涉及一种面向船舶轨迹重现任务的高精度路径跟踪控制方法。

背景技术：

1、随着船舶无人化的发展，基于海量ais数据提取基于ais数据点的规划路径，考虑了航线安全性、航行效率和环保因素，能够使船舶航行更加安全、高效、绿色。除此之外，在海难事故调查中，基于事故船舶历史ais数据和其他相关数据源，重现事故船的运动过程，能够帮助调查人员还原事故发生的情况，对事故发生原因调查和事故责任认定分析具有重要意义。

2、针对上述船舶轨迹重现的任务需求，现有的usv路径跟踪控制算法主要存在以下2点缺陷：

3、1)传统的制导算法的参考路径只能基于人为设定的长距离航路点生成，无法对短距离或不规则产生的航路点进行路径跟踪任务，因此无法满足对基于ais数据规划的路径进行轨迹重现的任务需求，存在一定的局限性。

4、2)现有的控制算法仅适用于开阔水域的船舶路径跟踪，无法满足某些特定水域的高跟踪精度需求，例如狭窄航道或多障碍物水域。此外船舶路径跟踪过程中可能会存在输入饱和或者计算负载过大的问题，从而导致执行器发生故障，这也成为了亟待解决的问题。

技术实现思路

1、本发明提供一种面向船舶轨迹重现任务的高精度路径跟踪控制方法，以克服上述技术问题。

2、为了实现上述目的，本发明的技术方案是：

3、一种面向船舶轨迹重现任务的高精度路径跟踪控制方法，包括以下步骤：

4、s1：获取三自由度usv的非线性数学模型与逻辑虚拟船lvs的数学模型；

5、s2：基于ais数据的航路点，根据所述逻辑虚拟船lvs的数学模型获取虚拟参考路径；

6、s3：基于所述虚拟参考路径，根据逻辑虚拟船lvs的数学模型与三自由度usv的非线性数学模型获取usv制导律；

7、s4：基于跟踪距离判定策略，根据所述usv制导律获取usv虚拟控制律，以获取usv虚拟控制律的动态面信号导数；

8、s5：根据所述usv虚拟控制律的动态面信号导数获取usv控制器的中间变量，并采用迭代控制技术根据中间变量构建usv容错控制器；

9、s6：根据所述usv容错控制器控制三自由度usv对虚拟参考路径进行实时路径跟踪控制。

10、进一步的，s1中所述三自由度usv的非线性数学模型的表达式为

11、

12、

13、式中：xj,yj,ψj分别表示usv第j次迭代的位置横坐标、位置纵坐标和艏向角；ν＝[uj,vj,rj]t表示usv第j次迭代的前进速度、横漂速度以及艏摇速度，且j＝1,2,...∈n*表示迭代次数；mu,mv,mr分别表示usv在前进方向、横漂方向以及艏摇方向上的水动力附加质量；fu(ν),fv(ν),fr(ν)分别表示usv在前进自由度、横漂自由度以及艏摇自由度上的非线性项；dwu,dwr,dwv分别表示usv在前进自由度、横漂自由度以及艏摇自由度上受到的外界海洋环境的干扰力和力矩；tu(·)表示usv的主机增益函数；fr(·)表示usv的舵机增益函数；anf表示与主机转速相关的故障控制输入；aδf表示与舵角相关的故障控制输入；an表示与主机转速相关的控制输入；aδ表示与舵角相关的控制输入；μn,μδ分别表示与主机转速和舵角相关的乘法执行器故障系数，且0＜μn≤1,0＜μδ≤1；φn,φδ分别表示与主机转速和舵角相关的加法执行器故障系数；tns,tne,tδs,tδe分别表示与主机转速和舵角相关的故障模型发生故障与结束故障的时间；

14、所述逻辑虚拟船lvs的数学模型的表达式为

15、

16、式中：xd,yd表示逻辑虚拟船lvs的位置横坐标与位置纵坐标；ψd表示逻辑虚拟船lvs的艏向角；ud表示逻辑虚拟船lvs的期望速度；rd表示逻辑虚拟船lvs的期望艏摇角速度。

17、进一步的，所述s3具体为：

18、所述虚拟参考路径包括直线段区域路径与曲弧段区域路径；

19、在所述虚拟参考路径的直线段区域路径时，所述逻辑虚拟船lvs的艏向角与期望艏摇角速度的表达式为

20、rd＝0,ψd＝arctan((yi-yi-1)/(xi-xi-1))        (4)

21、式中：(xi,yi),(xi-1,yi-1)分别表示第i个航路点与第i-1个航路点的位置坐标；

22、在所述虚拟参考路径的曲弧段区域路径时，且将曲弧段区域路径划分为若干圆弧段路径，则逻辑虚拟船lvs的期望艏摇角速度的表达式为

23、rd＝ud/rt          (5)

24、式中：rt表示转向半径；

25、则根据逻辑虚拟船lvs的数学模型与三自由度usv的非线性数学模型获取usv制导律，且所述usv制导律的表达式为

26、

27、式中：xd,yd分别表示逻辑虚拟船lvs的位置横纵坐标与位置纵坐标；x,y分别表示三自由度usv的位置横纵坐标与位置纵坐标；ψr表示逻辑虚拟船lvs相对于三自由度usv实船的方位角。

28、进一步的，所述s4具体包括以下步骤：

29、s41：获取跟踪距离判定策略，所述跟踪距离判定策略具体为

30、获取逻辑虚拟船lvs与三自由度usv的跟踪距离ze，判断所述跟踪距离ze与预设距离阈值ldbset的大小；

31、若判断所述跟踪距离ze小于等于预设距离阈值ldbset，则执行步骤s42；

32、若判断所述跟踪距离ze大于预设距离阈值ldbset，则设置辅助虚拟船avs，以辅助逻辑虚拟船lvs实现与三自由度usv之间的跟踪，并执行步骤s42；

33、s42：根据所述usv制导律获取三自由度usv的运动学误差，且所述运动学误差的方程表达式为

34、

35、

36、式中：jt(ψj)表示由三自由度usv的非线性数学模型所得的旋转矩阵；xe,j表示第j次迭代虚拟船距离三自由度usv实船的前向位置误差；ye,j表示第j次迭代虚拟船距离三自由度usv实船的横向位置误差；ψe,j表示第j次迭代虚拟船相对于三自由度usv实船的艏向误差；

37、s43：根据公式(7)与公式(8)推导获得公式(9)；

38、

39、s44：为了镇定虚拟船距离三自由度usv实船的前向位置误差xe,j与横向位置误差ye,j，设计所述usv虚拟控制律；

40、所述usv虚拟控制律的表达式为

41、

42、式中：kx,ky表示为正的设计参数；表示usv前进速度虚拟控制律；表示usv艏向虚拟控制律；

43、s45：基于dsc动态面技术，根据公式(10)获取usv前进速度与艏向的动态面信号导数；

44、所述usv前进速度与艏向的动态面信号导数的表达式为

45、

46、式中：均表示为大于零的时间常数；表示usv前进速度与艏向的动态面信号；表示动态面信号与usv虚拟控制律之间的差值，即

47、s46：引入误差变量则根据公式(11)获取usv艏摇速度虚拟控制律且所述usv艏摇速度虚拟控制律的表达式为

48、

49、式中：表示为正的设计参数；

50、且基于dsc动态面技术，根据公式(12)获取usv艏摇速度的动态面信号导数；所述usv艏摇速度的动态面信号导数的表达式为

51、

52、

53、式中：表示为大于零的时间常数；表示usv艏摇速度的动态面信号；表示usv艏摇速度的动态面信号与虚拟控制律之间的差值，即

54、进一步的，所述s5具体包括以下步骤：

55、s51：定义误差动态并对所述误差动态求导得到公式(14)；

56、

57、式中：fu(ν),fr(ν)表示usv在前进自由度与艏摇自由度上的非线性项；

58、s52：根据公式(14)采用鲁棒神经阻尼技术对fu(ν),fr(ν)进行鲁棒化处理，所述鲁棒化处理的表达式为

59、

60、式中：au,ar均表示神经网络权重；s(ν)表示高斯函数；εu(ν),εr(ν)表示逼近误差；bu,br表示au,ar的范数值；wu,wr表示au,ar的归一化值；ν表示输入向量的集合,且ν＝[u,v,r]；βν表示设计参数；

61、s53：根据公式(15)获取usv控制器的中间变量，则所述usv控制器的中间变量的表达式为

62、

63、

64、式中：表示设计usv主机转速控制器an中间变量的压缩函数；表示设计usv舵角控制器aδ中间变量的压缩函数；分别表示设计usv主机转速控制器an与舵角控制器aδ的中间变量；

65、s54：采用迭代控制技术，根据usv控制器的中间变量构建usv容错控制器，且所述usv容错控制器的表达式为

66、

67、

68、

69、式中：an,aδ分别表示与船舶主机转速和舵角相关的控制输入；分别表示第j次迭代船舶前进速度与艏摇方向上的自适应率；分别表示第j-1次迭代船舶前进速度与艏摇方向上的自适应率；ξu,j,ξr,j分别表示第j次迭代船舶前进速度与船舶艏摇速度的自适应参数；ξu,j-1,ξr,j-1分别表示第j-1次迭代船舶前进速度与船舶艏摇速度的自适应参数；φu,φr表示利用鲁棒神经阻尼技术合并的正参数；k1,k2,k3,k4,表示正的控制器设计参数；ku,kr表示鲁棒神经阻尼系数。

70、有益效果：本发明提供了一种面向船舶轨迹重现任务的高精度路径跟踪控制方法，基于ais数据的航路点，通过逻辑虚拟船lvs获取虚拟参考路径；基于所述虚拟参考路径，通过逻辑虚拟船lvs的数学模型与三自由度usv的非线性数学模型获取usv制导律；基于跟踪距离判定策略，根据usv制导律获取usv虚拟控制律，以获取usv虚拟控制律的动态面信号导数；虚拟参考路径由逻辑虚拟船lvs实时规划得出，同时在逻辑虚拟船lvs上跟踪距离判定策略适当增设辅助虚拟船avs防止执行器输入饱和，减少船舶计算负载；根据usv虚拟控制律的动态面信号导数获取usv控制器的中间变量，并采用迭代控制技术根据中间变量构建usv容错控制器，根据usv容错控制器控制三自由度usv对虚拟参考路径进行实时路径跟踪控制。本发明针对无人船在复杂水域航行过程中的高精度控制需求，引入了迭代学习控制(iterative learning control,ilc)方法，随着迭代次数的增加能够有效提高usv控制精度，且同时采用了容错技术以构建usv容错控制器，防止执行器故障发生，实现了usv高精度和低故障的路径跟踪需求，有效提高无人船航行的安全性和可靠性。