CN115170772A - 一种基于点云地图交互式可通过性分析的三维路径规划方法 - Google Patents

一种基于点云地图交互式可通过性分析的三维路径规划方法 Download PDF

Info

Publication number
CN115170772A
CN115170772A CN202210543960.7A CN202210543960A CN115170772A CN 115170772 A CN115170772 A CN 115170772A CN 202210543960 A CN202210543960 A CN 202210543960A CN 115170772 A CN115170772 A CN 115170772A
Authority
CN
China
Prior art keywords
grid
map
point cloud
passability
dimensional
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.)
Pending
Application number
CN202210543960.7A
Other languages
English (en)
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.)
University of Shanghai for Science and Technology
Original Assignee
University of Shanghai for Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Shanghai for Science and Technology filed Critical University of Shanghai for Science and Technology
Priority to CN202210543960.7A priority Critical patent/CN115170772A/zh
Publication of CN115170772A publication Critical patent/CN115170772A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/003—Navigation within 3D models or images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Software Systems (AREA)
  • Geometry (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于点云地图交互式可通过性分析的三维路径规划方法，包括如下步骤：基于点云数据，建立具有多层结构的栅格地图；基于多层栅格地图，生成三维地形与移动机器人交互式可通过性地图和层间可连通性地图；添加可通过性值作为代价因素，改进A*算法，生成基于点云地图交互式可通过性分析的三维规划路径。本发明能够结合移动机器人通行能力与地形信息，考虑断崖、陡坡、斜坡、底盘碰撞、未观测地形等对移动机器人的可通过性影响，规划出适用于复杂三维地形的移动机器人最短安全行进路径。

Description

一种基于点云地图交互式可通过性分析的三维路径规划方法
技术领域
本发明实施方式涉及机器人技术领域，是一种基于点云地图交互式可通过性分析的三维路径规划方法。
背景技术
随着计算机和控制技术的发展，移动机器人应用领域越来越广泛，其工作环境从室内结构化环境延伸到了各种室外的复杂环境。传统二维空间的避障路径规划通常简单地把地图分为可通过区和不可通过区，可为大部分的避障场景规划出一条安全路径。传统的路径规划算法有A*算法，D*算法和人工势场法等。其中A*算法实时性差；D*算法对距离较远的最短路径上发生的变化不适用；人工势场法容易陷入局部最优。这些二维路径规划算法都没有将三维复杂环境与移动机器人的通行能力进行结合，不适用于三维复杂地形的移动机器人路径规划。
专利文献公布号为CN112800925A的发明专利是基于深度学习的路径规划方法，使用经开源数据集预训练后的语义分割网络对当前地图信息进行可通过性分析。公布号为CN113419539A的发明专利通过引入坡度信息改进A*算法代价函数，从而利用距离代价求取最优路径，使得路径更加平缓。这两项发明专利只考虑到地形的障碍情况与地形坡度信息，没有考虑地形与车体运动学的交互关系，不能处理在三维复杂地形中常见的车体倾覆、底盘碰撞、车轮悬空等实际情况。
发明内容：
为了解决现有技术问题，本发明的目的在于克服已有技术存在的不足，提供一种基于点云地图交互式可通过性分析的三维路径规划方法，能够结合移动机器人通行能力与地形的物理信息，考虑断崖、陡坡、斜坡、底盘碰撞、未观测地形对移动机器人的可通过性影响，规划出适用于复杂三维地形的移动机器人最短安全行进路径。
为达到上述发明创造目的，本发明采用如下技术方案：
一种基于点云地图交互式可通过性分析的三维路径规划方法，包括如下三步骤：
第一步，基于点云数据，对点云数据的XYZ值区间进行分段划分，构建具有多层结构的栅格地图；
第二步，基于多层结构的栅格地图，综合分析移动机器人运动学和三维地形交互，计算移动机器人通过时可能发生的倾覆侧翻、底盘碰撞、车轮悬空等危险情况，生成基于三维地形与移动机器人交互的可通过性地图F-map和层间可连通性地图C-map；
第三步，基于可通过性地图F-map和层间可连通性地图C-map，修改A*算法的评价函数，添加可通过性值作为代价因素，生成基于点云地图交互式可通过性分析的改进A*算法，利用该算法计算出最短三维避障路径。
优选地，本发明基于点云地图交互式可通过性分析的三维路径规划方法，包括以下步骤：
(1)基于点云数据，构建具有多层结构的栅格地图；定义XY平面为水平平面、Z方向竖直向上，将点云数据所在的三维空间分割成均匀栅格，i,j分别为X,Y方向栅格位置的索引；去除不含点云的栅格后，在XY平面上的每个i,j位置，当两个靠近的含有点云的栅格之间距离大于设定阈值h时，这两个栅格属于不同层，从而把三维空间中的含有点云的栅格构建出多层结构的栅格地图，每个栅格层用
Figure BDA0003649075760000021
表示；其中，k是Z方向上所属层级的索引；栅格层
Figure BDA0003649075760000022
的上表面栅格定义为
Figure BDA0003649075760000023
所属栅格中最上层的均匀栅格；
(2)基于多层结构的栅格地图，综合分析移动机器人运动学和三维地形交互，计算移动机器人通过时可能发生的倾覆侧翻、底盘碰撞、车轮悬空等危险情况，生成基于三维地形与移动机器人交互的可通过性地图F-map和层间可连通性地图C-map；
(2-1)构建层间可连通性地图C-map可以通过下述步骤求得：
定义ai,j为点云数据所在三维空间在i,j位置的地图网格；遍历多层结构栅格地图中所有的栅格层
Figure BDA0003649075760000024
对于没有点云的栅格层
Figure BDA0003649075760000025
不对其赋予连通度值；当栅格层
Figure BDA0003649075760000026
中有点云数据时，用
Figure BDA0003649075760000027
表示网格ai,j中第k个栅格层
Figure BDA0003649075760000028
到相邻网格am,n的连通性值，其中{(m,n)|m＝i+1,i,i-1；n＝j+1,j,j-1；m,n不能同时为i,j}；
Figure BDA0003649075760000029
是am,n处的任意栅格层，
Figure BDA00036490757600000210
和
Figure BDA00036490757600000211
上表面栅格在Z方向上的坐标分别为
Figure BDA00036490757600000212
和
Figure BDA00036490757600000213
若两个水平方向上相邻的栅格层的上表面栅格在Z方向上的差值
Figure BDA00036490757600000214
大于车辆底盘高度阈值cmax，则认为
Figure BDA00036490757600000215
和
Figure BDA00036490757600000216
不相连、两者属于不同地形的表面；若am,n处任意一个栅格层均不与
Figure BDA00036490757600000217
相连，将Ck i,j→m,n设置为0；若am,n处存在差值
Figure BDA00036490757600000218
小于cmax的情况，视为两相邻栅格层相连，将
Figure BDA00036490757600000219
设置为1；点云数据所在三维空间中的所有
Figure BDA00036490757600000220
即构成该三维空间的层间可连通性地图C-map；
(2-2)基于多层结构栅格地图，计算栅格层
Figure BDA00036490757600000221
到水平方向上相邻的栅格层的可通过性
Figure BDA00036490757600000222
表示网格ai,j中第k个栅格层
Figure BDA00036490757600000223
到相邻网格am,n的连通性值，其中{(m,n)|m＝i+1,i,i-1；n＝j+1,j,j-1；m,n不能同时为i,j}；针对每个
Figure BDA00036490757600000224
存在8个
Figure BDA00036490757600000225
用来评价移动机器人在当前位置向周围八个方向行驶的安全性，点云数据所在三维空间中的所有
Figure BDA0003649075760000031
即构成该三维空间的可通过性地图F-map；分析当前栅格层
Figure BDA0003649075760000032
可通过性值
Figure BDA0003649075760000033
时，将机器人放置于地形上，机器人几何中心放置于该栅格层
Figure BDA0003649075760000034
的上表面栅格上方，机器人的前进方向分别沿着八个相邻网格的方向，车轮所在上表面栅格为在车轮中心沿XY方向投影的网格内与
Figure BDA0003649075760000035
的上表面栅格在Z方向上距离最近的上表面栅格；
计算
Figure BDA0003649075760000036
可通过性值的步骤如下：
Figure BDA0003649075760000037
分如下四种情况；第一种情况，若当前移动机器人车轮所在XY方向投影网格的点云为空，移动机器人在该位置的可通行性无法衡量，认为该位置和行驶方向是危险的，标记该栅格层的可通过性
Figure BDA0003649075760000038
为极大值η；第二种情况考虑底盘与地面障碍物发生碰撞，机器人底盘处点云与车轮拟合平面的高度差为
Figure BDA0003649075760000039
其中Zc表示当前移动机器人的所有车轮所在上表面栅格的拟合平面的法向量，
Figure BDA00036490757600000310
为所有车轮所在上表面栅格的点云向量，p为底盘在XY方向投影的网格范围内，高度与机器人几何中心最接近的上表面栅格的点云向量；若机器人底盘处点云与车轮拟合平面的高度差大于或等于机器人实际底盘高度H，即
Figure BDA00036490757600000311
Figure BDA00036490757600000312
则认为发生碰撞，并将可通过性值
Figure BDA00036490757600000313
标记为极大值η；第三种情况，计算前述车轮拟合平面的法向量Zc与竖直法向量Z之间的夹角α，当α大于夹角阈值
Figure BDA00036490757600000314
时认为发生侧翻打滑现象，将可通过性值
Figure BDA00036490757600000315
标记为极大值η；第四种情况，当前三种情况均不满足时，
Figure BDA00036490757600000316
可通过性值为|tanα|；
(3)在得到可通过性地图F-map与层间可连通性地图C-map的基础上，本方法采用改进A*算法，生成机器人在三维空间中的规划路径；
给定一个起始栅格和一个目标栅格，采用改进A*算法启发式地在8个方向上利用层间可连通性地图搜索相邻位置的栅格层，以此来扩展其路径；改进A*算法在三维空间中搜索当前栅格层
Figure BDA00036490757600000317
的相邻栅格层时，只考虑在C-map中
Figure BDA00036490757600000318
值为1的
Figure BDA00036490757600000319
的相邻栅格层，标记为
Figure BDA00036490757600000320
改进A*算法中的旅行成本
Figure BDA00036490757600000321
为累积路径成本
Figure BDA00036490757600000322
启发式目标距离
Figure BDA00036490757600000323
和可通过性地图F-map中可通过性值
Figure BDA00036490757600000324
的组合，即总旅行代价
Figure BDA00036490757600000325
其中λ为一个常量，用于平衡距离代价和可通过性代价，可根据可通过性地图的分辨率进行调整。
本发明与现有技术相比较，具有如下显而易见的突出实质性特点和显著优点：
1、本发明基于点云地图构建可通过性地图和层间可连通性地图，该两张地图综合分析了车体运动学与复杂地形的交互关系，能处理断崖、陡坡、斜坡、底盘碰撞、未观测地形对移动机器人的通过性影响，能够针对实际复杂地形进行符合移动机器人工程实践的安全性评估；
2、本发明根据层间可连通性信息和可通过性值改进A*算法，不仅兼顾了计算效率和路径代价，而且相比于传统路径规划方法，更适用于对复杂三维环境进行路径规划，更符合移动机器人野外应用场景对路径规划方法的要求。
附图说明
图1为本发明一种基于点云地图的交互式可通过性分析的三维路径规划方法流程图。
图2为本发明的可通过性地图定义图。
图3为本发明的移动机器人的简化几何模型。
图4为本发明的移动机器人地盘碰撞示意图。
图5为本发明的移动机器人的倾斜角示意图。
具体实施方式
下面结合附图及具体实施，对本发明作进一步详细说明。
参阅图1，图1是本发明的主框架流程，首先基于点云数据生成多层结构的栅格地图，接下来计算移动机器人通过时可能发生的倾覆侧翻、底盘碰撞、车轮悬空等危险情况，生成基于三维地形与移动机器人交互式可通过性地图和层间可连通性地图。最后，修改A*算法的评价函数，生成基于点云地图交互式可通过性分析的三维规划路径，具体步骤如下：
第一步，基于点云数据，定义XY平面为水平平面、Z方向竖直向上，将点云数据所在的三维空间分割成均匀栅格，i,j分别为X,Y方向栅格位置的索引；去除不含点云的栅格后，在XY平面上的每个i,j位置，当两个靠近的含有点云的栅格之间距离大于设定阈值h时，这两个栅格属于不同层，从而把三维空间中的含有点云的栅格构建出多层结构的栅格地图，每个栅格层用
Figure BDA0003649075760000041
表示。其中，k是Z方向上所属层级的索引。栅格层
Figure BDA0003649075760000042
的上表面栅格定义为
Figure BDA0003649075760000043
所属栅格中最上层的均匀栅格。
第二步，首先构建层间可连通性地图C-map。C-map可以通过下述步骤求得：
定义ai,j为点云数据所在三维空间在i,j位置的地图网格。遍历多层结构栅格地图中所有的栅格层
Figure BDA0003649075760000044
对于没有点云的栅格层
Figure BDA0003649075760000045
不对其赋予连通度值。当栅格层
Figure BDA0003649075760000046
中有点云数据时，用
Figure BDA0003649075760000047
表示网格ai,j中第k个栅格层
Figure BDA0003649075760000048
到相邻网格am,n的连通性值，其中{(m,n)|m＝i+1,i,i-1；n＝j+1,j,j-1；m,n不能同时为i,j}。
Figure BDA0003649075760000049
是am,n处的任意栅格层，
Figure BDA00036490757600000410
和
Figure BDA00036490757600000411
上表面栅格在Z方向上的坐标分别为
Figure BDA0003649075760000051
和
Figure BDA0003649075760000052
若两个水平方向上相邻的栅格层的上表面栅格在Z方向上的差值
Figure BDA0003649075760000053
大于车辆底盘高度阈值cmax，则认为
Figure BDA0003649075760000054
和
Figure BDA0003649075760000055
不相连、两者属于不同地形的表面。若am,n处任意一个栅格层均不与
Figure BDA0003649075760000056
相连，将Ck i,j→m,n设置为0；若am,n处存在差值
Figure BDA0003649075760000057
小于cmax的情况，视为两相邻栅格层相连，将Ck i,j→m,n设置为1。点云数据所在三维空间中的所有
Figure BDA0003649075760000058
即构成该三维空间的层间可连通性地图C-map。
基于多层结构栅格地图，计算栅格层
Figure BDA0003649075760000059
到水平方向上相邻的栅格层的可通过性
Figure BDA00036490757600000510
Figure BDA00036490757600000511
表示网格ai,j中第k个栅格层
Figure BDA00036490757600000512
到相邻网格am,n的连通性值，其中{(m,n)|m＝i+1,i,i-1；n＝j+1,j,j-1；m,n不能同时为i,j}。针对每个
Figure BDA00036490757600000513
存在8个
Figure BDA00036490757600000514
用来评价移动机器人在当前位置向周围八个方向行驶的安全性，点云数据所在三维空间中的所有
Figure BDA00036490757600000515
即构成该三维空间的可通过性地图F-map。分析当前栅格层
Figure BDA00036490757600000516
可通过性值
Figure BDA00036490757600000517
时，将机器人放置于地形上，机器人几何中心放置于该栅格层
Figure BDA00036490757600000518
的上表面栅格上方，机器人的前进方向分别沿着八个相邻网格的方向，车轮所在上表面栅格为在车轮中心沿XY方向投影的网格内与
Figure BDA00036490757600000519
的上表面栅格在Z方向上距离最近的上表面栅格。
参阅图2，图2是可通过性地图在x-y平面的示意图，图中的中心栅格
Figure BDA00036490757600000520
位于网格ai,j处，
Figure BDA00036490757600000521
表示网格ai,j中第k层栅格
Figure BDA00036490757600000522
到相邻网格am,n的连通性值，8个周围栅格为候选栅格，相对于当前栅格的角度值分别为φ＝0°,45°,90°,135°,180°,-45°,-90°,-135°。列举一个简易的移动机器人模型，参阅图3，其中L是车体的侧长，W为移动机器人的侧宽，H为移动机器人底盘高，R为车轮半径。
计算
Figure BDA00036490757600000523
可通过性值的步骤如下：
Figure BDA00036490757600000524
分如下四种情况。第一种情况，若当前移动机器人车轮所在XY方向投影网格的点云为空，移动机器人在该位置的可通行性无法衡量，认为该位置和行驶方向是危险的，标记该栅格层的可通过性
Figure BDA00036490757600000525
为极大值η。第二种情况考虑底盘与地面障碍物发生碰撞，机器人底盘处点云与车轮拟合平面的高度差为
Figure BDA00036490757600000526
其中Zc表示当前移动机器人的所有车轮所在上表面栅格的拟合平面的法向量，
Figure BDA00036490757600000527
为所有车轮所在上表面栅格的点云向量，p为底盘在XY方向投影的网格范围内，高度与机器人几何中心最接近的上表面栅格的点云向量。若机器人底盘处点云与车轮拟合平面的高度差大于或等于机器人实际底盘高度H，即
Figure BDA0003649075760000061
Figure BDA0003649075760000062
则认为发生碰撞，并将可通过性值
Figure BDA0003649075760000063
标记为极大值η。第三种情况，计算前述车轮拟合平面的法向量Zc与竖直法向量Z之间的夹角α，参考图5，图5是移动机器人的倾斜角α示意图，当α大于夹角阈值
Figure BDA0003649075760000064
时认为发生侧翻打滑现象，将可通过性值
Figure BDA0003649075760000065
标记为极大值η。第四种情况，当前三种情况均不满足时，
Figure BDA0003649075760000066
可通过性值为|tanα|。
第三步，在得到F-map与C-map的基础上，本方法采用改进A*算法，生成机器人在三维空间中的规划路径。给定一个起始栅格和一个目标栅格，采用改进A*算法启发式地在8个方向上利用层间可连通性地图搜索相邻位置的栅格层，以此来扩展其路径。改进A*算法在三维空间中搜索当前栅格层
Figure BDA0003649075760000067
的相邻栅格层时，只考虑在C-map中
Figure BDA0003649075760000068
值为1的
Figure BDA0003649075760000069
的相邻栅格层，标记为
Figure BDA00036490757600000610
改进A*算法中的旅行成本
Figure BDA00036490757600000611
是累积路径成本
Figure BDA00036490757600000612
启发式目标距离
Figure BDA00036490757600000613
和可通过性地图F-map中当前旅行代价
Figure BDA00036490757600000614
的组合，即总旅行代价
Figure BDA00036490757600000615
Figure BDA00036490757600000616
其中λ为一个常量，用于平衡距离代价和可通过性代价，需要根据可通过性地图的分辨率进行调整。
所述改进A*算法中，相对于传统A*算的改进主要在于：1)利用层间可连通性地图对相邻栅格层的连通性进行分析；2)利用可通过性地图对每个栅格层与其周围栅格层的可通过危险性进行评估；3)改进A*算法的旅行代价函数，添加可通过性值作为代价。
本发明上述实施例基于点云地图交互式可通过性分析的三维路径规划方法，包括如下步骤：基于点云数据，建立具有多层结构的栅格地图；基于多层栅格地图，生成三维地形与移动机器人交互式可通过性地图和层间可连通性地图；添加可通过性值作为代价因素，改进A*算法，生成基于点云地图交互式可通过性分析的三维规划路径。本发明上述实施例方法能够结合移动机器人通行能力与地形信息，考虑断崖、陡坡、斜坡、底盘碰撞、未观测地形等对移动机器人的可通过性影响，规划出适用于复杂三维地形的移动机器人最短安全行进路径。
上面对本发明实施例结合附图进行了说明，但本发明不限于上述实施例，还可以根据本发明的发明创造的目的做出多种变化，凡依据本发明技术方案的精神实质和原理下做的改变、修饰、替代、组合或简化，均应为等效的置换方式，只要符合本发明的发明目的，只要不背离本发明的技术原理和发明构思，都属于本发明的保护范围。

Claims (5)

1.一种基于点云地图交互式可通过性分析的三维路径规划方法，其特征在于，包括如下三步骤：
第一步，基于点云数据，对点云数据的XYZ值区间进行分段划分，构建具有多层结构的栅格地图；
第二步，基于多层结构的栅格地图，综合分析移动机器人运动学和三维地形交互，计算移动机器人通过时可能发生的倾覆侧翻、底盘碰撞、车轮悬空等危险情况，生成基于三维地形与移动机器人交互的可通过性地图F-map和层间可连通性地图C-map；
第三步，基于可通过性地图F-map和层间可连通性地图C-map，修改A*算法的评价函数，添加可通过性值作为代价因素，生成基于点云地图交互式可通过性分析的改进A*算法，利用该算法计算出最短三维避障路径。
2.根据权利要求1所述的基于点云地图交互式可通过性分析的三维路径规划方法，其特征在于：在第一步中，利用点云数据生成多层结构的栅格地图方法，步骤如下：
定义XY平面为水平平面、Z方向竖直向上，将点云数据所在的三维空间分割成均匀栅格，i,j分别为X,Y方向栅格位置的索引；去除不含点云的栅格后，在XY平面上的每个i,j位置，当两个靠近的含有点云的栅格之间距离大于设定阈值h时，这两个栅格属于不同层，从而把三维空间中的含有点云的栅格构建出多层结构的栅格地图，每个栅格层用
Figure FDA0003649075750000011
表示；其中，k是Z方向上所属层级的索引；栅格层
Figure FDA0003649075750000012
的上表面栅格定义为
Figure FDA0003649075750000013
所属栅格中最上层的均匀栅格。
3.根据权利要求1所述的基于点云地图交互式可通过性分析的三维路径规划方法，其特征在于：在第二步中，所述的层间可连通性地图C-map构建步骤为：
定义ai,j为点云数据所在三维空间在i,j位置的地图网格；遍历多层结构栅格地图中所有的栅格层
Figure FDA0003649075750000014
对于没有点云的栅格层
Figure FDA0003649075750000015
不对其赋予连通度值；当栅格层
Figure FDA0003649075750000016
中有点云数据时，用
Figure FDA0003649075750000017
表示网格ai,j中第k个栅格层
Figure FDA0003649075750000018
到相邻网格am,n的连通性值，其中{(m,n)|m＝i+1,i,i-1；n＝j+1,j,j-1；m,n不能同时为i,j}；
Figure FDA00036490757500000111
是am,n处的任意栅格层，
Figure FDA0003649075750000019
和
Figure FDA00036490757500000110
上表面栅格在Z方向上的坐标分别为
Figure FDA00036490757500000112
和
Figure FDA00036490757500000113
若两个水平方向上相邻的栅格层的上表面栅格在Z方向上的差值
Figure FDA00036490757500000114
大于车辆底盘高度阈值cmax，则认为
Figure FDA00036490757500000115
和
Figure FDA00036490757500000116
不相连、两者属于不同地形的表面；若am,n处任意一个栅格层均不与
Figure FDA00036490757500000117
相连，将
Figure FDA00036490757500000118
设置为0；若am,n处存在差值
Figure FDA00036490757500000119
小于cmax的情况，视为两相邻栅格层相连，将
Figure FDA00036490757500000120
设置为1；点云数据所在三维空间中的所有
Figure FDA0003649075750000021
即构成该三维空间的层间可连通性地图C-map。
4.根据权利要求1所述的基于点云地图交互式可通过性分析的三维路径规划方法，其特征在于：在第二步中，所述的基于点云数据与移动机器人交互式可通过性地图F-map，包括如下步骤：
基于多层结构栅格地图，计算栅格层
Figure FDA0003649075750000022
到水平方向上相邻的栅格层的可通过性
Figure FDA0003649075750000023
Figure FDA0003649075750000024
表示网格ai,j中第k个栅格层
Figure FDA0003649075750000025
到相邻网格am,n的连通性值，其中{(m,n)|m＝i+1,i,i-1；n＝j+1,j,j-1；m,n不能同时为i,j}；针对每个
Figure FDA0003649075750000026
存在8个
Figure FDA0003649075750000027
用来评价移动机器人在当前位置向周围八个方向行驶的安全性，点云数据所在三维空间中的所有
Figure FDA0003649075750000028
即构成该三维空间的可通过性地图F-map；分析当前栅格层
Figure FDA0003649075750000029
可通过性值
Figure FDA00036490757500000210
时，将机器人放置于地形上，机器人几何中心放置于该栅格层
Figure FDA00036490757500000211
的上表面栅格上方，机器人的前进方向分别沿着八个相邻网格的方向，车轮所在上表面栅格为在车轮中心沿XY方向投影的网格内与
Figure FDA00036490757500000212
的上表面栅格在Z方向上距离最近的上表面栅格；
计算可通过性值
Figure FDA00036490757500000213
的步骤如下：
Figure FDA00036490757500000214
分如下四种情况：
第一种情况：若当前移动机器人车轮所在XY方向投影网格的点云为空，移动机器人在该位置的可通行性无法衡量，认为该位置和行驶方向是危险的，标记该栅格层的可通过性
Figure FDA00036490757500000215
为极大值η；
第二种情况：考虑底盘与地面障碍物发生碰撞，机器人底盘处点云与车轮拟合平面的高度差为
Figure FDA00036490757500000216
其中Zc表示当前移动机器人的所有车轮所在上表面栅格的拟合平面的法向量，
Figure FDA00036490757500000217
为所有车轮所在上表面栅格的点云向量，p为底盘在XY方向投影的网格范围内，高度与机器人几何中心最接近的上表面栅格的点云向量；若机器人底盘处点云与车轮拟合平面的高度差大于或等于机器人实际底盘高度H，即
Figure FDA00036490757500000218
则认为发生碰撞，并将可通过性值
Figure FDA00036490757500000219
标记为极大值η；
第三种情况：计算前述车轮拟合平面的法向量Zc与竖直法向量Z之间的夹角α，当α大于夹角阈值
Figure FDA00036490757500000221
时认为发生侧翻打滑现象，将可通过性值
Figure FDA00036490757500000220
标记为极大值η；
第四种情况：当前三种情况均不满足时，
Figure FDA00036490757500000222
的可通过性值为|tanα|。
5.根据权利要求1所述的基于点云地图交互式可通过性分析的三维路径规划方法，其特征在于：在第三步中，所述的基于点云地图交互式可通过性分析的三维路径规划步骤如下：
首先，改进A*算法在三维空间中搜索当前栅格层
Figure FDA0003649075750000031
的相邻栅格层时，只考虑在C-map中
Figure FDA0003649075750000032
值为1的
Figure FDA0003649075750000033
的相邻栅格层，标记为
Figure FDA0003649075750000034
同时，改进A*算法中的旅行成本
Figure FDA0003649075750000035
为累积路径成本
Figure FDA0003649075750000038
启发式目标距离
Figure FDA0003649075750000037
和可通过性地图F-map中可通过性值
Figure FDA0003649075750000036
的组合，即总旅行代价
Figure FDA0003649075750000039
其中λ为一个常量，用于平衡距离代价和可通过性代价，可根据可通过性地图的分辨率进行调整。
CN202210543960.7A 2022-05-18 2022-05-18 一种基于点云地图交互式可通过性分析的三维路径规划方法 Pending CN115170772A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210543960.7A CN115170772A (zh) 2022-05-18 2022-05-18 一种基于点云地图交互式可通过性分析的三维路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210543960.7A CN115170772A (zh) 2022-05-18 2022-05-18 一种基于点云地图交互式可通过性分析的三维路径规划方法

Publications (1)

Publication Number Publication Date
CN115170772A true CN115170772A (zh) 2022-10-11

Family

ID=83484254

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210543960.7A Pending CN115170772A (zh) 2022-05-18 2022-05-18 一种基于点云地图交互式可通过性分析的三维路径规划方法

Country Status (1)

Country Link
CN (1) CN115170772A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115375713A (zh) * 2022-10-25 2022-11-22 杭州华橙软件技术有限公司 地面点云的分割方法、装置及计算机可读存储介质
CN115619900B (zh) * 2022-12-16 2023-03-10 中国科学技术大学 基于距离地图和概率路图的点云地图拓扑结构提取方法
CN117369480A (zh) * 2023-12-05 2024-01-09 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115375713A (zh) * 2022-10-25 2022-11-22 杭州华橙软件技术有限公司 地面点云的分割方法、装置及计算机可读存储介质
CN115619900B (zh) * 2022-12-16 2023-03-10 中国科学技术大学 基于距离地图和概率路图的点云地图拓扑结构提取方法
CN117369480A (zh) * 2023-12-05 2024-01-09 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统
CN117369480B (zh) * 2023-12-05 2024-02-23 北京理工大学 一种室内复杂环境下轮腿机器人路径规划方法与系统

Similar Documents

Publication Publication Date Title
CN115170772A (zh) 一种基于点云地图交互式可通过性分析的三维路径规划方法
Tang et al. Geometric A-star algorithm: An improved A-star algorithm for AGV path planning in a port environment
CN110108284B (zh) 一种顾及复杂环境约束的无人机三维航迹快速规划方法
CN106125764B (zh) 基于a*搜索的无人机路径动态规划方法
CN110531770B (zh) 一种基于改进的rrt路径规划方法和系统
CN105526942B (zh) 基于威胁估计的智能车辆路径规划方法
CN105843222B (zh) 一种六轮/腿机器人复合运动路径规划方法
CN112184736B (zh) 一种基于欧式聚类的多平面提取方法
CN115100622A (zh) 深部受限空间无人运输设备可行驶域检测和自主避障方法
Liu et al. 3D path planning in threat environment based on fuzzy logic
US11915599B2 (en) Grid based path search method for UAV delivery operations in urban environment
CN114706400B (zh) 一种越野环境下基于改进的a*算法的路径规划方法
CN110032182B (zh) 一种融合可视图法和稳定稀疏随机快速树机器人规划算法
CN115903879A (zh) 基于地形数据插值技术的无人机航迹规划方法
CN106708049A (zh) 一种多站接力导航下运动体的路径规划方法
Haddeler et al. Explore bravely: Wheeled-legged robots traverse in unknown rough environment
CN103226845A (zh) 基于tin的细化地表处理方法及系统
CN114791734A (zh) 一种基于语义信息串联的履带车辆的控制方法及系统
Fan et al. An Improved JPS Algorithm for Global Path Planning of the Seabed Mining Vehicle
CN116147653B (zh) 一种面向无人驾驶车辆的三维参考路径规划方法
Li et al. A Global Path Planning Method for Unmanned Ground Vehicles in Off-Road Scenarios Based on Terrain Data
CN115628748A (zh) 一种基于分层混合代价地图的改进Q-learning路径规划方法
Wan et al. Railway alignment optimization in regions with densely-distributed obstacles based on semantic topological maps
Naveen et al. 3D mapping and stability prediction for autonomous wheelchairs
CN112232560A (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