4、运动学逆解核心：给定平台位姿求解各支腿长度

Stewart平台的运动学逆解（Inverse Kinematics, IK）是六自由度模拟器控制中最基础、最核心的数学环节。其任务可概括为：已知动平台（上平台）在空间中的目标位姿（位置 + 姿态），求解六个驱动支腿（通常为电动缸或液压缸）各自所需的长度。逆解是实时控制系统生成位置指令的前提——控制器必须每秒计算数十至上千次支腿长度指令，才能驱动平台平滑运动。本章将从几何建模出发，推导完整的逆解公式，并给出可直接工程落地的MATLAB/Python实现，同时剖析数值稳定性与坐标系约定等关键细节。

上图展示了逆解的整体几何链路：基座坐标系{B}固定于地面，动平台坐标系{P}固连于上平台。逆解的本质是通过坐标变换，将动平台铰点在{P}系中的局部坐标转换到{B}系，再与基座铰点坐标做矢量差，得到六个支腿向量。下面逐步展开推导。

4.1 坐标系定义与铰点布局

在推导公式前，必须明确两个关键约定：坐标系方向与铰点圆周分布。工程中常见的布局是六铰点均匀分布在半径为R的圆周上，但相邻铰点间隔并非完全60°，而是采用“短边间隔”与“长边间隔”交替的设计（例如短边20°，长边100°），这种布局能避免支腿干涉并改善力传递特性。

设基座铰点B_i在{B}系中的坐标为：

B_i = [R_B·cos(θ_Bi), R_B·sin(θ_Bi), 0]^T   (i=1..6)

其中θ_Bi为第i个基座铰点的方位角。同理，动平台铰点P_i在{P}系中的坐标为：

P_i = [R_P·cos(θ_Pi), R_P·sin(θ_Pi), 0]^T

注意：θ_Bi与θ_Pi的初始方位角通常相差一个偏置角（如30°），这是为了避免支腿在零位时完全竖直或产生奇异位形。实际工程中，铰点还包含万向铰或球铰的安装高度，但建模时通常将铰点简化为理想点。

4.2 位姿描述与旋转矩阵

动平台在空间中的位姿由位置向量 t = [x, y, z]^T 和姿态矩阵 R 描述。姿态矩阵通常采用欧拉角或四元数表示。在模拟器控制中，Z-Y-X欧拉角（即先绕Z轴偏航γ，再绕新Y轴俯仰β，最后绕新X轴滚转α）最为常用，因为其物理意义直观，便于操作员理解。

对应的旋转矩阵为：

R = Rz(γ) · Ry(β) · Rx(α)

其中：
Rx(α) = [1,    0,     0;
         0,  cosα, -sinα;
         0,  sinα,  cosα]

Ry(β) = [ cosβ, 0, sinβ;
            0,   1,   0;
         -sinβ, 0, cosβ]

Rz(γ) = [cosγ, -sinγ, 0;
         sinγ,  cosγ, 0;
           0,     0,   1]

展开后得到完整的旋转矩阵元素，这是后续编程的基础。需要特别注意的是：欧拉角存在万向锁问题，当β接近±90°时，α与γ的旋转轴重合，导致姿态奇异。对于模拟器应用，通常限制β在±30°以内，可有效避开奇异区域。

4.3 逆解公式推导

逆解推导的核心思想是“从目标位姿反推各支腿向量”。设动平台铰点P_i在{P}系中的坐标为已知常量，则其在{B}系中的坐标为：

P_i^B = R · P_i + t

于是第i根支腿的向量为：

L_i = P_i^B - B_i = R · P_i + t - B_i

支腿长度为该向量的欧几里得范数：

L_i = ||L_i|| = sqrt(L_ix² + L_iy² + L_iz²)

将上述过程写成矩阵形式，可一次性计算全部六根支腿：

% 输入：
%   pos = [x, y, z]         位置
%   euler = [α, β, γ]       欧拉角（弧度）
%   B  = 6x3矩阵，基座铰点坐标
%   P  = 6x3矩阵，动平台铰点坐标（在{P}系中）
% 输出：
%   L  = 6x1向量，支腿长度

R = rotationMatrix(euler);   % 3x3旋转矩阵
L = zeros(6,1);
for i = 1:6
    P_B = R * P(i,:)' + pos';   % 动平台铰点在{B}系中的坐标
    L_vec = P_B - B(i,:)';      % 支腿向量
    L(i) = norm(L_vec);
end

这段伪代码展示了逆解的全部数学过程。实际工程中，还需考虑支腿的最小/最大行程限制，以及万向铰的转角限制，这些将在后文讨论。

4.4 MATLAB/Python实现与验证

下面给出完整的MATLAB实现，包含旋转矩阵构造、逆解计算和简单的验证逻辑：

function L = stewart_ik(pos, euler, B, P)
    % 输入：pos=[x,y,z], euler=[alpha,beta,gamma]（弧度）
    %       B: 6x3基座铰点坐标
    %       P: 6x3动平台铰点坐标（在{P}系中）
    % 输出：L: 6x1支腿长度
    
    % 构造旋转矩阵（Z-Y-X顺序）
    alpha = euler(1); beta = euler(2); gamma = euler(3);
    Rx = [1, 0, 0;
          0, cos(alpha), -sin(alpha);
          0, sin(alpha), cos(alpha)];
    Ry = [cos(beta), 0, sin(beta);
          0, 1, 0;
          -sin(beta), 0, cos(beta)];
    Rz = [cos(gamma), -sin(gamma), 0;
          sin(gamma), cos(gamma), 0;
          0, 0, 1];
    R = Rz * Ry * Rx;
    
    % 计算支腿长度
    L = zeros(6,1);
    for i = 1:6
        P_B = R * P(i,:)' + pos';   % 转换到基座坐标系
        L_vec = P_B - B(i,:)';
        L(i) = norm(L_vec);
    end
end

Python实现（使用NumPy）在结构上完全一致，仅语法不同。验证逆解正确性的常用方法是正解交叉验证：先给定一组支腿长度，用数值方法（如牛顿-拉夫森迭代）求正解得到位姿，再将该位姿代入逆解，检查得到的支腿长度是否与初始值一致。若误差小于1e-6，则说明逆解实现正确。

4.5 案例拆解：六自由度模拟器零位标定与行程校验

某飞行模拟器项目，平台机械参数如下：基座铰点分布半径R_B=1.2m，动平台铰点分布半径R_P=0.9m，短边间隔20°，长边间隔100°，动平台初始高度z0=1.5m。在系统调试阶段，工程师需要验证逆解模块的正确性，并检查各支腿在极限位姿下是否超出行程（电动缸行程为±0.4m）。

初始现象：当给定目标位姿为x=0.05m, y=-0.03m, z=1.52m, α=2°, β=-1.5°, γ=3°时，逆解计算出的六根支腿长度分别为：1.482m, 1.495m, 1.471m, 1.488m, 1.503m, 1.476m。所有长度均在行程范围内，但工程师发现第2根和第5根支腿的长度变化率明显大于其他支腿，怀疑存在运动耦合或铰点布局问题。

分析过程：首先检查铰点坐标输入是否正确。通过CAD模型导出实际铰点坐标，与理论值对比发现：第2个基座铰点存在约3mm的安装偏差。修正坐标后重新计算，支腿长度变化率趋于均匀。进一步分析发现，该偏差导致第2根支腿在平台做纯滚转运动时产生额外的伸缩量，长期运行会加剧电动缸磨损。

实际操作步骤：

1. 使用激光跟踪仪测量六个基座铰点和六个动平台铰点的实际三维坐标；
2. 将测量值替换理论值，更新B和P矩阵；
3. 重新运行逆解程序，对比修正前后的支腿长度差异；
4. 在极限位姿（如最大俯仰角±25°、最大滚转角±20°）下检查所有支腿长度是否在行程范围内；
5. 若某支腿超出行程，需调整平台初始高度或减小运动范围。

结果对比：修正铰点坐标后，在相同目标位姿下，支腿长度变为：1.479m, 1.491m, 1.468m, 1.485m, 1.499m, 1.473m。最大偏差从原来的8mm减小到2mm以内，且各支腿长度变化率更加均衡。该案例表明，铰点坐标的准确性对逆解结果有直接影响，工程实施中必须重视机械标定。

4.6 常见误区与工程复盘

在逆解实现过程中，工程师常犯以下错误：

复盘总结：逆解模块虽然数学上简单，但工程落地时需注意三个层次：数学正确性（旋转矩阵、坐标变换无误）、参数准确性（铰点坐标、初始高度精确）、控制鲁棒性（行程限制、奇异规避、数值稳定）。建议在代码中增加单元测试模块，自动验证逆解-正解的一致性，确保每次修改参数后系统仍能可靠运行。

此外，对于实时控制系统，逆解计算耗时需控制在微秒级。MATLAB实现可直接转换为C/C++代码，或使用MATLAB Coder生成嵌入式代码。Python实现可借助NumPy的向量化操作，避免显式循环，进一步提升计算效率。在下一阶段，我们将讨论如何将逆解模块集成到完整的控制循环中，并处理多线程同步与实时性问题。