CN114371716A - 一种用于消防机器人的自动驾驶巡检方法 - Google Patents
一种用于消防机器人的自动驾驶巡检方法 Download PDFInfo
- Publication number
- CN114371716A CN114371716A CN202210066355.5A CN202210066355A CN114371716A CN 114371716 A CN114371716 A CN 114371716A CN 202210066355 A CN202210066355 A CN 202210066355A CN 114371716 A CN114371716 A CN 114371716A
- Authority
- CN
- China
- Prior art keywords
- fire
- fighting robot
- point
- cloud data
- node
- 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 45
- 238000007689 inspection Methods 0.000 title claims abstract description 40
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 46
- 230000008569 process Effects 0.000 claims abstract description 19
- 238000010276 construction Methods 0.000 claims abstract description 15
- 238000011156 evaluation Methods 0.000 claims description 30
- 239000013598 vector Substances 0.000 claims description 21
- 230000008859 change Effects 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 11
- 150000001875 compounds Chemical class 0.000 claims description 9
- 230000007246 mechanism Effects 0.000 claims description 9
- 238000010606 normalization Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 9
- 230000007613 environmental effect Effects 0.000 claims description 8
- 230000004888 barrier function Effects 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000001419 dependent effect Effects 0.000 claims description 3
- 238000001514 detection method Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000002485 combustion reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000002265 prevention Effects 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- 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
-
- 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/0225—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving docking at a fixed facility, e.g. base station or loading bay
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种用于消防机器人的自动驾驶巡检方法,按如下步骤进行：S1:控制消防机器人在目标区域内移动，通过激光雷达获取点云数据，利用LOAM算法实现全局地图的构建与自身定位，建图完成后将全局地图保存到消防机器人的系统中；S2:消防机器人根据接收到的巡逻目标位置信息，利用全局地图在消防机器人的当前位置和巡逻目标位置规划出一条全局最优路径；消防机器人按照规划路径进行移动过程中，消防机器人接收定位信息并感知环境信息，再利用动态窗口算法实现实时路径规划和实时躲避障碍物，从而实现自动驾驶巡检。本发明可以使得消防机器人可以在复杂环境中规划出最优的自动驾驶路线，进而方便消防机器人的巡检，降低火灾发生的可能性。
Description
技术领域
本发明涉及消防预警技术领域，特别涉及一种用于消防机器人的自动驾驶巡检方法。
背景技术
火灾是现代社会中最经常发生的人为灾害，严重威胁着人类生命财产安全。现有的消防机器人是一种具有火源检测和主动灭火等功能，能够在火灾高危场所进行持续作业，减少火灾发生的机器。由于早期燃烧的小火不容易发现，而处于成长期的火灾扑灭难度较大，一旦仓库和工厂发生火灾就会造成重大人员伤亡和财产损失，所以对火灾防控重点场所进行火灾检测预警就特别重要。但由于物流仓库和工厂等火灾高危场所具有占地面积超大，建筑结构复杂，运行设备较多，安全风险较高和存放大量易燃物品等特点，而传统的消防机器人一般都是固定路线巡逻，难以适应于不同的或者复杂的环境。因此急需一种能够具有自动驾驶巡检功能的消防机器人，用于适应于不同的或者复杂的环境，降低火灾发生的可能性成为了申请人亟待解决的技术问题。
发明内容
本发明的目的在于，提供一种用于消防机器人的自动驾驶巡检方法。本发明可以使得消防机器人可以在复杂环境中规划出最优的自动驾驶路线，进而方便消防机器人的巡检，降低火灾发生的可能性。
本发明的技术方案：一种用于消防机器人的自动驾驶巡检方法,包括呈小车式的消防机器人，按如下步骤进行：
S1:控制消防机器人在目标区域内移动，通过激光雷达获取点云数据，利用LOAM算法实现全局地图的构建与自身定位，建图完成后将全局地图保存到消防机器人的系统中；
S2:消防机器人根据接收到的巡逻目标位置信息，利用全局地图在消防机器人的当前位置和巡逻目标位置规划出一条全局最优路径；消防机器人按照规划路径进行移动过程中，消防机器人接收定位信息并感知环境信息，再利用动态窗口算法实现实时路径规划和实时躲避障碍物，从而实现自动驾驶巡检。
上述的用于消防机器人的自动驾驶巡检方法,所述的全局地图建立是在消防机器人在目标区域内移动过程中，由激光雷达获取外界环境的点云数据，对于点云数据进行提取特征点，再通过匹配相邻时刻点云数据的特征点，来估计消防机器人的运动轨迹，获得消防机器人的位置变化，再将相邻时刻的位置变换进行融合，得到消防机器人所处的环境地图，最后根据环境地图的拼接获得全局地图。
前述的用于消防机器人的自动驾驶巡检方法,所述全局地图建立包括点云数据特征点的提取、特征点的匹配、运动估计以及环境地图的构建；
所述点云数据特征点的提取是通过提取边缘点和平面点具有代表性的点，利用该点附近几个点的坐标信息计算出该点的曲率，判断该点是否为特征点，其中第i个点的曲率C计算如下：
式中：k表示激光雷达扫描一次的周期，L表示雷达坐标系，S表示点的集合，j表示第i个点的相邻点的编号，
和
分别表示第i个点以及第j个点的某一状态；其中曲率最小的点归为平面点，曲率最大的点归为边缘点；
所述特征点的匹配是找出当前的边缘点和平面点对应前一帧数据的对应边缘线段和对应平面：已知边缘点匹配的线段上两点，计算出该边缘点到边缘线段的距离为：
式中，
为点A在激光雷达坐标系{Lk}中的坐标向量，其余符号同样表示某点在某坐标系中的坐标向量，×表示向量的叉乘运算；
边缘点到匹配的对应平面的距离为：
式中：
为点D在激光雷达坐标系{Lk}中的坐标向量，其余符号同样表示某点在某坐标系中的坐标向量，×表示向量的叉乘运算；
所述运动估计利用特征点来进行消防机器人的运动轨迹估计：通过插值的方式计算出该周期内任意时刻的位置估计，插值公式如下：
式中，
随时间变化的量，表示消防机器人的位置变换，插值使用的是当前时间t的变换；
使用旋转矩阵R和平移向量T表示前时刻点云数据中的点和前一时刻点云数据中的点的对应关系：
根据罗德里格斯公式，R表示为：
式中，
分别计算出点到线以及点到面的距离，并获得用于优化的误差函数：
式中，函数f(T)中的每一行对应一个特征点，d包含对应的距离；函数f(T)关于
的雅克比矩阵J表示为：
上式使用LM算法求解：
式中，λ是一个取决于LM算法的因子；
根据以上的求解公式，可以得到相邻时刻激光点云数据的位置改变量，计算出消防机器人位置变换的旋转矩阵R和平移向量T，从而估算出消防机器人的运动轨迹；
所述的环境地图的构建是:假设消防机器人在tk时刻到tk+1时刻的位置变换为
激光雷达获得的点云数据为Pk，在tk时刻已构建好的点云地图为Qk-1，在tk时刻激光雷达在世界坐标系下的位姿为
利用位置变换
将点云数据
转换到世界坐标系下，转换后的点云数据表示为Qk,即在tk时刻到tk+1时刻构建的局部环境地图为
此时利用匹配算法,将
与Ql-1进行融合，得到消防机器人在tk+1时刻的环境地图，将不同时刻的环境地图进行拼接获得全局地图。
前述的用于消防机器人的自动驾驶巡检方法,消防机器人根据接收到的巡逻目标位置后，利用全局路径规划器在当前位置和导航目标位置之间避开障碍物，规划出一条全局最优路径，再将全局最优路径发生至局部路径规划器，局部路径规划器利用传感器获取到的即时的障碍物信息与自身实时位置信息，规划出一条沿着全局最优路径并且能避开障碍物的局部路径，然后向消防机器人的底盘控制模块发送运动控制指令，实现消防机器人的自动驾驶巡检。
前述的用于消防机器人的自动驾驶巡检方法,所述的自动驾驶巡检是当消防机器人的底盘控制模块中的纵向控制器和横向控制器收到控制指令后，分别计算纵向控制量和横向控制量，转化为电压信号传递给电机驱动机构、制动执行机构和转向执行机构，进而控制消防机器人向目标位置运动。
前述的用于消防机器人的自动驾驶巡检方法,在消防机器人位置发生变化时，消防机器人中的里程计和激光雷达会实时更新里程计信息和点云信息，通过amcl定位器对消防机器人进行实时位置跟踪，避免移动机器人的运动轨迹偏离规划的路径。
前述的用于消防机器人的自动驾驶巡检方法,所述全局路径规划器采用A-Star算法进行全局最优路径规划，A-Star算法将搜索到满足成本评价函数的点作为下一次要搜索的点，并重复此过程，直到找到目标点，形成最优路径，其成本评价函数如下：
f(n)＝h(n)+g(n)；
其中,f(n)为当前位置n的成本评价函数,g(n)为消防机器人从初始位置到当前位置n的实际成本，h(n)为机器人从当前位置n到目标位置的估计成本；
选用欧几里得距离作为估价函数，使用距离公式的当前节点与目标节点之间的距离，表示为：
式中：xn、yn为当前节点坐标，xg、yg为目标节点的坐标。
前述的用于消防机器人的自动驾驶巡检方法,所述A-Star算法具体步骤如下：
S2.1:设定open列表和close列表，分别存储没有访问和已经访问过的节点信息；
S2.2:开始搜索时，close列表为空，访问过开始节点后，将其从open列表删除，加入close列表中，A-Star算法反复搜索，寻找目标点；
S2.3:在到达目标点之前，继续搜索其相邻节点，判断搜索到的新节点是否在open列表中，如果没有，将新节点加入open列表，计算这个节点的代价值，将此节点设置为当前搜索点的父节点加入close列表中；
S2.4:如果搜索到的新节点已经存在于open列表中，就计算这些节点的代价值，与前一次计算出来的相同节点的值进行对比，选取值最小的那一点作为下一次搜索点的父节点，并将此节点加入close列表中，然后继续更新open列表的信息；
S2.5:循环以上过程，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径。
前述的用于消防机器人的自动驾驶巡检方法,所述局部路径规划器利用动态窗口算法进行局部路径的规划，在动态窗口算法中，消防机器人在速度空间内采样，生成模拟轨迹，根据评价项对其评价，最后选择得分最高的一组，得到局部最优路径；
首先，动态窗口算法的速度搜索空间(v，w)本身存在无数组速度，根据消防机器人自身或环境因素将速度搜索空间(v，w)控制在一个动态范围内，即边界限定条件：
Vm＝{v∈[vmin，vmax]∩w∈[wmin，wmax]}；
式中:vmax和vmin分别为最大以及最小线速度，wmax和wmin分别为最大以及最小角速度；
在一个时间间隔内，消防机器人所能到达的速度在一个窗口内，即消防机器人具有最大加速度限制：
式中：dist(v，w)表示消防机器人与最近障碍物的路径长度；
消防机器人前进周期一个Δt内，有效的速度集合区间Vr为：
Vr＝Vm∩Vd∩Va；
在速度搜索空间中采样之后，使用评价函数来评价所采样的几组速度对应的轨迹，评价函数公式为：
G(v，w)＝α·heading(v，w)+β·dist(v，w)+γ·vel(v，w)；
其中，α，β和γ是每一项的权重因子，heading(v，w)表示评价消防机器采取速度搜索空间(v，w)时，所在轨迹移动方向与目标的角度值θ的函数，表达式为
vel(v，w)表示运动轨迹的速度大小，运动轨迹越快达到目标，评分越高，表达式为
并采用归一化处理使消防机器人的运动更加稳定平滑，归一化过程如下：
其中，nor_heading(l)、nor_dist(l)、nor_vel(l)分别表示待评价轨迹l的各项评价函数归一化后的占比；
最后根据评价函数的得分，得到局部最优路径。
与现有技术相比，本发明通过控制消防机器人在目标区域内移动，利用激光雷达获取点云数据，再通过LOAM算法实现全局地图的构建与自身定位，完成巡逻场景地图的高精度构建，并将全局地图保存到系统中，再根据已经建好的全局地图，利用全局地图在消防机器人的当前位置和巡逻目标位置规划出一条全局最优路径；消防机器人按照规划路径进行移动过程中，消防机器人接收定位信息并感知环境信息，再利用动态窗口算法实现实时路径规划和实时躲避障碍物，从而实现自动驾驶巡检。由此本发明可以使得消防机器人可以应用于不同的环境当中，具有良好的适配性，同时本发明可以使得消防机器人能够在接收到巡检命令后，按照最优的规划路线进行自动驾驶，避免碰撞的发生。此外，本发明设计了局部最优路径规划，在全局地图建立后，若消防机器人所处的目标区域变动，可以通过局部最优路径规划进行实时的路径优选和实时躲避障碍物，避免发生在区域环境变动后，消防机器人无法正常运行的问题。
附图说明
图1为本发明建立全局地图的流程图；
图2是基于LOAM的SLAM算法原理图；
图3是建立环境地图的流程图；
图4是消防机器人巡检模块整体框架流程图；
图5是自动驾驶的示意图；
图6是A-star算法流程图；
图7是动态窗口算法流程图；
图8是消防机器人避障效果图。
具体实施方式
下面结合附图和实施例对本发明作进一步的说明，但并不作为对本发明限制的依据。
实施例：一种用于消防机器人的自动驾驶巡检方法,包括呈小车式的消防机器人，消防机器人配备Velodyne VLP-16激光雷达传感器，可见光单目相机和红外热像仪、轮式编码器，消防机器人的巡检模块，基于ROS系统，通过Velodyne VLP-16激光雷达传感器、轮式编码器，利用SLAM技术，实现消防机器人的自主定位、建图和导航；按如下步骤进行：
S1:如图1所示，控制消防机器人在目标区域内移动，通过激光雷达获取点云数据，利用LOAM算法实现全局地图的构建与自身定位，建图完成后将全局地图保存到消防机器人的系统中；
所述的全局地图建立是在消防机器人在目标区域内移动过程中，由激光雷达获取外界环境的点云数据，对于点云数据进行提取特征点，再通过匹配相邻时刻点云数据的特征点，来估计消防机器人的运动轨迹，获得消防机器人的位置变化，再将相邻时刻的位置变换进行融合，产生10Hz的位置输出，得到消防机器人所处的环境地图，最后根据环境地图的拼接获得全局地图。
具体的，如图2所示，所述全局地图建立包括点云数据特征点的提取、特征点的匹配、运动估计以及环境地图的构建；
所述点云数据特征点的提取是通过提取边缘点和平面点具有代表性的点，利用该点附近几个点的坐标信息计算出该点的曲率，判断该点是否为特征点，其中第i个点的曲率C计算如下：
式中：k表示激光雷达扫描一次的周期，L表示雷达坐标系，S表示点的集合，j表示第i个点的相邻点的编号，
和
分别表示第i个点以及第j个点的某一状态；其中曲率最小的点归为平面点，曲率最大的点归为边缘点；
所述特征点的匹配是找出当前的边缘点和平面点对应前一帧数据的对应边缘线段和对应平面，针对LOAM算法所提出的边缘点和平面点，其特征点匹配不需要找出特征点之间的一一对应关系，只需要找出当前点云数据中的边缘点和平面点对应前一帧数据的对应线段和对应平面。
已知边缘点匹配的线段上两点，计算出该边缘点A到边缘线段(点B和点C构成的直线)的距离为：
式中，
为点A在激光雷达坐标系{Lk}中的坐标向量，其余符号同样表示某点在某坐标系中的坐标向量，×表示向量的叉乘运算；
边缘点D到匹配的对应平面(不共线的三点E、F、G组成的平面)的距离为：
式中：
为点D在激光雷达坐标系{Lk}中的坐标向量，其余符号同样表示某点在某坐标系中的坐标向量，×表示向量的叉乘运算；
由于短时间内，激光雷达的移动范围很小，可以合理的假设激光雷达是匀速移动的。求出一个周期终止时刻相对于起始时刻的转换矩阵，则可以通过插值的方式计算出该周期内任意时刻的位置估计。所述运动估计利用特征点来进行消防机器人的运动轨迹估计：通过插值的方式计算出该周期内任意时刻的位置估计，插值公式如下：
式中，
随时间变化的量，表示消防机器人的位置变换，插值使用的是当前时间t的变换；
为了找到当前时刻点云数据中的点和前一时刻点云数据中的点的对应关系，本实施例使用旋转矩阵R和平移向量T表示前时刻点云数据中的点和前一时刻点云数据中的点的对应关系：
由于直接求解旋转矩阵R的计算量太大，因此根据罗德里格斯公式，R表示为：
式中，
此时，可以分别计算出点到线以及点到面的距离，并获得用于优化的误差函数：
式中，函数f(T)中的每一行对应一个特征点，d包含对应的距离；函数f(T)关于
的雅克比矩阵J表示为：
上式使用LM算法求解：
式中，λ是一个取决于LM算法的因子；
根据以上的求解公式，可以得到相邻时刻激光点云数据的位置改变量，计算出消防机器人位置变换的旋转矩阵R和平移向量T，从而估算出消防机器人的运动轨迹；
如图3所示，所述的环境地图的构建是:假设消防机器人在tk时刻到tk+1时刻的位置变换为
激光雷达获得的点云数据为Pk，在tk时刻已构建好的点云地图为Qk-1，在tk时刻激光雷达在世界坐标系下的位姿为
利用位置变换
将点云数据
转换到世界坐标系下，转换后的点云数据表示为Qk,即在tk时刻到tk+1时刻构建的局部环境地图为
此时利用匹配算法,将
与Qk-1进行融合，得到消防机器人在tk+1时刻的环境地图，将不同时刻的环境地图进行拼接获得全局地图。
S2:如图4所示，消防机器人根据接收到的巡逻目标位置信息，利用全局地图在消防机器人的当前位置和巡逻目标位置规划出一条全局最优路径；消防机器人按照规划路径进行移动过程中，消防机器人接收定位信息并感知环境信息，再利用动态窗口算法实现实时路径规划和实时躲避障碍物，从而实现自动驾驶巡检。
具体的，如图5所示，消防机器人根据接收到的巡逻目标位置后，利用全局路径规划器在当前位置和导航目标位置之间避开障碍物，规划出一条全局最优路径，再将全局最优路径发生至局部路径规划器，局部路径规划器利用传感器获取到的即时的障碍物信息与自身实时位置信息，规划出一条沿着全局最优路径并且能避开障碍物的局部路径，然后向消防机器人的底盘控制模块发送运动控制指令，实现消防机器人的自动驾驶巡检。
所述的自动驾驶巡检是当消防机器人的底盘控制模块中的纵向控制器和横向控制器收到控制指令后，分别计算纵向控制量和横向控制量，转化为电压信号传递给电机驱动机构、制动执行机构和转向执行机构，进而控制消防机器人向目标位置运动。
在消防机器人位置发生变化时，消防机器人中的里程计和激光雷达会实时更新里程计信息和点云信息，通过amcl定位器对消防机器人进行实时位置跟踪，避免移动机器人的运动轨迹偏离规划的路径。
本实施例中，所述全局路径规划器采用A-Star算法进行全局最优路径规划，A-Star算法将搜索到满足成本评价函数的点作为下一次要搜索的点，并重复此过程，直到找到目标点，形成最优路径，其成本评价函数如下：
f(n)＝h(n)+g(n)；
其中,f(n)为当前位置n的成本评价函数,g(n)为消防机器人从初始位置到当前位置n的实际成本，h(n)为机器人从当前位置n到目标位置的估计成本；
选用欧几里得距离作为估价函数，使用距离公式的当前节点n与目标节点g之间的距离，表示为：
式中：xn、yn为当前节点坐标，xg、yg为目标节点的坐标。
如图6所示，所述A-Star算法具体步骤如下：
S2.1:设定open列表和close列表，分别存储没有访问和已经访问过的节点信息；
S2.2:开始搜索时，close列表为空，访问过开始节点后，将其从open列表删除，加入close列表中，A-Star算法反复搜索，寻找目标点；
S2.3:在到达目标点之前，继续搜索其相邻节点，判断搜索到的新节点是否在open列表中，如果没有，将新节点加入open列表，计算这个节点的代价值，将此节点设置为当前搜索点的父节点加入close列表中；
S2.4:如果搜索到的新节点已经存在于open列表中，就计算这些节点的代价值，与前一次计算出来的相同节点的值进行对比，选取值最小的那一点作为下一次搜索点的父节点，并将此节点加入close列表中，然后继续更新open列表的信息；
S2.5:循环以上过程，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径。
所述局部路径规划器利用动态窗口算法进行局部路径的规划，在动态窗口算法中，消防机器人在速度空间内采样，生成模拟轨迹，根据评价项对其评价，最后选择得分最高的一组，得到局部最优路径；
如图7所示，首先，动态窗口算法的速度搜索空间(v，w)本身存在无数组速度，根据消防机器人自身或环境因素将速度搜索空间(v，w)控制在一个动态范围内，即边界限定条件：
Vm＝{v∈[vmin，vmax]∩w∈[wmin，wmax]}；
式中:vmax和vmin分别为最大以及最小线速度，wmax和wmin分别为最大以及最小角速度；
在一个时间间隔内，消防机器人所能到达的速度在一个窗口内，即消防机器人具有最大加速度限制：
式中：dist(v，w)表示消防机器人与最近障碍物的路径长度；
消防机器人前进周期一个Δt内，有效的速度集合区间Vr为：
Vr＝Vm∩Vd∩Va；
在速度搜索空间中采样之后，使用评价函数来评价所采样的几组速度对应的轨迹，在局部路径规划过程中，评价函数的设计准则为：在避开障碍物保持合适的安全距离的同时，朝着目的地点沿着较短路径快速的前行。评价函数公式为：
G(v，w)＝α·heading(v，w)+β·dist(v，w)+γ·vel(v，w)；
其中，α，β和γ是每一项的权重因子，heading(v，w)表示评价消防机器人采取速度搜索空间(v，w)时，所在轨迹移动方向与目标的角度值θ的函数，表达式为
vel(v，w)表示运动轨迹的速度大小，运动轨迹越快达到目标，评分越高，表达式为
并采用归一化处理使消防机器人的运动更加稳定平滑，归一化过程如下：
其中，nor_heading(l)、nor_dist(l)、nor_vel(l)分别表示待评价轨迹l的各项评价函数归一化后的占比；
最后根据评价函数的得分，得到局部最优路径。如图8所示，虚线表示以某个采样速度运动的可能轨迹，根据轨迹评价函数对这些轨迹进行打分，中间位置的轨迹碰撞到障碍物的可能性较小，且路径最短，得分较高，为局部最优路径。
综上所述，本发明通过控制消防机器人在目标区域内移动，利用激光雷达获取点云数据，再通过LOAM算法实现全局地图的构建与自身定位，完成巡逻场景地图的高精度构建，并将全局地图保存到系统中，再根据已经建好的全局地图，利用全局地图在消防机器人的当前位置和巡逻目标位置规划出一条全局最优路径；消防机器人按照规划路径进行移动过程中，消防机器人接收定位信息并感知环境信息，再利用动态窗口算法实现实时路径规划和实时躲避障碍物，从而实现自动驾驶巡检。由此本发明可以使得消防机器人可以应用于不同的环境当中，具有良好的适配性，同时本发明可以使得消防机器人能够在接收到巡检命令后，按照最优的规划路线进行自动驾驶，避免碰撞的发生。此外，本发明设计了局部最优路径规划，在全局地图建立后，若消防机器人所处的目标区域变动，可以通过局部最优路径规划进行实时的路径优选和实时躲避障碍物，避免发生在区域环境变动后，消防机器人无法正常运行的问题。
Claims (9)
1.一种用于消防机器人的自动驾驶巡检方法,包括呈小车式的消防机器人，其特征在于：按如下步骤进行：
S1:控制消防机器人在目标区域内移动，通过激光雷达获取点云数据，利用LOAM算法实现全局地图的构建与自身定位，建图完成后将全局地图保存到消防机器人的系统中；
S2:消防机器人根据接收到的巡逻目标位置信息，利用全局地图在消防机器人的当前位置和巡逻目标位置规划出一条全局最优路径；消防机器人按照规划路径进行移动过程中，消防机器人接收定位信息并感知环境信息，再利用动态窗口算法实现实时路径规划和实时躲避障碍物，从而实现自动驾驶巡检。
2.根据权利要求1所述的用于消防机器人的自动驾驶巡检方法,其特征在于：所述的全局地图建立是在消防机器人在目标区域内移动过程中，由激光雷达获取外界环境的点云数据，对于点云数据进行提取特征点，再通过匹配相邻时刻点云数据的特征点，来估计消防机器人的运动轨迹，获得消防机器人的位置变化，再将相邻时刻的位置变换进行融合，得到消防机器人所处的环境地图，最后根据环境地图的拼接获得全局地图。
3.根据权利要求2所述的用于消防机器人的自动驾驶巡检方法,其特征在于：所述全局地图建立包括点云数据特征点的提取、特征点的匹配、运动估计以及环境地图的构建；
所述点云数据特征点的提取是通过提取边缘点和平面点具有代表性的点，利用该点附近几个点的坐标信息计算出该点的曲率，判断该点是否为特征点，其中第i个点的曲率C计算如下：
式中：k表示激光雷达扫描一次的周期，L表示雷达坐标系，S表示点的集合，j表示第i个点的相邻点的编号，
和
分别表示第i个点以及第j个点的某一状态；其中曲率最小的点归为平面点，曲率最大的点归为边缘点；
所述特征点的匹配是找出当前的边缘点和平面点对应前一帧数据的对应边缘线段和对应平面：已知边缘点匹配的线段上两点，计算出该边缘点到边缘线段的距离为：
式中，
为点A在激光雷达坐标系{Lk}中的坐标向量，其余符号同样表示某点在某坐标系中的坐标向量，×表示向量的叉乘运算；
边缘点到匹配的对应平面的距离为：
式中：
为点D在激光雷达坐标系{Lk}中的坐标向量，其余符号同样表示某点在某坐标系中的坐标向量，×表示向量的叉乘运算；
所述运动估计利用特征点来进行消防机器人的运动轨迹估计：通过插值的方式计算出该周期内任意时刻的位置估计，插值公式如下：
式中，
随时间变化的量，表示消防机器人的位置变换，插值使用的是当前时间t的变换；
使用旋转矩阵R和平移向量T表示前时刻点云数据中的点和前一时刻点云数据中的点的对应关系：
根据罗德里格斯公式，R表示为：
式中，
分别计算出点到线以及点到面的距离，并获得用于优化的误差函数：
式中，函数f(T)中的每一行对应一个特征点，d包含对应的距离；函数f(T)关于
的雅克比矩阵J表示为：
上式使用LM算法求解：
式中，λ是一个取决于LM算法的因子；
根据以上的求解公式，可以得到相邻时刻激光点云数据的位置改变量，计算出消防机器人位置变换的旋转矩阵R和平移向量T，从而估算出消防机器人的运动轨迹；
所述的环境地图的构建是:假设消防机器人在tk时刻到tk+1时刻的位置变换为
激光雷达获得的点云数据为Pk，在tk时刻已构建好的点云地图为Qk-1，在tk时刻激光雷达在世界坐标系下的位姿为
利用位置变换
将点云数据
转换到世界坐标系下，转换后的点云数据表示为Qk,即在tk时刻到tk+1时刻构建的局部环境地图为
此时利用匹配算法,将
与Qk-1进行融合，得到消防机器人在tk+1时刻的环境地图，将不同时刻的环境地图进行拼接获得全局地图。
4.根据权利要求1所述的用于消防机器人的自动驾驶巡检方法,其特征在于：消防机器人根据接收到的巡逻目标位置后，利用全局路径规划器在当前位置和导航目标位置之间避开障碍物，规划出一条全局最优路径，再将全局最优路径发生至局部路径规划器，局部路径规划器利用传感器获取到的即时的障碍物信息与自身实时位置信息，规划出一条沿着全局最优路径并且能避开障碍物的局部路径，然后向消防机器人的底盘控制模块发送运动控制指令，实现消防机器人的自动驾驶巡检。
5.根据权利要求4所述的用于消防机器人的自动驾驶巡检方法,其特征在于：所述的自动驾驶巡检是当消防机器人的底盘控制模块中的纵向控制器和横向控制器收到控制指令后，分别计算纵向控制量和横向控制量，转化为电压信号传递给电机驱动机构、制动执行机构和转向执行机构，进而控制消防机器人向目标位置运动。
6.根据权利要求4所述的用于消防机器人的自动驾驶巡检方法,其特征在于：在消防机器人位置发生变化时，消防机器人中的里程计和激光雷达会实时更新里程计信息和点云信息，通过amcl定位器对消防机器人进行实时位置跟踪，避免移动机器人的运动轨迹偏离规划的路径。
7.根据权利要求4所述的用于消防机器人的自动驾驶巡检方法,其特征在于：所述全局路径规划器采用A-Star算法进行全局最优路径规划，A-Star算法将搜索到满足成本评价函数的点作为下一次要搜索的点，并重复此过程，直到找到目标点，形成最优路径，其成本评价函数如下：
f(n)＝h(n)+g(n)；
其中,f(n)为当前位置n的成本评价函数,g(n)为消防机器人从初始位置到当前位置n的实际成本，h(n)为机器人从当前位置n到目标位置的估计成本；
选用欧几里得距离作为估价函数，使用距离公式的当前节点n与目标节点g之间的距离，表示为：
式中：xn、yn为当前节点坐标，xg、yg为目标节点的坐标。
8.根据权利要求7所述的用于消防机器人的自动驾驶巡检方法,其特征在于：所述A-Star算法具体步骤如下：
S2.1:设定open列表和close列表，分别存储没有访问和已经访问过的节点信息；
S2.2:开始搜索时，close列表为空，访问过开始节点后，将其从open列表删除，加入close列表中，A-Star算法反复搜索，寻找目标点；
S2.3:在到达目标点之前，继续搜索其相邻节点，判断搜索到的新节点是否在open列表中，如果没有，将新节点加入open列表，计算这个节点的代价值，将此节点设置为当前搜索点的父节点加入close列表中；
S2.4:如果搜索到的新节点已经存在于open列表中，就计算这些节点的代价值，与前一次计算出来的相同节点的值进行对比，选取值最小的那一点作为下一次搜索点的父节点，并将此节点加入close列表中，然后继续更新open列表的信息；
S2.5:循环以上过程，直到达到目标位置，当遍历到目标点，并且将目标点添加到close列表中时，循环结束；如果此时open列表为空，则表示没有路径。
9.根据权利要求7所述的用于消防机器人的自动驾驶巡检方法,其特征在于：所述局部路径规划器利用动态窗口算法进行局部路径的规划，在动态窗口算法中，消防机器人在速度空间内采样，生成模拟轨迹，根据评价项对其评价，最后选择得分最高的一组，得到局部最优路径；
首先，动态窗口算法的速度搜索空间(v，w)本身存在无数组速度，根据消防机器人自身或环境因素将速度搜索空间(v，w)控制在一个动态范围内，即边界限定条件：
Vm＝{v∈[vmin，vmax]∩w∈[wmin，wmax]}；
式中:vmax和vmin分别为最大以及最小线速度，wmax和wmin分别为最大以及最小角速度；
在一个时间间隔内，消防机器人所能到达的速度在一个窗口内，即消防机器人具有最大加速度限制：
式中：dist(v，w)表示消防机器人与最近障碍物的路径长度；
消防机器人前进周期一个Δt内，有效的速度集合区间Vr为：
Vr＝Vm∩Vd∩Va；
在速度搜索空间中采样之后，使用评价函数来评价所采样的几组速度对应的轨迹，评价函数公式为：
G(v，w)＝α·heading(v，w)+β·dist(v，w)+γ·vel(v，w)；
其中，α，β和γ是每一项的权重因子，heading(v，w)表示评价消防机器采取速度搜索空间(v，w)时，所在轨迹移动方向与目标的角度值θ的函数，表达式为
vel(v，w)表示运动轨迹的速度大小，运动轨迹越快达到目标，评分越高，表达式为
并采用归一化处理使消防机器人的运动更加稳定平滑，归一化过程如下：
其中，nor_heading(l)、nor_dist(l)、nor_vel(l)分别表示待评价轨迹l的各项评价函数归一化后的占比；
最后根据评价函数的得分，得到局部最优路径。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210066355.5A CN114371716B (zh) | 2022-01-20 | 2022-01-20 | 一种用于消防机器人的自动驾驶巡检方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210066355.5A CN114371716B (zh) | 2022-01-20 | 2022-01-20 | 一种用于消防机器人的自动驾驶巡检方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114371716A true CN114371716A (zh) | 2022-04-19 |
| CN114371716B CN114371716B (zh) | 2024-08-02 |
Family
ID=81146803
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210066355.5A Active CN114371716B (zh) | 2022-01-20 | 2022-01-20 | 一种用于消防机器人的自动驾驶巡检方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114371716B (zh) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115185268A (zh) * | 2022-06-22 | 2022-10-14 | 国网山东省电力公司鱼台县供电公司 | 一种基于双线性插值的变电站巡检路径规划方法及系统 |
| CN116125995A (zh) * | 2023-04-04 | 2023-05-16 | 华东交通大学 | 一种高铁巡检机器人的路径规划方法及系统 |
| CN116448118A (zh) * | 2023-04-17 | 2023-07-18 | 深圳市华辰信科电子有限公司 | 一种扫地机器人的工作路径优化方法和装置 |
| CN116500638A (zh) * | 2023-06-25 | 2023-07-28 | 江苏大学 | 一种基于slam技术的收割机机耕道自动导航方法及系统 |
| CN117589172A (zh) * | 2023-11-21 | 2024-02-23 | 国广顺能(上海)能源科技有限公司 | 移动机器人的路径规划方法、装置、电子设备及存储介质 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106681330A (zh) * | 2017-01-25 | 2017-05-17 | 北京航空航天大学 | 基于多传感器数据融合的机器人导航方法及装置 |
| CN109709801A (zh) * | 2018-12-11 | 2019-05-03 | 智灵飞（北京）科技有限公司 | 一种基于激光雷达的室内无人机定位系统及方法 |
| CN110223379A (zh) * | 2019-06-10 | 2019-09-10 | 于兴虎 | 基于激光雷达的三维点云重建方法 |
| CN110689622A (zh) * | 2019-07-05 | 2020-01-14 | 电子科技大学 | 一种基于点云分割匹配闭环校正的同步定位与构图算法 |
| CN111275748A (zh) * | 2020-01-15 | 2020-06-12 | 南京航空航天大学 | 动态环境下基于激光雷达的点云配准方法 |
| CN112325884A (zh) * | 2020-10-29 | 2021-02-05 | 广西科技大学 | 一种基于dwa的ros机器人局部路径规划方法 |
| CN112347840A (zh) * | 2020-08-25 | 2021-02-09 | 天津大学 | 视觉传感器激光雷达融合无人机定位与建图装置和方法 |
| CN112378408A (zh) * | 2020-11-26 | 2021-02-19 | 重庆大学 | 一种实现轮式移动机器人实时避障的路径规划方法 |
| CN113432600A (zh) * | 2021-06-09 | 2021-09-24 | 北京科技大学 | 基于多信息源的机器人即时定位与地图构建方法及系统 |
-
2022
- 2022-01-20 CN CN202210066355.5A patent/CN114371716B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106681330A (zh) * | 2017-01-25 | 2017-05-17 | 北京航空航天大学 | 基于多传感器数据融合的机器人导航方法及装置 |
| CN109709801A (zh) * | 2018-12-11 | 2019-05-03 | 智灵飞（北京）科技有限公司 | 一种基于激光雷达的室内无人机定位系统及方法 |
| CN110223379A (zh) * | 2019-06-10 | 2019-09-10 | 于兴虎 | 基于激光雷达的三维点云重建方法 |
| CN110689622A (zh) * | 2019-07-05 | 2020-01-14 | 电子科技大学 | 一种基于点云分割匹配闭环校正的同步定位与构图算法 |
| CN111275748A (zh) * | 2020-01-15 | 2020-06-12 | 南京航空航天大学 | 动态环境下基于激光雷达的点云配准方法 |
| CN112347840A (zh) * | 2020-08-25 | 2021-02-09 | 天津大学 | 视觉传感器激光雷达融合无人机定位与建图装置和方法 |
| CN112325884A (zh) * | 2020-10-29 | 2021-02-05 | 广西科技大学 | 一种基于dwa的ros机器人局部路径规划方法 |
| CN112378408A (zh) * | 2020-11-26 | 2021-02-19 | 重庆大学 | 一种实现轮式移动机器人实时避障的路径规划方法 |
| CN113432600A (zh) * | 2021-06-09 | 2021-09-24 | 北京科技大学 | 基于多信息源的机器人即时定位与地图构建方法及系统 |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115185268A (zh) * | 2022-06-22 | 2022-10-14 | 国网山东省电力公司鱼台县供电公司 | 一种基于双线性插值的变电站巡检路径规划方法及系统 |
| CN116125995A (zh) * | 2023-04-04 | 2023-05-16 | 华东交通大学 | 一种高铁巡检机器人的路径规划方法及系统 |
| CN116448118A (zh) * | 2023-04-17 | 2023-07-18 | 深圳市华辰信科电子有限公司 | 一种扫地机器人的工作路径优化方法和装置 |
| CN116448118B (zh) * | 2023-04-17 | 2023-10-31 | 深圳市华辰信科电子有限公司 | 一种扫地机器人的工作路径优化方法和装置 |
| CN116500638A (zh) * | 2023-06-25 | 2023-07-28 | 江苏大学 | 一种基于slam技术的收割机机耕道自动导航方法及系统 |
| CN116500638B (zh) * | 2023-06-25 | 2023-10-10 | 江苏大学 | 一种基于slam技术的收割机耕道自动导航方法及系统 |
| CN117589172A (zh) * | 2023-11-21 | 2024-02-23 | 国广顺能(上海)能源科技有限公司 | 移动机器人的路径规划方法、装置、电子设备及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114371716B (zh) | 2024-08-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114371716B (zh) | 一种用于消防机器人的自动驾驶巡检方法 | |
| CN110262495B (zh) | 可实现移动机器人自主导航与精确定位的控制系统及方法 | |
| CN108762264B (zh) | 基于人工势场与滚动窗口的机器人的动态避障方法 | |
| Vasiljević et al. | High-accuracy vehicle localization for autonomous warehousing | |
| CN112518739B (zh) | 履带式底盘机器人侦察智能化自主导航方法 | |
| Lacaze et al. | Path planning for autonomous vehicles driving over rough terrain | |
| Roberts et al. | Autonomous control of underground mining vehicles using reactive navigation | |
| Song et al. | Navigation control design of a mobile robot by integrating obstacle avoidance and LiDAR SLAM | |
| Fulgenzi et al. | Probabilistic motion planning among moving obstacles following typical motion patterns | |
| CN103558856A (zh) | 动态环境下服务动机器人导航方法 | |
| KR102303432B1 (ko) | 장애물의 특성을 고려한 dqn 및 slam 기반의 맵리스 내비게이션 시스템 및 그 처리 방법 | |
| CN214846390U (zh) | 基于自动导引小车的动态环境避障系统 | |
| CN112577506A (zh) | 一种自动驾驶局部路径规划方法和系统 | |
| Pritzl et al. | Cooperative navigation and guidance of a micro-scale aerial vehicle by an accompanying UAV using 3D LiDAR relative localization | |
| Butzke et al. | State lattice with controllers: Augmenting lattice-based path planning with controller-based motion primitives | |
| Bakambu et al. | Autonomous system for navigation and surveying in underground mines | |
| CN114527753A (zh) | 人机共融的楼宇间路径规划方法、计算机装置及程序产品 | |
| Wu et al. | Vision-based target detection and tracking system for a quadcopter | |
| Behrje et al. | An autonomous forklift with 3d time-of-flight camera-based localization and navigation | |
| Rasmussen et al. | Robot navigation using image sequences | |
| Huy et al. | A practical and optimal path planning for autonomous parking using fast marching algorithm and support vector machine | |
| Lai et al. | Hierarchical incremental path planning and situation-dependent optimized dynamic motion planning considering accelerations | |
| CN117111617A (zh) | 考虑感知盲区碰撞不确定性的机器人路径规划方法及系统 | |
| CN113741550A (zh) | 移动机器人跟随方法和系统 | |
| CN116520879A (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 |