腿足机器人之十- SLAM地图如何用于运动控制
相比于轮式机器人（如人形轮式机器，可以看成是扫地机器人之上加了一个人形上身，实际的导航还是扫地机器人那一套），腿足式机器人（如人形机器人、四足机器人狗）的运动控制复杂度远高于轮式机器人，需要解决以下核心问题：
- 复杂地形适应：楼梯、碎石、斜坡、火车铁轨等非结构化环境
- 动态平衡控制：保持运动中的稳定性（如ZMP判据）
- 高自由度协调：多关节协同运动（如12~28个自由度），（天工机器人2024.4具有42个自由度，内置多个视觉感知传感器及六维力传感器，配备惯性测量单元（IMU）和3D视觉传感器，拥有每秒550万亿次的算力。）
SLAM地图的核心作用：
- 地形几何建模：提供地面高度、坡度、障碍物尺寸等信息
- 语义理解：识别可通行区域（如草地、水泥地、台阶）
- 运动规划基础：结合地形信息生成安全步态和路径
腿足机器人SLAM地图的表示与处理
和到底机器人相比，腿足机器人可以双脚离地（小跑、跳跃），SLAM地图需要包含高度信息，因此使用三维地图，这可以使用点云地图、高程地图以及体素地图。
- 点云地图：通过RGB-D相机或激光雷达生成稠密3D点云
- 高程地图（Elevation Map）：将点云投影为2.5D网格，每个网格存储高度均值和方差：
h i , j = 1 N ∑ k = 1 N z k , σ i , j 2 = 1 N ∑ k = 1 N ( z k − h i , j ) 2 h_{i,j}=\frac{1}{N}\sum \limits_{k=1}^Nz_k, \sigma_{i,j}^2=\frac{1}{N}\sum \limits_{k=1}^N(z_k-h_{i,j})^2 hi,j=N1k=1∑Nzk,σi,j2=N1k=1∑N(zk−hi,j)2
其中 z k z_k zk为网格内低 k k k个点的高度。 - 体素地图（Voxel Map）：将空间划分为立方体单元，标记占用状态（如OctoMap）。
以高程地图为例，可以通过下式提取关键地形特征（坡度，以便评估机器人是否可以通过该倾斜度）：
其坡度计算如下：
s
l
o
p
e
i
,
j
=
arctan
(
(
∂
h
∂
x
)
2
+
(
∂
h
∂
y
)
2
)
\mathbf{slope}_{i,j}=\arctan \left({\sqrt{(\frac{\partial h}{\partial x})^2+ (\frac{\partial h}{\partial y})^2} }\right )
slopei,j=arctan((∂x∂h)2+(∂y∂h)2)
其中：
∂
h
∂
x
≈
h
i
+
1
,
j
−
h
i
−
1
,
j
2
△
x
\frac{\partial h}{\partial x} \approx \frac{h_{i+1,j }-h_{i-1,j}}{2 \triangle x}
∂x∂h≈2△xhi+1,j−hi−1,j，
∂
h
∂
y
≈
h
i
,
j
+
1
−
h
i
,
j
−
1
2
△
y
\frac{\partial h}{\partial y} \approx \frac{h_{i,j+1 }-h_{i,j-1}}{2 \triangle y}
∂y∂h≈2△yhi,j+1−hi,j−1
然后结合坡度、粗糙度、障碍物高度判断是否适合踩踏：
T
r
a
v
e
r
s
a
b
i
l
i
t
y
=
ω
1
⋅
e
−
k
1
⋅
s
l
o
p
e
+
ω
2
⋅
e
−
k
2
⋅
r
o
u
g
h
n
e
s
s
\mathbf{Traversability} = \omega_1 \cdot e^{-k_1 \cdot \mathbf{slope}} + \omega_2 \cdot e^{-k_2 \cdot \mathbf{roughness}}
Traversability=ω1⋅e−k1⋅slope+ω2⋅e−k2⋅roughness
其中：
ω
1
\omega_1
ω1，
ω
2
\omega_2
ω2为权重，
k
1
k_1
k1，
k
2
k_2
k2为衰减系数。
全局路径规划：地形感知的路径搜索
在有了地图之后，需要找到起点到目标点的最优路径（避开已知障碍物），首先进行全局路径规划（基于静态地图），常用的算法有A*、Dijkstra、RRT*等。然后再使用局部路径规划（避开动态障碍物，实时调整路径），常用的算法有动态窗口法（DWA）、人工势场法。
基于A*的三维路径规划
将高程地图转换为三维栅格，定义移动代价函数：
C
o
s
t
(
u
→
v
)
=
α
⋅
d
i
s
t
a
n
c
e
(
u
,
v
)
+
β
⋅
s
l
o
p
e
(
v
)
+
γ
⋅
r
o
u
g
h
n
e
s
s
(
v
)
\mathbf{Cost}(u→v) = \alpha \cdot \mathbf{distance}(u,v) + \beta \cdot \mathbf{slope}(v)+\gamma \cdot \mathbf{roughness} (v)
Cost(u→v)=α⋅distance(u,v)+β⋅slope(v)+γ⋅roughness(v)
-
α
\alpha
α、
β
\beta
β、
γ
\gamma
γ为权重系数，平衡路径长度与安全性。
改进启发函数：考虑地形起伏的欧氏距离：
h ( n ) = ( x g o a l − x n ) 2 + ( y g o a l − y n ) 2 + k h ⋅ ( h g o a l − h n ) 2 h(n) = \sqrt{(x_{goal}-x_n)^2 + (y_{goal}-y_n)^2 + k_h \cdot(h_{goal}-h_n)^2} h(n)=(xgoal−xn)2+(ygoal−yn)2+kh⋅(hgoal−hn)2
其中 h k h_k hk为高度权重因子。
基于RRT*的可行步态序列生成
针对腿足机器人，路径需满足落脚点约束：
- 采样落脚点：在可通行区域随机选择候选点。
- 运动学可达性检查：验证机器人腿部能否触及该点（逆运动学求解）。
- 稳定性评估：计算ZMP（零力矩点）是否在支撑多边形内，见腿足机器人之八-动力学
优化目标函数：
J = ∑ k = 1 N ( C o s t t e r r a i n ( p k ) + λ ⋅ ∣ ∣ p k − p k − 1 ∣ ∣ ) J= \sum \limits_{k=1}^N(Cost_{terrain}(p_k) + \lambda \cdot ||p_k - p_{k-1}||) J=k=1∑N(Costterrain(pk)+λ⋅∣∣pk−pk−1∣∣)
其中 p k p_k pk为第k步的落脚点， λ \lambda λ平衡步长与地形代价。
局部运动规划：实时步态调整与避障
动态窗口法的腿足适配
将速度空间扩展为 落脚点时序空间：
- 决策变量：下一步落脚点位置 x f , y f , z f x_f, y_f, z_f xf,yf,zf和接触地时间 t f t_f tf。
- 约束条件：
- 运动学极限： ∣ ∣ p f − p c u r r e n t ∣ ∣ ≤ l m a x ||p_f - p_{current}|| \le l_{max} ∣∣pf−pcurrent∣∣≤lmax ，表示退长限制；
- 动态平衡：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.预测模型：建立腿足机器人动力学模型（如单刚体模型）：
{
m
r
¨
=
Σ
i
=
1
N
l
e
g
s
f
i
−
m
g
I
ω
˙
+
ω
×
I
ω
=
Σ
i
=
1
N
l
e
g
s
(
r
i
×
f
i
)
\left\{ \begin{aligned} 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{aligned} \right.
⎩
⎨
⎧mr¨Iω˙+ω×Iω==Σi=1Nlegsfi−mgΣi=1Nlegs(ri×fi)
其中
f
i
f_i
fi是第
i
i
i条腿的地面反作用力，
r
i
r_i
ri为力作用点位置。
2.滚动优化：在时间窗口
[
t
,
t
+
T
]
[t, t+T]
[t,t+T]内求解最优落脚点和关节轨迹：
min
u
:
0
:
N
−
1
∑
k
=
0
N
−
1
(
∣
∣
x
k
−
x
r
e
f
∣
∣
Q
2
+
∣
∣
u
k
∣
∣
R
2
)
+
∣
∣
x
N
−
x
r
e
f
∣
∣
P
2
\min_{u:0:N-1} \sum \limits_{k=0}^{N-1}(||x_k - x_{ref}||_Q^2 + ||u_k||_R^2) + ||x_N - x_{ref}||_P^2
u:0:N−1mink=0∑N−1(∣∣xk−xref∣∣Q2+∣∣uk∣∣R2)+∣∣xN−xref∣∣P2
- 状态 x k x_k xk包括质心位置、速度、关节角度。
- 输入控制 u k u_k uk包括关节力矩或落脚点位置。
- 实时调整：根据SLAM更新的障碍物信息修正优化约束。
稳定性控制与SLAM定位的协同
零力矩点（ZMP）稳定性判据
ZMP定义为地面反作用力的合力作用点，需落在支撑多边形内：
x
Z
M
P
=
Σ
i
=
1
N
(
m
i
x
i
(
g
+
z
¨
i
)
−
m
i
z
i
x
¨
i
Σ
i
=
1
N
m
i
(
g
+
z
¨
i
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}
xZMP=Σi=1Nmi(g+z¨iΣi=1N(mixi(g+z¨i)−mizix¨i
简化模型下（质心高度恒定）：
x
Z
M
P
=
x
C
o
M
−
x
¨
C
o
M
⋅
z
C
o
M
g
x_{ZMP} = x_{CoM}-\frac{\ddot x_{CoM} \cdot z_{CoM}}{g}
xZMP=xCoM−gx¨CoM⋅zCoM
稳定性约束：
x
Z
M
P
∈
S
u
p
p
o
r
t
P
o
l
y
g
o
n
x_{ZMP} \in \mathbf{Support Polygon}
xZMP∈SupportPolygon
Boston Dynamics Atlas机器人的SLAM导航
- 硬件与传感器配置
- 激光雷达：Velodyne VLP-16，生成3D点云。
- IMU：测量本体角速度与加速度。
- 关节编码器：提供腿部运动学信息。
2.SLAM与运动控制流程
地图构建：
- 使用Google Cartographer融合激光雷达与IMU数据，构建3D栅格地图。
- 提取高程图与可通行区域（坡度<30°, 粗糙度<0.1m）。
- 全局路径规划：
- 在可通行区域上运行改进A*算法，避开陡坡与障碍物。
- 路径平滑化处理，确保步态序列的连贯性。
- 局部步态调整：
- 检测到动态障碍物（如移动箱子）时，MPC重新规划未来3步落脚点。
- 计算新落脚点的ZMP轨迹，调整质心运动以保持平衡。
- 控制执行：
- 逆运动学求解关节角度，PD控制器跟踪轨迹：
τ = K p ( q d e s − q ) + K d ( q ˙ d e s − q ˙ ) \tau = K_p(q_{des}-q) + K_d(\dot q_{des}-\dot q) τ=Kp(qdes−q)+Kd(q˙des−q˙) - 触地阶段力控：根据地面反作用力调整关节力矩。
