CN116147642B - 融合地形与力的四足机器人可达性地图构建方法及系统 - Google Patents

Abstract

本发明提供了一种融合地形与力的四足机器人可达性地图构建方法及系统，包括：四足机器人通过融合RGB‑D相机获取的视觉信息和IMU信息建立视觉‑惯性里程计，实时估计四足机器人的位姿；基于RGB‑D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值；当栅格高度小于可通过阈值时，则计算地图中的地形几何特征以及四足机器人足接触力特性；基于地形几何特征以及四足机器人足端力特性进行四足机器人运动过程中复杂地形的可达性分析；基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。

Description

融合地形与力的四足机器人可达性地图构建方法及系统

技术领域

本发明涉及机器人开发技术领域，具体地，涉及融合地形与力的四足机器人可达性地图构建方法及系统，更为具体地，涉及融合地形与力特征的四足机器人可达性地图构建方法及系统。

背景技术

近年来随着人工智能、控制、材料等学科不断取得进展，机器人的智能化、可靠性也取得了长足进步，被广泛应用于农业种植、工业生产、国防军事等各种领域。在机器人系统中，自主移动式机器人应用最为广泛常见，其灵活的应用场景，受到各行各业广泛关注。自主移动机器人主要分为三类：轮式机器人、履带式机器人和腿足式机器人。其中，腿足式机器人较轮式、履带式机器人而言，它不需要拘谨于平坦的路面条件，不仅可以通过森林、草地等崎岖的原始路面，也能够顺利地通过楼梯、台阶及较大坡度的路面，复杂环境适应能力更强。因此，腿足式机器人很早就在军事上应用于战场物资装备运输。但如何提升腿足式机器人的复杂条件下的通行能力，进一步提升其无人化程度，适应更加复杂多变的任务环境，完成自主行走任务，成为了目前需要重点关注的问题。与此同时，对环境精准的感知以及对地形地图的构建成为了机器人领域不可或缺的内容。

轮式、履带式机器人中常见的地图构建方法主要是利用二维栅格地图来描述外界环境，通过判断地标的栅格是否被占用来表示该地标是否存在障碍物。这一种表示方法结构简单、数据存储空间小，比较适用于平面机器人的自主路径规划和导航。但是由于传统二维栅格地图缺少所处环境的高度维度，很难应用于腿足式机器人的落脚点规划及避障。此外，另一种三维栅格地图由于数据存储量较大，进行实时计算和分析处理所需要的计算资源较大，也给四足机器人实时任务运动带来了较大挑战。因此，如何在不大幅增加数据存储量、保持良好的实时处理能力的条件下，更加有效地表示腿足式机器人的外部环境成为了需要解决的技术难点。

专利文献CN110264572A(申请号：201910544272.0)公开了一种融合几何特性与力学特性的地形建模方法及系统，设计用于星球车在复杂环境进行地形建模，能够具有生成视觉感知语义信息更新功能与多维度地形表征优势，但该项专利中对地形语义分割过程及地图存储具有占据大量计算资源与存储资源的缺点，不容易在实时要求性高、较大范围建图的四足机器人系统上使用。

专利文献CN115185266A(申请号：202210668685.1)公开了一种基于地形可通过性的机器人控制方法及系统，能够在多种通过程度区域规划机器人的路径，具有在结构化环境合理对机器人进行路径规划和运动控制功能与场景通行特征表征丰富的优势,但该项专利在非机构化环境下，利用三角化重建方式具有地形几何特征表示不够细致的缺点。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种融合地形与力特征的四足机器人可达性地图构建方法及系统。

根据本发明提供的一种融合地形与力特征的四足机器人可达性地图构建方法，包括：

步骤S1：四足机器人通过融合RGB-D相机获取的视觉信息和IMU信息建立视觉-惯性里程计，基于视觉-惯性里程计实时估计四足机器人的位姿；

步骤S2：基于RGB-D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿，建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值，并将高度信息记录到栅格地图中；

步骤S3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；

步骤S4：基于足底多重力传感器计算四足机器人足接触力特性；

步骤S5：基于地形几何特征以及四足机器人足接触力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；

步骤S6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。

优选地，所述步骤S1采用：通过RGB-D相机采集点云信息，基于ICP匹配结果求解计算点云间的位姿变换，再基于求解的位姿变换进一步迭代匹配的位置，根据观测值以及IMU测量值进行全局优化，得到全局最小误差的位姿。

优选地，所述步骤S3采用：以栅格地图中栅格为单位，根据栅格位置中点云集来获取当前栅格处的地形几何特征；

地形几何特征包括：坡度S、曲率C、粗糙度R和起伏度M；

所述坡度S是基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法获取栅格处点云集的中心法向量，根据中心法向量与机器人中心坐标系Z轴之间的夹角计算栅格的地形坡度；

所述曲率C采用：对栅格中的点云集/>进行集中化处理得到矩阵/>，根据集中化的矩阵/>计算点云集/>的协方差矩阵/>，再对协方差矩阵/>进行特征值分解，基于协方差矩阵的特征值计算栅格的曲率；

所述粗糙度R是利用栅格内点云集拟合曲面的面积与单位栅格面积大小之差确定粗糙度；

所述起伏度M是根据栅格的高度值与周围8个栅格的高度值比较，得到最大的高程值以及最小的高程值，基于最大的高程值和最小高程值之差确定起伏度。

优选地，所述足底多重力传感器能够同时感知足底多维力、接触面温度以及环境温度；

四足机器人的4个足底多重力传感器能够测量水平面上全方向的力和力矩云图；

足底多重力传感器在垂直方向上也能够同时实现拉力和压力测量。

优选地，所述步骤S4采用：基于足底多重力传感器分析法向足端等效刚度K和足底力切向摩擦系数；

所述法向足端等效刚度K反应四足机器人足底在接触不同地形时的沉陷程度；四足机器人足底与地面支撑过程使用Hunt-Crossly碰撞模型描述；

其中，表示足端法向量支撑力；K表示足端等效刚度；N表示足端等效阻尼；/>表示足端变形量；m1、n1、p分别是待定的阻尼项指数；

所述足底力切向摩擦系数是基于Coulomb摩擦模型预测足底切向摩擦力获得的；

其中，表示足底力切向摩擦系数；/>表示切向摩擦力；/>表示法向支撑力；/>表示切向相对滑动速度。

优选地，所述步骤S5采用：

栅格处的可达性得分/>，当/>时表示该栅格地形适宜通行，当/>时表示该栅格地形不适合通行；

栅格的可通行得分采用：

其中，，分别为几何特征项和足端接触力特征项的权重系数，分别为对应的几何特征的阈值，/>分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，该处栅格直接被判为不可达。

优选地，所述步骤S6采用：将建立的2.5D高程栅格地图以及栅格的可达性得分地图相结合，栅格中每一个地图的高程值代表该栅格的高度，如果栅格高度大于四足机器人可通行的最大高度阈值，则栅格设置为不可通行，栅格的可通过得分直接设置为0；如果栅格高度小于四足机器人可通行的最大高度阈值，进行几何地形分析和接触力分析，计算地形几何特征参数以及足端接触力特征参数，融合评估栅格的可达性得分score；综合全局的地形可达性分数，得到四足机器人基于地形评价的可达性地图。

根据本发明提供的一种融合地形与力特征的四足机器人可达性地图构建系统，包括：

模块M1：四足机器人通过融合RGB-D相机获取的视觉信息和IMU信息建立视觉-惯性里程计，基于视觉-惯性里程计实时估计四足机器人的位姿；

模块M2：基于RGB-D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿，建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值，并将高度信息记录到栅格地图中；

模块M3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；

模块M4：基于足底多重力传感器计算四足机器人足接触力特性；

模块M5：基于地形几何特征以及四足机器人足接触力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；

模块M6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。

优选地，所述模块M3采用：以栅格地图中栅格为单位，根据栅格位置中点云集来获取当前栅格处的地形几何特征；

地形几何特征包括：坡度S、曲率C、粗糙度R和起伏度M；

所述坡度S是基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法获取栅格处点云集的中心法向量，根据中心法向量与机器人中心坐标系Z轴之间的夹角计算栅格的地形坡度；

所述曲率C采用：对栅格中的点云集/>进行集中化处理得到矩阵/>，根据集中化的矩阵/>计算点云集/>的协方差矩阵/>，再对协方差矩阵/>进行特征值分解，基于协方差矩阵的特征值计算栅格的曲率；

所述粗糙度R是利用栅格内点云集拟合曲面的面积与单位栅格面积大小之差确定粗糙度；

所述起伏度M是根据栅格的高度值与周围8个栅格的高度值比较，得到最大的高程值以及最小的高程值，基于最大的高程值和最小高程值之差确定起伏度。

优选地，所述足底多重力传感器能够同时感知足底多维力、接触面温度以及环境温度；

四足机器人的4个足底多重力传感器能够测量水平面上全方向的力和力矩云图；

足底多重力传感器在垂直方向上也能够同时实现拉力和压力测量；

所述模块M4采用：基于足底多重力传感器分析法向足端等效刚度K和足底力切向摩擦系数；

所述法向足端等效刚度K反应四足机器人足底在接触不同地形时的沉陷程度；四足机器人足底与地面支撑过程使用Hunt-Crossly碰撞模型描述；

其中，表示足端法向量支撑力；K表示足端等效刚度；N表示足端等效阻尼；/>表示足端变形量；m1、n1、p分别是待定的阻尼项指数；

所述足底力切向摩擦系数是基于Coulomb摩擦模型预测足底切向摩擦力获得的；

其中，表示足底力切向摩擦系数；/>表示切向摩擦力；/>表示法向支撑力；/>表示切向相对滑动速度；

所述模块M5采用：

栅格处的可达性得分/>，当/>时表示该栅格地形适宜通行，当/>时表示该栅格地形不适合通行；

栅格的可通行得分采用：

其中，，分别为几何特征项和足端接触力特征项的权重系数，分别为对应的几何特征的阈值，/>分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，该处栅格直接被判为不可达。

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

1、本发明建立了基于观测优化的是视觉里程计，通过3D-3D深度图像帧间进行ICP匹配以及基于路标观测的全局位姿优化，建立了四足机器人视觉里程计，实现了四足机器人位姿的估计，能更好的消除视觉里程计的累计误差；

2、本发明通过相机深度数据以及四足机器人优化位姿，构建了全局三维点云地图，采用栅格高度值估计与迭代更新，实现了再从点云数据中重新恢复了高度信息，建立了基于栅格的2.5D高程地图；相比于传统3D地图，本发明建立的2.5D高程地图存储数据量更小，同时也保存了地图通行的主要信息；

3、本发明通过比较栅格高度值和四足机器人可通过最大高度阈值，直接将高度值大于最大高度阈值的栅格剔除几何计算，实现了可通过性地图计算量的大幅减小，使地图更关注于更新高度阈值以下的地形几何特征计算和可达性分析；

4、本发明通过提出地形的四项几何特征：坡度S、曲率C、粗糙度R和起伏度M，为四足机器人评估复杂地形以及落足点选择提供了更加细致全面的评价指标，实现了可达性评价方案；

5、本发明针对四足机器人实际行走过程中检测到的足端多维力反馈，进行了相关足底接触力特征计算，严格分析了法向足端等效刚度K和切向摩擦系数，这些参数反应了不同地形足底所感知到的接触力特征，补充了纯视觉地形几何分析对非结构化地形内在特性感知能力不足的缺点；

6、本发明建立了融合地形几何特征与足底接触力特征评估的四足机器人可达性地图，通过在栅格2D地图上增加一项可达性得分Score数据，实现了较全面地评价四足机器人平台在复杂地形可到达性能，同时也大大减小地图存储数据量，其作为代价地图，能有效地减小四足机器人在路径规划以及落足点选择方面的地图数据查找速度，提升机器人反应能力。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为四足机器人可达性地图构建方法流程图；

图2为实时估计四足机器人的位姿流程图；

图3为2.5D栅格地图建立流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1

针对现有地图构建技术中的缺陷，本发明的目的是提供一种基于地形几何特征评估的四足机器人可达性地图构建方法及系统。对实现四足机器人平稳、高效的运动有着重要借鉴意义。

所述基于地形几何特征评估的四足机器人可达性地图构建方法，如图1至3所示，包括：

步骤S1：四足机器人基于头部携带的RGB-D相机进行特征点提取、相邻帧的图像匹配，同时融合配置的IMU三维加速度信息，建立视觉-惯性里程计，实时估计四足机器人的位置；

具体地，所述步骤S1采用：根据深度相机所采集的深度和图像，对相邻的两帧图像进行ICP匹配，获取相机在地图坐标系中的位置和姿态。基于点云间的距离计算两个点云块的匹配，基于匹配结果求解计算点云间的位姿变换，然后再基于求解的位姿变换进一步迭代匹配的位姿，根据观测进行全局优化，得到局部最小误差的的位姿。

步骤S2：在相邻帧中选取特征点具有代表性的关键帧，基于关键帧的RGB-D图像深度信息以及相关位姿估计信息，建立机器人全局点云地图并同时建立2D栅格地图以及进行栅格高度估计，建立栅格2.5D高程地图。

具体地，所述步骤S2采用：

步骤S2.1：在地图构建算法中，定义四个坐标系，分别为：惯性坐标系I，机器人质心坐标系O，相机坐标系C以及地图参考坐标系M。根据RGB信息和深度信息，建立点云深度图像，将深度图像用于转换为地图中点云的高度信息。

更为具体地，相机对点云中一点P的测量值为，M下的高度信息p，从三维点云信息中高度数据为：/>；

其中，分别表示相机坐标系C相对于地图参考坐标系M的旋转矩阵和平移向量。/>表示投影向量。考虑到相机测量存在的噪声，测量值p对相机位姿和测量值/>存在扰动噪声。本发明假设测量高度值p存在噪声，且服从高斯分布，方差/>通过协方差矩阵进行更新。

所述协方差更新公式为：

其中，是深度相机的噪声协方差矩阵，/>是由于深度相机旋转不确定性带来的协方差矩阵。

高度测量值作为一阶卡尔曼滤波观测量，更新上一次的估计值/>，得到当前的估计值/>，下次高度的估计值和方差更新公式为：

多组数据进行筛选原则：测量过程中，如果同一个栅格存在多组不同的高度数据，根据马氏距离对测量数据进行筛选，具体做法是：计算两组数据的相似程度，在每一次跟新测量时，忽略比当前高度小的数据集，取最大高度测量值作为该栅格的高度信息。

自此，根据深度信息构建了三维点云信息，再从点云数据中重新恢复了高度信息，建立了基于栅格的2.5D高程地图。

步骤S3：当栅格高度小于四足机器人可通行的最大高度阈值时，基于点云地图中的点云信息，进行栅格地图中的地形几何特征计算，地形几何特征包括坡度、曲率、粗糙度、起伏度。

具体地，如果估计的栅格高度小于四足机器人可通行的最大高度阈值，则进行几何地形分析，基于三维点云地图中的点云数据，计算地形的几何特征。地形的几何特征主要考虑以下四点：坡度S、曲率C、粗糙度R和起伏度M。以栅格地图中栅格为单位，根据栅格位置中点云集来获取该栅格处的地形几何特征信息，使四足机器人在面对台阶、崎岖不平的道路时，更好的计算起伏性，减小碰撞可能性，增加路况的可通行性。

所述坡度S计算：栅格的坡度描述了地形的一般倾斜角度，对于四足机器人而言，地形的坡度越大，机器人腿部打滑可能性越大，行走稳定性越差。基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法来获取栅格处点云集的中心法向量，然后再根据中心法向量与机器人中心坐标系Z轴之间的夹角来计算栅格的地形坡度。具体做法如下：

根据栅格点云集平面与其法向量垂直的中心思想，将求解法向量转化为最小二乘问题，将目标函数进行最小化计算，其目标函数是f，其中n为平面法向量，/>为栅格内点云。m表示栅格中点云个数，T表示转置；进一步推导

f

其中，是一个/>的协方差矩阵。通过奇异值分解(Singular Value Decomposition, SVD)对目标求解即可得到平面法向量n。平面法向量n与Z轴（方向向量表示为/>）之间的夹角是栅格/>处的坡度值/>；

所述曲率C计算：栅格的曲率描述了地形的大致凹凸程度，适当的凹凸可以防止四足机器人运动过程中的足底打滑，但是过度的凹凸程度又会卡住足底，阻碍关节运动。在构建可通行地图过程中，需要评估栅格附近的曲率变化情况，具体的做法如下：

首先对栅格中的点云集/>进行集中化处理，得到矩阵/>：

再进一步根据集中化的矩阵计算点云集/>的协方差矩阵/>

再进行特征值分解：

其中，为协方差矩阵的特征值，/>为协方差矩阵的特征向量。栅格的曲率为/>。

所述粗糙度R计算：形容地形的地面的光滑程度，往往粗糙度越高，四足机器人的足底越不容易打滑，足端行走支撑更加稳定。粗糙度的评价是根据栅格内点云集拟合曲面的面积/>与单位栅格面积/>大小之差来确定，具体计算方法如下：

所述起伏度M计算：反映了地形相对高度差，当四足机器人在行走过程中遇到了台阶或者较高的障碍物时，足端的抬起及下落不仅需要当前地形有适合的坡度、曲率和粗糙度，还需要有足够的空间完成机械腿的抬起和下落动作。当面临较高的台阶时，机械腿可能在足端运动过程中碰到台阶边缘，导致自身重力失衡；又如当机械腿落足到一块狭小凹陷的缝隙路况区域时，比较容易会发生卡陷等情况。因此，利用起伏度来描述机器人周围环境可能发生碰撞概率是必要的。具体做法是，根据栅格的高度值/>与周围8个栅格的高度值/>比较，得到最大的高程值/>,以及最小的高程值/>，定义该处的起伏度/>。

步骤S4：针对动态复杂的野外地形，基于四足机器人足端配置的足底多维力传感器，开展足底力法向足端等效刚度K和足底力切向摩擦系数研究，综合分析四足机器人足底接触力特性；

所述足底多维力传感器能同时感知足底多维力、接触面温度及环境温度，机器人结构中的4个足底多重传感器能够测量水平面上全方向的力和力矩云图，水平力检测具备高密度识别能力，水平云图密度识别力的角度≤30°，水平力测量量程0-500N。足底传感器垂直方向上，足底多重力传感器也能同时实现拉力和压力测量，不仅满足四足机器人控制中所需的足底压力测量，还能进行机器人足端卡缝时拉伸力脱困测试，垂直方向上量程0-200公斤，测量精度小于1%，安全过载不大于150%，极限过载不大于200%。足底温度传感器量程为0-100℃，精度小于1%。

足底接触力特性计算：野外环境多为泥泞、湿滑的复杂地形，四足机器人在泥地、沙地上行走时足端会发生沉陷，过大沉陷量会使四足机器人丧失行动能力；在光滑潮湿的瓷砖、雪地及冰面上行进时，足底容易产生打滑现象，削弱运动能力。针对这种动态复杂的地形，基于四足机器人足底配置的足底多维力传感器，开展足底力法向足端等效刚度K和足底力切向摩擦系数研究，综合分析四足机器人足底接触力特性，评估地形的实际行走状态。

所述法向足端等效刚度K：反应四足机器人足底在接触不同地形时的沉陷程度。四足机器人足底与地面支撑过程建模使用Hunt-Crossly碰撞模型描述，利用公式表达。

其中，表示足端法向量支撑力，K表示足端等效刚度，N表示足端等效阻尼，/>表示足端变形量，m1、n1、p分别是待定的阻尼项指数。法向足端等效刚度K表示为。

对于四足机器人而言，足端法向支撑力由足底多重力传感器实时读取，C是足部阻尼弹簧通过外部测量获取，足端沉陷量是通过检测腿部各关节编码器读数进行运动学结算，上述三个变量可当作已知量。

待定的阻尼项指数m1、n1、p通过附加不同的载荷实验，可以辨别准确的阻尼项指数。

所述切向摩擦系数：四足机器人足底在地面切向方向上相对运动时会产生切向摩擦力，地面会对足底产生反向作用力即足底推进力。足底切向摩擦力的大小受到法向支撑力的影响，当四足机器人足底与地面产生相对滑动时，足底切向摩擦力的预测可以使用Coulomb摩擦模型进行预测。具体预测公式如下：

式中的表示切向摩擦系数，/>表示切向摩擦力，/>表示法向支撑力，/>表示切向相对滑动速度。

，/>、/>分别由足底多重力传感器在法向和切向方向实时读取，足底末速度/>由足部电机编码器进行运动学求解。

步骤S5：基于地形几何特征以及四足机器人足接触力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；

可达性分析：在定义及计算了地形几何特征：坡度S、曲率C、粗糙度R和起伏度M，以及足底接触力特征：法向足端等效刚度K、切向摩擦系数之后，开始对地形的可达性进行分析并进行定量计算。栅格/>处的可达性得分/>，此值描述了栅格的可通行程度，/>时表示该栅格地形适宜通行，当/>时表示该栅格地形不适合通行。

具体地，栅格的可通行得分由以下公式给出：

其中，分别为几何特征项和足端接触力特征项的权重系数，分别为对应的几何特征的阈值，/>分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，该处栅格直接被判为不可达。

步骤S6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。

具体地，根据建立的2.5D高程栅格地图以及栅格的可达性得分地图相结合，栅格中每一个地图的高程值代表该栅格的高度，如果高度大于四足机器人可通行的最大高度阈值，则栅格设置为不可通行，栅格的可通过得分直接设置为0（不可通过）；如果栅格高度小于四足可通行的最大高度阈值，进行几何地形分析和接触力分析，计算地形几何特征参数以及足端接触力特征参数，融合评估栅格的可达性得分score。栅格可达得分代表该处的可到达程度，综合全局的地形可达性分数，就得到了四足机器人基于地形评价的可达性地图。

所述基于地形几何特征评估的四足机器人可达性地图构建系统，包括：

模块M1：四足机器人基于头部携带的RGB-D相机进行特征点提取、相邻帧的图像匹配，同时融合配置的IMU三维加速度信息，建立视觉-惯性里程计，实时估计四足机器人的位置；

具体地，所述模块M1采用：根据深度相机所采集的深度和图像，对相邻的两帧图像进行ICP匹配，获取相机在地图坐标系中的位置和姿态。基于点云间的距离计算两个点云块的匹配，基于匹配结果求解计算点云间的位姿变换，然后再基于求解的位姿变换进一步迭代匹配的位姿，根据观测进行全局优化，得到局部最小误差的的位姿。

模块M2：基于RGB-D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿，建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值，并将高度信息记录到栅格地图中；

具体地，所述模块M2采用：

模块M2.1：在地图构建算法中，定义四个坐标系，分别为：惯性坐标系I，机器人质心坐标系O，相机坐标系C以及地图参考坐标系M。根据RGB信息和深度信息，建立点云深度图像，将深度图像用于转换为地图中点云的高度信息。

更为具体地，相机对点云中一点P的测量值为，M下的高度信息p，从三维点云信息中高度数据为：/>；

其中，分别表示相机坐标系C相对于地图参考坐标系M的旋转矩阵和平移向量。/>表示投影向量。考虑到相机测量存在的噪声，测量值p对相机位姿和测量值/>存在扰动噪声。本发明假设测量高度值p存在噪声，且服从高斯分布，方差/>通过协方差矩阵进行更新。

所述协方差更新公式为：

其中，是深度相机的噪声协方差矩阵，/>是由于深度相机旋转不确定性带来的协方差矩阵。

高度测量值作为一阶卡尔曼滤波观测量，更新上一次的估计值/>，得到当前的估计值/>，下次高度的估计值和方差更新公式为：

多组数据进行筛选原则：测量过程中，如果同一个栅格存在多组不同的高度数据，根据马氏距离对测量数据进行筛选，具体做法是：计算两组数据的相似程度，在每一次跟新测量时，忽略比当前高度小的数据集，取最大高度测量值作为该栅格的高度信息。

自此，根据深度信息构建了三维点云信息，再从点云数据中重新恢复了高度信息，建立了基于栅格的2.5D高程地图。

模块M3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；

具体地，如果估计的栅格高度小于四足机器人可通行的最大高度阈值，则进行几何地形分析，基于三维点云地图中的点云数据，计算地形的几何特征。地形的几何特征主要考虑以下四点：坡度S、曲率C、粗糙度R和起伏度M。以栅格地图中栅格为单位，根据栅格位置中点云集来获取该栅格处的地形几何特征信息，使四足机器人在面对台阶、崎岖不平的道路时，更好的计算起伏性，减小碰撞可能性，增加路况的可通行性。

所述坡度S计算：栅格的坡度描述了地形的一般倾斜角度，对于四足机器人而言，地形的坡度越大，机器人腿部打滑可能性越大，行走稳定性越差。基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法来获取栅格处点云集的中心法向量，然后再根据中心法向量与机器人中心坐标系Z轴之间的夹角来计算栅格的地形坡度。具体做法如下：

根据栅格点云集平面与其法向量垂直的中心思想，将求解法向量转化为最小二乘问题，将目标函数进行最小化计算，其目标函数是，其中n为平面法向量，/>为栅格内点云。m表示栅格中点云个数，T表示转置；进一步推导

其中，是一个/>的协方差矩阵。通过奇异值分解(Singular Value Decomposition, SVD)对目标求解即可得到平面法向量n。平面法向量n与Z轴（方向向量表示为/>）之间的夹角是栅格/>处的坡度值/>；

所述曲率C计算：栅格的曲率描述了地形的大致凹凸程度，适当的凹凸可以防止四足机器人运动过程中的足底打滑，但是过度的凹凸程度又会卡住足底，阻碍关节运动。在构建可通行地图过程中，需要评估栅格附近的曲率变化情况，具体的做法如下：

首先对栅格中的点云集/>进行集中化处理，得到矩阵/>：/>

再进一步根据集中化的矩阵计算点云集/>的协方差矩阵/>

再进行特征值分解：

其中，为协方差矩阵的特征值，/>为协方差矩阵的特征向量。栅格的曲率为/>。

所述粗糙度R计算：形容地形的地面的光滑程度，往往粗糙度越高，四足机器人的足底越不容易打滑，足端行走支撑更加稳定。粗糙度的评价是根据栅格内点云集拟合曲面的面积/>与单位栅格面积/>大小之差来确定，具体计算方法如下：

所述起伏度M计算：反映了地形相对高度差，当四足机器人在行走过程中遇到了台阶或者较高的障碍物时，足端的抬起及下落不仅需要当前地形有适合的坡度、曲率和粗糙度，还需要有足够的空间完成机械腿的抬起和下落动作。当面临较高的台阶时，机械腿可能在足端运动过程中碰到台阶边缘，导致自身重力失衡；又如当机械腿落足到一块狭小凹陷的缝隙路况区域时，比较容易会发生卡陷等情况。因此，利用起伏度来描述机器人周围环境可能发生碰撞概率是必要的。具体做法是，根据栅格的高度值/>与周围8个栅格的高度值/>比较，得到最大的高程值/>,以及最小的高程值/>，定义该处的起伏度/>。

模块M4：基于足底多重力传感器计算四足机器人足接触力特性；

所述足底多维力传感器能同时感知足底多维力、接触面温度及环境温度，机器人结构中的4个足底多重传感器能够测量水平面上全方向的力和力矩云图，水平力检测具备高密度识别能力，水平云图密度识别力的角度≤30°，水平力测量量程0-500N。足底传感器垂直方向上，足底多重力传感器也能同时实现拉力和压力测量，不仅满足四足机器人控制中所需的足底压力测量，还能进行机器人足端卡缝时拉伸力脱困测试，垂直方向上量程0-200公斤，测量精度小于1%，安全过载不大于150%，极限过载不大于200%。足底温度传感器量程为0-100℃，精度小于1%。

足底接触力特性计算：野外环境多为泥泞、湿滑的复杂地形，四足机器人在泥地、沙地上行走时足端会发生沉陷，过大沉陷量会使四足机器人丧失行动能力；在光滑潮湿的瓷砖、雪地及冰面上行进时，足底容易产生打滑现象，削弱运动能力。针对这种动态复杂的地形，基于四足机器人足底配置的足底多维力传感器，开展足底力法向足端等效刚度K和足底力切向摩擦系数研究，综合分析四足机器人足底接触力特性，评估地形的实际行走状态。

所述法向足端等效刚度K：反应四足机器人足底在接触不同地形时的沉陷程度。四足机器人足底与地面支撑过程建模使用Hunt-Crossly碰撞模型描述，利用公式表达。

其中，表示足端法向量支撑力，K表示足端等效刚度，N表示足端等效阻尼，/>表示足端变形量；m1、n1、p分别是待定的阻尼项指数。法向足端等效刚度K表示为。

对于四足机器人而言，足端法向支撑力由足底多重力传感器实时读取，C是足部阻尼弹簧通过外部测量获取，足端沉陷量是通过检测腿部各关节编码器读数进行运动学结算，上述三个变量可当作已知量。

待定的阻尼项指数m1、n1、p，通过附加不同的载荷实验，可以辨别准确的阻尼项指数。

所述切向摩擦系数：四足机器人足底在地面切向方向上相对运动时会产生切向摩擦力，地面会对足底产生反向作用力即足底推进力。足底切向摩擦力的大小受到法向支撑力的影响，当四足机器人足底与地面产生相对滑动时，足底切向摩擦力的预测可以使用Coulomb摩擦模型进行预测。具体预测公式如下：

式中的表示切向摩擦系数，/>表示切向摩擦力，/>表示法向支撑力，/>表示切向相对滑动速度。

，/>、/>分别由足底多重力传感器在法向和切向方向实时读取，足底末速度/>由足部电机编码器进行运动学求解。

模块M5：基于地形几何特征以及四足机器人足接触力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；

可达性分析：在定义及计算了地形几何特征：坡度S、曲率C、粗糙度R和起伏度M，以及足底接触力特征：法向足端等效刚度K、切向摩擦系数之后，开始对地形的可达性进行分析并进行定量计算。栅格/>处的可达性得分/>，此值描述了栅格的可通行程度，/>时表示该栅格地形适宜通行，当/>时表示该栅格地形不适合通行。

具体地，栅格的可通行得分由以下公式给出：

其中，分别为几何特征项和足端接触力特征项的权重系数，分别为对应的几何特征的阈值，/>分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，该处栅格直接被判为不可达。

模块M6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。

具体地，根据建立的2.5D高程栅格地图以及栅格的可达性得分地图相结合，栅格中每一个地图的高程值代表该栅格的高度，如果高度大于四足机器人可通行的最大高度阈值，则栅格设置为不可通行，栅格的可通过得分直接设置为0（不可通过）；如果栅格高度小于四足可通行的最大高度阈值，进行几何地形分析和接触力分析，计算地形几何特征参数以及足端接触力特征参数，融合评估栅格的可达性得分score。栅格可达得分代表该处的可到达程度，综合全局的地形可达性分数，就得到了四足机器人基于地形评价的可达性地图。

本发明利用四足机器人以RGB-D相机为主的外感知模块和足底多重传感器和IMU的内感知模块，不仅提出了地形坡度S、曲率C、粗糙度R和起伏度M的几何特征，还提出了四足机器人足端基于Hunt-Crossly碰撞模型的法向足端等效刚度K计算和基于Coulomb摩擦模型的足底力切向摩擦系数预测，为四足机器人评估复杂地形以及落足点选择提供了更加细致全面的评价指标。此外还建立了融合地形几何特征与足底接触力特征的四足机器人可达性地图，在栅格2D地图上增加可达性得分数据，较全面地评价了四足机器人平台在复杂地形可到达性能，较大减小地图存储数据量，有效减小四足机器人在路径规划以及落足点选择方面的地图数据的查找速度，提升机器人反应能力。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (8)

1.一种融合地形与力的四足机器人可达性地图构建方法，其特征在于，包括：

步骤S1：四足机器人通过融合RGB-D相机获取的视觉信息和IMU信息建立视觉-惯性里程计，基于视觉-惯性里程计实时估计四足机器人的位姿；

步骤S2：基于RGB-D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿，建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值，并将高度信息记录到栅格地图中；

步骤S3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；

步骤S4：基于足底多重力传感器计算四足机器人足接触力特性；

步骤S5：基于地形几何特征以及四足机器人足接触力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；

步骤S6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图；

所述步骤S2采用：在相邻帧中选取特征点具有代表性的关键帧，基于关键帧的RGB-D图像深度信息以及相关位姿估计信息，建立机器人全局点云地图并同时建立2D栅格地图以及进行栅格高度估计，建立栅格2.5D高程地图；

所述步骤S3采用：以栅格地图中栅格为单位，根据栅格位置中点云集来获取当前栅格处的地形几何特征；

地形几何特征包括：坡度S、曲率C、粗糙度R和起伏度M；

所述坡度S是基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法获取栅格处点云集的中心法向量，根据中心法向量与机器人中心坐标系Z轴之间的夹角计算栅格的地形坡度；

所述曲率C采用：对栅格中的点云集/>进行集中化处理得到矩阵/>，根据集中化的矩阵/>计算点云集/>的协方差矩阵/>，再对协方差矩阵/>进行特征值分解，基于协方差矩阵的特征值计算栅格的曲率；

所述粗糙度R是利用栅格内点云集拟合曲面的面积与单位栅格面积大小之差确定粗糙度；

所述起伏度M是根据栅格的高度值与周围8个栅格的高度值比较，得到最大的高程值以及最小的高程值，基于最大的高程值和最小高程值之差确定起伏度；

所述坡度S计算：基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法来获取栅格处点云集的中心法向量，然后再根据中心法向量与机器人中心坐标系Z轴之间的夹角来计算栅格的地形坡度；具体做法如下：

根据栅格点云集平面与其法向量垂直的中心思想，将求解法向量转化为最小二乘问题，将目标函数进行最小化计算，其目标函数是f，其中n为平面法向量，/>为栅格内点云；m表示栅格中点云个数，T表示转置；进一步推导

f

其中，是一个/>的协方差矩阵；通过奇异值分解对目标求解即可得到平面法向量n；平面法向量n与Z轴之间的夹角是栅格/>处的坡度值/>,Z轴方向向量表示为/>；

所述曲率C计算：在构建可通行地图过程中，需要评估栅格附近的曲率变化情况，具体的做法如下：

首先对栅格中的点云集/>进行集中化处理，得到矩阵/>：

再进一步根据集中化的矩阵计算点云集/>的协方差矩阵/>

再进行特征值分解：

其中，为协方差矩阵的特征值，/>为协方差矩阵的特征向量；栅格的曲率为/>；

所述粗糙度R计算：粗糙度的评价是根据栅格内点云集/>拟合曲面的面积与单位栅格面积/>大小之差来确定，具体计算方法如下：

所述起伏度M计算：根据栅格的高度值/>与周围8个栅格的高度值/>比较，得到最大的高程值/>,以及最小的高程值/>，定义该处的起伏度。

2.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S1采用：通过RGB-D相机采集点云信息，基于ICP匹配结果求解计算点云间的位姿变换，再基于求解的位姿变换进一步迭代匹配的位置，根据观测值以及IMU测量值进行全局优化，得到全局最小误差的位姿。

3.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述足底多重力传感器能够同时感知足底多维力、接触面温度以及环境温度；

四足机器人的4个足底多重力传感器能够测量水平面上全方向的力和力矩云图；

足底多重力传感器在垂直方向上也能够同时实现拉力和压力测量。

4.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S4采用：基于足底多重力传感器分析法向足端等效刚度K和足底力切向摩擦系数；

所述法向足端等效刚度K反应四足机器人足底在接触不同地形时的沉陷程度；四足机器人足底与地面支撑过程使用Hunt-Crossly碰撞模型描述；

其中，表示足端法向量支撑力；K表示足端等效刚度；N表示足端等效阻尼；/>表示足端变形量；m1、n1、p分别是待定的阻尼项指数；/>表示一次求导；

所述足底力切向摩擦系数是基于Coulomb摩擦模型预测足底切向摩擦力获得的；

其中，表示足底力切向摩擦系数；/>表示切向摩擦力；/>表示法向支撑力；/>表示切向相对滑动速度。

5.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S5采用：

栅格处的可达性得分/>，当/>时表示该栅格地形适宜通行，当/>时表示该栅格地形不适合通行；

栅格的可通行得分采用：

其中，，/>分别为几何特征项和足端接触力特征项的权重系数，/>分别为对应的几何特征的阈值，/>分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，则相对应的栅格直接被判为不可达/>。

6.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S6采用：将建立的2.5D高程栅格地图以及栅格的可达性得分地图相结合，栅格中每一个地图的高程值代表该栅格的高度，如果栅格高度大于四足机器人可通行的最大高度阈值，则栅格设置为不可通行，栅格的可通过得分直接设置为0；如果栅格高度小于四足机器人可通行的最大高度阈值，进行几何地形分析和接触力分析，计算地形几何特征参数以及足端接触力特征参数，融合评估栅格的可达性得分score；综合全局的地形可达性分数，得到四足机器人基于地形评价的可达性地图。

7.一种融合地形与力的四足机器人可达性地图构建系统，其特征在于，包括：

模块M1：四足机器人通过融合RGB-D相机获取的视觉信息和IMU信息建立视觉-惯性里程计，基于视觉-惯性里程计实时估计四足机器人的位姿；

模块M2：基于RGB-D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿，建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值，并将高度信息记录到栅格地图中；

模块M3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；

模块M4：基于足底多重力传感器计算四足机器人足接触力特性；

模块M5：基于地形几何特征以及四足机器人足接触力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；

模块M6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图；

所述模块M2采用：在相邻帧中选取特征点具有代表性的关键帧，基于关键帧的RGB-D图像深度信息以及相关位姿估计信息，建立机器人全局点云地图并同时建立2D栅格地图以及进行栅格高度估计，建立栅格2.5D高程地图；

所述模块M3采用：以栅格地图中栅格为单位，根据栅格位置中点云集来获取当前栅格处的地形几何特征；

地形几何特征包括：坡度S、曲率C、粗糙度R和起伏度M；

所述坡度S是基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法获取栅格处点云集的中心法向量，根据中心法向量与机器人中心坐标系Z轴之间的夹角计算栅格的地形坡度；

所述曲率C采用：对栅格中的点云集/>进行集中化处理得到矩阵/>，根据集中化的矩阵/>计算点云集/>的协方差矩阵/>，再对协方差矩阵/>进行特征值分解，基于协方差矩阵的特征值计算栅格的曲率；

所述粗糙度R是利用栅格内点云集拟合曲面的面积与单位栅格面积大小之差确定粗糙度；

所述起伏度M是根据栅格的高度值与周围8个栅格的高度值比较，得到最大的高程值以及最小的高程值，基于最大的高程值和最小高程值之差确定起伏度；

所述坡度S计算：基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法来获取栅格处点云集的中心法向量，然后再根据中心法向量与机器人中心坐标系Z轴之间的夹角来计算栅格的地形坡度；具体做法如下：

根据栅格点云集平面与其法向量垂直的中心思想，将求解法向量转化为最小二乘问题，将目标函数进行最小化计算，其目标函数是f，其中n为平面法向量，/>为栅格内点云；m表示栅格中点云个数，T表示转置；进一步推导

f

其中，是一个/>的协方差矩阵；通过奇异值分解对目标求解即可得到平面法向量n；平面法向量n与Z轴之间的夹角是栅格/>处的坡度值/>，Z轴的方向向量表示为/>；

所述曲率C计算：在构建可通行地图过程中，需要评估栅格附近的曲率变化情况，具体的做法如下：

首先对栅格中的点云集/>进行集中化处理，得到矩阵/>：

再进一步根据集中化的矩阵计算点云集/>的协方差矩阵/>

再进行特征值分解：

其中，为协方差矩阵的特征值，/>为协方差矩阵的特征向量；栅格的曲率为/>；

所述粗糙度R计算：粗糙度的评价是根据栅格内点云集/>拟合曲面的面积与单位栅格面积/>大小之差来确定，具体计算方法如下：

所述起伏度M计算：根据栅格的高度值/>与周围8个栅格的高度值/>比较，得到最大的高程值/>,以及最小的高程值/>，定义该处的起伏度。

8.根据权利要求7所述的融合地形与力的四足机器人可达性地图构建系统，其特征在于，所述足底多重力传感器能够同时感知足底多维力、接触面温度以及环境温度；

四足机器人的4个足底多重力传感器能够测量水平面上全方向的力和力矩云图；

足底多重力传感器在垂直方向上也能够同时实现拉力和压力测量；

所述模块M4采用：基于足底多重力传感器分析法向足端等效刚度K和足底力切向摩擦系数；

所述法向足端等效刚度K反应四足机器人足底在接触不同地形时的沉陷程度；四足机器人足底与地面支撑过程使用Hunt-Crossly碰撞模型描述；

其中，表示足端法向量支撑力；K表示足端等效刚度；N表示足端等效阻尼；/>表示足端变形量；m1、n1、p分别是待定的阻尼项指数；/>表示一次求导；

所述足底力切向摩擦系数是基于Coulomb摩擦模型预测足底切向摩擦力获得的；

其中，表示足底力切向摩擦系数；/>表示切向摩擦力；/>表示法向支撑力；/>表示切向相对滑动速度；

所述模块M5采用：

栅格处的可达性得分/>，当/>时表示该栅格地形适宜通行，当/>时表示该栅格地形不适合通行；

栅格的可通行得分采用：

其中，，/>分别为几何特征项和足端接触力特征项的权重系数，/>分别为对应的几何特征的阈值，/>分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，则相对应的栅格直接被判为不可达/>。