腿足机器人之十- SLAM地图如何用于运动控制

相比于轮式机器人（如人形轮式机器，可以看成是扫地机器人之上加了一个人形上身，实际的导航还是扫地机器人那一套），腿足式机器人（如人形机器人、四足机器人狗）的运动控制复杂度远高于轮式机器人，需要解决以下核心问题：

• 复杂地形适应：楼梯、碎石、斜坡、火车铁轨等非结构化环境
• 动态平衡控制：保持运动中的稳定性（如ZMP判据）
• 高自由度协调：多关节协同运动（如12~28个自由度），（天工机器人2024.4具有42个自由度，内置多个视觉感知传感器及六维力传感器，配备惯性测量单元（IMU）和3D视觉传感器，拥有每秒550万亿次的算力。）
  

SLAM地图的核心作用：

• 地形几何建模：提供地面高度、坡度、障碍物尺寸等信息
• 语义理解：识别可通行区域（如草地、水泥地、台阶）
• 运动规划基础：结合地形信息生成安全步态和路径

腿足机器人SLAM地图的表示与处理

和到底机器人相比，腿足机器人可以双脚离地（小跑、跳跃），SLAM地图需要包含高度信息，因此使用三维地图，这可以使用点云地图、高程地图以及体素地图。

• 点云地图：通过RGB-D相机或激光雷达生成稠密3D点云
• 高程地图（Elevation Map）：将点云投影为2.5D网格，每个网格存储高度均值和方差：
  $$h_{i,j}=\frac{1}{N}\sum _{k=1}^N{z}_k,{\sigma }_{i,j}^2=\frac{1}{N}\sum _{k=1}^N(z_k-h_{i,j}{)}^2$$
  其中$z_k$为网格内低$k$个点的高度。
• 体素地图（Voxel Map）：将空间划分为立方体单元，标记占用状态（如OctoMap）。

以高程地图为例，可以通过下式提取关键地形特征（坡度，以便评估机器人是否可以通过该倾斜度）：
其坡度计算如下：
$${\mathbf{slope}}_{i,j}=\arctan \left(\sqrt{(\frac{\partial h}{\partial x}{)}^2+(\frac{\partial h}{\partial y}{)}^2}\right)$$
其中：$\frac{\partial h}{\partial x}\approx \frac{h_{i+1,j}-h_{i-1,j}}{2△x}$，$\frac{\partial h}{\partial y}\approx \frac{h_{i,j+1}-h_{i,j-1}}{2△y}$
然后结合坡度、粗糙度、障碍物高度判断是否适合踩踏：
$$\mathbf{Traversability}={\omega }_1\cdot e^{-k_1\cdot \mathbf{slope}}+{\omega }_2\cdot e^{-k_2\cdot \mathbf{roughness}}$$
其中：${\omega }_1$，${\omega }_2$为权重，$k_1$，$k_2$为衰减系数。

全局路径规划：地形感知的路径搜索

在有了地图之后，需要找到起点到目标点的最优路径（避开已知障碍物），首先进行全局路径规划（基于静态地图），常用的算法有A*、Dijkstra、RRT*等。然后再使用局部路径规划（避开动态障碍物，实时调整路径），常用的算法有动态窗口法（DWA）、人工势场法。

基于A*的三维路径规划

将高程地图转换为三维栅格，定义移动代价函数：
$$\mathbf{Cost}(u\to v)=\alpha \cdot \mathbf{distance}(u,v)+\beta \cdot \mathbf{slope}(v)+\gamma \cdot \mathbf{roughness}(v)$$

• $\alpha$、$\beta$、$\gamma$为权重系数，平衡路径长度与安全性。
  改进启发函数：考虑地形起伏的欧氏距离：
  $$h(n)=\sqrt{(x_{goal}-x_n{)}^2+(y_{goal}-y_n{)}^2+k_h\cdot (h_{goal}-h_n{)}^2}$$
  其中$h_k$为高度权重因子。

基于RRT*的可行步态序列生成

针对腿足机器人，路径需满足落脚点约束：

• 采样落脚点：在可通行区域随机选择候选点。
• 运动学可达性检查：验证机器人腿部能否触及该点（逆运动学求解）。
• 稳定性评估：计算ZMP（零力矩点）是否在支撑多边形内，见腿足机器人之八-动力学
  优化目标函数：
  $$J=\sum _{k=1}^N(Cos{t}_{terrain}(p_k)+\lambda \cdot ||p_k-p_{k-1}||)$$
  其中$p_k$为第k步的落脚点，$\lambda$平衡步长与地形代价。

局部运动规划：实时步态调整与避障

动态窗口法的腿足适配

将速度空间扩展为 落脚点时序空间：

• 决策变量：下一步落脚点位置$x_f,y_f,z_f$和接触地时间$t_f$。
• 约束条件：
  • 运动学极限：$||p_f-p_{current}||\le l_{max}$ ，表示退长限制；
  • 动态平衡：ZMP轨迹在支撑多边形内。
  • 避障：落脚点不与障碍物区域重叠。
• 评价函数：
  KaTeX parse error: Undefined control sequence: \codt at position 19: …re(p_f)=\alpha \̲c̲o̲d̲t̲ ̲dist_to_goal(p_…

模型预测控制（MPC）与步态优化

MPC框架步骤：
1.预测模型：建立腿足机器人动力学模型（如单刚体模型）：
$$\{\begin{array}{rlr}m\ddot{r}& =& {\Sigma }_{i=1}^{N_{legs}}{f}_i-mg\\ I\dot{\omega }+\omega \times I\omega & =& {\Sigma }_{i=1}^{N_{legs}(r_i\times f_i)}\end{array}$$
其中$f_i$是第$i$条腿的地面反作用力，$r_i$为力作用点位置。
2.滚动优化：在时间窗口$[t,t+T]$内求解最优落脚点和关节轨迹：
$$\underset{u:0:N-1}{\min }\sum _{k=0}^{N-1}(||x_k-x_{ref}|{|}_Q^2+||u_k|{|}_R^2)+||x_N-x_{ref}|{|}_P^2$$

• 状态$x_k$包括质心位置、速度、关节角度。
• 输入控制$u_k$包括关节力矩或落脚点位置。

3. 实时调整：根据SLAM更新的障碍物信息修正优化约束。

稳定性控制与SLAM定位的协同

零力矩点（ZMP）稳定性判据
ZMP定义为地面反作用力的合力作用点，需落在支撑多边形内：
$$x_{ZMP}=\frac{{\Sigma }_{i=1}^N(m_i{x}_i(g+{\ddot{z}}_i)-m_i{z}_i{\ddot{x}}_i}{{\Sigma }_{i=1}^N{m}_i(g+{\ddot{z}}_i}$$
简化模型下（质心高度恒定）：
$$x_{ZMP}=x_{CoM}-\frac{{\ddot{x}}_{CoM}\cdot z_{CoM}}{g}$$
稳定性约束：
$$x_{ZMP}\in \mathbf{SupportPolygon}$$

Boston Dynamics Atlas机器人的SLAM导航

1. 硬件与传感器配置

• 激光雷达：Velodyne VLP-16，生成3D点云。
• IMU：测量本体角速度与加速度。
• 关节编码器：提供腿部运动学信息。

2.SLAM与运动控制流程
地图构建：

• 使用Google Cartographer融合激光雷达与IMU数据，构建3D栅格地图。
• 提取高程图与可通行区域（坡度<30°, 粗糙度<0.1m）。

3. 全局路径规划：

• 在可通行区域上运行改进A*算法，避开陡坡与障碍物。
• 路径平滑化处理，确保步态序列的连贯性。

4. 局部步态调整：

• 检测到动态障碍物（如移动箱子）时，MPC重新规划未来3步落脚点。
• 计算新落脚点的ZMP轨迹，调整质心运动以保持平衡。

5. 控制执行：

• 逆运动学求解关节角度，PD控制器跟踪轨迹：
  $$\tau =K_p(q_{des}-q)+K_d({\dot{q}}_{des}-\dot{q})$$
• 触地阶段力控：根据地面反作用力调整关节力矩。