4. 正运动学建模：位置正解、数值迭代法（牛顿-拉夫森）、解析法简介
正运动学，说白了就是「已知六个腿长，求动平台的位置和姿态」。
这个问题在 Stewart 平台里其实挺让人头疼的。为什么？因为反解很容易，给个位姿就能算出腿长；但正解呢，给你六个长度，要反推出平台在哪、朝哪，这玩意儿是非线性的，没有唯一的显式解。
我当年第一次做正解的时候，心想「不就是解个方程组嘛」，结果一上手就懵了。六个方程，高度耦合，还带三角函数，手算根本不可能。后来才明白，正解必须靠数值方法，或者在某些特殊构型下用解析法。
这一节，我就带你走一遍正运动学的三种主流思路：位置正解的基本概念、牛顿-拉夫森数值迭代法，以及解析法的适用场景。
4.1 位置正解到底在解什么？
先回顾一下我们的模型。Stewart 平台有上下两个平台，上平台通过六条腿连接到下平台。每条腿的长度由伺服电动缸控制，记作 \( L_1, L_2, ..., L_6 \)。
正解的目标是：给定这六个长度，求动坐标系 \(\{P\}\) 相对于静坐标系 \(\{B\}\) 的位姿。
位姿包含六个自由度：
- 位置：\( x, y, z \)（动坐标系原点在静坐标系中的坐标）
- 姿态：通常用欧拉角 \( \phi, \theta, \psi \)（滚转、俯仰、偏航）表示
所以，正解本质上就是求解一个六元非线性方程组：
F(X) = 0
其中 X = [x, y, z, φ, θ, ψ]^T
F_i(X) = ||P_i(X) - B_i|| - L_i = 0, i = 1..6每个方程代表一条腿的长度约束。\( P_i \) 是上平台第 i 个铰点在静坐标系下的坐标，它是 X 的函数；\( B_i \) 是下平台铰点坐标，固定不变。
核心难点：这个方程组没有封闭解。你没法像解一元二次方程那样，直接套公式算出结果。必须用迭代法逼近。
4.2 牛顿-拉夫森法：最常用的数值迭代法
数值迭代法里，我个人最常用的是牛顿-拉夫森法（Newton-Raphson）。为什么？因为它收敛快，精度高。只要初值给得好，三五步就能收敛到工程可用的精度。
它的核心思想很简单：用切线去逼近根。
对于单变量函数 \( f(x) = 0 \)，迭代公式是：
x_{k+1} = x_k - f(x_k) / f'(x_k)对于我们的六元方程组，推广成：
X_{k+1} = X_k - J^{-1}(X_k) * F(X_k)其中 \( J \) 是雅可比矩阵，6×6 的矩阵，每个元素是 \( \partial F_i / \partial X_j \)。
嗯，这里要注意：雅可比矩阵的推导是重头戏。每个偏导数都要手算出来，不能靠数值差分——数值差分虽然省事，但会引入截断误差，影响收敛速度。
我在项目中遇到过一个问题：雅可比矩阵在奇异位形附近会变得病态，导致迭代发散。后来我加了一个阻尼因子（Levenberg-Marquardt 的思想），才稳定下来。
4.2.1 算法步骤
- 给定初值 \( X_0 \)。一般取平台处于中位时的位姿，比如 \( [0, 0, H, 0, 0, 0] \)。
- 计算残差 \( F(X_k) \)。就是当前位姿下，每条腿的理论长度与实际长度的差值。
- 计算雅可比矩阵 \( J(X_k) \)。
- 求解线性方程组 \( J \Delta X = -F \)，得到增量 \( \Delta X \)。
- 更新位姿 \( X_{k+1} = X_k + \Delta X \)。
- 检查收敛：如果 \( ||F|| < \epsilon \) 或 \( ||\Delta X|| < \epsilon \)，停止；否则回到第 2 步。
小技巧：收敛阈值 \( \epsilon \) 我一般取 1e-6 米（位置）和 1e-6 弧度（姿态）。对于运动模拟器来说，这个精度已经远高于机械系统的重复定位精度了。
4.2.2 代码实现（Python 伪代码）
import numpy as np
def stewart_forward_kinematics(L_target, X0, max_iter=50, tol=1e-6):
X = X0.copy()
for i in range(max_iter):
F = compute_residual(X, L_target)
if np.linalg.norm(F) < tol:
break
J = compute_jacobian(X)
delta_X = np.linalg.solve(J, -F)
X = X + delta_X
return X
def compute_residual(X, L_target):
# 根据当前位姿 X 计算六条腿的长度
L_current = forward_leg_lengths(X)
return L_current - L_target
def compute_jacobian(X):
# 解析推导的雅可比矩阵，6x6
# 每个元素是 dL_i / dX_j
J = np.zeros((6, 6))
# ... 具体推导略，后面章节会详细展开
return J避坑指南：我曾经在初值给得离真实解太远时，迭代直接炸了——位姿飞到了十万八千里外。后来我加了一个「步长限制」：每次迭代的增量 \( \Delta X \) 不能超过某个最大值，比如位置增量不超过 0.1 米，姿态增量不超过 0.1 弧度。这样即使初值不好，也能慢慢爬回来。
4.3 解析法简介：什么时候能用？
解析法，就是直接推导出位姿关于腿长的显式表达式。听起来很美好，但现实很骨感。
对于一般的 Stewart 平台（6-6 构型，即上下平台各有 6 个铰点，且分布不规则），解析解是不存在的。这是数学上已经证明的结论——六次以上的多项式方程没有根式解。
但是，在某些特殊构型下，解析法是可行的：
- 3-3 构型：上下平台各 3 个铰点，两两重合。这种构型有封闭解。
- 6-3 构型：上平台 6 个铰点，下平台 3 个铰点（或反过来）。也有解析解。
- 平面 Stewart 平台：所有铰点共面，且运动限制在平面内。
我记得有一次做教学演示平台，用的就是 3-3 构型。当时我直接推导出了解析公式，代码里一行就搞定了正解，连迭代都不用。那感觉，真爽。
但对于大多数工业级运动模拟器，用的都是 6-6 构型（铰点均匀分布在圆周上），这时候老老实实用牛顿-拉夫森法吧。
4.4 三种方法的对比
| 方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 牛顿-拉夫森法 | 收敛快，精度高，通用性强 | 需要初值，可能发散，需推导雅可比 | 6-6 构型，实时控制 |
| 解析法 | 无迭代，速度快，结果唯一 | 仅适用于特殊构型 | 3-3、6-3 构型，教学演示 |
| 其他数值法（如粒子群） | 不需要初值，全局搜索 | 收敛慢，不适合实时 | 离线标定、初始位姿估计 |
4.5 本章知识体系
下面这张图，帮你理清正运动学建模的完整脉络：
你看，整个正运动学建模，其实就是从「腿长」到「位姿」的映射。牛顿-拉夫森法是工程中最实用的方案，解析法虽然快但限制多，其他数值法更多用于离线场景。
我个人建议：先掌握牛顿-拉夫森法，把它写成一个通用的正解函数。然后，如果你的平台构型特殊，再考虑解析法优化。这样，不管遇到什么平台，你都能搞定正解。
最后一个小提醒：正解在实时控制中通常不会每周期都跑——太慢了。一般是在系统启动时跑一次，用来标定初始位姿。运动过程中，我们更多依赖反解和动力学模型。但正解作为理论基础，你必须懂。
公众号:蓝海资料掘金营，微信deep3321