CN109883427B - 一种基于Dubins路径和概率威胁地图的无人船的避障方法 - Google Patents

一种基于Dubins路径和概率威胁地图的无人船的避障方法 Download PDF

Info

Publication number
CN109883427B
CN109883427B CN201910230111.4A CN201910230111A CN109883427B CN 109883427 B CN109883427 B CN 109883427B CN 201910230111 A CN201910230111 A CN 201910230111A CN 109883427 B CN109883427 B CN 109883427B
Authority
CN
China
Prior art keywords
threat
obstacle
path
circle
unmanned ship
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910230111.4A
Other languages
English (en)
Other versions
CN109883427A (zh
Inventor
杨晓飞
王哲
李德明
李佳敏
黄霖
叶辉
朱志宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN201910230111.4A priority Critical patent/CN109883427B/zh
Publication of CN109883427A publication Critical patent/CN109883427A/zh
Application granted granted Critical
Publication of CN109883427B publication Critical patent/CN109883427B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及无人船路径规划领域，具体地说，是一种基于Dubins路径和概率威胁地图的无人船的避障方法。主要包括获取初始位姿点，终止位姿点，无人船的最小转弯半径以及障碍物在二维的概率威胁地图上的信息。当无人船在运动的过程中遇到障碍物时，根据障碍物威胁概率值的大小去决定是否忽略其威胁或是否要冒着一定的威胁去缩短路径。通过改进的算法设置节点的代价评估函数，通过比较不同路径下其值的大小，去规划出局部最优路径，从而规划出一条比较合理的从初始位姿点到终止位姿点的Dubins路径。

Description

一种基于Dubins路径和概率威胁地图的无人船的避障方法
技术领域
本发明涉及无人船路径规划领域，具体地说，是一种基于Dubins路径和概率威胁地图的无人船的避障方法。
背景技术
随着科技的进步与发展，无人船逐渐步入大众的视野。无人船的路径规划对于无人船的发展显然起着举足轻重的作用。无人船的出现对于海上搜救，水域监视等方面提供了极大的帮助。传统的路径规划通常是在栅格法的基础上进行实施的，这就存在这样一个问题，规划出的路径类似于连续的折线，方向会发生突变。而无人船无法实现方向的突变，它存在一个最小转弯半径。所以栅格法并不能适用于无人船的路径规划，需要换用一个其他的路径规划方法对其进行规划。在无人船执行任务的过程中，遇到的不同的障碍物的威胁大小不同，无需所有都需要规避，此时为了提高无人船的工作效率，就需要在路程与威胁之间进行抉择，合理规划出一条风险低、路程短的路径。
发明内容
为了解决上述技术问题，本发明提供一种基于Dubins路径和概率威胁地图的无人船的避障方法，解决其路径规划问题。
为了实现上述目的，本发明采用的具体技术方案如下：
一种基于Dubins路径和概率威胁地图的无人船的避障方法，主要包括以下步骤：
(1)获取初始位姿点，终止位姿点，无人船的最小转弯半径以及障碍物在二维的概率威胁地图上的信息；
(2)根据初始位姿点和终止位姿点作出初始圆与终止圆，并作出符合初始方向与终止方向的初始圆与终止圆的公切线，判断路径是否受到障碍物的影响；
(3)若无障碍物影响，则选取路径长度最短的路径；若有障碍物优先选取穿过障碍物数量少的路径，从而确定唯一的初始圆与终止圆；
(4)根据障碍物威胁概率值的大小去决定是否忽略其威胁或是否要冒着一定的威胁去缩短路径；
(5)通过改进的算法设置节点的代价评估函数，通过比较相邻两个节点之间代价的大小，规划出一条节点之间最优的路径，完成路径规划。
本发明进一步改进，所述步骤(3)中确定唯一初始圆与终止圆为以下步骤：
步骤1由于无人船在水上的航行距离通常远大于其最小转弯半径，因此，这里只考虑初始圆与终止圆相离的情况；根据初始位姿点、终止位姿点和无人船的最小转弯半径作出对应的2个初始圆与2个终止圆；
步骤2作每一对初始圆与终止圆之间符合初始位姿点方向与终止位姿点方向的切线，一共可以作出4条。判断是否存在不经过障碍圆的路径：若存在，则选取不经过障碍圆的切线并跳转至步骤4，若不存在，则进行步骤3；
步骤3比较每条切线上经过障碍圆数目的多少，选取经过障碍圆数目最少的切线；
步骤4确定选取的切线所对应的初始圆与终止圆为唯一的初始圆与终止圆。
本发明进一步改进，所述步骤(4)中的障碍物威胁概率值是指在障碍物的中心，概率威胁就大，在障碍物的边缘，概率威胁就小，进行路径规划时，存在这样两个威胁概率值的临界点：第一个是无人船可以忽略的最大威胁阈值，当障碍物区域的概率威胁值小于它时，无人船便可以忽略其威胁；另一个是无人船不能承受的最小威胁阈值，当障碍物区域的概率威胁值大于它时，无人船便不能通过这片区域，由于要生成Dubins路径，所以将障碍物的影响范围当做一个圆，为该区域的概率威胁值；以障碍物的中心作半径不同的圆，取每一个圆上的最大概率威胁值作为每一个对应圆弧的概率威胁值。
本发明进一步改进，在从初始节点到终止节点的过程如下：
(1)如果没有障碍物，则不生成节点；如果遇到障碍物，则找初始圆与第一个障碍圆影响范围内的同心圆之间的公切线，由于初始圆是有方向的，每一对初始圆与同心圆只能生成两条路径；
(2)通过比较这些不同的路径的节点的代价，选择一条最优的路径航行，因为路径上的同心圆与终止圆都有方向，所以他们之间只存在一条公切线；
(3)将同心圆上对应的切点当做初始节点；
(4)之后重复上述操作，直到到达终止位姿点。
本发明进一步改进，所述步骤(5)中的改进的A*算法节点的代价评估函数为：
Figure BDA0002005261320000031
其中，n表示待扩展的节点，
Figure BDA0002005261320000032
称为深度因子，n表示从初始节点到当前节点的代价值；
Figure BDA0002005261320000033
称为启发因子，表示从前节点到终止节点的最小路径代价
Figure BDA0002005261320000034
的估计值；当满足时，
Figure BDA0002005261320000035
算法是可以采纳的。
本发明进一步改进，所述节点代价评估函数中，设计深度因子为：
Figure BDA0002005261320000036
其中，M(n)表示无人船从初始节点运动到当前节点所付出的距离的代价，T(n)表示无人船从初始节点运动到当前节点所付出的威胁的代价，αM+αT＝1，他们分别表示距离与威胁在无人船做出选择进行路径规划时所占的权重，即权衡距离与威胁；
其中T(n)的表达式为
Figure BDA0002005261320000037
其中，T(n-1)为从初始点沿相同路径到当前节点n的父节点n-1的威胁条件概率值；
Figure BDA0002005261320000038
为从父节点n-1到节点n，障碍物n影响范围内航线段的直线路径长度Ln的威胁条件概率值加上转弯对应的圆弧距离R的航线段上的威胁条件概率值，因为只有在遇到障碍物时才会面临威胁，所以R与转过的角度以及转弯半径有关；f(n)为节点n处的总的威胁概率密度值；fMAX为权利要求3中提到的无人船可以忽略的最大威胁阈值。
Figure BDA0002005261320000039
表示飞行器从父节点n-1航行到节点n的过程中所受到的威胁。这里为了简便运算，不考虑威胁的时变特性，假定在威胁区概率威胁值由内向外均匀变化，则
Figure BDA00020052613200000310
可以用下式表示：
Figure BDA00020052613200000311
本发明进一步改进，所述节点代价评估函数中，设计启发因子为：
Figure BDA0002005261320000041
其中，hM(n)为路径长度启发因子；hT(n)为威胁程度启发因子；αM+αT＝1，他们分别表示距离与威胁在无人船做出选择进行路径规划时所占的权重，即权衡距离与威胁；
从当前节点qnow生成到达终止位姿点的Dubins路径，如果路径中存在障碍物，则向前方各障碍物分别生成CW和CCW两种类型的避障Dubins路径，找到其中安全的若干条路径对应的位姿和避障方向
Figure BDA0002005261320000042
则
Figure BDA0002005261320000043
其中，L(*)表示从第i个位姿点到第j个位姿点的最短Dubins路径长度，上标d表示避障方向，d＝{CW,CCW}，L(qjqF)为节点的估算代价；
对于威胁程度启发因子hT，由于是将障碍物的概率威胁分布规划为以圆环或圆表示的两个区域，圆表示威胁概率值过大，无人船无法通过区域，圆环表示威胁概率值较大，无人船可以通过但有一定的风险的区域，在圆环区域去一定的距离等间隔作同心圆，做出若干个圆。因此在生成Dubins路径时，只考虑上一个节点与a个同心圆的切线，根据避障方向，可以生成2a条路径，比较各个路径的代价，选出局部最优路径；假定在威胁区概率威胁值由内向外均匀变化，hT可以用下式表示
Figure BDA0002005261320000044
其中，
Figure BDA0002005261320000045
为无人船绕过障碍物n的避障路径长度，L(lnln+1)为无人船进入障碍物n的影响范围内沿直线航行的路径长度，αn为无人船绕过障碍物n时转过的角度，Tn为无人船绕过障碍物n的过程中的概率威胁值。Tn可以用下式表示
Figure BDA0002005261320000046
其中，r为无人船绕过障碍物时的转弯半径，r1为障碍物n概率威胁分布区域最大的半径，r2为障碍物n概率威胁分布区域内圆部分的半径，f(n)为障碍物n概率威胁分布区域圆环部分的概率威胁值。
本发明进一步改进，在从当前节点生成到达终止位姿点的Dubins路径时，路径中存在障碍物，障碍物的概率威胁分布区域的最大半径小于无人船的最小转弯半径时，可以将障碍物的概率威胁分布区域扩大至半径等于无人船的最小转弯半径，增加的区域的概率威胁值为，之后重复权利要求7中的后续步骤，进行路径规划。
本发明进一步改进，规划出邻近两个节点之间的最优路径，所有节点之间的路径共同构成无人船从初始位姿点到达终止位姿点之间的路径，每两个相邻节点之间的最优路径根据代价评估函数得到结果，寻出最佳解。
本发明的有益效果：本发明基于Dubins能够更加快速准确有效的规划出一条风险低、路程短的路径，提高了无人船的工作效率，同时在遇到障碍物时能够智能的分辨出不同的障碍物的威胁大小不同，对于小型威胁障碍物时无需做出规避动作，不仅增强无人船的效率性，并且也具备智能化优点。
附图说明
图1为本发明的流程图；
图2为本发明确定唯一初始圆与终止圆的流程图；
图3为本发明的一个障碍圆的概率威胁值的分布图；
图4为本发明两种类型的避障Dubins路径示意图；
图5为本发明进行避障时相邻两节点之间的步长R的示意图；
图6为本发明避障时规划最优路径的流程图。
具体实施方式
为了加深对本发明的理解，下面将结合实施例对本发明做进一步详细描述，该实施例仅用于解释本发明，并不对本发明的保护范围构成限定。
实施例：如图1所示，一种基于Dubins路径和概率威胁地图的无人船的避障方法。
步骤1获取初始位姿点、终止位姿点和障碍物在地图上的信息。障碍物在二维的地图上的信息是存在威胁概率的。比如，障碍物的中心，概率威胁就大，障碍物的边缘，概率威胁就小。在进行路径规划时，存在这样两个威胁概率值的临界点。第一个是无人船可以忽略的最大威胁阈值，当障碍物区域的概率威胁值小于它时，无人船便可以忽略其威胁。另一个是无人船不能承受的最小威胁阈值，当障碍物区域的概率威胁值大于它时，无人船便不能通过这片区域。f(n)为该区域的概率威胁值。以障碍物的中心作半径不同的圆，取每一个圆上的最大概率威胁值作为每一个对应圆弧的概率威胁值。根据初始位姿点、终止位姿点以及无人船最小转弯半径的限制作出每一对初始圆与终止圆的公切线。由于无人船在水上的航行距离通常远大于其最小转弯半径，因此，这里只考虑初始圆与终止圆相离的情况。判断是否存在不经过障碍物的公切线。若存在，则跳转至步骤5，若不存在，则进行步骤2.
步骤2从这些切线中，选出经过障碍圆数量最少的切线，并将这个切线对应的初始圆与终止圆确定为唯一的初始圆与终止圆。
步骤3以路径上最接近初始圆的障碍物的中心作半径大于禁航区半径，小于影响范围半径的多个半径差值为1米同心圆。并分别作出初始圆与这些同心圆的CW、CCW方向的避障路径。
步骤4计算每条路径的代价评估函数的值。改进的A*算法的代价评估函数W(n)为
Figure BDA0002005261320000061
其中，n表示待扩展的节点，
Figure BDA0002005261320000062
称为深度因子，表示从初始节点到当前节点n的代价值；
Figure BDA0002005261320000063
称为启发因子，表示从前节点n到终止节点的最小路径代
Figure BDA0002005261320000064
的估计值。
当满足
Figure BDA0002005261320000065
时，算法是可以采纳的。
节点代价评估函数中，设计深度因子为
Figure BDA0002005261320000066
其中，M(n)表示无人船从初始节点运动到当前节点所付出的距离的代价，T(n)表示无人船从初始节点运动到当前节点所付出的威胁的代价，αM+αT＝1，他们分别表示距离与威胁在无人船做出选择进行路径规划时所占的权重，即权衡距离与威胁。
其中T(n)的表达式为
Figure BDA0002005261320000071
其中，T(n-1)为从初始点沿相同路径到当前节点n的父节点n-1的威胁条件概率值；
Figure BDA0002005261320000072
为从父节点n-1到节点n，障碍物n影响范围内航线段的直线路径长度Ln的威胁条件概率值加上转弯对应的圆弧距离R的航线段上的威胁条件概率值，因为只有在遇到障碍物时才会面临威胁，所以R与转过的角度以及转弯半径有关；f(n)为节点n处的总的威胁概率密度值；fMAX为权利要求3中提到的无人船可以忽略的最大威胁阈值。
Figure BDA0002005261320000073
表示飞行器从父节点n-1航行到节点n的过程中所受到的威胁。这里为了简便运算，不考虑威胁的时变特性，假定在威胁区概率威胁值由内向外均匀变化，则
Figure BDA0002005261320000074
可以用下式表示：
Figure BDA0002005261320000075
节点代价评估函数中，设计启发因子为
Figure BDA0002005261320000076
其中，hM(n)为路径长度启发因子；hT(n)为威胁程度启发因子；αM+αT＝1，他们分别表示距离与威胁在无人船做出选择进行路径规划时所占的权重，即权衡距离与威胁。
从当前节点qnow生成到达终止位姿点的Dubins路径，如果路径中存在障碍物，则向前方各障碍物分别生成CW和CCW两种类型的避障Dubins路径，找到其中安全的若干条路径对应的位姿和避障方向
Figure BDA0002005261320000077
则
Figure BDA0002005261320000078
其中，L(*)表示从第i个位姿点到第j个位姿点的最短Dubins路径长度，上标d表示避障方向，d＝{CW,CCW}，L(qjqF)为节点的估算代价。
对于威胁程度启发因子hT，由于我们将障碍物的概率威胁分布规划为以圆环或圆表示的两个区域，圆表示威胁概率值过大，无人船无法通过区域，圆环表示威胁概率值较大，无人船可以通过但有一定的风险的区域，在圆环区域去一定的距离等间隔作同心圆，做出若干个圆。因此我们在生成Dubins路径时，只考虑上一个节点与a个同心圆的切线。根据避障方向，可以生成2a条路径。比较各个路径的代价，选出局部最优路径。假定在威胁区概率威胁值由内向外均匀变化，hT可以用下式表示
Figure BDA0002005261320000081
其中，
Figure BDA0002005261320000082
为无人船绕过障碍物n的避障路径长度。L(lnln+1)为无人船进入障碍物n的影响范围内沿直线航行的路径长度，αn为无人船绕过障碍物n时转过的角度，Tn为无人船绕过障碍物n的过程中的概率威胁值。Tn可以用下式表示
Figure BDA0002005261320000083
其中，r为无人船绕过障碍物时的转弯半径，r1为障碍物n概率威胁分布区域最大的半径，r2为障碍物n概率威胁分布区域内圆部分的半径，f(n)为障碍物n概率威胁分布区域圆环部分的概率威胁值。
比较每条路径的代价评估函数的值，从中选出最优路径，并将这条路径与同心圆的切点作为新的初始位姿点。
步骤5生成节点之间的最优路径，判断初始位姿点是否到达终止位姿点。若到达，则路径规划完成。若没到达，则跳转至步骤3。
本实施例中，确定唯一初始圆与终止圆的流程如图2所示
步骤1由于无人船在水上的航行距离通常远大于其最小转弯半径，因此，这里只考虑初始圆与终止圆相离的情况。根据初始位姿点、终止位姿点和无人船的最小转弯半径作出对应的2个初始圆与2个终止圆。
步骤2作每一对初始圆与终止圆之间符合初始位姿点方向与终止位姿点方向的切线，一共可以作出4条。判断是否存在不经过障碍圆的路径。若存在，则选取不经过障碍圆的切线并跳转至步骤4。若不存在，则进行步骤3。
步骤3比较每条切线上经过障碍圆数目的多少，选取经过障碍圆数目最少的切线。
步骤4确定选取的切线所对应的初始圆与终止圆为唯一的初始圆与终止圆。本发明中所提及的障碍物的影响范围障碍圆的概率威胁值的分布如图3所示。
本实施例中，遇到障碍物时CW和CCW两种类型的避障Dubins路径示意图如图4所示。
本实施例中，进行避障时，相邻两节点之间的步长R的示意图如图5所示
本实施例中，避障时规划最优路径的流程图如图6所示。
步骤1以路径上最接近初始圆的障碍物的中心作半径大于禁航区半径，小于影响范围半径的多个半径差值为1米同心圆。分别作初始圆与这些同心圆的切线，并生成CW和CCW两种类型的避障Dubins路径。
步骤2比较所有路径的代价评估函数的值。选取出最优路径，并将最优路径对应的同心圆上的切点作为新的初始位姿点，其对应的同心圆作为新的初始圆。
以上显示和描述了本发明的基本原理、主要特征及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化状态改进，这些变化和改进都在要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (7)

1.一种基于Dubins路径和概率威胁地图的无人船的避障方法,其特性在于,包括以下步骤：
(1)获取初始位姿点Ps,终止位姿点Pf,无人船的最小转弯半径以及障碍物在二维的概率威胁地图上的信息；
(2)根据初始位姿点和终止位姿点作出初始圆与终止圆,并作出符合初始方向与终止方向的初始圆与终止圆的公切线,判断路径是否受到障碍物的影响；
(3)若无障碍物影响,则选取路径长度最短的路径,若有障碍物优先选取穿过障碍物数量少的路径,从而确定唯一的初始圆与终止圆；
(4)根据障碍物威胁概率值的大小去决定是否忽略其威胁或是否要冒着一定的威胁去缩短路径；
(5)通过改进的A*算法设置节点的代价评估函数,通过比较相邻两个节点之间代价的大小,规划出一条节点之间最优的路径,完成路径规划；
在所述步骤(3)中确定唯一初始圆与终止圆的具体流程如下：
流程1、由于无人船在水上的航行距离远大于其最小转弯半径,因此,这里只考虑初始圆与终止圆相离的情况,根据初始位姿点、终止位姿点和无人船的最小转弯半径作出对应的2个初始圆与2个终止圆,
流程2、作每一对初始圆与终止圆之间符合初始位姿点方向与终止位姿点方向的切线,一共作出4条,判断是否存在不经过障碍圆的路径：若存在,则选取不经过障碍圆的切线并跳转至流程4,若不存在,则进行流程3,
流程3、比较每条切线上经过障碍圆数目的多少,选取经过障碍圆数目最少的切线,
流程4、确定选取的切线所对应的初始圆与终止圆为唯一的初始圆与终止圆；
所述步骤(4)中的障碍物威胁概率值是指在障碍物的中心,概率威胁就大,在障碍物的边缘,概率威胁就小,进行路径规划时,存在这样两个威胁概率值的临界点：第一个是无人船可以忽略的最大威胁阈值,当障碍物区域的概率威胁值小于它时,无人船便可以忽略其威胁；另一个是无人船不能承受的最小威胁阈值,当障碍物区域的概率威胁值大于它时,无人船便不能通过这片区域,由于要生成Dubins路径,所以将障碍物的影响范围当做一个圆,为该区域的概率威胁值；以障碍物的中心作半径不同的圆,取每一个圆上的最大概率威胁值作为每一个对应圆弧的概率威胁值。
2.根据权利要求1所述的一种基于Dubins路径和概率威胁地图的无人船的避障方法,其特征在于,在从初始节点到终止节点的过程如下：
(1)如果没有障碍物,则不生成节点,如果遇到障碍物,则找初始圆与第一个障碍圆影响范围内的同心圆之间的公切线,由于初始圆是有方向的,每一对初始圆与同心圆只能生成两条路径；
(2)通过比较这些不同的路径的节点的代价,选择一条最优的路径航行,因为路径上的同心圆与终止圆都有方向,所以他们之间只存在一条公切线；
(3)将同心圆上对应的切点当做初始节点；
(4)之后重复上述操作,直到到达终止位姿点。
3.根据权利要求1所述的一种基于Dubins路径和概率威胁地图的无人船的避障方法,其特征在于,所述步骤(5)中的改进的A*算法节点的代价评估函数为：
Figure FDA0004054426710000021
其中,n表示待扩展的节点,
Figure FDA0004054426710000022
称为深度因子,W(n)表示从初始节点到当前节点的代价值,
Figure FDA0004054426710000023
称为启发因子,表示从当前节点到终止节点的最小路径代价
Figure FDA0004054426710000024
的估计值,当满足时,
Figure FDA0004054426710000025
算法是可以采纳的。
4.根据权利要求3所述的一种基于Dubins路径和概率威胁地图的无人船的避障方法,其特征在于,所述节点代价评估函数中,设计深度因子为：
Figure FDA0004054426710000026
其中,M(n)表示无人船从初始节点运动到当前节点所付出的距离的代价,T(n)表示无人船从初始节点运动到当前节点所付出的威胁的价,αM+αT＝1,他们分别表示距离与威胁在无人船做出选择进行路径规划时所占的权重,即权衡距离与威胁,其中T(n)的表达式为：
Figure FDA0004054426710000031
其中,T(n-1)为从初始点沿相同路径到当前节点n的父节点n-1的威胁条件概率值,TsL(n)为从父节点n-1到节点n,障碍物n影响范围内航线段的直线路径长度Ln的威胁条件概率值加上转弯对应的圆弧距离R的航线段上的威胁条件概率值,因为只有在遇到障碍物时才会面临威胁,所以,R与转过的角度以及转弯半径有关,f(n)为节点n处的总的威胁概率密度值,fMAX为无人船可以忽略的最大威胁阈值,
Figure FDA0004054426710000032
表示飞行器从父节点n-1航行到节点n的过程中所受到的威胁,不考虑威胁的时变特性,假定在威胁区概率威胁值由内向外均匀变化,则
Figure FDA0004054426710000033
可以用下式表示：
Figure FDA0004054426710000034
5.根据权利要求4中的一种基于Dubins路径和概率威胁地图的无人船的避障方法,其特征在于所述节点代价评估函数中,设计启发因子为：
Figure FDA0004054426710000035
其中,hM(n)为路径长度启发因子,hT(n)为威胁程度启发因子,αM+αT＝1,他们分别表示距离与威胁在无人船做出选择进行路径规划时所占的权重,即权衡距离与威胁,从当前节点qnow生成到达终止位姿点的Dubins路径,如果路径中存在障碍物,则向前方各障碍物分别生成CW和CCW两种类型的避障Dubins路径,找到其中安全的若干条路径对应的位姿和避障方向
Figure FDA0004054426710000036
则
Figure FDA0004054426710000037
其中,L(*)表示从第i个位姿点到第j个位姿点的最短Dubins路径长度,上标d表示避障方向,d＝{CW,CCW},L(qjqF)为节点的估算代价,对于威胁程度启发因子hT,由于是将障碍物的概率威胁分布规划为以圆环或圆表示的两个区域,圆表示威胁概率值过大,无人船无法通过区域,圆环表示威胁概率值较大,无人船可以通过但有一定的风险的区域,在圆环区域取一定的距离等间隔作同心圆,做出若干个圆,因此在生成Dubins路径时,只考虑上一个节点与a个同心圆的切线,根据避障方向,可以生成2a条路径,比较各个路径的代价,选出局部最优路径,假定在威胁区概率威胁值由内向外均匀变化,hT可以用下式表示：
Figure FDA0004054426710000041
其中,
Figure FDA0004054426710000042
为无人船绕过障碍物n的避障路径长度,L(lnln+1)为无人船进入障碍物n的影响范围内沿直线航行的路径长度,αn为无人船绕过障碍物n时转过的角度,Tn为无人船绕过障碍物n的过程中的概率威胁值,Tn可以用下式表示：
Figure FDA0004054426710000043
其中,r为无人船绕过障碍物时的转弯半径,r1为障碍物n概率威胁分布区域最大的半径,r2为障碍物n概率威胁分布区域内圆部分的半径,f(n)为障碍物n概率威胁分布区域圆环部分的概率威胁值。
6.根据权利要求5所述的一种基于Dubins路径和概率威胁地图的无人船的避障方法,其特征在于,在从当前节点生成到达终止位姿点的Dubins路径时,路径中存在障碍物,障碍物的概率威胁分布区域的最大半径小于无人船的最小转弯半径时,将障碍物的概率威胁分布区域扩大至半径等于无人船的最小转弯半径,增加的区域的概率威胁值为0,之后重复执行所述的设计启发因子的步骤。
7.根据权利要求6所述的一种基于Dubins路径和概率威胁地图的无人船的避障方法,其特征在于,规划出邻近两个节点之间的最优路径,所有节点之间的路径共同构成无人船从初始位姿点到达终止位姿点之间的路径,每两个相邻节点之间的最优路径根据代价评估函数得到结果,寻出最佳解。
CN201910230111.4A 2019-03-25 2019-03-25 一种基于Dubins路径和概率威胁地图的无人船的避障方法 Active CN109883427B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910230111.4A CN109883427B (zh) 2019-03-25 2019-03-25 一种基于Dubins路径和概率威胁地图的无人船的避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910230111.4A CN109883427B (zh) 2019-03-25 2019-03-25 一种基于Dubins路径和概率威胁地图的无人船的避障方法

Publications (2)

Publication Number Publication Date
CN109883427A CN109883427A (zh) 2019-06-14
CN109883427B true CN109883427B (zh) 2023-04-07

Family

ID=66934160

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910230111.4A Active CN109883427B (zh) 2019-03-25 2019-03-25 一种基于Dubins路径和概率威胁地图的无人船的避障方法

Country Status (1)

Country Link
CN (1) CN109883427B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110297500A (zh) * 2019-06-28 2019-10-01 天津大学 一种给定多航路点下无人机的连续航迹规划方法
CN113515109B (zh) * 2021-04-16 2024-04-09 广东工业大学 一种模拟海洋动态不确定环境的航行器路径规划方法
CN113296505B (zh) * 2021-05-19 2022-10-25 华南理工大学 一种基于速变los的无人船多模式路径跟踪控制方法
CN113625723B (zh) * 2021-08-22 2022-05-27 广东海洋大学 一种无人船动态避碰控制系统
CN115686069A (zh) * 2022-11-15 2023-02-03 杭州国科骏飞光电科技有限公司 一种无人机集群的同步协调控制方法及系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103676944A (zh) * 2013-12-11 2014-03-26 北京理工大学 基于Dubins路径和稀疏A*搜索的无人机航迹规划方法
CN108536149A (zh) * 2018-04-25 2018-09-14 吉林大学 一种基于Dubins路径的无人驾驶车辆避障控制装置及控制方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10663966B2 (en) * 2017-03-29 2020-05-26 Mitsubishi Electric Research Laboratories, Inc. Vehicle motion control system and method

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103676944A (zh) * 2013-12-11 2014-03-26 北京理工大学 基于Dubins路径和稀疏A*搜索的无人机航迹规划方法
CN108536149A (zh) * 2018-04-25 2018-09-14 吉林大学 一种基于Dubins路径的无人驾驶车辆避障控制装置及控制方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
An Approximation Path Planning Algorithm for Fixed-wing UAVs in Stationary Obstacle Environment;WANG Zhong，et al.;《Proceedings of the 33rd Chinese Control Conference》;20140730;全文 *
基于Dubins曲线和改进A＊算法的AUV路径规划方法;胡蔷等;《计算机测量与控制》;20160825;第24卷(第08期);259-262 *
基于Dubins路径的A~*算法的多无人机路径规划;宋雪倩等;《电光与控制》;20180825(第11期);全文 *

Also Published As

Publication number Publication date
CN109883427A (zh) 2019-06-14

Similar Documents

Publication Publication Date Title
CN109883427B (zh) 一种基于Dubins路径和概率威胁地图的无人船的避障方法
CN112904842B (zh) 一种基于代价势场的移动机器人路径规划与优化方法
CN112327885B (zh) 一种无人船自适应全局-局部混合路径规划的方法
CN109933067B (zh) 一种基于遗传算法和粒子群算法的无人艇避碰方法
Xie et al. A path planning approach based on multi-direction A* algorithm for ships navigating within wind farm waters
CN109597417B (zh) 一种基于避碰准则的多usv群体协同避碰规划方法
CN112327850B (zh) 一种水面无人艇路径规划方法
Cheng et al. A genetic algorithm-inspired UUV path planner based on dynamic programming
CN110470299A (zh) 一种基于圆形路障规避的海上航路规划算法
EP3633318B1 (en) An apparatus for determining an optimal route of a maritime ship
CN113032896B (zh) 一种基于船舶驾驶员偏好的避碰辅助决策方法
Song et al. T $^\star $: Time-Optimal Risk-Aware Motion Planning for Curvature-Constrained Vehicles
Tsou Integration of a geographic information system and evolutionary computation for automatic routing in coastal navigation
CN113050684B (zh) 一种面向突发威胁的无人机航迹规划算法
CN111881580A (zh) 一种无人船规避障碍物的运动规划方法
Lee et al. Multi-resolution path planning for marine surface vehicle considering environmental effects
Blaich et al. Extended grid based collision avoidance considering COLREGs for vessels
Veneti et al. An evolutionary approach to multi-objective ship weather routing
CN113538973A (zh) 一种基于改进粒子群算法的船舶自动避碰方法
Vahid et al. Optimal path planning for unmanned surface vehicle using new modified local search ant colony optimization
CN116952239A (zh) 一种基于改进a*与dwa融合的无人艇路径规划方法
CN114019967B (zh) 一种适用于狭长航道的无人艇航线规划方法
Li et al. Dynamic algorithm for safe and reachable frontier point generation for robot exploration
CN114967696A (zh) 一种无人艇全局路径规划方法
CN115951682A (zh) 一种考虑洋流存在条件下的带约束四目标auv全局路径规划方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
EE01 Entry into force of recordation of patent licensing contract
EE01 Entry into force of recordation of patent licensing contract

Application publication date: 20190614

Assignee: CSIC PRIDE (NANJING) ATMOSPHERE MARINE INFORMATION SYSTEM Co.,Ltd.

Assignor: JIANGSU University OF SCIENCE AND TECHNOLOGY

Contract record no.: X2022320000094

Denomination of invention: An obstacle avoidance method for unmanned ships based on dubins path and probability threat map

License type: Common License

Record date: 20220609

CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Yang Xiaofei

Inventor after: Wang Zhe

Inventor after: Li Deming

Inventor after: Li Jiamin

Inventor after: Huang Lin

Inventor after: Ye Hui

Inventor after: Zhu Zhiyu

Inventor before: Wang Zhe

Inventor before: Li Deming

Inventor before: Li Jiamin

Inventor before: Huang Lin

Inventor before: Yang Xiaofei

Inventor before: Ye Hui

Inventor before: Zhu Zhiyu

GR01 Patent grant
GR01 Patent grant