一种融合海流空间梯度惩罚的无人船路径规划方法

摘要: 本发明涉及无人船智能航行,尤其涉及无人船路径规划,具体是指一种融合海流空间梯度惩罚的无人船路径规划方法。背景技术:1、无人船的自主航行能力直接决定任务执行的效率...
一种融合海流空间梯度惩罚的无人船路径规划方法

本发明涉及无人船智能航行,尤其涉及无人船路径规划,具体是指一种融合海流空间梯度惩罚的无人船路径规划方法。

背景技术:

1、无人船的自主航行能力直接决定任务执行的效率与安全性,无人船路径规划是自主航行系统的核心环节。与陆地移动机器人相比,无人船航行持续受海流作用,顺流与逆流工况下对地航速差异显著,若仅依据几何距离规划路径,会造成实际航行时间与预估结果偏差较大,甚至出现动力不足无法抵达目标点的问题。

2、现有计入海流的a*类路径规划方法,大多直接采用单点栅格海流速度计算通行代价,并未将海流的空间变化特征纳入代价评价,普遍存在以下不足:第一,在洋流交汇、近岸涡旋等流速空间梯度较大的紊流区域,单点流速无法代表局部区域流场特征,航时预估偏差风险高,路径实际执行效果不可控;第二,传统栅格a*输出路径存在大量锯齿状拐点,未充分考虑无人船最小转弯半径等机动约束,跟踪时容易产生较大跟踪误差;第三,面对动态障碍物时多数方案采用全图重规划,计算开销随海域范围增大显著上升,难以满足近海复杂场景实时性需求。

3、部分改进方案采用混合算法或者全局-局部分层规划架构,但存在状态空间膨胀算力高、全局与局部代价评价标准不一致、拼接处航向突变等问题,缺少针对海流空间紊乱特性的代价抑制手段,综合规划效果有限。

4、因此,亟需一种能够在搜索阶段主动绕避高梯度紊流区域、同时兼顾路径机动可行性与动态避障实时性的无人船路径规划方法。

技术实现思路

1、本发明的目的在于提供一种融合海流空间梯度惩罚的无人船路径规划方法,以解决现有海流a*路径规划方法在洋流交汇与近岸涡旋等高梯度紊流区域因单点流速无法表征局部流场而导致航时预估偏差大、栅格离散搜索输出路径存在锯齿拐点且未考虑最小转弯半径约束而导致机动可行性不足、动态障碍物场景下全图重规划计算开销随海域范围增大而显著上升的问题。

2、为解决上述问题,本发明提供一种融合海流空间梯度惩罚的无人船路径规划方法,包括:

3、s1.采集目标海域的海流矢量数据并融合障碍物感知数据,构建栅格化航行环境;

4、s2.构建通行时间代价模型,包括构建包含基础航时、海流空间梯度惩罚项、转向约束代价项、障碍风险代价项的累计代价函数,构建启发函数并结合累计代价函数,获取状态总估价函数;

5、s3.以栅格节点、栅格节点进入航向构成的二元组作为搜索状态,以无人船起点对应的栅格节点为起点,以无人船目标点对应的栅格节点为终点,基于s2构建的通行时间代价模型在栅格化航行环境中执行全局搜索,输出经无人船机动约束校验通过后的全局路径;

6、s4.基于障碍物感知数据,对全局路径执行碰撞检测,计算全局路径中各关键航路点的最小预测碰撞时间,并对最小预测碰撞时间达到碰撞预警阈值的关键航路点执行局部窗口重规划,输出经无人船机动约束校验通过后的初步拟合路径;

7、s5.在初步拟合路径中逐栅格计算海流时变偏差,基于预设的失效比例阈值,提取失效栅格并输出失效增量片段,对失效增量片段执行增量式重规划,输出无人船机动约束校验通过后的无人船规划路径。

8、s1包括:

9、s1.1,采用双线性插值对目标海域进行离散栅格划分,构建栅格流场;

10、s1.2,提取栅格流场中的所有静态障碍物数据,并结合无人船几何尺寸对栅格流场中的静态障碍物执行膨胀处理,在栅格流场中将膨胀处理后的静态障碍物对应的栅格标记为禁航栅格;

11、s1.3,采集栅格流场中各栅格中心点的海流矢量数据,利用中心差分法计算海流速度雅可比矩阵;

12、s1.4,基于海流速度雅可比矩阵计算栅格流场的空间梯度标量,经均值滑动窗口滤波生成海流空间梯度标量场;

13、s1.5,获取动态障碍物感知数据并映射至s1.4获取的海流空间梯度标量场中,基于动态障碍物感知数据动态更新栅格流场的通行格栅,构建栅格化航行环境;

14、s1.4中,空间梯度标量为:

15、;

16、式中,表示栅格中心点的海流速度矢量,表示海流速度雅可比矩阵,表示frobenius范数,表示的第个分量,表示对应的空间坐标,表示基于索引的遍历求和函数。

17、s2中,状态总估价函数为:

18、;

19、;

20、;

21、;

22、式中,表示当前节点索引,表示邻域节点索引,表示当前节点的状态,表示邻域节点的状态,表示邻域节点的状态总估价,表示邻域节点的累计代价,表示当前节点的累计代价,表示从当前节点驶向邻域节点的累计代价函数,表示邻域节点的启发值,表示启发函数,表示当前节点的进入航向,表示邻域节点的进入航向。

23、从当前节点驶向邻域节点的累计代价函数为:

24、;

25、;

26、式中,表示从当前节点驶向邻域节点的单段航时基础值,表示从当前节点驶向邻域节点的转向约束代价项,表示从当前节点驶向邻域节点的障碍风险代价项,表示从当前节点驶向邻域节点的基础航时,表示从当前节点驶向邻域节点的海流空间梯度惩罚项。

27、从当前节点驶向邻域节点的基础航时为:

28、;

29、;

30、式中,表示当前节点与邻域节点的间距,表示从当前节点驶向邻域节点的对地航速,表示极小正数阈值,为最大取值函数,表示无人船对水航速,表示当前节点对应的栅格中心的海流速度矢量,表示垂直航迹单位向量,表示航迹方向单位向量;

31、从当前节点驶向邻域节点的海流空间梯度惩罚项为:

32、;

33、式中,表示梯度惩罚权重系数;

34、从当前节点驶向邻域节点的转向约束代价项为:

35、;

36、式中,表示最大转向角对应的标准操纵耗时系数,表示当前节点的进入航向与邻域节点的进入航向之间的夹角,表示八邻域最大离散转向角;

37、从当前节点驶向邻域节点的障碍风险代价项为:

38、;

39、;

40、;

41、;

42、式中,表示当前节点驶向邻域节点的障碍风险值,表示静态障碍权重,表示以自然数e为底的指数函数,表示邻域节点距离邻域节点最近的障碍物的距离,表示静态障碍高斯尺度系数,表示风险值的中间计算参数,表示障碍物索引,表示邻域节点相对第个障碍物的位置矢量,表示邻域节点的对地位置矢量,表示第个障碍物的对地位置矢量,表示向量的模,表示动态障碍权重,表示动态障碍高斯尺度系数,表示基于索引的最大值遍历搜索函数,表示无人船与第个障碍物的预测碰撞时间,表示时间尺度系数。

43、从当前节点驶向邻域节点的启发函数为:

44、;

45、式中,表示邻域节点m到目标点g的八邻域最短几何距离,表示海流最大顺流速度投影分量。

46、s3中,以栅格节点、栅格节点进入航向构成的二元组为搜索函数,利用通行时间代价模型,以无人船起点为初始点,以无人船目标点为终点,执行八邻域栅格图搜索,构建从无人船起点至无人船目标点的全局路径,包括:

47、s3.1,构建节点列表,并在节点列表中保存无人船起点对应的栅格节点和无人船目标点对应的栅格节点;

48、s3.2,从八个邻域栅格中,选择状态总估价最小的通行栅格作为当前扩展格栅,并当前扩真格栅对应的栅格节点作为扩展节点,将扩展节点保存至节点列表;

49、s3.3,对扩展节点进行终点判断,当判断扩展节点与无人船目标点对应的栅格节点重合时,输出节点列表;当判断扩展节点与无人船目标点对应的栅格节点未重合时,以扩展节点为中心,执行八邻域搜索,计算各邻域节点的状态总估价并返回步骤s3.2;

50、s3.4,重复执行步骤s3.2和步骤s3.3,直至s3.3输出节点列表后,根据节点列表中各栅格节点构建从无人船起点至无人船目标点的离散栅格路径;

51、s3.5,采用道格拉斯普克算法对离散栅格路径执行航路点精简,输出关键航路点;

52、s3.6,以关键航路点为型值点,生成分段三次贝塞尔平滑曲线并施加无人船机动约束,经无人船机动约束检验通过后,输出全局路径。

53、s4中,通过设定预警半径、预警阈值,基于动态障碍物感知数据,对s3.6生成的全局路径执行碰撞检测,输出初步拟合路径,预警阈值包括一级阈值和二级阈值,满足;

54、s4包括:

55、s4.1,以s3.5输出的各关键航路点为中心,在栅格化航行环境中逐点提取与关键航路点之间的距离不大于预警半径的所有栅格节点,构建障碍物碰撞检测环境;

56、s4.2,基于动态障碍物感知数据,对障碍物碰撞检测环境中的所有动态障碍物进行提取,获取各动态障碍物的航向和航速;

57、s4.3,根据动态障碍物的航向和航速、无人船的航向和航速,预测无人船在对应关键航路点处,与各动态障碍物的碰撞时间,并输出最小动态碰撞时间,作为对应关键航路点的最小预测碰撞时间;

58、s4.4,结合最小碰撞时间、一级阈值和二级阈值对关键航路点执行碰撞检测,当大于时,将s3.6获取的全局路径作为初步拟合路径,当不大于时,以扩展节点为中心点设定局部搜索窗口;

59、s4.5,当不大于但大于时,更新局部搜索窗口内的障碍风险值,基于障碍风险值计算偏移量,并对局部搜索窗口内的全局路径对应的关键航路点执行偏移调整,输出局部航行点,并更新局部搜索窗口内的关键航路点;当不大于时,触发局部窗口重规划,根据全局路径与局部搜索窗口的交点,映射得到局部搜索的起点和终点,基于局部搜索的起点和终点,复用通行时间代价模型逐点执行八邻域搜索,输出局部航行点,并更新局部搜索窗口内的关键航路点;

60、s4.6,以局部航行点为型值点,生成分段三次贝塞尔平滑曲线,获取局部路径,将局部路径与局部搜索窗口外的全局路径进行拼接,获取局部拼接路径,并对局部拼接路径执行无人船机动约束校验;

61、s4.7,当s4.6的检验通过时,输出局部拼接路径作为初步拟合路径,当s4.6的检验不通过时,扩大局部搜索窗口后重新执行步骤s4.5和步骤s4.6,直至s4.6获取的局部拼接路径通过无人船机动约束校验后,输出s4.6获取的局部拼接路径作为初步拟合路径;当局部搜索窗口扩大至最大窗口尺寸,局部拼接路径仍无法通过无人船机动约束校验时,在对应航路点处触发停船避险指令。

62、s5包括:

63、s5.1,设定失效比例阈值和超限栅格上限,沿着初步拟合路径逐栅格计算海流时变偏差,将标记为失效栅格,将失效栅格构成的连续片段标记为失效片段;

64、s5.2,将失效片段中失效栅格数量不少于的失效片段标记为失效增量片段;

65、s5.3,当初步拟合路径中,不存在失效增量片段时,输出初步拟合路径作为无人船规划路径,当初步拟合路径中存在失效增量片段时,触发增量式重规划,沿着初步拟合路径在失效片段两端分别扩展2个栅格,构建增量搜索区域;

66、s5.4,为通行时间代价模型的启发函数设置优先系数,利用设定优先系数后的通行时间代价模型,以当前的增量搜索区域的起点栅格为搜索起点,以当前的增量搜索区域外,距离增量搜索区域终点最近的关键航路点为搜索终点,逐点执行八邻域搜索,输出增量航路点,并更新增量搜索区域内的关键航路点;

67、s5.5,以增量航路点为型值点,生成分段三次贝塞尔平滑曲线,获取增量路径,将增量路径与增量搜索区域外的初步拟合路径进行拼接,获取增量拼接路径,并对增量拼接路径执行无人船机动约束校验;

68、s5.6,当s5.5的检验通过时,输出增量拼接路径作为无人船规划路径,当s4.6的检验不通过时,沿着初步拟合路径,在增量搜索区域的两端分别扩展2个栅格,作为补充增量搜索区域,重新执行步骤s5.4和步骤s5.5,当无人船机动约束校验通过时,输出s5.5获取的增量拼接路径作为初步拟合路径;当无人船机动约束校验仍无法通过,返回步骤s4,将局部搜索窗口设置为最大窗口尺寸,执行局部窗口重规划。

69、无人船机动约束为:;表示关键航路点索引,表示第个关键航路点处的曲率半径,表示无人船最小转弯半径。

70、相对比现有技术,本发明具有以下有益效果:

71、本发明通过空间梯度惩罚绕避高梯度紊流区域,通过转向软代价与贝塞尔平滑、最小转弯半径硬校验保证机动可行性,通过局部窗口重规划降低动态避障场景算力、通过增量式重规划降低时变流场修正场景算力,在路径质量、机动可行性与计算效率之间取得了平衡,解决现有海流a*路径规划方法在洋流交汇与近岸涡旋等高梯度紊流区域因单点流速无法表征局部流场而导致航时预估偏差大、栅格离散搜索输出路径存在锯齿拐点且未考虑最小转弯半径约束而导致机动可行性不足、动态障碍物场景下全图重规划计算开销随海域范围增大而显著上升的问题。