4、运动学基础（三）：位置正解（Forward Kinematics）的数值解法（Newton-Raphson法）与实现
各位好，欢迎来到这一讲。
上一讲我们聊了位置反解，也就是给定平台位姿，算六个腿长。那个过程很直接，解析解摆在那，闭着眼睛都能算出来。但反过来——位置正解，就没那么友好了。
说白了，位置正解就是：我知道六个腿长，你给我算出平台到底在什么位置、什么姿态。
你可能会想，反解有解析解，正解难道没有吗？
嗯，很遗憾。Stewart平台的正解，没有封闭形式的解析解。这是一个高度耦合的非线性方程组问题。我当年刚入行时，天真地以为找个数学软件就能符号推导出来，结果算了三天三夜，内存爆了，啥也没出来。
所以，我们得用数值方法。今天重点讲Newton-Raphson法。
4.1 为什么正解这么难？
先直观感受一下问题的复杂度。
Stewart平台有6个自由度：3个位置（x, y, z），3个姿态（通常用欧拉角或四元数表示）。每个腿长是一个关于这6个变量的非线性函数。6个腿长，就是6个方程。
方程组长这样：
f₁(x, y, z, α, β, γ) = L₁² - |P₁ - B₁|² = 0
f₂(x, y, z, α, β, γ) = L₂² - |P₂ - B₂|² = 0
...
f₆(x, y, z, α, β, γ) = L₆² - |P₆ - B₆|² = 0其中Pᵢ是上铰点在全局坐标系下的坐标，Bᵢ是下铰点坐标。Pᵢ依赖于平台的位姿参数。
这6个方程互相耦合，没法直接解。你想想看，一个变量动一下，所有方程都跟着变。
核心难点：正解问题本质上是一个多维非线性方程组求根问题。没有捷径，只能迭代。
4.2 Newton-Raphson法的核心思想
Newton-Raphson法，说白了就是用切线去逼近根。
一维情况大家都很熟：给定一个函数f(x)=0，先猜一个x₀，然后算f(x₀)和f'(x₀)，下一个猜测就是：
x₁ = x₀ - f(x₀) / f'(x₀)重复，直到f(x)足够接近0。
推广到多维，公式变成：
Xₖ₊₁ = Xₖ - J⁻¹(Xₖ) · F(Xₖ)其中：
- X 是6维向量 [x, y, z, α, β, γ]ᵀ
- F 是6维残差向量 [f₁, f₂, ..., f₆]ᵀ
- J 是6×6的雅可比矩阵，Jᵢⱼ = ∂fᵢ / ∂Xⱼ
每次迭代，我们都要算雅可比矩阵，然后解一个线性方程组（而不是真的求逆，那样太慢）。
我的经验：实际工程中，千万别直接求J⁻¹。用LU分解或者QR分解解线性方程组J·ΔX = -F，数值稳定性好得多。我曾经在早期代码里直接用了inv()函数，结果遇到奇异构型时直接崩了。
4.3 雅可比矩阵怎么算？
这是最关键的步骤。雅可比矩阵的每个元素是残差对位姿参数的偏导数。
以f₁为例：
f₁ = L₁² - (P₁ - B₁)·(P₁ - B₁)其中P₁ = R·p₁ + T，R是旋转矩阵，T是平移向量。
那么：
∂f₁/∂x = -2·(P₁ - B₁)·∂P₁/∂x
∂P₁/∂x = [1, 0, 0]ᵀ对于姿态参数（比如用ZYX欧拉角），偏导数会涉及旋转矩阵对欧拉角的导数。这部分推导比较繁琐，但好在有规律可循。
我建议你不要手算所有偏导数。容易出错，而且改一个参数就要重算。我在项目中一般用两种方式：
- 解析法：推导出偏导数的解析表达式，然后硬编码。效率最高，但开发周期长。
- 数值微分：用有限差分近似。开发快，但精度稍差，且多了一次函数求值。
我个人习惯是：先用数值微分快速验证算法，再换成解析法提高性能。
4.4 算法流程
下面这张图展示了Newton-Raphson法求解正解的核心流程：
流程其实不复杂，核心就三步：算残差 → 算雅可比 → 解方程更新。重复直到收敛。
4.5 代码实现
下面给出一份核心代码。我用的是Python + NumPy，方便演示。实际嵌入式系统里，我一般用C++ + Eigen。
import numpy as np
def stewart_forward_kinematics(leg_lengths, base_points, platform_points,
X0, tol=1e-8, max_iter=50):
"""
Stewart平台位置正解 - Newton-Raphson法
参数:
leg_lengths: 6个腿长 [L1, L2, ..., L6]
base_points: 6个下铰点坐标 (6x3)
platform_points: 6个上铰点在平台坐标系下的坐标 (6x3)
X0: 初始猜测 [x, y, z, alpha, beta, gamma]
tol: 收敛容差
max_iter: 最大迭代次数
返回:
X: 收敛后的位姿
converged: 是否收敛
"""
X = X0.copy()
for iteration in range(max_iter):
# 1. 计算当前位姿下的上铰点全局坐标
R = euler_to_rotation_matrix(X[3], X[4], X[5])
T = X[0:3]
P_global = (R @ platform_points.T).T + T # 6x3
# 2. 计算残差向量 F
diff = P_global - base_points
lengths_sq = np.sum(diff**2, axis=1)
F = leg_lengths**2 - lengths_sq # 6维向量
# 3. 检查收敛
if np.linalg.norm(F) < tol:
return X, True
# 4. 计算雅可比矩阵 (数值微分)
J = np.zeros((6, 6))
delta = 1e-6
for j in range(6):
X_plus = X.copy()
X_plus[j] += delta
R_plus = euler_to_rotation_matrix(X_plus[3], X_plus[4], X_plus[5])
T_plus = X_plus[0:3]
P_plus = (R_plus @ platform_points.T).T + T_plus
diff_plus = P_plus - base_points
lengths_sq_plus = np.sum(diff_plus**2, axis=1)
F_plus = leg_lengths**2 - lengths_sq_plus
J[:, j] = (F_plus - F) / delta
# 5. 解线性方程组 J * dX = -F
try:
dX = np.linalg.solve(J, -F)
except np.linalg.LinAlgError:
print("警告：雅可比矩阵奇异，使用伪逆")
dX = np.linalg.lstsq(J, -F, rcond=None)[0]
# 6. 更新位姿
X = X + dX
return X, False # 未收敛注意：数值微分的步长delta很关键。太大则截断误差大，太小则舍入误差大。我一般取sqrt(eps) ≈ 1e-6。如果你用双精度浮点，这个值比较安全。
4.6 收敛性分析与改进
Newton-Raphson法有个臭名昭著的问题：对初始猜测敏感。
如果初始猜测离真实解太远，迭代可能发散，或者收敛到错误的解（正解可能有多个）。
我在实际项目中遇到过这种情况：平台快速运动时，用上一时刻的位姿作为初始猜测，结果迭代直接飞了。后来怎么解决的？
三个技巧：
- 阻尼Newton法：在更新步上加一个阻尼因子λ，Xₖ₊₁ = Xₖ + λ·ΔX，λ∈(0,1]。牺牲一点收敛速度，换取稳定性。
- 先粗后精：先用低精度快速迭代几步，再用高精度精细收敛。
- 多初始点：如果计算资源允许，从多个初始点出发，取残差最小的那个。
我的建议：对于实时控制，我通常用上一时刻的位姿作为初始猜测，加上阻尼因子0.5~0.8。这样既保证了实时性，又不会轻易发散。如果连续3次迭代不收敛，我会切换到备用初始猜测（通常是零位）。
4.7 精度与性能权衡
正解的计算精度直接影响控制效果。但精度越高，计算时间越长。
下面是我在项目中常用的参数配置：
| 应用场景 | 收敛容差 | 最大迭代次数 | 单次耗时（典型） | 说明 |
|---|---|---|---|---|
| 离线仿真 | 1e-12 | 100 | ~5ms | 追求极致精度 |
| 实时控制（1kHz） | 1e-6 | 10 | ~0.1ms | 平衡精度与速度 |
| 嵌入式（低算力） | 1e-4 | 5 | ~0.02ms | 够用就行 |
你看，不同场景对精度的要求差别很大。我个人的原则是：控制周期内能算完的前提下，精度越高越好。但千万别为了精度把控制周期拖垮了。
4.8 常见坑与避坑指南
最后，分享几个我踩过的坑：
- 姿态表示奇异：用欧拉角时，当β接近±90°，旋转矩阵出现奇异。我后来改用四元数，虽然多了一个变量，但避免了奇异问题。
- 雅可比矩阵奇异：当平台处于某些特殊构型时，雅可比矩阵可能奇异。这时候解线性方程组会失败。我的做法是检测条件数，如果太大就改用伪逆。
- 初始猜测太差：如果平台运动范围很大，上一时刻的位姿可能不是好猜测。我建议在启动时做一次粗搜索，或者用运动学反解先估算一个大概。
核心总结：Newton-Raphson法求解Stewart平台位置正解，关键在于雅可比矩阵的准确计算和初始猜测的合理选择。掌握了这两点，剩下的就是迭代循环了。
好了，这一讲就到这里。正解是Stewart平台控制的基础，虽然比反解麻烦，但掌握了数值解法，你就能应对绝大多数工程场景。下一讲我们会聊正解的另一种方法——解析法，敬请期待。
公众号:蓝海资料掘金营，微信deep3321