艇运动学约束下的无人艇局部避障规划方法、系统、设备及介质与流程

1.本发明属于无人艇的自主航行方法，特别是一种艇运动学约束下的无人艇局部避障规划方法、系统、设备及介质。


背景技术：

2.无人艇在自主航行中需要具备全局航路规划和局部避障规划的能力，全局航路规划是从任务需求(起点、目的点、任务内容)出发，在约束条件(航路经济性、水文气象干扰、地球曲率半径等)下，基于先验障碍物信息(安全区、人工装订障碍物、海图障碍物信息)，规划设计长距离全局航路；局部避障规划则是在无人艇按照全局航路航行过程中，遇到全局航路规划过程中未计入的障碍物，规划调整航速或航向，驶离原全局路径、躲避障碍物，在完成原目标任务的前提下，尽可能地确保快速安全航行。
3.现有局部避障规划方法可以分为基于图搜索的算法(包括稀疏a*算法搜索算法及dijkstra算法等)以及随机算法(包括蚁群算法、rrt算法、粒子群算法、遗传算法、速度避障法等)。基于图搜索的算法往往需要对空间中障碍物进行确定描述及对障碍物以外的空间进行确定性建模，需要占用大量的计算时间，即存在算法收敛速度慢的问题，不能满足无人艇避障的实时性要求。蚁群优化算法及粒子群算法等不能很好地解决非完整约束下的路径规划问题，即无法保证路径的有效性。速度避障法不具备全局寻优能力，容易陷入局部最优，严重情况下导致在小范围内转圈无法完成航行任务。
4.rrt算法作为一种基于随机采样的单路径搜索算法，能够通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，并通过限制采样点从而有效地解决高维空间和复杂约束的路径规划问题。但传统rrt算法不考虑无人艇的运动学特性，为了实现避障，会出现高速大幅度转弯的情况，大幅降低艇的平稳性、连续性，严重时会导致艇的倾覆。
5.现有技术存在的主要问题是：
6.(1)不能符合无人艇在运动学方面受非完整性约束、控制上具有强时滞性及非线性的特点、所处环境具有时变性的特点；
7.(2)全局性不足，容易陷入局部最优，导致艇在一定范围内打转；
8.(3)实时性不足，需要较长的规划时间。


技术实现要素：

9.本发明的目的在于提供一种艇运动学约束下的无人艇局部避障规划方法、系统、设备及介质，实现满足无人艇实时避障要求的全局优化航路，保障无人艇安全平稳航行，提高了计算效率。
10.实现本发明目的的技术解决方案为：一种艇运动学约束下的无人艇局部避障规划方法，包括步骤:
11.步骤1、根据无人艇安全航迹结束点和恢复点，初始化无人艇局部避障航路的搜索
树；
12.步骤2、在航路空间中搜索新的位置点；
13.步骤3、判断无人艇运动到新的位置点是否符合艇的动力学要求；
14.步骤4、检测航路是否穿过障碍物；
15.步骤5、判断是否已到达安全航迹恢复点；
16.步骤6、根据无人艇的航行能力，为了保障航行的平滑性，对搜索树进行剪枝处理。
17.一种艇运动学约束下的无人艇局部避障规划系统，包括初始化单元、搜索单元、动力学要求判断单元、障碍物判断单元、到达恢复点判断单元和剪枝处理单元，其中：
18.所述初始化单元根据无人艇安全航迹结束点和恢复点，初始化无人艇局部避障航路的搜索树；
19.所述搜索单元用于在航路空间中搜索新的位置点
20.所述动力学要求判断单元用于判断无人艇运动到新的位置点是否符合艇的动力学要求；
21.所述障碍物判断单元用于检测航路是否穿过障碍物；
22.所述到达恢复点判断单元用于判断是否已到达安全航迹恢复点；
23.所述剪枝处理单元用于根据无人艇的航行能力对搜索树进行剪枝处理。
24.该系统包含上述艇运动学约束下的无人艇局部避障规划方法的全部技术特征，在此不再累述。
25.一种艇运动学约束下的无人艇局部避障规划设备，包括：存储器、处理器和存储在所述存储器上的计算机程序，所述处理器执行所述计算机程序时实现所述的艇运动学约束下的无人艇局部避障规划方法的步骤。
26.一种计算机存储介质，所述计算机存储介质存储有可执行程序，所述可执行程序被处理器执行实现所述的艇运动学约束下的无人艇局部避障规划方法的步骤。
27.本发明与现有技术相比，其显著优点为：
28.(1)本发明不需要对空间进行网格化等确定性建模，能够快速有效地搜索空间，通过状态空间的随机采样点，把搜索快速导向空白区域，搜索时间短；
29.(2)本发明可以将无人艇动力学约束的问题纳入到处理过程中，保障了运动的稳定性；
30.(3)本发明通过基于无人艇运动学的剪枝处理，提高了艇运动的平滑性，保障艇避障的优化航路。
附图说明
31.图1是本发明方法的流程示意图。
具体实施方式
32.结合图1，本发明提出的一种艇运动学约束下的无人艇局部避障规划方法包括：
33.定义：无人艇全局规划航路上安全航迹结束点pt
begin
＝(x
begin
,y
begin
)，安全航迹恢复点pt
end
＝(x
end
,y
end
)，其中pt
begin
和pt
end
即为局部避障规划的起点和终点。障碍物列表为且0《i≤n，其中为正整数域，n为正整数，obi＝[obxi,obyi,obri]，
obxi为障碍物i的形心x坐标，obyi为障碍物i的形心y坐标，obri为障碍物i的外接圆半径。
[0034]
步骤1、初始化以pt
begin
为根节点的搜索树。
[0035]
置tree
avoid
＝{pt
begin
}，以及tree
avoid
生长点pt
grow
为pt
begin
。
[0036]
步骤2、在航路空间中搜索新的位置点。
[0037]
步骤2-1，根据pt
begin
和pt
end
之间的距离，设置初始搜索步长ls
org
，计算公式为：
[0038][0039]
其中α为先验常数；
[0040]
步骤2-2，在航行区域内随机生成一个点pt
random
；
[0041]
步骤2-3，计算tree
avoid
中距离pt
random
最近的点pt
near
，即使得为最小，对于
[0042]
步骤2-4，计算离pt
near
最近障碍物的距离dm
grow
，计算公式为：
[0043][0044]
步骤2-5，计算动态搜索步长ls
now
，公式为：
[0045][0046]
其中μ、γ为先验常数，使得在搜索树距离障碍物较远时加大搜索步长，加快算法的收敛速度，在搜索树距离障碍物较近时减小搜索步长，保障算法跳出局部最小区域；
[0047]
步骤2-6，以pt
near
为父节点，向pt
random
延伸ls
now
，得到新的位置点pt
new
＝[x
new
,y
new
]，
[0048][0049][0050]
步骤3、判断无人艇运动到新的位置点是否符合艇的动力学要求，具体包括：
[0051]
步骤3-1，从搜索树tree
avoid
中查找到pt
near
的父节点pt
father
；
[0052]
步骤3-2，采用三角公式可以计算得到线段pt
father
pt
near
与线段pt
near
pt
new
之间的夹角θ
turn
；
[0053]
步骤3-3，若π-θ
turn
《τ，则符合艇的动力学要求，转到步骤4；否则为不符合，转到步骤2的第二步步骤2-2，其中τ为无人艇在当前航速下的最大转向角；
[0054]
步骤4、检测pt
new
与pt
near
之间连线是否穿过障碍物。
[0055]
步骤4-1，对于记obcenteri＝[obxi,obyi]为障碍物i形心；
[0056]
步骤4-2，采用三角公式计算可得线段pt
near
pt
new
与线段pt
near
obcenteri之间的夹角θ
target
；
[0057]
步骤4-3，采用三角公式计算可得pt
near
到obi外接圆切线与线段pt
near
obcenteri之间夹角θ
circle
。
[0058]
步骤4-4，若θ
target
《θ
circle
，则可以判断pt
new
与pt
near
之间连线穿过障碍物，转到步骤2的第二步步骤2-2；
[0059]
步骤4-5，直到对obstacle中所有obi，pt
new
与pt
near
之间连线都不穿过障碍物，则将pt
new
加入到搜索树tree
avoid
中，将pt
near
作为pt
new
的父节点。
[0060]
步骤5、判断是否已到达安全航迹恢复点
[0061]
计算pt
new
与pt
end
距离，若距离小于预设值d
min
，则判断与pt
end
相遇，则将pt
end
加入到搜索树tree
avoid
中，把pt
new
作为pt
end
的父节点，进入下一步；否则转到步骤2的第二步步骤2-2。
[0062]
步骤6、根据无人艇的航行能力，为了保障航行的平滑性，对搜索树tree
avoid
进行剪枝处理。
[0063]
步骤6-1，将pt
begin
置为pt
pre
；
[0064]
步骤6-2，把pt
pre
的子节点置为pt
prune
，从tree
avoid
中获取pt
prune
的子节点pt
aft
，以及pt
aft
的子节点pt
ac
，以及pt
ac
的子节点pt
bc
；
[0065]
步骤6-3，与步骤3方法类似，判断线段pt
pre
pt
aft
与线段pt
aft
pt
ac
，是否满足艇的动力学要求；
[0066]
步骤6-4，若满足艇的动力学要求，与步骤3方法类似，判断线段pt
aft
pt
ace
与线段pt
ac
pt
bc
，是否满足艇的动力学要求；否则，将pt
prune
置为pt
pre
，转入步骤6-7；
[0067]
步骤6-5，若满足艇的动力学要求，则与步骤4方法类似，检测pt
pre
与pt
aft
之间连线是否穿过障碍物；否则，将pt
prune
置为pt
pre
，转入步骤6-7；
[0068]
步骤6-6，若穿过障碍物，将pt
prune
置为pt
pre
，则转入步骤6-7；否则，将pt
prune
从搜索树tree
avoid
中移除，将pt
pre
作为pt
aft
的父节点，将pt
aft
置为pt
prune
，转入步骤6-7；
[0069]
步骤6-7，重复步骤6-2和步骤6-3，直到pt
prune
为pt
end
的父节点；
[0070]
步骤6-8，将tree
avoid
输出作为局部避障规划航路点序列。
[0071]
一种艇运动学约束下的无人艇局部避障规划系统，包括初始化单元、搜索单元、动力学要求判断单元、障碍物判断单元、到达恢复点判断单元和剪枝处理单元，其中：
[0072]
所述初始化单元根据无人艇安全航迹结束点和恢复点，初始化无人艇局部避障航路的搜索树；
[0073]
所述搜索单元用于在航路空间中搜索新的位置点
[0074]
所述动力学要求判断单元用于判断无人艇运动到新的位置点是否符合艇的动力学要求；
[0075]
所述障碍物判断单元用于检测航路是否穿过障碍物；
[0076]
所述到达恢复点判断单元用于判断是否已到达安全航迹恢复点；
[0077]
所述剪枝处理单元用于根据无人艇的航行能力对搜索树进行剪枝处理。
[0078]
该系统包含上述艇运动学约束下的无人艇局部避障规划方法的全部技术特征，在此不再累述。
[0079]
本实施方式还提出一种艇运动学约束下的无人艇局部避障规划设备，包括：存储器、处理器和存储在所述存储器上的计算机程序，所述处理器执行所述计算机程序时实现所述的艇运动学约束下的无人艇局部避障规划方法的步骤。
[0080]
本实施方式还涉及一种计算机存储介质，所述计算机存储介质存储有可执行程序，所述可执行程序被处理器执行实现所述的艇运动学约束下的无人艇局部避障规划方法的步骤。
[0081]
所述的方法针对无人艇自主航行所需具备的局部避障规划能力需求，在机器人rrt算法的基础上，通过加强以无人艇运动学要求为约束条件的判断，以及保障航行平滑性的剪枝处理，实现满足无人艇实时避障要求的全局优化航路，保障无人艇安全平稳航行。