3. 运动学正解（Forward Kinematics）
好，咱们进入正题。运动学正解，英文叫 Forward Kinematics，简称 FK。说白了，就是这样一个问题：
已知六个电动缸的长度，求上平台的位置和姿态。
你想想看，Stewart 平台有六个腿，每个腿的长度我都知道，那平台到底摆成了什么姿势？这就是正解要回答的。
听起来好像挺简单？嗯，其实恰恰相反。正解比逆解难得多。逆解是已知姿态求腿长，那是解析解，闭式公式一套就出来。正解呢？没有闭式解。这是个典型的非线性方程组求解问题。
核心难点：六个非线性方程，六个未知数（三个位置 + 三个姿态角），没有解析解，只能数值迭代。
3.1 问题定义
先搭个数学模型。我习惯这样定义：
- 已知量：六个腿的长度 L₁, L₂, ..., L₆
- 未知量：上平台中心位置 (x, y, z) 和姿态角 (α, β, γ)，或者用旋转矩阵 R 表示
- 约束方程：每个腿的长度等于上下铰点之间的欧氏距离
数学上写成这样：
对于第 i 个腿：
f_i(x, y, z, α, β, γ) = ||R * a_i + p - b_i||² - L_i² = 0
其中：
- a_i 是上铰点在平台坐标系中的坐标
- b_i 是下铰点在基座坐标系中的坐标
- p = (x, y, z) 是平台中心位置
- R 是旋转矩阵，由 α, β, γ 决定六个方程，六个未知数。理论上可解。但问题是——这些方程高度非线性，耦合严重。你没法直接写出 x = ? 这样的公式。
我的经验：在实际项目中，我一般把姿态用四元数表示，而不是欧拉角。为什么？因为欧拉角有万向锁问题，迭代过程中容易翻车。四元数虽然多了一个变量（四个数），但约束条件 q₀²+q₁²+q₂²+q₃²=1 反而让数值稳定性更好。
3.2 数值解法：Newton-Raphson
说到数值解法，Newton-Raphson 是最经典、最常用的方法。说白了就是：猜一个初始值，然后不断修正，直到误差足够小。
迭代公式长这样：
x_{k+1} = x_k - J⁻¹(x_k) * F(x_k)
其中：
- x_k 是第 k 步的估计值（6维向量）
- F(x_k) 是残差向量（6维）
- J 是雅可比矩阵（6×6），每个元素 ∂f_i/∂x_j每次迭代，你都要计算雅可比矩阵，然后解一个线性方程组。嗯，这里要注意——雅可比矩阵的解析表达式是可以推导出来的，但我个人习惯用数值差分来近似。为什么？省事，而且不容易写错。
来个简单的代码框架：
// Newton-Raphson 求解运动学正解
bool forwardKinematicsNR(double* L, double* pose) {
double x[6] = {0, 0, 0.5, 0, 0, 0}; // 初始猜测
double F[6], J[6][6];
double dx[6];
for (int iter = 0; iter < 50; iter++) {
computeResiduals(x, L, F); // 计算残差
double err = norm(F);
if (err < 1e-8) { // 收敛判断
memcpy(pose, x, 6*sizeof(double));
return true;
}
computeJacobian(x, J); // 计算雅可比矩阵
solveLinearSystem(J, F, dx); // 解 J*dx = -F
for (int i = 0; i < 6; i++) {
x[i] += dx[i]; // 更新估计值
}
}
return false; // 未收敛
}我曾经踩过的坑：初始猜测太离谱，导致迭代发散。有一次在调试时，初始姿态给了个 90 度的俯仰角，结果 Newton-Raphson 直接飞到天上去。后来我加了个策略：如果迭代超过 20 步还没收敛，就换个初始值重新来。
Newton-Raphson 的优缺点很明显：
| 优点 | 缺点 |
|---|---|
| 收敛速度快（二阶收敛） | 需要计算雅可比矩阵 |
| 精度高（可达机器精度） | 对初始值敏感 |
| 实现相对简单 | 雅可比矩阵可能奇异 |
3.3 解析解法概述
说到解析解法，我得先泼盆冷水——Stewart 平台的完全解析正解，目前学术界还没搞定。这是个开放问题。
但是！对于某些特殊构型，是有解析解的。比如：
- 3-3 构型：上下铰点各三个，对称分布。这种情况可以化简为三次方程求解
- 6-3 构型：上平台三个铰点，下平台六个。也有简化形式
- 平面 Stewart 平台：所有铰点共面，可以降维处理
解析解的基本思路是：利用几何约束，把六个方程逐步消元，最终归结为一个高次多项式方程。我记得有篇经典论文（Husty, 1996）把一般 6-6 构型化简到了 40 次多项式。嗯，40 次——你想想看，这在实际工程中基本没法用。
我的建议：除非你的平台构型非常特殊（比如 3-3 构型），否则别在解析解上花太多时间。数值解法配合好的初始值估计，完全够用。
3.4 精度与实时性权衡
这才是工程实践中最头疼的问题。你想想看：
- 控制周期通常 1ms，甚至 0.5ms
- Newton-Raphson 一次迭代大约 0.1ms（在 STM32H7 上实测）
- 一般需要 3-5 次迭代才能收敛
也就是说，纯 Newton-Raphson 可能要占掉一半的控制周期。这还没算其他任务。
那怎么办？我分享几个实战技巧：
- 热启动：上一时刻的解作为当前时刻的初始值。因为平台运动是连续的，相邻时刻的姿态变化很小，一般 1-2 次迭代就够
- 降低精度要求：控制精度不需要太高。位置误差 0.1mm，角度误差 0.1°，对大多数应用足够了。把收敛阈值从 1e-8 放宽到 1e-5，迭代次数能减少一半
- 查表 + 插值：对于重复性高的轨迹，可以离线算好正解表，运行时查表加线性插值。我有个项目就是这么干的，实时性直接拉满
- 简化模型：如果平台运动范围不大，可以用小角度假设，把旋转矩阵线性化。这样正解就变成了线性方程组，一次求解搞定
实战数据（基于 STM32H743，主频 480MHz）：
| 方法 | 单次耗时 | 精度 | 适用场景 |
|---|---|---|---|
| Newton-Raphson（5次迭代） | ~0.5ms | 1e-8 | 高精度离线计算 |
| Newton-Raphson（热启动，2次迭代） | ~0.2ms | 1e-5 | 实时控制 |
| 查表 + 线性插值 | ~0.01ms | 取决于表密度 | 高速实时控制 |
| 小角度线性化 | ~0.05ms | 小角度时较高 | 微振动抑制 |
注意：查表法虽然快，但会占用大量 Flash 空间。六维查表，每维 100 个点，那就是 100⁶ = 1e12 个点——显然不现实。实际做法是只对常用工作空间建表，或者用稀疏表加插值。
最后说一句：正解在实时控制中其实用得不多。大多数时候，我们用的是逆解——给定姿态算腿长。正解主要用于：
- 标定：用外部测量设备测出实际姿态，和正解结果对比，校准结构参数
- 状态估计：当传感器（比如 IMU）失效时，用腿长传感器推算姿态
- 离线仿真：验证轨迹规划是否正确
嗯，正解这块内容不少，但核心就一句话：数值迭代是王道，解析解是奢侈品，实时性靠热启动和精度妥协来保证。