CN115143964A - 一种基于2.5d代价地图的四足机器人自主导航方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及四足机器人控制领域，具体涉及一种基于2.5D代价地图的四足机器人自主导航方法。

背景技术

四足机器人相比较于传统的轮式和履带式机器人而言，能更好地克服崎岖和非结构化的地形，为在具有挑战的场景下的自主导航提供了可能性和应用性。其中，为了保证四足机器人导航的安全可靠，掌握环境的特征及其可穿越性非常重要。然而，现有的研究主要集中在提高机器人本体腿部的运动能力以及鲁棒抗干扰能力，而缺少对于环境信息的感知和利用。

现有的四足机器人导航技术通常还是和轮式机器人一样使用单线激光雷达和摄像头等传感器采集2D的环境地图信息，有一些先进的做法采用深度相机获取高程图对地图做简单的贴标签处理。得到地图信息后将四足机器人近似为一个点，忽略其整体位姿的可通过性和运动方向，在地图上运行如A*、DWA等路径规划算法获得路径，随后在该路径上采用同一种步态直至到达目标点。

四足机器人的自主导航采用传统轮式履带式的导航技术，构建普通的2D代价地图，在一些粗糙非结构化地形中运动可能会发生侧翻，或者遇到一些超过其最大抬腿高度阈值的障碍物而停下。

现有四足机器人的步态通常设计比较多样，但是运用比较单一，从开始位置到目标位置的运动往往只采用一种步态，或依赖人工给出控制指令进行手动步态切换。

机器人的代价地图是指通过激光雷达等传感器对环境进行扫描构建的一张包含周围障碍物信息和可行区域信息的栅格地图，该地图将为后续进行机器人路径规划提供环境信息。传统的最常用的代价地图就是依靠ROS自带的gmapping功能包扫描得到的2D环境全局地图，地图包括灰色区域为未探索的区域、白色区域为可自由移动区域以及黑色区域为障碍物。2.5D代价地图则是在前者基础上通过RGBD相机或者深度相机将障碍物或者地形的高度信息引入到了栅格地图中，它不包含环境的多层信息，所以是伪3D的。近几年由于波士顿动力公司的spot四足机器人引起的热潮，研究者开始聚焦三维环境下的机器人可靠自主高效运动。关于四足机器人技术的研究还不成熟大多都在探索，主要包括：1)本体运动学动力学的研究，致力于如何能让四足机器人真的像四足动物一样实现灵巧的运动，从而应对复杂地形。2)步态研究，开发出更多复杂的运动模式，常见纯步态研究包括像小跑，迈步、站立、快跑等。3)环境感知研究，通过搭载各类传感器，与环境交互并提高对环境的适应性。4)驱动续航研究，由最开始的液压驱动，到电液混合驱动再到现在主要的电驱动方式，简化机械结构提升四足机器人的能量效率，实现快速持续的运动。

本发明提出的基于2.5D代价地图的四足机器人自主导航方法可根据地形情况自动切换不同的步态，从而提高了机器人的运动效率和环境适应性。

发明内容

为解决上述技术问题，本发明提供一种基于2.5D代价地图的四足机器人自主导航方法。

为解决上述技术问题，本发明采用如下技术方案：

一种基于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进行判断；

步骤四：根据栅格的高度、斜率、平坦度以及四足机器人的整体足迹可通过性，得到步态切换控制信号，在不同栅格处对四足机器人的步态进行切换。

进一步地，所述传感器包括安装在四足机器人上的深度相机以及激光雷达；通过深度相机和激光雷达对四足机器人周围进行360度全覆盖的3D距离测量以及校准反射测量，得到所述的高程图。

具体地，步骤一中，惯性坐标系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(ρ，σ²)，ρ为均值，σ²为方差，高度

其中，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轴的夹角p_i即为斜率；

栅格周围圆形区域的高度h_i与平面s的标准差即为平坦度f_i：

N为圆形区域包含的栅格数，h_aver是高度均值；

通过栅格的斜率p_i、高度h_i、平坦度f_i计算每个栅格的可通过性值w_i：

其中p_max、h_max、f_max分别是四足机器人的最大坡度阈值、高度阈值和平坦度阈值；l₁、l₂、l₃为权重值且总和为1；计算过程中，如果w_i小于0则w_i直接分配为0，可通过性值w_i分配为0表示不可通过；可通过性值w_i分配为1表示完全可通过。

具体地，计算相邻状态的代价函数f，相邻状态的代价函数f即从当前状态(x，y，θ)到下一状态(x′，y′，θ′)的代价函数f：

f＝f₁+f₂；

其中，f₁为可通过性代价，f₂为转向代价；

可通过性代价

其中d为两个状态间的欧氏距离，w_f为四足机器人的整体足迹可通过性，

为四足机器人的移动方向，l₃、l₄、l₅为系数；

从状态(x，y，θ)到状态(x′，y′，θ′)移动的角度

δ＝atan2(y′-y，x′-x)；

使用航向角θ与状态间移动角度δ的差值Δθ来计算转向代价，转向代价f₂用于惩罚四足机器人侧着走代价f₂₁和向后走代价f₂₂：

f₂＝l₆f₂₁+l₇f₂₂；

其中l₆、l₇分别为侧着走代价和向后走代价的权重，l₆与l₇和为1。

具体地，步骤三中，在用外接圆近似四足机器人时，半径记为c，为了避免与障碍物的碰撞，使外接圆膨胀直至碰到障碍物或达到给出的阈值c_max，膨胀圆半径记为c_i，用影响因子μ对整体足迹可通过性w_f进行修正w_f＝μw_f；影响因子

其中，∈是调整影响因子的参数。

具体地，步骤四中，以n个栅格为一块区域，对可通过性代价地图进行划分，计算一块区域栅格的平均平坦度f_aver、斜率p_aver以及高度h_aver，当f_aver接近0时四足机器人切换为小跑步态，p_aver大于0时四足机器人切换为慢走步态。

与现有技术相比，本发明的有益技术效果是：

本发明根据高程图得到可通过性代价地图，四足机器人根据可通过性代价地图对相关位置的可穿越性做出评估，并选择安全的路径进行导航，从而减少运动过程中四足机器人发生倾倒或者停滞的情况；此外，为了提高四足机器人的运动能力、环境适应性以及运动效率，考虑了四足机器人整体足迹的可穿越性，并提出了一种步态切换框架，使得机器人能够根据代价地图切换相应的运动步态。

附图说明

图1为本发明四足机器人自主导航方法的整体流程图；

图2为本发明高程图的坐标示意图；

图3为本发明路径规划的流程图；

图4为本发明采用矩形的外接圆(左)、矩形的内接圆(中)、矩形(右)对四足机器人进行近似的效果图；

图5为本发明步态切换流程图。

具体实施方式

下面结合附图对本发明的一种优选实施方式作详细的说明。

本发明中的四足机器人自主导航方法通过机载外部感知传感器构建以四足机器人为中心的2.5D高程图，得到高程图以后为每个栅格计算对应的可通过性值wi，得到环境的2.5D可通过性代价地图用于路径规划；将可通过性代价地图用于分析四足机器人的整体足迹可通过性，对得到的路径评估相邻状态间的代价函数，同时根据不同的地形对机器人的步态进行切换控制，导航至目标；其中可通过性值范围为0到1，越靠近1表示可通过的概率越大，

四足机器人自主导航方法的整体流程如图1所示。

1.环境可通过性代价地图

本发明在四足机器人头部安装Kinect深度相机，结合Velodyne VLP-16激光雷达实现对四足机器人周围360度全覆盖、3D距离测量以及校准反射测量得到初步的2.5D高程图。

(1)设惯性坐标系I固定在环境中，真实地形相对惯性坐标系I静止，Kinect深度相机坐标系记为S，经平移T_IS和旋转C_IS后与I关联，二者变换关系通过四足机器人的状态估计得到，用六维位姿协方差矩阵∑_p表征。定义高程图坐标系为M，指定平移T_SM和旋转C_SM使得M的z轴与I保持对齐。将深度相机的测量值作为空间点映射到M，M中的栅格(x，y)处产生高度测量值h，由高斯分布近似为h～N(ρ，σ²)，ρ为均值，σ²为方差。

h计算公式：

h＝P(C_SM ^T(q)S_h-T_M)；

C_SM由单位四元数q参数化，S_h是栅格(x，y)在深度相机坐标下的位置，T_M是栅格(x，y)在高程图下的位置，投影矩阵P＝[001]，将三维测量映射到标量高度h。

为了获得高度h的方差σ_h ²，由上述公式推出深度相机测量J_S和旋转J_q的雅可比矩阵：

方差σ_h ²＝J_S∑_pJ_S ^T+J_q∑_p，qJ_q ^T；其中∑_p，q是深度相机旋转的协方差矩阵。

高程图的坐标示意如图2所示。

(2)求得高程图相应栅格(x_i，y_i)的高度h_i以后，下面计算栅格的斜率和粗糙度。在栅格周围圆形区域拟合一个平面s，平面s法线与惯性坐标系z轴的夹角p_i即为斜率，栅格周围圆形区域的高度值与平面的标准差即为平坦度f_i：

其中，N为圆形区域包含的栅格数，h_aver是高度均值。

(3)得到栅格的斜率p_i、栅高度h_i、平坦度f_i后，计算每个栅格的可通过性值w_i，实现高程图向可通过代价地图的转换；计算公式为：

其中p_max、h_max、f_max分别是四足机器人根据自身电机运动能力等硬约束所给出的最大坡度阈值、高度阈值和平坦度阈值，l₁、l₂、l₃是总和为1的各项权重。计算过程中如果w_i小于0则w_i直接分配为0，可通过性值w_i分配为0表示不可通过；如果某一部分栅格为平坦地形，可通过性值w_i分配为1表示完全可通过。

2.路径规划

基于前面得到2.5D可通过行代价地图，评估各个栅格可通过性的同时，采用如图3所示的分层规划器架构进行路径规划。首先在全局路径规划器中用RRT^*全局路径规划算法在可通过行代价地图上规划出一条从起始位置到目标位置的路径，传给下一层再用操作频率更高的局部路径规划器来对四足机器人周围的局部代价地图进行实时更新，修正全局路径。

路径规划流程如图3所示。

3.代价函数

从状态(x，y，θ)到下一状态(x′，y′，θ′)的代价函数f由可通过性代价f₁和转向代价f₂构成，计算公式：

f＝f₁+f₂；

为计算f₁和f₂，需要知道两个状态间的长度d、足迹可通过性w_f、移动方向

本实施例中两个采用欧氏距离。可通过性代价f₁计算公式：

其中l₃是权重系数，l₄、l₅为用于调整与d相比可通过性影响的系数，从状态(x，y，θ)到状态(x′，y′，θ′)移动的角度

δ＝atan2(y′-y，x′-x)；

使用航向角θ与状态间移动角度δ的差值Δθ来计算转向代价，这里转向代价用于惩罚四足机器人侧着走代价f₂₁和向后走代价f₂₂，计算公式：

f₂＝l₆f₂₁+l₇f₂₂；

其中l₆、l₇分别为侧着走代价和向后走代价的权重，l₆与l₇和为1。

4.四足机器人足迹可通过性

前面已得到关于环境和机器人本身运动能力的可通过性代价地图，现在具体考虑四足机器人的整体足迹是否能穿越可通过性代价地图。交替使用两种不同的方法近似表示四足机器人，一种是考虑运动方向的矩形形式表示四足机器人，一种是不考虑运动方向的圆形形式。整体足迹可通过性w_f主要由环境可通过性代价地图中机器人相应位姿的足迹内所有栅格的可通过性值w_i取平均值给出，范围也是[0，1]。

机器人的足迹(也称为状态)表示为(x，y，θ)，分别代表位置和航向角度(保持与规划路径相切)。首选采用矩形的外接圆来近似实际四足机器人，在特定位置譬如狭窄的走廊则外接圆可能过于保守导致无法通过，换用矩形的内接圆来近似四足机器人，上面两种近似后若无法得到有效的路径，则采用矩形进行规划。

在用矩形外接圆来近似四足机器人时，半径记为c，为了避免与障碍物的碰撞，使外接圆膨胀直至碰到障碍物或达到给出的阈值c_max，膨胀圆半径记为c_i，用影响因子μ对足端可通过性w_f进行修正w_f＝μw_f。μ计算公式：

其中∈是调整影响因子的参数。

采用矩形的外接圆、矩形的内接圆、矩形对四足机器人进行近似的效果如图4所示。

5.步态切换

四足机器人本身的运动控制设计里具有对角小跑、站立、慢走等多种步态。在可通过性代价地图里已经得到栅格的高度、斜率、平坦度以及四足机器人的整体足迹可通过性。在四足机器人运动过程中根据地形不同分配不同的步态，四足机器人的步态控制器部分记为：

G＝g₁、g₂、g₃、…、g_i、…、g_m；

g_i代表设计好的步态；根据点云特征为每个栅格分配相应的步态切换控制信号N_i＝(…，g_i)，其中i为栅格编号，g_i示机器人经过该栅格i应切换的步态，…表示其他信息譬如位置、可通过性值等。

以n个栅格(n由人为设置)为一块区域对2.5D可通过性代价地图进行划分，计算一块区域栅格的平均平坦度f_aver、斜率P_aver以及高度h_aver。本实施例中，f_aver接近0时，步态控制器切换为小跑步态，p_aver大于0时，步态控制器切换为慢走步态。

步态切换流程见图5。

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

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内，不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立技术方案，说明书的这种叙述方式仅仅是为了清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (6)

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(ρ，σ²)，ρ为均值，σ²为方差，高度

其中，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轴的夹角p_i即为斜率；

栅格周围圆形区域的高度h_i与平面s的标准差即为平坦度f_i：

N为圆形区域包含的栅格数，h_aver是高度均值；

通过栅格的斜率p_i、高度h_i、平坦度f_i计算每个栅格的可通过性值w_i：

其中p_max、h_max、f_max分别是四足机器人的最大坡度阈值、高度阈值和平坦度阈值；l₁、l₂、l₃为权重值且总和为1；计算过程中，如果w_i小于0则w_i直接分配为0，可通过性值w_i分配为0表示不可通过；可通过性值w_i分配为1表示完全可通过。

4.根据权利要求1所述的基于2.5D代价地图的四足机器人自主导航方法，其特征在于：计算相邻状态的代价函数f，相邻状态的代价函数f即从当前状态(x，y，θ)到下一状态(x′，y′，θ′)的代价函数f：

f＝f₁+f₂；

其中，f₁为可通过性代价，f₂为转向代价；

可通过性代价

其中d为两个状态间的欧氏距离，w_f为四足机器人的整体足迹可通过性，

为四足机器人的移动方向，l₃、l₄、l₅为系数；

从状态(x，y，θ)到状态(x′，y′，θ′)移动的角度

δ＝atan2(y′-y，x′-x)；

使用航向角θ与状态间移动角度δ的差值

来计算转向代价，转向代价f₂用于惩罚四足机器人侧着走代价f₂₁和向后走代价f₂₂：

f₂＝l₆f₂₁+l₇f₂₂；

其中l₆、l₇分别为侧着走代价和向后走代价的权重，l₆与l₇和为1。

5.根据权利要求1所述的基于2.5D代价地图的四足机器人自主导航方法，其特征在于：步骤三中，在用外接圆近似四足机器人时，半径记为c，为了避免与障碍物的碰撞，使外接圆膨胀直至碰到障碍物或达到给出的阈值c_max，膨胀圆半径记为c_i，用影响因子μ对整体足迹可通过性w_f进行修正w_f＝μw_f；影响因子

其中，∈是调整影响因子的参数。

6.根据权利要求1所述的基于2.5D代价地图的四足机器人自主导航方法，其特征在于：步骤四中，以n个栅格为一块区域，对可通过性代价地图进行划分，计算一块区域栅格的平均平坦度f_aver、斜率p_aver以及高度h_aver，当f_aver接近0时四足机器人切换为小跑步态，p_aver大于0时四足机器人切换为慢走步态。

Images