一种基于2.5D代价地图的四足机器人自主导航方法技术

本发明专利技术涉及四足机器人控制领域，公开了一种基于2.5D代价地图的四足机器人自主导航方法，可以结合障碍物和环境的高度信息，过滤提取出环境中不同的地形信息比如台阶、斜坡、粗糙地面等，最终计算出相关位置的可通过性代价地图；该可通过性代价地图可使机器人在路径规划过程中自动选择易于通行的平整地貌，从而极大降低了机器人的事故率。大降低了机器人的事故率。大降低了机器人的事故率。

【技术实现步骤摘要】
一种基于2.5D代价地图的四足机器人自主导航方法


[0001]本专利技术涉及四足机器人控制领域，具体涉及一种基于2.5D代价地图的四足机器人自主导航方法。

技术介绍

[0002]四足机器人相比较于传统的轮式和履带式机器人而言，能更好地克服崎岖和非结构化的地形，为在具有挑战的场景下的自主导航提供了可能性和应用性。其中，为了保证四足机器人导航的安全可靠，掌握环境的特征及其可穿越性非常重要。然而，现有的研究主要集中在提高机器人本体腿部的运动能力以及鲁棒抗干扰能力，而缺少对于环境信息的感知和利用。
[0003]现有的四足机器人导航技术通常还是和轮式机器人一样使用单线激光雷达和摄像头等传感器采集2D的环境地图信息，有一些先进的做法采用深度相机获取高程图对地图做简单的贴标签处理。得到地图信息后将四足机器人近似为一个点，忽略其整体位姿的可通过性和运动方向，在地图上运行如A*、DWA等路径规划算法获得路径，随后在该路径上采用同一种步态直至到达目标点。
[0004]四足机器人的自主导航采用传统轮式履带式的导航技术，构建普通的2D代价地图，在一些粗糙非结构化地形中运动可能会发生侧翻，或者遇到一些超过其最大抬腿高度阈值的障碍物而停下。
[0005]现有四足机器人的步态通常设计比较多样，但是运用比较单一，从开始位置到目标位置的运动往往只采用一种步态，或依赖人工给出控制指令进行手动步态切换。
[0006]机器人的代价地图是指通过激光雷达等传感器对环境进行扫描构建的一张包含周围障碍物信息和可行区域信息的栅格地图，该地图将为后续进行机器人路径规划提供环境信息。传统的最常用的代价地图就是依靠ROS自带的gmapping功能包扫描得到的2D环境全局地图，地图包括灰色区域为未探索的区域、白色区域为可自由移动区域以及黑色区域为障碍物。2.5D代价地图则是在前者基础上通过RGBD相机或者深度相机将障碍物或者地形的高度信息引入到了栅格地图中，它不包含环境的多层信息，所以是伪3D的。近几年由于波士顿动力公司的spot四足机器人引起的热潮，研究者开始聚焦三维环境下的机器人可靠自主高效运动。关于四足机器人技术的研究还不成熟大多都在探索，主要包括：1)本体运动学动力学的研究，致力于如何能让四足机器人真的像四足动物一样实现灵巧的运动，从而应对复杂地形。2)步态研究，开发出更多复杂的运动模式，常见纯步态研究包括像小跑，迈步、站立、快跑等。3)环境感知研究，通过搭载各类传感器，与环境交互并提高对环境的适应性。4)驱动续航研究，由最开始的液压驱动，到电液混合驱动再到现在主要的电驱动方式，简化机械结构提升四足机器人的能量效率，实现快速持续的运动。
[0007]本专利技术提出的基于2.5D代价地图的四足机器人自主导航方法可根据地形情况自动切换不同的步态，从而提高了机器人的运动效率和环境适应性。

技术实现思路

[0008]为解决上述技术问题，本专利技术提供一种基于2.5D代价地图的四足机器人自主导航方法。
[0009]为解决上述技术问题，本专利技术采用如下技术方案：
[0010]一种基于2.5D代价地图的四足机器人自主导航方法，包括以下步骤：
[0011]步骤一：通过四足机器人上的传感器获取环境中的高程图，对惯性坐标系、传感器坐标系以及高程图坐标系处理后，计算得到高程图坐标系中栅格(x
i
,y
i
)处的高度h
i
，根据高度h
i
计算栅格的斜率p
i
以及平坦度f
i
，进而通过栅格的高度h
i
、斜率p
i
、平坦度f
i
计算栅格(x
i
,y
i
)的可通过性值w
i
，得到可通过性代价地图；
[0012]步骤二：通过全局路径规划器和局部路径规划器相结合生成全局路径：全局路径规划器使用RRT
*
全局路径规划算法在通过性代价地图上规划出一条从起始位置到目标位置的全局路径，局部路径规划器对四足机器人周围的局部代价地图进行实时更新并修正全局路径，得到规划路径；
[0013]步骤三：依次采用矩形的外接圆、矩形的内接圆以及矩形对四足机器人进行近似，对四足机器人沿规划路径运动时在可通过性代价地图中的整体足迹可通过性w
f
进行判断；
[0014]步骤四：根据栅格的高度、斜率、平坦度以及四足机器人的整体足迹可通过性，得到步态切换控制信号，在不同栅格处对四足机器人的步态进行切换。
[0015]进一步地，所述传感器包括安装在四足机器人上的深度相机以及激光雷达；通过深度相机和激光雷达对四足机器人周围进行360度全覆盖的3D距离测量以及校准反射测量，得到所述的高程图。
[0016]具体地，步骤一中，惯性坐标系I固定在环境中，深度相机坐标系记为S，经平移T
IS
和旋转C
IS
后与I关联，惯性坐标系I和深度相机坐标系S通过四足机器人的状态估计得到，用六维位姿协方差矩阵∑
p
表征；高程图坐标系为M，指定平移T
SM
和旋转C
SM
使得M的z轴与I保持对齐，将深度相机的测量值作为空间点映射到M，高程图坐标系M中的栅格(x
i
，y
i
)处的高度测量值为h
i
，由高斯分布近似为h
i
～N(ρ，σ2)，ρ为均值，σ2为方差，高度
[0017][0018]其中，C
SM
由单位四元数q参数化，是栅格(x
i
，y
i
)在深度相机坐标下的位置，T
M
是栅格(x
i
，y
i
)在高程图下的位置，投影矩阵P＝[0 0 1]；
[0019]深度相机测量的雅可比矩阵J
S
和旋转的雅可比矩阵J
q
：
[0020][0021]方差
[0022]其中∑
p，q
是深度相机旋转的协方差矩阵；
[0023]在栅格周围圆形区域拟合一个平面s，平面s的法线与惯性坐标系z轴的夹角p
i
即为斜率；
[0024]栅格周围圆形区域的高度h
i
与平面s的标准差即为平坦度f
i
：
[0025][0026]N为圆形区域包含的栅格数，h
aver
是高度均值；
[0027]通过栅格的斜率p
i
、高度h
i
、平坦度f
i
计算每个栅格的可通过性值w
i
：
[0028][0029]其中p
max
、h
max
、f
max
分别是四足机器人的最大坡度阈值、高度阈值和平坦度阈值；l1、l2、l3本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于2.5D代价地图的四足机器人自主导航方法，包括以下步骤：步骤一：通过四足机器人上的传感器获取环境中的高程图，对惯性坐标系、传感器坐标系以及高程图坐标系处理后，计算得到高程图坐标系中栅格(x
i
，y
i
)处的高度h
i
，根据高度h
i
计算栅格的斜率p
i
以及平坦度f
i
，进而通过栅格的高度h
i
、斜率p
i
、平坦度f
i
计算栅格(x
i
，y
i
)的可通过性值w
i
，得到可通过性代价地图；步骤二：通过全局路径规划器和局部路径规划器相结合生成全局路径：全局路径规划器使用RRT
*
全局路径规划算法在通过性代价地图上规划出一条从起始位置到目标位置的全局路径，局部路径规划器对四足机器人周围的局部代价地图进行实时更新并修正全局路径，得到规划路径；步骤三：依次采用矩形的外接圆、矩形的内接圆以及矩形对四足机器人进行近似，对四足机器人沿规划路径运动时在可通过性代价地图中的整体足迹可通过性w
f
进行判断；步骤四：根据栅格的高度、斜率、平坦度以及四足机器人的整体足迹可通过性，得到步态切换控制信号，在不同栅格处对四足机器人的步态进行切换。2.根据权利要求1所述的基于2.5D代价地图的四足机器人自主导航方法，其特征在于：所述传感器包括安装在四足机器人上的深度相机以及激光雷达；通过深度相机和激光雷达对四足机器人周围进行360度全覆盖的3D距离测量以及校准反射测量，得到所述的高程图。3.根据权利要求2所述的基于2.5D代价地图的四足机器人自主导航方法，其特征在于：步骤一中，惯性坐标系I固定在环境中，深度相机坐标系记为S，经平移T
IS
和旋转C
IS
后与I关联，惯性坐标系I和深度相机坐标系S通过四足机器人的状态估计得到，用六维位姿协方差矩阵∑
p
表征；高程图坐标系为M，指定平移T
SM
和旋转C
SM
使得M的z轴与I保持对齐，将深度相机的测量值作为空间点映射到M，高程图坐标系M中的栅格(x
i
，y
i
)处的高度测量值为h
i
，由高斯分布近似为h
i
～N(ρ，σ2)，ρ为均值，σ2为方差，高度其中，C
SM
由单位四元数q参数化，是栅格(x
i
，y
i
)在深度相机坐标下的位置，T
M
是栅格(x
i
，y
i
)在高程图下的位置，投影矩阵P＝[0 0 1]；深度相机测量的雅可比矩阵J
S
和旋转的雅可比矩阵J
q
：方差其中∑
p，q
是深度相机旋转的协方差矩阵；在栅格周围圆形区域拟合一个平面s，平面s的法线与惯性坐标系z轴...

【专利技术属性】
技术研发人员：秦家虎，焦袁，闫成真，刘轻尘，马麒超，
申请(专利权)人：中国科学技术大学，
类型：发明
国别省市：