一种未知复杂障碍物环境下AUV运动规划方法

本发明专利涉及自主水下航行器，具体描述为一种未知复杂障碍物环境下auv运动规划方法。

背景技术：

1、自主水下航行器(auv)是人类用来探索海洋未知环境的有力工具，在未来，auv的发展空间会越来越大，伴随着研究热点的出现，研究限制也越来越多。运动规划是水下任务的基础，水下环境复杂多变，存在auv无法预先获知的静态、动态障碍物。为了保证水下任务的可执行性，auv需具备良好的局部规划能力，即使在未知复杂障碍物环境中也要具备对目标点的可到达性，对动态障碍物也要能够安全有效地避障。如何安全有效的在未知复杂障碍物环境中实现运动规划是十分必要的；

2、对于现有auv运动规划方法的研究成果进行检索，大部分的auv运动规划方法都基于a*、rrt、apf等路径规划方法，这些方法不能虑auv的动态约束(如速度和环境的实时变化)并实时调整运动，只是规划出到终点无碰撞的路径。基于dwa的方法不仅可以规划路径，还可以考虑auv的动态特性，如速度、加速度、运动学和动力学约束等，规划出可行的运动轨迹。目前基于dwa的方法研究，尚未解决在存在复杂凹形障碍物和陷阱区域的未知环境中，由于评价值局部最优而导致的停滞问题，也没有相关研究能够提高dwa对于动态障碍物的避障性能，不能很好的应用于auv在未知复杂障碍物环境中的运动规划。

技术实现思路

1、针对上述问题，本发明提出一种auv在未知复杂障碍物环境中的运动规划方法，该方法通过以下创新点解决了传统auv运动规划中的局部停滞问题，显著提高了避障有效性和安全性：

2、改进dwa算法：改进速度空间和评价函数，以提高轨迹预测的广度和适应性。

3、rrt算法优化：用于局部避障导引点的规划，加入关键点提取策略以剔除非关键节点，提高auv航行效率。

4、障碍物采样窗口：增强环境感知能力，当无法绕开障碍时，引入改进的rrt规划导引点，避免auv陷入停滞。

5、动态避障策略：根据障碍物运动状态计算避障风险，及时调整路径以规避潜在碰撞。

6、算法融合：将rrt规划的导引点纳入dwa算法的评价体系，实现两种算法的高效结合。

7、包括如下步骤：

8、步骤一：建立auv运动学模型，考虑其运动学特性和性能限制。将定义为auv状态向量，其中{xk,yk}代表auv在k时刻的位置，代表auv艏向角，即auv水平转角。auv运动学方程可以表示为其中：

9、

10、式中：vk代表auv运动速度。

11、步骤二：建立环境感知模型，实时获取和处理周围障碍物信息。选用英国tritech公司的microndst紧凑型数字成像声呐用于地图构建和信息探测，满足以下条件的物体可被auv声呐探测到。

12、(x-x0)2+(y-y0)2≤r2

13、式中：(x0,y0)为auv的坐标；(x,y)为物体坐标；r为探测距离阈值。

14、步骤三：建立障碍物环境模型，以适应多变的水下环境。由静态栅格组成存在半封闭空间的不规则障碍物模型，用于表示海底山脉、峡谷等复杂地形，黑色栅格区域表示障碍物区域，空白区域表示可通行区域。为增加动态避障的安全性，根据动态障碍物的体长将其膨胀为等直径的圆形。

15、步骤四：计算auv在预测周期内的可行运动轨迹。基于运动方程和采样速度预估auv在预测周期内的运动轨迹，预测周期分为多个时刻，两个相邻时刻之间的间隔为一个运动周期，由于间隔较短，因此可以将auv在每个运动周期内的运动简化为线性运动，并根据下式更新轨迹信息。

16、

17、式中：vu，ωu为下一时刻期望的速度和艏向角速度。

18、步骤五：生成auv可达的速度空间。根据auv在预测周期内能达到的速度状态范围建立速度空间，用于采样生成预测轨迹，速度空间如下：

19、vw＝{(vw,ωw)t}

20、其中：

21、

22、式中：vw，ωw分别表示动态窗口内auv速度、艏向角速度；va0，ωa0分别为窗口初始auv速度、艏向角速度；a，α分别为auv最大加速度、最大艏向角加速度；vmax为auv最大速度；t为预测周期；max(·)为最大值函数，min(·)为最小值函数。

23、步骤六：通过综合评价函数，筛选最优轨迹。本发明优化并重设了dwa评价函数，评价函数如下:

24、

25、式中：goal(v,ω)为目标评价函数；velocity(v)为速度评价函数；obstacle(v,ω)为障碍物评价函数；cgoal，cvelocity，cobstacle分别为各评价函数对应的权重系数。

26、本发明的目标评价函数根据轨迹末端auv与目标点的航向差异和距离计算评价值，目标评价函数如下：

27、

28、式中：xa，xgoal分别为预测轨迹末端auv的笛卡尔空间坐标和目标点的笛卡尔空间坐标；分别为预测轨迹末端auv的艏向角、艏向角与目标方向的差角；cd为权重调节系数；atan2(x2-x1)函数返回点x1至点x2的方位角。

29、本发明的速度评价函数根据每条轨迹的平均速度计算评价值，速度评价函数如下：

30、

31、式中：xi-1，xi表示前一时刻和后一时刻auv位置坐标。

32、本发明的障碍物评价函数根据轨迹与障碍物的最短距离计算评价值，距离越远评价值越高，障碍物评价函数如下：

33、obstacle(v,ω)＝min(||xoi-xai||)

34、式中：xai，xoi分别为轨迹中某节点的坐标和据该点最近的障碍物点坐标。

35、步骤七：持续监测停滞风险，及时调整避障策略。在声呐范围内加入采样窗口，用于判断前方是否存在连续的障碍物空间，对是否会陷入停滞进行预测。以auv坐标点位为原点，auv艏向为极轴建立极坐标系，采样窗口的空间为:

36、

37、式中：θw为采样角度；dw为采样距离；θr为开角参数；ρ为距离参数。

38、在此空间内以采样角度θr和距离ρ设置采样线。若采样线被障碍物遮挡，则该采样线被障碍物占用，占用值loi为1，根据每条采样线的占用值计算窗口的占用值：

39、

40、

41、式中：loi为采样线占用值；fo为所有采样线占用值的相反数之和；fa为窗口的占用值，当fa＝1时，认为窗口被障碍物占用，auv运动前向没有可以逃离障碍物的通路，存在陷入停滞的风险。

42、步骤八：存在停滞和碰撞时，运用rrt规划局部避障导引点。为避免规划算法陷入局部停滞，本发明提出将dwa与rrt算法融合，利用rrt的快速性和大范围搜索能力，在有停滞风险时规划避障导引点，引导dwa规划路径及时逃离局部障碍物区域。

43、当存在停滞风险时，改进rrt在已知区域内搜索非障碍物区域节点，判断标准为在已知区域内与全局目标点间无已知障碍物遮挡，搜索到该节点后将该节点设为局部目标点，回溯该点至auv当前位置的路径，将路径中所有连接节点加入导引点集合。若auv到达局部目标点后仍陷于障碍物区域，则将新探知的区域纳入已知区域，重复以上局部rrt搜索过程，直至逃离障碍物区域。

44、在遭遇动态障碍物时，将rrt引入局部，规划动态避障导引点，实现更安全的动态避障。为了提高安全性，将障碍物膨胀为以长度为直径的圆形区域，根据auv的性能指标和动态障碍物的速度、运动方向等状态信息，设立碰撞风险区域、危险区域和安全区域。

45、以端点在动态障碍物区域边界上，与边界相切且与障碍物速度vo方向同向的两条射线作为边界，若auv处于两条射线以内的区域，则存在与动态障碍物相撞的风险，该区域即为碰撞风险区域。对于碰撞危险区域的设置，最大化考虑auv在风险区域内机动所需的时间，包括减速、掉头、加速、转向并逃离风险区域一系列运动过程所需要的时间，根据auv当前速度、最大加速度和障碍物速度计算风险区内auv与障碍物之间的距离阈值ds：

46、

47、式中：do为动态障碍物膨胀圆直径；dp为距离阈值调节参数；tω为auv完成所有转向机动需要的时间，表达式如下：

48、

49、碰撞风险区域内距离障碍物的距离小于等于ds的区域设为碰撞危险区域。当auv位于非危险区域的风险区域时，存在碰撞风险但暂无危险，此时不干预auv的局部规划，当auv进入危险区域边界时则认为仅凭局部规划算法不能安全有效避障，引入改进rrt规划动态避障导引点。rrt局部规划时会裁剪位于危险区域内的搜索节点，当搜索节点处于风险区域之外时停止搜索，将该节点设为动态避障局部目标点，回溯路径节点建立导引点集合。

50、步骤九：优化rrt规划结果。rrt得到初始导引点集合后，其中可能会存在冗余导引点，为提高auv航行效率，引入关键导引点选取规则。在非碰撞前提下删除所有冗余导引点，生成最简导引点集合，用于引导auv脱离障碍物区域。

51、步骤十：判断auv是否到达目标点，如果到达auv停止航行；如果没有到达，返回步骤四。