[发明专利]一种应用于跨异构多层空间的轮式机器人路径规划方法

权利要求书

1.一种应用于跨异构多层空间的轮式机器人路径规划方法，其特征在于：所述方法具体过程为：

步骤1、构建多层空间的地图网络；

步骤2、基于多层空间的地图网络对地图网络中的路径进行搜索；

所述步骤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：建立二维栅格地图列表、路径节点列表，初始状态二维栅格地图列表和路径节点列表为空列表；

以第一张二维栅格地图建图起点为原点，建立地图坐标系F₀，该地图坐标系同时作为世界坐标系，将地图坐标系中当前位置point₀添加到路径节点列表中，并记录该节点的位置姿态，位置姿态由齐次变换矩阵表示为

其中，表示地图坐标系中当前位置point₀的位置姿态；I₃与I₄分别表示3阶与4阶单位矩阵；O_3×1＝(0 0 0)^T，O_1×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在当前地图坐标系内的位置姿态转换为在世界坐标系内的位置姿态：

其中，表示二维栅格地图建图起点位置姿态在世界坐标系F₀内转换为齐次变换矩阵；表示二维栅格地图建图终点位置姿态在世界坐标系F₀内转换为齐次变换矩阵；

步骤15：在已有的路径节点列表中搜索当前二维栅格地图建图终点，根据世界坐标系内位置姿态判断当前二维栅格地图建图终点是否已存在于路径节点列表中：如果不存在，则将该终点添加到路径节点列表中，将该地图添加到二维栅格地图列表中；如果已存在，则将地图添加到二维栅格地图列表中，并将该二维栅格地图的建图终点标记为路径节点列表中已有的终点节点；

步骤16：如果二维栅格地图网络已包含多层空间的全部可通行区域，则判定为多层空间地图网络静态地图层构建完成，执行步骤17；如果多层空间地图构建未完成，则在当前位置返回步骤13，开始新的二维栅格地图构建；

步骤17：对静态地图层进行膨胀处理，得到代价地图层costmap_j；

步骤18：基于静态地图层构建地形地图层terrainmap_j。