CN114625150A - 基于危险指数和距离函数的快速蚁群无人艇动态避障方法 - Google Patents

基于危险指数和距离函数的快速蚁群无人艇动态避障方法 Download PDF

Info

Publication number
CN114625150A
CN114625150A CN202210532561.0A CN202210532561A CN114625150A CN 114625150 A CN114625150 A CN 114625150A CN 202210532561 A CN202210532561 A CN 202210532561A CN 114625150 A CN114625150 A CN 114625150A
Authority
CN
China
Prior art keywords
path
obstacle
ant colony
unmanned ship
distance
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.)
Granted
Application number
CN202210532561.0A
Other languages
English (en)
Other versions
CN114625150B (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.)
Nanjing Huihe Information Technology Co ltd
Original Assignee
Nanjing Huihe Information Technology Co ltd
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 Nanjing Huihe Information Technology Co ltd filed Critical Nanjing Huihe Information Technology Co ltd
Priority to CN202210532561.0A priority Critical patent/CN114625150B/zh
Publication of CN114625150A publication Critical patent/CN114625150A/zh
Application granted granted Critical
Publication of CN114625150B publication Critical patent/CN114625150B/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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

Abstract

本发明公开了一种基于危险指数和距离函数的快速蚁群无人艇动态避障方法，采用改进蚁群算法进行全局路径规划，得到起始位置到目标点的可行路径；利用动态窗口法实现局部路径规划，以蚁群算法关键节点为子目标位置进行局部实时避障，直至到达目的点。本发明综合考虑了路径规划安全性和收敛速度问题；改进蚁群算法转移概率，加入危险指数，保证全局规划路径的安全可靠；为了快速实时避障，在动态窗口法评价函数上加入了路径融合距离函数；实现无人艇在复杂水域环境下动态避障，保证无人艇安全可靠地到达目的地。

Description

基于危险指数和距离函数的快速蚁群无人艇动态避障方法
技术领域
本发明属于路径规划算法技术领域，具体涉及一种基于危险指数和距离函数的快速蚁群无人艇动态避障方法。
背景技术
水面无人艇（Unmanned Surface Vessels，USV）可以实现巡航、监测等功能，在军事和民用领域受到了越来越广泛的应用。其中路径规划是无人艇研究领域的一项重要技术，是无人艇保证自身安全，实现自主巡航的基础。
无人水面艇的路径规划可以分为全局路径规划算法和局部路径规划算法。全局路径规划需要提前获取整个水域的环境信息，并在该区域内进行规划；常用的有Dijkstra算法、A*算法、蚁群算法、遗传算法、粒子群优化算法等。其中Dijkstra算法、A*算法算法复杂度低，但无法很好的处理复杂的路径信息；而蚁群算法是一种智能仿生学的路径规划算法，具有较好的鲁棒性、规划速度也有了一定的提升。
局部路径规划是指在全局未知或者部分未知的水域环境下通过硬件设备自主获取一定范围内的环境信息；常见的局部路径规划算法主要有动态窗口法、人工势场法、快速扩展随机树算法、速度障碍法等。其中人工势场法多用于对静态障碍物进行避障，若目标位置周围不存在障碍物时，具有算法速度较快。动态窗口法能根据无人艇的运动状态信息，并结合无人艇传感器信息实时分析周边障碍物情况，实现了对障碍物的动态避障能力。
本发明采用的路径规划算法是将全局路径规划算法与局部路径规划算法相结合，基于安全蚁群算法和动态窗口法融合的无人艇动态平滑避障算法，该避障算法能在保证无人艇自身安全的前提下较快地实现平滑处理动态障碍物，可以实现基于全局信息的最优可行解。
发明内容
发明目的：针对上述现有技术的不足，本发明提一种基于危险指数和距离函数的快速蚁群无人艇动态避障方法，解决在复杂场景下动态平滑回归避障，规划路径难以保持其路径安全性的问题。
技术方案：本发明所述的一种基于危险指数和距离函数的快速蚁群无人艇动态避障方法，包括如下步骤：
（1）对预先采集到的真实水域地图信息进行栅格化处理，并保存栅格化后的地图信息，确定无人艇航行初始地点和目标点；
（2）在栅格化地图上，启动蚁群算法，根据加入危险系数的转移概率进行全局路径的规划，储存其路径拐点坐标信息，并启动信息素更新策略；
（3）对栅格化地图进行障碍物检测，若发现动态障碍物，则以最近的拐点作为子目标点；如果子目标点在障碍物上，将下一个拐点作为子目标点；并利用加入路径融合距离函数后的动态窗口法进行动态避障；
（4）对步骤（3）寻找子目标点进行循环，直至局部路径规划子目标点为全局路径规划的目标点，则表明该避障算法结束，输出全局最优安全路径。
进一步地，步骤（1）所述栅格化后的地图信息能够显示障碍物以及周围环境信息，每一个栅格用唯一的序列号与其位置坐标相对应，其对应关系为：
Figure 717072DEST_PATH_IMAGE001
其中，
Figure 774896DEST_PATH_IMAGE002
为第
Figure 926392DEST_PATH_IMAGE003
个栅格的位置坐标；
Figure 252331DEST_PATH_IMAGE003
为第
Figure 290825DEST_PATH_IMAGE003
个网格的序号；
Figure 676807DEST_PATH_IMAGE004
是冗余运算符；
Figure 925386DEST_PATH_IMAGE005
表示取整操作；
Figure 179650DEST_PATH_IMAGE006
和
Figure 462863DEST_PATH_IMAGE007
分别为行方向和列方向的栅格数；
Figure 128069DEST_PATH_IMAGE008
为比例系数。
进一步地，步骤(2)所述加入危险系数的转移概率为：
Figure 129523DEST_PATH_IMAGE009
其中，
Figure 797265DEST_PATH_IMAGE010
为
Figure 325198DEST_PATH_IMAGE011
时刻原始蚁群算法的信息素浓度，
Figure 787403DEST_PATH_IMAGE012
是启发函数，表示
Figure 885940DEST_PATH_IMAGE011
时 刻当前网络位置
Figure 622952DEST_PATH_IMAGE013
移动到第
Figure 615179DEST_PATH_IMAGE014
个栅格的路径启发式信息；
Figure 638499DEST_PATH_IMAGE015
为自适应障碍抑制因子，取值 范围为
Figure 348966DEST_PATH_IMAGE016
；加入危险指数DI，将无人艇和障碍物质心之间的距离、无人艇相对障碍物矢面 的惯量因子作为碰撞严重程度的主要影响，计算方法如下：
Figure 624089DEST_PATH_IMAGE017
其中，
Figure 844724DEST_PATH_IMAGE018
为无人艇和障碍物质心之间的距离影响因子，
Figure 179890DEST_PATH_IMAGE019
是距离 比例因子；
Figure 236708DEST_PATH_IMAGE020
、
Figure 315522DEST_PATH_IMAGE021
分别是无人艇质心接近障碍物质心所允许的最小距离和对无人艇安 全产生影响的最大距离；无人艇相对障碍物矢面的惯量影响因子：
Figure 282341DEST_PATH_IMAGE022
其中，
Figure 398196DEST_PATH_IMAGE023
为障碍物在基坐标系下的惯量矩阵的最大主转动惯量在无人艇矢状面上 的分量，
Figure 83255DEST_PATH_IMAGE024
为最大允许转动惯量，当
Figure 700181DEST_PATH_IMAGE023
超过最大允许转动惯量时，若同障碍物发生潜在碰 撞，无人艇将受到严重冲击；
将路径规划阶段总的危险指数定义为上述距离影响因子和惯量影响因子之积：
Figure 646141DEST_PATH_IMAGE025
当危险指数DI比大时，则自适应障碍抑制因子
Figure 57531DEST_PATH_IMAGE015
趋近于0，表示无人艇和障碍物 之间的危险性较大；当危险指数DI比小时，则
Figure 229886DEST_PATH_IMAGE015
趋近于1，表示无人艇和障碍物之间较为安 全。
进一步地，步骤(2)所述信息素更新策略加入自适应挥发因子，具体如下：
Figure 24404DEST_PATH_IMAGE026
其中，
Figure 700236DEST_PATH_IMAGE027
为蚂蚁
Figure 548107DEST_PATH_IMAGE028
在路径
Figure 66813DEST_PATH_IMAGE029
上的信息素浓度；
Figure 759962DEST_PATH_IMAGE030
为信息素强度；
Figure 696825DEST_PATH_IMAGE031
为各个 路径的最优指标；
Figure 450018DEST_PATH_IMAGE032
为区域内最优路径长度；
Figure 596965DEST_PATH_IMAGE033
为转移概率搜索时的转弯次数；
Figure 218439DEST_PATH_IMAGE034
和
Figure 134443DEST_PATH_IMAGE035
分别 为区域内最优路径长度和转弯次数的重要程度指标；
Figure 324116DEST_PATH_IMAGE036
其中，
Figure 801102DEST_PATH_IMAGE037
为
Figure 101634DEST_PATH_IMAGE038
时刻原始蚁群算法的信息素浓度，
Figure 137723DEST_PATH_IMAGE039
为当前 迭代路径
Figure 357351DEST_PATH_IMAGE029
上信息素的增量，加入自适应信息素挥发因子
Figure 213312DEST_PATH_IMAGE040
，如式：
Figure 317534DEST_PATH_IMAGE041
其中，
Figure 817917DEST_PATH_IMAGE042
为调整系数；
Figure 349392DEST_PATH_IMAGE043
为当前迭代次数；
Figure 692649DEST_PATH_IMAGE044
为最大迭代次数。
进一步地，所述步骤(3)通过以下公式实现：
在动态窗口法评价函数上加入了路径融合距离函数：
Figure 194037DEST_PATH_IMAGE045
其中，
Figure 939140DEST_PATH_IMAGE046
为路径融合距离，
Figure 641516DEST_PATH_IMAGE047
为DWA在第
Figure 472069DEST_PATH_IMAGE048
个局部窗口内基于预测速度轨迹 而推演出的局部路径末端坐标；
Figure 26416DEST_PATH_IMAGE049
为改进安全蚁群算法所得到的相应拐弯节点坐标；将 路径融合距离函数加入到评价函数中，改进后的评价函数为：
Figure 626025DEST_PATH_IMAGE050
其中，为方位角评价函数表 示在当前速度下，模拟轨迹终点方向与目标之间的方位角偏差；
Figure 499303DEST_PATH_IMAGE051
为速度对应轨迹上 离障碍物的最近距离；
Figure 941785DEST_PATH_IMAGE052
为当前速度大小评价函数；
Figure 925922DEST_PATH_IMAGE046
为路径融合距离函数；
Figure 380037DEST_PATH_IMAGE053
为平滑函数，
Figure 299583DEST_PATH_IMAGE054
、
Figure 839148DEST_PATH_IMAGE055
、
Figure 626976DEST_PATH_IMAGE056
、
Figure 325810DEST_PATH_IMAGE057
分别4项函数的加权系数，表示各项在评价函数中分配的权重比 例。
有益效果：与现有技术相比，本发明的有益效果：安全蚁群算法先规划出一条全局路径，能避开静态障碍物；而改进后的动态窗口法(Dynamic window Approach，DWA)能根据全局路径，进行窗口迭代，从而对动态障碍物成功避障；经过避障算法规划后，相比于单一的蚁群算法，避障算法规划整体路径长度基本一致；由此本发明不仅具有安全蚁群算法路径搜索的高效率以及其很大安全指数，还具有动态窗口法能对动态障碍物平滑避障的特性；不仅能有效降低路径搜索长度，还具有较大安全指数，以保证整个搜索过程中无人艇始终远离障碍物并处于低风险状态；本发明还避免了单一算法容易陷入局部最优解这一问题，提高了算法的稳定性；本发明在保证搜索路径安全的前提下，解决了全局路径规划算法实时性较差和局部路径规划算法缺乏全局信息的问题，同时又能找到全局最优解并实现动态平滑避障。
附图说明
图1为本发明的流程图；
图2为路径融合示意图；
图3为蚁群算法对比实验结果图，其中， (a）采用本发明安全蚁群输出路径图；（b）采用原始蚁群算法输出路径图；
图4为采用本发明避障结果图，其中，（a）为本发明提出的安全蚁群算法规划的全局路径图；（b）为本发明无人艇实时避障示意图；（c）为本发明无人艇输出路径图。
具体实施方式
下面结合附图对本发明作进一步的详细说明。
本发明提出一种基于危险指数和距离函数的快速蚁群无人艇动态避障方法，如图1所示，具体包括以下步骤：
步骤1：预处理阶段，无人艇利用采集到的真实水域信息进行栅格化处理，并保存栅格化后的地图信息，确定无人艇航行初始地点和目标点。栅格化地图能够简单有效地显示障碍物以及周围环境信息，每一个栅格都可以用唯一的序列号与其位置坐标相对应；其对应关系为：
Figure 540891DEST_PATH_IMAGE058
其中，
Figure 833332DEST_PATH_IMAGE002
为第
Figure 267593DEST_PATH_IMAGE003
个栅格的位置坐标；
Figure 696301DEST_PATH_IMAGE003
为第
Figure 347862DEST_PATH_IMAGE003
个网格的序号；
Figure 721074DEST_PATH_IMAGE004
是冗余运算符；
Figure 850704DEST_PATH_IMAGE005
表示取整操作；
Figure 399497DEST_PATH_IMAGE006
和
Figure 566168DEST_PATH_IMAGE007
分别为行方向和列方向的栅格数；
Figure 567622DEST_PATH_IMAGE008
为比例系数。
步骤2：启动安全蚁群算法，在栅格化地图上，采用改进安全蚁群算法进行全局路径的规划，并储存其路径拐点坐标信息。
（2.1）参数初始化，释放蚂蚁并设定搜索起点；路径寻优，并根据转移概率选择栅格前进。转移概率为：
Figure 500943DEST_PATH_IMAGE059
其中，
Figure 497718DEST_PATH_IMAGE010
为
Figure 225502DEST_PATH_IMAGE011
时刻原始蚁群算法的信息素浓度，
Figure 714252DEST_PATH_IMAGE012
是启发函数，表示
Figure 559586DEST_PATH_IMAGE011
时 刻当前网络位置
Figure 817392DEST_PATH_IMAGE060
移动到第
Figure 716078DEST_PATH_IMAGE014
个栅格的路径启发式信息；
Figure 816758DEST_PATH_IMAGE015
为自适应障碍抑制因子，取值 范围为
Figure 826303DEST_PATH_IMAGE016
。加入危险指数DI，将无人艇和障碍物质心之间的距离、无人艇相对障碍物矢面 的惯量因子作为碰撞严重程度的主要影响，计算方法如下：无人艇和障碍物质心之间的距 离影响因子：
Figure 938615DEST_PATH_IMAGE061
其中，
Figure 883568DEST_PATH_IMAGE019
是距离比例因子；
Figure 346911DEST_PATH_IMAGE020
、
Figure 160146DEST_PATH_IMAGE021
分别是无人艇质心接近障碍物质 心所允许的最小距离和对无人艇安全产生影响的最大距离。无人艇相对障碍物矢面的惯量 影响因子：
Figure 392544DEST_PATH_IMAGE062
其中，
Figure 757666DEST_PATH_IMAGE023
为障碍物在基坐标系下的惯量矩阵的最大主转动惯量在无人艇矢状面上 的分量，
Figure 442726DEST_PATH_IMAGE024
为最大允许转动惯量，当
Figure 59652DEST_PATH_IMAGE023
超过该值时，若同障碍物发生潜在碰撞，无人艇将 受到严重冲击。
将路径规划阶段总的危险指数定义为上述距离影响因子和惯量影响因子之积：
Figure 254879DEST_PATH_IMAGE025
当危险指数DI比大时，则自适应障碍抑制因子
Figure 931848DEST_PATH_IMAGE015
趋近于0，表示无人艇和障碍物 之间的危险性较大。当危险指数DI比小时，则
Figure 104203DEST_PATH_IMAGE015
趋近于1，表示无人艇和障碍物之间较为安 全。从而实现了通过轮盘赌状态转移规则在选择路径时，更偏向于无人艇向远离障碍物方 向上的路径进行转移，增强了制导效果。
（2.2）判断是否完成迭代，若未完成则增加蚂蚁数量，继续迭代搜索。若已完成则更新信息素，改进信息素更新方法如下：
Figure 118295DEST_PATH_IMAGE063
其中，
Figure 325286DEST_PATH_IMAGE027
为蚂蚁
Figure 173156DEST_PATH_IMAGE028
在路径
Figure 708174DEST_PATH_IMAGE029
上的信息素浓度；
Figure 666903DEST_PATH_IMAGE030
为信息素强度；
Figure 197241DEST_PATH_IMAGE031
为各个 路径的最优指标；
Figure 340646DEST_PATH_IMAGE032
为区域内最优路径长度；
Figure 487594DEST_PATH_IMAGE033
为转移概率搜索时的转弯次数；
Figure 984434DEST_PATH_IMAGE034
和
Figure 274339DEST_PATH_IMAGE035
分 别为区域内最优路径长度和转弯次数的重要程度指标。
Figure 198433DEST_PATH_IMAGE064
其中，
Figure 832676DEST_PATH_IMAGE037
为
Figure 992262DEST_PATH_IMAGE038
时 刻原始蚁群算法的信息素浓度，
Figure 28351DEST_PATH_IMAGE039
为当前迭代路径
Figure 123346DEST_PATH_IMAGE029
上信息素的增量。通过式（9） 可以看到，信息素挥发因子
Figure 854673DEST_PATH_IMAGE065
也是影响信息素浓度的重要因素。在蚁群算法利用信息素寻找 最优路径的过程中，影响算法性能的重要因素除了信息素浓度的更新规则外，还有信息素 挥发因子。为了保持算法在全局规划时的准确性和收敛速度，本发明加入了自适应信息素 挥发因子
Figure 958895DEST_PATH_IMAGE065
，如式：
Figure 583912DEST_PATH_IMAGE066
其中，
Figure 974442DEST_PATH_IMAGE042
为调整系数；
Figure 317699DEST_PATH_IMAGE043
为当前迭代次数；
Figure 960032DEST_PATH_IMAGE044
为最大迭代次数。在算法的早期阶段， 挥发因子
Figure 102473DEST_PATH_IMAGE065
很小，因为
Figure 804850DEST_PATH_IMAGE043
很小，相应的每个路径的信息素浓度差很小；蚁群的指导功能削弱， 这提高了蚂蚁的全球搜索范围和算法的准确性。随着
Figure 369824DEST_PATH_IMAGE043
的越来越大，使得挥发因子
Figure 940482DEST_PATH_IMAGE065
迅速增 大，各路径的信息素浓度差增大，增强了蚁群的引导功能，提高了蚁群的搜索速度，使算法 快速收敛。
步骤3：启动动态窗口法，对栅格化地图进行障碍物检测；若发现动态障碍物，则以最近的拐点作为子目标点；如果子目标点在障碍物上，那就下一个拐点作为子目标点；并利用加入路径融合距离函数后的动态窗口法进行动态避障。
计算路径融合距离函数：
Figure 540091DEST_PATH_IMAGE067
其中，
Figure 413369DEST_PATH_IMAGE047
为DWA在第
Figure 731218DEST_PATH_IMAGE048
个局部窗口内基于预测速度轨迹而推演出的局部路径末端 坐标；
Figure 325141DEST_PATH_IMAGE049
为改进安全蚁群算法所得到的相应拐弯节点坐标。
将路径融合距离函数加入到评价函数中，改进后的评价函数为：
Figure 44836DEST_PATH_IMAGE068
其中，
Figure 89015DEST_PATH_IMAGE069
为方位角评价函数表示在当前速度下，模拟轨迹终点方向与目标 之间的方位角偏差；
Figure 753215DEST_PATH_IMAGE051
为速度对应轨迹上离障碍物的最近距离；
Figure 541042DEST_PATH_IMAGE052
为当前速度 大小评价函数；
Figure 115243DEST_PATH_IMAGE046
为路径融合距离函数；
Figure 704225DEST_PATH_IMAGE053
为平滑函数，
Figure 731087DEST_PATH_IMAGE054
、
Figure 57026DEST_PATH_IMAGE055
、
Figure 610367DEST_PATH_IMAGE056
、
Figure 996349DEST_PATH_IMAGE057
分别为4项函数的加 权系数，表示各项在评价函数中分配的权重比例。
计算最小的改进评价函数，以蚁群算法规划的拐点坐标
Figure 776086DEST_PATH_IMAGE002
作为子目标点，如图2 所示进行路径融合。进行窗口迭代，若在该阶段动态窗口中无人艇已到达了最近的子目标 点，则结束该阶段的局部路径规划。
步骤4：对寻找子目标点进行循环，直至局部路径规划子目标点为全局路径规划的目标点，则表明该避障算法结束，输出全局最优安全路径。
为验证前文所设计算法的可行性和可靠性，实验环境为：WIN10，i5-6300HQ CPU；编译环境为：MATLAB R2020a仿真平台。采用传统蚁群算法与本专利所提安全蚁群算法进行比较分析；从上述路径规划算法的路径搜索长度、安全指数维度为指标进行评价，从而对比阐述本专利改进安全蚁群算法的优劣性。
路径搜索长度能直观展现蚁群算法根据转移概率搜索路径的效率大小。公式如式：
Figure 515503DEST_PATH_IMAGE070
其中，n表示该路径长度中栅格的总个数；
Figure 798717DEST_PATH_IMAGE071
表示当前栅格位置；
Figure 621179DEST_PATH_IMAGE072
表示 下一栅格位置。
安全指数确保了无人艇在航行时尽可能避免发生碰撞，该值和航线与障碍物距离直接相关。在本文中引入了通过路径的危险网格数，以评估安全性。安全指数定义如式：
Figure 622634DEST_PATH_IMAGE073
其中：
Figure 415009DEST_PATH_IMAGE074
其中，
Figure 552729DEST_PATH_IMAGE075
为安全指数；
Figure 546093DEST_PATH_IMAGE076
是从栅格路径到最近障碍物的距离；
Figure 408745DEST_PATH_IMAGE077
是全局路径上 的危险网格数，定义无人艇路径规划时与障碍物距离小于等于1为危险网格；
Figure 614598DEST_PATH_IMAGE078
为路径 上最近障碍物坐标。
图3为蚁群算法对比实验结果图，图3中(a）为采用本发明提出的安全蚁群输出路径图；图3中（b）为采用原始蚁群算法输出路径图；通过仿真结果可以计算路径长度和安全指数两种指标；结果如表1所示：
表1为安全蚁群算法和原始蚁群算法实验对比结果
Figure 872404DEST_PATH_IMAGE079
从计算结果可以看出，原始蚁群算法所规划路径冗余，且没有考虑到路线的平滑 性，导致路径搜索长度
Figure 161303DEST_PATH_IMAGE080
相较于本发明改进蚁群算法数值大了许多。此外，安全蚁群算法主 要特点是能保证无人艇航行时自身安全性。和原始算法相比由于加入了危险指数这一指 标，所以该算法在计算转移概率选择最优路径时都保持了和障碍物一定的安全距离，故而 具有最大的安全指数
Figure 871770DEST_PATH_IMAGE075
。
如图4中（a）至图4中（c）所示，对于水域环境信息，本发明能由出发位置到目标位置规划出一条可行路径，并且都能成功避障。相比于单一的蚁群算法，本发明避障算法具有安全蚁群算法保证规划路线的安全性，还具有动态窗口法能对动态障碍物平滑避障特性。
本发明经过避障算法规划后，相比于单一的蚁群算法具有安全蚁群算法路径搜索的高效率以及其很大安全指数，还具有动态窗口法能对动态障碍物平滑避障的特性；不仅能有效降低路径搜索长度，避障算法具有较大安全指数，以保证整个搜索过程中无人艇始终远离障碍物并处于低风险状态。此外，改进后的避障算法还避免了单一算法容易陷入局部最优解这一问题，提高了算法的稳定性。

Claims (5)

1.一种基于危险指数和距离函数的快速蚁群无人艇动态避障方法，其特征在于，包括以下步骤：
（1）对预先采集到的真实水域地图信息进行栅格化处理，并保存栅格化后的地图信息，确定无人艇航行初始地点和目标点；
（2）在栅格化地图上，启动蚁群算法，根据加入危险系数的转移概率进行全局路径的规划，储存其路径拐点坐标信息，并启动信息素更新策略；
（3）对栅格化地图进行障碍物检测，若发现动态障碍物，则以最近的拐点作为子目标点；如果子目标点在障碍物上，将下一个拐点作为子目标点；并利用加入路径融合距离函数后的动态窗口法进行动态避障；
（4）对步骤（3）寻找子目标点进行循环，直至局部路径规划子目标点为全局路径规划的目标点，则表明该避障算法结束，输出全局最优安全路径。
2.根据权利要求1所述的基于危险指数和距离函数的快速蚁群无人艇动态避障方法，其特征在于，步骤（1）所述栅格化后的地图信息能够显示障碍物以及周围环境信息，每一个栅格用唯一的序列号与其位置坐标相对应，其对应关系为:
Figure 216473DEST_PATH_IMAGE001
其中，
Figure 930351DEST_PATH_IMAGE002
为第
Figure 116613DEST_PATH_IMAGE003
个栅格的位置坐标；
Figure 246243DEST_PATH_IMAGE003
为第
Figure 591773DEST_PATH_IMAGE003
个网格的序号；
Figure 338537DEST_PATH_IMAGE004
是冗余运算符；
Figure 136729DEST_PATH_IMAGE005
表示 取整操作；
Figure 70050DEST_PATH_IMAGE006
和
Figure 411032DEST_PATH_IMAGE007
分别为行方向和列方向的栅格数；
Figure 138817DEST_PATH_IMAGE008
为比例系数。
3.根据权利要求1所述的基于危险指数和距离函数的快速蚁群无人艇动态避障方法，其特征在于，步骤(2)所述加入危险系数的转移概率为：
Figure 424305DEST_PATH_IMAGE009
Figure 82688DEST_PATH_IMAGE010
其中，
Figure 340494DEST_PATH_IMAGE011
为
Figure 301497DEST_PATH_IMAGE012
时刻原始蚁群算法的信息素浓度，
Figure 215226DEST_PATH_IMAGE013
是启发函数，表示
Figure 224771DEST_PATH_IMAGE012
时刻当 前网络位置
Figure 664979DEST_PATH_IMAGE014
移动到第
Figure 734566DEST_PATH_IMAGE015
个栅格的路径启发式信息；
Figure 119280DEST_PATH_IMAGE016
为自适应障碍抑制因子，取值范 围为
Figure 994832DEST_PATH_IMAGE017
；加入危险指数DI，将无人艇和障碍物质心之间的距离、无人艇相对障碍物矢面 的惯量因子作为碰撞严重程度的主要影响，计算方法如下：
Figure 227230DEST_PATH_IMAGE018
其中，
Figure 670981DEST_PATH_IMAGE019
为无人艇和障碍物质心之间的距离影响因子，
Figure 356041DEST_PATH_IMAGE020
是距离比例因 子；
Figure 769704DEST_PATH_IMAGE021
、
Figure 43560DEST_PATH_IMAGE022
分别是无人艇质心接近障碍物质心所允许的最小距离和对无人艇安全产生影 响的最大距离；无人艇相对障碍物矢面的惯量影响因子：
Figure 720529DEST_PATH_IMAGE023
其中，
Figure 750275DEST_PATH_IMAGE024
为障碍物在基坐标系下的惯量矩阵的最大主转动惯量在无人艇矢状面上的分 量，
Figure 357843DEST_PATH_IMAGE025
为最大允许转动惯量，当
Figure 95992DEST_PATH_IMAGE024
超过最大允许转动惯量时，若同障碍物发生潜在碰撞，无 人艇将受到严重冲击；
将路径规划阶段总的危险指数定义为上述距离影响因子和惯量影响因子之积：
Figure 615966DEST_PATH_IMAGE026
当危险指数DI比大时，则自适应障碍抑制因子
Figure 72355DEST_PATH_IMAGE027
趋近于0，表示无人艇和障碍物之间的 危险性较大；当危险指数DI比小时，则
Figure 955385DEST_PATH_IMAGE027
趋近于1，表示无人艇和障碍物之间较为安全。
4.根据权利要求1所述的基于危险指数和距离函数的快速蚁群无人艇动态避障方法，其特征在于，步骤(2)所述信息素更新策略加入自适应挥发因子，具体如下：
Figure 548041DEST_PATH_IMAGE028
其中，
Figure 504495DEST_PATH_IMAGE029
为蚂蚁k在路径
Figure 651443DEST_PATH_IMAGE030
上的信息素浓度；
Figure 476179DEST_PATH_IMAGE031
为信息素强度，
Figure 392183DEST_PATH_IMAGE032
为各个路径 的最优指标；
Figure 237648DEST_PATH_IMAGE033
为区域内最优路径长度；
Figure 934208DEST_PATH_IMAGE034
为转移概率搜索时的转弯次数；
Figure 906844DEST_PATH_IMAGE035
和
Figure 739670DEST_PATH_IMAGE036
分别为 区域内最优路径长度和转弯次数的重要程度指标；
Figure 834665DEST_PATH_IMAGE037
其中，
Figure 877577DEST_PATH_IMAGE038
为
Figure 44116DEST_PATH_IMAGE039
时刻原始蚁群算法的信息素浓度，
Figure 669132DEST_PATH_IMAGE040
为当前迭代 路径
Figure 872712DEST_PATH_IMAGE030
上信息素的增量；加入自适应信息素挥发因子
Figure 278285DEST_PATH_IMAGE041
，如式：
Figure 186198DEST_PATH_IMAGE042
其中，
Figure 118251DEST_PATH_IMAGE043
为调整系数；
Figure 617365DEST_PATH_IMAGE044
为当前迭代次数；
Figure 447918DEST_PATH_IMAGE045
为最大迭代次数。
5.根据权利要求1所述的基于危险指数和距离函数的快速蚁群无人艇动态避障方法，其特征在于，所述步骤(3)通过以下公式实现：
在动态窗口法评价函数上加入了路径融合距离函数：
Figure 831626DEST_PATH_IMAGE046
其中，
Figure 431235DEST_PATH_IMAGE047
为路径融合距离，
Figure 366830DEST_PATH_IMAGE048
为DWA在第
Figure 684679DEST_PATH_IMAGE049
个局部窗口内基于预测速度轨迹而推演 出的局部路径末端坐标；
Figure 593116DEST_PATH_IMAGE050
为改进安全蚁群算法所得到的相应拐弯节点坐标；将路径融 合距离函数加入到评价函数中，改进后的评价函数为：
Figure 375127DEST_PATH_IMAGE051
其中，
Figure 419307DEST_PATH_IMAGE052
为方位角评价函数表示在当前速度下，模拟轨迹终点方向与目标之间 的方位角偏差；
Figure 162135DEST_PATH_IMAGE053
为速度对应轨迹上离障碍物的最近距离；
Figure 481121DEST_PATH_IMAGE054
为当前速度大小 评价函数；
Figure 242272DEST_PATH_IMAGE055
为路径融合距离函数；
Figure 457353DEST_PATH_IMAGE056
为平滑系数；
Figure 280953DEST_PATH_IMAGE057
分别为4 项函数的加权系数，表示各项在评价函数中分配的权重比例。
CN202210532561.0A 2022-05-17 2022-05-17 基于危险系数和距离函数的快速蚁群无人艇动态避障方法 Active CN114625150B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210532561.0A CN114625150B (zh) 2022-05-17 2022-05-17 基于危险系数和距离函数的快速蚁群无人艇动态避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210532561.0A CN114625150B (zh) 2022-05-17 2022-05-17 基于危险系数和距离函数的快速蚁群无人艇动态避障方法

Publications (2)

Publication Number Publication Date
CN114625150A true CN114625150A (zh) 2022-06-14
CN114625150B CN114625150B (zh) 2022-08-19

Family

ID=81907242

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210532561.0A Active CN114625150B (zh) 2022-05-17 2022-05-17 基于危险系数和距离函数的快速蚁群无人艇动态避障方法

Country Status (1)

Country Link
CN (1) CN114625150B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115016510A (zh) * 2022-08-08 2022-09-06 武汉工程大学 一种机器人导航避障方法、装置以及存储介质
CN115357031A (zh) * 2022-10-19 2022-11-18 武汉理工大学 一种基于改进蚁群算法的船舶路径规划方法及系统
CN115640921A (zh) * 2022-10-12 2023-01-24 中南大学湘雅医院 一种院内危重患者转运路径规划方法和系统

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109976343A (zh) * 2019-03-26 2019-07-05 电子科技大学 一种基于动态窗口法的主动避障方法
CN110057368A (zh) * 2019-05-22 2019-07-26 合肥工业大学 一种新型室内定位与导航方法
CN110531762A (zh) * 2019-08-21 2019-12-03 东南大学 一种基于改进人工势场法的机器人路径规划方法
CN111347429A (zh) * 2020-04-16 2020-06-30 淮阴工学院 一种基于改进蚁群算法的可碰撞检测机械臂路径规划方法
CN111413965A (zh) * 2020-03-11 2020-07-14 西安工程大学 一种基于uav协同感知的ugv行驶路径规划方法
CN112631294A (zh) * 2020-12-16 2021-04-09 上海应用技术大学 一种移动机器人智能路径规划方法
CN113848919A (zh) * 2021-09-29 2021-12-28 河北大学 一种基于蚁群算法的室内agv路径规划方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109976343A (zh) * 2019-03-26 2019-07-05 电子科技大学 一种基于动态窗口法的主动避障方法
CN110057368A (zh) * 2019-05-22 2019-07-26 合肥工业大学 一种新型室内定位与导航方法
CN110531762A (zh) * 2019-08-21 2019-12-03 东南大学 一种基于改进人工势场法的机器人路径规划方法
CN111413965A (zh) * 2020-03-11 2020-07-14 西安工程大学 一种基于uav协同感知的ugv行驶路径规划方法
CN111347429A (zh) * 2020-04-16 2020-06-30 淮阴工学院 一种基于改进蚁群算法的可碰撞检测机械臂路径规划方法
CN112631294A (zh) * 2020-12-16 2021-04-09 上海应用技术大学 一种移动机器人智能路径规划方法
CN113848919A (zh) * 2021-09-29 2021-12-28 河北大学 一种基于蚁群算法的室内agv路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
LEI SHAO 等: ""Mobile Robot Path Planning Based on Improved Ant Colony Fusion Dynamic Window Approach"", 《PROCEEDINGS OF 2021 IEEE INTERNATIONAL CONFERENCE ON MECHATRONICS AND AUTOMATION》 *
苗长伟: ""室内移动机器人路径规划技术研究"", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115016510A (zh) * 2022-08-08 2022-09-06 武汉工程大学 一种机器人导航避障方法、装置以及存储介质
CN115640921A (zh) * 2022-10-12 2023-01-24 中南大学湘雅医院 一种院内危重患者转运路径规划方法和系统
CN115357031A (zh) * 2022-10-19 2022-11-18 武汉理工大学 一种基于改进蚁群算法的船舶路径规划方法及系统

Also Published As

Publication number Publication date
CN114625150B (zh) 2022-08-19

Similar Documents

Publication Publication Date Title
CN109506655B (zh) 基于非均匀建模的改进蚁群路径规划算法
CN111780777B (zh) 一种基于改进a*算法和深度强化学习的无人车路径规划方法
CN114625150B (zh) 基于危险系数和距离函数的快速蚁群无人艇动态避障方法
CN109933067B (zh) 一种基于遗传算法和粒子群算法的无人艇避碰方法
CN107037809A (zh) 一种基于改进蚁群算法的无人艇避碰方法
CN110887484A (zh) 基于改进遗传算法的移动机器人路径规划方法及存储介质
CN115079705A (zh) 基于改进a星融合dwa优化算法的巡检机器人路径规划方法
CN111063218A (zh) 一种船舶避碰决策方法
Wang et al. Cooperative collision avoidance for unmanned surface vehicles based on improved genetic algorithm
Zhu et al. Application of improved Dijkstra algorithm in intelligent ship path planning
CN111678524A (zh) 一种基于飞行安全的救援航空器路径规划方法及系统
Zheng et al. A decision-making method for ship collision avoidance based on improved cultural particle swarm
CN112033413A (zh) 一种结合环境信息的改进a*算法
Wang et al. Local path optimization method for unmanned ship based on particle swarm acceleration calculation and dynamic optimal control
CN111338356A (zh) 改进分布式遗传算法的多目标无人船避碰路径规划方法
CN110906935A (zh) 一种无人艇路径规划方法
Du et al. An optimized path planning method for coastal ships based on improved DDPG and DP
CN116225066A (zh) 一种基于混沌映射鹈鹕优化算法的无人机路径优化方法
CN113341998A (zh) 一种改进蚁群算法的三维水下欠驱动auv路径规划方法
CN114594788A (zh) 一种未知环境下四旋翼无人飞行器航迹规划方法及系统
CN110320907B (zh) 一种基于改进蚁群算法和椭圆碰撞锥推演模型的无人水面艇双层避碰方法
Yang et al. Improved reinforcement learning for collision-free local path planning of dynamic obstacle
CN111998858B (zh) 一种基于改进a*算法的无人机航路规划方法
CN116880561A (zh) 基于改进粒子群无人机路径规划安全增强的优化方法及系统
Rajendran et al. Wave-aware trajectory planning for unmanned surface vehicles operating in congested environments

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
GR01 Patent grant
GR01 Patent grant