3. 运动学反解（IK）：位置反解数学模型、支腿长度计算、雅可比矩阵的几何推导
好，咱们进入正题。运动学反解，英文叫 Inverse Kinematics，简称 IK。说白了，就是我知道平台要摆到什么姿态，反过来算六个支腿该伸长多少。
这玩意儿是 Stewart 平台控制的基础。你想想看，你给控制器发个指令：“平台给我往左偏 10 度”，控制器得知道每个电机转多少圈吧？这个“转多少圈”就是从 IK 算出来的。
我个人习惯把 IK 分成两步走：位置反解和速度反解。位置反解算长度，速度反解算雅可比矩阵。咱们一个一个来。
3.1 位置反解的数学模型
先搭个坐标系。我习惯在静平台上建一个固定坐标系 {B}，原点在静平台中心。动平台上建一个动坐标系 {P}，原点在动平台中心。
动平台有六个自由度：三个位置 (x, y, z) 和三个姿态角 (roll, pitch, yaw)。通常我们用欧拉角或者旋转矩阵来表示姿态。
每个支腿连接两个点：下铰点 Bi（在静平台上）和上铰点 Pi（在动平台上）。注意，Pi 是在动坐标系下的坐标，得转换到固定坐标系下才能算长度。
转换公式很简单：
P_i_fixed = R * P_i_local + T其中 R 是旋转矩阵，T 是动平台中心的位置向量 [x, y, z]^T。
然后，第 i 个支腿的向量就是：
L_i = P_i_fixed - B_i支腿长度就是向量的模：
l_i = ||L_i||嗯，数学上就这么简单。但实际工程中，坑可不少。
3.2 支腿长度计算实战
好，咱们来点实际的。假设你拿到了平台的几何参数：
| 参数 | 符号 | 说明 |
|---|---|---|
| 静平台铰点半径 | R_b | 静平台铰点分布圆的半径 |
| 动平台铰点半径 | R_p | 动平台铰点分布圆的半径 |
| 铰点角度偏移 | θ_b, θ_p | 铰点相对于坐标轴的角度 |
| 初始高度 | h0 | 平台处于中位时的高度 |
计算步骤是这样的：
- 先算出每个铰点在各自坐标系下的坐标
- 给定目标姿态 (x, y, z, roll, pitch, yaw)
- 计算旋转矩阵 R
- 把动平台铰点转换到固定坐标系
- 计算每个支腿的向量和长度
代码实现大概长这样：
function [leg_lengths] = stewart_ik(pose, params)
% pose: [x, y, z, roll, pitch, yaw]
% params: 平台几何参数结构体
% 1. 计算旋转矩阵
R = euler_to_rotmat(pose(4), pose(5), pose(6));
% 2. 位置向量
T = pose(1:3)';
% 3. 对每个支腿
for i = 1:6
% 动平台铰点在固定坐标系下的位置
P_fixed = R * params.P_local(:, i) + T;
% 支腿向量
L = P_fixed - params.B_fixed(:, i);
% 支腿长度
leg_lengths(i) = norm(L);
end
end3.3 雅可比矩阵的几何推导
雅可比矩阵，说白了就是支腿长度变化率和平台速度之间的映射关系。为什么需要它？因为力控制要用到。你想做阻抗控制、力位混合控制，都离不开雅可比矩阵。
从几何角度看，雅可比矩阵的每一行其实就是一个支腿方向的单位向量和该方向相对于平台中心的力矩臂的叉乘。
推导过程是这样的：
支腿长度的变化率等于支腿方向上的速度分量：
dl_i/dt = u_i · v_p + (r_i × u_i) · ω其中：
- u_i 是支腿方向的单位向量
- v_p 是平台中心的线速度
- ω 是平台的角速度
- r_i 是从平台中心到上铰点的向量
写成矩阵形式：
dl/dt = J * [v_p; ω]雅可比矩阵 J 的每一行是：
J(i, :) = [u_i^T, (r_i × u_i)^T]这个矩阵是 6x6 的。有了它，你就可以把关节空间的力映射到操作空间：
F = J^T * τ其中 τ 是支腿的驱动力，F 是平台受到的广义力（力和力矩）。
3.4 雅可比矩阵的数值验证
理论推导完了，怎么验证你算对了？我有个土办法：
给平台一个微小的位移 δx，然后通过位置反解算出支腿长度的变化 δl。再用 δl/δx 来近似雅可比矩阵的某一列。如果和解析解一致，那就对了。
% 数值验证雅可比矩阵
delta = 1e-6;
J_numerical = zeros(6, 6);
for j = 1:6
pose_plus = pose;
pose_plus(j) = pose_plus(j) + delta;
l_plus = stewart_ik(pose_plus, params);
l_minus = stewart_ik(pose, params);
J_numerical(:, j) = (l_plus - l_minus) / delta;
end
% 比较 J_numerical 和解析 J3.5 工程中的注意事项
最后，聊几个实际工程中容易出问题的地方：
- 奇异位形： 当平台处于某些特殊姿态时，雅可比矩阵会奇异。这时候力控制会失效。我建议在轨迹规划时避开这些区域。
- 铰点布局： 铰点的角度分布会影响雅可比矩阵的条件数。条件数越小，力传递性能越好。设计时尽量让条件数接近 1。
- 实时性： IK 计算要快。我一般用 C 语言实现，单次 IK 计算控制在 10 微秒以内。如果用了 Python，记得用 NumPy 向量化操作。
好了，运动学反解这部分就讲到这里。记住，IK 是 Stewart 平台控制的基石。位置反解算长度，雅可比矩阵算力映射。两者缺一不可。