3. 逆运动学建模：位置反解、向量回路法、求解各支腿长度

好，咱们进入正题。逆运动学，说白了就是——我知道平台要摆什么姿态，反过来算六个腿该伸多长。这是整个Stewart平台控制的核心，也是我当年踩坑最多的地方。

你想想看，正运动学是已知腿长求姿态，那玩意儿复杂得要命，通常得靠数值迭代。但逆运动学不一样，它有解析解。为什么？因为结构是确定的，每个铰链点的位置是固定的，我们只需要做几何运算就行。

3.1 坐标系建立

我个人习惯，先把两个坐标系定下来：

• 静坐标系 {B}：固定在下平台中心，Z轴向上
• 动坐标系 {P}：固定在上平台中心，随平台运动

嗯，这里要注意：两个坐标系的初始方向要一致，不然算出来的角度全是错的。我在项目中遇到过，有同事把坐标系方向搞反了，结果平台一运行直接撞限位，吓出一身冷汗。

3.2 向量回路法原理

为什么叫“向量回路”？你看啊，从下平台铰链点B_i到上平台铰链点P_i，这条腿就是一个向量。我们可以沿着一条闭合回路走：

B → O_B → O_P → P → B

写成向量形式就是：

L_i = R * p_i + t - b_i

其中：

• L_i — 第i条腿的向量
• R — 旋转矩阵（描述上平台姿态）
• p_i — 上平台铰链点在动系中的坐标
• t — 上平台中心在静系中的位置向量
• b_i — 下平台铰链点在静系中的坐标

腿长就是L_i的模长。就这么简单？对，数学上就这么简单。但实际工程里，坑都在细节里。

3.3 铰链点坐标计算

上下平台的铰链点不是随便排的。常见的布局是六个铰链点均匀分布在两个圆上，但上下平台的半径不同，角度也有偏移。

上平台铰链点（动系中）：

p_i = [r_p * cos(θ_pi), r_p * sin(θ_pi), 0]^T

下平台铰链点（静系中）：

b_i = [r_b * cos(θ_bi), r_b * sin(θ_bi), 0]^T

这里的θ_pi和θ_bi不是随便给的。我见过有人直接平均分60度，结果平台运动到某些位置时腿会打架。正确的做法是考虑铰链的物理安装角度，通常上下平台的铰链要错开30度。

参数	符号	典型值	说明
上平台半径	rp	0.5 m	铰链点分布圆半径
下平台半径	rb	0.8 m	通常比上平台大
铰链偏角	α	15°	相邻铰链的半角
初始高度	h0	1.0 m	平台中位高度

3.4 完整求解流程

好了，咱们把整个流程串起来。我一般这么干：

1. 输入目标位姿：位置(x, y, z)和姿态(roll, pitch, yaw)
2. 计算旋转矩阵R：从欧拉角或四元数得到
3. 计算每个铰链点的位置向量：p_i和b_i
4. 代入向量回路公式：L_i = R * p_i + t - b_i
5. 求模长：l_i = ||L_i||
6. 输出六个腿长

代码实现大概长这样：

import numpy as np

def stewart_inverse_kinematics(pose, params):
    """
    逆运动学求解
    pose: [x, y, z, roll, pitch, yaw]
    params: 平台参数
    """
    x, y, z, roll, pitch, yaw = pose
    
    # 计算旋转矩阵
    Rx = np.array([[1, 0, 0],
                   [0, np.cos(roll), -np.sin(roll)],
                   [0, np.sin(roll), np.cos(roll)]])
    Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)],
                   [0, 1, 0],
                   [-np.sin(pitch), 0, np.cos(pitch)]])
    Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0],
                   [np.sin(yaw), np.cos(yaw), 0],
                   [0, 0, 1]])
    R = Rz @ Ry @ Rx
    
    # 位置向量
    t = np.array([x, y, z])
    
    leg_lengths = []
    for i in range(6):
        # 向量回路
        L = R @ params['p_upper'][i] + t - params['b_lower'][i]
        length = np.linalg.norm(L)
        leg_lengths.append(length)
    
    return np.array(leg_lengths)

3.5 验证与调试

代码写完了，怎么知道对不对？我的做法是：

• 零位测试：输入(0,0,h₀,0,0,0)，所有腿长应该相等
• 单轴运动：只改变一个自由度，观察腿长变化是否平滑
• 极限位置：测试平台在最大姿态角时，腿长是否在行程范围内

我记得有一次，零位测试时发现腿长不一样，查了半天发现是铰链点坐标算错了——角度单位用了度，但np.cos默认是弧度。这种低级错误，嗯，谁还没犯过呢？

3.6 本章知识体系

下面这张图，是我自己总结的逆运动学知识结构，你一看就明白：

这张图把整个逆运动学的流程和关键模块都串起来了。你从输入开始，经过向量回路这个核心计算，再到输出腿长。旁边的三个子模块是必须提前准备好的东西。最后别忘了验证，这一步省不得。

好了，逆运动学这块就讲到这里。说白了就是几何运算，但细节决定成败。坐标系、旋转顺序、铰链点布局，任何一个地方出问题，平台都跑不起来。我当年调试时，光验证零位就花了两天——后来发现是铰链点角度算错了半度。嗯，这种教训，一次就够了。