Part IV · 第 13 章 反馈控制 · 13.7Part IV · Ch. 13 Feedback Control · 13.7

全身控制（QP）Whole-Body Control (QP)

人形和四足机器人要同时做好几件事：保持平衡、稳住躯干、伸手去拿东西，而且脚只能推不能拉、不能打滑、电机力矩有上限。全身控制把这一切写成一个二次规划：每个控制周期求一次关节加速度、关节力矩和接触力，让各个任务尽量满足，同时严格遵守浮动基座动力学、接触、摩擦锥和力矩限制。这一节在一个平面人形机器人上实时求解这个 QP，看它怎样分配重量、抬脚、在湿滑的沟里撑住、扶墙前倾，以及怎样用严格的优先级保护平衡。 Humanoids and quadrupeds must do several things at once: keep their balance, hold the torso steady, reach for an object, while their feet can only push, must not slip, and their motors have torque limits. Whole-body control writes all of this as one quadratic program: every control cycle it solves for joint accelerations, joint torques and contact forces that satisfy the tasks as well as possible while strictly obeying the floating-base dynamics, the contacts, the friction cones and the torque limits. This lesson solves that QP live on a planar humanoid and watches it shift its weight, lift a foot, brace itself in a slippery trench, lean on a wall, and protect its balance with strict priorities.

约 45 分钟~45 min 前置：6.7 约束优化与 KKT · 6.8 线性规划与二次规划 · 8.3 质量矩阵与科氏力 · 8.4 接触与摩擦 · 9.2 足式与人形机器人基础 · 13.6 操作空间控制Prereqs: 6.7 Constrained Optimization · 6.8 Linear & Quadratic Programming · 8.3 Mass Matrix & Coriolis Terms · 8.4 Contact & Friction · 9.2 Legged & Humanoid Basics · 13.6 Operational-Space Control 5 个交互5 interactives

    1为什么机器人需要它Why robots need this

    13.6 节的操作空间控制处理的是一只固定在桌上的手臂：一个主任务（手爪），再把第二个任务投影到它的零空间里。换成一个用两只脚站着的人形机器人，问题一下子多了起来：

    • 任务很多，而且会打架。质心要待在脚的上方，躯干要保持直立，手要伸向目标，摆动脚要抬起来，剩下的自由度还要保持一个"像样"的姿态。目标够不着的时候，必须有人让步：谁让？
    • 基座没有电机。9.2 节说过，浮动基座运动方程的前 6 行（平面里是 3 行）没有关节力矩，躯干的加速度只能靠接触力和摆动肢体的惯性耦合来改变。
    • 接触力受不等式约束。脚只能推地，不能拉地；切向力必须在摩擦锥里（8.4 节）；压力中心必须在脚掌内。这些都是不等式，零空间投影处理不了。
    • 电机有上限。力矩不能超过额定值。如果先算出"理想力矩"再截断，截断后的力矩已经和接触力对不上了，脚会滑，机器人会倒。

    全身控制（whole-body control，WBC）的答案是：每个控制周期解一个二次规划（6.8 节）。决策变量是关节加速度 \(\ddot{\mathbf q}\)、关节力矩 \(\boldsymbol\tau\) 和接触力 \(\mathbf f\)；目标函数是各任务误差的加权平方和；约束是物理：动力学方程、接触不动、摩擦锥、力矩上下限。QP 是凸的，有唯一解，几十个变量的问题解一次远不到 1 毫秒，所以可以放进 500 Hz 到 1 kHz 的控制回路。

    The operational-space control of Lesson 13.6 handled an arm bolted to a table: one main task (the gripper), with a second task projected into its null space. Put the same ideas on a humanoid standing on two feet and the problem grows at once:

    • Many tasks, and they conflict. The centre of mass must stay over the feet, the torso upright, the hand must reach for its target, a swing foot must lift, and the leftover freedom should keep a sensible posture. When a target is out of reach, someone has to give way: who?
    • The base has no motor. As Lesson 9.2 showed, the first six rows (three in the plane) of the floating-base equations of motion contain no joint torque: the trunk's acceleration can only be changed by contact forces and the inertial coupling of moving limbs.
    • Contact forces obey inequalities. A foot can push on the ground but not pull; the tangential force must stay inside the friction cone (Lesson 8.4); the centre of pressure must stay under the sole. These are inequalities, which a null-space projection cannot handle.
    • Motors have limits. Torques may not exceed their ratings. If you first compute "ideal" torques and then clip them, the clipped torques no longer agree with the contact forces you planned: a foot slips and the robot falls.

    Whole-body control (WBC) answers all of this with one quadratic program per control cycle (Lesson 6.8). The decision variables are the joint accelerations \(\ddot{\mathbf q}\), the joint torques \(\boldsymbol\tau\) and the contact forces \(\mathbf f\); the objective is a weighted sum of squared task errors; the constraints are the physics: the equations of motion, contacts that do not move, friction cones, torque bounds. The QP is convex with a unique solution, and a problem with a few dozen variables solves in well under a millisecond, so it fits inside a 500 Hz to 1 kHz control loop.

    本节用的机器人The robot used in this lesson

    一个侧视（矢状面）的平面人形：躯干（含头）28 kg，每条腿是大腿 6 kg、小腿 3 kg、脚 1 kg，一条手臂 2.5 + 1.5 kg，共 51.5 kg。广义坐标 \(\mathbf q=(x,y,\theta,q_1,\dots,q_8)\in\R^{11}\)：前 3 个是骨盆的位置和躯干俯仰角（浮动基座，没有电机），后 8 个是两侧的髋、膝、踝和手臂的肩、肘。每只脚用脚跟和脚尖两个接触点。脚下的接触是真正仿真出来的（刚硬的弹簧阻尼接触加库仑静摩擦，0.5 ms 步长），全身控制器每 2 ms（500 Hz）解一次 QP，并不知道接触是怎么仿真的。

    A planar humanoid seen from the side (sagittal plane): torso with head 28 kg, each leg a 6 kg thigh, 3 kg shank and 1 kg foot, one arm of 2.5 + 1.5 kg, 51.5 kg in total. Generalized coordinates \(\mathbf q=(x,y,\theta,q_1,\dots,q_8)\in\R^{11}\): the first three are the pelvis position and the torso pitch (the floating base, no motor), the other eight are hip, knee and ankle of both legs plus shoulder and elbow. Each foot touches the ground at its heel and its toe. The contacts are genuinely simulated (stiff spring–damper contacts with Coulomb stiction, 0.5 ms steps), and the whole-body controller solves its QP every 2 ms (500 Hz) without knowing how the contacts are simulated.

    2直觉：任务是愿望，物理是法律Intuition: tasks are wishes, physics is law

    把控制器要决定的量想成一个点 \(\mathbf x\)（真实问题里它有几十维，下图只画两维，可以想成两个关节加速度）。每个任务是一个愿望："希望 \(A_k\mathbf x=\mathbf b_k\)"，愿望之间可能互相矛盾，所以只能"尽量"满足，惩罚 \(\|A_k\mathbf x-\mathbf b_k\|^2\)。约束是法律：方框外的点（比如超出力矩上限）根本不能选。剩下的问题是：两个愿望冲突时听谁的？有两种回答：

    • 加权：把愿望加权求和，\(\min\ w\,\|A_1\mathbf x-\mathbf b_1\|^2+\|A_2\mathbf x-\mathbf b_2\|^2\)。权重越大，任务 1 越被照顾，但只要 \(w\) 有限，它就永远不会被完全满足：两个任务在讨价还价。
    • 严格优先级（分层、字典序）：先只管任务 1，求出它能做到的最好结果；再在"不让任务 1 变差"的所有点里，尽量满足任务 2。任务 2 一点也影响不了任务 1。

    下图中红线是任务 1（"落在这条线上"），蓝色手柄是任务 2 的目标点，青色方框是约束。拖动红线的两个端点和蓝色目标，改变权重，在右上角切换两种方式。

    Think of the quantities the controller decides as one point \(\mathbf x\) (a real problem has dozens of dimensions; the figure shows two, think of two joint accelerations). Each task is a wish, "I would like \(A_k\mathbf x=\mathbf b_k\)". Wishes may contradict each other, so they are only met "as well as possible", by penalizing \(\|A_k\mathbf x-\mathbf b_k\|^2\). Constraints are law: a point outside the box (say, beyond a torque limit) may not be chosen at all. What remains is the question: when two wishes conflict, which one wins? There are two answers:

    • Weighting: add the wishes with weights, \(\min\ w\,\|A_1\mathbf x-\mathbf b_1\|^2+\|A_2\mathbf x-\mathbf b_2\|^2\). The larger the weight, the more task 1 is favoured, but for any finite \(w\) it is never fully met: the two tasks bargain.
    • Strict priority (hierarchical, lexicographic): first care only about task 1 and find the best it can achieve; then, among all points that do not make task 1 any worse, satisfy task 2 as well as possible. Task 2 cannot affect task 1 at all.

    In the figure the red line is task 1 ("lie on this line"), the blue handle is task 2's target point, and the cyan box is the constraint. Drag the two ends of the red line and the blue target, change the weight, and switch between the two schemes at the top right.

    FIG 1两个冲突的任务：加权还是严格优先Two conflicting tasks: weights or strict priority
    决策空间 (x₁, x₂)：拖动红色端点和蓝色目标DECISION SPACE (x₁, x₂) · drag the red ends and the blue target
    琥珀色实心点是当前方式的解；空心绿环是严格优先级的解；紫色虚线是权重 \(w\) 从 0.01 增加到 10⁴ 时加权解走过的路径：它从蓝色目标附近出发，逐渐靠近绿环，但要 \(w\to\infty\) 才到达。方框不起作用时，加权解离红线的距离正好是"蓝点到红线的距离"的 \(1/(1+w)\) 倍。方框半宽 \(L\) 代表执行器的上限。 The filled amber dot is the solution of the current scheme; the hollow green ring is the strict-priority solution; the dashed purple curve is the path of the weighted solution as \(w\) goes from 0.01 to 10⁴: it starts near the blue target and approaches the green ring, but only reaches it as \(w\to\infty\). While the box is inactive, the weighted solution's distance from the red line is exactly \(1/(1+w)\) times the blue target's distance from it. The box half-width \(L\) stands for the actuator limits.
    试一试Try this
    • 在"加权"模式下，把任务 1 的残差（解到红线的距离）压到 0.01 以下。需要多大的 \(w\)？如果要压到 0.0001 呢？In "weighted" mode, push task 1's residual (the solution's distance from the red line) below 0.01. How large must \(w\) be? And for 0.0001?
    • 切换到"严格优先"，把红线整条拖到方框外面（离方框至少 0.05）。现在任务 1 根本做不到：严格优先级给出的是哪个点？任务 2 还有没有发言权？Switch to "strict priority" and drag the red line entirely outside the box (at least 0.05 away from it). Task 1 is now impossible: which point does the strict hierarchy return? Does task 2 still have any say?
    核心洞见Key insight

    权重表达的是"多重要"，不是"谁优先"：加权解永远是一个折中，权重比要非常大才接近严格优先，而极端的权重比又会让问题病态。严格优先级把问题拆成一串 QP，每一层只在上一层的最优解集里挑选。两种方式都把物理当作硬约束：不论任务怎么排，方框外的点都不会被选中。

    A weight says "how important", not "who goes first": the weighted solution is always a compromise, it approaches strict priority only for very large weight ratios, and extreme ratios make the problem ill-conditioned. A strict hierarchy splits the problem into a chain of QPs, each choosing only within the set of optimal solutions of the level above. Both treat physics as a hard constraint: however the tasks are ranked, no point outside the box is ever chosen.

    3动手玩：每 2 毫秒一个 QP，让机器人站住Play: one QP every 2 ms keeps a robot standing

    下面是完整的全身控制器在实时运行。每个周期它读取机器人的状态 \((\mathbf q,\dot{\mathbf q})\)，把四个任务写成关于 \(\ddot{\mathbf q}\) 的线性方程，再加上物理约束，解出 \((\ddot{\mathbf q},\mathbf f)\) 和由它们决定的 \(\boldsymbol\tau\)，把 \(\boldsymbol\tau\) 发给电机：

    • 质心（权重 10）：质心加速度跟踪一个 PD 规律，把质心拉向绿色空心圆（可拖动）。
    • 躯干（权重 5）：躯干保持直立。
    • 手（权重 1）：手跟踪红色空心圆（可拖动）。
    • 姿态（权重 0.001）：关节轻轻地回到初始角度，只用来消耗"剩下的"自由度。

    琥珀色箭头是仿真器里地面真正作用在脚上的力，青色楔形是控制器使用的摩擦锥；某个摩擦约束在 QP 里处于"有效"（力压在锥的边上）时，楔形变成红色。右侧是 8 个关节的力矩，竖线是上限。

    Below, the complete whole-body controller runs in real time. Every cycle it reads the robot's state \((\mathbf q,\dot{\mathbf q})\), writes four tasks as linear equations in \(\ddot{\mathbf q}\), adds the physical constraints, solves for \((\ddot{\mathbf q},\mathbf f)\) and the torques \(\boldsymbol\tau\) they imply, and sends \(\boldsymbol\tau\) to the motors:

    • Centre of mass (weight 10): the COM acceleration follows a PD law that pulls the COM toward the hollow green circle (draggable).
    • Torso (weight 5): keep the torso upright.
    • Hand (weight 1): the hand follows the hollow red circle (draggable).
    • Posture (weight 0.001): the joints drift gently back to their initial angles, just to use up the "leftover" freedom.

    The amber arrows are the forces the simulated ground really exerts on the feet; the cyan wedges are the friction cones the controller uses, and a wedge turns red when that friction constraint is active in the QP (the force sits on the edge of the cone). On the right are the torques of the eight joints, with the limits as end marks.

    FIG 2全身控制实验室：平面人形Whole-body control lab: a planar humanoid
    拖动绿色质心目标和红色手目标drag the green COM target and the red hand target
    琥珀色箭头：仿真接触力（1 m ↔ 800 N），画在地面以下、指向脚。绿色实心圆是质心，竖直虚线是它在地面上的投影；青色横条是支撑区间（最后方的脚跟到最前方的脚尖）。蓝色是前腿，紫色是后腿，红色是手臂。控制器给摩擦锥留了 10% 的安全余量（用 \(0.9\mu\)）。"推一下"在颈部施加 150 N、持续 0.1 s 的向前推力。 Amber arrows: simulated contact forces (1 m ↔ 800 N), drawn below the ground and pointing at the foot. The filled green circle is the COM and the vertical dashed line its projection on the ground; the cyan bar is the support interval (rearmost heel to foremost toe). Blue is the front leg, purple the back leg, red the arm. The controller keeps a 10% safety margin on the friction cones (it uses \(0.9\mu\)). "Push" applies a forward force of 150 N at the neck for 0.1 s.
    试一试Try this
    • 两脚着地，把质心目标往前拖，让前脚承担至少 75% 的竖直力，并等质心停稳（速度小于 3 cm/s）。把目标一直拖到最前面时，质心为什么停在离目标还差一点的地方？（看后腿。）With both feet down, drag the COM target forward until the front foot carries at least 75% of the vertical force, and wait for the COM to settle (speed below 3 cm/s). When you drag the target all the way forward, why does the COM stop a little short of it? (Look at the back leg.)
    • 先把质心移到前脚上方，再按"抬起后脚"，单脚站稳 2 秒（后脚跟离地超过 5 cm）。如果质心不在前脚上方就抬脚，会发生什么？First move the COM over the front foot, then press "Lift back foot" and balance on one foot for 2 s (back heel more than 5 cm off the ground). What happens if you lift the foot while the COM is not over the front foot?
    • 把腿部力矩上限降到 45 N·m 或更低，让至少一个力矩约束连续 1 秒处于有效状态（力矩条变红），而机器人仍然站着。QP 把负担转给了谁？Lower the leg torque limit to 45 N·m or less so that at least one torque constraint stays active for 1 s in a row (its bar turns red) while the robot keeps standing. Who did the QP hand the load to?
    核心洞见Key insight

    控制器从不直接决定"前脚出多少力"：它只说"质心往这里加速"，QP 自己在两脚之间分配力，并且保证每只脚的力都在摩擦锥里、压力中心在脚掌内、每个关节的力矩不超限。抬脚只是把一只脚的接触约束从 QP 里拿掉、换成一个"脚往上走"的任务；同一套代码，约束集合变了而已。力矩上限一旦起作用，QP 会改用别的关节、别的力分配，而不是像"先算后截断"那样把力矩硬削掉。

    The controller never decides "how much force the front foot gives": it only says "accelerate the COM toward here", and the QP distributes the force between the feet itself, guaranteeing that every foot force is inside its friction cone, every centre of pressure under its sole, and every joint torque within its limit. Lifting a foot just removes that foot's contact constraint from the QP and adds a "move the foot up" task: the same code, a different constraint set. Once a torque limit binds, the QP switches to other joints and another force distribution, instead of chopping the torque off the way "compute, then clip" would.

    4第二个视角：接触力住在锥里，内力是免费的Second view: contact forces live in cones, and internal forces are free

    两只脚站在地上时，"支撑体重"并不能唯一决定两只脚各自的力：你可以让两只脚互相向外蹬（或者互相向里夹），这种内力大小相等、方向相反，合力为零，不改变身体的运动。在平地上内力没什么用；但在斜面上它是救命的。

    下图让机器人站在一条 V 形沟里，前脚在右坡，后脚在左坡，坡度 \(\alpha\)。如果每只脚的力都是竖直的（"只撑住重量"），力和坡面法线的夹角就是 \(\alpha\)，需要 \(\mu\ge\tan\alpha\) 才不打滑。但两只脚可以向外蹬两侧坡面：内力把两个力都转向各自的法线，于是摩擦系数小得多也能站住，代价是关节要多出一些力矩。爬烟囱的人用的就是这一招。

    右上角切换控制器："QP" 把摩擦锥写成约束；"无锥约束"解的是同一个 QP，但把所有不等式（摩擦锥和力矩上限）都去掉，再把力矩截断到上限再发出，相当于"先求最小二乘解，再截断"。

    When both feet are on the ground, "carry the body weight" does not fix each foot's force uniquely: the feet may push outward against each other (or squeeze inward). Such internal forces are equal and opposite, sum to zero, and do not change the body's motion. On flat ground they are of little use; on slopes they save you.

    In the figure the robot stands in a V-shaped trench, the front foot on the right slope and the back foot on the left one, both at angle \(\alpha\). If each foot's force were vertical ("just carry the weight"), it would make the angle \(\alpha\) with the slope's normal, and the feet would need \(\mu\ge\tan\alpha\) not to slip. But the feet can push outward against the two slopes: the internal force turns both forces toward their normals, so the robot can stand on a much smaller friction coefficient, at the price of some extra joint torque. Chimney climbers use exactly this trick.

    The top-right selector switches controllers: "QP" writes the friction cones as constraints; "no cone constraints" solves the same QP with every inequality removed (friction cones and torque limits), then clips the torques to their limits before sending them, which amounts to "least-squares solution, then clip".

    FIG 3湿滑的 V 形沟：摩擦锥约束与内力A slippery V-shaped trench: cone constraints and internal force
    琥珀色箭头是仿真的接触力（1 m ↔ 800 N），青色楔形是控制器使用的摩擦锥（半角 \(\arctan0.9\mu\)），在 QP 里有效时变红。脚开始打滑时，那个接触点上画一个红色叉。改变坡度会把机器人重新放回沟里。"内力"读数是前脚受力的水平分量（向外蹬时为负，即坡面把前脚往回推）。 Amber arrows are the simulated contact forces (1 m ↔ 800 N); cyan wedges are the friction cones the controller uses (half-angle \(\arctan0.9\mu\)), red when active in the QP. When a foot starts to slip, a red cross marks that contact point. Changing the slope puts the robot back into the trench. The "internal force" readout is the horizontal component of the force on the front foot (negative when the feet push outward, i.e. the slope pushes the front foot back).
    试一试Try this
    • 用 QP 控制器，把 \(\mu\) 降到 0.25 或更低（远小于 \(\tan\alpha\)），让机器人站满 3 秒，两只脚的滑移都小于 1 cm。内力读数是多少？With the QP controller, lower \(\mu\) to 0.25 or less (well below \(\tan\alpha\)) and keep the robot standing for 3 s with both feet slipping less than 1 cm. What does the internal-force readout show?
    • 切换到"无锥约束"，在 \(\mu<\tan\alpha\) 下看着某只脚滑出超过 5 cm。再把 \(\mu\) 调到 \(\tan\alpha\) 以上并重置：这次还滑吗？Switch to "no cone constraints" and, with \(\mu<\tan\alpha\), watch a foot slide more than 5 cm. Then raise \(\mu\) above \(\tan\alpha\) and reset: does it still slide?

    5第三个视角：多一个接触，多一片天地Third view: one more contact, a lot more room

    9.2 节的结论是：在平地上静止站立，质心必须在支撑多边形上方。但这个结论只对"所有接触都在同一个水平面、只靠摩擦"成立。一旦手撑在墙上，墙可以水平地推身体，平衡条件就变了：静止时，所有接触力必须平衡重力，即 \(\sum_i\mathbf f_i=(0,mg)\)，且对任一点的力矩之和为零。对给定的接触点和摩擦系数，能做到这一点的质心横坐标构成一个区间（Bretl 与 Lall 称之为静平衡区域），求它的两端各是一个线性规划。

    下图在机器人前面立了一面墙。打开"手扶墙"，手会先靠到墙上，碰到之后控制器把它当作第三个接触（法线指向 \(-x\)，有自己的摩擦锥）。再把质心目标往前拖：绿色区间是当前接触集合下的静平衡区域，它远远越过了脚尖。

    The conclusion of Lesson 9.2 was that to stand still on flat ground the COM must lie above the support polygon. That holds only when all contacts lie in one horizontal plane and work by friction. Put a hand on a wall, and the wall can push the body horizontally: the balance condition changes. Standing still, the contact forces must balance gravity, \(\sum_i\mathbf f_i=(0,mg)\), with zero total moment about any point. For given contact points and friction coefficients the COM positions \(x\) for which this is possible form an interval (Bretl and Lall call it the static equilibrium region), and each of its two ends is a linear program.

    The figure puts a wall in front of the robot. Switch on "hand on wall": the hand first moves to the wall, and once it touches, the controller treats it as a third contact (normal along \(-x\), with its own friction cone). Then drag the COM target forward: the green interval is the static equilibrium region of the current contact set, and it reaches far beyond the toes.

    FIG 4扶墙前倾：多接触的静平衡区域Leaning on a wall: the multi-contact static equilibrium region
    拖动绿色质心目标drag the green COM target
    地面上的绿色横条是当前接触集合的静平衡区域（只检查力和摩擦，不检查力矩上限和够不够得着，那是 QP 的事），青色细条是只有两只脚时的区域。区域越过墙面时，横条在墙处截断并画一个箭头，数值见右侧读数。墙在 \(x=0.6\) m。躯干前倾超过 45° 时算作摔倒。这个场景里躯干任务的权重降到 0.1，身体可以前倾。 The green bar on the ground is the static equilibrium region of the current contact set (it checks only forces and friction, not torque limits or reach, which are the QP's business); the thin cyan bar is the region with the two feet alone. When the region reaches past the wall, its bar stops at the wall and ends in an arrow, and the value is in the readout. The wall is at \(x=0.6\) m. A torso pitched more than 45° counts as fallen. In this scene the torso task's weight is lowered to 0.1 so the body can lean.
    试一试Try this
    • 打开"手扶墙"，等手接触墙之后，把质心保持在前脚尖前方至少 4 cm 处，持续 2 秒。这时手上有多大的力？质心为什么跟不上目标？（看哪个锥是红的。）Switch on "hand on wall", wait for the hand to touch, then hold the COM at least 4 cm ahead of the front toe for 2 s. How large is the hand force? Why does the COM lag behind its target? (See which cone is red.)
    • 质心还在脚尖前方时，关掉"手扶墙"。绿色区间立刻缩回到两只脚上，质心落在了外面：机器人会怎样？While the COM is still ahead of the toe, switch "hand on wall" off. The green interval shrinks back to the feet at once and the COM is outside it: what does the robot do?
    一个概念，三个视角One idea, three views

    优化视角（FIG 1）：全身控制是"在物理允许的集合里，按优先顺序满足愿望"。力的视角（FIG 2、FIG 3）：QP 的大部分工作是分配接触力：在两只脚之间分配重量，在需要时加入内力，同时让每个力留在各自的锥里。接触集合的视角（FIG 4）：机器人能做什么，取决于它此刻碰着哪些东西；每加一个接触，QP 就多一组变量和约束，能达到的质心区域、能产生的加速度就变大。规划层（落脚点、扶哪里）决定接触集合，全身控制在给定的接触集合里把每一毫秒做对。

    Optimization view (FIG 1): whole-body control "satisfies wishes in order of priority, within the set physics allows". Force view (FIG 2, FIG 3): most of the QP's work is distributing contact forces: sharing the weight between the feet, adding internal forces when needed, and keeping every force inside its own cone. Contact-set view (FIG 4): what the robot can do depends on what it is touching right now; every extra contact adds variables and constraints to the QP and enlarges the COM region it can hold and the accelerations it can produce. A planning layer (footholds, where to brace) chooses the contact set, and whole-body control gets every millisecond right within it.

    6再看数学The math

    提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮（FIG 1 的加权路径和严格解，FIG 2 的浮动基座、接触点、摩擦锥、力矩条和质心）。Tip: hover the highlighted symbols in the formulas and the matching element lights up (FIG 1's weighted path and strict solution; FIG 2's floating base, contact points, friction cones, torque bars and COM).

    任务：加速度层面的线性方程Tasks: linear equations at the acceleration level

    任务坐标 \(\mathbf x_k=\boldsymbol\phi_k(\mathbf q)\)（质心位置、手的位置、躯干角、某个关节角……）的加速度对 \(\ddot{\mathbf q}\) 是仿射的：\(\ddot{\mathbf x}_k=J_k\ddot{\mathbf q}+\dot J_k\dot{\mathbf q}\)（7.4 节）。期望加速度用和 13.6 节一样的 PD 规律给出，于是每个任务是 \(\ddot{\mathbf q}\) 的一个线性最小二乘项：

    The acceleration of a task coordinate \(\mathbf x_k=\boldsymbol\phi_k(\mathbf q)\) (COM position, hand position, torso angle, a joint angle, …) is affine in \(\ddot{\mathbf q}\): \(\ddot{\mathbf x}_k=J_k\ddot{\mathbf q}+\dot J_k\dot{\mathbf q}\) (Lesson 7.4). The desired acceleration comes from the same PD law as in Lesson 13.6, so each task is one linear least-squares term in \(\ddot{\mathbf q}\):

    $$ \hl{com}{e_k(\ddot{\mathbf q})=\big\|J_k\ddot{\mathbf q}+\dot J_k\dot{\mathbf q}-\ddot{\mathbf x}_k^{\mathrm{des}}\big\|^2},\qquad \ddot{\mathbf x}_k^{\mathrm{des}}=\ddot{\mathbf x}_k^{\mathrm{ref}}+K_{d}\big(\dot{\mathbf x}_k^{\mathrm{ref}}-\dot{\mathbf x}_k\big)+K_{p}\big(\mathbf x_k^{\mathrm{ref}}-\mathbf x_k\big). $$

    质心的雅可比就是各连杆质心雅可比按质量的加权平均，\(J_{\mathrm{com}}=\tfrac1m\sum_i m_iJ_{c,i}\)。本课的增益都按临界阻尼取：质心 \(\omega=10\) rad/s（\(K_p=100,\ K_d=20\)），躯干 14 rad/s，手 10 rad/s，姿态 6 rad/s。

    The COM Jacobian is the mass-weighted average of the links' centre-of-mass Jacobians, \(J_{\mathrm{com}}=\tfrac1m\sum_i m_iJ_{c,i}\). All gains in this lesson are critically damped: COM \(\omega=10\) rad/s (\(K_p=100,\ K_d=20\)), torso 14 rad/s, hand 10 rad/s, posture 6 rad/s.

    一个 QP：变量、目标与物理约束One QP: variables, objective and physical constraints

    把所有任务加权相加，再写上物理（第一行是目标，下面四行是约束）：

    Add all tasks with weights and write down the physics (the first line is the objective, the four lines below it are the constraints):

    $$ \begin{aligned} \min_{\ddot{\mathbf q},\,\boldsymbol\tau,\,\mathbf f}\quad & \hl{wsum}{\textstyle\sum_k w_k\, e_k(\ddot{\mathbf q})}+\varepsilon_a\|\ddot{\mathbf q}\|^2+\varepsilon_f\|\mathbf f\|^2\\ & \hl{dyn}{M(\mathbf q)\ddot{\mathbf q}+\mathbf h(\mathbf q,\dot{\mathbf q})=S^{\top}\boldsymbol\tau+J_c(\mathbf q)^{\top}\mathbf f}\\ & \hl{contact}{J_c\ddot{\mathbf q}+\dot J_c\dot{\mathbf q}=\mathbf 0}\\ & \hl{cone}{\mathbf f_i\in\mathcal K_i:\ |f_{t,i}|\le\mu f_{n,i}}\\ & \hl{tau}{\boldsymbol\tau_{\min}\le\boldsymbol\tau\le\boldsymbol\tau_{\max}} \end{aligned} $$

    逐行看：

    • 动力学：9.2 节的浮动基座方程，本节沿用 9.2 节的记号：\(\mathbf h\) 就是 8.3 节和 13.6 节的偏置项 \(\mathbf b\)（科氏/离心项与重力之和），\(S=[\,0\ \ I\,]\) 选出有电机的关节，\(\mathbf f\) 是接触力。8.4 节把同一个方程写成 \(S\boldsymbol\tau\) 和 \(\boldsymbol\lambda\)：那里的 \(S\) 就是这里的 \(S^\top\)；本节把 \(\lambda\) 留给 QP 的乘子。它是关于 \((\ddot{\mathbf q},\boldsymbol\tau,\mathbf f)\) 的线性等式：\(M,\mathbf h,J_c\) 在当前状态下都是已知的数。
    • 接触：着地的脚（或扶墙的手）不加速。脚是一个刚体，平面里约束它的 3 个加速度分量（脚跟点的 2 个加上脚的角加速度）；手是一个点接触，约束 2 个分量。本课实际用的是 \(J_c\ddot{\mathbf q}+\dot J_c\dot{\mathbf q}=-20\,J_c\dot{\mathbf q}\)，多出的一项把接触点残余的速度阻尼掉。
    • 摩擦锥：8.4 节的库仑锥。平面里它恰好是两个半平面 \((\pm\mathbf t-\mu\mathbf n)^{\top}\mathbf f_i\le0\)，是精确的线性约束（两条不等式合起来已经蕴含 \(f_n\ge0\)，只要 \(\mu>0\)）；三维里圆锥用 8.4 节的 \(k\) 棱锥内接近似，每个接触 4 到 8 条不等式。每只脚在脚跟、脚尖各有一个接触点，两个法向力都非负，正好等价于"压力中心在脚掌内"（9.2 节的 ZMP 条件）。
    • 力矩上限：盒约束。

    目标是二次的，约束全是线性的：这是一个凸 QP。两个很小的正则项 \(\varepsilon_a=10^{-4}\)、\(\varepsilon_f=10^{-3}\)（力以 100 N 为单位）让 Hessian 严格正定、解唯一，也决定了"什么都不影响任务时"内力取最小。

    Row by row:

    • Dynamics: the floating-base equation of Lesson 9.2, in 9.2's notation: \(\mathbf h\) is the bias vector that Lesson 8.3 and Lesson 13.6 call \(\mathbf b\) (Coriolis/centrifugal terms plus gravity), \(S=[\,0\ \ I\,]\) selects the motorized joints, and \(\mathbf f\) holds the contact forces. Lesson 8.4 writes the same equation with \(S\boldsymbol\tau\) and \(\boldsymbol\lambda\): its \(S\) is our \(S^\top\), and this lesson keeps \(\lambda\) for the QP's multipliers. It is a linear equality in \((\ddot{\mathbf q},\boldsymbol\tau,\mathbf f)\): at the current state \(M,\mathbf h,J_c\) are known numbers.
    • Contact: a foot on the ground (or the hand on the wall) does not accelerate. A foot is a rigid body, so in the plane its 3 acceleration components are constrained (2 at the heel point plus the foot's angular acceleration); the hand is a point contact with 2. The lesson actually uses \(J_c\ddot{\mathbf q}+\dot J_c\dot{\mathbf q}=-20\,J_c\dot{\mathbf q}\), whose extra term damps any residual velocity of the contact points.
    • Friction cone: the Coulomb cone of Lesson 8.4. In the plane it is exactly two half-planes \((\pm\mathbf t-\mu\mathbf n)^{\top}\mathbf f_i\le0\), an exact linear constraint (for \(\mu>0\) the two inequalities together already imply \(f_n\ge0\)); in 3D the round cone is replaced by Lesson 8.4's inscribed \(k\)-edge pyramid, 4 to 8 inequalities per contact. Each foot has contact points at the heel and the toe, and both normal forces being non-negative is exactly "the centre of pressure lies under the sole" (the ZMP condition of Lesson 9.2).
    • Torque limits: box constraints.

    Quadratic objective, linear constraints: a convex QP. Two small regularizers \(\varepsilon_a=10^{-4}\) and \(\varepsilon_f=10^{-3}\) (forces in units of 100 N) make the Hessian strictly positive definite and the solution unique, and decide that internal forces are as small as possible whenever they do not matter for the tasks.

    消去 \(\boldsymbol\tau\)：只剩基座的那几行Eliminating \(\boldsymbol\tau\): only the base rows remain

    把动力学方程按"基座行"（下标 u，没有电机）和"关节行"（下标 a）拆开：

    Split the dynamics into the "base rows" (subscript u, unactuated) and the "joint rows" (subscript a):

    $$ M_u\ddot{\mathbf q}+\mathbf h_u=J_{c,u}^{\top}\mathbf f,\qquad \boldsymbol\tau=M_a\ddot{\mathbf q}+\mathbf h_a-J_{c,a}^{\top}\mathbf f . $$

    第一式是 3 行（三维里 6 行）。关节行里每个 \(\tau_j\) 都可以由 \((\ddot{\mathbf q},\mathbf f)\) 直接算出，所以 \(\boldsymbol\tau\) 不必当作变量：力矩上限变成关于 \((\ddot{\mathbf q},\mathbf f)\) 的线性不等式。本课的实现就是这样做的。两脚着地时变量是 \(\ddot{\mathbf q}\in\R^{11}\) 加 4 个接触点 × 2 个力分量，共 19 个；等式 9 条（3 条基座行 + 2 只脚 × 3 条接触行）；不等式 24 条（4 个点 × 2 条摩擦 + 8 个关节 × 2 条力矩）。抬起一只脚后变成 15 个变量、6 条等式、20 条不等式。基座行正是"躯干只能靠接触力加速"这句话的方程形式：去掉所有接触，\(J_{c,u}^\top\mathbf f=0\)，质心只能自由落体。

    The first equation has 3 rows (6 in 3D). Each \(\tau_j\) in the joint rows can be computed directly from \((\ddot{\mathbf q},\mathbf f)\), so \(\boldsymbol\tau\) need not be a variable: the torque limits become linear inequalities in \((\ddot{\mathbf q},\mathbf f)\). That is how this lesson's implementation works. With both feet down the variables are \(\ddot{\mathbf q}\in\R^{11}\) plus 4 contact points × 2 force components, 19 in all; 9 equalities (3 base rows + 2 feet × 3 contact rows); 24 inequalities (4 points × 2 friction rows + 8 joints × 2 torque rows). With one foot lifted: 15 variables, 6 equalities, 20 inequalities. The base rows are the equation behind "the trunk can only be accelerated by contact forces": remove all contacts, \(J_{c,u}^\top\mathbf f=0\), and the COM can only fall freely.

    严格优先级：一串 QPStrict priorities: a chain of QPs

    把任务分成层 \(1,\dots,p\)。第 \(k\) 层解

    Group the tasks into levels \(1,\dots,p\). Level \(k\) solves

    $$ \hl{lex}{\mathbf x_k^{*}=\arg\min_{\mathbf x}\ \|A_k\mathbf x-\mathbf b_k\|^2},\qquad \mathbf x\in\mathcal P,\qquad A_j\mathbf x=A_j\mathbf x_j^{*}\ \ (j\lt k), $$

    其中 \(\mathbf x=(\ddot{\mathbf q},\mathbf f)\)，\(\mathcal P\) 是满足上面全部物理约束的集合。因为 \(\|A_j\mathbf x-\mathbf b_j\|^2\) 对 \(A_j\mathbf x\) 是严格凸的，第 \(j\) 层的最优值 \(A_j\mathbf x_j^*\) 是唯一的，把它冻结下来，后面的层就只能在"不让第 \(j\) 层变差"的点里挑。只有等式任务、没有不等式时，这和 13.6 节的零空间投影（Nakamura 等，1987；Siciliano 与 Slotine，1991）给出同样的结果；一旦有不等式（摩擦锥、力矩上限，或者"关节角不超过某值"这类不等式任务），零空间公式就失效，必须用分层 QP（Kanoun、Lamiraux 与 Wieber，2011；Escande、Mansard 与 Wieber，2014）。本课的实现有两个工程细节：冻结的等式写成 \(\pm10^{-6}\) 的细带，防止数值误差让下一层被判为不可行；低层加了更大的阻尼 \(\varepsilon=10^{-2}\)，避免够不着的目标在奇异位形附近要求极大的加速度（与 13.6 节的阻尼同一个道理）。

    with \(\mathbf x=(\ddot{\mathbf q},\mathbf f)\) and \(\mathcal P\) the set satisfying all the physical constraints above. Since \(\|A_j\mathbf x-\mathbf b_j\|^2\) is strictly convex in \(A_j\mathbf x\), the optimal value \(A_j\mathbf x_j^*\) of level \(j\) is unique; freezing it means later levels may only choose among points that do not make level \(j\) worse. With equality tasks only and no inequalities this gives the same result as the null-space projection of Lesson 13.6 (Nakamura et al., 1987; Siciliano and Slotine, 1991); once inequalities appear (friction cones, torque limits, or inequality tasks such as "this joint angle stays below a bound") the null-space formulas fail and a hierarchical QP is needed (Kanoun, Lamiraux and Wieber, 2011; Escande, Mansard and Wieber, 2014). This lesson's implementation has two engineering details: the frozen equalities are thin bands of \(\pm10^{-6}\), so that round-off does not make the next level look infeasible, and lower levels get a larger damping \(\varepsilon=10^{-2}\), so that an unreachable target does not demand huge accelerations near a singular posture (the same reason as the damping of Lesson 13.6).

    加权和分层的关系：把加权目标写成 \(w\,e_1+e_2\)，令 \(w\to\infty\)，解收敛到分层解（FIG 1 的紫色路径）。但有限的 \(w\) 总是折中，而太大的 \(w\) 让 Hessian 的条件数随 \(w\) 增长，数值上不可靠。实践中两者常常混用：少数几层严格优先级，每层内部用权重。

    How weights and levels relate: write the weighted objective as \(w\,e_1+e_2\) and let \(w\to\infty\); the solution converges to the hierarchical one (FIG 1's purple path). A finite \(w\) is always a compromise, though, and a very large \(w\) makes the Hessian's condition number grow with \(w\), which is numerically unreliable. In practice the two are mixed: a few strictly ordered levels, with weights inside each level.

    求解器：从无约束最小值出发的对偶有效集法The solver: a dual active-set method that starts at the unconstrained minimum

    本页所有的 QP 都由一个内嵌的 Goldfarb–Idnani 对偶有效集法求解（1983，也是 quadprog 和 eiquadprog 用的算法）：先求无约束最小值 \(\mathbf x=-H^{-1}\mathbf g\)，然后每次把一条违反的约束加入有效集，沿着保持 KKT 条件的方向走，如果某条有效约束的乘子要变负，就把它移出。它不需要可行的初始点，而且每一步结束时都满足 6.7 节的对偶可行性。站立时它通常 0 到 6 次迭代就结束。返回的乘子 \(\lambda\) 有明确含义：某个力矩上限的 \(\lambda\) 是"这个上限每放宽 1 N·m，求解器的目标 \(\tfrac12\mathbf x^\top H\mathbf x+\mathbf g^\top\mathbf x\) 的最优值下降多少"（一阶近似），FIG 2 用它判断哪些约束在起作用。

    Every QP on this page is solved by an embedded Goldfarb–Idnani dual active-set method (1983; the algorithm behind quadprog and eiquadprog): start from the unconstrained minimum \(\mathbf x=-H^{-1}\mathbf g\), add one violated constraint at a time to the active set, step along a direction that keeps the KKT conditions, and drop an active constraint whose multiplier would turn negative. It needs no feasible starting point, and it stays dual feasible in the sense of Lesson 6.7 after every step. While the robot stands it usually finishes in 0 to 6 iterations. The returned multipliers \(\lambda\) have a clear meaning: the \(\lambda\) of a torque limit says how much the optimal value of the solver's objective \(\tfrac12\mathbf x^\top H\mathbf x+\mathbf g^\top\mathbf x\) would drop per N·m that limit is relaxed (to first order), and FIG 2 uses them to tell which constraints are working.

    7机器人中的它：保护平衡的优先级In robotics: priorities that protect balance

    真实的人形机器人要伸手去够一个远处的东西时，最重要的是不要摔倒。下图让手去追一个可以拖到很远的目标，比较两种写法：

    • 加权：质心 10，手 \(w_{\mathrm{h}}\)（滑块），躯干 0.1，姿态 0.001。
    • 严格优先：第 1 层质心（加上抬脚时的摆动脚），第 2 层手，第 3 层躯干和姿态。

    两种写法都有同样的物理约束，QP 都不会让脚打滑或抬起。区别在于：加权时，手的权重越大，QP 越愿意牺牲质心去换手的距离，质心可能被拖到脚尖附近，失去了承受扰动的余量。

    When a real humanoid reaches for something far away, the most important thing is not to fall over. In the figure the hand chases a target you can drag far ahead, under two formulations:

    • Weighted: COM 10, hand \(w_{\mathrm{h}}\) (slider), torso 0.1, posture 0.001.
    • Strict priority: level 1 the COM (plus the swing foot when one is lifted), level 2 the hand, level 3 torso and posture.

    Both have the same physical constraints, and in both the QP never lets a foot slip or lift. The difference: with weights, the heavier the hand weight, the more willing the QP is to trade COM position for hand distance, and the COM may be dragged near the toe, losing the margin it needs to absorb a disturbance.

    FIG 5伸手够远处：加权与分层 QPReaching far: weighted versus hierarchical QP
    拖动红色手目标drag the red hand target
    绿色空心圆是固定的质心目标（初始质心位置），绿色实心圆是实际质心；青色横条是支撑区间。"推一下"在颈部施加 200 N、持续 0.1 s 的向前推力。分层模式每个周期解 3 个 QP。在较慢的设备上仿真会自动放慢，但时间步长不变。 The hollow green circle is the fixed COM target (the initial COM position), the filled green circle the actual COM; the cyan bar is the support interval. "Push" applies a forward force of 200 N at the neck for 0.1 s. The hierarchical mode solves 3 QPs per cycle. On slower devices the simulation automatically runs slower than real time, with the same time step.
    试一试Try this
    • 在"加权"模式下把 \(w_{\mathrm{h}}\) 调到 100 或更大，把手目标拖到 \(x\ge1.1\) m（手臂根本够不着）。机器人会怎样？In "weighted" mode set \(w_{\mathrm{h}}\) to 100 or more and drag the hand target to \(x\ge1.1\) m (far out of the arm's reach). What does the robot do?
    • 切换到"严格优先"，同样把手目标放在 \(x\ge1.1\) m：机器人站满 3 秒，质心离目标不到 5 mm，而手离目标还很远。Switch to "strict priority" with the hand target again at \(x\ge1.1\) m: the robot stands for 3 s with the COM within 5 mm of its target, while the hand is still far from its target.
    • 严格优先模式下，手目标放在 \(x\ge0.9\) m，按"推一下"，推完后站稳 2 秒。再在加权模式（\(w_{\mathrm{h}}=100\)）下对目标 \((1.0,\,0.8)\) 做同样的事。In strict-priority mode, with the hand target at \(x\ge0.9\) m, press "Push" and stay up for 2 s afterwards. Then do the same in weighted mode (\(w_{\mathrm{h}}=100\)) with the target at \((1.0,\,0.8)\).
    真实系统里In real systems

    DARPA 机器人挑战赛的 Atlas：多支队伍都用 QP 全身控制驱动 Boston Dynamics 的液压人形 Atlas。MIT 队的控制器每个周期解一个 QP，用一个利用稀疏性、并从上一周期的有效集热启动的定制有效集求解器，使整个控制周期达到 1 kHz（Kuindersma、Permenter 与 Tedrake，2014；Kuindersma 等，2016）；IHMC 队采用基于动量的 QP 框架（Koolen 等，2016），WPI–CMU 队采用全身优化控制（Feng 等，2015）。四足：ANYmal 上用分层 QP 做全身控制（Bellicoso 等，2016）；MIT Mini Cheetah 把凸 MPC（13.4 节）给出的地面反力交给"全身冲量控制"WBIC，由它算出关节力矩（Kim 等，2019）：这就是 9.2 节提到的"MPC + 全身控制"分层结构，MPC 负责往前看几百毫秒，WBC 负责把每一毫秒做对。扭矩控制的人形：Sarcos 人形下半身上的分层逆动力学与动量控制（Herzog 等，2016），力矩控制下的接触力优化分配（Righetti 等，2013）。软件：TSID（基于 Pinocchio，Stack of Tasks 系列）、OpenSoT、IHMC 的开源控制栈；常用 QP 求解器有 eiquadprog、qpOASES、OSQP 和 ProxQP。

    Atlas at the DARPA Robotics Challenge: several teams drove Boston Dynamics' hydraulic humanoid Atlas with QP-based whole-body control. MIT's controller solved one QP per cycle and used a custom active-set solver that exploits sparsity and warm-starts from the previous cycle's active set, fast enough for 1 kHz control (Kuindersma, Permenter and Tedrake, 2014; Kuindersma et al., 2016); IHMC used a momentum-based QP framework (Koolen et al., 2016), and WPI–CMU an optimization-based full-body controller (Feng et al., 2015). Quadrupeds: ANYmal uses a hierarchical QP for whole-body control (Bellicoso et al., 2016); the MIT Mini Cheetah hands the ground reaction forces from its convex MPC (Lesson 13.4) to "whole-body impulse control" (WBIC), which computes the joint torques (Kim et al., 2019): the "MPC + whole-body control" layering mentioned in Lesson 9.2, with MPC looking a few hundred milliseconds ahead and WBC getting every millisecond right. Torque-controlled humanoids: hierarchical inverse dynamics with momentum control on the lower body of the Sarcos humanoid (Herzog et al., 2016) and optimal contact-force distribution under inverse-dynamics control (Righetti et al., 2013). Software: TSID (built on Pinocchio, from the Stack of Tasks family), OpenSoT and IHMC's open-source control stack; common QP solvers are eiquadprog, qpOASES, OSQP and ProxQP.

    8常见误区Common pitfalls

    误区 1：用权重表达优先级Pitfall 1: expressing priority with weights

    FIG 1 里，方框不起作用时，任务 1 的残差是 \(d/(1+w)\)：要比 \(d\) 小 100 倍，\(w\) 就要约 100；小 10⁴ 倍就要约 10⁴。权重比太大时 Hessian 的条件数也跟着变大，数值误差会污染低权重的任务。FIG 5 里，\(w_{\mathrm{h}}=100\) 的机器人站着，却被一次 200 N 的推力推倒，而分层 QP 承受住了同样的推力：权重的问题常常不在静止时暴露，而在扰动下暴露。安全相关的任务（平衡、接触）应该放在更高的层里，或者干脆写成约束。

    In FIG 1, while the box is inactive, task 1's residual is \(d/(1+w)\): to make it 100 times smaller than \(d\) you need \(w\approx100\), for 10⁴ times \(w\approx10^4\). Large weight ratios also inflate the Hessian's condition number, and round-off then pollutes the low-weight tasks. In FIG 5 the robot with \(w_{\mathrm{h}}=100\) stands, yet one 200 N push knocks it over, while the hierarchical QP survives the same push: the trouble with weights often shows not at rest but under disturbance. Safety-relevant tasks (balance, contacts) belong in higher levels, or should simply be constraints.

    误区 2：先求解，再截断Pitfall 2: solve first, clip afterwards

    把不等式丢掉、求一个最小二乘解、再把力矩截断到上限、把力"投影"回锥里，看上去省事，但截断后的力矩和计划的接触力已经不满足动力学方程了：实际的接触力会自己"找"一个解，往往就在锥外。FIG 3 的"无锥约束"就是这样：\(\mu<\tan\alpha\) 时脚顺着坡面滑下去。约束必须放在优化里面，求解器才能用内力、换关节、改分配来满足它们。

    Dropping the inequalities, solving least squares, then clipping the torques to their limits and "projecting" the forces back into the cones looks convenient, but the clipped torques and the planned contact forces no longer satisfy the equations of motion: the actual contact forces "find" their own solution, often outside the cone. That is FIG 3's "no cone constraints": for \(\mu<\tan\alpha\) the feet simply slide down the slopes. Constraints must live inside the optimization, so that the solver can satisfy them with internal forces, other joints and other distributions.

    误区 3：用真实的 \(\mu\)、硬接触且不留后路Pitfall 3: using the true \(\mu\), hard contacts and no fallback

    QP 会把力推到锥的边上（FIG 3 里红色的锥），而真实的 \(\mu\) 只是估计值，三维的棱锥近似又会改变边界，所以控制器要用比估计值小的 \(\mu\)（本课用 0.9 倍，工程上常常更保守）。另一方面，硬约束之间可能互相矛盾，例如力矩上限太低、根本撑不住体重：QP 无解。控制器必须有后备方案，常见做法是给接触或任务约束加松弛变量。本课的后备方案是：QP 无解时，把接触约束改成权重很大的任务再解一次。

    The QP pushes forces right onto the edge of the cone (the red cones of FIG 3), while the true \(\mu\) is only an estimate and 3D pyramid approximations move the boundary, so the controller should use a smaller \(\mu\) than estimated (this lesson uses 0.9 times; practice is often more conservative). Separately, hard constraints can contradict each other, say torque limits too low to carry the body weight: the QP has no solution. A controller needs a fallback, typically slack variables on contact or task constraints. This lesson's fallback: when the QP is infeasible, the contact constraints become tasks with a very large weight and the QP is solved again.

    误区 4：突然切换接触Pitfall 4: switching contacts abruptly

    本课的"抬起后脚"是突然把一只脚的接触约束拿掉。如果那一刻那只脚还承担着体重，QP 的解会跳变：力矩突变，机器人可能摔倒（FIG 2 的第二个任务）。真实控制器会在抬脚前用几十到几百毫秒把这只脚的力降到零（例如对它的法向力加一个随时间收紧的上限），落脚时同样逐渐加载；接触检测本身也会有误差，需要容错。

    This lesson's "Lift back foot" removes a foot's contact constraint at once. If that foot still carries weight at that moment, the QP's solution jumps: the torques change abruptly and the robot may fall (FIG 2's second task). Real controllers unload the foot over tens to hundreds of milliseconds before lift-off (for example with an upper bound on its normal force that tightens over time), and load it gradually at touchdown; contact detection itself is imperfect and must be handled gracefully.

    误区 5：指望全身控制"往前看"Pitfall 5: expecting whole-body control to look ahead

    WBC 的 QP 只管当前这一个周期的加速度。它保证这一刻的力是可行的，却不保证一秒以后还有可行解：质心以很大的速度冲向脚尖时，每一步的 QP 都可行，最后仍然会倒。要避免这类情况，需要能预测未来的层：MPC（13.4 节）、捕获点式的落脚规划（9.2 节），或者学习得到的策略。WBC 是执行层，不是规划层。

    The WBC QP only decides the accelerations of the current cycle. It guarantees that the forces are feasible now, not that a feasible solution will still exist a second later: if the COM rushes toward the toe fast enough, every single QP is feasible and the robot still falls. Avoiding that needs a layer that predicts: MPC (Lesson 13.4), capture-point footstep planning (Lesson 9.2), or a learned policy. WBC is the execution layer, not the planning layer.

    9小练习Check yourself

    1. 浮动基座运动方程的前 3 行（平面）里没有关节力矩。这意味着什么？1. The first 3 rows (planar) of the floating-base equations of motion contain no joint torque. What does that mean?
    \(S^\top\boldsymbol\tau\) 的前几个分量为零，所以 \(M_u\ddot{\mathbf q}+\mathbf h_u=J_{c,u}^\top\mathbf f\)。这正是 QP 里最关键的等式：没有它，控制器会假装能直接"推躯干"。删掉它（选项 A）就丢掉了物理；它并不要求加速度为零（B）；电机也无法直接作用在基座上（D），只能通过接触力间接作用。The first components of \(S^\top\boldsymbol\tau\) are zero, so \(M_u\ddot{\mathbf q}+\mathbf h_u=J_{c,u}^\top\mathbf f\). It is the most important equality of the QP: without it the controller would pretend it could "push the trunk" directly. Dropping it (option A) throws away the physics; it does not force zero acceleration (B); and motors cannot act on the base directly (D), only indirectly through contact forces.
    2. 任务 1 的权重 \(w=1000\)，任务 2 的权重为 1，两者冲突、约束不起作用。下面哪个说法对？2. Task 1 has weight \(w=1000\), task 2 weight 1; they conflict and no constraint is active. Which statement is right?
    FIG 1 里残差正好是 \(d/(1+w)\)，\(w=1000\) 时约为 \(d/1000\)，不是零（A 错）。任务 2 仍在起作用，它决定了解沿着红线方向的位置，也让解稍微偏离红线（C 错）。权重不影响可行性（D 错）。In FIG 1 the residual is exactly \(d/(1+w)\), about \(d/1000\) for \(w=1000\), not zero (A is wrong). Task 2 still acts: it fixes where along the red line the solution sits, and pulls it slightly off the line (C is wrong). Weights do not affect feasibility (D is wrong).
    3. FIG 3 里，\(\mu=0.2\)、坡度 25°（\(\tan25^\circ\approx0.47\)）。QP 控制器站住了，"无锥约束"控制器滑倒了。为什么？3. In FIG 3 with \(\mu=0.2\) and a 25° slope (\(\tan25^\circ\approx0.47\)), the QP controller stands while the "no cone constraints" controller slides. Why?
    两只脚的内力合力为零，不改变身体的运动，所以任务代价对它几乎无所谓；只有把摩擦锥写成约束，QP 才"知道"需要它。QP 用的是 \(0.9\mu\)，比真实值还小（B 错）；仿真的接触只能推不能拉（C 错）；在沟里站立需要的力矩不大，截断并不是原因（D 错）。The internal force sums to zero and does not change the body's motion, so the task cost hardly cares about it; only with the friction cones as constraints does the QP "know" it is needed. The QP uses \(0.9\mu\), even less than the true value (B is wrong); the simulated contacts can only push, never pull (C is wrong); standing in the trench needs little torque, so clipping is not the cause (D is wrong).
    4. 本课的平面机器人有 11 个广义坐标、8 个电机，两只脚各有 2 个接触点（每点 2 个力分量）。如果把 \(\ddot{\mathbf q}\)、\(\boldsymbol\tau\)、\(\mathbf f\) 都当作变量，QP 有多少个变量？消去 \(\boldsymbol\tau\) 之后呢？4. This lesson's planar robot has 11 generalized coordinates and 8 motors, and each foot has 2 contact points (2 force components each). With \(\ddot{\mathbf q}\), \(\boldsymbol\tau\) and \(\mathbf f\) all as variables, how many variables does the QP have? And after eliminating \(\boldsymbol\tau\)?
    \(11+8+4\times2=27\)。关节行把每个 \(\tau_j\) 写成 \((\ddot{\mathbf q},\mathbf f)\) 的函数，消去 8 个力矩后剩 \(11+8=19\) 个变量，力矩上限变成 16 条关于它们的不等式。\(11+8+4\times2=27\). The joint rows express each \(\tau_j\) through \((\ddot{\mathbf q},\mathbf f)\), so eliminating the 8 torques leaves \(11+8=19\) variables, and the torque limits become 16 inequalities in them.
    5. 在三维里，为什么全身控制常把摩擦圆锥换成一个内接的四棱锥？5. In 3D, why does whole-body control often replace the friction cone by an inscribed four-sided pyramid?
    圆锥 \(\|\mathbf f_t\|\le\mu f_n\) 是凸的（C 错），但它是二阶锥约束，不是线性的，需要 SOCP 求解器；四棱锥 \(|f_x|\le\tfrac{\mu}{\sqrt2}f_n,\ |f_y|\le\tfrac{\mu}{\sqrt2}f_n\)（内接）或 \(|f_x|,|f_y|\le\mu f_n\)（外切）是 4 条线性不等式。内接的那个比圆锥小（A 错），保证安全；任何一种都要求 \(f_n\ge0\)（D 错）。The cone \(\|\mathbf f_t\|\le\mu f_n\) is convex (C is wrong), but it is a second-order cone constraint, not linear, and needs an SOCP solver; the pyramid \(|f_x|\le\tfrac{\mu}{\sqrt2}f_n,\ |f_y|\le\tfrac{\mu}{\sqrt2}f_n\) (inscribed) or \(|f_x|,|f_y|\le\mu f_n\) (circumscribed) is 4 linear inequalities. The inscribed one is smaller than the cone (A is wrong), which keeps it safe; both require \(f_n\ge0\) (D is wrong).

    10延伸阅读Further reading

    • L. Sentis & O. Khatib, Synthesis of Whole-Body Behaviors through Hierarchical Control of Behavioral Primitives, Int. J. Humanoid Robotics 2(4), 2005 — 把操作空间控制推广到带优先级的全身控制Operational-space control generalized to prioritized whole-body control
    • Y. Nakamura, H. Hanafusa & T. Yoshikawa, Task-Priority Based Redundancy Control of Robot Manipulators, Int. J. Robotics Research 6(2), 1987; B. Siciliano & J.-J. E. Slotine, A General Framework for Managing Multiple Tasks in Highly Redundant Robotic Systems, ICAR 1991 — 零空间投影的任务优先级Task priority by null-space projection
    • O. Kanoun, F. Lamiraux & P.-B. Wieber, Kinematic Control of Redundant Manipulators: Generalizing the Task-Priority Framework to Inequality Task, IEEE Trans. Robotics 27(4), 2011 — 带不等式的任务优先级Task priorities with inequalities
    • A. Escande, N. Mansard & P.-B. Wieber, Hierarchical Quadratic Programming: Fast Online Humanoid-Robot Motion Generation, Int. J. Robotics Research 33(7), 2014 — 分层 QP 的高效求解Efficient solution of hierarchical QPs
    • L. Saab, O. E. Ramos, F. Keith, N. Mansard, P. Souères & J.-Y. Fourquet, Dynamic Whole-Body Motion Generation under Rigid Contacts and Other Unilateral Constraints, IEEE Trans. Robotics 29(2), 2013 — 动力学层面的分层控制与接触约束Hierarchical control at the dynamics level with contact constraints
    • L. Righetti, J. Buchli, M. Mistry, M. Kalakrishnan & S. Schaal, Optimal Distribution of Contact Forces with Inverse-Dynamics Control, Int. J. Robotics Research 32(3), 2013 — 接触力分配与内力Contact-force distribution and internal forces
    • S. Kuindersma, F. Permenter & R. Tedrake, An Efficiently Solvable Quadratic Program for Stabilizing Dynamic Locomotion, ICRA 2014; S. Kuindersma et al., Optimization-Based Locomotion Planning, Estimation, and Control Design for the Atlas Humanoid Robot, Autonomous Robots 40(3), 2016 — Atlas 上的实时 QP 控制Real-time QP control on Atlas
    • T. Koolen et al., Design of a Momentum-Based Control Framework and Application to the Humanoid Robot Atlas, Int. J. Humanoid Robotics 13(1), 2016 — IHMC 的动量 QP 框架IHMC's momentum-based QP framework
    • T. Bretl & S. Lall, Testing Static Equilibrium for Legged Robots, IEEE Trans. Robotics 24(4), 2008 — 多接触的静平衡区域（FIG 4）The multi-contact static equilibrium region (FIG 4)
    • D. Goldfarb & A. Idnani, A Numerically Stable Dual Method for Solving Strictly Convex Quadratic Programs, Mathematical Programming 27, 1983 — 本页使用的 QP 算法The QP algorithm used on this page
    • P. M. Wensing, M. Posa, Y. Hu, A. Escande, N. Mansard & A. Del Prete, Optimization-Based Control for Dynamic Legged Robots, IEEE Trans. Robotics 40, 2024 — 足式机器人 MPC 与全身控制的综述A survey of MPC and whole-body control for legged robots