4、运动控制基础：正逆运动学、步态规划、平衡控制
各位同学，今天我们来聊聊人形机器人运动控制里最核心的三个东西。说实话，这三个东西搞明白了，你基本就掌握了让机器人「站起来、走起来、不倒下去」的全部秘密。
我最早接触这个领域的时候，以为运动控制就是写几个PID循环。后来发现，嗯，太天真了。人形机器人比工业机械臂复杂得多——它有两条腿，两条胳膊，还有几十个关节要协调。你想想看，光让它在平地上走稳，就已经够折腾了。
4.1 正运动学与逆运动学
先说运动学。运动学分两种：正运动学和逆运动学。说白了，正运动学就是「我知道每个关节转了多少度，你告诉我手和脚在哪儿」。逆运动学反过来——「我知道手要伸到那个位置，你告诉我每个关节该转多少度」。
我在项目中遇到过这样一个坑：正运动学算起来很快，但逆运动学就麻烦了。因为逆运动学通常有多个解，甚至无解。比如你让机器人去够一个太远的东西，它胳膊不够长，那就无解。这时候你得做妥协，或者换个姿势。
核心公式（简化版）：
正运动学：T = f(θ₁, θ₂, ..., θₙ) —— 关节角度 → 末端位姿
逆运动学：θ₁, θ₂, ..., θₙ = f⁻¹(T) —— 末端位姿 → 关节角度
实际工程中，我们一般用DH参数法建立运动学模型。每个关节一个坐标系，然后通过齐次变换矩阵把末端位置算出来。逆运动学呢，我建议用数值解法——比如牛顿-拉夫森迭代法。虽然慢一点，但通用性强，不用针对每个机器人单独推导解析解。
我的小技巧： 调试逆运动学的时候，先用手动模式把机器人摆到目标位置，记录下关节角度，然后用这个角度去验证你的逆解对不对。我吃过这个亏——算出来一个解，结果机器人直接把自己拧成了麻花。
4.2 步态规划
步态规划，说白了就是「怎么迈腿」。人走路看起来简单，但让机器人走起来，你会发现每一步都是学问。
常见的步态有两种：静态步态和动态步态。静态步态走得慢，但稳——每一步都保证重心投影在支撑多边形内。动态步态走得快，但需要靠惯性来维持平衡——就像你跑步时其实是在「往前倒」，只是每次都在摔倒前迈出下一步。
我个人的习惯是：刚开始做步态规划时，先用静态步态。等机器人能稳定走了，再上动态步态。别一上来就搞动态，摔坏了电机心疼的是你。
步态规划的核心参数包括：
- 步长：每一步迈多远。步长太大，重心偏移太多，容易倒。
- 步频：每秒迈几步。步频太快，电机响应跟不上。
- 抬脚高度：脚离地多高。抬太高浪费能量，抬太低容易绊倒。
- 双支撑时间：两只脚同时着地的时间。这个时间越长越稳，但走得慢。
注意： 步态规划不是简单的正弦波。你得考虑脚落地时的冲击力。我曾经在水泥地上测试，结果脚底传感器直接爆了——因为落地速度没控制好。后来我加了落地缓冲曲线，问题才解决。
下面是我常用的一个简单步态规划流程：
// 伪代码：单步规划
function planSingleStep(startPos, endPos, stepHeight, stepTime):
// 1. 计算步长向量
stepVector = endPos - startPos
// 2. 生成足端轨迹（用三次样条或贝塞尔曲线）
for t in [0, stepTime]:
// 水平方向：匀速运动
x(t) = startPos.x + stepVector.x * (t / stepTime)
// 垂直方向：先抬后落，中间最高
z(t) = startPos.z + stepHeight * sin(π * t / stepTime)
// 3. 用逆运动学计算关节角度
jointAngles = inverseKinematics(x(t), z(t))
return jointAngles
4.3 平衡控制
平衡控制，这是人形机器人最头疼的问题。没有之一。
你想想看，一个1米多高的机器人，几十公斤重，就靠两个脚掌撑着。稍微有点扰动——比如地面不平、被人推了一下、或者自己迈步时重心没跟上——立马就倒。
平衡控制的核心是「零力矩点」（ZMP）理论。ZMP是地面反作用力的等效作用点。如果ZMP始终落在支撑多边形内，机器人就不会倒。支撑多边形就是脚掌的轮廓——单脚支撑时就是那只脚的轮廓，双脚支撑时就是两只脚围成的区域。
ZMP判据：
ZMP ∈ 支撑多边形 → 稳定
ZMP ∉ 支撑多边形 → 即将摔倒
实际控制中，我们通常用PD控制器来调节ZMP。比如检测到ZMP偏左了，就调整髋关节和踝关节的角度，把重心往右拉回来。我做过一个实验：让机器人站在一个晃动的平台上，ZMP控制器的响应频率至少要50Hz以上才能稳住。低于这个频率，机器人就像喝醉了一样晃来晃去。
还有一种方法是「角动量控制」。说白了，就是利用手臂的摆动来抵消身体的旋转。你走路时手臂会自然摆动，对吧？机器人也一样。我见过一个团队，他们的机器人被推了一下，手臂立刻甩起来，像走钢丝的人那样保持平衡——看着挺酷的。
避坑指南： 我曾经在平衡控制里犯过一个低级错误——把IMU装反了。结果机器人一启动就往地上栽，我还以为是算法写错了。查了三天，最后发现是传感器方向标定错了。所以，调试之前一定先检查传感器方向。
4.4 知识体系总览
下面这张图是我自己整理的，把运动控制的三个核心模块串起来了。你可以看到，正逆运动学是基础，步态规划是上层，平衡控制是保障。三者缺一不可。
4.5 实践建议
最后，给各位一些实践上的建议。这些是我踩过坑之后总结出来的：
- 先仿真，后实物。 在Gazebo或MuJoCo里把运动学、步态、平衡都调好了，再上真机。真机摔一次的成本够你买好几年的云服务器。
- 从简单场景开始。 先让机器人在平地上走，再试斜坡、楼梯、不平整地面。别一上来就让它跑酷。
- 做好摔倒保护。 无论你算法多牛，机器人总有摔倒的时候。设计好摔倒检测和缓冲动作，能省很多维修费。
- 记录所有数据。 关节角度、IMU数据、ZMP位置、电机电流——全都记录下来。出了问题，这些数据就是你的破案线索。
再强调一次： 运动控制不是一蹴而就的事。我见过太多人，花了两周搭好模型，花了两天调参数，然后花了两月修机器人。慢慢来，稳着走，比什么都重要。
好了，这一章的内容就到这里。运动控制是基础中的基础，后面我们会在这个基础上讲更高级的东西——比如全身协调控制、力控、以及多模态感知融合。先把今天这三个概念吃透，后面就好办了。