CN114577214B - 一种应用于跨异构多层空间的轮式机器人路径规划方法 - Google Patents

一种应用于跨异构多层空间的轮式机器人路径规划方法 Download PDF

Info

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
Application number
CN202210203947.7A
Other languages
English (en)
Other versions
CN114577214A (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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of 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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN202210203947.7A priority Critical patent/CN114577214B/zh
Publication of CN114577214A publication Critical patent/CN114577214A/zh
Application granted granted Critical
Publication of CN114577214B publication Critical patent/CN114577214B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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；
二维栅格地图的建图起点与建图终点位置姿态在世界坐标系F0内转换为齐次变换矩阵，表示为
Figure BDA0003528396510000041
二维栅格地图的建图终点在地图坐标系Fj中的位置姿态转换为齐次变换矩阵，表示为
Figure BDA0003528396510000042
多层空间地图分为静态地图层staticmapj、代价地图层costmapj与地形地图层terrainmapj；
其中静态地图层将地图区域划分为可通行区域、障碍占据区域和未知区域；
代价地图层由静态地图对障碍占据区域进行膨胀处理得到，用于路径规划中的障碍物躲避；
地形地图层包含地图区域的斜坡与楼梯区域的位置与梯度方向信息，用于规划机器人在特殊区域的路径规划，以满足机器人的运动约束；
二维栅格地图以nj×mj矩阵形式存储，行方向为二维栅格地图坐标系y方向，列方向为二维栅格地图坐标系x方向，并记录二维栅格地图矩阵原点在地图坐标系中的x坐标oxj与y坐标oyj，并记录二维栅格地图坐标系相对于世界坐标系的位置姿态；
二维栅格地图表示为：
Figure BDA0003528396510000051
其中，staticmapj为静态地图层，costmapj为代价地图层，terrainmapj为地形地图层；staticmapj,costmapj,
Figure BDA0003528396510000052
Figure BDA0003528396510000053
表示地图坐标系Fj在世界坐标系F0中的位置姿态(以齐次变换矩阵表示)，startj、destj分别为二维栅格地图的建图起点与建图终点；
地图构建过程中，机器人利用所携带的激光雷达传感器、惯性测量单元传感器(IMU)、里程计、高度计等传感器建立多层空间的地图网络，机器人的路径规划通过地图网络实现；
步骤12：建立二维栅格地图列表、路径节点列表，初始状态二维栅格地图列表和路径节点列表为空列表；
以第一张二维栅格地图建图起点为原点，建立地图坐标系F0(第一个地图坐标系同时作为世界坐标系)，该地图坐标系同时作为世界坐标系，将地图坐标系中当前位置point0(坐标原点)添加到路径节点列表中，并记录该节点(当前位置point0)的位置姿态，位置姿态由齐次变换矩阵表示为
Figure BDA0003528396510000054
其中，
Figure BDA0003528396510000055
表示地图坐标系中当前位置point0的位置姿态；I3与I4分别表示3阶与4阶单位矩阵；O3×1＝(0 0 0)T，O1×3＝(0 0 0)；
步骤13：操作人员遥控机器人运动至地图网络已有路径节点处，以当前位置为二维栅格地图建图起点startj，建立地图坐标系Fj，采集激光雷达、IMU与里程计数据，进行二维栅格地图构建(构建方法为SLAM)；
遥控机器人运动至环境中合适位置，进行二维栅格地图构建(构建方法为SLAM)，使当前构建二维栅格地图对应区域在二维空间上不会出现重叠，将机器人当前位置作为二维栅格地图建图终点，即地图网络中的路径节点，记为destj，完成当前地图坐标系Fj下地图的二维栅格地图静态地图层部分构建；
步骤14：记录二维栅格地图建图终点destj在当前地图坐标系内的位置姿态，其中地图坐标系x坐标与y坐标由二维栅格地图构建过程得到，z坐标由高度计测得，姿态角度由IMU测得，并将二维栅格地图建图终点destj在当前地图坐标系内的位置姿态转换为齐次变换矩阵：
Figure BDA0003528396510000061
其中，
Figure BDA0003528396510000062
表示二维栅格地图建图终点destj在当前地图坐标系内的姿态旋转矩阵，(xj,yj,zj)表示二维栅格地图建图终点destj在当前地图坐标系内的位置坐标向量，T表示求转置；
将二维栅格地图建图终点destj在当前地图坐标系内的位置姿态转换为在世界坐标系内的位置姿态：
Figure BDA0003528396510000063
其中，
Figure BDA0003528396510000064
表示二维栅格地图建图起点位置姿态在世界坐标系F0内转换为齐次变换矩阵；
Figure BDA0003528396510000065
表示二维栅格地图建图终点位置姿态在世界坐标系F0内转换为齐次变换矩阵；
步骤15：在已有的路径节点列表中搜索当前二维栅格地图建图终点，根据世界坐标系内位置姿态判断当前二维栅格地图建图终点是否已存在于路径节点列表中：如果不存在，则将该终点添加到路径节点列表中，将该地图添加到二维栅格地图列表中；如果已存在，则将地图添加到二维栅格地图列表中，并将该二维栅格地图的建图终点标记为路径节点列表中已有的终点节点；
步骤16：如果二维栅格地图网络已包含多层空间的全部可通行区域，则判定为多层空间地图网络静态地图层构建完成，执行步骤17；如果多层空间地图构建未完成，则在当前位置返回步骤13，开始新的二维栅格地图构建；
步骤17：对静态地图层进行膨胀处理，得到代价地图层costmapj；
步骤18：基于静态地图层构建地形地图层terrainmapj。
其它步骤及参数与具体实施方式一相同。
具体实施方式三：本实施方式与具体实施方式一或二不同的是，所述步骤17中对静态地图进行膨胀处理，得到代价地图层costmapj(以costmapj(cx1,cy1)表示costmapj矩阵第cx1行第cy1列元素)；具体过程为：
对地图网络静态地图层中包含的障碍物与地图边界进行膨胀处理，在地图坐标系内膨胀半径为rinflate，设地图栅格分辨率为f，则在二维栅格地图内的膨胀栅格数为：
Figure BDA0003528396510000071
在静态地图层中，被障碍物占据的栅格对应值为1，可通行区域栅格对应值为0，未知区域为-1；
对于静态地图层障碍物占据栅格，取以膨胀栅格数为半径的邻域：
Figure BDA0003528396510000072
其中，(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：节点的扩展；具体过程为：
从开放节点列表中选择总体估计代价最小的节点作为当前最优节点，记为point*，寻找地图网络中与point*相邻的节点，记为
Figure BDA0003528396510000081
其中n为与point*相邻的节点个数；
在二维栅格地图(二维栅格地图为连接当前最优节点point*到相邻节点的二维栅格地图)中搜索从当前最优节点point*到相邻节点
Figure BDA0003528396510000082
的路径，路径记为
Figure BDA0003528396510000083
路径长度记为
Figure BDA0003528396510000084
两节点之间高度差记为
Figure BDA0003528396510000085
估计相邻节点
Figure BDA0003528396510000086
总体估计代价，其中相邻节点
Figure BDA0003528396510000087
耗散函数值为当前最优节点耗散函数值与从当前最优节点到当前相邻节点(当前相邻节点
Figure BDA0003528396510000088
)的代价值，相邻节点
Figure BDA0003528396510000089
启发函数值为相邻节点
Figure BDA00035283965100000810
到目标点的欧氏距离，表示为：
Figure BDA00035283965100000811
Figure BDA00035283965100000812
Figure BDA00035283965100000813
其中，
Figure BDA00035283965100000814
为相邻节点
Figure BDA00035283965100000815
耗散函数值，g(point*)为当前最优节点耗散函数值，Kh为设定的高度代价因数，Kh>0；，
Figure BDA0003528396510000091
为相邻节点
Figure BDA0003528396510000092
启发函数值，
Figure BDA0003528396510000093
为
Figure BDA0003528396510000094
在世界坐标系内的位置坐标(包含x、y、z三个坐标分量)
Figure BDA0003528396510000095
Figure BDA0003528396510000096
为路径规划终点在世界坐标系内的位置坐标(包含x、y、z三个坐标分量)
Figure BDA0003528396510000097
Figure BDA0003528396510000098
为相邻节点
Figure BDA0003528396510000099
总体估计代价；
步骤24：节点列表更新；具体过程为：
步骤241：判断相邻节点
Figure BDA00035283965100000910
所在位置是否已在开放节点列表中；
1)如果开放节点列表中已存在与相邻节点
Figure BDA00035283965100000911
位置相同的节点pointold，则比较开放节点列表已有节点pointold与相邻节点
Figure BDA00035283965100000912
的总体估计代价：如果相邻节点
Figure BDA00035283965100000913
总体估计代价不超过(小于等于)pointold的总体估计代价，则将开放节点列表中已有节点pointold删除，将相邻节点
Figure BDA00035283965100000914
添加到开放节点列表中；如果相邻节点
Figure BDA00035283965100000915
总体估计代价大于pointold的总体估计代价，则保留已有节点pointold；
2)如果开放节点列表中不存在与相邻节点
Figure BDA00035283965100000916
位置相同的节点，则将相邻节点
Figure BDA00035283965100000917
添加到开放节点列表中；
步骤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*到相邻节点
Figure BDA0003528396510000111
的路径；具体过程为：
步骤231：将二维栅格地图的起点与终点的路径节点位置姿态从世界坐标系变换到当前地图坐标系Fj中；具体过程为：
设二维栅格地图内路径规划的起点与终点分别为pointi1和pointi2，在世界坐标系内的位置姿态齐次变换矩阵别为
Figure BDA0003528396510000112
和
Figure BDA0003528396510000113
设二维栅格地图内地图坐标系在世界坐标系内的位置姿态齐次变换矩阵为
Figure BDA0003528396510000114
则路径规划起点与终点在当前地图坐标系内的位置姿态齐次变换矩阵为：
Figure BDA0003528396510000115
Figure BDA0003528396510000116
提取起点与终点在当前地图坐标系Fj中的位置姿态齐次变换矩阵中的x坐标与y坐标部分记为xsj，ysj，xdj，ydj：
Figure BDA0003528396510000117
式中mij为齐次变换矩阵M的第i行第j列元素；
再将位置坐标映射到栅格坐标系内得到栅格坐标，得到起点与终点栅格点坐标分别为：
Figure BDA0003528396510000118
Figure BDA0003528396510000119
其中，cellstart为起点栅格节点坐标，(cxs,cys)为起点栅格节点x坐标与y坐标，(oxj,oyj)为二维栅格地图矩阵原点在地图坐标系中的坐标，f为二维栅格地图分辨率，celldest为终点栅格节点坐标，(cxd,cyd)为终点栅格节点x坐标与y坐标；
步骤232：在二维栅格地图坐标系内采用A*方法以栅格为基本单元进行路径搜索，搜索初始化：具体过程为：
建立开放栅格节点列表、关闭栅格节点列表，初始开放栅格节点列表、关闭栅格节点列表为空列表；
计算机器人路径规划起点栅格节点cellstart总体估计代价；
机器人路径规划起点栅格节点设置为根节点，即父节点为空，将该节点写入开放栅格节点列表；
所述计算机器人路径规划起点栅格节点cellstart总体估计代价的具体过程为：
机器人路径规划起点栅格节点耗散函数值为0，启发函数值为由路径规划起点栅格节点到终点栅格节点的欧氏距离，栅格节点耗散函数值与启发函数值相加得到机器人路径规划起点栅格节点总体估计代价；
步骤233：栅格节点的扩展，由开放栅格节点列表中当前最优栅格节点cell*得到相邻节点
Figure BDA0003528396510000121
步骤234：栅格节点列表更新；具体过程为：
判断开放栅格节点列表中是否存在与相邻节点
Figure BDA0003528396510000122
位置相同的栅格节点cellold；
如果开放栅格节点列表中不存在与相邻节点
Figure BDA0003528396510000123
位置相同的栅格节点，则将相邻栅格节点
Figure BDA0003528396510000124
添加到开放栅格节点列表中；
如果开放栅格节点列表中已存在与相邻栅格节点
Figure BDA0003528396510000125
位置相同的栅格节点cellold，则比较已有栅格节点cellold与相邻栅格节点
Figure BDA0003528396510000126
的总体估计代价，如果相邻栅格节点
Figure BDA0003528396510000127
总体估计代价小于等于已有栅格节点cellold总体估计代价，则将开放栅格节点列表中已有栅格节点cellold删除，将相邻栅格节点
Figure BDA0003528396510000128
添加到开放栅格节点列表中；如果相邻栅格节点
Figure BDA0003528396510000129
总体估计代价大于已有栅格节点cellold总体估计代价，则保留已有栅格节点cellold；
步骤235：终点栅格节点到达状态判断；
如果当前最优栅格节点cell*已到达终点栅格节点(与目标点欧氏距离小于一个栅格长度)，则进行步骤236；如果未到达终点栅格节点，则返回步骤233，继续搜索；
步骤236：路径生成；具体过程为：
完成路径搜索后回溯路径搜索过程，创建路径栅格点列表，从终点栅格节点开始，记录当前栅格节点，并搜索当前栅格节点的父节点，循环迭代执行，直到搜索到起点栅格节点处，将路径栅格点列表倒序排列，可得路径栅格节点列表，将路径栅格节点列表中栅格坐标变换为地图坐标系坐标，得二维栅格地图中的运动路径。
其它步骤及参数与具体实施方式一至六之一相同。
具体实施方式八：本实施方式与具体实施方式一至七之一不同的是，所述步骤233中栅格节点的扩展，由开放栅格节点列表中当前最优栅格节点cell*得到相邻节点
Figure BDA0003528396510000131
具体过程为：
在二维栅格地图中，将坐标值不是整数的栅格点称为广义栅格节点；
从开放栅格节点列表中选择总体估计代价最小的栅格节点作为当前最优栅格节点，记为
Figure BDA0003528396510000132
判断栅格状态在各地图层的状态：
Figure BDA0003528396510000133
为当前最优栅格节点坐标点；
1)如果当前最优栅格节点不在地形地图层中标记的斜坡与楼梯区域上，则选择与当前最优栅格节点相邻的8个栅格点(如果该栅格节点为广义栅格节点，则对坐标按四舍五入取整选取临近栅格节点)，记为
Figure BDA0003528396510000134
栅格坐标记为：
Figure BDA0003528396510000135
其中，
Figure BDA0003528396510000136
为与当前最优栅格节点相邻的栅格点的坐标，(Δxk,Δyk)为当前最优栅格节点坐标移动距离；
判断相邻栅格点
Figure BDA0003528396510000137
状态，如果相邻栅格点
Figure BDA0003528396510000138
在代价地图层的对应值为1，表示该相邻栅格点
Figure BDA0003528396510000139
被障碍物占据，将该相邻栅格点
Figure BDA00035283965100001310
判定为无效栅格点并删除；如果相邻栅格点
Figure BDA00035283965100001311
在代价地图层的对应值为0，则估计相邻栅格点
Figure BDA00035283965100001312
的总体估计代价值；
所述相邻栅格点
Figure BDA00035283965100001313
的总体估计代价值的求解过程为：
相邻栅格点
Figure BDA00035283965100001314
耗散函数值为父节点耗散函数值与从父节点到当前相邻栅格点
Figure BDA00035283965100001315
的代价值，相邻栅格点
Figure BDA0003528396510000141
启发函数值为当前相邻栅格点
Figure BDA0003528396510000142
到终点栅格节点的欧氏距离，表示为：
Figure BDA0003528396510000143
Figure BDA0003528396510000144
Figure BDA0003528396510000145
其中，g(cell*)为当前最优栅格节点耗散函数值，
Figure BDA0003528396510000146
为相邻栅格点
Figure BDA0003528396510000147
耗散函数值，
Figure BDA0003528396510000148
为相邻栅格点
Figure BDA0003528396510000149
启发函数值，
Figure BDA00035283965100001410
为相邻栅格点
Figure BDA00035283965100001411
的总体估计代价值；
2)如果当前最优栅格节点位于地形地图层中标记的斜坡与楼梯区域上，则沿所标记的斜坡与楼梯区域梯度方向与梯度方向反方向扩展当前最优栅格节点，当前最优栅格节点的相邻栅格节点记为：
Figure BDA00035283965100001412
Figure BDA00035283965100001413
其中，
Figure BDA00035283965100001414
为当前最优栅格节点的相邻栅格节点的坐标，f为二维栅格地图分辨率；
判断相邻栅格节点状态，由于斜坡楼梯区域梯度方向可能不平行于x轴或y轴坐标轴，相邻栅格节点x坐标与y坐标可能不是整数，无法在栅格地图中寻找对应点。改为将相邻栅格节点坐标取整后在栅格地图中查询对应值；
判断相邻栅格节点状态，如果相邻栅格节点在代价地图层的对应值为1，表示相邻栅格节点被障碍物占据，将该相邻栅格节点判定为无效栅格点并删除；如果相邻栅格节点在代价地图层的对应值为0，则估计相邻栅格节点的总体估计代价值；
所述估计相邻栅格节点的总体估计代价值的求解过程为：
相邻栅格节点耗散函数值为当前最优栅格节点耗散函数值与从当前最优栅格节点到当前相邻栅格节点的代价值，相邻栅格节点启发函数值为当前相邻栅格节点到终点栅格节点的欧氏距离，表示为：
Figure BDA0003528396510000151
Figure BDA0003528396510000152
Figure BDA0003528396510000153
其中，
Figure BDA0003528396510000154
为相邻栅格节点，
Figure BDA0003528396510000155
为
Figure BDA0003528396510000156
和
Figure BDA0003528396510000157
Figure BDA0003528396510000158
为相邻栅格节点耗散函数值，
Figure BDA0003528396510000159
为相邻栅格节点的坐标，
Figure BDA00035283965100001510
为
Figure BDA00035283965100001511
和
Figure BDA00035283965100001512
Figure BDA00035283965100001513
为相邻栅格节点启发函数值，
Figure BDA00035283965100001514
为相邻栅格节点的总体估计代价值。
其它步骤及参数与具体实施方式一至七之一相同。
具体实施方式九：本实施方式与具体实施方式一至八之一不同的是，所述步骤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条二维栅格地图规划路径组成，记为：
Figure BDA00035283965100001515
Figure BDA0003528396510000161
其中，
Figure BDA0003528396510000162
为第
Figure BDA0003528396510000163
个和第
Figure BDA0003528396510000164
个路径节点，属于临时地图网络中的临时路径节点集合Pointstemp，
Figure BDA0003528396510000165
为在二维栅格地图内从
Figure BDA0003528396510000166
到
Figure BDA0003528396510000167
的规划路径，(xs,ys)为二维栅格地图内规划路径点的栅格坐标，
Figure BDA0003528396510000168
为规划路径上栅格点个数。
其它步骤及参数与具体实施方式一至九之一相同。
本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

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：建立二维栅格地图列表、路径节点列表，初始状态二维栅格地图列表和路径节点列表为空列表；
以第一张二维栅格地图建图起点为原点，建立地图坐标系F0，该地图坐标系同时作为世界坐标系，将地图坐标系中当前位置point0添加到路径节点列表中，并记录该节点的位置姿态，位置姿态由齐次变换矩阵表示为
Figure FDA0003793098820000011
其中，
Figure FDA0003793098820000012
表示地图坐标系中当前位置point0的位置姿态；I3与I4分别表示3阶与4阶单位矩阵；O3×1＝(0 0 0)T，O1×3＝(0 0 0)；
步骤13：遥控机器人运动至地图网络已有路径节点处，以当前位置为二维栅格地图建图起点startj，建立地图坐标系Fj，采集激光雷达、IMU与里程计数据，进行二维栅格地图构建；
遥控机器人运动，进行二维栅格地图构建，使当前构建二维栅格地图对应区域在二维空间上不会出现重叠，将机器人当前位置作为二维栅格地图建图终点，即地图网络中的路径节点，记为destj，完成当前地图坐标系Fj下地图的二维栅格地图静态地图层部分构建；
步骤14：记录二维栅格地图建图终点destj在当前地图坐标系内的位置姿态，其中地图坐标系x坐标与y坐标由二维栅格地图构建过程得到，z坐标由高度计测得，姿态角度由IMU测得，并将二维栅格地图建图终点destj在当前地图坐标系内的位置姿态转换为齐次变换矩阵：
Figure FDA0003793098820000021
其中，
Figure FDA0003793098820000022
表示二维栅格地图建图终点destj在当前地图坐标系内的姿态旋转矩阵，(xj,yj,zj)表示二维栅格地图建图终点destj在当前地图坐标系内的位置坐标向量，T表示求转置；
将二维栅格地图建图终点destj在当前地图坐标系内的位置姿态转换为在世界坐标系内的位置姿态：
Figure FDA0003793098820000023
其中，
Figure FDA0003793098820000024
表示二维栅格地图建图起点位置姿态在世界坐标系F0内转换为齐次变换矩阵；
Figure FDA0003793098820000025
表示二维栅格地图建图终点位置姿态在世界坐标系F0内转换为齐次变换矩阵；
步骤15：在已有的路径节点列表中搜索当前二维栅格地图建图终点，根据世界坐标系内位置姿态判断当前二维栅格地图建图终点是否已存在于路径节点列表中：如果不存在，则将该终点添加到路径节点列表中，将该地图添加到二维栅格地图列表中；如果已存在，则将地图添加到二维栅格地图列表中，并将该二维栅格地图的建图终点标记为路径节点列表中已有的终点节点；
步骤16：如果二维栅格地图网络已包含多层空间的全部可通行区域，则判定为多层空间地图网络静态地图层构建完成，执行步骤17；如果多层空间地图构建未完成，则在当前位置返回步骤13，开始新的二维栅格地图构建；
步骤17：对静态地图层进行膨胀处理，得到代价地图层costmapj；
步骤18：基于静态地图层构建地形地图层terrainmapj。
2.根据权利要求1所述一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述步骤17中对静态地图进行膨胀处理，得到代价地图层costmapj；具体过程为：
对地图网络静态地图层中包含的障碍物与地图边界进行膨胀处理，在地图坐标系内膨胀半径为rinflate，设地图栅格分辨率为f，则在二维栅格地图内的膨胀栅格数为：
Figure FDA0003793098820000031
在静态地图层中，被障碍物占据的栅格对应值为1，可通行区域栅格对应值为0，未知区域为-1；
对于静态地图层障碍物占据栅格，取以膨胀栅格数为半径的邻域：
Figure FDA0003793098820000032
其中，(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：节点的扩展；具体过程为：
从开放节点列表中选择总体估计代价最小的节点作为当前最优节点，记为point*，寻找地图网络中与point*相邻的节点，记为
Figure FDA0003793098820000041
其中n为与point*相邻的节点个数；
搜索从当前最优节点point*到相邻节点
Figure FDA0003793098820000042
的路径，路径记为
Figure FDA0003793098820000043
路径长度记为
Figure FDA0003793098820000044
两节点之间高度差记为
Figure FDA0003793098820000045
估计相邻节点
Figure FDA0003793098820000046
总体估计代价，表示为：
Figure FDA0003793098820000047
Figure FDA0003793098820000048
Figure FDA0003793098820000051
其中，
Figure FDA0003793098820000052
为相邻节点
Figure FDA0003793098820000053
耗散函数值，g(point*)为当前最优节点耗散函数值，Kh为设定的高度代价因数，Kh>0；，
Figure FDA0003793098820000054
为相邻节点
Figure FDA0003793098820000055
启发函数值，
Figure FDA0003793098820000056
为
Figure FDA0003793098820000057
在世界坐标系内的位置坐标，
Figure FDA0003793098820000058
为路径规划终点在世界坐标系内的位置坐标，
Figure FDA0003793098820000059
为相邻节点
Figure FDA00037930988200000510
总体估计代价；
步骤24：节点列表更新；具体过程为：
步骤241：判断相邻节点
Figure FDA00037930988200000511
所在位置是否已在开放节点列表中；
1)如果开放节点列表中已存在与相邻节点
Figure FDA00037930988200000512
位置相同的节点pointold，则比较开放节点列表已有节点pointold与相邻节点
Figure FDA00037930988200000513
的总体估计代价：如果相邻节点
Figure FDA00037930988200000514
总体估计代价不超过pointold的总体估计代价，则将开放节点列表中已有节点pointold删除，将相邻节点
Figure FDA00037930988200000515
添加到开放节点列表中；如果相邻节点
Figure FDA00037930988200000516
总体估计代价大于pointold的总体估计代价，则保留已有节点pointold；
2)如果开放节点列表中不存在与相邻节点
Figure FDA00037930988200000517
位置相同的节点，则将相邻节点
Figure FDA00037930988200000518
添加到开放节点列表中；
步骤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添加到地图网络的二维栅格地图列表中，作为临时二维栅格地图列表；
临时地图网络由临时路径节点列表与临时二维栅格地图列表组成，设置为无向网络。
6.根据权利要求5所述一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述步骤23中搜索从当前最优节点point*到相邻节点
Figure FDA0003793098820000061
的路径；具体过程为：
步骤231：将二维栅格地图的起点与终点的路径节点位置姿态从世界坐标系变换到当前地图坐标系Fj中；具体过程为：
设二维栅格地图内路径规划的起点与终点分别为pointi1和pointi2，在世界坐标系内的位置姿态齐次变换矩阵别为
Figure FDA0003793098820000062
和
Figure FDA0003793098820000063
设二维栅格地图内地图坐标系在世界坐标系内的位置姿态齐次变换矩阵为
Figure FDA0003793098820000064
则路径规划起点与终点在当前地图坐标系内的位置姿态齐次变换矩阵为：
Figure FDA0003793098820000065
Figure FDA0003793098820000066
提取起点与终点在当前地图坐标系Fj中的位置姿态齐次变换矩阵中的x坐标与y坐标部分记为xsj，ysj，xdj，ydj：
Figure FDA0003793098820000067
式中mij为齐次变换矩阵M的第i行第j列元素；
再将位置坐标映射到栅格坐标系内得到栅格坐标，得到起点与终点栅格点坐标分别为：
Figure FDA0003793098820000071
Figure FDA0003793098820000072
其中，cellstart为起点栅格节点坐标，(cxs,cys)为起点栅格节点x坐标与y坐标，(oxj,oyj)为二维栅格地图矩阵原点在地图坐标系中的坐标，f为二维栅格地图分辨率，celldest为终点栅格节点坐标，(cxd,cyd)为终点栅格节点x坐标与y坐标；
步骤232：搜索初始化：具体过程为：
建立开放栅格节点列表、关闭栅格节点列表，初始开放栅格节点列表、关闭栅格节点列表为空列表；
计算机器人路径规划起点栅格节点cellstart总体估计代价；
机器人路径规划起点栅格节点设置为根节点，即父节点为空，将该节点写入开放栅格节点列表；
所述计算机器人路径规划起点栅格节点cellstart总体估计代价的具体过程为：
机器人路径规划起点栅格节点耗散函数值为0，启发函数值为由路径规划起点栅格节点到终点栅格节点的欧氏距离，栅格节点耗散函数值与启发函数值相加得到机器人路径规划起点栅格节点总体估计代价；
步骤233：栅格节点的扩展，由开放栅格节点列表中当前最优栅格节点cell*得到相邻节点
Figure FDA0003793098820000073
步骤234：栅格节点列表更新；具体过程为：
判断开放栅格节点列表中是否存在与相邻节点
Figure FDA0003793098820000074
位置相同的栅格节点cellold；
如果开放栅格节点列表中不存在与相邻节点
Figure FDA0003793098820000075
位置相同的栅格节点，则将相邻栅格节点
Figure FDA0003793098820000081
添加到开放栅格节点列表中；
如果开放栅格节点列表中已存在与相邻栅格节点
Figure FDA0003793098820000082
位置相同的栅格节点cellold，则比较已有栅格节点cellold与相邻栅格节点
Figure FDA0003793098820000083
的总体估计代价，如果相邻栅格节点
Figure FDA0003793098820000084
总体估计代价小于等于已有栅格节点cellold总体估计代价，则将开放栅格节点列表中已有栅格节点cellold删除，将相邻栅格节点
Figure FDA0003793098820000085
添加到开放栅格节点列表中；如果相邻栅格节点
Figure FDA0003793098820000086
总体估计代价大于已有栅格节点cellold总体估计代价，则保留已有栅格节点cellold；
步骤235：终点栅格节点到达状态判断；
如果当前最优栅格节点cell*已到达终点栅格节点，则进行步骤236；如果未到达终点栅格节点，则返回步骤233；
步骤236：路径生成；具体过程为：
创建路径栅格点列表，从终点栅格节点开始，记录当前栅格节点，并搜索当前栅格节点的父节点，循环迭代执行，直到搜索到起点栅格节点处，将路径栅格点列表倒序排列，可得路径栅格节点列表，将路径栅格节点列表中栅格坐标变换为地图坐标系坐标，得二维栅格地图中的运动路径。
7.根据权利要求6所述一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述步骤233中栅格节点的扩展，由开放栅格节点列表中当前最优栅格节点cell*得到相邻节点
Figure FDA0003793098820000087
具体过程为：
从开放栅格节点列表中选择总体估计代价最小的栅格节点作为当前最优栅格节点，记为
Figure FDA0003793098820000088
Figure FDA0003793098820000089
为当前最优栅格节点坐标点；
1)如果当前最优栅格节点不在地形地图层中标记的斜坡与楼梯区域上，则选择与当前最优栅格节点相邻的8个栅格点，记为
Figure FDA00037930988200000810
栅格坐标记为：
Figure FDA00037930988200000811
其中，
Figure FDA00037930988200000812
为与当前最优栅格节点相邻的栅格点的坐标，(Δxk,Δyk)为当前最优栅格节点坐标移动距离；
判断相邻栅格点
Figure FDA0003793098820000091
状态，如果相邻栅格点
Figure FDA0003793098820000092
在代价地图层的对应值为1，表示该相邻栅格点
Figure FDA0003793098820000093
被障碍物占据，将该相邻栅格点
Figure FDA0003793098820000094
判定为无效栅格点并删除；如果相邻栅格点
Figure FDA0003793098820000095
在代价地图层的对应值为0，则估计相邻栅格点
Figure FDA0003793098820000096
的总体估计代价值；
所述相邻栅格点
Figure FDA0003793098820000097
的总体估计代价值的求解过程为：
Figure FDA0003793098820000098
Figure FDA0003793098820000099
Figure FDA00037930988200000910
其中，g(cell*)为当前最优栅格节点耗散函数值，
Figure FDA00037930988200000911
为相邻栅格点
Figure FDA00037930988200000912
耗散函数值，
Figure FDA00037930988200000913
为相邻栅格点
Figure FDA00037930988200000914
启发函数值，
Figure FDA00037930988200000915
为相邻栅格点
Figure FDA00037930988200000916
的总体估计代价值；
2)如果当前最优栅格节点位于地形地图层中标记的斜坡与楼梯区域上，则沿所标记的斜坡与楼梯区域梯度方向与梯度方向反方向扩展当前最优栅格节点，当前最优栅格节点的相邻栅格节点记为：
Figure FDA00037930988200000917
Figure FDA00037930988200000918
其中，
Figure FDA00037930988200000919
为当前最优栅格节点的相邻栅格节点的坐标，f为二维栅格地图分辨率；
判断相邻栅格节点状态，如果相邻栅格节点在代价地图层的对应值为1，表示相邻栅格节点被障碍物占据，将该相邻栅格节点判定为无效栅格点并删除；如果相邻栅格节点在代价地图层的对应值为0，则估计相邻栅格节点的总体估计代价值；
所述估计相邻栅格节点的总体估计代价值的求解过程为：
Figure FDA00037930988200000920
Figure FDA0003793098820000101
Figure FDA0003793098820000102
其中，
Figure FDA0003793098820000103
为相邻栅格节点，
Figure FDA0003793098820000104
为
Figure FDA0003793098820000105
和
Figure FDA0003793098820000106
Figure FDA0003793098820000107
为相邻栅格节点耗散函数值，
Figure FDA0003793098820000108
为相邻栅格节点的坐标，
Figure FDA0003793098820000109
为
Figure FDA00037930988200001010
和
Figure FDA00037930988200001011
为相邻栅格节点启发函数值，
Figure FDA00037930988200001012
为相邻栅格节点的总体估计代价值。
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}。
9.根据权利要求8所述一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述步骤26中地图网络中的规划路径由Npoint个路径节点和Npoint-1条二维栅格地图规划路径组成，记为：
Figure FDA00037930988200001013
Figure FDA00037930988200001014
其中，
Figure FDA0003793098820000111
为第
Figure FDA0003793098820000112
个和第
Figure FDA0003793098820000113
个路径节点，属于临时地图网络中的临时路径节点集合Pointstemp，
Figure FDA0003793098820000114
为在二维栅格地图内从
Figure FDA0003793098820000115
到
Figure FDA0003793098820000116
的规划路径，(xs,ys)为二维栅格地图内规划路径点的栅格坐标，
Figure FDA0003793098820000117
为规划路径上栅格点个数。
CN202210203947.7A 2022-03-02 2022-03-02 一种应用于跨异构多层空间的轮式机器人路径规划方法 Active CN114577214B (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102275300B1 (ko) * 2019-07-05 2021-07-08 엘지전자 주식회사 이동 로봇 및 그 제어방법

Patent Citations (7)

* Cited by examiner, † Cited by third party
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