一种动力学约束下基于采样的无人艇安全轨迹生成方法

本发明涉及无人艇路径规划领域，特别是一种动力学约束下基于采样的无人艇安全轨迹生成方法。

背景技术：

1、海洋具有多种多样的生物资源，对地球生态系统以及人类社会的繁荣发展都具有不可替代的重要作用。因此，保护海洋生态环境、合理开发利用海洋资源以及减少海洋环境污染至关重要。无人艇因其体型小，灵活性高，成本低，自主性强等优点被广泛应用于海洋资源勘探开发、海洋安全保障以及灾害响应救援等领域。

2、近年来无人艇路径规划问题受到国内外相关学者的广泛关注。为了在复杂的海洋作业环境下生成无碰撞轨迹，大量的无人艇全局路径规划算法被提出。hanguen kim针对无人艇初始位置航向角约束以及角速度约束，提出了一种基于theta*算法的路径规划方法，在考虑无人艇航向信息的情况下实时生成具有高精度角度分辨率的路径，降低了无人艇运动的时间成本和路径成本。hanlin niu针对海流影响下无人艇海上续航问题，将voronoi图、可见性图、能量消耗函数与dijkstra算法相结合，使无人艇在满足避碰的条件下实现了从起点到终点的路径规划，提高了能量利用率与路径质量。dimitrios v.lyridis针对复杂海洋环境下无人艇多模态约束与多目标优化问题，提出了一种改进的模糊逻辑蚁群优化算法，在考虑风、浪、流的扰动下提高了路径质量与收敛速度。xinghai guo针对无人艇导航问题，提出一种混沌共享学习粒子群优化算法来求解扩展旅行商问题与非线性多目标模型。在考虑避碰规则的前提下快速规划出有效的全局路径。jiabin yu针对空间变化洋流，在rrt*算法中引入虚拟场采样算法和洋流约束函数，在解决洋流影响的前提下减少了无人艇航行时间。上述算法可分为基于搜索的路径规划算法，智能仿生路径规划算法和基于采样的路径规划算法。

3、在复杂的海洋环境中，基于搜索的路径规划算法和智能仿生路径规划算法搜索路径时需要强大的算力，这是无人船的板载计算机难以实现的。所以人们普遍采用基于采样的轻量级算法进行路径规划，其中最常见的且变种最多的为rrt算法。而以往基于rrt的路径规划方法的优化方向主要分为两种，第一种是在不满足无人艇动力学约束和路径平滑的前提下提高采样效率以缩短路径搜索时间，但无法使无人艇在满足自身动力学约束下的实际运动轨迹无碰撞。第二种是在不考虑计算需求的情况下对全局路径进行变形得到无碰撞的路径，其缺点是在路径变形之后容易产生不必要的路径段。

4、综上，现有无人艇全局路径规划算法存在如下不足：

5、第一，现有基于搜索的路径规划算法和智能仿生路径规划算法在搜索路径时大多对算力要求高。然而，在复杂的海洋作业环境下，仅靠板载计算机无法快速有效的规划出满足无人艇需求的安全轨迹。

6、第二，现有基于采样的无人艇路径规划算法大多注重缩短路径搜索时间或者减短路径长度。而在实际应用中，无人艇在运动中受限于其速度、航向、路径曲率等动力学约束的影响，难以完美跟踪预设的安全无碰撞轨迹。因此在靠近障碍物的非安全区域内，无人艇存在一定的碰撞风险。

7、第三，现有基于采样的无人艇路径规划算法在考虑避碰时，得到的全局路径大多会因为路径变形的随机性产生不必要的路径段，导致无人艇产生多余的转弯动作，降低了实际路径的质量。

技术实现思路

1、为了解决上述问题，本发明采用的技术方案是：一种动力学约束下基于采样的无人艇安全轨迹生成方法，包括以下步骤：

2、基于无人艇的实际航迹角ψ与无人艇艏摇角速度为ω，设计侧滑角速度观测器，输出无人艇侧滑角速度的估计值

3、基于障碍物坐标qo及半径ro，起始点坐标qs以及目标点坐标qg，设计基于碰撞检测的路径规划算法，首先采用rrt算法搜索一条从起点qs到终点qg的路径作为初始路径；

4、对该初始路径进行剪枝操作，去除掉增加无人艇曲率的冗余节点，得到剪枝后的无碰撞路径的点集合ωfcut；

5、基于无人艇相对于目标点qtarget的距离ρ、无人艇航迹角ψ、无人艇期望朝向θ以及无人艇侧滑角速度的估计值设计位姿控制器；

6、基于剪枝后的无碰撞路径的点集合ωfcut，输入到位姿控制器中，得到预测的加速度和角加速度；

7、基于预测的加速度和角加速度，输出到无人艇运动学模型，得到无人艇的预测轨迹；

8、基于无人艇的预测轨迹与路径变形策略，得到减枝后的无碰撞路径点集合ωfinal；

9、减枝后的无碰撞路径点集合ωfinal输入到位姿控制器中，得到实际加速度和角加速度；

10、基于实际的加速度和角加速度，输出到无人艇运动学模型，得到无人艇的安全轨迹。

11、进一步地，所述侧滑角速度观测器设计如下：

12、

13、其中，k1＞0,k2＞0，分别为无人艇侧滑角速度观测器的常值系数，为无人艇侧滑角速度的估计值，为无人艇的实际航迹角的估计值，表示艏摇角速度；ψ是无人艇的实际航迹角。

14、进一步地：所述位姿控制器包括参考速度、参考角速度模块和加速度模块和角加速度模块；

15、所述参考速度、参考角速度模块设计如下：

16、uref＝kρtanh(kυρ)                    (4)

17、

18、其中：kρ＞0，kυ＞0，且满足为当前位置到目标点qtarget的欧几里得距离，其中，xtarget，ytarget分别为目标点qtarget的横坐标与纵坐标，α＝ψ-θ为无人艇朝向与无人艇和目标位置连线之间的夹角，且θ＝a tan2(ytarget-y,xtarget-x)为无人艇和目标位置连线与全局坐标系x轴之间的夹角，uref是无人艇实际合速度，ωref是无人艇艏摇角速度。

19、进一步地：所述加速度模块设计如下：

20、如果：u≤uref且u＜umax：

21、

22、如果：u≤uref且u＝umax：

23、a＝0

24、如果：u>uref且u>-umax：

25、

26、如果：u>uref且u＝-umax：

27、a＝0      (6)

28、其中，u为无人艇当前时刻的合速度，umax为无人艇合速度的最大值，t为步长。

29、进一步地：所述角加速度模块设计如下：

30、如果：ω≤ωref且ω＜ωmax：

31、

32、如果：ω≤ωref且ω＝ωmax：

33、

34、如果：ω>ωref且ω>-ωmax：

35、

36、如果：ω>ωref且ω＝-ωmax：

37、

38、其中，ω为无人艇当前时刻的艏摇角速度，ωmax为无人艇艏摇角速度的最大值。

39、进一步地：所述基于碰撞检测的路径规划算法设计如下：

40、已知障碍物坐标qo及半径ro，起始点坐标qs以及目标点坐标qg，采用rrt算法搜索一条从起点qs到终点qg的路径作为初始路径的过程如下：

41、步骤s11：初始化地图，并将障碍物坐标qo及半径ro，起始点坐标qs以及目标点坐标qg导入；

42、步骤s12：初始化全部采样路径点集合ω为空集，初始化从起始点到目标点无碰撞路径的点集合ωf为空集，其中ωf∈ω；

43、步骤s13：移除的集合ω内的元素集ωf，并将qs加入集合ω；

44、步骤s14：初始化步长lstep与概率p；

45、步骤s15：采样得到随机点qrand，有p概率将全图随机点坐标赋值给qrand，有1-p概率将目标点qg坐标赋值给qrand；

46、步骤s16：qnear代表ω中距离qrand最近点，将集合ω中距离qrand最近点的值赋值给qnear，得到新采样点qnew，qnew满足：其中步长ldis_average为qrand到各障碍物圆心距离的平均值；

47、步骤s17：若新采样点qnew与qnear的连线与障碍物o碰撞，碰撞点为qcollision，则将新采样点qnew赋值为：若新采样点qnew与qnear的连线不与障碍物o碰撞，则无需修改qnew的值；

48、步骤s18：将新采样点qnew加入集合ω；

49、步骤s19：ε表示目标区域的容忍度系数，且ε＞0，若||qnew-qg||＞ε，则认为没有到达目标点，返回步骤s15重新执行循环，若||qnew-qg||≤ε，则认为到达目标点，结束步骤s15-步骤s18的循环，从qnew开始索引父节点，直至父节点为qs，将所有索引节点加入无碰撞路径的点集合ωf，并返回ωf。

50、进一步地：所述对该初始路径进行剪枝操作，去除掉增加无人艇曲率的冗余节点，得到剪枝后的无碰撞路径的点集合ωfcut的过程如下：

51、步骤s21：将ωf元素个数赋值给n；

52、步骤s22：定义qi为第i个无碰撞路径点，i∈{2,3,...,n-1}，i＝2；

53、步骤s23：如果集合ωf中qi-1→qi+1的线段与障碍物无碰撞，则将qi从ωf移除，否则无动作，i＝i+1；

54、步骤s24：如果i＞n-1，返回剪枝后的无碰撞路径的点集合ωfcut，如果i≤n-1，则返回步骤s23重新执行循环。

55、进一步地：所述基于剪枝后的无碰撞路径的点集合ωfcut，输入到位姿控制器中，得到预测的加速度和角加速度；基于无人艇的预测轨迹与路径变形策略，得到减枝后的无碰撞路径点集合ωfinal；

56、步骤s31：将ωfcut元素个数赋值给n2；

57、步骤s32：定义qj为第j个无碰撞路径点，j∈{1,2,3,...,n2-1}，j＝1；

58、步骤s33：将起点qj，目标点qj+1输入到位姿控制器中，得到预测合加速度apj与预测艏摇角加速度输出到无人艇运动学模型，得到qj→qj+1的预测轨迹；

59、步骤s34：如果集合ωfcut中qj→qj+1的轨迹与障碍物无碰撞，则j＝j+1，否则返回碰撞点qcollision，重新修正ωfcut中距离碰撞点最近的点qx为：其中qx∈{qj,qj+1}，η表示膨胀系数，且η＞0，j＝j；

60、步骤s35：如果j＞n-1，返回剪枝后的无碰撞路径点集合ωfinal，否则返回步骤s33重新执行循环。

61、本发明提供的一种动力学约束下基于采样的无人艇安全轨迹生成方法，具有以下优点：

62、与现有技术相比，本发明具有以下有益效果：

63、第一，与现有的基于搜索的路径规划算法和智能仿生路径规划算法相比，本发明所设计的动力学约束下基于采样的无人艇安全轨迹生成方法对地图进行启发性采样，在搜索更少节点的前提下更快导向终点。消耗更少的计算机算力与内存，实现了算法的轻量化。

64、第二，与现有基于采样的无人艇路径规划算法相比，本发明采用满足无人艇动力学约束的位姿控制器对生成轨迹进行碰撞预检测，并对距离碰撞点最近路径点进行定向微调以避免碰撞发生，同时采用侧滑角速度观测器对侧滑角速度进行补偿。使得实际轨迹与预设轨迹重合度更高且满足避碰约束。

65、第三，与现有基于采样的无人艇路径规划算法相比，本方法在采样得到完整路径后进行剪枝以去掉非必要的路径点，减少了无人艇不必要的转弯动作，提高了实际路径的质量。