基于Cartographer和A*的四足机器人导航方案

REF：基于 ROS2 的足式机器人导航系统研究与优化

1. 国内外知名四足机器人

• 日本东京工业大学：
  • PV-Ⅱ：重 10kg，高约 1m，基于 3 自由度的比例缩放机构
  • TITAN-IV：结合姿态和触觉传感器，实现对环境的实时感知与适应，调整运动步态以应对不同地形

  

  d445ec08fcf0de6b896f354041827bea.png

• 韩国成均馆大学：
  • AiDIN I：具备出色的爬坡能力，能够有效避免翻滚和滑倒
  • AiDIN III：具有 0.35m/s 最大速度，能够以小跑步态稳定攀爬 20°的坡度，具备承载 3kg 重量的能力
  • AiDIN VIII：具有轻巧且运动范围广的腿部机构，可实现更动态的运动，采用视觉系统来识别地形和识别环境，采用扭矩可控的模块化执行器单元，将 BLDC 电机与关节扭矩传感器集成

  

  fa2d4be24b83eb425aaa6772a835d05d.png

• 波士顿动力公司：
  • BigDog：设计目的在于战场上协助士兵运输物资，山地和雪地等复杂地形中自如移动
  • 猎豹仿生四足机器人：卓越的室内奔跑能力，最高行进速度可达到 45km/h
  • Spotmini 四足机器人：机械臂重约 30kg，拥有优秀的 Trot 步态，能自主攀爬楼梯和应对复杂地形，同时具备多功能载荷能力以及出色的机械臂稳定与力控制性能

  

  2c6de522958a47a2fd70b3cd57931d25.png

• 苏黎世联邦理工学院： ANYmal，能够实时采集环境数据，建立高精度空间模型并完成自主定位，支持最优路径动态计算与障碍物智能规避

57b71a55f5fee0b7eae07183a14330a9.png

• MIT 仿生工程实验室：
  • Cheetah 3 系列标准版：卓越的运动能力，能自主跨越障碍物，以每小时 30 英里的高速奔跑
  • Mini 版：完成了后空翻，仅重 9 公斤，能够完全倒地重新站立，执行器来自航空模型，硬件成本总计3600 美元

  

  ef1a0c526e949b3bb0d25667fa29ddc7.png

• 宇树科技：
  • Go2 ：采用 ISS2.0智能伴随系统，采用无限矢量定位技术机控制技术，遥控距离大于 30 米，在平衡性和稳定性表现出色，跌倒能迅速自行恢复站立
  • Unitree B2-W：能完成托马斯全旋、侧空翻等动作，具备在陡峭山坡上快速滑行、浅水中疾走以及逆流爬坡的能力，充分展现了其优秀的运动能力和极强的环境适应能力

  

  84bff22471d34e232befb997a3a0eab2.png

• 杭州云深处科技：
  • 绝影系列：在电站、工厂、管廊巡检、以及应急救援、消防侦查等多种应用环境中落地应用
  • 山猫全地形机器人：搭配轮足形态，能攀爬 80cm 高台，5m/s 移动，上下 22cm 连续台阶，适应多种复杂地形

  

  e26230f9e8e5408f39832c8f086aa401.png

2. 四足机器人本体

• 腿部的构型： 常见的腿部构型包括全肘式、全膝式、前肘后膝式和前膝后肘式
  • 全肘式：腿部肘关节朝向后方，减少膝关节运动中与前方障碍物的碰撞风险，适合大步长的运动
  • 全膝式：膝关节朝前设计，执行高速行进、跳跃或下楼梯等复杂运动时，更大的运动灵活性和稳定
  • 前肘后膝式：采用对角线同步运动模式，降低了动态运动过程中的惯性力矩，适合复杂和崎岖的路面
  • 前膝后肘式：其腿部关节远离机身，为货物提供更大的承载空间，适用于多负载任务，载重小，速率低场景

  

  b14556a9045870dca768314bb8d91d06.png

  

  cc40fd2b0142eb39eede29a8cd6fc540.png

• 正运动学： 通过关节空间参数推导末端执行器的操作空间位姿，建立包含机身基准坐标系和腿部链式坐标系的完整参考系，运用 D-H 参数法推导从髋关节到足端的正向运动学方程

06c020c55ebf267d5ebf16e1629b8167.png

• 逆向运动学： 根据期望的足端位姿反解关节变量，先在三维任务空间内生成足端的连续运动路径，通过建立机体坐标系与各腿部关节坐标系的转换关系，计算对应的关节角度序列

e5e10a6f029e04c17b42f5c7959f4477.png

• 腿部轨迹空间： 建立连杆坐标系和齐次变换矩阵，为串联机构提供标准化建模流程，运动学建模仅需完成单腿分析即可推广至全部肢体

ec57723bc0e3a30a689155a37594ba5f.png

• 硬件平台： 树莓派 4B 作为主控单元，负责协调机器人的硬件和决策过程，包括硬件初始化、数据采集、决策处理、运动控制和实时调整几个步骤

b403d787e34ef0a74da531bd087c3e21.png

3. 经典激光SLAM建图算法

95089f7807b4e191ecc236450f6055ae.png

0ad0477c7b4aac00a1ad578d2d4ea830.png

• fast-slam： 将 SLAM 划分为两个独立的子任务：一是通过粒子滤波估计机器人位姿，二是通过扩展卡尔曼滤波（EKF）计算地图点的后验概率分布。这一方法通过结合粒子滤波与卡尔曼滤波的优势，使得机器人能够高效地进行定位与地图构建

45d979a93ca2eaafcf71c96bcea08faf.png

• Gmapping： 采用选择性重采样机制，基于粒子权重分布进行优先级排序，重点对低质量粒子实施重采样操作，避免了粒子退化现象

f395af9cedba5da19e7688ffe3c35742.png

• Karto： 采用高度方向优化和非迭代平方根分解技术，前端通过多帧数据融合构建局部子图，并运用相关性扫描匹配算法实现激光数据与局部地图的对齐，从而完成位姿估计；后端优化模块通过稀疏图优化，对运动轨迹和环境特征进行全局调整，减少累积误差，并对机器人的轨迹和环境地图进行调整；在闭环检测方面，通过构建候选数据帧的局部地图，并使用 scan-to-map 匹配策略，判断是否存在回环闭合

df0d9bc319a0ac93f52e1298a8635a72.png

• Cartographer： 多层级处理框架，前端进行传感器时空校准、运动预测和点云滤波预处理；采用动态子图分割策略，结合CSM 匹配与梯度下降优化，实现亚像素级配准精度；后端采用稀疏位姿图优化框架，通过多子图全局关联检测闭环，并应用分支限界算法优化搜索空间，提升闭环检测效率

d12fe8115cd21f751ccf743c9f5d3a9e.png

6d4643a5265fc51fffa3ff1ce1ef4b95.png

94550448f03fd7331265fc0917221eef.png

4. 优化 Cartographer 算法建图

Cartographer 算法采用两级优化架构，前端处理阶段首先建立栅格地图，通过位姿估计将激光雷达数据帧对齐，随后将这些数据整合到子地图（Submap）中形成局部优化结果；后端处理则基于扫描帧间的位姿约束关系，采用分支定界算法进行全局优化，在保证位姿优化精度的同时，降低了系统对计算资源的需求，使算法能够适应更大规模的场景建图需求。

• 均匀降采样： 通过空间网格划分实现点云密度的均匀降低，在空间中定义一个半径为 r 的球体，并选择该球体内距离球心最近的点作为代表点。
• 双边滤波： 通过空间位置和像素强度两个维度滤波，在去除噪声的同时有效地保持图像或点云的结构和细节

a6fac4051c7f044c4fe123261eb02955.png

• f(i，j)：表示图像在 q(i，j)处的像素值
• f(k，l)：为中心坐标点的像素值
• 𝜎：是像素值域上的高斯函数的标准差，用于控制像素的权值

2d0eda8762eaee6ce92353cbaf4b6338.png

• q(i，j)：窗口的其他系数坐标
• p(k，l)：为中心坐标点
• 𝜎：是空间域上的高斯核函数的标准差，用于控制空间邻近性对滤波器权值的影响
• 权值𝜔𝑑：定义域核，是通过空间高斯函数来衡量两个点之间的空间距离的影响

70abe2ca7807c4a80d75f53523d230dc.png

• 空间域核：根据空间距离加权，确保靠近的点对结果影响较大，其中：
• 值（强度）域核：依据像素或点的灰度值差异加权，从而确保颜色或强度差异较大的点对滤波结果的影响较小，其中：

• 曲率滤波降采样： 经过曲率滤波后的点云数据保留了重要特征与众多的点云，依托对曲率的筛选，曲率越大的点越重要，被优先选取时可以较好提取相关特征角点，对于不同曲率表面的物体或结构，曲率降采样可以处理不同的点，而并非是按照网格思想的聚合计算，结果精确度高。
  • 高斯曲率：该点两个主曲率乘积，揭示该点周围表面的局部弯曲特性，描述表面在该点的“内在”几何属性
  • 平均曲率：两个主曲率的算术平均，能够提供表面总体弯曲度的量化，通常用于描述表面弯曲的整体趋势

  

  d3b9c8bace5e47d83fa36dc64044dbdd.png

  

  698d4650c1c54a35b1e35b870c4ed7cd.png

• 体素滤波降采样： 计算点云的包围盒，根据设定的体素尺寸或划分的格点数，将空间划分为多个小体素，在每个立方体单元内，按特定策略选取一个代表点，例如体素内所有点的均值、几何中心，或直接选择距离体素中心最近的点，最终，所有体素的代表点构成降采样后的新点云。

8fbf1ce9efa9d676d7dfbf1478bd9236.png

• 点云配准： 优化目标是通过位姿变换𝑇𝑘来最小化配准误差

22a69fba026867c11c0258755cd6dbdc.png

• 前端回环检测： 当系统识别到机器人重返历史场景时，会建立新的位姿约束关系，重新优化整个图以消除累积误差， 尤其是位姿漂移

e45219d28f7f82df6bf05db0b3f3bb9d.png

• 子图生成匹配： 随着机器人的运动，新的关键帧会不断加入当前的子图，形成了一个局部子图，子图中的每个关键帧通过图优化与其他关键帧连接。
  • 一幅地图：包括很多子图，全部子图融合成一幅完整的地图
  • 一个子图：由一定数量的关键帧构成，一系列连续的关键帧整合成一个局部子图
  • 一个关键帧：包含该时刻机器人的位姿以及该位姿下的局部地图信息

  

  630f810d8efd52312f2f67878fe6addb.png

• 后端回环匹配： 描述成一个非线性的最小二乘问题，在后台线程中每隔几秒钟使用 Ceres 库求解一次。

7adb49a0c57320e3fb3b80703f3118d4.png

5. 优化A*实现全局路径规划

• Navigation 2： 根据机器人的当前状态和目标位置，生成并执行相应的导航动作，通过调用Planner、Controller、Recovery三个核心服务模块实现闭环控制。服务器接收到目标位置后，先由规划器服务器执行路径计算，再控制器服务器负责路径跟随，若在运动过程中遇到障碍导致停滞，系统将自动激活恢复器服务器，调用相应的恢复策略使机器人脱离困境

8e8e225f09b4abc7e6ab0372227bcd14.png

• 规划器服务器(Planner Server)： 管理全局路径规划，该模块是一个服务器框架而非具体规划算法实现，通过加载不同的全局路径规划插件来扩展其功能，常见的算法包括 A*、Dijkstra、快速随机树（RRT）

7a7f7d1578dce704ce0f157ed77041fd.png

• 控制器服务器(Controller Server)： 局部路径跟踪实现机器人运动控制，通过整合全局路径规划结果、实时障碍物以及局部代价地图，计算出最优运动指令，实现上，各类局部路径规划器以插件形式存在，遵循统一接口规范接入服务器

c4e0996728ef4085cb07582561495635.png

• 恢复器服务器(Recovery Server)： 处理机器人异常状态，支持加载多种恢复行为插件，用于解决机器人被困等突发状况。
• 基于车辆动力学优化路径规划算法： 利用 Dubins （仅前进）或 Reeps-Shepp （可前进可后退）模型，通过引入航向角𝜃构建三维状态空间，使搜索过程能够考虑车辆的实际运动学特性，包括转向半径等约束条件，搜索空间从经典 A*平直变为有弧度的曲线。

5d653cc074aea64818d56ee2bf47e8d2.png

708dbd364ff6e06220e78650ee0fe96b.png

• 通过动态权重优化全局路径规划算法： 权重的合理调节能在搜索效率与路径质量之间实现动态平衡，在预估函数前增加一个系数 w，从而改变启发函数的权重

d652c29d31a0c5f2bd9da2165ed597d0.png

7346d6eef2c516b359e085ea20a1ba29.png

• 基于路径进展的动态权重：在路径规划的初期，机器人距离目标较远时，应优先减少搜索空间，以提高搜索效率

a7d61f0f2f47e99f571d512a8313ca15.png

• 基于环境复杂度的动态权重：当机器人周围障碍物密集时，可能存在较多绕行情况，此时应降低 h(n)的影响

6ab7f25e33f5aacad3449c98a96d114a.png

• 基于路径平滑度的动态权重：如果当前路径的转向较多或不平滑，则更关注实际代价 g(n)，以优化路径平滑度

52d283ef84307a74f27b3d6d34af7370.png

• 微小偏移优化终点前搜索：当存在多个节点具有相同的启发函数值时，优先选择距离终点更近的节点有助于加速搜索过程。由于距终点近通常意味着预估代价 h(n)更小，因此在启发函数中为 h(n)添加一个微小偏移量 e，使靠近终点的节点在数值上略微占优，从而更容易被优先扩展