足式与人形机器人基础

轮子需要一条路，腿只需要一个个落脚点：台阶、碎石、楼梯、灾后废墟，只有足式机器人进得去；人形机器人更是希望直接使用为人类设计的世界。但是腿把机械臂里没有的三个难题带了进来：

• 身体是"浮动"的。机械臂的底座拧在桌上，关节一转末端就动。足式机器人的躯干（浮动基座）没有被任何东西固定，它的位姿属于 \(SE(3)\)（7.2 节），其中 6 个自由度没有电机：关节力矩只能通过脚底的接触力，间接地推动躯干。
• 接触是间歇的、单向的。脚只能推地，不能拉地（法向力 \(\ge 0\)）；切向力还不能超过摩擦锥（8.4 节）。每迈一步，脚离地、落地，接触模式就切换一次。
• 平衡不是目标，是约束。静止的机械臂不会倒；站着的人形机器人像竖在指尖的一支笔，没有控制就在零点几秒内倒下。

这一节要建立一套最小的"足式直觉工具箱"：支撑多边形（什么时候不用动也不会倒）、浮动基座上的力与力矩平衡（脚到底能给躯干什么）、线性倒立摆与 ZMP / 捕获点（怎样走、怎样被推了还不倒）、步态（四足怎样安排脚的节奏）。真实的足式控制器（Part IV 的 MPC 与全身控制，Part VI 的强化学习）都建立在这些概念之上。

把机器人摆在地上，所有脚底接触点的凸包叫支撑多边形。只要质心（COM）沿重力方向在地面上的投影落在支撑多边形内，机器人静止就不会倒；投影越过某条边，它就绕这条边翻倒。投影点到最近边的距离叫稳定裕度。四足站立时多边形很大；抬起一条腿，它变成三角形，质心必须先挪进三角形；双足时多边形只是两只脚和它们之间的区域。

斜坡会"移动"投影点：质心高度为 \(h\)、坡度为 \(\alpha\) 时，重力方向的投影相对于"沿坡面法向"的投影向下坡方向偏移 \(h\tan\alpha\)，所以重心越高，在斜坡上越容易翻。同时，沿坡面下滑的趋势 \(\tan\alpha\) 超过摩擦系数 \(\mu\) 时，脚会打滑，这时翻倒之前先滑倒（8.4 节）。

支撑多边形只回答"静止时倒不倒"。要回答"能不能走起来、跑起来"，必须问：脚能给躯干什么力？每只脚给出一个地面反力 \(\mathbf f_i\)，它受两条约束：法向分量 \(f_z\ge0\)（单向），切向分量 \(|f_x|\le\mu f_z\)（摩擦锥，半角 \(\arctan\mu\)，8.4 节）。把所有脚的力加起来，再加上重力，就是整个机器人的牛顿–欧拉方程（8.2 节）：

左边是整台机器人质心的加速度和绕质心的角动量变化率（所有关节力矩都是内力，在求和中抵消了，所以式子里没有 \(\boldsymbol\tau\)）。右边只有脚上的力。这就是"躯干只能通过脚被推动"的数学形式。下图是矢状面（侧视图）里的两只脚：拖动脚尖的力手柄，力被自动投影到摩擦锥内（最近点投影，1.9 节）。紫色箭头是躯干受到的净力 \(m\ddot{\mathbf c}\)，琥珀色弧线是绕质心的净力矩。想让机器人静止，力要满足 \(\sum f_z=mg\)、\(\sum f_x=0\) 和力矩为零。

现在加上动力学。假设质心保持恒定高度 \(z_c\)、腿没有质量、不考虑绕质心的角动量变化，那么质心的水平运动满足一个线性方程，叫线性倒立摆模型（LIPM）：

\(p\) 是脚下的支撑点（即 ZMP，下文解释）。对人形机器人 \(z_c\approx0.85\) m，\(\omega\approx3.4\ \mathrm{s^{-1}}\)，时间常数 \(1/\omega\approx0.3\) s：质心偏离脚 \(x-p\)，会以这个时间常数指数地"飞离"。走路就是每隔一步换一个支撑点，让质心在落到下一只脚上方时刚好被接住。下图让一个 LIPM 行走器按这个规则连续迈步：每一步结束时，把新脚放在 \(p_{\text{new}}=\hl{dcm}{\xi_{\text{end}}}-b\) 处（\(\xi=x+\dot x/\omega\) 是下文的"捕获点"，\(b\) 由期望速度和步时决定，见"再看数学"）。拖动滑块改变质心高度、步时和期望速度。

站着的机器人被突然推了一把，质心得到速度 \(\dot x_0\)。捕获点（Pratt 等 2006，也叫发散分量 DCM）\(\xi=x+\dot x/\omega\) 回答："脚落在哪里，机器人才会恰好停在那只脚上方？"答案就是 \(p=\xi\)：因为 \(\dot\xi=\omega(\xi-p)\)，脚放在 \(\xi\) 处，\(\xi\) 就不再变化，\(x\) 以时间常数 \(1/\omega\) 收敛到 \(\xi\)。但迈步需要时间 \(t_s\)，这段时间里 \(\xi\) 仍在以 \(e^{\omega t_s}\) 增长，所以实际要落的位置是 \(\xi(t_s)=\xi_0e^{\omega t_s}\)。

脚能够到的范围有限（腿长、关节限位），于是出现捕获区域：推得不太狠时，只靠踝关节的力矩把 ZMP 移到脚尖就够了（"踝策略"，\(\xi_0\) 落在脚的范围内）；更大的推力要迈步（"迈步策略"）；再大，脚够不到 \(\xi(t_s)\)，就一定倒。这就是人被推时会下意识迈出一步的原因：脚太小，踝策略的容量只有几厘米。

取矢状面，\(x\) 向前、\(z\) 向上。整台机器人质量为 \(m\)，质心在 \((x,z)\)，地面反力的合力 \(\mathbf F=(F_x,F_z)\) 作用在地面上的一点 \(p\)（压力中心 CoP）。牛顿–欧拉方程给出 \(F_x=m\ddot x\)，\(F_z=m(\ddot z+g)\)，合力对质心的力矩等于角动量变化率 \(\dot H\)（逆时针为正）：

这就是 ZMP（零力矩点）的表达式：它是地面上那一点，在它上面，重力与惯性力的合力矩的水平分量为零。LIPM 做了四个假设：(1) 质量集中在质心；(2) 质心高度恒定，\(z=z_c\)，\(\ddot z=0\)；(3) 腿无质量（摆动腿不影响质心）；(4) \(\dot H\approx0\)。代入即得

闭式解可以直接代回方程验证，FIG 3 的读数把它与 RK4 数值积分比较（一步之内差在 \(10^{-10}\) m 以下）。\(E\) 叫轨道能量：\(\dot E=\dot x\ddot x-\omega^2(x-p)\dot x=0\)，所以在一步之内相平面轨迹是双曲线。\(E>0\) 说明质心会越过脚正上方继续前进，\(E<0\) 说明会在越过之前折返，\(E=0\) 恰好是两条流形。两条路得到同一个方程：对"刚性腿长 \(\ell\) 的摆"，\(x=\ell\sin\theta\)，\(\ddot\theta=\tfrac g\ell\sin\theta\)，小角度下 \(\sin\theta\approx\theta\)（2.4 节的线性化）给出 \(\omega=\sqrt{g/\ell}\)；LIPM 的"恒高度"假设则不要求角度小，但要求腿能伸缩来保持高度。

平地上，各接触点的法向力 \(f_{z,i}\ge0\)，压力中心是它们的加权平均：

凸组合必定落在接触点的凸包内，所以真实的 CoP 永远在支撑多边形里；平地上 ZMP 与 CoP 重合。FIG 2 的读数用 \(\sum x_if_{z,i}/\sum f_{z,i}\) 和上面的 ZMP 公式（其中 \(\dot H\) 取脚力对质心的合力矩）各算一遍：两者在代数上等价，差值只是浮点误差，它检验的是公式与代码一致，并不是一次独立的测量。如果某个运动"需要"的 ZMP 在多边形之外，那个点只是一个虚构的 ZMP：真实的 CoP 会停在边上，多出来的力矩变成绕这条边的角加速度，脚开始滚动，机器人开始翻倒。ZMP 条件只管法向力的单向性（\(f_z\ge0\)）；切向力的限制是另一个独立的条件，即 \(\|\mathbf f_t\|\le\mu f_z\)，另外还有绕竖直轴的摩擦力矩（偏航）。ZMP 在脚内，不保证不打滑。

令 \(\xi=x+\dot x/\omega\)，则 \(\dot\xi=\dot x+\ddot x/\omega=\dot x+\omega(x-p)=\omega\big(\hl{dcm}{\xi}-p\big)\)，同时 \(\dot x=-\omega(x-\xi)\)。所以 \(\xi\) 的方程是一个独立的一阶不稳定系统，而 \(x\) 只是它的一个低通滤波后的跟随者。单支撑相内

周期步态：要求每一步开始时相对新脚的状态相同，且每步前进 \(L\)、历时 \(T\)。设开始时 \(x_0\)、\(\dot x_0\)（相对脚），结束时 \(x_T\)、\(\dot x_T\)。周期性要求 \(x_T-L=x_0\)，\(\dot x_T=\dot x_0\)，解得 \(x_0=-L/2\)，\(\dot x_0=\tfrac{\omega L}{2}\coth\tfrac{\omega T}{2}\)（FIG 3 的行走器正是从这组初值出发的，两个条件都成立到 \(10^{-16}\)）。平均速度 \(v=L/T\)；轨道能量 \(E=\tfrac12\omega^2(L/2)^2/\sinh^2(\omega T/2)\)。每步开始时相对新脚的捕获点是 \(b=\xi_{0}=\tfrac L2(\coth\tfrac{\omega T}{2}-1)\)，因此 DCM 落脚规则是

这一步把下一步开始时的 \(\xi\) 精确设成目标值（对不稳定模态是"一步到位"的），稳定模态每步以因子 \(e^{-\omega T}\approx0.2\) 衰减，所以 FIG 3 改变速度后两三步就进入新的周期步态。期望速度为 0 时 \(b=0\)，规则变成 \(p=\xi\)，正是捕获点。

7.2 节：浮动基座机器人的构型是 \(q=({}^{W}T_{B},\boldsymbol\theta)\in SE(3)\times\mathbb R^n\)（速度 \(\dot q\) 取 \(6+n\) 维：基座 twist 与关节速度）。8.3 节的运动方程加上接触力，写成

\(S^\top\boldsymbol\tau\) 的前 6 个分量恒为 0：电机只作用在关节上。所以前 6 行（基座方向）是 \(M_{bb}\dot{\mathbf v}_b+M_{b\theta}\ddot{\boldsymbol\theta}+h_b=\sum_iJ_{c,i,b}^{\top}\mathbf f_i\)：基座加速度只能通过摆动关节带来的惯性耦合和接触力改变。接触力本身又由 \(J_c\dot{\mathbf v}+\dot J_c\mathbf v=0\)（脚不滑动）这类约束和关节力矩共同决定（8.4 节）。把前 6 行写成整体的力与力矩（所有关节的内力抵消），就得到 \(\sum_i\mathbf f_i+m\mathbf g=m\ddot{\mathbf c}\) 与 \(\dot{\mathbf h}_G=\sum_i(\mathbf p_i-\mathbf c)\times\mathbf f_i\)（加上脚上的接触力矩）：这是质心动力学（centroidal dynamics）。地面力能产生的净 wrench（7.6 节）构成一个凸锥 \(\{\sum_iG_i\mathbf f_i:\mathbf f_i\in\mathcal F_i\}\)，足式 MPC 和全身控制就是在这个锥里选 \(\mathbf f_i\)（Part IV，可写成 QP，6.8 节）。

Raibert（1986）的单腿跳跃机用一条朴素的规则平衡速度：脚落在"中性点" \(\dot x\,T_s/2\) 前方，再加一个速度反馈项，\(x_{\text{foot}}=\dot x\,\tfrac{T_s}{2}+k(\dot x-\dot x_{\text{des}})\)，\(T_s\) 是支撑时间。形式上它和 DCM 落脚规则是近亲。把腿换成弹簧，就得到跑步的模型 SLIP（弹簧加载倒立摆）：质心在支撑相像在弹簧上弹跳，腾空相是抛体。LIPM 的支撑相不做功（\(E\) 不变）；真实机器人落脚时有碰撞损失、蹬地时补充能量，被动行走机（McGeer 1990）则靠斜坡上的重力势能补回。

四足动物（和四足机器人）按固定的节奏轮流抬腿，这个节奏叫步态。每条腿在一个周期里有一段支撑相（脚在地上，占周期的比例叫占空比 \(\beta\)）和一段摆动相，四条腿的周期之间错开一定的相位。下图左上是步态图（Hildebrand 图）：每行一条腿，实心是支撑，空白是摆动；左下画出每个时刻支撑多边形能给出的最大静态稳定裕度（多边形内最大内切圆的半径）；右侧是俯视动画：实心圆是支撑脚，空心圆是摆动脚，紫环是理想的静态质心位置（质心在身体里，所以机器人必须在迈步之前先晃动身体，把质心挪过去）。