一种基于改进RRT与DWA的无人船路径规划方法

一种基于改进rrt与dwa的无人船路径规划方法
技术领域
1.本发明涉及路径规划技术领域，尤其涉及一种基于改进rrt与dwa的无人船路径规划方法。


背景技术：

2.随着人工智能技术的不断推新，无人船将在海洋探测中占据重要一环。无人船有着作业效率高，成本低等特点，更加适用于海洋资源勘探，定点巡逻，无人运输。要实现上述功能，无人船的路径规划不可避免，通过对路径的合理规划，能够在确保无人船的安全性的同时，节省能源和时间的消耗。
3.路径规划通常根据环境已知和未知两种情况划分为两种不同的类型，其中，环境已知时称为全局路径规划，常用的有dijkstra，a*，rrt，rrt*算法等，对于环境未知的局部路径规划，常采用dwa，apf，teb算法等。但是海洋环境的变化莫测往往导致在环境已知的情况下也会出现未知事物，这就要求无人船在已知环境的场景下还得具备局部避障能力。
4.rrt*算法采用随机采样的方式，结构简单对设备性能要求低，同时全图搜索能够确保找到路径，随着算法迭代次数的增加，路径也会不断优化。但是rrt*的随机采样的方式过于随机，难以保证时效性，在路径优化过程中的随机采样，耗费的时间也会不断增加。通过对rrt*的采样点进行限制，减少算法所需的时间，是目前常用的改进方法。dwa算法是对动态障碍的局部规划算法，但评价函数的权值不能适应不同的环境。


技术实现要素：

5.针对现有方法的不足，本发明改进rrt*算法避免了传统rrt*算法采样的过于随机，减少了算法搜索和路径优化的时间；改进dwa算法增强了不同环境的适应性；最后利用两种改进算法的优点，将两者融合应用在无人船的路径规划上，解决了以最优的路径到达目标位置并实时躲避动态障碍物的问题。
6.本发明所采用的技术方案是：一种基于改进rrt与dwa的无人船路径规划方法包括以下步骤：
7.步骤一、初始化地图、无人船的初始状态和设置改进rrt*算法的根节点和步长；
8.进一步的，初始化地图包括地图的大小、静态障碍物的大小与位置、动态障碍物的大小、位置和运动速度；无人船的初始状态包括：无人船的起点和终点，初始化无人船的运动状态。
9.步骤二、构建改进rrt*算法的树；
10.进一步的，具体包括：
11.步骤21、确立随机采样点x
rand
，采样点x
rand
根据采样概率选择终点，将采样点x
rand
放入生长树集合中；
12.步骤22、遍历生长树集合，选择与随机采样点x
rand
距离最小的点x
near
，根据x
rand
和x
near
的连线方向确定生长方向；
13.步骤23、x
near
根据生长方向生长一个步长，得到新的节点x
new
；
14.步骤24、判断x
new
是否在障碍物的范围内，如果在范围内则删除x
new
；否则循环执行并判断，如果不在范围内，再判断x
near
与x
new
的连线是否经过障碍物，如果经过障碍物删除x
new
；否则将x
new
放入生长树节点集合。
15.步骤三、重新为节点x
new
选择父节点；
16.进一步的，具体包括：
17.步骤31、搜索节点x
new
若干步长范围内的邻近节点，将邻近节点作为节点x
new
的备选父节点；
18.步骤32、计算每个邻近节点到x
new
的路径代价加上起点到对应每个邻近节点的路径代价；
19.步骤33、计算x
new
与起点之间的路径代价；该路径代价称为原始路径代价；
20.步骤34、比较步骤32与步骤33的路径代价，选择最小路径代价的节点为x
new
的父节点。
21.步骤四、以节点x
new
为中心若干步长范围内重新为随机树布线；
22.进一步的，具体包括：
23.步骤41、在节点x
new
若干步长范围内将x
new
作为邻近节点的父节点，计算邻近节点以x
new
作为父节点到起点的路径代价，并与邻近节点经过原父节点到达起点的路径代价进行比较；
24.步骤42、若以x
new
作为父节点的路径代价小于经过原父节点的路径代价，则将邻近节点与原父节点的连线删除，同时连接邻近节点与x
new
；若以x
new
作为父节点的路径代价大于经过原父节点的路径代价，则不进行重布线；
25.步骤43、重布线之后，判断x
new
与终点间的距离，若距离大于步长或小于步长但x
new
与终点的连线与障碍物有碰撞，则重新采样；否则将x
new
与终点直接相连，并从终点向前回溯，得到初始路径。
26.步骤五、得到初始路径后，从终点开始回溯父节点，并将终点依次与回溯父节点相连，构成新路径，判断路径是否与障碍物相碰，选择最小路径代价进行路径优化；
27.进一步的，具体包括：
28.步骤51、判断新路径是否经过障碍物，若不经过障碍物，则直接相连，同时判断新路径是否包含起点，若包含起点，则停止，找到最优路径；
29.步骤52、若经过障碍物，则连接回溯父节点的下一个节点，接着从下一个节点开始再次与回溯父节点依次相连，不断构成新路径，并重复步骤51；
30.步骤53、对优化后的路径进行分段，每个分段点作为局部终点，分段的次数根据起点与终点的实际距离划分。
31.步骤六、根据无人船的初始状态和约束条件，进行速度采样，推算出无人船的前行轨迹，选取得分最高的前行路径；
32.进一步的，具体包括：
33.步骤61、设置无人船当前位置坐标、当前线速度、角速度；当无人船相邻时刻为ms级时，相邻两点之间的运动轨迹为直线，得到无人船的下个状态的位置坐标；
34.步骤62、对无人船的速度进行采样，并设置约束条件；
35.进一步的，约束条件包括：
36.约束无人船所有矢量速度的集合的范围，公式为：
[0037]vs
＝{(v,ω)|v∈[v
min
,v
max
],ω∈[ω
min
,ω
max
]}
[0038]
式中，v
min
,v
max
分别代表无人船线速度的最小值和最大值；ω
min
,ω
max
分别代表无人船角速度的最小值和最大值；
[0039]
根据无人船的电机约束无人船在有限时间内的速度，公式为：
[0040][0041]
式中，vc，分别表示无人船的当前线速度，线速度的最大减速度，线速度的最大加速度；ωc，分别表示无人船的当前角速度，角速度的最大减速度，角速度的最大加速度；
[0042]
根据无人船躲避障碍物时的安全性要求，约束无人船的速度，公式为：
[0043][0044]
式中，dist(v,ω)为无人船的轨迹末端与最近障碍物的距离；
[0045]
步骤63、通过评价函数对无人船的每条轨迹进行评分，并对对评价函数的每一项进行归一化处理。
[0046]
进一步的，评价函数的评分公式为：
[0047]
g(v,ω)＝σ(α
·
heading(v,ω)+β
·
dist(v,ω)+γ
·
velocity(v,ω))
[0048]
式中，heading(v,ω)是方位角评价函数；dist(v,ω)为模拟无人船的轨迹末端与最近障碍物的距离；velocity(v,ω)表示模拟无人船的速度大小；α、β、γ代表权值，σ用来平滑权重。
[0049]
进一步的，还包括改进评价函数，公式为：
[0050]
g(v,ω)＝σ(α
·
(1+dist_goal)
·
heading(v,ω)+β
·
dist2(v,ω)+γ
·
(6r-dist(v,ω))
·
velocity(v,ω))
[0051]
式中，dist_goal表示模拟无人船轨迹末端与终点的距离，r表示最近障碍物的半径。
[0052]
本发明的有益效果：
[0053]
1、通过在随机采样时，使采样点有一定概率选择终点，可以缩减算法的搜索时间；
[0054]
2、从终点开始不断向前回溯父节点，并连接进行碰撞检测，最终得到一条优化的路径，相比rrt*在空间中的随机采样，然后不断优化路径，耗时更少；在相同的时间内，改进后的rrt*算法得到的路径代价也小于原rrt*的路径代价；
[0055]
3、对dwa算法的评价函数进行改进，改进后的算法相比原算法适应性更强，对比相同环境，耗费的时间缩小，路径更优；
[0056]
4、将改进后的rrt*和dwa算法相结合，既能弥补rrt*不能局部避障的需求，也能弥补dwa算法不能选择最优路径的缺点，最终无人船能够躲避已知环境中的未知障碍，并能选择最优路径。
附图说明
[0057]
图1是本发明的基于改进rrt*与dwa的无人船路径规划方法流程图；
[0058]
图2是rrt*算法重新选取父节点的过程；
[0059]
图3是rrt*算法重布线的过程；
[0060]
图4是rrt*算法优化路径的示例；
[0061]
图5(a)、(b)分别是rrt*与改进rrt*路径规划对比图；
[0062]
图6(a)、(b)分别是dwa路径规划和改进dwa路径规划对比图；
[0063]
图7是结合改进rrt*算法与dwa算法的路径规划图。
具体实施方式
[0064]
下面结合附图和实施例对本发明作进一步说明，此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
[0065]
如图1所示，一种基于改进rrt与dwa的无人船路径规划方法包括以下步骤：
[0066]
步骤一、初始化设置；
[0067]
对整个地图进行初始化设置，确定无人船的初始状态，设置改进rrt*算法的根节点和步长；
[0068]
包括如下步骤：
[0069]
步骤11、利用matlab软件构建无人船驾驶地图，并对地图进行初始化设置，包括地图的大小、静态障碍物的大小与位置、动态障碍物的大小、位置和运动速度；在地图中添加无人船的起点和终点，初始化无人船的运动状态。
[0070]
步骤12、以无人船的起点作为改进rrt*算法的根节点，并将根节点放入生长树的节点集合中。
[0071]
步骤13、设置初始生长步长，步长的大小可根据已知环境信息进行调整，地图较大时，可相应的增大步长大小，否则过短的步长会导致搜索速度变慢；同理，地图范围较小时，过大的步长容易与障碍物碰撞。
[0072]
步骤2、构建改进rrt*算法的树；
[0073]
获取采样点，根据采样点与最近距离节点确定方向，并按步长大小生长，判断新生长的节点是否与障碍物碰撞；
[0074]
具体包括如下步骤：
[0075]
步骤21、在地图范围空间中随机采样，在采样时判断采样概率，若采样概率在0-0.1之间，则采样点选择终点(即目标点)；若概率范围在其他范围，则采样点随机采样，随机采样点命名为x
rand
；
[0076]
步骤22、遍历生长树集合，将生长树中的每个节点与随机采样点x
rand
的距离计算出来，同时比较每个节点与随机采样点x
rand
的距离大小，挑选出距离最小的节点，命名为x
near
，将x
near
加入生长树的节点集合，将选取出的节点x
near
与x
rand
相连接作为生长树的生长方向；
[0077]
步骤23、从x
near
与x
rand
相连接的方向进行生长，生长距离等于初始设置的步长，x
near
按照生长方向生长一个步长的距离得到一个新的节点，扩展后的新节点命名为x
new
，x
near
是x
new
的父节点；
[0078]
步骤24、判断新节点x
new
是否在障碍物的范围内，若生长的新节点x
new
在障碍物范围内，则将新节点x
new
删除，重复步骤21；若新节点x
new
不在障碍物范围内，判断x
near
与x
new
的
连线是否经过障碍物，若经过，删除新节点x
new
，并重复步骤21；若没有经过，则将新节点x
new
放入生长树节点集合中；
[0079]
步骤三、重新为x
new
选择父节点；
[0080]
每生长一个新节点x
new
，则在其一定范围内找出邻近节点，计算每个邻近节点与起点的路径代价，计算每个邻近节点与新节点x
new
的路径代价，将两者的路径代价相加，构成总路径代价；将总路径代价与x
new
经过原父节点到达起点的路径代价相比较，选择最小路径，得到新的父节点。
[0081]
包括如下步骤：
[0082]
步骤31、以新节点x
new
为圆心，以两倍步长为半径画一个圆，搜索圆域内x
new
的邻近节点，搜索邻近节点时不包含x
new
的父节点，其余邻近节点作为x
new
节点的备选父节点；
[0083]
步骤32、计算每个邻近节点到x
new
的路径代价加上起点到对应每个邻近节点的路径代价；
[0084]
步骤33、计算x
new
与起点之间的路径代价，该路径代价称为原始路径代价；
[0085]
步骤34、将原始路径代价与步骤32中的每个路径代价之和进行比较；若步骤32中的每个路径代价之和都大于原始路径之和，则不采取操作；若小于路径之和，则选取路径代价之和最小的路径，其对应的邻近节点作为x
new
新的父节点，连接邻近节点与x
new
，删除x
new
与原父节点的连线
[0086]
以图2为例：图2(a)表示x
new
原父节点连线图，其中j节点为x
new
节点，节点a为起点，虚线圆框内为搜索范围，节点g为节点j的原父节点，用实线相连接，节点f、i、e为节点j的邻近节点，用虚线连接(仅是为了便于观察计算，实际上未连接)，数字代表两个节点之间的路径代价；计算每个邻近节点到达起点a的路径代价，计算每个邻近节点到达节点j的路径代价，并将两者的路径代价之和相加。其中节点f为邻近节点，对应路径为a-c-f-j，对应路径代价之和为2+3+4＝9。节点i为邻近节点，对应路径为a-c-f-i-j，对应路径代价之和为2+3+3+3＝11。节点e为邻近节点，对应路径为a-e-j，对应路径代价这和为8+4＝12。接着计算节点j经过原父节点g到达起点的路径代价，对应路径为a-e-g-j，对应路径代价为8+5+2＝15。对比上述4个路径代价，选择最小路径代价9，对应的邻近节点f作为节点j新的父节点，最后连接节点f和节点j，删除节点j与节点g的连线，重选父节点后的连线如图2(b)所示。
[0087]
步骤四、随机树的重布线的过程；
[0088]
当x
new
选择了新的父节点之后，会对生长的随机树进行重新布线的操作来进一步减少路径代价；
[0089]
具体包括如下步骤：
[0090]
步骤41、重复步骤31中画圆的过程，并搜索x
new
的邻近节点；假设将x
new
作为邻近节点的父节点，计算邻近节点以x
new
作为父节点到起点的路径代价，并与邻近节点经过原父节点到达起点的路径代价进行比较；
[0091]
步骤42、若以x
new
作为父节点的路径代价小于经过原父节点的路径代价，则将邻近节点与原父节点的连线删除，同时连接邻近节点与x
new
；若以x
new
作为父节点的路径代价大于经过原父节点的路径代价，则不进行重布线操作；
[0092]
步骤43、重布线之后，判断x
new
与终点间的距离，若两者距离大于步长或者两者距离之和小于步长但两者的连线与障碍物有碰撞，则重复步骤二至四；若两者距离之和小于
步长且两者的连线与障碍物无碰撞，则将x
new
与终点直接连接，此时从终点向前不断回溯父节点便能找到一条初始路径。
[0093]
以图3进行说明：图3(a)表示初始布线图，j节点为x
new
节点，节点a为起点，虚线圆框内为搜索范围，节点f为节点j的原父节点，用实线相连接，节点i、e、g为节点j的邻近节点，用虚线连接，数字代表两个节点之间的路径代价。假设将节点j作为邻近节点新的父节点，计算每个邻近节点经过新的父节点到达起点的路径代价，计算每个邻近节点经过原父节点到达起点的路径代价，将两者路径代价相比较，选择路径代价小的对应节点进行布线；其中，节点i为邻近节点，对应的两个路径分别为a-c-f-j-i和a-c-f-i，路径代价分别2+3+4+3＝12和2+3+3＝8，8《12，因此仍选择节点f作为节点i的父节点；节点e为邻近节点时，对应路径为a-c-f-j-e和a-e，对应路径代价为2+3+4+4＝13，8，8《13，因此仍保持节点a作为节点e的父节点；节点g为邻近节点时，对应路径代价为a-c-f-j-g，a-e-g，对应路径代价为2+3+4+2＝11，8+5＝13，11《13，因此删除节点g与原父节点e的连线，连接节点g与节点j，重布线后的结果如图3(b)所示。
[0094]
步骤五、路径优化；
[0095]
得到初始路径后，从终点开始回溯父节点，并将终点依次与回溯父节点相连，构成新的路径；
[0096]
具体包括如下步骤：
[0097]
步骤51、判断新路径是否经过障碍物，若不经过障碍物，则直接相连，同时判断新路径是否包含起点，若包含起点，则停止，同时找到最优路径；
[0098]
步骤52、若经过障碍物，则连接回溯父节点的下一个节点，接着从下一个节点开始再次与回溯父节点依次相连，不断构成新路径，并重复步骤51；
[0099]
具体的以图4进行说明；
[0100]
图4(a)是得到的初始路径，连接终点与x
parent4
，构成的新路径与障碍物无碰撞，不包含起点，则将终点与x
parent3
相连，新路径碰撞障碍物，在图4(b)中以点虚线所示，则连接终点和x
parent3
的下一个节点x
parent4
；在图4(b)中以虚线所示，接着从x
parent4
开始依次连接向前回溯的父节点，发现x
parent4
与x
parentl
相连接时发生碰撞，因此连接x
parent4
和x
parent2
；最后x
parent2
与回溯父节点相连时，发现新路径包含起点且不经过障碍物，此时循环结束，得到优化路径如图4(b)中的虚线所示。
[0101]
步骤53、对优化后的路径进行分段，每个分段点作为局部终点，分段的次数根据起点与终点的实际距离划分，当距离较短时，可不分段处理；当距离较远时，可分成多个路径。
[0102]
步骤六、利用dwa算法(dynamic window approach)确定无人船的轨迹；
[0103]
每个分段路径根据无人船的初始状态和约束条件，便能进行速度采样，从而推算出无人船的前行轨迹。
[0104]
具体包括如下步骤：
[0105]
步骤61、假设无人船当前位置为(x，y)，当前线速度v，角速度ω；当无人船相邻时刻δt很短(ms级)时，运动距离短，相邻两点之间的运动轨迹可以看成直线，则无人船的移动距离如下所示：
[0106]
δs＝v
×
δt
[0107]
转换成坐标表示：
[0108]
δx＝v
×
δt
×
cos(θ)
[0109]
δy＝v
×
δt
×
sin(θ)
[0110]
无人船的下个状态的位置坐标为：
[0111]
x＝x+v
×
δt
×
cos(θ)
[0112]
y＝y+v
×
δt
×
sin(θ)
[0113]
θ＝θ+ω
×
δt
[0114]
公式中的θ值为无人船的前进方向与x轴的夹角。
[0115]
步骤62、得到无人船的运动模型，需要进行速度采样；无人船在实际运动中根据自身的硬件设备与外界的环境因素，会将速度采样约束在一定的范围内，对应约束如下：
[0116]vs
＝{(v,ω)|v∈[v
min
,v
max
],ω∈[ω
min
,ω
max
]}
[0117]
式中，v
min
,v
max
分别代表无人船线速度的最小值和最大值；ω
min
,ω
max
分别代表无人船角速度的最小值和最大值；vs表示无人船所有矢量速度的集合。
[0118]
无人船的电机性能影响着加速度，而电机的力矩有限，不能无限大，无人船在有限时间内的速度约束如下：
[0119][0120]
式中，vc，分别表示无人船的当前线速度，线速度的最大减速度，线速度的最大加速度；ωc，分别表示无人船的当前角速度，角速度的最大减速度，角速度的最大加速度。
[0121]
同时考虑到无人船在躲避障碍物时，确保船只的安全性，在与障碍物碰撞前停止前进，对应的约束公式如下：
[0122][0123]
式中，dist(v,ω)为模拟无人船的轨迹末端与最近障碍物的距离；
[0124]
步骤63、有了无人船的运动模型和采样速度就能模拟出无人船的多条轨迹；此时根据评价函数来评价每条轨迹的得分，评价函数如下所示：
[0125]
g(v,ω)＝σ(α
·
heading(v,ω)+β
·
dist(v,ω)+γ
·
velocity(v,ω))
[0126]
式中，heading(v,ω)是方位角评价函数，表示无人船与终点之间的角度差，差值越小，得分越高；dist(v,ω)为模拟无人船的轨迹末端与最近障碍物的距离，距离越远，得分越高；velocity(v,ω)表示模拟无人船的速度大小，速度越大，得分越高；α、β、γ代表权值，σ用来平滑权重。
[0127]
由于海洋环境的变幻莫测，固定权值的评价函数并不能适应多变的环境，因此对评价函数作进一步改进，改进后的评价函数如下所示：
[0128]
g(v,ω)＝σ(α
·
(1+dist_goal)
·
heading(v,ω)+β
·
dist2(v,ω)+γ
·
(6r-dist(v,ω))
·
velocity(v,ω))
[0129]
式中，dist_goal表示模拟无人船轨迹末端与终点的距离，r表示最近障碍物的半径；改进后的评价函数能够使无人船在与终点距离较远时，增加航向的比重；当与障碍物的距离较近时，保持较快的速度绕开障碍物。
[0130]
接着对评价函数的每一项进行归一化处理，保证评价函数不会因为某一项过大或者过小而产生剧烈影响，归一化过程如下：
[0131][0132][0133][0134]
式中，n表示所有采样的轨迹，i表示待评价的当前轨迹；最后选取评价函数得分最高模拟轨迹作为无人船的实际航线。
[0135]
如图5-7为本发明方法的效果，从图5可以看出改进后的rrt*的路径规划相比与改进前的rrt*的路径更短；图6中可以看出改进评价函数的dwa算法的路径明显短与常用评价函数的dwa算法；图7为融合改进rrt*和评价函数的dwa算法的无人船的路径效果图。
[0136]
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。

技术特征：
1.一种基于改进rrt*与dwa的无人船路径规划方法，其特征在于，包括以下步骤：步骤一、初始化地图、无人船的初始状态和设置改进rrt*算法的根节点和步长；步骤二、构建改进rrt*算法的树；步骤三、为节点x
new
选择父节点；步骤四、以节点x
new
为中心若干步长范围重新随机树布线；步骤五、得到初始路径后，从终点开始回溯父节点，并将终点依次与回溯父节点相连，构成新路径，判断路径是否与障碍物相碰，选择最小路径代价进行路径优化；步骤六、根据无人船的初始状态和约束条件进行速度采样，推算出无人船的前行轨迹，选取得分最高的前行路径。2.根据权利要求1所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，初始化地图包括地图的大小、静态障碍物的大小与位置、动态障碍物的大小、位置和运动速度；无人船的初始状态包括：无人船的起点和终点，初始化无人船的运动状态。3.根据权利要求1所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，步骤二具体包括：步骤21、确立随机采样点x
rand
，采样点x
rand
根据采样概率选择终点，将采样点x
rand
放入生长树集合中；步骤22、遍历生长树集合，选择与随机采样点x
rand
距离最小的点x
near
，根据x
rand
和x
near
的连线方向确定生长方向；步骤23、x
near
根据生长方向生长一个步长，得到新的节点x
new
；步骤24、判断x
new
是否在障碍物的范围内，如果在范围内则删除x
new
；否则循环执行并判断，如果不在范围内，再判断x
near
与x
new
的连线是否经过障碍物，如果经过障碍物删除x
new
；否则将x
new
放入生长树节点集合。4.根据权利要求3所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，步骤三具体包括：步骤31、搜索节点x
new
若干步长范围内的邻近节点，将邻近节点作为节点x
new
的备选父节点；步骤32、计算每个邻近节点到x
new
的路径代价加上起点到对应每个邻近节点的路径代价；步骤33、计算x
new
与起点之间的路径代价；该路径代价称为原始路径代价；步骤34、比较步骤32与步骤33的路径代价，选择最小路径代价的节点为x
new
的父节点。5.根据权利要求4所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，步骤四具体包括：步骤41、在节点x
new
若干步长范围内将x
new
作为邻近节点的父节点，计算邻近节点以x
new
作为父节点到起点的路径代价，并与邻近节点经过原父节点到达起点的路径代价进行比较；步骤42、若以x
new
作为父节点的路径代价小于经过原父节点的路径代价，则将邻近节点与原父节点的连线删除，同时连接邻近节点与x
new
；若以x
new
作为父节点的路径代价大于经过原父节点的路径代价，则不进行重布线；步骤43、重布线之后，判断x
new
与终点间的距离，若距离大于步长或小于步长但x
new
与终
点的连线与障碍物有碰撞，则重新采样；否则将x
new
与终点直接相连，并从终点向前回溯，得到初始路径。6.根据权利要求5所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，步骤五具体包括：步骤51、判断新路径是否经过障碍物，若不经过障碍物，则直接相连，同时判断新路径是否包含起点，若包含起点，则停止，找到最优路径；步骤52、若经过障碍物，则连接回溯父节点的下一个节点，接着从下一个节点开始再次与回溯父节点依次相连，不断构成新路径，并重复步骤51；步骤53、对优化后的路径进行分段，每个分段点作为局部终点，分段的次数根据起点与终点的实际距离划分。7.根据权利要求1所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，步骤六具体包括：步骤61、设置无人船当前位置坐标、当前线速度、角速度；当无人船相邻时刻为ms级时，相邻两点之间的运动轨迹为直线，得到无人船的下个状态的位置坐标；步骤62、对无人船的速度进行采样，并设置约束条件；步骤63、通过评价函数对无人船的每条轨迹进行评分，并对对评价函数的每一项进行归一化处理。8.根据权利要求7所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，约束条件包括：约束无人船所有矢量速度的集合的范围，公式为：v
s
＝{(v,ω)|v∈[v
min
,v
max
],ω∈[ω
min
,ω
max
]}式中，v
min
,v
max
分别代表无人船线速度的最小值和最大值；ω
min
,ω
max
分别代表无人船角速度的最小值和最大值；根据无人船的电机约束无人船在有限时间内的速度，公式为：式中，v
c
，分别表示无人船的当前线速度，线速度的最大减速度，线速度的最大加速度；ω
c
，分别表示无人船的当前角速度，角速度的最大减速度，角速度的最大加速度；根据无人船躲避障碍物时的安全性要求，约束无人船的速度，公式为：式中，dist(v,ω)为无人船的轨迹末端与最近障碍物的距离；步骤63、通过评价函数对无人船的每条轨迹进行评分，并对对评价函数的每一项进行归一化处理。9.根据权利要求7所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，评价函数的评分公式为：g(v,ω)＝σ(α
·
heading(v,ω)+β
·
dist(v,ω)+γ
·
velocity(v,ω))式中，heading(v,ω)是方位角评价函数；dist(v,ω)为模拟无人船的轨迹末端与最近障碍物的距离；velocity(v,ω)表示模拟无人船的速度大小；α、β、γ代表权值，σ用来平滑
权重。10.根据权利要求9所述的基于改进rrt*与dwa的无人船路径规划方法，其特征在于，评价函数的公式还包括：g(v,ω)＝σ(α
·
(1+dist_goal)
·
heading(v,ω)+β
·
dist2(v,ω)+γ
·
(6r-dist(v,ω))
·
velocity(v,ω))式中，dist_goal表示模拟无人船轨迹末端与终点的距离，r表示最近障碍物的半径。

技术总结
本发明涉及路径规划技术领域，尤其涉及一种基于改进RRT*与DWA的无人船路径规划方法，包括：初始化地图、无人船的初始状态和设置改进RRT*算法的根节点和步长；构建改进RRT*算法的树；为节点X


技术研发人员：储开斌 卢艺 张继 冯成涛
受保护的技术使用者：常州大学
技术研发日：2023.05.31
技术公布日：2023/9/6