六足螃蟹机逆运动学入门：ESP32实时解算不卡顿

六足机器人为什么比四足难控制

六足机器人在结构上比四足稳定得多，三条腿着地就能保持平衡，理论上可以做各种高难度姿态。但控制难度也更大：18个自由度，比四足的12个多了50%，每一步都要解算逆运动学把目标坐标转换成18个关节角度。

如果在控制板上逐关节用三角函数算，计算量很大，ESP32这种级别的MCU可能跟不上实时性要求。本文介绍一种在ESP32上高效实现六足逆运动学的方法，实测控制频率稳定在200Hz，18个关节同步更新不卡顿。

简单说，逆运动学就是"我知道脚掌要落在哪个位置，反推三个关节分别要转多少度"。六足机器人每条腿有3个关节：基座旋转（yaw）、大腿俯仰（pitch）、小腿俯仰（pitch）。已知脚掌的目标坐标(x,y,z)，求三个关节角度。

数学推导过程是这样的：基座旋转角度等于脚掌在水平面上投影的方位角，用atan2(y,x)直接算。大腿和小腿的角度用几何法求解：把腿的两个连杆和地面构成一个三角形，用余弦定理反算两个角度。

关键是计算效率。如果用标准数学库的sin/cos/atan2函数，18个关节解算一次大约需要100-200微秒。200Hz的控制频率意味着每5毫秒就要算一次，100-200微秒完全来得及。但如果你还在循环里做了大量的浮点运算或者动态内存分配，就可能超时。

第一个优化是把三角函数查表化。ESP32有硬件FPU，标准的math库已经很快了，但如果你想再快一点，可以预先生成一个sin/cos的查找表（LUT），用空间换时间。一个360点的float查找表只需要1.4KB，完全放得下。

第二个优化是避免动态内存分配。在控制循环里不要用new、malloc或者创建String对象。所有数组和变量都预先分配好，在循环里只做计算和赋值。

第三个优化是用双核。ESP32有两个核心，一个核专门跑逆运动学解算和舵机控制，另一个核负责通讯（WiFi/蓝牙）和上层逻辑。两个核之间用一个共享变量传递目标坐标，用mutex做同步。这样通讯中断不会影响运动控制的实时性。

六足机器人最常用的步态是三角步态：6条腿分成两组，1-3-5和2-4-6交替抬起和放下。每组3条腿在支撑相保持稳定，另一组3条腿在摆动相移动到下一个落脚点。

实现三角步态的关键是相位控制。给一个全局相位变量phase，从0到2PI循环。当phase在0到PI时，组A支撑、组B摆动；当phase在PI到2PI时，组B支撑、组A摆动。每条腿的脚掌轨迹是一个半椭圆：支撑相走直线（脚掌在地面滑动），摆动相走弧线（脚掌抬离地面）。

每个控制周期，根据当前phase计算每条腿的脚掌目标坐标，然后调用逆运动学解算出18个关节角度，最后把角度发给对应的舵机。整个流程控制在2毫秒以内，200Hz绰绰有余。

我用一台18个STS3215舵机组装的六足螃蟹机做了测试：ESP32跑200Hz控制循环，WiFi同时开着接收遥控指令。连续行走10分钟，控制周期抖动小于0.1毫秒，步态稳定没有明显摇晃。机身尺寸25cm，重量800g，最大行走速度约0.3m/s。

一个容易踩的坑是舵机的初始角度标定。18个舵机如果初始角度没对准，逆运动学算出来的角度就会偏，导致某些关节超出限位或者腿的着地位置不对。建议组装的时候先给每个舵机设好中位，安装腿的时候让大腿水平、小腿垂直，以此作为零位。

核心代码结构很简洁：一个主循环跑在Core 1，每个周期做三件事：更新phase变量、计算18条腿的目标坐标、调用逆运动学函数算角度、通过串口总线发给舵机。Core 0跑一个通讯任务，接收WiFi遥控器的速度指令，转换成机体速度后传给Core 1。两个核之间用xQueue做消息传递。

这种双核架构的好处是运动控制和通讯完全解耦，WiFi断连或者延迟不会影响步态。如果你要加视觉模块或者其他传感器，也可以放在Core 0，不影响实时控制。

最后建议新手先从三角步态开始，不要一上来就搞自由步态或者全向移动。三角步态是最稳定的，18个舵机协调好就能走起来。等三角步态调稳了，再尝试其他步态和地形适应。