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

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

Info

Publication number
CN116147642A
CN116147642A CN202310384282.9A CN202310384282A CN116147642A CN 116147642 A CN116147642 A CN 116147642A CN 202310384282 A CN202310384282 A CN 202310384282A CN 116147642 A CN116147642 A CN 116147642A
Authority
CN
China
Prior art keywords
grid
map
force
terrain
robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310384282.9A
Other languages
English (en)
Other versions
CN116147642B (zh
Inventor
李智军
王冲
李国欣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Science and Technology of China USTC
Original Assignee
University of Science and Technology of China USTC
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Science and Technology of China USTC filed Critical University of Science and Technology of China USTC
Priority to CN202310384282.9A priority Critical patent/CN116147642B/zh
Publication of CN116147642A publication Critical patent/CN116147642A/zh
Application granted granted Critical
Publication of CN116147642B publication Critical patent/CN116147642B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01V—GEOPHYSICS; GRAVITATIONAL MEASUREMENTS; DETECTING MASSES OR OBJECTS; TAGS
    • G01V7/00—Measuring gravitational fields or waves; Gravimetric prospecting or detecting
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/04—Context-preserving transformations, e.g. by using an importance map
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/64—Analysis of geometric attributes of convexity or concavity
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2200/00—Indexing scheme for image data processing or generation, in general
    • G06T2200/08—Indexing scheme for image data processing or generation, in general involving all processing steps from image acquisition to 3D model generation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10024—Color image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Geometry (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Software Systems (AREA)
  • Automation & Control Theory (AREA)
  • Geophysics (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Graphics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

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采用：以栅格地图中栅格为单位，根据栅格位置
Figure SMS_1
中点云集来获取当前栅格处的地形几何特征；
地形几何特征包括：坡度S、曲率C、粗糙度R和起伏度M；
所述坡度S是基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法获取栅格处点云集的中心法向量，根据中心法向量与机器人中心坐标系Z轴之间的夹角计算栅格的地形坡度；
所述曲率C采用：对栅格
Figure SMS_2
中的点云集
Figure SMS_3
进行集中化处理得到矩阵
Figure SMS_4
，根据集中化的矩阵
Figure SMS_5
计算点云集
Figure SMS_6
的协方差矩阵
Figure SMS_7
，再对协方差矩阵
Figure SMS_8
进行特征值分解，基于协方差矩阵的特征值计算栅格的曲率；
所述粗糙度R是利用栅格内点云集拟合曲面的面积与单位栅格面积大小之差确定粗糙度；
所述起伏度M是根据栅格的高度值与周围8个栅格的高度值比较，得到最大的高程值以及最小的高程值，基于最大的高程值和最小高程值之差确定起伏度。
优选地，所述足底多重力传感器能够同时感知足底多维力、接触面温度以及环境温度；
四足机器人的4个足底多重力传感器能够测量水平面上全方向的力和力矩云图；
足底多重力传感器在垂直方向上也能够同时实现拉力和压力测量。
优选地，所述步骤S4采用：基于足底多重力传感器分析法向足端等效刚度K和足底力切向摩擦系数
Figure SMS_9
；
所述法向足端等效刚度K反应四足机器人足底在接触不同地形时的沉陷程度；四足机器人足底与地面支撑过程使用Hunt-Crossly碰撞模型描述；
Figure SMS_10
其中，
Figure SMS_11
表示足端法向量支撑力；K表示足端等效刚度；C表示足端等效阻尼；
Figure SMS_12
表示足端变形量；m、n、p分别是待定的阻尼项指数；
所述足底力切向摩擦系数
Figure SMS_13
是基于Coulomb摩擦模型预测足底切向摩擦力获得的；
Figure SMS_14
其中，
Figure SMS_15
表示足底力切向摩擦系数；
Figure SMS_16
表示切向摩擦力；
Figure SMS_17
表示法向支撑力；
Figure SMS_18
表示切向相对滑动速度。
优选地，所述步骤S5采用：
栅格
Figure SMS_19
处的可达性得分
Figure SMS_20
，当
Figure SMS_21
时表示该栅格地形适宜通行，当
Figure SMS_22
时表示该栅格地形不适合通行；
栅格的可通行得分采用：
Figure SMS_23
其中，
Figure SMS_24
，
Figure SMS_25
分别为几何特征项和足端接触力特征项的权重系数，
Figure SMS_26
分别为对应的几何特征的阈值，
Figure SMS_27
分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，该处栅格直接被判为不可达
Figure SMS_28
。
优选地，所述步骤S6采用：将建立的2.5D高程栅格地图以及栅格的可达性得分地图相结合，栅格中每一个地图的高程值代表该栅格的高度，如果栅格高度大于四足机器人可通行的最大高度阈值，则栅格设置为不可通行，栅格的可通过得分直接设置为0；如果栅格高度小于四足机器人可通行的最大高度阈值，进行几何地形分析和接触力分析，计算地形几何特征参数以及足端接触力特征参数，融合评估栅格的可达性得分score；综合全局的地形可达性分数，得到四足机器人基于地形评价的可达性地图。
根据本发明提供的一种融合地形与力特征的四足机器人可达性地图构建系统，包括：
模块M1：四足机器人通过融合RGB-D相机获取的视觉信息和IMU信息建立视觉-惯性里程计，基于视觉-惯性里程计实时估计四足机器人的位姿；
模块M2：基于RGB-D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿，建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值，并将高度信息记录到栅格地图中；
模块M3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；
模块M4：基于足底多重力传感器计算四足机器人足接触力特性；
模块M5：基于地形几何特征以及四足机器人足端力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；
模块M6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。
优选地，所述模块M3采用：以栅格地图中栅格为单位，根据栅格位置
Figure SMS_29
中点云集来获取当前栅格处的地形几何特征；
地形几何特征包括：坡度S、曲率C、粗糙度R和起伏度M；
所述坡度S是基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法获取栅格处点云集的中心法向量，根据中心法向量与机器人中心坐标系Z轴之间的夹角计算栅格的地形坡度；
所述曲率C采用：对栅格
Figure SMS_30
中的点云集
Figure SMS_31
进行集中化处理得到矩阵
Figure SMS_32
，根据集中化的矩阵
Figure SMS_33
计算点云集
Figure SMS_34
的协方差矩阵
Figure SMS_35
，再对协方差矩阵
Figure SMS_36
进行特征值分解，基于协方差矩阵的特征值计算栅格的曲率；
所述粗糙度R是利用栅格内点云集拟合曲面的面积与单位栅格面积大小之差确定粗糙度；
所述起伏度M是根据栅格的高度值与周围8个栅格的高度值比较，得到最大的高程值以及最小的高程值，基于最大的高程值和最小高程值之差确定起伏度。
优选地，所述足底多重力传感器能够同时感知足底多维力、接触面温度以及环境温度；
四足机器人的4个足底多重力传感器能够测量水平面上全方向的力和力矩云图；
足底多重力传感器在垂直方向上也能够同时实现拉力和压力测量；
所述模块M4采用：基于足底多重力传感器分析法向足端等效刚度K和足底力切向摩擦系数
Figure SMS_37
；
所述法向足端等效刚度K反应四足机器人足底在接触不同地形时的沉陷程度；四足机器人足底与地面支撑过程使用Hunt-Crossly碰撞模型描述；
Figure SMS_38
其中，
Figure SMS_39
表示足端法向量支撑力；K表示足端等效刚度；C表示足端等效阻尼；
Figure SMS_40
表示足端变形量；m、n、p分别是待定的阻尼项指数；
所述足底力切向摩擦系数
Figure SMS_41
是基于Coulomb摩擦模型预测足底切向摩擦力获得的；
Figure SMS_42
其中，
Figure SMS_43
表示足底力切向摩擦系数；
Figure SMS_44
表示切向摩擦力；
Figure SMS_45
表示法向支撑力；
Figure SMS_46
表示切向相对滑动速度；
所述模块M5采用：
栅格
Figure SMS_47
处的可达性得分
Figure SMS_48
，当
Figure SMS_49
时表示该栅格地形适宜通行，当
Figure SMS_50
时表示该栅格地形不适合通行；
栅格的可通行得分采用：
Figure SMS_51
其中，
Figure SMS_52
，
Figure SMS_53
分别为几何特征项和足端接触力特征项的权重系数，
Figure SMS_54
分别为对应的几何特征的阈值，
Figure SMS_55
分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，该处栅格直接被判为不可达
Figure SMS_56
。
与现有技术相比，本发明具有如下的有益效果：
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的测量值为
Figure SMS_57
，M下的高度信息p，从三维点云信息中高度数据为：
Figure SMS_58
；
其中，
Figure SMS_59
分别表示相机坐标系C相对于地图参考坐标系M的旋转矩阵和平移向量。
Figure SMS_60
表示投影向量。考虑到相机测量存在的噪声，测量值p对相机位姿
Figure SMS_61
和测量值
Figure SMS_62
存在扰动噪声。本发明假设测量高度值p存在噪声，且服从高斯分布
Figure SMS_63
，方差
Figure SMS_64
通过协方差矩阵进行更新。
所述协方差更新公式为：
Figure SMS_65
Figure SMS_66
其中，
Figure SMS_67
是深度相机的噪声协方差矩阵，
Figure SMS_68
是由于深度相机旋转不确定性带来的协方差矩阵。
高度测量值
Figure SMS_69
作为一阶卡尔曼滤波观测量，更新上一次的估计值
Figure SMS_70
，得到当前的估计值
Figure SMS_71
，下次高度的估计值和方差更新公式为：
Figure SMS_72
多组数据进行筛选原则：测量过程中，如果同一个栅格存在多组不同的高度数据，根据马氏距离对测量数据进行筛选，具体做法是：计算两组数据的相似程度，在每一次跟新测量时，忽略比当前高度小的数据集，取最大高度测量值作为该栅格的高度信息。
自此，根据深度信息构建了三维点云信息，再从点云数据中重新恢复了高度信息，建立了基于栅格的2.5D高程地图。
步骤S3：当栅格高度小于四足机器人可通行的最大高度阈值时，基于点云地图中的点云信息，进行栅格地图中的地形几何特征计算，地形几何特征包括坡度、曲率、粗糙度、起伏度。
具体地，如果估计的栅格高度小于四足机器人可通行的最大高度阈值，则进行几何地形分析，基于三维点云地图中的点云数据，计算地形的几何特征。地形的几何特征主要考虑以下四点：坡度S、曲率C、粗糙度R和起伏度M。以栅格地图中栅格为单位，根据栅格位置
Figure SMS_73
中点云集来获取该栅格处的地形几何特征信息，使四足机器人在面对台阶、崎岖不平的道路时，更好的计算起伏性，减小碰撞可能性，增加路况的可通行性。
所述坡度S计算：栅格的坡度描述了地形的一般倾斜角度，对于四足机器人而言，地形的坡度越大，机器人腿部打滑可能性越大，行走稳定性越差。基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法来获取栅格处点云集的中心法向量，然后再根据中心法向量与机器人中心坐标系Z轴之间的夹角来计算栅格的地形坡度。具体做法如下：
根据栅格点云集平面与其法向量垂直的中心思想，将求解法向量转化为最小二乘问题，将目标函数进行最小化计算，其目标函数是f
Figure SMS_74
，其中n为平面法向量，
Figure SMS_75
为栅格内点云。m表示栅格中点云个数，T表示转置；进一步推导
f
Figure SMS_76
其中，
Figure SMS_77
是一个
Figure SMS_78
的协方差矩阵。通过奇异值分解(Singular Value Decomposition, SVD)对目标求解即可得到平面法向量n。平面法向量n与Z轴（方向向量表示为
Figure SMS_79
）之间的夹角是栅格
Figure SMS_80
处的坡度值
Figure SMS_81
；
Figure SMS_82
所述曲率C计算：栅格的曲率描述了地形的大致凹凸程度，适当的凹凸可以防止四足机器人运动过程中的足底打滑，但是过度的凹凸程度又会卡住足底，阻碍关节运动。在构建可通行地图过程中，需要评估栅格附近的曲率变化情况，具体的做法如下：
首先对栅格
Figure SMS_83
中的点云集
Figure SMS_84
进行集中化处理，得到矩阵
Figure SMS_85
：
Figure SMS_86
再进一步根据集中化的矩阵
Figure SMS_87
计算点云集
Figure SMS_88
的协方差矩阵
Figure SMS_89
Figure SMS_90
再进行特征值分解：
Figure SMS_91
其中，
Figure SMS_92
为协方差矩阵的特征值，
Figure SMS_93
为协方差矩阵的特征向量。栅格的曲率为
Figure SMS_94
。
所述粗糙度R计算：形容地形的地面的光滑程度，往往粗糙度越高，四足机器人的足底越不容易打滑，足端行走支撑更加稳定。粗糙度的评价是根据栅格
Figure SMS_95
内点云集
Figure SMS_96
拟合曲面的面积
Figure SMS_97
与单位栅格面积
Figure SMS_98
大小之差来确定，具体计算方法如下：
Figure SMS_99
所述起伏度M计算：反映了地形相对高度差，当四足机器人在行走过程中遇到了台阶或者较高的障碍物时，足端的抬起及下落不仅需要当前地形有适合的坡度、曲率和粗糙度，还需要有足够的空间完成机械腿的抬起和下落动作。当面临较高的台阶时，机械腿可能在足端运动过程中碰到台阶边缘，导致自身重力失衡；又如当机械腿落足到一块狭小凹陷的缝隙路况区域时，比较容易会发生卡陷等情况。因此，利用起伏度来描述机器人周围环境可能发生碰撞概率是必要的。具体做法是，根据栅格
Figure SMS_100
的高度值
Figure SMS_101
与周围8个栅格的高度值
Figure SMS_102
比较，得到最大的高程值
Figure SMS_103
,以及最小的高程值
Figure SMS_104
，定义该处的起伏度
Figure SMS_105
。
步骤S4：针对动态复杂的野外地形，基于四足机器人足端配置的足底多维力传感器，开展足底力法向足端等效刚度K和足底力切向摩擦系数
Figure SMS_106
研究，综合分析四足机器人足底接触力特性；
所述足底多维力传感器能同时感知足底多维力、接触面温度及环境温度，机器人结构中的4个足底多重传感器能够测量水平面上全方向的力和力矩云图，水平力检测具备高密度识别能力，水平云图密度识别力的角度≤30°，水平力测量量程0-500N。足底传感器垂直方向上，足底多重力传感器也能同时实现拉力和压力测量，不仅满足四足机器人控制中所需的足底压力测量，还能进行机器人足端卡缝时拉伸力脱困测试，垂直方向上量程0-200公斤，测量精度小于1%，安全过载不大于150%，极限过载不大于200%。足底温度传感器量程为0-100℃，精度小于1%。
足底接触力特性计算：野外环境多为泥泞、湿滑的复杂地形，四足机器人在泥地、沙地上行走时足端会发生沉陷，过大沉陷量会使四足机器人丧失行动能力；在光滑潮湿的瓷砖、雪地及冰面上行进时，足底容易产生打滑现象，削弱运动能力。针对这种动态复杂的地形，基于四足机器人足底配置的足底多维力传感器，开展足底力法向足端等效刚度K和足底力切向摩擦系数
Figure SMS_107
研究，综合分析四足机器人足底接触力特性，评估地形的实际行走状态。
所述法向足端等效刚度K：反应四足机器人足底在接触不同地形时的沉陷程度。四足机器人足底与地面支撑过程建模使用Hunt-Crossly碰撞模型描述，利用公式
Figure SMS_108
表达。
其中，
Figure SMS_109
表示足端法向量支撑力，K表示足端等效刚度，C表示足端等效阻尼，
Figure SMS_110
表示足端变形量，m、n、p分别是待定的阻尼项指数。法向足端等效刚度K表示为
Figure SMS_111
。
对于四足机器人而言，
Figure SMS_112
足端法向支撑力由足底多重力传感器实时读取，C是足部阻尼弹簧通过外部测量获取，足端沉陷量是通过检测腿部各关节编码器读数进行运动学结算，上述三个变量可当作已知量。
待定的阻尼项指数m、n、p，通过附加不同的载荷实验，可以辨别准确的阻尼项指数。
所述切向摩擦系数
Figure SMS_113
：四足机器人足底在地面切向方向上相对运动时会产生切向摩擦力，地面会对足底产生反向作用力即足底推进力。足底切向摩擦力的大小受到法向支撑力的影响，当四足机器人足底与地面产生相对滑动时，足底切向摩擦力的预测可以使用Coulomb摩擦模型进行预测。具体预测公式如下：
Figure SMS_114
式中的
Figure SMS_115
表示切向摩擦系数，
Figure SMS_116
表示切向摩擦力，
Figure SMS_117
表示法向支撑力，
Figure SMS_118
表示切向相对滑动速度。
Figure SMS_119
，
Figure SMS_120
、
Figure SMS_121
分别由足底多重力传感器在法向和切向方向实时读取，足底末速度
Figure SMS_122
由足部电机编码器进行运动学求解。
步骤S5：基于地形几何特征以及四足机器人足端力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；
可达性分析：在定义及计算了地形几何特征：坡度S、曲率C、粗糙度R和起伏度M，以及足底接触力特征：法向足端等效刚度K、切向摩擦系数
Figure SMS_123
之后，开始对地形的可达性进行分析并进行定量计算。栅格
Figure SMS_124
处的可达性得分
Figure SMS_125
，此值描述了栅格的可通行程度，
Figure SMS_126
时表示该栅格地形适宜通行，当
Figure SMS_127
时表示该栅格地形不适合通行。
具体地，栅格的可通行得分由以下公式给出：
Figure SMS_128
其中
Figure SMS_129
，
Figure SMS_130
分别为几何特征项和足端接触力特征项的权重系数，
Figure SMS_131
分别为对应的几何特征的阈值，
Figure SMS_132
分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，该处栅格直接被判为不可达
Figure SMS_133
。
步骤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的测量值为
Figure SMS_134
，M下的高度信息p，从三维点云信息中高度数据为：
Figure SMS_135
；
其中，
Figure SMS_136
分别表示相机坐标系C相对于地图参考坐标系M的旋转矩阵和平移向量。
Figure SMS_137
表示投影向量。考虑到相机测量存在的噪声，测量值p对相机位姿
Figure SMS_138
和测量值
Figure SMS_139
存在扰动噪声。本发明假设测量高度值p存在噪声，且服从高斯分布
Figure SMS_140
，方差
Figure SMS_141
通过协方差矩阵进行更新。
所述协方差更新公式为：
Figure SMS_142
其中，
Figure SMS_143
是深度相机的噪声协方差矩阵，
Figure SMS_144
是由于深度相机旋转不确定性带来的协方差矩阵。
高度测量值
Figure SMS_145
作为一阶卡尔曼滤波观测量，更新上一次的估计值
Figure SMS_146
，得到当前的估计值
Figure SMS_147
，下次高度的估计值和方差更新公式为：
Figure SMS_148
多组数据进行筛选原则：测量过程中，如果同一个栅格存在多组不同的高度数据，根据马氏距离对测量数据进行筛选，具体做法是：计算两组数据的相似程度，在每一次跟新测量时，忽略比当前高度小的数据集，取最大高度测量值作为该栅格的高度信息。
自此，根据深度信息构建了三维点云信息，再从点云数据中重新恢复了高度信息，建立了基于栅格的2.5D高程地图。
模块M3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；
具体地，如果估计的栅格高度小于四足机器人可通行的最大高度阈值，则进行几何地形分析，基于三维点云地图中的点云数据，计算地形的几何特征。地形的几何特征主要考虑以下四点：坡度S、曲率C、粗糙度R和起伏度M。以栅格地图中栅格为单位，根据栅格位置
Figure SMS_149
中点云集来获取该栅格处的地形几何特征信息，使四足机器人在面对台阶、崎岖不平的道路时，更好的计算起伏性，减小碰撞可能性，增加路况的可通行性。
所述坡度S计算：栅格的坡度描述了地形的一般倾斜角度，对于四足机器人而言，地形的坡度越大，机器人腿部打滑可能性越大，行走稳定性越差。基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法来获取栅格处点云集的中心法向量，然后再根据中心法向量与机器人中心坐标系Z轴之间的夹角来计算栅格的地形坡度。具体做法如下：
根据栅格点云集平面与其法向量垂直的中心思想，将求解法向量转化为最小二乘问题，将目标函数进行最小化计算，其目标函数是
Figure SMS_150
，其中n为平面法向量，
Figure SMS_151
为栅格内点云。m表示栅格中点云个数，T表示转置；进一步推导
Figure SMS_152
其中，
Figure SMS_153
是一个
Figure SMS_154
的协方差矩阵。通过奇异值分解(SingularValue Decomposition, SVD)对目标求解即可得到平面法向量n。平面法向量n与Z轴（方向向量表示为
Figure SMS_155
）之间的夹角是栅格
Figure SMS_156
处的坡度值
Figure SMS_157
；
Figure SMS_158
所述曲率C计算：栅格的曲率描述了地形的大致凹凸程度，适当的凹凸可以防止四足机器人运动过程中的足底打滑，但是过度的凹凸程度又会卡住足底，阻碍关节运动。在构建可通行地图过程中，需要评估栅格附近的曲率变化情况，具体的做法如下：
首先对栅格
Figure SMS_159
中的点云集
Figure SMS_160
进行集中化处理，得到矩阵
Figure SMS_161
：
Figure SMS_162
再进一步根据集中化的矩阵
Figure SMS_163
计算点云集
Figure SMS_164
的协方差矩阵
Figure SMS_165
Figure SMS_166
再进行特征值分解：
Figure SMS_167
其中，
Figure SMS_168
为协方差矩阵的特征值，
Figure SMS_169
为协方差矩阵的特征向量。栅格的曲率为
Figure SMS_170
。
所述粗糙度R计算：形容地形的地面的光滑程度，往往粗糙度越高，四足机器人的足底越不容易打滑，足端行走支撑更加稳定。粗糙度的评价是根据栅格
Figure SMS_171
内点云集
Figure SMS_172
拟合曲面的面积
Figure SMS_173
与单位栅格面积
Figure SMS_174
大小之差来确定，具体计算方法如下：
Figure SMS_175
所述起伏度M计算：反映了地形相对高度差，当四足机器人在行走过程中遇到了台阶或者较高的障碍物时，足端的抬起及下落不仅需要当前地形有适合的坡度、曲率和粗糙度，还需要有足够的空间完成机械腿的抬起和下落动作。当面临较高的台阶时，机械腿可能在足端运动过程中碰到台阶边缘，导致自身重力失衡；又如当机械腿落足到一块狭小凹陷的缝隙路况区域时，比较容易会发生卡陷等情况。因此，利用起伏度来描述机器人周围环境可能发生碰撞概率是必要的。具体做法是，根据栅格
Figure SMS_176
的高度值
Figure SMS_177
与周围8个栅格的高度值
Figure SMS_178
比较，得到最大的高程值
Figure SMS_179
,以及最小的高程值
Figure SMS_180
，定义该处的起伏度
Figure SMS_181
。
模块M4：基于足底多重力传感器计算四足机器人足接触力特性；
所述足底多维力传感器能同时感知足底多维力、接触面温度及环境温度，机器人结构中的4个足底多重传感器能够测量水平面上全方向的力和力矩云图，水平力检测具备高密度识别能力，水平云图密度识别力的角度≤30°，水平力测量量程0-500N。足底传感器垂直方向上，足底多重力传感器也能同时实现拉力和压力测量，不仅满足四足机器人控制中所需的足底压力测量，还能进行机器人足端卡缝时拉伸力脱困测试，垂直方向上量程0-200公斤，测量精度小于1%，安全过载不大于150%，极限过载不大于200%。足底温度传感器量程为0-100℃，精度小于1%。
足底接触力特性计算：野外环境多为泥泞、湿滑的复杂地形，四足机器人在泥地、沙地上行走时足端会发生沉陷，过大沉陷量会使四足机器人丧失行动能力；在光滑潮湿的瓷砖、雪地及冰面上行进时，足底容易产生打滑现象，削弱运动能力。针对这种动态复杂的地形，基于四足机器人足底配置的足底多维力传感器，开展足底力法向足端等效刚度K和足底力切向摩擦系数
Figure SMS_182
研究，综合分析四足机器人足底接触力特性，评估地形的实际行走状态。
所述法向足端等效刚度K：反应四足机器人足底在接触不同地形时的沉陷程度。四足机器人足底与地面支撑过程建模使用Hunt-Crossly碰撞模型描述，利用公式
Figure SMS_183
表达。
其中，
Figure SMS_184
表示足端法向量支撑力，K表示足端等效刚度，C表示足端等效阻尼，
Figure SMS_185
表示足端变形量，m、n、p分别是待定的阻尼项指数。法向足端等效刚度K表示为
Figure SMS_186
。
对于四足机器人而言，
Figure SMS_187
足端法向支撑力由足底多重力传感器实时读取，C是足部阻尼弹簧通过外部测量获取，足端沉陷量是通过检测腿部各关节编码器读数进行运动学结算，上述三个变量可当作已知量。
待定的阻尼项指数m、n、p，通过附加不同的载荷实验，可以辨别准确的阻尼项指数。
所述切向摩擦系数
Figure SMS_188
：四足机器人足底在地面切向方向上相对运动时会产生切向摩擦力，地面会对足底产生反向作用力即足底推进力。足底切向摩擦力的大小受到法向支撑力的影响，当四足机器人足底与地面产生相对滑动时，足底切向摩擦力的预测可以使用Coulomb摩擦模型进行预测。具体预测公式如下：
Figure SMS_189
式中的
Figure SMS_190
表示切向摩擦系数，
Figure SMS_191
表示切向摩擦力，
Figure SMS_192
表示法向支撑力，
Figure SMS_193
表示切向相对滑动速度。
Figure SMS_194
，
Figure SMS_195
、
Figure SMS_196
分别由足底多重力传感器在法向和切向方向实时读取，足底末速度
Figure SMS_197
由足部电机编码器进行运动学求解。
模块M5：基于地形几何特征以及四足机器人足端力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；
可达性分析：在定义及计算了地形几何特征：坡度S、曲率C、粗糙度R和起伏度M，以及足底接触力特征：法向足端等效刚度K、切向摩擦系数
Figure SMS_198
之后，开始对地形的可达性进行分析并进行定量计算。栅格
Figure SMS_199
处的可达性得分
Figure SMS_200
，此值描述了栅格的可通行程度，
Figure SMS_201
时表示该栅格地形适宜通行，当
Figure SMS_202
时表示该栅格地形不适合通行。
具体地，栅格的可通行得分由以下公式给出：
Figure SMS_203
其中
Figure SMS_204
，
Figure SMS_205
分别为几何特征项和足端接触力特征项的权重系数，
Figure SMS_206
分别为对应的几何特征的阈值，
Figure SMS_207
分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，该处栅格直接被判为不可达
Figure SMS_208
。
模块M6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。
具体地，根据建立的2.5D高程栅格地图以及栅格的可达性得分地图相结合，栅格中每一个地图的高程值代表该栅格的高度，如果高度大于四足机器人可通行的最大高度阈值，则栅格设置为不可通行，栅格的可通过得分直接设置为0（不可通过）；如果栅格高度小于四足可通行的最大高度阈值，进行几何地形分析和接触力分析，计算地形几何特征参数以及足端接触力特征参数，融合评估栅格的可达性得分score。栅格可达得分代表该处的可到达程度，综合全局的地形可达性分数，就得到了四足机器人基于地形评价的可达性地图。
本发明利用四足机器人以RGB-D相机为主的外感知模块和足底多重传感器和IMU的内感知模块，不仅提出了地形坡度S、曲率C、粗糙度R和起伏度M的几何特征，还提出了四足机器人足端基于Hunt-Crossly碰撞模型的法向足端等效刚度K计算和基于Coulomb摩擦模型的足底力切向摩擦系数预测，为四足机器人评估复杂地形以及落足点选择提供了更加细致全面的评价指标。此外还建立了融合地形几何特征与足底接触力特征的四足机器人可达性地图，在栅格2D地图上增加可达性得分数据，较全面地评价了四足机器人平台在复杂地形可到达性能，较大减小地图存储数据量，有效减小四足机器人在路径规划以及落足点选择方面的地图数据的查找速度，提升机器人反应能力。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种融合地形与力的四足机器人可达性地图构建方法，其特征在于，包括：
步骤S1：四足机器人通过融合RGB-D相机获取的视觉信息和IMU信息建立视觉-惯性里程计，基于视觉-惯性里程计实时估计四足机器人的位姿；
步骤S2：基于RGB-D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿，建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值，并将高度信息记录到栅格地图中；
步骤S3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；
步骤S4：基于足底多重力传感器计算四足机器人足接触力特性；
步骤S5：基于地形几何特征以及四足机器人足端力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；
步骤S6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。
2.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S1采用：通过RGB-D相机采集点云信息，基于ICP匹配结果求解计算点云间的位姿变换，再基于求解的位姿变换进一步迭代匹配的位置，根据观测值以及IMU测量值进行全局优化，得到全局最小误差的位姿。
3.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S3采用：以栅格地图中栅格为单位，根据栅格位置
Figure QLYQS_1
中点云集来获取当前栅格处的地形几何特征；
地形几何特征包括：坡度S、曲率C、粗糙度R和起伏度M；
所述坡度S是基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法获取栅格处点云集的中心法向量，根据中心法向量与机器人中心坐标系Z轴之间的夹角计算栅格的地形坡度；
所述曲率C采用：对栅格
Figure QLYQS_2
中的点云集
Figure QLYQS_3
进行集中化处理得到矩阵
Figure QLYQS_4
，根据集中化的矩阵
Figure QLYQS_5
计算点云集
Figure QLYQS_6
的协方差矩阵
Figure QLYQS_7
，再对协方差矩阵
Figure QLYQS_8
进行特征值分解，基于协方差矩阵的特征值计算栅格的曲率；
所述粗糙度R是利用栅格内点云集拟合曲面的面积与单位栅格面积大小之差确定粗糙度；
所述起伏度M是根据栅格的高度值与周围8个栅格的高度值比较，得到最大的高程值以及最小的高程值，基于最大的高程值和最小高程值之差确定起伏度。
4.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述足底多重力传感器能够同时感知足底多维力、接触面温度以及环境温度；
四足机器人的4个足底多重力传感器能够测量水平面上全方向的力和力矩云图；
足底多重力传感器在垂直方向上也能够同时实现拉力和压力测量。
5.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S4采用：基于足底多重力传感器分析法向足端等效刚度K和足底力切向摩擦系数
Figure QLYQS_9
；
所述法向足端等效刚度K反应四足机器人足底在接触不同地形时的沉陷程度；四足机器人足底与地面支撑过程使用Hunt-Crossly碰撞模型描述；
Figure QLYQS_10
其中，
Figure QLYQS_11
表示足端法向量支撑力；K表示足端等效刚度；C表示足端等效阻尼；
Figure QLYQS_12
表示足端变形量；m、n、p分别是待定的阻尼项指数；
Figure QLYQS_13
表示一次求导；
所述足底力切向摩擦系数
Figure QLYQS_14
是基于Coulomb摩擦模型预测足底切向摩擦力获得的；
Figure QLYQS_15
其中，
Figure QLYQS_16
表示足底力切向摩擦系数；
Figure QLYQS_17
表示切向摩擦力；
Figure QLYQS_18
表示法向支撑力；
Figure QLYQS_19
表示切向相对滑动速度。
6.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S5采用：
栅格
Figure QLYQS_20
处的可达性得分
Figure QLYQS_21
，当
Figure QLYQS_22
时表示该栅格地形适宜通行，当
Figure QLYQS_23
时表示该栅格地形不适合通行；
栅格的可通行得分采用：
Figure QLYQS_24
其中，
Figure QLYQS_25
，
Figure QLYQS_26
分别为几何特征项和足端接触力特征项的权重系数，
Figure QLYQS_27
分别为对应的几何特征的阈值，
Figure QLYQS_28
分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，则相对应的栅格直接被判为不可达
Figure QLYQS_29
。
7.根据权利要求1所述的融合地形与力的四足机器人可达性地图构建方法，其特征在于，所述步骤S6采用：将建立的2.5D高程栅格地图以及栅格的可达性得分地图相结合，栅格中每一个地图的高程值代表该栅格的高度，如果栅格高度大于四足机器人可通行的最大高度阈值，则栅格设置为不可通行，栅格的可通过得分直接设置为0；如果栅格高度小于四足机器人可通行的最大高度阈值，进行几何地形分析和接触力分析，计算地形几何特征参数以及足端接触力特征参数，融合评估栅格的可达性得分score；综合全局的地形可达性分数，得到四足机器人基于地形评价的可达性地图。
8.一种融合地形与力的四足机器人可达性地图构建系统，其特征在于，包括：
模块M1：四足机器人通过融合RGB-D相机获取的视觉信息和IMU信息建立视觉-惯性里程计，基于视觉-惯性里程计实时估计四足机器人的位姿；
模块M2：基于RGB-D相机采集到的环境中的彩色图像和深度图像以及当前四足机器人的估计位姿，建立基于深度测量值的三维点云地图；将三维点云地图转化为二维栅格地图，并在点云的基础上估计栅格中点云的高度值，并将高度信息记录到栅格地图中；
模块M3：当栅格高度小于四足机器人可通行的最大高度阈值时，则基于已建立的点云地图中的点云信息进行地图中的地形几何特征计算；
模块M4：基于足底多重力传感器计算四足机器人足接触力特性；
模块M5：基于地形几何特征以及四足机器人足端力特性进行四足机器人运动过程中复杂地形的可达性分析，更新地图中可达性得分；
模块M6：基于建立的栅格2.5D高程地图以及地形可达性得分建立可达性地图。
9.根据权利要求8所述的融合地形与力的四足机器人可达性地图构建系统，其特征在于，所述模块M3采用：以栅格地图中栅格为单位，根据栅格位置
Figure QLYQS_30
中点云集来获取当前栅格处的地形几何特征；
地形几何特征包括：坡度S、曲率C、粗糙度R和起伏度M；
所述坡度S是基于空间中的点云信息特点以及平面与法向量的关系，采用空间矢量法获取栅格处点云集的中心法向量，根据中心法向量与机器人中心坐标系Z轴之间的夹角计算栅格的地形坡度；
所述曲率C采用：对栅格
Figure QLYQS_31
中的点云集
Figure QLYQS_32
进行集中化处理得到矩阵
Figure QLYQS_33
，根据集中化的矩阵
Figure QLYQS_34
计算点云集
Figure QLYQS_35
的协方差矩阵
Figure QLYQS_36
，再对协方差矩阵
Figure QLYQS_37
进行特征值分解，基于协方差矩阵的特征值计算栅格的曲率；
所述粗糙度R是利用栅格内点云集拟合曲面的面积与单位栅格面积大小之差确定粗糙度；
所述起伏度M是根据栅格的高度值与周围8个栅格的高度值比较，得到最大的高程值以及最小的高程值，基于最大的高程值和最小高程值之差确定起伏度。
10.根据权利要求8所述的融合地形与力的四足机器人可达性地图构建系统，其特征在于，所述足底多重力传感器能够同时感知足底多维力、接触面温度以及环境温度；
四足机器人的4个足底多重力传感器能够测量水平面上全方向的力和力矩云图；
足底多重力传感器在垂直方向上也能够同时实现拉力和压力测量；
所述模块M4采用：基于足底多重力传感器分析法向足端等效刚度K和足底力切向摩擦系数
Figure QLYQS_38
；
所述法向足端等效刚度K反应四足机器人足底在接触不同地形时的沉陷程度；四足机器人足底与地面支撑过程使用Hunt-Crossly碰撞模型描述；
Figure QLYQS_39
其中，
Figure QLYQS_40
表示足端法向量支撑力；K表示足端等效刚度；C表示足端等效阻尼；
Figure QLYQS_41
表示足端变形量；m、n、p分别是待定的阻尼项指数；
Figure QLYQS_42
表示一次求导；
所述足底力切向摩擦系数
Figure QLYQS_43
是基于Coulomb摩擦模型预测足底切向摩擦力获得的；
Figure QLYQS_44
其中，
Figure QLYQS_45
表示足底力切向摩擦系数；
Figure QLYQS_46
表示切向摩擦力；
Figure QLYQS_47
表示法向支撑力；
Figure QLYQS_48
表示切向相对滑动速度；
所述模块M5采用：
栅格
Figure QLYQS_49
处的可达性得分
Figure QLYQS_50
，当
Figure QLYQS_51
时表示该栅格地形适宜通行，当
Figure QLYQS_52
时表示该栅格地形不适合通行；
栅格的可通行得分采用：
Figure QLYQS_53
其中，
Figure QLYQS_54
，
Figure QLYQS_55
分别为几何特征项和足端接触力特征项的权重系数，
Figure QLYQS_56
分别为对应的几何特征的阈值，
Figure QLYQS_57
分别为对应的足端接触力特征的阈值，当几何特征或者接触力特征大于对应特征的阈值时，则相对应的栅格直接被判为不可达
Figure QLYQS_58
。
CN202310384282.9A 2023-04-12 2023-04-12 融合地形与力的四足机器人可达性地图构建方法及系统 Active CN116147642B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310384282.9A CN116147642B (zh) 2023-04-12 2023-04-12 融合地形与力的四足机器人可达性地图构建方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310384282.9A CN116147642B (zh) 2023-04-12 2023-04-12 融合地形与力的四足机器人可达性地图构建方法及系统

Publications (2)

Publication Number Publication Date
CN116147642A true CN116147642A (zh) 2023-05-23
CN116147642B CN116147642B (zh) 2023-08-29

Family

ID=86356489

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310384282.9A Active CN116147642B (zh) 2023-04-12 2023-04-12 融合地形与力的四足机器人可达性地图构建方法及系统

Country Status (1)

Country Link
CN (1) CN116147642B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117557681A (zh) * 2024-01-09 2024-02-13 山东鲁邦地理信息工程有限公司 一种基于多源测绘数据的高精度地形图生成方法及装置
CN117611759A (zh) * 2023-11-30 2024-02-27 博雅达勘测规划设计集团有限公司 基于三维模型的评分地图生成方法、装置、终端及存储介质
CN117863190A (zh) * 2024-03-08 2024-04-12 广州小鹏汽车科技有限公司 足式机器人的移动控制方法及足式机器人
CN118050006A (zh) * 2024-04-16 2024-05-17 山东大学 一种四足机器人全局路径规划方法及系统
CN118545181A (zh) * 2024-07-26 2024-08-27 湖南大学 风力发电机叶片检测攀爬机器人及其运动控制方法
CN118545181B (zh) * 2024-07-26 2024-09-24 湖南大学 风力发电机叶片检测攀爬机器人及其运动控制方法

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104951601A (zh) * 2015-06-04 2015-09-30 大连理工大学 一种海冰-海洋结构相互作用的离散元高性能仿真系统
CN108009350A (zh) * 2017-11-30 2018-05-08 南京理工大学 一种复杂表面间接触碰撞参数识别方法
CN108303099A (zh) * 2018-06-14 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维视觉slam的无人机室内自主导航方法
CN109753632A (zh) * 2018-11-01 2019-05-14 北京理工大学 一种基于数据挖掘的表面粗糙度监测模型及构建方法
CN111123911A (zh) * 2019-11-22 2020-05-08 北京空间飞行器总体设计部 一种腿足式智能星表探测机器人感知系统及其工作方法
CN111596665A (zh) * 2020-05-29 2020-08-28 浙江大学 一种适用于腿足机器人规划的稠密高度地图构建方法
CN112212867A (zh) * 2020-10-19 2021-01-12 中国科学技术大学 一种机器人自定位与导航的方法及系统
CN112907491A (zh) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及系统
US20220206491A1 (en) * 2020-12-29 2022-06-30 Korea University Research And Business Foundation Mobile robot capable of evaluating self-training based traversability
CN114970186A (zh) * 2022-06-08 2022-08-30 哈尔滨理工大学 一种复杂地形中探测器高保真动力学仿真系统构建方法
CN115143964A (zh) * 2022-07-05 2022-10-04 中国科学技术大学 一种基于2.5d代价地图的四足机器人自主导航方法
CN115185266A (zh) * 2022-06-14 2022-10-14 重庆大学 一种基于地形可通过性的机器人控制方法及系统
WO2023273169A1 (zh) * 2021-06-30 2023-01-05 同济人工智能研究院(苏州)有限公司 一种融合视觉与激光的2.5d地图构建方法
CN115619953A (zh) * 2022-10-31 2023-01-17 山东大学 一种面向崎岖地形的移动机器人地形建图方法及系统
CN115639823A (zh) * 2022-10-27 2023-01-24 山东大学 崎岖起伏地形下机器人地形感知与移动控制方法及系统

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104951601A (zh) * 2015-06-04 2015-09-30 大连理工大学 一种海冰-海洋结构相互作用的离散元高性能仿真系统
CN108009350A (zh) * 2017-11-30 2018-05-08 南京理工大学 一种复杂表面间接触碰撞参数识别方法
CN108303099A (zh) * 2018-06-14 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维视觉slam的无人机室内自主导航方法
CN109753632A (zh) * 2018-11-01 2019-05-14 北京理工大学 一种基于数据挖掘的表面粗糙度监测模型及构建方法
CN111123911A (zh) * 2019-11-22 2020-05-08 北京空间飞行器总体设计部 一种腿足式智能星表探测机器人感知系统及其工作方法
CN111596665A (zh) * 2020-05-29 2020-08-28 浙江大学 一种适用于腿足机器人规划的稠密高度地图构建方法
CN112212867A (zh) * 2020-10-19 2021-01-12 中国科学技术大学 一种机器人自定位与导航的方法及系统
US20220206491A1 (en) * 2020-12-29 2022-06-30 Korea University Research And Business Foundation Mobile robot capable of evaluating self-training based traversability
CN112907491A (zh) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及系统
WO2023273169A1 (zh) * 2021-06-30 2023-01-05 同济人工智能研究院(苏州)有限公司 一种融合视觉与激光的2.5d地图构建方法
CN114970186A (zh) * 2022-06-08 2022-08-30 哈尔滨理工大学 一种复杂地形中探测器高保真动力学仿真系统构建方法
CN115185266A (zh) * 2022-06-14 2022-10-14 重庆大学 一种基于地形可通过性的机器人控制方法及系统
CN115143964A (zh) * 2022-07-05 2022-10-04 中国科学技术大学 一种基于2.5d代价地图的四足机器人自主导航方法
CN115639823A (zh) * 2022-10-27 2023-01-24 山东大学 崎岖起伏地形下机器人地形感知与移动控制方法及系统
CN115619953A (zh) * 2022-10-31 2023-01-17 山东大学 一种面向崎岖地形的移动机器人地形建图方法及系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
ZHIJUN LI;SHENGZHI XIAO;SHUZHI GE;HANG SU: "Constrained Multilegged Robot System Modeling and Fuzzy Control With Uncertain Kinematics and Dynamics Incorporating Foot Force Optimization", 《IEEE TRANSACTIONS ON SYSTEM,MAN,AND CYBEMETICS》, vol. 46, no. 1, pages 1 - 15, XP011594344, DOI: 10.1109/TSMC.2015.2422267 *
曾键;夏益民;蔡锦炜;方祺;杨子林;蔡奕峻;: "一种融合激光与视觉传感器的栅格地图构建方法", 工业控制计算机, no. 09, pages 95 - 97 *
朱晓辉: "轮式仿人机器人导航与路径规划研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 7, pages 140 - 444 *
贺威， 崔荣鑫, CHOON KI AHN, 李智军: "仿生机器人系统: 建模、设计、控制和感知 专题简介", 《中国科学：信息科学》, vol. 50, no. 7, pages 1125 - 1126 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117611759A (zh) * 2023-11-30 2024-02-27 博雅达勘测规划设计集团有限公司 基于三维模型的评分地图生成方法、装置、终端及存储介质
CN117557681A (zh) * 2024-01-09 2024-02-13 山东鲁邦地理信息工程有限公司 一种基于多源测绘数据的高精度地形图生成方法及装置
CN117557681B (zh) * 2024-01-09 2024-04-16 山东鲁邦地理信息工程有限公司 一种基于多源测绘数据的高精度地形图生成方法及装置
CN117863190A (zh) * 2024-03-08 2024-04-12 广州小鹏汽车科技有限公司 足式机器人的移动控制方法及足式机器人
CN118050006A (zh) * 2024-04-16 2024-05-17 山东大学 一种四足机器人全局路径规划方法及系统
CN118545181A (zh) * 2024-07-26 2024-08-27 湖南大学 风力发电机叶片检测攀爬机器人及其运动控制方法
CN118545181B (zh) * 2024-07-26 2024-09-24 湖南大学 风力发电机叶片检测攀爬机器人及其运动控制方法

Also Published As

Publication number Publication date
CN116147642B (zh) 2023-08-29

Similar Documents

Publication Publication Date Title
CN116147642B (zh) 融合地形与力的四足机器人可达性地图构建方法及系统
Fankhauser et al. Probabilistic terrain mapping for mobile robots with uncertain localization
CN112639502B (zh) 机器人位姿估计
Mutz et al. Large-scale mapping in complex field scenarios using an autonomous car
Maier et al. Real-time navigation in 3D environments based on depth camera data
CN110264572A (zh) 一种融合几何特性与力学特性的地形建模方法及系统
JP2018522345A (ja) リアルタイムのマッピングと位置確認のための方法及び装置
CN114442621A (zh) 一种基于四足机器人的自主探索和建图系统
Hata et al. Monte Carlo localization on Gaussian process occupancy maps for urban environments
CN115185266A (zh) 一种基于地形可通过性的机器人控制方法及系统
Schwendner et al. Using embodied data for localization and mapping
Ewen et al. These maps are made for walking: Real-time terrain property estimation for mobile robots
CN113741503A (zh) 一种自主定位式无人机及其室内路径自主规划方法
Parra-Tsunekawa et al. A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles
Haddeler et al. Traversability analysis with vision and terrain probing for safe legged robot navigation
Lobo et al. World feature detection and mapping using stereovision and inertial sensors
Purnawarman et al. The methodology for obtaining nonlinear and continuous three-dimensional topographic data using inertial and optical measuring instruments of unmanned ground systems
Zhang et al. Vision-assisted localization and terrain reconstruction with quadruped robots
Zhang et al. Supervoxel plane segmentation and multi-contact motion generation for humanoid stair climbing
Jordan et al. Real-time pose estimation on elevation maps for wheeled vehicles
Fong et al. Representing a 3-d environment with a 2 1/2-d map structure
Baek et al. A Mobile Robot Framework in Industrial Disaster for Human Rescue
Bhujbal et al. Probabilistic Method for Mapping & 3D SLAM of an off-road Terrain with Four Wheeled Robot
Castejón et al. Compact modeling technique for outdoor navigation
A Bekhti Traversability cost prediction of outdoor terrains for mobile robot using image features

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant