双轮足机器人 5 连杆逆运动学：从几何模型到嵌入式实现

一、引言

运动学逆解（Inverse Kinematics, IK）是腿足式机器人控制的核心问题之一。正运动学是"给定关节角度，求足端位置"------这是唯一确定的。逆运动学反过来------"给定足端位置，求关节角度"------则要复杂得多：解可能不存在、不唯一，甚至没有封闭解。

本机器人的腿机构采用5 连杆并联式设计，每侧腿由 4 根连杆 + 2 个舵机构成一个闭合五杆机构（后文简写为 5 连杆）。与常见的串联式腿（如工业机械臂构型、仿生狗腿）相比：

对比维度	串联式 (如 SpotMicro)	并联式 (本系统)
刚度	低 (悬臂受力)	高 (三角形封闭受力)
承载比	低	高
工作空间	大	较小
运动学	解析解简单	解析解存在但稍复杂
舵机负载	大 (自重+负载力矩)	小 (力由连杆分担)

5 连杆并联机构能以较小功率的舵机驱动较大的负载，代价是工作空间有限、运动学稍显繁琐。这对导盲机器人而言是合理的取舍------导盲场景需要的是稳定的低速移动和高承载（电池、传感器、手柄推力），而非大范围动态行走。

---

二、机构几何建模

2.1 单侧腿的 5 连杆模型

本机器人的每条腿（左/右）各由 2 个舵机驱动。从侧面看，该机构可抽象为下图所示的闭合 5 连杆结构：

                Front Hip(β)         Rear Hip(α)
                    ●──────────────────●        ← L5 (机架)
                   ╱                    ╲
                  ╱                      ╲
              L1 ╱                        ╲ L4
                ╱                          ╲
               ╱                            ╲
           knee●                              ●knee
               ╲                            ╱
                 ╲                        ╱
                L2 ╲                    ╱ L3
                     ╲                ╱
                       ╲            ╱
			 	       	 ╲        ╱
					       ╲    ╱
                  			  ● (足端, L2 与 L3 交汇)
                  (x_target, y_target)

符号定义：

符号	值（以本项目为例）	含义
L1	60mm	前髋→前膝连杆长度 (舵机 β 到前膝)
L2	100mm	前膝→足端连杆长度
L3	100mm	后膝→足端连杆长度
L4	60mm	后髋→后膝连杆长度 (舵机 α 到后膝)
L5	40mm	前后髋关节中心距 (两个舵机输出轴距离)

两个关键特点：

1. 前后对称：L1 = L4 = 60mm, L2 = L3 = 100mm，这使得机构的运动学具有对称性，简化了解析解
2. 足端共享：两个开链（α 链和 β 链）的末端指向同一个足端点（x, y），形成一个闭合约束------这是并联机构的核心特征

2.2 坐标系约定

以前髋关节（舵机 β 的输出轴中心）为原点 O，建立单侧腿的局部坐标系：

• X 轴：水平向前（但实际机器人中是后髋指向前髋为正）
• Y 轴：竖直向上（腿伸直方向为正）

在这个坐标系下：

前髋 (Servo β):   (0, 0)
后髋 (Servo α):   (-L5, 0) = (-40, 0)
足端目标位置:     (x, y)

参数 x 和 y 即为用户的输入------coordTarget.x 和 coordTarget.yLeft/Right。x 控制腿部的前后倾斜（前伸/后缩），y 控制腿部的高度（蹲/站）。

---

三、逆运动学推导

3.1 思路拆解

5 连杆闭链的逆解可以拆成两个独立的 2 连杆开链来处理，这是因为前后两个开链共享同一个末端位置 (x, y)。对于每个 2 连杆开链，标准解析解存在且唯一（在关节限位内）。

问题分解:
  输入: 足端位置 (x, y)
  
  子问题 1: 前链 (L1 + L2), 原点 (0,0) → 求角度 β
  子问题 2: 后链 (L4 + L3), 原点 (-L5, 0) → 求角度 α
  
  输出: (β, α) 两个舵机角度

3.2 回顾：标准 2 连杆 IK

假设有一个 2 连杆机构，连杆长度分别为 a 和 b，末端位置为 (x, y)，求关节角 q1 和 q2：

末端到达条件:
  x = a·cos(q1) + b·cos(q1 + q2)
  y = a·sin(q1) + b·sin(q1 + q2)

平方相加消去 q2:
  x² + y² = a² + b² + 2ab·cos(q2)
  
求解 q2 (肘式解, 即 q2 < 0):
  q2 = -arccos((x² + y² - a² - b²) / (2ab))

求解 q1:
  q1 = atan2(y, x) - atan2(b·sin(q2), a + b·cos(q2))

但本系统的代码中使用的是另一种等价形式------几何法：通过余弦定理直接求 q1：

设:  c = sqrt(x² + y²)          (末端到原点的距离)
     cos(q1) = (a² + c² - b²) / (2ac)

由余弦定理:
     c² = a² + b² - 2ab·cos(π - q2)  →  q2 = ...

     a² = b² + c² - 2bc·cos(φ)  其中 φ 是连杆 b 与 c 之间的夹角

     q1 = atan2(y, x) ± φ

具体形式取决于使用哪种参数化。代码中使用了 atan2 的变体，这是 IK 求解的常见形式。

3.3 前链：求解 β (前舵机)

前链以 (0, 0) 为原点，连杆长度 L1=60, L2=100，末端为 (x, y)：

// 前链: L1(上臂) + L2(前臂)
float a1 = 2 * x * L1;               // 2·x·L1
float b1 = 2 * y * L1;               // 2·y·L1
float c1 = x*x + y*y + L1*L1 - L2*L2;

这是余弦定理的变形。a1, b1, c1 是 atan2 形式的余弦定理系数。推导过程如下：

令 d² = x² + y²（足端到前髋的距离平方），在三角形中由余弦定理：

cos(β) = (L1² + d² - L2²) / (2·L1·d)

但我们需要 β 本身而非它的余弦。标准解为：

β = 2·atan2(b1 ± sqrt(a1² + b1² - c1²), a1 + c1)

代码中取 + 号分支（对应机构在正常姿态下的解）：

IKParam.alpha1 = 2 * atan2(b1 + sqrt(a1*a1 + b1*b1 - c1*c1), a1 + c1);

几何含义 ：a1² + b1² - c1² 的符号决定了解的存在性。如果该值为负，说明 (x, y) 不在 L1+L2 的可达范围内------此时 sqrt 对负数开方会得到 NaN。但在实际运行中，legControl() 函数会通过 PID 渐进逼近目标值，确保 x, y 始终在可达范围内。

3.4 后链：求解 α (后舵机)

后链的原点不在 (0, 0) 而在 (-L5, 0) = (-40, 0)，使用连杆长度 L4=60, L3=100：

// 后链: L4(上臂) + L3(前臂), 髋关节偏移 L5
float d1 = 2 * L4 * (x - L5);         // 2·L4·(x - L5)
float e1 = 2 * L4 * y;                // 2·L4·y
float f1 = (x-L5)*(x-L5) + L4*L4 + y*y - L3*L3;

这里的 (x - L5) 对 x 进行偏移补偿，将后髋关节的位置"移动"到原点进行计算：

IKParam.beta1 = 2 * atan2(e1 - sqrt(d1*d1 + e1*e1 - f1*f1), d1 + f1);

注意这里取的是 - 号分支------后链的解选用了另一个根，这是由机构的实际几何构型决定的。前链取 +、后链取 -，两个分支的选择共同保证了 5 连杆机构的闭合解具有物理可实现性。

3.5 角度处理与输出映射

// 弧度转角度
alpha1ToAngle = (int)((IKParam.alpha1 / 6.28) * 360);
beta1ToAngle  = (int)((IKParam.beta1  / 6.28) * 360);

// 角度限幅 + 映射到舵机脉宽
motorsTarget.servoLeftRear  = map(alpha1ToAngle, 0, 180, 103, 327);
motorsTarget.servoLeftFront = map(beta1ToAngle,  0, 180, 103, 327);

PWM 脉宽映射关系：103 对应 500μs（0°），327 对应 2500μs（180°），这是 MG996R 类舵机的标准脉宽范围。

3.6 右腿

右腿的解算与左腿完全对称，只不过使用的参数是 (x, yRight)：

float a2 = 2 * IKParam.XRight * L1;
float b2 = 2 * IKParam.YRight * L1;
float c2 = ...;  // 与左腿形式相同, 使用 XRight/YRight

IKParam.alpha2 = 2 * atan2(b2 + sqrt(...), a2 + c2);
IKParam.beta2  = 2 * atan2(e2 - sqrt(...), d2 + f2);

servoRightFront = map(alpha2ToAngle, 0, 180, 103, 327);
servoRightRear  = map(beta2ToAngle,  0, 180, 103, 327);

---

四、正运动学验证

逆解算出的角度是否正确？可以反过来做一次正运动学验证。

给定 (α, β)，计算足端位置 (x, y)：

前链正解:
  x_front = L1·cos(β) + L2·cos(β₁)
  y_front = L1·sin(β) + L2·sin(β₁)

后链正解 (从偏移原点):
  x_rear  = -L5 + L4·cos(α) + L3·cos(α₁)
  y_rear  = L4·sin(α) + L3·sin(α₁)

两个正解应给出相同的 (x, y)。如果代码实现正确，计算出的位置将完全等于 IK 的输入。这就是闭链机构的一致性约束------是验证 IK 代码正确性的最直接方法。

本系统在调参阶段使用了一条简单的测试逻辑：固定 y=70，令 x 从 -20 到 20 步进，记录每次解算的 α 和 β，再用正运动学还原 (x', y')，验证 |x - x'| < 1mm。

---

五、从 IK 到舵机控制：完整调用链

逆运动学在控制系统中处于中间层------上层是姿态与位置控制，下层是舵机 PWM 驱动：

legControl()                  // 上层: 根据 IMU 反馈 + 目标速度计算 (x, y)
  ↓
inverseKinematics()           // 中层: 将 (x, y) 转换为 (α, β) 舵机角度
  ↓
setServoAngle()               // 下层: PCA9685 输出 PWM 脉宽

5.1 legControl：计算 IK 输入坐标

void legControl() {
    // 由 PID 渐进设定腿高和前进量
    controlTarget.legLeft  += PID_Height.Kp * (robotMotion.updown - controlTarget.legLeft);
    controlTarget.legRight += PID_Height.Kp * (robotMotion.updown - controlTarget.legRight);

    // 滚转补偿 (左右腿高度差)
    coordTarget.yLeft  = constrain(controlTarget.legRollLeft  + controlTarget.legLeft, ...);
    coordTarget.yRight = constrain(controlTarget.legRollRight + controlTarget.legRight, ...);

    // 前进 (x 方向)
    coordTarget.x = coordTarget.x + PID_XCoord.Kp * (controlTarget.forward - coordTarget.x) + wheel_X_off;

    robotPose.height = (coordTarget.yLeft + coordTarget.yRight) / 2;
}

三个关键点：

第一 ：coordTarget.x 是经过 PID 渐进逼近的，而非直接等于目标值。这避免了 IK 输入突变导致舵机角度跳变。

第二 ：wheel_X_off = 2.2 是一个经验补偿值，用于抵消 BLDC 轮毂电机在 x 方向上的物理偏置。如果没有这个补偿，腿部收缩到最短时轮子不在同一垂线上，导致机器人无法垂直于地面站立。

第三 ：coordTarget.yLeft 和 coordTarget.yRight 在正常行走时保持相等（无滚转），通过同步增减来调节整机高度。当需要侧倾转弯时（目前禁用），两者差值产生滚转角控制。

5.2 inverseKinematics：角度解算

实现代码已在第 3 节详细分析。注意每个周期都会执行逆解------在 50-200Hz 的控制频率下，这意味着每秒进行 50-200 次 IK 解算。OpenMV 负责视觉和规划，ESP32 的 240MHz 双核处理器完全能负担这个计算量。

5.3 setServoAngle：PWM 输出

void setServoAngle(sLeftFront, sLeftRear, sRightFront, sRightRear) {
    servos.setAngle(3, 90 + LFSERVO_OFFSET + sLeftFront);   // 左前
    servos.setAngle(4, 90 + LRSERVO_OFFSET + sLeftRear);    // 左后
    servos.setAngle(5, 270 + RFSERVO_OFFSET - sRightFront);  // 右前
    servos.setAngle(6, 270 + RRSERVO_OFFSET - sRightRear);  // 右后
}

这里有两个值得注意的工程细节：

角度偏置 (LFSERVO_OFFSET = -2, LRSERVO_OFFSET = -8 等)：每个舵机的物理安装角度存在差异，通过四个独立偏置在软件层面进行机械零位校准。这比人工调整舵机舵盘角度更精确且可复现。

符号反转 ：右腿的舵机安装方向与左腿相反，因此右腿的角度输出用 270 - angle 而非 90 + angle。如果不做这个反转，左右腿的 IK 计算虽然正确，但舵机的实际旋转方向会相反，导致右腿无法正确执行。

5.4 完整的调用链时序

main.cpp loop() (50-200Hz):
  1. 传感器读取 (IMU, 超声, OpenMV 帧)
  2. legControl()       → 计算 coordTarget (x, yLeft, yRight)
  3. inverseKinematics() → 计算 4 个舵机角度 (α₁, β₁, α₂, β₂)
  4. robotRun()          → PID → BLDC 轮速
  5. setServoAngle()     → PWM → 4 舵机

---

六、工作空间与可达性

6.1 可达空间计算

给定连杆参数，左腿的工作空间是以下约束的交集：

约束 1 (前链):  sqrt(x² + y²) ≤ L1 + L2 = 160mm
约束 2 (前链):  sqrt(x² + y²) ≥ |L1 - L2| = 40mm
约束 3 (后链):  sqrt((x+40)² + y²) ≤ L4 + L3 = 160mm
约束 4 (后链):  sqrt((x+40)² + y²) ≥ |L4 - L3| = 40mm
约束 5 (关节角度): 0 ≤ α ≤ 180°, 0 ≤ β ≤ 180°
约束 6 (实际限位): 70mm ≤ y ≤ 130mm (由 ROBOT_LOWEST_FOR_MOT / ROBOT_HIGHEST 限定)

在正常行走模式下, y 取值范围 [70, 130]mm, x 取值范围约为 [-30, 30]mm。这意味着每条腿的可活动范围是一个约 60mm × 60mm 的矩形区域。

6.2 奇异位形

当 5 连杆处于以下构型时，机构进入奇异状态：

1. 完全伸直 (α = β = 0°)：前后链共线，没有侧向刚度
2. 重叠 (两个膝盖接触)：α 和 β 使两个前臂/后臂交叉，机构锁死

代码中的角度限幅处理了第一种情况：

if (IKParam.beta1 < 0) IKParam.beta1 = 0;  // 防止前链反向折叠

6.3 从腿高到整机高度

robotPose.height 定义为左右腿高的平均值：

robotPose.height = (coordTarget.yLeft + coordTarget.yRight) / 2;

这意味着机身高度是受控的，且与腿的 x 倾斜无关。在行走过程中保持 yLeft ≈ yRight 使机身水平，x 的变化则用于调节质心位置以配合前后运动的加减速。

---

七、总结

本系统的 5 连杆逆运动学在设计上有几个关键特征：

1. 闭链机构选型------以牺牲部分工作空间换取了更高的刚度和承载能力，适合导盲机器人的重载低速场景
2. 拆解为双 2 连杆求解------利用前后开链共享末端位置的特性，把闭链问题转化为两个独立的开链问题，获得了解析解而非迭代解，保证了 200Hz 的实时解算能力
3. PID 渐进 + IK 瞬时计算------上层 PID 缓慢调制 (x, y) 目标值，下层 IK 瞬时计算对应角度。这种"位置平滑 + 角度瞬解"的架构使舵机运动平滑无抖动
4. 软件零位校准 ------通过 LFSERVO_OFFSET 等四个参数补偿机械安装误差，相当于在软件层面实现了"调零"，之后更换舵机时只需重测偏置值，无需调整机械结构