【技术实现步骤摘要】
一种应用于跨异构多层空间的轮式机器人路径规划方法
[0001]本专利技术涉及多层空间的移动机器人路径规划方法，属于机器人建图与导航算法与
技术介绍
[0002]轮式移动机器人作为常见的移动机器人，具有结构简单、成本低、稳定性好的特点，可以在多种场景下发挥重要作用。移动机器人采用激光雷达、摄像头、惯性测量单元、里程计等多种传感器构建环境地图、进行自身定位，在地图中规划运动路径与运动速度以到达预定目标。目前机器人在二维平面内的定位与路径规划、运动规划方法已经比较成熟。
[0003]随着科学技术的发展，轮式移动机器人的应用领域日益广泛，活动范围不再局限于二维平面，逐渐扩展到三维多层空间。跨异构多层空间由多个高度不同的二维平面区域与平面之间的连接区域组成，各二维平面区域具有不同的形状，常见的跨异构多层空间包括地铁站、机场、地下停车场等。机器人在跨异构多层复杂空间内执行清洁、巡逻等任务过程中，需要进行多层空间中的运动路径规划。相比于二维平面，多层空间的路径规划问题的环境复杂度增大，路径规划的计算量增大。跨异构多层复杂...
【技术保护点】
【技术特征摘要】
1.一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述方法具体过程为：步骤1、构建多层空间的地图网络；步骤2、基于多层空间的地图网络对地图网络中的路径进行搜索。2.根据权利要求1所述一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述步骤1中构建多层空间的地图网络；具体过程为：步骤11、对多层空间在楼梯与平面连接处、楼梯转台位置进行分割，划分为若干区域，各区域在二维平面上的投影不出现重叠；多层空间地图网络由空间中的路径节点与各区域的二维栅格地图连接组成；多层空间地图网络由空间中的路径节点作为网络的节点，由二维栅格地图作为网络的边；多层空间地图网络的路径节点集合记为Points＝{point
i
|i＝0,1,2,
…
,N
‑
1}；其中N为地图网络中路径节点个数；point
i
为第i个路径节点；多层空间地图网络的二维栅格地图集合记为Maps＝{map
j
|j＝0,1,2,
…
,M
‑
1}；其中M为地图网络中二维栅格地图个数；map
j
为第j个二维栅格地图；多层空间地图网络信息由路径节点集合和二维栅格地图集合组成，记为GraphNet＝(Points,Maps)；多层空间地图网络分为静态地图层staticmap
j
、代价地图层costmap
j
和地形地图层terrainmap
j
；步骤12：建立二维栅格地图列表、路径节点列表，初始状态二维栅格地图列表和路径节点列表为空列表；以第一张二维栅格地图建图起点为原点，建立地图坐标系F0，该地图坐标系同时作为世界坐标系，将地图坐标系中当前位置point0添加到路径节点列表中，并记录该节点的位置姿态，位置姿态由齐次变换矩阵表示为其中，表示地图坐标系中当前位置point0的位置姿态；I3与I4分别表示3阶与4阶单位矩阵；O3×1＝(0 0 0)
T
，O1×3＝(0 0 0)；步骤13：遥控机器人运动至地图网络已有路径节点处，以当前位置为二维栅格地图建图起点start
j
，建立地图坐标系F
j
，采集激光雷达、IMU与里程计数据，进行二维栅格地图构建；遥控机器人运动，进行二维栅格地图构建，使当前构建二维栅格地图对应区域在二维空间上不会出现重叠，将机器人当前位置作为二维栅格地图建图终点，即地图网络中的路径节点，记为dest
j
，完成当前地图坐标系F
j
下地图的二维栅格地图静态地图层部分构建；步骤14：记录二维栅格地图建图终点dest
j
在当前地图坐标系内的位置姿态，其中地图坐标系x坐标与y坐标由二维栅格地图构建过程得到，z坐标由高度计测得，姿态角度由IMU测得，并将二维栅格地图建图终点dest
j
在当前地图坐标系内的位置姿态转换为齐次变换矩阵：
其中，表示二维栅格地图建图终点dest
j
在当前地图坐标系内的姿态旋转矩阵，(x
j
,y
j
,z
j
)表示二维栅格地图建图终点dest
j
在当前地图坐标系内的位置坐标向量，T表示求转置；将二维栅格地图建图终点dest
j
在当前地图坐标系内的位置姿态转换为在世界坐标系内的位置姿态：其中，表示二维栅格地图建图起点位置姿态在世界坐标系F0内转换为齐次变换矩阵；表示二维栅格地图建图终点位置姿态在世界坐标系F0内转换为齐次变换矩阵；步骤15：在已有的路径节点列表中搜索当前二维栅格地图建图终点，根据世界坐标系内位置姿态判断当前二维栅格地图建图终点是否已存在于路径节点列表中：如果不存在，则将该终点添加到路径节点列表中，将该地图添加到二维栅格地图列表中；如果已存在，则将地图添加到二维栅格地图列表中，并将该二维栅格地图的建图终点标记为路径节点列表中已有的终点节点；步骤16：如果二维栅格地图网络已包含多层空间的全部可通行区域，则判定为多层空间地图网络静态地图层构建完成，执行步骤17；如果多层空间地图构建未完成，则在当前位置返回步骤13，开始新的二维栅格地图构建；步骤17：对静态地图层进行膨胀处理，得到代价地图层costmap
j
；步骤18：基于静态地图层构建地形地图层terrainmap
j
。3.根据权利要求1或2所述一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述步骤17中对静态地图进行膨胀处理，得到代价地图层costmap
j
；具体过程为：对地图网络静态地图层中包含的障碍物与地图边界进行膨胀处理，在地图坐标系内膨胀半径为r
inflate
，设地图栅格分辨率为f，则在二维栅格地图内的膨胀栅格数为：在静态地图层中，被障碍物占据的栅格对应值为1，可通行区域栅格对应值为0，未知区域为
‑
1；对于静态地图层障碍物占据栅格，取以膨胀栅格数为半径的邻域：U(c
x
,c
y
)＝{(c
x1
,c
y1
)|[(c
x
‑
c
x1
)2+(c
y
‑
c
y1
)2]
1/2
＜c
inflate
}其中，(c
x
,c
y
)表示邻域中心栅格点坐标，(c
x1
,c
y1
)表示邻域内栅格点坐标；将代价地图在该邻域内的栅格的对应值置为1，表示该区域内的栅格点不可通行：costmap
j
(c
x1
,c
y1
)＝1 if(c
x1
,c
y1
)∈U(c
x
,c
y
),staticmap
j
(c
x
,c
y
)＝1。4.根据权利要求3所述一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述步骤18中基于静态地图层构建地形地图层terrainmap
j
；具体过程为：
在二维栅格地图上选择多边形区域作为斜坡与楼梯区域，记为Polygon
terrain
，并给定斜坡与楼梯区域梯度方向角度θ
grad
；在地形地图层中，选取多边形区域内的栅格点，将栅格对应数值赋值为梯度方向角度：terrainmap
j
(c
x1
,c
y1
)＝θ
grad if(c
x1
,c
y1
)∈Polygon
terrain
。5.根据权利要求4所述一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述步骤2中基于多层空间的地图网络对地图网络中的路径进行搜索；具体过程为：步骤21：确定临时地图网络；步骤22：搜索初始化；具体过程为：建立开放节点列表、关闭节点列表，初始开放节点列表、关闭节点列表为空列表；计算机器人路径规划起点路径节点point
start
总体估计代价；机器人路径规划起点路径节点设置为根节点，即父节点为空，将该节点写入开放节点列表；所述计算机器人路径规划起点路径节点point
start
总体估计代价的具体过程为：机器人路径规划起点路径节点耗散函数值g(point
start
)为0，启发函数值h(point
start
)为由路径规划起点到目标点的欧氏距离，从而得到路径规划起点路径节点point
start
总体估计代价f(point
start
)＝g(point
start
)+h(point
start
)；步骤23：节点的扩展；具体过程为：从开放节点列表中选择总体估计代价最小的节点作为当前最优节点，记为point
*
，寻找地图网络中与point
*
相邻的节点，记为其中n为与point
*
相邻的节点个数；搜索从当前最优节点point
*
到相邻节点的路径，路径记为路径长度记为两节点之间高度差记为估计相邻节点总体估计代价，表示为：总体估计代价，表示为：总体估计代价，表示为：其中，为相邻节点耗散函数值，g(point
*
)为当前最优节点耗散函数值，K
h
为设定的高度代价因数，K
h
>0；，为相邻节点启发函数值，为在世界坐标系内的位置坐标，为路径规划终点在世界坐标系内的位置坐标，为相邻节点总体估计代价；步骤24：节点列表更新；具体过程为：步骤241：判断相邻节点所在位置是否已在开放节点列表中；
1)如果开放节点列表中已存在与相邻节点位置相同的节点point
old
，则比较开放节点列表已有节点point
old
与相邻节点的总体估计代价：如果相邻节点总体估计代价不超过point
old
的总体估计代价，则将开放节点列表中已有节点point
old
删除，将相邻节点添加到开放节点列表中；如果相邻节点总体估计代价大于point
old
的总体估计代价，则保留已有节点point
old
；2)如果开放节点列表中不存在与相邻节点位置相同的节点，则将相邻节点添加到开放节点列表中；步骤242：将当前最优节点point<...
【专利技术属性】
技术研发人员：吴立刚，田昊宇，高亚斌，姚蔚然，齐乃明，吴承伟，刘健行，孙光辉，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。