CN115562255A - 基于空地协同的多智能消防机器人消防水带防缠绕方法 - Google Patents
基于空地协同的多智能消防机器人消防水带防缠绕方法 Download PDFInfo
- Publication number
- CN115562255A CN115562255A CN202211124347.8A CN202211124347A CN115562255A CN 115562255 A CN115562255 A CN 115562255A CN 202211124347 A CN202211124347 A CN 202211124347A CN 115562255 A CN115562255 A CN 115562255A
- Authority
- CN
- China
- Prior art keywords
- robot
- node
- path
- fire
- cost
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000004804 winding Methods 0.000 title claims abstract description 39
- 230000008569 process Effects 0.000 claims abstract description 23
- 230000033001 locomotion Effects 0.000 claims abstract description 17
- 238000012544 monitoring process Methods 0.000 claims abstract description 13
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 13
- 230000004888 barrier function Effects 0.000 claims description 12
- 238000009499 grossing Methods 0.000 claims description 10
- 230000000694 effects Effects 0.000 claims description 7
- 238000011478 gradient descent method Methods 0.000 claims description 4
- 238000005457 optimization Methods 0.000 claims description 4
- 230000009191 jumping Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 41
- 238000010586 diagram Methods 0.000 description 13
- 230000011218 segmentation Effects 0.000 description 9
- 238000012549 training Methods 0.000 description 8
- 238000005381 potential energy Methods 0.000 description 7
- 208000027418 Wounds and injury Diseases 0.000 description 5
- 230000008859 change Effects 0.000 description 5
- 238000012360 testing method Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000018109 developmental process Effects 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 238000010845 search algorithm Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 239000013598 vector Substances 0.000 description 2
- 206010000369 Accident Diseases 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000009172 bursting Effects 0.000 description 1
- 238000002485 combustion reaction Methods 0.000 description 1
- 150000001875 compounds Chemical class 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 229910052731 fluorine Inorganic materials 0.000 description 1
- 125000001153 fluoro group Chemical group F* 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 239000000941 radioactive substance Substances 0.000 description 1
- 239000000779 smoke Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001988 toxicity Effects 0.000 description 1
- 231100000419 toxicity Toxicity 0.000 description 1
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/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于空地协同的多智能消防机器人消防水带防缠绕方法，包括：基于空基影像采集监测多个机器人的移动；所述机器人为拖拽着消防水带的智能消防机器人；根据监测数据确定出消防水带出现交叉的各机器人的移动优先级；机器人在前往新的目标位置执行新任务时，根据消防水带的交叉情况，判断机器人移动是否会造成消防水带之间的缠绕；否，对执行新任务的机器人进行路径规划，按照规划路径移动到新的目标位置；是，按照移动优先级从高到低依次对机器人进行路径规划；按照规划路径依次进行移动到各机器人的目标位置，防止机器人在移动过程中水带缠绕。本发明解决地面多智能消防机器人的路径规划和水带缠绕问题。
Description
技术领域
本发明属于空地协同和路径规划技术领域，具体涉及一种基于空地协同的多智能消防机器人消防水带防缠绕方法。
背景技术
随着社会经济的迅猛发展和城市化进展的不断加快，高层、地下及石油化工建筑的不断涌现，导致化学危险品和放射性物质泄漏以及燃烧、***、坍塌的事故隐患增加，事故发生的概率也相应提高。一旦发生灾害事故，消防员面对高温、黑暗、有毒和浓烟等危害环境时，若没有消防机器人协助而贸然冲进现场，不仅不能完成任务，还会徒增人员伤亡。
面向强干扰、高动态等复杂火灾环境下的消防需求，为实现机器人***的任务能力扩展及整体消防效能提升，采取空-地、地-地一体化多模式、异构群集的消防侦察机器人***及新型作业模式，实现能力互补和行动协调，已成为未来消防应用的主要发展方向。
利用异构无人***在火灾事故现场进行侦察、救援任务时，需要重点考虑消防机器人消防水带的影响，尤其当机器人拖拽水带移动时，不但需要考虑机器人的移动路径问题，还需要考虑水带与水带之间、水带与障碍物之间的交叉缠绕问题。为此，亟需一种结合路径规划算法与水带防缠绕策略的方案，来解决机器人路径和水带缠绕的问题。
发明内容
鉴于上述的分析，本发明旨在公开一种基于空地协同的多智能消防机器人消防水带防缠绕方法，以解决地面多智能消防机器人的路径规划和水带缠绕问题。
本发明公开了一种基于空地协同的多智能消防机器人消防水带防缠绕方法，包括：
步骤S101、基于空基影像采集监测多个机器人的移动；所述机器人为拖拽着消防水带的智能消防机器人；
步骤S102、根据监测数据确定出，消防水带出现交叉的各机器人的移动优先级；
步骤S103、机器人在前往新的目标位置执行新任务时，根据消防水带的交叉情况，判断机器人移动是否会造成消防水带之间的缠绕；否，进入步骤S104，是，进入步骤S105；
步骤S104、对执行新任务的机器人进行路径规划，按照规划路径移动到新的目标位置；
步骤S105、按照移动优先级从高到低依次对机器人进行路径规划；按照规划路径依次进行移动到各机器人的目标位置，防止机器人在移动过程中水带缠绕。
进一步地，若消防水带出现交叉现象，各机器人的移动优先级确定方法为：
根据监测数据确定出消防水带被压的先后次序；
按照消防水带被压的先后次序依次确定机器人移动优先级，水带压在最上方的机器人移动优先级最高，压在最下方的机器人移动优先级最低。
进一步地，所述路径规划采用基于多引导点牵引作用的改进混合A*路径规划算法，使得机器人规划的路径能被牵引到设定的多个引导点附近，在多障碍物环境下规划出安全可靠的路径。
进一步地，具体的路径规划，包括以下步骤：
建立用于进行路径搜索的数字地图；
在数字地图中采用混合A*搜索，搜索出基于消防机器人运动学和最小安全转向约束，满足路径连续性的粗路径；
根据所述粗路径，基于梯度下降优化算法的轨迹平滑器，对混合搜索的粗路径进行优化，构建出适合地面多机器人安全执行、连续平滑的路径。
进一步地，所述数字地图基于遥感观测数据生成的机器人可通达路径的二值栅格地图，以及利用二值栅格地图建立Voronoi图，进行结合构建出的混合地图。
进一步地，所述混合A*搜索，具体包括：
步骤S501、输入混合地图；
步骤S502、进行搜索的初始化；
所述初始化包括建立Open_Set和Closed_Set两个列表，两个列表初始化时都为空列表，其中，Open_Set列表存储待扩展节点，Closed_Set列表存储已扩展的节点或存在障碍物的节点；
步骤S503、判断Open_Set列表为非空列表，从Open_Set列表中选择总代价最小的节点作为扩展节点进行扩展，得到与扩展节点状态连续的多个扩展子节点，并将扩展节点从Open_Set列表移除至Closed_Set列表中；
步骤S504、判断扩展节点是否是目标点；若是，则搜索结束；若不是，则继续扩展该节点的子节点；
步骤S505、判断扩展子节点否与障碍物发生碰撞，若是，则舍弃该子节点；若不是，跳入下一步骤；
步骤S506、将出现在同一网格中的当前扩展子节点与原有节点的实际代价进行比较，当前扩展子节点的实际代价低于原有节点的实际代价时，将当前扩展子节点的连续状态与该网格单元关联，并将当前扩展子节点所在栅格设置为子节点所在栅格的父栅格，并将扩展子节点放入Open_Set列表中，更新扩展子节点的总代价；
步骤S507、循环上述过程中步骤S503到步骤S506，直到搜索到目标节点，并将目标节点放入Open_Set列表；
步骤S508、从Open_Set列表中的目标节点开始回朔所有遍历过的父节点，直至起始节点，得到所搜索的路径，路径搜索完毕，算法终止；如果Open_Set列表为空，则表明没有规划出可行路径，算法终止。
进一步地，搜索过程中每个节点的总代价包括实际代价和启发式估计代价；
所述实际代价为起始节点到当前节点的代价，包括当前节点的父节点代价以及父节点到当前节点的代价，其中后者包括了父节点到当前节点的实际路径长度、二值栅格地图中父节点到当前节点的转向代价和Voronoi图中从父节点到当前节点的代价；
所述启发式估计代价为当前节点到目标节点的启发式估计代价，由基于非完整约束的启发式函数和基于完整约束的启发式函数中较大的值决定。
进一步地，基于多引导点牵引作用进行路径优化中，采用基于多引导点作用下的路径平滑器，对粗路径进行平滑处理得到适合地面机器人安全执行、连续平滑的路径。
进一步地，所述梯度下降平滑器中，基于包括障碍物项、曲率项、平滑项、voronoi场项和多引导点函数项，建立最小化代价函数，利用梯度下降法求解最优路径；
所述最小化代价函数P＝Pobs+Pcur+Psmo+Pvor+Pgui；
Pobs障碍物项的代价函数；Pcur曲率项的代价函数；Psmo为平滑项的代价函数；
Pvor维诺势场函数项的代价函数；Pgui为多引导点函数项的代价函数。
进一步地，当对于每一个搜索节点xi有|xi-gi|＜ρmax时，所述多引导点函数项的代价函数Pgui为：
式中，xi为路径上顶点的二维平面坐标；gi为距离节点xi最近的引导点的位置；ρmax为引导点影响代价函数的最大距离的阈值；引导点权重wgui为引导点影响路径变化的系数。
本发明可实现以下有益效果：
本发明公开的基于空地协同的多智能消防机器人消防水带防缠绕方法，实现了拖拽着消防水带的智能消防机器人在执行消防任务时，消防水带交叉现象的发现，确定消防水带出现交叉的各机器人的移动优先级；以及通过在机器人在前往新的目标位置执行新任务时的路径规划，防止了机器人在移动中消防水带之间或者消防水带与障碍物的缠绕问题。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明实施例中的基于空地协同的多智能消防机器人消防水带防缠绕方法流程图；
图2(a)为本发明实施例中的两台存在消防水带交叉的消防机器人执行灭火任务的示意图；
图2(b)为本发明实施例中的规划路径时不考虑消防水带的交叉和障碍物时发生水带缠绕的示意图；
图2(c)为本发明实施例中的对优先级高的灭火机器人1的路径规划图；
图2(d)为本发明实施例中的对优先级低的灭火机器人2的路径规划图；
图3为本发明实施例中的改进混合A*算法进行可靠移动路径规划实现方法流程图；
图4为本发明实施例中的深度语义分割网络模型的训练和测试过程流程图；
图5为本发明实施例中的混合A*算法基本流程图；
图6为本发明实施例中的混合A*算法的3个前向搜索节点扩展方式图；
图7为本发明实施例中的混合A*算法搜索实例图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。
本发明的一个实施例公开了一种基于空地协同的多智能消防机器人消防水带防缠绕方法，如图1所示，包括：
步骤S101、基于空基影像采集监测多个机器人的移动；所述机器人为拖拽着消防水带的智能消防机器人；
步骤S102、根据监测数据确定出，消防水带出现交叉的各机器人的移动优先级；
步骤S103、机器人在前往新的目标位置执行新任务时，根据消防水带的交叉情况，判断机器人移动是否会造成消防水带之间的缠绕；否，进入步骤S104，是，进入步骤S105；
步骤S104、对执行新任务的机器人进行路径规划，按照规划路径移动到新的目标位置；
步骤S105、按照移动优先级从高到低依次对机器人进行路径规划；按照规划路径依次进行移动到各机器人的目标位置，防止机器人在移动过程中水带缠绕。
具体的，在步骤S101中，用空中无人机观测地面多消防机器人及其消防水带的移动情况。
在步骤S102中，监测数据为无人机回传的视频；通过无人机回传的视频判断消防水带之间出现交叉现象；
各机器人的移动优先级确定方法为：
根据监测数据确定出消防水带被压的先后次序；
按照消防水带被压的先后次序依次确定机器人移动优先级，水带压在最上方的机器人移动优先级最高，压在最下方的机器人移动优先级最低；
若没有出现交叉现象，无人机则继续观察。
具体的，在步骤S103中，如果消防水带的不存在交叉情况，且机器人在前往新的目标位置执行新任务时，也不会造成水带的交叉和缠绕情况，则按照步骤S104中的路径规划方法，对执行新任务的机器人进行路径规划，按照规划路径移动到新的目标位置；
如果消防水带的存在交叉情况，且水带被压在下面的消防机器人如果先于消防水带压在上面的消防机器人移动，则交叉情况不会解除，且当规划路径需经过压在上面的消防水带时，就会造成缠绕问题，而缠绕会造成消防水带的阻塞或断裂，从而造成事故，在本实施例中，当判断出机器人移动会造成消防水带之间的缠绕；进入步骤S105；
在本实施例中，步骤S105中解决消防水带缠绕问题，所采用的按照移动优先级从高到低依次对机器人进行路径规划，借鉴了计算机领域中“堆栈”的思想，即：“后入先出”，通过依次按照优先级从高到低对机器人进行移动，监测机器人移动次序和移动的位置，就能防止水带的多次交叉缠绕。
如图2(a)、(b)、(c)、(d)所示，如图2(a)为两台存在消防水带交叉的消防机器人执行灭火任务的示意图，在执行完灭火任务后，灭火机器人1和2均有新的规划的终点位姿；图2(b)为规划路径时不考虑消防水带的交叉和障碍物时发生水带缠绕的示意图；图2(c)为采用本实施例的路径规划对优先级高的灭火机器人1的路径规划图；从图中可见，通过规划的路径，使灭火机器人1在到达终点位置时，通过设置的引导点，避免了灭火机器人1的水带与障碍物的缠绕，又由于灭火机器人1的水带压在灭火机器人2的水带的上方，移动不会造成水带的缠绕。图2(d)为采用本实施例的路径规划对优先级低的灭火机器人2的路径规划图；在灭火机器人1移动后，灭火机器人2在依照规划的路径移动，使灭火机器人2在到达终点位置时，通过设置的引导点，避免了灭火机器人2的水带与障碍物的缠绕。由于灭火机器人1的事先移动解决了水带的交叉问题，不会发生灭火机器人1和灭火机器人2水带的缠绕问题。
值得注意的是在机器人移动过程中，不但需要考虑消防机器人拖拽水带运动过程中应避免机器人出现较大幅度自转或后退而导致水带弯折爆裂，还需要避免水带与障碍物之间发生交叉缠绕的问题，基于上述约束条件，
在本实施例中的步骤S104和S105中的路径规划中均为采用基于多引导点牵引作用的改进混合A*路径规划算法，使得机器人规划的路径能被牵引到设定的多个引导点附近，在多障碍物环境下成功规划出一条安全可靠的路径。
具体的，基于多引导点牵引作用的改进混合A*路径规划方法，如图3所示，包括以下步骤：
步骤S301、建立用于进行路径搜索的数字地图；
步骤S302、在数字地图中采用混合A*搜索，搜索出基于消防机器人运动学和最小安全转向约束，满足路径连续性的粗路径；
步骤S303、根据所述粗路径，基于梯度下降优化算法的轨迹平滑器，对混合搜索的粗路径进行优化，构建出适合地面多机器人安全执行、连续平滑的路径。
具体的，步骤S301中，所述数字地图基于遥感观测数据生成的机器人可通达路径的二值栅格地图，以及利用二值栅格地图建立Voronoi图，进行结合构建出的混合地图。
传统的路径规划算法的一个不足之处在于它规划的路径过于靠近障碍物，即它会选择无碰撞的最短路径。为了权衡最短路径与远离障碍物之间的矛盾，一种常见方法是使用势场惩罚机器人接近障碍物，然而，传统的势场存在几个严重的缺点。
首先，常规势场在狭窄的通道中容易产生高势区域，这使得路径穿越这些通道的计算成本很高。
其次，由于障碍物周围的作用势场通常定义为与障碍物的距离的函数，这意味着计算给定势场值时需要计算在有效半径内包含所有障碍物的势能，这可能在计算上代价很大。
为了解决这些问题，本实施例引入Voronoi场，根据二值栅格化地图的几何形状重新调整势场分布。
具体的，在二值栅格地图中，利用Voronoi势场函数选取离障碍物一定的距离阈值，构建出现场空间的二维Voronoi图；
Voronoi场用来定义路径长度和接近障碍物之间的权衡关系。Voronoi势场函数定义如下：
式中，dO和dV分别为与最近障碍物的距离和与最近的广义泰森多边形(GVD)的边的距离，a∈[0,∞)，
为所需要调节的参数，分别控制了势能值的下降速率与控制的范围。当
时表达式(1)成立；其他情况下，ρV(x,y)＝0。
该势能场具有如下性质：
(i)当
势能为0。
(ii)势能值ρV(x,y)∈[0,1]，且分布连续。
(iii)当地图上的点(x,y)在障碍物上或里面时，势能值达到最大值。
(iv)当地图上的点(x,y)在广义维诺图的边上时，其势能值达到最小值。
与传统的势场相比，Voronoi场的关键优势在于，场的值与导航的势场可以成比例缩放。因此，即使是狭窄的U型口也可导航，这对于传统的势场并不总是如此。另外由于通道中间位置处势场为零，可以为多机器人编队行驶提供可靠安全的路径。
优选的，基于ROS(机器人操作***)下利用构建的Voronoi势场函数可生成Voronoi图。
将Voronoi图和二值栅格地图叠加合并构建出混合地图。
在本实施例中的一个具体的方案中，基于遥感观测数据生成的机器人可通达路径的二值栅格地图；可采用以下方法实现：
1)采用无人机的遥测数据建立消防机器人的任务区域的空间三维模型；
在建立空间三维模型中，可采用无人机观测任务区域的序列影像；进行实时三维重建恢复出目标区域表面的密集三维点云；在根据密集三维点云建立出空间三维模型。
在本实施例中可采用成熟应用实时三维重建技术，使得无人机对任务现场进行实时三维重建变得现实。
如果任务区域空间较大、建图具有实时性要求以及包含多个可用无人机的情况下可以通过以下方法，建立空间三维模型；
将目标区域划分为若干目标子区域，利用至少一架无人机获取各目标子区域的序列影像，通过图传装置将影像缩率图传至地面服务器；
在地面服务器中，进行实时三维重建恢复出目标子区域表面的密集三维点云；
利用基于三维点云数据的多地图拼接融合算法将单无人机构图进行整体融合，从而高效构建出目标区域完整的空间三维模型；
根据各无人机获取的子目标区域三维点云图，提取待拼接点云地图的边缘像素点，建立像素点变换数学模型，将地图拼接问题转化为三维点云的数据拼接与配准，得到像素点的平移旋转矩阵；根据像素点变换结果，对待拼接地图进行相应的旋转、平移和融合，获取目标区域完整的空间三维模型的精确拼接结果。
2)采用基于卷积神经网络预测遥感图像的深度学习方法，提取所述空间三维模型场景中路面区域，构建地面移动机器人的可靠移动路径区域，将其进行栅格化插值得到可通达路径的二值栅格地图；
所述机器人可通达路径的二值栅格地图包括：
制作点云数据训练集；
根据实景的三维模型，对包括建筑物、植被、地面、车辆、行人、湖泊、墙面和路基在内的特征进行标注，构建数据训练集；
建立用于提取可靠移动路径区域的深度语义分割网络模型；
在本实施例中，采用经典结构的编码器-解码器模型的图像语义分割模型作为进行可靠移动路径区域提取的深度语义分割网络模型。
利用所述数据训练集训练和测试所述深度语义分割网络模型；
利用所述训练数据集，训练建立的深度语义分割网络模型，将构建的完整目标区域三维模型的测试数据输入到训练后的深度语义分割网络模型，得到遥感图像的可靠移动路径区域的提取结果。
利用训练后的深度语义分割网络模型，对目标区域完整的空间三维模型进行提取，得到目标区域中的机器人可靠移动路径区域；
根据机器人可靠移动路径区域，将其进行栅格化插值得到可通达路径的二值栅格地图。
所述栅格化插值可采用现有的地图栅格化方法进行。
如图4所示，为深度语义分割网络模型的训练和测试过程流程图；
具体的，在步骤S302的混合A*搜索中，在混合地图中采用混合A*搜索，搜索出基于地面机器人运动学和最小安全转向约束，满足路径连续性的粗路径。
启发式搜索算法中A*算法比较通用，但考虑到消防机器人的车辆运动学约束与传统A*具有离散性的缺点，采用A*算法的变种形式，混合A*算法作为全局路径的搜索技术。
相比于传统A*算法包括x方向与y方向位置信息的二维搜索空间，混合A*算法搜索的空间维度为四维，其中增加了代表机器人当前的朝向信息和第四个维度代表了机器人前后运动的模式，向前运动或是向后运动。
传统A*算法因为状态离散性特质，导致其规划出来的路径不能直接被机器人所执行。为了克服这一问题，混合A*用连续的机器人的状态分配到对应离散的网格中，这样连接起来连续的坐标将被实际的机器人执行。
混合A*算法是基于启发式函数的搜索算法，从起始点开始向目标点通过扩展节点方式进行路径搜索，利用评价函数来衡量搜索过程中每个节点的总代价。评价函数为f(s)＝g(s)+h(s)，其中g(s)表示为从起始节点到当前节点的实际代价，满足如下递推关系：
g(si)＝g(si-1)+cost(sk-1,sk)
＝g(si-1)+(1+a·turncost(sk-1,sk)+b·mapcost(sk-1，sk))·dist(sk-1，sk)
式中，si为当前节点(子节点)，si-1为前一个节点(父节点)；g(si)为当前节点的实际代价，g(si-1)为当前节点的父节点的实际代价；cost(sk-1,sk)为从父节点到当前节点的代价；a和b为各项权重因子；dist(sk-1,sk)为从父节点到当前节点的路径长度；turncost(sk-1,sk)为从父节点到当前节点的转向代价，直行时候该值为0，转向时该值为1。该项作用是惩罚规划路径转弯，使机器人能尽可能保持直行；mapcost(sk-1,sk)为Voronoi图从父节点到当前节点的代价，在障碍物安全距离内该值为无穷大，否则为0。该项作用是使规划路径避开障碍物，保证机器人行驶安全。
第二项h(s)表示从当前节点到目标节点的启发式估计代价，由基于非完整约束的启发式函数和基于完整约束的启发式函数中较大的值决定，其表达式为h(si)＝max h1(si),h2(si)，其中h1为忽略环境障碍、考虑机器人非完整性约束的条件下得到的启发式数值，一般选取Reeds-Shepp或Dubins得到的路径长度；h2为忽略机器人非完整性约束、考虑环境约束的条件下得到的启发式数值，一般选取A*算法搜索得到的路径长度。
具体的，混合A*算法基本流程如图5所示：包括以下步骤：
步骤S501、输入混合地图；
步骤S502、进行初始化；
所述初始化包括建立Open_Set和Closed_Set两个列表，两个列表初始化时都为空列表，其中Open_Set列表存储待扩展节点，Closed_Set列表存储已扩展的节点或存在障碍物的节点。获取起始点s0(x0,y0,θ0)和目标点sg(xg,yg,θg)，以及差速移动机器人的运动学模型。将起始点放入Open_Set列表中；
步骤S503、判断Open_Set列表为非空列表，从Open_Set列表中选择总代价最小的节点作为扩展节点进行扩展，得到与扩展节点状态连续的多个扩展子节点，并将扩展节点从Open_Set列表移除至Closed_Set列表中；
其中，当检索Open_Set列表中为空列表时，则路径搜索失败，该地图不存在可到达目标位置的路径。如不为空列表，则从Open_Set列表中，选择总代价最小的节点si(xi,yi,θi)作为扩展节点，扩展出3个与该节点状态连续的子节点，并将节点si从Open_Set列表中移出至Closed_Set列表中；所述xi,yi为坐标值，θi为方向角。
优选的，所述扩展方向为节点在前进运动方向上分别进行左转、右转以及直行三种方向，扩展的三个连续子节点如图6所示。对于直线节点，机器人沿直线运动方向移动；对于转弯节点，机器人沿圆弧运动方向进行移动，扩展时可采用略大机器人的最小转弯半径的弧长。
步骤S504、判断扩展节点si是否是目标点sg；若是，则搜索结束；若不是，则继续扩展该节点si的子节点si+1；
步骤S505、判断扩展子节点si+1否与障碍物发生碰撞(是否在障碍物里面)，若是，则舍弃该子节点；若不是，跳入下一步骤；
步骤S506、将出现在同一网格中的当前扩展子节点si+1与原有节点sopen的实际代价进行比较，当前扩展子节点的实际代价g(si+1)低于原有节点的实际代价g(sopen)时，将当前扩展子节点的连续状态与该网格单元关联，并将当前扩展子节点si+1所在栅格设置为子节点所在栅格的父栅格，并将扩展子节点放入Open_Set列表中，更新扩展子节点的总代价f(si+1)；否则，丢弃当前搜索的子节点。
步骤S507、循环上述过程中步骤S503到步骤S506，直到搜索到目标节点，并将目标节点放入Open_Set列表；
步骤S508、从Open_Set列表中的目标节点开始回朔所有遍历过的父节点，直至起始节点，得到所搜索的路径，路径搜索完毕，算法终止；如果Open_Set列表为空，则表明没有规划出可行路径，算法终止。
图7为以扩展3个前向节点为例展示了混合A*算法的搜索实例。
步骤S403、根据所述粗路径，基于梯度下降优化算法的轨迹平滑器，对混合搜索的粗路径进行优化，构建出适合地面多机器人安全执行、连续平滑的路径。
尽管上述混合A*算法可以输出一条可供机器人执行的连续路径，但机器人执行过程中通常包含许多非必要的转向动作，另外，在利用拖拽水带机器人进行灭火任务的场景中，还需解决环境中的障碍物对机器人水带拖拽过程中的缠绕问题。
因此，在本实施例中，公开了基于多引导点作用下的路径平滑器，处理得到安全可靠更优的路径。
所述梯度下降平滑器，基于包括障碍物项、曲率项、平滑项、voronoi场项和多引导点函数项，建立最小化代价函数，利用梯度下降法求解最优路径。
具体的，梯度下降平滑器的最小化代价函数P：
P＝Pobs+Pcur+Psmo+Pvor+Pgui (2)
(1)障碍物项
当对于每一个搜索节点xi有|xi-oi|≤dmax时，定义代价函数Pobs为：
式中，N为总节点数；xi为路径上顶点的二维平面坐标；oi为距离节点xi最近的障碍物的位置；dmax为障碍物影响代价函数的最大距离的阈值；σobs为二次惩罚函数，使得节点越接近障碍物时惩罚值越大；障碍物权重wobs为障碍物影响路径变化的系数。这一项可以使机器人有效地避免与障碍物发生碰撞。
(2)曲率项
为了确保路径可执行，曲率项设定了每个顶点的瞬时曲率的上限，即假设
定义代价函数Pcur为：
式中，顶点xi处的位移向量定义为Δxi＝xi-xi-1，顶点切向角度的变化值表示为
最大允许曲率用κmax表示；σcur为二次惩罚函数；曲率权重wcur控制对路径变化的影响。这一项限制了每个节点处轨迹的瞬时曲率，并强制执行车辆的非完整约束。
(3)平滑项
平滑项评估顶点之间的位移向量。该项能够平滑间隔不均匀以及方向变化幅度大的节点。定义代价函数Psmo为：
式中，wsmo表示平滑权重，决定了平滑项对路径变化的影响。
(4)维诺势场函数项
对于
定义代价函数Pvor为
式中，dO表示节点到达最近障碍物的距离；dedg表示节点到达广义泰森多边形最近边缘的距离；
表示影响到Voronoi势场函数的障碍物的最大距离；α控制场的衰减率；wvor为权重系数，代表对路径的影响。这一项能够有效地引导路径远离狭窄或宽阔通道的障碍物。越靠近障碍物，Pvor的值越大，越接近1；越靠近voronoi边，Pvor的值越接近0。
(5)多引导点函数项
当对于每一个搜索节点xi有|xi-gi|＜ρmax时，定义代价函数Pgui为：
式中，xi为路径上顶点的二维平面坐标；gi为距离节点xi最近的引导点的位置；ρmax为引导点影响代价函数的最大距离的阈值；引导点权重wgui为引导点影响路径变化的系数。当路径搜索节点越靠近引导点距离ρmax的边界，Pgui的值越大；越靠近引导点，Pgui的值越小。这一项可以路径节点快速引导到引导点附近。
确定代价函数P之后，利用梯度下降法求解最优路径，在梯度下降算法的实际运用中，通常选择梯度的绝对值作为算法停止的标准，通过限制最大的迭代次数保证机器人运动的连续性。
综上所述，本发明实施例公开的基于空地协同的多智能消防机器人消防水带防缠绕方法，实现了拖拽着消防水带的智能消防机器人在执行消防任务时，消防水带交叉现象的发现，确定消防水带出现交叉的各机器人的移动优先级；以及通过在机器人在前往新的目标位置执行新任务时的路径规划，防止了机器人在移动中消防水带之间或者消防水带与障碍物的缠绕问题。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种基于空地协同的多智能消防机器人消防水带防缠绕方法，其特征在于，包括：
步骤S101、基于空基影像采集监测多个机器人的移动；所述机器人为拖拽着消防水带的智能消防机器人；
步骤S102、根据监测数据确定出，消防水带出现交叉的各机器人的移动优先级；
步骤S103、机器人在前往新的目标位置执行新任务时，根据消防水带的交叉情况，判断机器人移动是否会造成消防水带之间的缠绕；否，进入步骤S104，是，进入步骤S105；
步骤S104、对执行新任务的机器人进行路径规划，按照规划路径移动到新的目标位置；
步骤S105、按照移动优先级从高到低依次对机器人进行路径规划；按照规划路径依次进行移动到各机器人的目标位置，防止机器人在移动过程中水带缠绕。
2.根据权利要求1所述的多智能消防机器人消防水带防缠绕方法，其特征在于，若消防水带出现交叉现象，各机器人的移动优先级确定方法为：
根据监测数据确定出消防水带被压的先后次序；
按照消防水带被压的先后次序依次确定机器人移动优先级，水带压在最上方的机器人移动优先级最高，压在最下方的机器人移动优先级最低。
3.根据权利要求1所述的多智能消防机器人消防水带防缠绕方法，其特征在于，所述路径规划采用基于多引导点牵引作用的改进混合A*路径规划算法，使得机器人规划的路径能被牵引到设定的多个引导点附近，在多障碍物环境下规划出安全可靠的路径。
4.根据权利要求3所述的多智能消防机器人消防水带防缠绕方法，其特征在于，
具体的路径规划，包括以下步骤：
建立用于进行路径搜索的数字地图；
在数字地图中采用混合A*搜索，搜索出基于消防机器人运动学和最小安全转向约束，满足路径连续性的粗路径；
根据所述粗路径，基于梯度下降优化算法的轨迹平滑器，对混合搜索的粗路径进行优化，构建出适合地面多机器人安全执行、连续平滑的路径。
5.根据权利要求4所述的多智能消防机器人消防水带防缠绕方法，其特征在于，
所述数字地图基于遥感观测数据生成的机器人可通达路径的二值栅格地图，以及利用二值栅格地图建立Voronoi图，进行结合构建出的混合地图。
6.根据权利要求5所述的多智能消防机器人消防水带防缠绕方法，其特征在于，
所述混合A*搜索，具体包括：
步骤S501、输入混合地图；
步骤S502、进行搜索的初始化；
所述初始化包括建立Open_Set和Closed_Set两个列表，两个列表初始化时都为空列表，其中，Open_Set列表存储待扩展节点，Closed_Set列表存储已扩展的节点或存在障碍物的节点；
步骤S503、判断Open_Set列表为非空列表，从Open_Set列表中选择总代价最小的节点作为扩展节点进行扩展，得到与扩展节点状态连续的多个扩展子节点，并将扩展节点从Open_Set列表移除至Closed_Set列表中；
步骤S504、判断扩展节点是否是目标点；若是，则搜索结束；若不是，则继续扩展该节点的子节点；
步骤S505、判断扩展子节点否与障碍物发生碰撞，若是，则舍弃该子节点；若不是，跳入下一步骤；
步骤S506、将出现在同一网格中的当前扩展子节点与原有节点的实际代价进行比较，当前扩展子节点的实际代价低于原有节点的实际代价时，将当前扩展子节点的连续状态与该网格单元关联，并将当前扩展子节点所在栅格设置为子节点所在栅格的父栅格，并将扩展子节点放入Open_Set列表中，更新扩展子节点的总代价；
步骤S507、循环上述过程中步骤S503到步骤S506，直到搜索到目标节点，并将目标节点放入Open_Set列表；
步骤S508、从Open_Set列表中的目标节点开始回朔所有遍历过的父节点，直至起始节点，得到所搜索的路径，路径搜索完毕，算法终止；如果Open_Set列表为空，则表明没有规划出可行路径，算法终止。
7.根据权利要求6所述的多智能消防机器人消防水带防缠绕方法，其特征在于，
搜索过程中每个节点的总代价包括实际代价和启发式估计代价；
所述实际代价为起始节点到当前节点的代价，包括当前节点的父节点代价以及父节点到当前节点的代价，其中后者包括了父节点到当前节点的实际路径长度、二值栅格地图中父节点到当前节点的转向代价和Voronoi图中从父节点到当前节点的代价；
所述启发式估计代价为当前节点到目标节点的启发式估计代价，由基于非完整约束的启发式函数和基于完整约束的启发式函数中较大的值决定。
8.根据权利要求6所述的多智能消防机器人消防水带防缠绕方法，其特征在于，
基于多引导点牵引作用进行路径优化中，采用基于多引导点作用下的路径平滑器，对粗路径进行平滑处理得到适合地面机器人安全执行、连续平滑的路径。
9.根据权利要求8所述的多智能消防机器人消防水带防缠绕方法，其特征在于，
所述梯度下降平滑器中，基于包括障碍物项、曲率项、平滑项、voronoi场项和多引导点函数项，建立最小化代价函数，利用梯度下降法求解最优路径；
所述最小化代价函数P＝Pobs+Pcur+Psmo+Pvor+Pgui；
Pobs障碍物项的代价函数；Pcur曲率项的代价函数；Psmo为平滑项的代价函数；
Pvor维诺势场函数项的代价函数；Pgui为多引导点函数项的代价函数。
10.根据权利要求9所述的多智能消防机器人消防水带防缠绕方法，其特征在于，
当对于每一个搜索节点xi有|xi-gi|＜ρmax时，所述多引导点函数项的代价函数Pgui为：
式中，xi为路径上顶点的二维平面坐标；gi为距离节点xi最近的引导点的位置；ρmax为引导点影响代价函数的最大距离的阈值；引导点权重wgui为引导点影响路径变化的系数。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211124347.8A CN115562255B (zh) | 2022-09-13 | 2022-09-13 | 基于空地协同的多智能消防机器人消防水带防缠绕方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211124347.8A CN115562255B (zh) | 2022-09-13 | 2022-09-13 | 基于空地协同的多智能消防机器人消防水带防缠绕方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115562255A true CN115562255A (zh) | 2023-01-03 |
| CN115562255B CN115562255B (zh) | 2024-07-19 |
Family
ID=84741971
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211124347.8A Active CN115562255B (zh) | 2022-09-13 | 2022-09-13 | 基于空地协同的多智能消防机器人消防水带防缠绕方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115562255B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108549388A (zh) * | 2018-05-24 | 2018-09-18 | 苏州智伟达机器人科技有限公司 | 一种基于改进a星策略的移动机器人路径规划方法 |
| CN110989599A (zh) * | 2019-12-09 | 2020-04-10 | 国网智能科技股份有限公司 | 一种变电站消防机器人自主作业控制方法及*** |
| CN110975194A (zh) * | 2019-12-09 | 2020-04-10 | 国网智能科技股份有限公司 | 一种变电站消防机器人辅助方法及*** |
| CN112161627A (zh) * | 2020-09-23 | 2021-01-01 | 同济大学 | 一种消防机器人智能路径规划方法 |
| CN112327856A (zh) * | 2020-11-13 | 2021-02-05 | 云南电网有限责任公司保山供电局 | 一种基于改进A-star算法的机器人路径规划方法 |
| CN113144470A (zh) * | 2021-04-07 | 2021-07-23 | 安徽相品智能科技有限公司 | 一种消防应急预警处理灭火一体化控制*** |
| CN113359718A (zh) * | 2021-05-26 | 2021-09-07 | 西安理工大学 | 移动机器人全局路径规划与局部路径规划融合方法及设备 |
| CN113485369A (zh) * | 2021-08-03 | 2021-10-08 | 浙江大学 | 改进a*算法的室内移动机器人路径规划和路径优化方法 |
| CN114166235A (zh) * | 2021-12-06 | 2022-03-11 | 福建工程学院 | 一种基于优化a-star算法的全局动态平滑路径规划方法 |
| CN115373399A (zh) * | 2022-09-13 | 2022-11-22 | 中国安全生产科学研究院 | 一种基于空地协同的地面机器人路径规划方法 |
-
2022
- 2022-09-13 CN CN202211124347.8A patent/CN115562255B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108549388A (zh) * | 2018-05-24 | 2018-09-18 | 苏州智伟达机器人科技有限公司 | 一种基于改进a星策略的移动机器人路径规划方法 |
| CN110989599A (zh) * | 2019-12-09 | 2020-04-10 | 国网智能科技股份有限公司 | 一种变电站消防机器人自主作业控制方法及*** |
| CN110975194A (zh) * | 2019-12-09 | 2020-04-10 | 国网智能科技股份有限公司 | 一种变电站消防机器人辅助方法及*** |
| CN112161627A (zh) * | 2020-09-23 | 2021-01-01 | 同济大学 | 一种消防机器人智能路径规划方法 |
| CN112327856A (zh) * | 2020-11-13 | 2021-02-05 | 云南电网有限责任公司保山供电局 | 一种基于改进A-star算法的机器人路径规划方法 |
| CN113144470A (zh) * | 2021-04-07 | 2021-07-23 | 安徽相品智能科技有限公司 | 一种消防应急预警处理灭火一体化控制*** |
| CN113359718A (zh) * | 2021-05-26 | 2021-09-07 | 西安理工大学 | 移动机器人全局路径规划与局部路径规划融合方法及设备 |
| CN113485369A (zh) * | 2021-08-03 | 2021-10-08 | 浙江大学 | 改进a*算法的室内移动机器人路径规划和路径优化方法 |
| CN114166235A (zh) * | 2021-12-06 | 2022-03-11 | 福建工程学院 | 一种基于优化a-star算法的全局动态平滑路径规划方法 |
| CN115373399A (zh) * | 2022-09-13 | 2022-11-22 | 中国安全生产科学研究院 | 一种基于空地协同的地面机器人路径规划方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115562255B (zh) | 2024-07-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115373399B (zh) | 一种基于空地协同的地面机器人路径规划方法 | |
| Hu et al. | Voronoi-based multi-robot autonomous exploration in unknown environments via deep reinforcement learning | |
| Dang et al. | Graph-based path planning for autonomous robotic exploration in subterranean environments | |
| CN107168305B (zh) | 路口场景下基于Bezier和VFH的无人车轨迹规划方法 | |
| Gao et al. | An improved frontier-based approach for autonomous exploration | |
| Dang et al. | Explore locally, plan globally: A path planning framework for autonomous robotic exploration in subterranean environments | |
| CN115079705A (zh) | 基于改进a星融合dwa优化算法的巡检机器人路径规划方法 | |
| Reinhart et al. | Learning-based path planning for autonomous exploration of subterranean environments | |
| Kim et al. | PLGRIM: Hierarchical value learning for large-scale exploration in unknown environments | |
| Yang et al. | Graph-based topological exploration planning in large-scale 3d environments | |
| Ntakolia et al. | Autonomous path planning with obstacle avoidance for smart assistive systems | |
| Lidoris et al. | The autonomous city explorer (ACE) project—mobile robot navigation in highly populated urban environments | |
| CN113433937B (zh) | 基于启发式探索的分层导航避障***、分层导航避障方法 | |
| CN117406771B (zh) | 一种基于四旋翼无人机的高效自主探索方法、***及设备 | |
| Petit et al. | Tape: Tether-aware path planning for autonomous exploration of unknown 3d cavities using a tangle-compatible tethered aerial robot | |
| CN117371760B (zh) | 考虑截止时间和拥塞缓解的分层式客船人员应急疏散方法 | |
| Yang et al. | Path planning and collision avoidance methods for distributed multi-robot systems in complex dynamic environments | |
| Yan et al. | Mui-tare: Cooperative multi-agent exploration with unknown initial position | |
| Rocha et al. | Plannie: A benchmark framework for autonomous robots path planning algorithms integrated to simulated and real environments | |
| Mohammad et al. | A robust and fast occlusion-based frontier method for autonomous navigation in unknown cluttered environments | |
| CN115562255A (zh) | 基于空地协同的多智能消防机器人消防水带防缠绕方法 | |
| Masehian et al. | NRR: a nonholonomic random replanner for navigation of car-like robots in unknown environments | |
| CN116718190A (zh) | 一种长距离密集人群场景下的移动机器人路径规划方法 | |
| Dang | Autonomous mobile robot path planning based on enhanced A* algorithm integrating with time elastic band | |
| CN115326070A (zh) | 城市低空环境下基于三维切线图法的无人机航迹规划方法 |
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 |