CN113848881A - 一种消防车路径规划方法、***、终端以及存储介质 - Google Patents

一种消防车路径规划方法、***、终端以及存储介质 Download PDF

Info

Publication number
CN113848881A
CN113848881A CN202111011479.5A CN202111011479A CN113848881A CN 113848881 A CN113848881 A CN 113848881A CN 202111011479 A CN202111011479 A CN 202111011479A CN 113848881 A CN113848881 A CN 113848881A
Authority
CN
China
Prior art keywords
path
fire fighting
fire
tree
planning
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
CN202111011479.5A
Other languages
English (en)
Other versions
CN113848881B (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.)
Nari Technology Co Ltd
Original Assignee
Nari 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 Nari Technology Co Ltd filed Critical Nari Technology Co Ltd
Priority to CN202111011479.5A priority Critical patent/CN113848881B/zh
Publication of CN113848881A publication Critical patent/CN113848881A/zh
Application granted granted Critical
Publication of CN113848881B publication Critical patent/CN113848881B/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/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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

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

一种消防车路径规划方法、***、终端以及存储介质
技术领域
本发明属于路径规划技术领域，具体涉及一种消防车路径规划方法、***、终端以及存储介质。
背景技术
随着科学技术不断进步发展，机器人技术在消防中得到了应用，但在实际应用过程中，智能化控制技术并没有很好得到展现。对于消防机器人，以高效规划出一条从起点到消防灭火点无碰撞安全的路径是智能化的基础，也是智能化消防机器人重要组成部分。在传统路径规划方法，常见的有栅格地图搜索法、粒子群算法、快速随机树搜索法。然而，栅格地图搜索法容易构建，但路径规划效率低，空间浪费，栅格的分辨率不依赖于环境的复杂度。粒子群算法运算量大，效率低下，编程代码繁琐。快速随机树搜索法虽然能在复杂环境总能搜索一条路径到达目标点，但对于环境简单的同样也需要较长时间，搜索效率低，影响消防灭火工作效率。
申请号为202011012951.2的中国发明专利申请中，公开了一种消防机器人智能路径规划方法，提出MAKLINK图论建立环境，利用Dijkstra算法和改进型蚁群算法，能够在火场空间环境中实现路径规划，但方法收敛速度慢，搜索效率低，并且针对动态障碍物无法进行动态避障规划。
申请号201611070810.X的中国发明专利申请中，公开了一种机器人路径规划方法，该规划方法采用最大转角限制探索随机点的快速扩展树算法，并以机器人安全半径圆弧进行建立动态窗口检测对动态障碍物局部避障规划。该快速扩展树算法是以最大转角限制探索随机点，限制了搜索方向和范围，容易出现局部无解，并且对于环境模型简单不能快速搜索到达目标点；建立动态窗口检测对动态障碍物局部避障规划，没有对动态障碍物运动预测，造成动态障碍物环境模型是不确定，局部规划得到的路径不能避开障碍物；机器人实时位姿是通过里程计获取存在滑动误差，没有进行位姿修正，局部规划初始点存在误差，造成动态避障规划效果差。
发明内容
针对上述问题，本发明提出一种消防车路径规划方法、***、终端以及存储介质，能快速完成搜索最优路径，并能对动态障碍物进行避障规划，实现控制消防车无碰撞安全到达目标点。
为了实现上述技术目的，达到上述技术效果，本发明通过以下技术方案实现：
第一方面，本发明提供了一种消防车路径规划方法，包括：
获取环境模型，所述环境模型中包括固定障碍物相对于消防车库为世界坐标系下的位姿；
基于所述环境模型，获得搜索边界信息和障碍物信息，并采用目标趋向快速扩展树搜索，获得全局路径；
基于所述全局路径，发送控制信号至消防车，使得消防车从消防车库沿着全局路径走向目标点；
接收消防车行走过程中反馈的固定障碍物信息，并基于接收到的固定障碍物位姿修正消防车当前位姿；
接收消防车行走过程中反馈的动态障碍物信息，基于接收到的动态障碍物进行障碍物运动碰撞预测，并基于预测结果确定是否进行局部路径规划，并利用局部路径规划更新全局路径。
可选地，所述全局路径的获得方法包括：
S1：获取当前位置为路径规划开始父节点Tree0和消防最佳目标地位置为路径规划目标点Treee；
S2：基于当前节点和路径规划目标点，生成趋向目标临时点；
S3：检测趋向目标临时点是否与障碍物发生碰撞，若没有碰撞则将该趋向目标临时点eTem添加为树节点Treei+1，判断是否到达路径规划目标点，到达路径规划目标点时完成路径搜索，保存全局路径Tree，所述全局路径Tree包括若干个树节点；如果没有到达路径规划目标点则跳到S2；如果趋向目标临时点与障碍物发生碰撞，则生成指向性临时点并跳到S3；
S4：采用贝塞尔曲线进行全局路径Tree优化处理，得到平滑曲线。
可选地，所述趋向目标临时点的通过以下公式生成：
Figure BDA0003238599760000021
其中，eTem为趋向目标临时点，Treei为当前节点，Treee为路径规划目标点，ε为搜索步长。
可选地，所述指向性临时点通过以下公式生成：
indTem＝Treei+ε·I
其中，indTem为指向性临时点，I为单位化随机向量，Treei为当前节点，ε为搜索步长。
可选地，所述消防车当前相对于世界坐标系的位姿通过以下公式修正，具体为：
Figure BDA0003238599760000031
其中，
Figure BDA0003238599760000032
为消防车当前相对于世界坐标系的位姿，
Figure BDA0003238599760000033
为第i号固定障碍物相对消防车坐标系，
Figure BDA0003238599760000034
为基于世界坐标系第i号固定障碍物的位姿，其中定义消防车库停车点为世界坐标系，inv()表示矩阵的求逆运算。
可选地，所述障碍物运动碰撞预测方法包括：
Ti时刻动态障碍物相对于世界坐标系位置为
Figure BDA0003238599760000035
Ti+Δt时刻动态障碍物相对于世界坐标系位置为
Figure BDA0003238599760000036
预测Ti+ΔT时刻障碍物相对于世界坐标系运动位姿为：
Figure BDA0003238599760000037
其中，
Figure BDA0003238599760000038
为Ti+ΔT时刻障碍物预测位置，
Figure BDA0003238599760000039
为Ti时刻消防车相对于世界坐标系位置，
Figure BDA00032385997600000310
为Ti时刻传感器获取动态障碍物相对于消防车位置，
Figure BDA00032385997600000311
为Ti+1时刻消防车相对于世界坐标系位置，
Figure BDA00032385997600000312
为Ti+1时刻传感器获取动态障碍物相对于消防车位置，ΔT＞Δt。
可选地，所述局部路径的规划方法包括：
将Ti+ΔT时刻障碍物预测位置
Figure BDA00032385997600000313
更新到环境模型中；
以Ti时刻消防车位置
Figure BDA00032385997600000314
为路径规划开始父节点，以全局路径的Ti+Δt时刻位置为路径规划目标点，进行上述目标趋向快速扩展树搜索方法搜索局部路径，将原全局路径的Ti～Ti+ΔT时刻路点替换为所述规划局部路径的路点，完成更新全局路径。
第二方面，本发明提供了一种消防车路径规划***，包括：
获取模块，用于获取环境模型，所述环境模型中包括固定障碍物相对于消防车库为世界坐标系下的位姿；
全局路径规划模块，用于基于所述环境模型，获得搜索边界信息和障碍物信息，并采用目标趋向快速扩展树搜索，获得全局路径；
发送模块，用于基于所述全局路径，发送控制信号至消防车，使得消防车从消防车库沿着全局路径走向目标点；
修正模块，用于接收消防车行走过程中反馈的固定障碍物信息，并基于接收到的固定障碍物位姿修正消防车当前位姿；
局部路径规划模块，用于接收消防车行走过程中反馈的动态障碍物信息，基于接收到的动态障碍物进行障碍物运动碰撞预测，并基于预测结果确定是否进行局部路径规划，并利用局部路径规划更新全局路径。
第三方面，本发明提供了一种消防车路径规划终端，包括处理器、与所述处理器耦接的存储器；
所述存储器存储有用于实现第一方面任一项所述的消防车路径规划方法的程序指令；
所述处理器用于执行所述存储器存储的所述程序指令以控制消防车路径规划以及处理传感信息。
第四方面，本发明提供了一种存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一方面任一项所述方法的步骤。
与现有技术相比，本发明的有益效果：
本发明一种消防车路径规划方法、***、终端以及存储介质，能快速完成搜索最优路径，并能对动态障碍物进行避障规划，实现控制消防车无碰撞安全到达目标点，减少消防车发生碰撞、无法自主规划到达目标点，从而无法完成消防灭火任务。本发明能够加快路径规划速度，提高消防灭火效率，减少人民生命财产损失，具有较高经济实用价值。
附图说明
为了使本发明的内容更容易被清楚地理解，下面根据具体实施例并结合附图，对本发明作进一步详细的说明，其中：
图1为本发明的一种消防车路径规划方法流程示意图；
图2为本发明的目标趋向快速扩展树搜索方法流程示意图；
图3为本发明的全局路径规划的路径示意图；
图4为本发明的局部规划路径示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明的保护范围。
下面结合附图对本发明的应用原理作详细的描述。
由于现有消防车路径规划控制***智能化程度低，不能在陌生环境快速完成指定避障规划，大多数规划方法收敛速度慢，搜索效率低；对动态障碍物没有进行动态避障规划，或者避障效果不明显，没有对动态障碍物运动预测，消防车当前位姿获取不精准，造成局部规划避障效果差。为此，本发明中提供了一种消防车路径规划方法、***、终端以及存储介质，采用目标趋向快速扩展树搜索方法，完成全局路径规划，解决了收敛速度慢、搜索效率低等问题，实现了快速搜索得到最优路径；采用根据固定障碍物位姿修正当前消防车位姿和对动态障碍物运动检测方法，完成对动态障碍物进行避障规划，解决了没有对动态障碍物运动预测，消防车当前位姿获取不精准，造成局部规划避障效果差等问题，实现了对动态障碍物精准避障顺利到达目标点。
实施例1
本发明实施例中提供了一种消防车路径规划方法，包括：
获取环境模型，所述环境模型中包括固定障碍物相对于消防车库为世界坐标系下的位姿；
基于所述环境模型，获得搜索边界信息和障碍物信息，并采用目标趋向快速扩展树搜索，获得全局路径；
基于所述全局路径，发送控制信号至消防车，使得消防车从消防车库沿着全局路径走向目标点；
接收消防车行走过程中反馈的固定障碍物信息，并基于接收到的固定障碍物位姿修正消防车当前位姿；
接收消防车行走过程中反馈的动态障碍物信息，基于接收到的动态障碍物进行障碍物运动碰撞预测，并基于预测结果确定是否进行局部路径规划，并利用局部路径规划更新全局路径。
在本发明实施例的一种具体实施方式中，如图3所示，所述全局路径的获得方法包括：
S1：获取当前位置为路径规划开始父节点Tree0和消防最佳目标地位置为路径规划目标点Treee；
S2：基于当前节点和路径规划目标点，生成趋向目标临时点；
S3：检测趋向目标临时点是否与障碍物发生碰撞，若没有碰撞则将该趋向目标临时点eTem添加为树节点Treei+1，判断是否到达路径规划目标点，到达路径规划目标点时完成路径搜索，保存全局路径Tree，所述全局路径Tree包括若干个树节点；如果没有到达路径规划目标点则跳到S2；如果趋向目标临时点与障碍物发生碰撞，则生成指向性临时点并跳到S3；
S4：采用贝塞尔曲线进行全局路径Tree优化处理，得到平滑曲线。
其中，所述趋向目标临时点的通过以下公式生成：
Figure BDA0003238599760000061
其中，eTem为趋向目标临时点，Treei为当前节点，Treee为路径规划目标点，ε为搜索步长。
所述指向性临时点通过以下公式生成：
indTem＝Treei+ε·I
其中，indTem为指向性临时点，I为单位化随机向量，Treei为当前节点，ε为搜索步长。
在本发明实施例的一种具体实施方式中，所述消防车当前相对于世界坐标系的位姿通过以下公式修正，具体为：
Figure BDA0003238599760000062
其中，
Figure BDA0003238599760000063
为消防车当前相对于世界坐标系的位姿，
Figure BDA0003238599760000064
为第i号固定障碍物相对消防车坐标系，
Figure BDA0003238599760000065
为基于世界坐标系第i号固定障碍物的位姿，其中定义消防车库停车点为世界坐标系，inv()表示矩阵的求逆运算。
在本发明实施例的一种具体实施方式中，如图2所示，所述障碍物运动碰撞预测方法包括：
Ti时刻动态障碍物相对于世界坐标系位置为
Figure BDA0003238599760000066
Ti+Δt时刻动态障碍物相对于世界坐标系位置为
Figure BDA0003238599760000067
预测Ti+ΔT时刻障碍物相对于世界坐标系运动位姿为：
Figure BDA0003238599760000068
其中，
Figure BDA0003238599760000069
为Ti+ΔT时刻障碍物预测位置，
Figure BDA00032385997600000610
为Ti时刻消防车相对于世界坐标系位置，
Figure BDA00032385997600000611
为Ti时刻传感器获取动态障碍物相对于消防车位置，
Figure BDA00032385997600000612
为Ti+1时刻消防车相对于世界坐标系位置，
Figure BDA00032385997600000613
为Ti+1时刻传感器获取动态障碍物相对于消防车位置，ΔT＞Δt。
在本发明实施例的一种具体实施方式中，如图4所示，所述局部路径的规划方法包括：
将Ti+ΔT时刻障碍物预测位置
Figure BDA00032385997600000614
更新到环境模型中；
以Ti时刻消防车位置
Figure BDA00032385997600000615
为路径规划开始父节点，以全局路径的Ti+Δt时刻位置为路径规划目标点，进行上述目标趋向快速扩展树搜索方法搜索局部路径，将原全局路径的Ti～Ti+ΔT时刻路点替换为所述规划局部路径的路点，完成更新全局路径。
下面结合图1和图2对本发明实施例中的消防车路径规划方法进行详细说明。
步骤100：建立环境模型。
通过人工使用测量工具或者使用消防车激光/视觉传感器对环境进行数据测量，以消防车库为世界坐标系原点，标定出固定障碍物位姿
Figure BDA0003238599760000071
其中i代表固定障碍物的序号。
步骤200：根据步骤100所建立的环境模型数据信息，得到搜索边界信息和障碍物信息，采用目标趋向快速扩展树搜索，完成全局路径规划。具体步骤如下：
第一步：获取当前位置为路径规划开始父节点Tree0和消防最佳位置为路径规划目标点Treee；
第二步：生成趋向目标临时点；
第三步：检测临时点是否与障碍物发生碰撞，若没有碰撞则将该临时点Tem添加为树节点Treei+1，判断是否到达目标点，到达目标点时完成路径搜索，寄存器保存全局路径Tree，所述的全局路径Tree由树节点Treei组成；如果没有到达目标点则跳到第二步；如果检测临时点与障碍物发生碰撞，则生成指向性临时点跳到第三步。
所述趋向目标临时点的计算公式为
Figure BDA0003238599760000072
Treei为当前节点，Treee为目标点，ε为搜索步长，朝着目标点搜索，减少生成无用临时点和路径弯折，加快了无障碍时搜索速度。
所述指向性临时点的计算公式为indTem＝Treei+ε·I，I为单位化随机向量，进行随机选着向量集{[a1,b1,c1]T；[a2,b2,c2]T；…；[an,bn,cn]T}。采用随机向量集作用是限制临时点随机增长方向，增快搜索速度，减少无用方向增长；
第四步：采用贝塞尔曲线进行路径优化处理，得到平滑曲线，减少消防车移动弯折转向。
步骤300：根据步骤200得到全局路径Tree，控制消防车从消防车车库沿着全局路径走向目标点，行走过程中通过消防车视觉/激光雷达不断检测障碍物；
步骤310：根据步骤300消防车视觉/激光雷达不断检测障碍物，检测到固定障碍物位姿修正当前机器人位姿，修正当前消防车位姿
Figure BDA0003238599760000073
其中
Figure BDA0003238599760000074
为通过消防车传感器视野内获取第i号固定障碍物相对消防车坐标系，
Figure BDA0003238599760000075
为第i号固定障碍物相对于以消防车库为世界坐标系位姿，inv()表示矩阵的求逆运算。
步骤320：根据步骤300消防车视觉/激光雷达不断检测障碍物，检测到动态障碍物，则进行障碍物运动碰撞预测：Ti时刻动态障碍物相对于世界坐标系位置
Figure BDA0003238599760000081
Ti+Δt时刻动态障碍物相对于世界坐标系位置
Figure BDA0003238599760000082
预测Ti+ΔT时刻障碍物相对于世界坐标系运动位姿
Figure BDA0003238599760000083
其中
Figure BDA0003238599760000084
为Ti时刻消防车相对于世界坐标系位置，
Figure BDA0003238599760000085
为Ti时刻传感器获取动态障碍物相对于消防车位置，
Figure BDA0003238599760000086
为Ti+1时刻消防车相对于世界坐标系位置，
Figure BDA0003238599760000087
为Ti+1时刻传感器获取动态障碍物相对于消防车位置，ΔT＞Δt。
步骤330：根据步骤320进行预测障碍物运动与消防车将发生碰撞可能，进行局部路径规划：
Ti+ΔT时刻障碍物预测位置
Figure BDA0003238599760000088
更新到建立环境模型中，以Ti时刻消防车位置
Figure BDA0003238599760000089
为路径规划开始父节点，以全局路径的Ti+ΔT时刻位置为路径规划目标点，进行上述目标趋向快速扩展树搜索方法对局部路径进行搜索，将原全局路径的Ti～Ti+ΔT时刻路点替换为所述规划局部路径的路点，完成更新全局路径。
步骤331：根据步骤320动态障碍物运动预测，如果没有预测到障碍物与消防车发生碰撞，则继续下发当前全局路径给消防车，并且跳到步骤300继续进行下一时刻障碍物检测，直至消防车到达消防目的地后停止下发全局路径给消防车以及停止障碍物检测。。
实施例2
本发明实施例中提供了一种消防车路径规划***，包括：
获取模块，用于获取环境模型，所述环境模型中包括固定障碍物相对于消防车库为世界坐标系下的位姿；
全局路径规划模块，用于基于所述环境模型，获得搜索边界信息和障碍物信息，并采用目标趋向快速扩展树搜索，获得全局路径；
发送模块，用于基于所述全局路径，发送控制信号至消防车，使得消防车从消防车库沿着全局路径走向目标点；
修正模块，用于接收消防车行走过程中反馈的固定障碍物信息，并基于接收到的固定障碍物位姿修正消防车当前位姿；
局部路径规划模块，用于接收消防车行走过程中反馈的动态障碍物信息，基于接收到的动态障碍物进行障碍物运动碰撞预测，并基于预测结果确定是否进行局部路径规划，并利用局部路径规划更新全局路径。
其余部分均与实施例1相同。
实施例3
本发明实施例中提供了一种消防车路径规划终端，包括处理器、与所述处理器耦接的存储器；
所述存储器存储有用于实现实施例1任一项所述的消防车路径规划方法的程序指令；
所述处理器用于执行所述存储器存储的所述程序指令以控制消防车路径规划以及处理传感信息。
实施例4
本发明实施例中提供了一种存储介质，其上存储有计算机程序，该程序被处理器执行时实现实施例1中任一项所述方法的步骤。
以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (10)

1.一种消防车路径规划方法，其特征在于，包括：
获取环境模型，所述环境模型中包括固定障碍物相对于消防车库为世界坐标系下的位姿；
基于所述环境模型，获得搜索边界信息和障碍物信息，并采用目标趋向快速扩展树搜索，获得全局路径；
基于所述全局路径，发送控制信号至消防车，使得消防车从消防车库沿着全局路径走向目标点；
接收消防车行走过程中反馈的固定障碍物信息，并基于接收到的固定障碍物位姿修正消防车当前相对于世界坐标系的位姿；
接收消防车行走过程中反馈的动态障碍物信息，基于接收到的动态障碍物进行障碍物运动碰撞预测，并基于预测结果确定是否进行局部路径规划，并利用局部路径规划更新全局路径。
2.根据权利要求1所述的一种消防车路径规划方法，其特征在于，所述全局路径的获得方法包括：
S1：获取当前位置为路径规划开始父节点Tree0和消防最佳目标地位置为路径规划目标点Treee；
S2：基于当前节点和路径规划目标点，生成趋向目标临时点；
S3：检测趋向目标临时点是否与障碍物发生碰撞，若没有碰撞则将该趋向目标临时点eTem添加为树节点Treei+1，判断是否到达路径规划目标点，到达路径规划目标点时完成路径搜索，保存全局路径Tree，所述全局路径Tree包括若干个树节点；如果没有到达路径规划目标点则跳到S2；如果趋向目标临时点与障碍物发生碰撞，则生成指向性临时点并跳到S3；
S4：采用贝塞尔曲线进行全局路径Tree优化处理，得到平滑曲线。
3.根据权利要求2所述的一种消防车路径规划方法，其特征在于：所述趋向目标临时点的通过以下公式生成：
Figure FDA0003238599750000011
其中，eTem为趋向目标临时点，Treei为当前节点，Treee为路径规划目标点，ε为搜索步长。
4.根据权利要求2所述的一种消防车路径规划方法，其特征在于：所述指向性临时点通过以下公式生成：
indTem＝Treei+ε·I
其中，indTem为指向性临时点，I为单位化随机向量，Treei为当前节点，ε为搜索步长。
5.根据权利要求1所述的一种消防车路径规划方法，其特征在于，所述消防车当前相对于世界坐标系的位姿通过以下公式修正，具体为：
Figure FDA0003238599750000021
其中，
Figure FDA0003238599750000022
为消防车当前相对于世界坐标系的位姿，
Figure FDA0003238599750000023
为第i号固定障碍物相对消防车坐标系，
Figure FDA0003238599750000024
为基于世界坐标系第i号固定障碍物的位姿，其中定义消防车库停车点为世界坐标系，inv()表示矩阵的求逆运算。
6.根据权利要求1所述的一种消防车路径规划方法，其特征在于，所述障碍物运动碰撞预测方法包括：
Ti时刻动态障碍物相对于世界坐标系位置为
Figure FDA0003238599750000025
Ti+Δt时刻动态障碍物相对于世界坐标系位置为
Figure FDA0003238599750000026
预测Ti+ΔT时刻障碍物相对于世界坐标系运动位姿为：
Figure FDA0003238599750000027
其中，
Figure FDA0003238599750000028
为Ti+ΔT时刻障碍物预测位置，
Figure FDA0003238599750000029
为Ti时刻消防车相对于世界坐标系位置，
Figure FDA00032385997500000210
为Ti时刻传感器获取动态障碍物相对于消防车位置，
Figure FDA00032385997500000211
为Ti+1时刻消防车相对于世界坐标系位置，
Figure FDA00032385997500000212
为Ti+1时刻传感器获取动态障碍物相对于消防车位置，ΔT>Δt。
7.根据权利要求1所述的一种消防车路径规划方法，其特征在于，所述局部路径的规划方法包括：
将Ti+ΔT时刻障碍物预测位置
Figure FDA00032385997500000213
更新到环境模型中；
以Ti时刻消防车位置
Figure FDA00032385997500000214
为路径规划开始父节点，以全局路径的Ti+Δt时刻位置为路径规划目标点，进行上述目标趋向快速扩展树搜索方法搜索局部路径，将原全局路径的Ti～Ti+ΔT时刻路点替换为所述规划局部路径的路点，完成更新全局路径。
8.一种消防车路径规划***，其特征在于，包括：
获取模块，用于获取环境模型，所述环境模型中包括固定障碍物相对于消防车库为世界坐标系下的位姿；
全局路径规划模块，用于基于所述环境模型，获得搜索边界信息和障碍物信息，并采用目标趋向快速扩展树搜索，获得全局路径；
发送模块，用于基于所述全局路径，发送控制信号至消防车，使得消防车从消防车库沿着全局路径走向目标点；
修正模块，用于接收消防车行走过程中反馈的固定障碍物信息，并基于接收到的固定障碍物位姿修正消防车当前位姿；
局部路径规划模块，用于接收消防车行走过程中反馈的动态障碍物信息，基于接收到的动态障碍物进行障碍物运动碰撞预测，并基于预测结果确定是否进行局部路径规划，并利用局部路径规划更新全局路径。
9.一种消防车路径规划终端，其特征在于，包括处理器、与所述处理器耦接的存储器；所述存储器存储有用于实现权利要求1-7任一项所述的消防车路径规划方法的程序指令；
所述处理器用于执行所述存储器存储的所述程序指令以控制消防车路径规划以及处理传感信息。
10.一种存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1-7任一项所述方法的步骤。
CN202111011479.5A 2021-08-31 2021-08-31 一种消防车路径规划方法、***、终端以及存储介质 Active CN113848881B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111011479.5A CN113848881B (zh) 2021-08-31 2021-08-31 一种消防车路径规划方法、***、终端以及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111011479.5A CN113848881B (zh) 2021-08-31 2021-08-31 一种消防车路径规划方法、***、终端以及存储介质

Publications (2)

Publication Number Publication Date
CN113848881A true CN113848881A (zh) 2021-12-28
CN113848881B CN113848881B (zh) 2023-11-21

Family

ID=78976644

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111011479.5A Active CN113848881B (zh) 2021-08-31 2021-08-31 一种消防车路径规划方法、***、终端以及存储介质

Country Status (1)

Country Link
CN (1) CN113848881B (zh)

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103823466A (zh) * 2013-05-23 2014-05-28 电子科技大学 一种动态环境下移动机器人路径规划方法
CN103941737A (zh) * 2014-05-09 2014-07-23 济南大学 一种复杂环境下拖挂式移动机器人的运动规划与控制方法
EP2827210A1 (en) * 2013-07-15 2015-01-21 BAE Systems PLC Route planning
CN106406320A (zh) * 2016-11-29 2017-02-15 重庆重智机器人研究院有限公司 机器人路径规划方法及规划路线的机器人
RU2625602C1 (ru) * 2016-08-11 2017-07-17 ФЕДЕРАЛЬНОЕ ГОСУДАРСТВЕННОЕ БЮДЖЕТНОЕ УЧРЕЖДЕНИЕ "ВСЕРОССИЙСКИЙ ОРДЕНА "ЗНАК ПОЧЕТА" НАУЧНО-ИССЛЕДОВАТЕЛЬСКИЙ ИНСТИТУТ ПРОТИВОПОЖАРНОЙ ОБОРОНЫ МИНИСТЕРСТВА РОССИЙСКОЙ ФЕДЕРАЦИИ ПО ДЕЛАМ ГРАЖДАНСКОЙ ОБОРОНЫ, ЧРЕЗВЫЧАЙНЫМ СИТУАЦИЯМ И ЛИКВИДАЦИИ ПОСЛЕДСТВИЙ СТИХИЙНЫХ БЕДСТВИЙ" (ФГБУ ВНИИПО МЧС России) Способ обнаружения границы локального подземного торфяного пожара и робот для проведения разведки подземных торфяных пожаров
CN108170134A (zh) * 2017-11-15 2018-06-15 国电南瑞科技股份有限公司 一种变电站智能巡检机器人路径规划方法
CN109753072A (zh) * 2019-01-23 2019-05-14 西安工业大学 一种移动机器人混合路径规划方法
US20200118446A1 (en) * 2018-10-12 2020-04-16 Aurora Flight Sciences Corporation Trajectory Planner for a Vehicle
EP3794422A2 (de) * 2018-05-15 2021-03-24 Minimax GmbH & Co KG Brandschutzroboter, system umfassend den brandschutzroboter, sowie verfahren zur verwendung desselben
CN112859855A (zh) * 2021-01-11 2021-05-28 金陵科技学院 一种基于蝗虫优化算法的机器人多目标路径规划
CN113276109A (zh) * 2021-04-21 2021-08-20 国网上海市电力公司 一种基于rrt算法的双机械臂解耦运动规划方法及***

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103823466A (zh) * 2013-05-23 2014-05-28 电子科技大学 一种动态环境下移动机器人路径规划方法
EP2827210A1 (en) * 2013-07-15 2015-01-21 BAE Systems PLC Route planning
CN103941737A (zh) * 2014-05-09 2014-07-23 济南大学 一种复杂环境下拖挂式移动机器人的运动规划与控制方法
RU2625602C1 (ru) * 2016-08-11 2017-07-17 ФЕДЕРАЛЬНОЕ ГОСУДАРСТВЕННОЕ БЮДЖЕТНОЕ УЧРЕЖДЕНИЕ "ВСЕРОССИЙСКИЙ ОРДЕНА "ЗНАК ПОЧЕТА" НАУЧНО-ИССЛЕДОВАТЕЛЬСКИЙ ИНСТИТУТ ПРОТИВОПОЖАРНОЙ ОБОРОНЫ МИНИСТЕРСТВА РОССИЙСКОЙ ФЕДЕРАЦИИ ПО ДЕЛАМ ГРАЖДАНСКОЙ ОБОРОНЫ, ЧРЕЗВЫЧАЙНЫМ СИТУАЦИЯМ И ЛИКВИДАЦИИ ПОСЛЕДСТВИЙ СТИХИЙНЫХ БЕДСТВИЙ" (ФГБУ ВНИИПО МЧС России) Способ обнаружения границы локального подземного торфяного пожара и робот для проведения разведки подземных торфяных пожаров
CN106406320A (zh) * 2016-11-29 2017-02-15 重庆重智机器人研究院有限公司 机器人路径规划方法及规划路线的机器人
CN108170134A (zh) * 2017-11-15 2018-06-15 国电南瑞科技股份有限公司 一种变电站智能巡检机器人路径规划方法
EP3794422A2 (de) * 2018-05-15 2021-03-24 Minimax GmbH & Co KG Brandschutzroboter, system umfassend den brandschutzroboter, sowie verfahren zur verwendung desselben
US20200118446A1 (en) * 2018-10-12 2020-04-16 Aurora Flight Sciences Corporation Trajectory Planner for a Vehicle
CN109753072A (zh) * 2019-01-23 2019-05-14 西安工业大学 一种移动机器人混合路径规划方法
CN112859855A (zh) * 2021-01-11 2021-05-28 金陵科技学院 一种基于蝗虫优化算法的机器人多目标路径规划
CN113276109A (zh) * 2021-04-21 2021-08-20 国网上海市电力公司 一种基于rrt算法的双机械臂解耦运动规划方法及***

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
KUI QIAN 等: "Robot path planning optimization method based on heuristic multi-directional rapidly-exploring tree", 《COMPUTERS AND ELECTRICAL ENGINEERING》, vol. 85, pages 1 - 11 *
胡晓敏 等: "新型树启发式搜索算法的机器人路径规划", 《计算机工程与应用》, vol. 56, no. 11, pages 164 - 171 *

Also Published As

Publication number Publication date
CN113848881B (zh) 2023-11-21

Similar Documents

Publication Publication Date Title
US9995589B2 (en) Path plan generating apparatus for mobile body
CN112985416B (zh) 激光与视觉信息融合的鲁棒定位和建图方法及***
US9274526B2 (en) Autonomous vehicle and method of estimating self position of autonomous vehicle
US8515612B2 (en) Route planning method, route planning device and autonomous mobile device
US8352074B2 (en) Path planning apparatus and method of robot
KR100772912B1 (ko) 절대 방위각을 이용한 로봇 및 이를 이용한 맵 작성 방법
JP2011054082A (ja) 自律移動装置
KR20090126414A (ko) 로봇 및 그 경로생성방법
CN113467476B (zh) 考虑转角约束的无碰撞检测快速随机树全局路径规划方法
Choi et al. Efficient simultaneous localization and mapping based on ceiling-view: ceiling boundary feature map approach
CN115562290A (zh) 一种基于a星惩罚控制优化算法的机器人路径规划方法
Yin et al. Combinatorial inertial guidance system for an automated guided vehicle
CN108544491B (zh) 一种综合考虑距离与方向两因素的移动机器人避障方法
Zhou et al. Slam algorithm and navigation for indoor mobile robot based on ros
CN113341999A (zh) 一种基于优化d*算法的叉车路径规划方法及装置
Lienke et al. An ad-hoc sampling-based planner for on-road automated driving
Zhang et al. Self-positioning for mobile robot indoor navigation based on wheel odometry, inertia measurement unit and ultra wideband
CN113848881B (zh) 一种消防车路径规划方法、***、终端以及存储介质
CN116358525A (zh) 基于激光雷达的建图与定位方法、***及工程车辆
CN116009558A (zh) 一种结合运动学约束的移动机器人路径规划方法
JP6589578B2 (ja) 移動量推定装置、自律移動体、及び移動量の推定方法
Meng et al. A navigation framework for mobile robots with 3D LiDAR and monocular camera
CN111413982A (zh) 一种规划多辆车的循迹路线的方法及终端
Zhang et al. A visual slam system with laser assisted optimization
CN112415516A (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
GR01 Patent grant