CN114577214B - 一种应用于跨异构多层空间的轮式机器人路径规划方法 - Google Patents
一种应用于跨异构多层空间的轮式机器人路径规划方法 Download PDFInfo
- Publication number
- CN114577214B CN114577214B CN202210203947.7A CN202210203947A CN114577214B CN 114577214 B CN114577214 B CN 114577214B CN 202210203947 A CN202210203947 A CN 202210203947A CN 114577214 B CN114577214 B CN 114577214B
- Authority
- CN
- China
- Prior art keywords
- map
- grid
- node
- path
- point
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 94
- 230000008569 process Effects 0.000 claims abstract description 54
- 238000013507 mapping Methods 0.000 claims abstract description 10
- 238000010276 construction Methods 0.000 claims description 31
- 230000036544 posture Effects 0.000 claims description 31
- 239000011159 matrix material Substances 0.000 claims description 29
- 230000003068 static effect Effects 0.000 claims description 25
- 230000009466 transformation Effects 0.000 claims description 16
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 230000004888 barrier function Effects 0.000 claims description 5
- 238000005259 measurement Methods 0.000 claims description 4
- 230000001131 transforming effect Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 3
- 241000287196 Asthenes Species 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种应用于跨异构多层空间的轮式机器人路径规划方法,本发明涉及多层空间的移动机器人路径规划方法。本发明的目的是为了解决路径规划算法无法获得准确的多层空间的机器人路径规划的问题。一种应用于跨异构多层空间的轮式机器人路径规划方法具体过程为:步骤1、构建多层空间的地图网络;步骤2、基于多层空间的地图网络对地图网络中的路径进行搜索。本发明用于机器人建图与导航算法与技术领域。
Description
技术领域
本发明涉及多层空间的移动机器人路径规划方法,属于机器人建图与导航算法与技术领域。
背景技术
轮式移动机器人作为常见的移动机器人,具有结构简单、成本低、稳定性好的特点,可以在多种场景下发挥重要作用。移动机器人采用激光雷达、摄像头、惯性测量单元、里程计等多种传感器构建环境地图、进行自身定位,在地图中规划运动路径与运动速度以到达预定目标。目前机器人在二维平面内的定位与路径规划、运动规划方法已经比较成熟。
随着科学技术的发展,轮式移动机器人的应用领域日益广泛,活动范围不再局限于二维平面,逐渐扩展到三维多层空间。跨异构多层空间由多个高度不同的二维平面区域与平面之间的连接区域组成,各二维平面区域具有不同的形状,常见的跨异构多层空间包括地铁站、机场、地下停车场等。机器人在跨异构多层复杂空间内执行清洁、巡逻等任务过程中,需要进行多层空间中的运动路径规划。相比于二维平面,多层空间的路径规划问题的环境复杂度增大,路径规划的计算量增大。跨异构多层复杂空间在二维平面上的映射会产生区域重叠问题,影响二维平面路径规划方法的有效性。此外,跨异构多层复杂空间的路径规划中还存在二维平面内不会出现的机器人高度变化、轮式移动机器人姿态约束等问题。这些问题使得传统二维地图的路径规划算法难以直接应用于跨异构多层复杂空间。
发明内容
本发明的目的是为了解决现有跨异构多层复杂空间的路径规划中还存在二维平面内不会出现的机器人高度变化、轮式移动机器人姿态约束等问题,这些问题使得传统二维地图的路径规划算法难以直接应用于跨异构多层复杂空间,导致无法获得准确的多层空间的机器人路径规划的问题,而提出一种应用于跨异构多层空间的轮式机器人路径规划方法。
一种应用于跨异构多层空间的轮式机器人路径规划方法具体过程为:
步骤1、构建多层空间的地图网络;
步骤2、基于多层空间的地图网络对地图网络中的路径进行搜索。
本发明的有益效果为:
1.本发明提出了以多个二维栅格地图组成地图网络描述跨异构多层复杂空间的方法。相比于常规以三维点云地图描述空间环境的方法,本发明可以有效降低地图存储数据量;
2.本发明提出了基于地图网络的分层A*搜索路径规划方法,以分层二维空间规划的方法解决了跨异构多层空间路径规划的问题;
3.本发明考虑轮式机器人运动约束,提出了考虑地形因素的二维栅格地图路径规划方法,解决了机器人在复杂地形环境下的运动约束问题。
附图说明
图1为本发明地图网络示意图;
图2a为本发明空间区域地图示意图;
图2b为本发明多层二维栅格地图示意图;
图3为本发明地图网络中路径规划示意图;
图4为本发明二维栅格地图路径规划示意图;
图5为本发明地图网络构建流程图;
图6为本发明地图网络内路径规划流程图;
图7为本发明二维栅格地图内路径规划流程图。
具体实施方式
具体实施方式一:本实施方式一种应用于跨异构多层空间的轮式机器人路径规划方法具体过程为:
如图1所示,地图网络由多个路径节点与多个地图组成,由单张地图作为连接地图网络中各路径节点的边。
如图2a、2b所示,左图所示跨异构空间的部分区域包含楼层平面区域与楼梯斜坡区域,平面区域内包含障碍物。对应二维地图如右图所示,静态地图层包含SLAM建图测得的边界与障碍物位置,代价地图层以一定膨胀半径对障碍边界部分作膨胀,剩余区域为可通行区域,地形地图层中标记楼梯斜坡区域信息。
如图3所示,图中point0至point8为地图网络原有路径节点,pointstart和pointdest为路径规划的起点与终点,分别位于地图mapstart和mapdest上。创建起点、终点与各自原有地图建图起点与终点之间的二维栅格地图mapstart1、mapstart2、mapdest1、mapdest2,以原有二维栅格地图mapstart和mapdest内容作为新建二维栅格地图内容,并替换地图起点与终点位置信息。以原有地图网络和新建路径节点、新建二维栅格地图组成临时地图网络,在临时地图网络中从起点pointstart处开始A*搜索。
如图4所示,在二维栅格地图中规划从起点栅格点到目标栅格点的路径。图中1点处不在二维栅格地图地形地图层的楼梯斜坡区域上,则按常规方式向该栅格点周围8个栅格点搜索扩展,对8个相邻栅格点进行可行性判断、代价估计等处理;图中2点位于楼梯斜坡区域,则沿斜坡梯度方向及其反方向产生广义栅格点,对2个新建广义栅格点进行可行性判断、代价估计等处理。
跨异构多层空间是指空间中各层平面的构型、边界和障碍物形状、层与层之间的连接是不一样的,是一种异构的多层空间,例如机场火车站等楼宇多层空间。这种方法可以实现机器人跨层的运动,所以是跨异构多层空间的轮式机器人路径规划方法。
本发明主要用于解决跨异构多层空间的轮式移动机器人路径规划问题,主要包括多层空间的地图网络构建方法、基于地图网络的分层A*搜索路径规划方法两部分。
步骤1、构建多层空间的地图网络(多层空间指地铁站、机场、地下停车场等多层建筑,多层为大于等于2层);
步骤2、基于多层空间的地图网络对地图网络中的路径进行搜索。
具体实施方式二:本实施方式与具体实施方式一不同的是,所述步骤1中构建多层空间的地图网络(多层空间指地铁站、机场、地下停车场等多层建筑,多层为大于等于2层);具体过程为:
步骤11、对多层空间在楼梯与平面连接处、楼梯转台等位置进行分割,划分为若干区域,各区域在二维平面上的投影不出现重叠;
多层空间地图网络由空间中的路径节点(区域分割位置)与各区域的二维栅格地图连接组成;
多层空间地图网络由空间中的路径节点作为网络的节点,由二维栅格地图作为网络的边;
多层空间地图网络的路径节点集合记为Points={pointi|i=0,1,2,…,N-1};
其中N为地图网络中路径节点个数;pointi为第i个路径节点;
多层空间地图网络的二维栅格地图集合记为Maps={mapj|j=0,1,2,…,M-1};
其中M为地图网络中二维栅格地图个数;mapj为第j个二维栅格地图;
多层空间地图网络信息由路径节点集合和二维栅格地图集合组成,记为GraphNet=(Points,Maps);
多层空间地图网络分为静态地图层staticmapj、代价地图层costmapj和地形地图层terrainmapj;
二维栅格地图两端为路径点,其中一点作为二维栅格地图的建图原点(坐标系原点);
二维栅格地图由多层空间的一部分区域在二维平面内的投影以一定的分辨率(一般为0.05m)栅格化得到,记录该区域二维栅格地图的建图起点与建图终点startj,destj∈Points;
多层空间地图分为静态地图层staticmapj、代价地图层costmapj与地形地图层terrainmapj;
其中静态地图层将地图区域划分为可通行区域、障碍占据区域和未知区域;
代价地图层由静态地图对障碍占据区域进行膨胀处理得到,用于路径规划中的障碍物躲避;
地形地图层包含地图区域的斜坡与楼梯区域的位置与梯度方向信息,用于规划机器人在特殊区域的路径规划,以满足机器人的运动约束;
二维栅格地图以nj×mj矩阵形式存储,行方向为二维栅格地图坐标系y方向,列方向为二维栅格地图坐标系x方向,并记录二维栅格地图矩阵原点在地图坐标系中的x坐标oxj与y坐标oyj,并记录二维栅格地图坐标系相对于世界坐标系的位置姿态;
二维栅格地图表示为:
其中,staticmapj为静态地图层,costmapj为代价地图层,terrainmapj为地形地图层;staticmapj,costmapj, 表示地图坐标系Fj在世界坐标系F0中的位置姿态(以齐次变换矩阵表示),startj、destj分别为二维栅格地图的建图起点与建图终点;
地图构建过程中,机器人利用所携带的激光雷达传感器、惯性测量单元传感器(IMU)、里程计、高度计等传感器建立多层空间的地图网络,机器人的路径规划通过地图网络实现;
步骤12:建立二维栅格地图列表、路径节点列表,初始状态二维栅格地图列表和路径节点列表为空列表;
以第一张二维栅格地图建图起点为原点,建立地图坐标系F0(第一个地图坐标系同时作为世界坐标系),该地图坐标系同时作为世界坐标系,将地图坐标系中当前位置point0(坐标原点)添加到路径节点列表中,并记录该节点(当前位置point0)的位置姿态,位置姿态由齐次变换矩阵表示为
步骤13:操作人员遥控机器人运动至地图网络已有路径节点处,以当前位置为二维栅格地图建图起点startj,建立地图坐标系Fj,采集激光雷达、IMU与里程计数据,进行二维栅格地图构建(构建方法为SLAM);
遥控机器人运动至环境中合适位置,进行二维栅格地图构建(构建方法为SLAM),使当前构建二维栅格地图对应区域在二维空间上不会出现重叠,将机器人当前位置作为二维栅格地图建图终点,即地图网络中的路径节点,记为destj,完成当前地图坐标系Fj下地图的二维栅格地图静态地图层部分构建;
步骤14:记录二维栅格地图建图终点destj在当前地图坐标系内的位置姿态,其中地图坐标系x坐标与y坐标由二维栅格地图构建过程得到,z坐标由高度计测得,姿态角度由IMU测得,并将二维栅格地图建图终点destj在当前地图坐标系内的位置姿态转换为齐次变换矩阵:
将二维栅格地图建图终点destj在当前地图坐标系内的位置姿态转换为在世界坐标系内的位置姿态:
步骤15:在已有的路径节点列表中搜索当前二维栅格地图建图终点,根据世界坐标系内位置姿态判断当前二维栅格地图建图终点是否已存在于路径节点列表中:如果不存在,则将该终点添加到路径节点列表中,将该地图添加到二维栅格地图列表中;如果已存在,则将地图添加到二维栅格地图列表中,并将该二维栅格地图的建图终点标记为路径节点列表中已有的终点节点;
步骤16:如果二维栅格地图网络已包含多层空间的全部可通行区域,则判定为多层空间地图网络静态地图层构建完成,执行步骤17;如果多层空间地图构建未完成,则在当前位置返回步骤13,开始新的二维栅格地图构建;
步骤17:对静态地图层进行膨胀处理,得到代价地图层costmapj;
步骤18:基于静态地图层构建地形地图层terrainmapj。
其它步骤及参数与具体实施方式一相同。
具体实施方式三:本实施方式与具体实施方式一或二不同的是,所述步骤17中对静态地图进行膨胀处理,得到代价地图层costmapj(以costmapj(cx1,cy1)表示costmapj矩阵第cx1行第cy1列元素);具体过程为:
对地图网络静态地图层中包含的障碍物与地图边界进行膨胀处理,在地图坐标系内膨胀半径为rinflate,设地图栅格分辨率为f,则在二维栅格地图内的膨胀栅格数为:
在静态地图层中,被障碍物占据的栅格对应值为1,可通行区域栅格对应值为0,未知区域为-1;
对于静态地图层障碍物占据栅格,取以膨胀栅格数为半径的邻域:
其中,(cx,cy)表示邻域中心栅格点坐标,(cx1,cy1)表示邻域内栅格点坐标;
将代价地图在该邻域内的栅格的对应值置为1,表示该区域内的栅格点不可通行:
costmapj(cx1,cy1)=1if(cx1,cy1)∈U(cx,cy),staticmapj(cx,cy)=1。
步其它步骤及参数与具体实施方式一或二相同。
具体实施方式四:本实施方式与具体实施方式一至三之一不同的是,所述步骤18中基于静态地图层构建地形地图层terrainmapj(以terrainmapj(cx1,cy1)表示terrainmapj矩阵第cx1行第cy1列元素);具体过程为:
在多层空间中,单个楼梯、斜坡等特殊地形区域一般为多边形,多边形区域内高度梯度方向一般相同。在地形地图层生成过程中,操作人员在二维栅格地图上选择多边形区域作为斜坡与楼梯区域,记为Polygonterrain,并给定斜坡与楼梯区域梯度方向角度θgrad;
在地形地图层中,程序选取操作人员选取多边形区域内的栅格点,将栅格对应数值赋值为梯度方向角度:
terrainmapj(cx1,cy1)=θgrad if(cx1,cy1)∈Polygonterrain。
其它步骤及参数与具体实施方式一至三之一相同。
具体实施方式五:本实施方式与具体实施方式一至四之一不同的是,所述步骤2中基于多层空间的地图网络对地图网络中的路径进行搜索;具体过程为:
步骤21:确定临时地图网络;
步骤22:临时地图网络A*搜索初始化;具体过程为:
建立开放节点列表、关闭节点列表,初始开放节点列表、关闭节点列表为空列表;
计算机器人路径规划起点路径节点pointstart总体估计代价;
机器人路径规划起点路径节点设置为根节点,即父节点为空,将该节点写入开放节点列表;
所述计算机器人路径规划起点路径节点pointstart总体估计代价的具体过程为:
机器人路径规划起点路径节点耗散函数值g(pointstart)为0,启发函数值h(pointstart)为由路径规划起点到目标点的欧氏距离,从而得到路径规划起点路径节点pointstart总体估计代价f(pointstart)=g(pointstart)+h(pointstart);
步骤23:节点的扩展;具体过程为:
其中,为相邻节点耗散函数值,g(point*)为当前最优节点耗散函数值,Kh为设定的高度代价因数,Kh>0;,为相邻节点启发函数值,为在世界坐标系内的位置坐标(包含x、y、z三个坐标分量) 为路径规划终点在世界坐标系内的位置坐标(包含x、y、z三个坐标分量) 为相邻节点总体估计代价;
步骤24:节点列表更新;具体过程为:
1)如果开放节点列表中已存在与相邻节点位置相同的节点pointold,则比较开放节点列表已有节点pointold与相邻节点的总体估计代价:如果相邻节点总体估计代价不超过(小于等于)pointold的总体估计代价,则将开放节点列表中已有节点pointold删除,将相邻节点添加到开放节点列表中;如果相邻节点总体估计代价大于pointold的总体估计代价,则保留已有节点pointold;
步骤242:将当前最优节点point*从开放节点列表中删除,添加到关闭节点列表中。
步骤25:目标(机器人路径规划终点路径节点pointdest)到达状态判断;具体过程为:
如果当前最优节点(步骤23里面的point*)已到达目标节点(当前最优节点为目标节点就完成),则执行步骤26;如果未到达目标节点,则返回步骤23,继续搜索;
步骤26:路径生成;具体过程为:
完成路径搜索后回溯路径搜索过程,创建路径点列表,从目标节点(机器人路径规划终点路径节点pointdest)开始,记录当前节点位置并写入路径点列表,寻找当前节点的父节点,循环迭代执行,直到搜索到机器人路径规划起点路径节点pointstart处,将路径点列表倒序排列,可得地图网络中的规划路径。
其它步骤及参数与具体实施方式一至四之一相同。
具体实施方式六:本实施方式与具体实施方式一至五之一不同的是,所述步骤21中确定临时地图网络;具体过程为:
对已有地图网络进行扩展,生成临时地图网络;
确定机器人当前位置与目标位置及两点所在二维栅格地图,将当前位置与目标位置两点设置为机器人路径规划起点路径节点pointstart和机器人路径规划终点路径节点pointdest,添加到地图网络的路径节点列表中,作为临时路径节点列表;
建立路径节点pointstart到所在区域二维栅格地图mapstart建图起点与建图终点的二维栅格地图mapstart1、mapstart2;
建立路径节点pointdest到所在区域二维栅格地图mapdest建图起点与建图终点的二维栅格地图mapdest1、mapdest2;
将二维栅格地图mapstart、mapdest、mapstart1、mapstart2、mapdest1、mapdest2添加到地图网络的二维栅格地图列表中,作为临时二维栅格地图列表;如图3;
临时地图网络由临时路径节点列表与临时二维栅格地图列表组成,设置为无向网络(每相邻两路径节点可以双向通行);
Pointstemp={Points,pointstart,pointdest}
mapstart1=mapstart,mapstart2=mapstart
mapdest1=mapdest,mapdest2=mapdest
Mapstemp={Maps,mapstart1,mapstart2,mapdest1,mapdest2}
GraphNettemp=(Pointstemp,Mapstemp)
式中,Pointstemp为临时路径节点列表,Points为地图网络的路径节点列表,Maps为地图网络的二维栅格地图列表,Mapstemp为临时二维栅格地图列表,GraphNettemp为临时地图网络。
其它步骤及参数与具体实施方式一至五之一相同。
具体实施方式七:本实施方式与具体实施方式一至六之一不同的是,所述步骤23中在二维栅格地图(二维栅格地图为连接当前最优节点point*到相邻节点的二维栅格地图)中搜索从当前最优节点point*到相邻节点的路径;具体过程为:
步骤231:将二维栅格地图的起点与终点的路径节点位置姿态从世界坐标系变换到当前地图坐标系Fj中;具体过程为:
设二维栅格地图内路径规划的起点与终点分别为pointi1和pointi2,在世界坐标系内的位置姿态齐次变换矩阵别为和设二维栅格地图内地图坐标系在世界坐标系内的位置姿态齐次变换矩阵为则路径规划起点与终点在当前地图坐标系内的位置姿态齐次变换矩阵为:
提取起点与终点在当前地图坐标系Fj中的位置姿态齐次变换矩阵中的x坐标与y坐标部分记为xsj,ysj,xdj,ydj:
式中mij为齐次变换矩阵M的第i行第j列元素;
再将位置坐标映射到栅格坐标系内得到栅格坐标,得到起点与终点栅格点坐标分别为:
其中,cellstart为起点栅格节点坐标,(cxs,cys)为起点栅格节点x坐标与y坐标,(oxj,oyj)为二维栅格地图矩阵原点在地图坐标系中的坐标,f为二维栅格地图分辨率,celldest为终点栅格节点坐标,(cxd,cyd)为终点栅格节点x坐标与y坐标;
步骤232:在二维栅格地图坐标系内采用A*方法以栅格为基本单元进行路径搜索,搜索初始化:具体过程为:
建立开放栅格节点列表、关闭栅格节点列表,初始开放栅格节点列表、关闭栅格节点列表为空列表;
计算机器人路径规划起点栅格节点cellstart总体估计代价;
机器人路径规划起点栅格节点设置为根节点,即父节点为空,将该节点写入开放栅格节点列表;
所述计算机器人路径规划起点栅格节点cellstart总体估计代价的具体过程为:
机器人路径规划起点栅格节点耗散函数值为0,启发函数值为由路径规划起点栅格节点到终点栅格节点的欧氏距离,栅格节点耗散函数值与启发函数值相加得到机器人路径规划起点栅格节点总体估计代价;
步骤234:栅格节点列表更新;具体过程为:
如果开放栅格节点列表中已存在与相邻栅格节点位置相同的栅格节点cellold,则比较已有栅格节点cellold与相邻栅格节点的总体估计代价,如果相邻栅格节点总体估计代价小于等于已有栅格节点cellold总体估计代价,则将开放栅格节点列表中已有栅格节点cellold删除,将相邻栅格节点添加到开放栅格节点列表中;如果相邻栅格节点总体估计代价大于已有栅格节点cellold总体估计代价,则保留已有栅格节点cellold;
步骤235:终点栅格节点到达状态判断;
如果当前最优栅格节点cell*已到达终点栅格节点(与目标点欧氏距离小于一个栅格长度),则进行步骤236;如果未到达终点栅格节点,则返回步骤233,继续搜索;
步骤236:路径生成;具体过程为:
完成路径搜索后回溯路径搜索过程,创建路径栅格点列表,从终点栅格节点开始,记录当前栅格节点,并搜索当前栅格节点的父节点,循环迭代执行,直到搜索到起点栅格节点处,将路径栅格点列表倒序排列,可得路径栅格节点列表,将路径栅格节点列表中栅格坐标变换为地图坐标系坐标,得二维栅格地图中的运动路径。
其它步骤及参数与具体实施方式一至六之一相同。
在二维栅格地图中,将坐标值不是整数的栅格点称为广义栅格节点;
判断相邻栅格点状态,如果相邻栅格点在代价地图层的对应值为1,表示该相邻栅格点被障碍物占据,将该相邻栅格点判定为无效栅格点并删除;如果相邻栅格点在代价地图层的对应值为0,则估计相邻栅格点的总体估计代价值;
2)如果当前最优栅格节点位于地形地图层中标记的斜坡与楼梯区域上,则沿所标记的斜坡与楼梯区域梯度方向与梯度方向反方向扩展当前最优栅格节点,当前最优栅格节点的相邻栅格节点记为:
判断相邻栅格节点状态,由于斜坡楼梯区域梯度方向可能不平行于x轴或y轴坐标轴,相邻栅格节点x坐标与y坐标可能不是整数,无法在栅格地图中寻找对应点。改为将相邻栅格节点坐标取整后在栅格地图中查询对应值;
判断相邻栅格节点状态,如果相邻栅格节点在代价地图层的对应值为1,表示相邻栅格节点被障碍物占据,将该相邻栅格节点判定为无效栅格点并删除;如果相邻栅格节点在代价地图层的对应值为0,则估计相邻栅格节点的总体估计代价值;
所述估计相邻栅格节点的总体估计代价值的求解过程为:
相邻栅格节点耗散函数值为当前最优栅格节点耗散函数值与从当前最优栅格节点到当前相邻栅格节点的代价值,相邻栅格节点启发函数值为当前相邻栅格节点到终点栅格节点的欧氏距离,表示为:
其它步骤及参数与具体实施方式一至七之一相同。
具体实施方式九:本实施方式与具体实施方式一至八之一不同的是,所述步骤236中将路径栅格节点列表中栅格坐标变换为地图坐标系坐标,得二维栅格地图中的运动路径;具体过程为:
A*搜索完成后可得长度为Nj的路径栅格节点列表为:
cellpath(pointi1,pointi2)={(cxk,cyk)|k=1,2,…,Nj}
其中,cxk与cyk为路径点栅格坐标;
将栅格坐标变换为地图坐标系坐标
(xk,yk)=(cxkf+oxj,cykf+oyj)
其中,(xk,yk)为路径点坐标;
可得二维栅格地图中的运动路径:
path(pointi1,pointi2)={(xk,yk)|k=1,2,…,Nj}。
其它步骤及参数与具体实施方式一至八之一相同。
具体实施方式十:本实施方式与具体实施方式一至九之一不同的是,所述步骤26中地图网络中的规划路径由Npoint个路径节点和Npoint-1条二维栅格地图规划路径组成,记为:
其它步骤及参数与具体实施方式一至九之一相同。
本发明还可有其它多种实施例,在不背离本发明精神及其实质的情况下,本领域技术人员当可根据本发明作出各种相应的改变和变形,但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。
Claims (9)
1.一种应用于跨异构多层空间的轮式机器人路径规划方法,其特征在于:所述方法具体过程为:
步骤1、构建多层空间的地图网络;
步骤2、基于多层空间的地图网络对地图网络中的路径进行搜索;
所述步骤1中构建多层空间的地图网络;具体过程为:
步骤11、对多层空间在楼梯与平面连接处、楼梯转台位置进行分割,划分为若干区域,各区域在二维平面上的投影不出现重叠;
多层空间地图网络由空间中的路径节点与各区域的二维栅格地图连接组成;
多层空间地图网络由空间中的路径节点作为网络的节点,由二维栅格地图作为网络的边;
多层空间地图网络的路径节点集合记为Points={pointi|i=0,1,2,…,N-1};
其中N为地图网络中路径节点个数;pointi为第i个路径节点;
多层空间地图网络的二维栅格地图集合记为Maps={mapj|j=0,1,2,…,M-1};
其中M为地图网络中二维栅格地图个数;mapj为第j个二维栅格地图;
多层空间地图网络信息由路径节点集合和二维栅格地图集合组成,记为GraphNet=(Points,Maps);
多层空间地图网络分为静态地图层staticmapj、代价地图层costmapj和地形地图层terrainmapj;
步骤12:建立二维栅格地图列表、路径节点列表,初始状态二维栅格地图列表和路径节点列表为空列表;
步骤13:遥控机器人运动至地图网络已有路径节点处,以当前位置为二维栅格地图建图起点startj,建立地图坐标系Fj,采集激光雷达、IMU与里程计数据,进行二维栅格地图构建;
遥控机器人运动,进行二维栅格地图构建,使当前构建二维栅格地图对应区域在二维空间上不会出现重叠,将机器人当前位置作为二维栅格地图建图终点,即地图网络中的路径节点,记为destj,完成当前地图坐标系Fj下地图的二维栅格地图静态地图层部分构建;
步骤14:记录二维栅格地图建图终点destj在当前地图坐标系内的位置姿态,其中地图坐标系x坐标与y坐标由二维栅格地图构建过程得到,z坐标由高度计测得,姿态角度由IMU测得,并将二维栅格地图建图终点destj在当前地图坐标系内的位置姿态转换为齐次变换矩阵:
将二维栅格地图建图终点destj在当前地图坐标系内的位置姿态转换为在世界坐标系内的位置姿态:
步骤15:在已有的路径节点列表中搜索当前二维栅格地图建图终点,根据世界坐标系内位置姿态判断当前二维栅格地图建图终点是否已存在于路径节点列表中:如果不存在,则将该终点添加到路径节点列表中,将该地图添加到二维栅格地图列表中;如果已存在,则将地图添加到二维栅格地图列表中,并将该二维栅格地图的建图终点标记为路径节点列表中已有的终点节点;
步骤16:如果二维栅格地图网络已包含多层空间的全部可通行区域,则判定为多层空间地图网络静态地图层构建完成,执行步骤17;如果多层空间地图构建未完成,则在当前位置返回步骤13,开始新的二维栅格地图构建;
步骤17:对静态地图层进行膨胀处理,得到代价地图层costmapj;
步骤18:基于静态地图层构建地形地图层terrainmapj。
2.根据权利要求1所述一种应用于跨异构多层空间的轮式机器人路径规划方法,其特征在于:所述步骤17中对静态地图进行膨胀处理,得到代价地图层costmapj;具体过程为:
对地图网络静态地图层中包含的障碍物与地图边界进行膨胀处理,在地图坐标系内膨胀半径为rinflate,设地图栅格分辨率为f,则在二维栅格地图内的膨胀栅格数为:
在静态地图层中,被障碍物占据的栅格对应值为1,可通行区域栅格对应值为0,未知区域为-1;
对于静态地图层障碍物占据栅格,取以膨胀栅格数为半径的邻域:
其中,(cx,cy)表示邻域中心栅格点坐标,(cx1,cy1)表示邻域内栅格点坐标;
将代价地图在该邻域内的栅格的对应值置为1,表示该区域内的栅格点不可通行:
costmapj(cx1,cy1)=1 if(cx1,cy1)∈U(cx,cy),staticmapj(cx,cy)=1。
3.根据权利要求2所述一种应用于跨异构多层空间的轮式机器人路径规划方法,其特征在于:所述步骤18中基于静态地图层构建地形地图层terrainmapj;具体过程为:
在二维栅格地图上选择多边形区域作为斜坡与楼梯区域,记为Polygonterrain,并给定斜坡与楼梯区域梯度方向角度θgrad;
在地形地图层中,选取多边形区域内的栅格点,将栅格对应数值赋值为梯度方向角度:
terrainmapj(cx1,cy1)=θgrad if(cx1,cy1)∈Polygonterrain。
4.根据权利要求3所述一种应用于跨异构多层空间的轮式机器人路径规划方法,其特征在于:所述步骤2中基于多层空间的地图网络对地图网络中的路径进行搜索;具体过程为:
步骤21:确定临时地图网络;
步骤22:搜索初始化;具体过程为:
建立开放节点列表、关闭节点列表,初始开放节点列表、关闭节点列表为空列表;
计算机器人路径规划起点路径节点pointstart总体估计代价;
机器人路径规划起点路径节点设置为根节点,即父节点为空,将该节点写入开放节点列表;
所述计算机器人路径规划起点路径节点pointstart总体估计代价的具体过程为:
机器人路径规划起点路径节点耗散函数值g(pointstart)为0,启发函数值h(pointstart)为由路径规划起点到目标点的欧氏距离,从而得到路径规划起点路径节点pointstart总体估计代价f(pointstart)=g(pointstart)+h(pointstart);
步骤23:节点的扩展;具体过程为:
其中,为相邻节点耗散函数值,g(point*)为当前最优节点耗散函数值,Kh为设定的高度代价因数,Kh>0;,为相邻节点启发函数值,为在世界坐标系内的位置坐标,为路径规划终点在世界坐标系内的位置坐标,为相邻节点总体估计代价;
步骤24:节点列表更新;具体过程为:
1)如果开放节点列表中已存在与相邻节点位置相同的节点pointold,则比较开放节点列表已有节点pointold与相邻节点的总体估计代价:如果相邻节点总体估计代价不超过pointold的总体估计代价,则将开放节点列表中已有节点pointold删除,将相邻节点添加到开放节点列表中;如果相邻节点总体估计代价大于pointold的总体估计代价,则保留已有节点pointold;
步骤242:将当前最优节点point*从开放节点列表中删除,添加到关闭节点列表中;
步骤25:目标到达状态判断;具体过程为:
如果当前最优节点已到达目标节点,则执行步骤26;如果未到达目标节点,则返回步骤23;
步骤26:路径生成;具体过程为:
创建路径点列表,从目标节点开始,记录当前节点位置并写入路径点列表,寻找当前节点的父节点,循环迭代执行,直到搜索到机器人路径规划起点路径节点pointstart处,将路径点列表倒序排列,可得地图网络中的规划路径。
5.根据权利要求4所述一种应用于跨异构多层空间的轮式机器人路径规划方法,其特征在于:所述步骤21中确定临时地图网络;具体过程为:
确定机器人当前位置与目标位置及两点所在二维栅格地图,将当前位置与目标位置两点设置为机器人路径规划起点路径节点pointstart和机器人路径规划终点路径节点pointdest,添加到地图网络的路径节点列表中,作为临时路径节点列表;
建立路径节点pointstart到所在区域二维栅格地图mapstart建图起点与建图终点的二维栅格地图mapstart1、mapstart2;
建立路径节点pointdest到所在区域二维栅格地图mapdest建图起点与建图终点的二维栅格地图mapdest1、mapdest2;
将二维栅格地图mapstart、mapdest、mapstart1、mapstart2、mapdest1、mapdest2添加到地图网络的二维栅格地图列表中,作为临时二维栅格地图列表;
临时地图网络由临时路径节点列表与临时二维栅格地图列表组成,设置为无向网络。
步骤231:将二维栅格地图的起点与终点的路径节点位置姿态从世界坐标系变换到当前地图坐标系Fj中;具体过程为:
设二维栅格地图内路径规划的起点与终点分别为pointi1和pointi2,在世界坐标系内的位置姿态齐次变换矩阵别为和设二维栅格地图内地图坐标系在世界坐标系内的位置姿态齐次变换矩阵为则路径规划起点与终点在当前地图坐标系内的位置姿态齐次变换矩阵为:
提取起点与终点在当前地图坐标系Fj中的位置姿态齐次变换矩阵中的x坐标与y坐标部分记为xsj,ysj,xdj,ydj:
式中mij为齐次变换矩阵M的第i行第j列元素;
再将位置坐标映射到栅格坐标系内得到栅格坐标,得到起点与终点栅格点坐标分别为:
其中,cellstart为起点栅格节点坐标,(cxs,cys)为起点栅格节点x坐标与y坐标,(oxj,oyj)为二维栅格地图矩阵原点在地图坐标系中的坐标,f为二维栅格地图分辨率,celldest为终点栅格节点坐标,(cxd,cyd)为终点栅格节点x坐标与y坐标;
步骤232:搜索初始化:具体过程为:
建立开放栅格节点列表、关闭栅格节点列表,初始开放栅格节点列表、关闭栅格节点列表为空列表;
计算机器人路径规划起点栅格节点cellstart总体估计代价;
机器人路径规划起点栅格节点设置为根节点,即父节点为空,将该节点写入开放栅格节点列表;
所述计算机器人路径规划起点栅格节点cellstart总体估计代价的具体过程为:
机器人路径规划起点栅格节点耗散函数值为0,启发函数值为由路径规划起点栅格节点到终点栅格节点的欧氏距离,栅格节点耗散函数值与启发函数值相加得到机器人路径规划起点栅格节点总体估计代价;
步骤234:栅格节点列表更新;具体过程为:
如果开放栅格节点列表中已存在与相邻栅格节点位置相同的栅格节点cellold,则比较已有栅格节点cellold与相邻栅格节点的总体估计代价,如果相邻栅格节点总体估计代价小于等于已有栅格节点cellold总体估计代价,则将开放栅格节点列表中已有栅格节点cellold删除,将相邻栅格节点添加到开放栅格节点列表中;如果相邻栅格节点总体估计代价大于已有栅格节点cellold总体估计代价,则保留已有栅格节点cellold;
步骤235:终点栅格节点到达状态判断;
如果当前最优栅格节点cell*已到达终点栅格节点,则进行步骤236;如果未到达终点栅格节点,则返回步骤233;
步骤236:路径生成;具体过程为:
创建路径栅格点列表,从终点栅格节点开始,记录当前栅格节点,并搜索当前栅格节点的父节点,循环迭代执行,直到搜索到起点栅格节点处,将路径栅格点列表倒序排列,可得路径栅格节点列表,将路径栅格节点列表中栅格坐标变换为地图坐标系坐标,得二维栅格地图中的运动路径。
判断相邻栅格点状态,如果相邻栅格点在代价地图层的对应值为1,表示该相邻栅格点被障碍物占据,将该相邻栅格点判定为无效栅格点并删除;如果相邻栅格点在代价地图层的对应值为0,则估计相邻栅格点的总体估计代价值;
2)如果当前最优栅格节点位于地形地图层中标记的斜坡与楼梯区域上,则沿所标记的斜坡与楼梯区域梯度方向与梯度方向反方向扩展当前最优栅格节点,当前最优栅格节点的相邻栅格节点记为:
判断相邻栅格节点状态,如果相邻栅格节点在代价地图层的对应值为1,表示相邻栅格节点被障碍物占据,将该相邻栅格节点判定为无效栅格点并删除;如果相邻栅格节点在代价地图层的对应值为0,则估计相邻栅格节点的总体估计代价值;
所述估计相邻栅格节点的总体估计代价值的求解过程为:
8.根据权利要求7所述一种应用于跨异构多层空间的轮式机器人路径规划方法,其特征在于:所述步骤236中将路径栅格节点列表中栅格坐标变换为地图坐标系坐标,得二维栅格地图中的运动路径;具体过程为:
长度为Nj的路径栅格节点列表为:
cellpath(pointi1,pointi2)={(cxk,cyk)|k=1,2,…,Nj}
其中,cxk与cyk为路径点栅格坐标;
将栅格坐标变换为地图坐标系坐标
(xk,yk)=(cxkf+oxj,cykf+oyj)
其中,(xk,yk)为路径点坐标;
可得二维栅格地图中的运动路径:
path(pointi1,pointi2)={(xk,yk)|k=1,2,…,Nj}。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210203947.7A CN114577214B (zh) | 2022-03-02 | 2022-03-02 | 一种应用于跨异构多层空间的轮式机器人路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210203947.7A CN114577214B (zh) | 2022-03-02 | 2022-03-02 | 一种应用于跨异构多层空间的轮式机器人路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114577214A CN114577214A (zh) | 2022-06-03 |
CN114577214B true CN114577214B (zh) | 2022-09-20 |
Family
ID=81772370
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210203947.7A Active CN114577214B (zh) | 2022-03-02 | 2022-03-02 | 一种应用于跨异构多层空间的轮式机器人路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114577214B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114777793B (zh) * | 2022-06-16 | 2022-09-20 | 盈嘉互联(北京)科技有限公司 | 一种面向任意导航主体的bim地图提取及路径规划方法 |
CN115080676B (zh) * | 2022-06-16 | 2024-06-18 | 重庆邮电大学 | 移动机器人组合地图动态管理方法 |
CN117485929B (zh) * | 2023-12-29 | 2024-03-19 | 中国电力工程顾问集团西南电力设计院有限公司 | 一种基于智能控制的无人化堆取料控制***及方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2017173990A1 (zh) * | 2016-04-07 | 2017-10-12 | 北京进化者机器人科技有限公司 | 一种机器人避障中的最短路径规划方法 |
CN107368076A (zh) * | 2017-07-31 | 2017-11-21 | 中南大学 | 一种智能环境下机器人运动路径深度学习控制规划方法 |
CN107677279A (zh) * | 2017-09-26 | 2018-02-09 | 上海思岚科技有限公司 | 一种定位建图的方法及*** |
CN112197778A (zh) * | 2020-09-08 | 2021-01-08 | 南京理工大学 | 基于改进a*算法的轮式机场巡界机器人路径规划方法 |
CN113033907A (zh) * | 2021-04-07 | 2021-06-25 | 上海钛米机器人股份有限公司 | 路径规划方法及装置、电子设备、存储介质 |
CN113110457A (zh) * | 2021-04-19 | 2021-07-13 | 杭州视熵科技有限公司 | 在室内复杂动态环境中智能机器人的自主覆盖巡检方法 |
CN113269837A (zh) * | 2021-04-27 | 2021-08-17 | 西安交通大学 | 一种适用于复杂三维环境的定位导航方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR102275300B1 (ko) * | 2019-07-05 | 2021-07-08 | 엘지전자 주식회사 | 이동 로봇 및 그 제어방법 |
-
2022
- 2022-03-02 CN CN202210203947.7A patent/CN114577214B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2017173990A1 (zh) * | 2016-04-07 | 2017-10-12 | 北京进化者机器人科技有限公司 | 一种机器人避障中的最短路径规划方法 |
CN107368076A (zh) * | 2017-07-31 | 2017-11-21 | 中南大学 | 一种智能环境下机器人运动路径深度学习控制规划方法 |
CN107677279A (zh) * | 2017-09-26 | 2018-02-09 | 上海思岚科技有限公司 | 一种定位建图的方法及*** |
CN112197778A (zh) * | 2020-09-08 | 2021-01-08 | 南京理工大学 | 基于改进a*算法的轮式机场巡界机器人路径规划方法 |
CN113033907A (zh) * | 2021-04-07 | 2021-06-25 | 上海钛米机器人股份有限公司 | 路径规划方法及装置、电子设备、存储介质 |
CN113110457A (zh) * | 2021-04-19 | 2021-07-13 | 杭州视熵科技有限公司 | 在室内复杂动态环境中智能机器人的自主覆盖巡检方法 |
CN113269837A (zh) * | 2021-04-27 | 2021-08-17 | 西安交通大学 | 一种适用于复杂三维环境的定位导航方法 |
Also Published As
Publication number | Publication date |
---|---|
CN114577214A (zh) | 2022-06-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114577214B (zh) | 一种应用于跨异构多层空间的轮式机器人路径规划方法 | |
Cao et al. | TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments. | |
Abd Algfoor et al. | A comprehensive study on pathfinding techniques for robotics and video games | |
CN111402339B (zh) | 一种实时定位方法、装置、***及存储介质 | |
CN110703747A (zh) | 一种基于简化广义Voronoi图的机器人自主探索方法 | |
CN108334080B (zh) | 一种针对机器人导航的虚拟墙自动生成方法 | |
CN114510057A (zh) | 一种室内环境中基于ros的移动机器人自主导航方法 | |
CN110146085B (zh) | 基于建图和快速探索随机树的无人机实时规避重规划方法 | |
CN113776534B (zh) | 一种基于立体剖分网格的无人机三维时变空域导航方法 | |
CN110471426A (zh) | 基于量子狼群算法的无人驾驶智能车自动避碰方法 | |
Song et al. | Online coverage and inspection planning for 3D modeling | |
CN112432648A (zh) | 一种移动型机器人安全运动轨迹的实时规划方法 | |
CN113341991A (zh) | 一种基于动态窗口和冗余节点过滤的路径优化方法 | |
CN112835064B (zh) | 一种建图定位方法、***、终端及介质 | |
CN110763247A (zh) | 基于可视图和贪心算法结合的机器人路径规划方法 | |
CN114777793B (zh) | 一种面向任意导航主体的bim地图提取及路径规划方法 | |
CN114967744A (zh) | 一种多无人机协同避障的规划方法 | |
Wu et al. | A non-rigid hierarchical discrete grid structure and its application to UAVs conflict detection and path planning | |
Jarvis | Distance transform based path planning for robot navigation | |
Si et al. | TOM-Odometry: A generalized localization framework based on topological map and odometry | |
Jan et al. | A computationally efficient complete area coverage algorithm for intelligent mobile robot navigation | |
CN116338729A (zh) | 一种基于多层地图的三维激光雷达导航方法 | |
CN116610109A (zh) | 基于梯度的前向蚁群算法无人车路径规划方法 | |
CN114882339B (zh) | 基于实时稠密点云地图的煤矿巷道孔眼自主识别方法 | |
Zhai et al. | Semantic enrichment of BIM with IndoorGML for quadruped robot navigation and automated 3D scanning |
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 |