无人船路径规划与跟踪方法及系统与流程

本发明涉及无人船，具体地，涉及一种无人船路径规划与跟踪。尤其涉及一种基于自适应los规划与前馈艏摇抑制pid的路径规划与跟踪方法及系统。

背景技术：

1、当前无人船的路径规划与跟踪主要包括基于采样或搜索技术的路径搜索；经过路径搜索后，得到相应关键路径点；基于路经点，基于曲线拟合方法得到参数化轨迹，或得到离散折线轨迹；基于得到的轨迹，实现相应的路径跟踪算法。当前的路径规划与跟踪算法依赖于大量人工经验，参数众多，且存在输出振荡大，人机工程学不友好，船舶动力学载荷高，跟踪误差振荡大，收敛速度慢等问题。

2、因此，市场上需要一种能够提高船舶控制性能的无人船路径规划与跟踪方法及系统。

技术实现思路

1、针对现有技术中的缺陷，本发明的目的是提供一种无人船路径规划与跟踪方法及系统。

2、根据本发明提供的一种无人船路径规划与跟踪方法，包括：

3、步骤s1：获取周围环境障碍物信息、边界信息和船舶信息，并根据实际任务需要建立相对应尺寸的任务地图；

4、步骤s2：在所述任务地图中初始化采样区域并进行路径搜索得到关键点序列；

5、步骤s3：根据所述关键点序列进行光滑参考路径拟合，生成船舶平滑路径；

6、步骤s4：获取船舶状态，并根据船舶状态和船舶平滑路径，使用自适应los算法生成船舶目标转向角度；

7、步骤s5：根据所述船舶目标转向角度，建立闭环控制系统，实现对船舶的控制。

8、优选地，所述步骤s1中根据实际任务需要建立相对应尺寸的任务地图，包括：

9、步骤s1.1：对周围环境障碍物信息和边界信息进行汇集，确定可通行区域和不可通行区域，并对所述可通行区域和不可通行区域进行二值化；

10、步骤s1.2：根据船舶信息中的船舶尺度，确定船舶安全圈的大小；

11、步骤s1.3：根据所述船舶安全圈的大小，对任务地图的不可通行区域进行膨胀和合并处理，将船舶处理为质点；

12、步骤s1.4：计算障碍物稠密度，障碍物稠密度计算公式如下：

13、

14、其中，ρobs表示障碍物稠密度，sobs表示障碍物面积，smap表示地图面积；

15、步骤s1.5：计算自适应rrt的搜索距离，计算公式如下：

16、

17、其中，drrt表示自适应rrt的搜索距离，dmin表示最小搜索距离，dmax表示最大搜索距离，krrt为常量；

18、步骤s1.6：设置rrt最大搜索次数。

19、优选地，所述步骤s2包括：

20、步骤s2.1：在采样区域中进行采样得到点cnew；

21、步骤s2.2：从点cnew到另一个节点cnear上生成直线cc’，若||cc′||＜drrt且两点中间没有障碍物，则确定点cnew为新节点；若||cc′||＜drrt且两点中间有障碍物，则返回步骤s2.1；若||cc′||≥drrt，则在点cnear旁边取一点n，使得点cnear到点n的距离为drrt，若点n不在障碍物内且路径上没有障碍物，则确定点n为新的点cnew，否则返回步骤s2.1；

22、步骤s2.3：如果当前点cnew距离目标点小于drrt，且所述点cnew到目标点中间没有障碍物，则认为路径搜索已经完成，计算整个路径长度dpath，并将所述点cnew加入节点集合中且设置父节点为cnear；

23、步骤s2.4：根据点cnew到父节点cnear的距离dcc′，计算点cnew的路程总代价，计算公式如下：

24、lcnew＝lcnear+dcc′

25、其中，lcnew表示点cnew的路程总代价，lcnear表示cnear点的路程代价，dcc′表示点cnew到父节点cnear的距离；

26、步骤s2.5：以点cnew为圆心，直径为dcheck的圆形区域内，进行节点去重减枝；

27、重复执行步骤s2.1至步骤s2.5，直至完成路径搜索，得到关键点序列。

28、优选地，步骤s3包括：

29、步骤s3.1：带时间参数的b样条曲线p(t)＝∑pini，k(u)，其中，pi表示关键点序列的第i点，ni，k(u)表示关键点坐标影响权重的多项式系数，其中i表示坐标索引，k表示多项式最高的幂数，其中，若ti≤t≤ti+1，有ni，0(u)＝1，否则有ni，0(u)＝0；

30、步骤s3.2：

31、步骤s3.3：

32、其中，t代表时间，为自变量。ti+k为第i+k段曲线被分配的总航行时长。n’i，k(u)表示关键点坐标影响权重的多项式系数随时间的导数。

33、优选地，步骤s4包括：

34、步骤s4.1：根据船舶实际坐标p′(t)与带参数路径坐标p(t)进行路径跟踪，其中，

35、p′(t)＝(x′(t)，y′(t))

36、p(t)＝(x(t)，y(t))

37、进行船舶自适应los算法，计算船舶当前位置到目标点的实际距离，计算公式如下：

38、

39、式中，draw表示船舶当前位置到目标点的实际距离；

40、步骤s4.2：获取船舶艏向角θ，根据路径获取路径点处的切线角度φ，公式如下：

41、φ＝δ(y(t))/δ(x(t))；

42、步骤s4.3：计算船舶距离符号signd，计算公式如下：

43、signd＝sign((p(t)-p′())*(cos(φ)，sin(φ)))；

44、步骤s4.4：计算带符号的船舶距离，计算公式如下：

45、dsign＝singd*draw

46、其中，dsign表示带符号的船舶距离；

47、步骤s4.5：根据船舶距离符号signd和带符号的船舶距离dsign确定视线法los前视距离δ；

48、步骤s4.6：基于视线法los前视距离，获取目标点p″(t)＝(x″(t)，y″(t))，其中

49、x″(t)＝x(t)+δ*cos(φ)

50、y″(t)＝y(t)+δ*sin(φ)；

51、步骤s4.7：计算船舶目标航向角和船舶的运动漂移角，计算公式如下：

52、

53、

54、其中，φtarget表示船舶目标航向角，φdrift表示船舶的运动漂移角，v表示随船坐标系下船舶横荡速度，u表示随船坐标系下船舶纵荡速度；

55、步骤s4.8：计算船舶目标转向角度，计算公式如下：

56、θd＝θ+φdrift-φtarget

57、其中，θd表示船舶目标转向角度。

58、优选地，步骤s4.5包括：

59、若signd>0，则视线法los前视距离计算公式如下：

60、

61、其中，δmin表示最小前视距离，δmax表示最大前视距离，klos为正常数；

62、若signd＜0，则视线法los前视距离计算公式如下：

63、δ＝-dsign+δmax。

64、优选地，所述步骤s5包括：

65、步骤s5.1：令前馈艏摇控制律为：

66、δadd＝kadde(t)*w(t)if e(t)*w(t)>0

67、否则δadd＝0，其中，δadd表示额外的舵角控制输出，w(t)表示船艏与目标方向的夹角，kadd表示一常数，e(t)表示目标转向角度；

68、步骤s5.2：综合前馈控制和反馈控制可得综合舵角输出为：

69、δsum＝δadd+δbase

70、其中，δsum表示综合舵角输出，δbase表示船舶的控制舵角；

71、步骤s5.3：基于船舶与目标点的距离差距，开发船舶推力计算算法，所述船舶推力计算公式如下：

72、

73、其中，thrust表示船舶推力，kpv表示比例常数，dsign带符号的船舶距离，kiv表示积分常数，kdv表示微分常数；

74、步骤s5.4：对积分项目∫kivdsign进行抗饱和处理；

75、步骤s5.5：维护一时间长度为tvlong的误差队列；

76、步骤s5.6：当误差dsign较大且积分器处于饱和状态时，将积分项目从控制算法中剥离；

77、步骤s5.7：基于维护的误差队列，对微分项目进行处理，其中a为一较大常数

78、

79、其中，dsign为带符号的船舶距离，为t时刻的距离值，为(t-k)时刻的距离值，k为时间间隔。

80、根据本发明提供的一种无人船路径规划与跟踪系统，包括：

81、模块m1：获取周围环境障碍物信息、边界信息和船舶信息，并根据实际任务需要建立相对应尺寸的任务地图；

82、模块m2：在所述任务地图中初始化采样区域并进行路径搜索得到关键点序列；

83、模块m3：根据所述关键点序列进行光滑参考路径拟合，生成船舶平滑路径；

84、模块m4：获取船舶状态，并根据船舶状态和船舶平滑路径，使用自适应los算法生成船舶目标转向角度；

85、模块m5：根据所述船舶目标转向角度，建立闭环控制系统，实现对船舶的控制。

86、优选地，模块m4包括：

87、模块m4.1：根据船舶实际坐标p′(t)与带参数路径坐标p(t)进行路径跟踪，其中，

88、p′(t)＝(x′(t)，y′(t))

89、p(t)＝(x(t)，y(t))

90、进行船舶自适应los算法，计算船舶当前位置到目标点的实际距离，计算公式如下：

91、

92、式中，draw表示船舶当前位置到目标点的实际距离；

93、模块m4.2：获取船舶艏向角θ，根据路径获取路径点处的切线角度φ，公式如下：

94、φ＝δ(y(t))/δ(x(t))；

95、模块m4.3：计算船舶距离符号signd，计算公式如下：

96、signd＝sign((p(t)-p′())*(cos(φ)，sin(φ)))；

97、模块m4.4：计算带符号的船舶距离，计算公式如下：

98、dsign＝singd*draw

99、其中，dsign表示带符号的船舶距离；

100、模块m4.5：根据船舶距离符号signd和带符号的船舶距离dsign确定视线法los前视距离δ；

101、模块m4.6：基于视线法los前视距离，获取目标点p″(t)＝(x″(t)，y″(t))，其中

102、x″(t)＝x(t)+δ*cos(φ)

103、y″(t)＝y(t)+δ*sin(φ)；

104、模块m4.7：计算船舶目标航向角和船舶的运动漂移角，计算公式如下：

105、

106、

107、其中，φtarget表示船舶目标航向角，φdrift表示船舶的运动漂移角，v表示随船坐标系下船舶横荡速度，u表示随船坐标系下船舶纵荡速度；

108、模块m4.8：计算船舶目标转向角度，计算公式如下：

109、θd＝θ+φdrift-φtarget

110、其中，θd表示船舶目标转向角度。

111、优选地，模块m4.5包括：

112、若signd>0，则视线法los前视距离计算公式如下：

113、

114、其中，δmin表示最小前视距离，δmax表示最大前视距离，klos为正常数；

115、若signd＜0，则视线法los前视距离计算公式如下：

116、δ＝-dsign+δmax。

117、与现有技术相比，本发明具有如下的有益效果：

118、本发明采取先验随机搜索树采样进行路径搜索与关键点查询，基于关键路经点使用三次样条曲线实现平滑时间参数轨迹生成，将参数化轨迹作为期望轨迹，提出了自适应视线法(自适应los算法)实现船舶目标航向角生成，并提出前馈艏摇抑制pid算法实现船舶的高效和精准舵角控制。有效降低了船舶的跟踪误差，减少了船舶的艏摇，提高了人机工程友好度，降低了船员的负担，也减少了船体的负荷，解决了欠驱动船舶的路径规划与跟踪问题，对大惯性矩船舶的路径跟踪性能提升尤其明显。