4、位置反解（Inverse Kinematics）原理：已知上平台位姿，求各支腿长度。几何法推导。

各位工程师朋友，欢迎来到这一讲。

咱们今天聊的，是并联机器人里最核心、也最绕不开的一个问题——位置反解。说白了，就是给你一个上平台的目标位姿（位置+姿态），你得算出六个支腿分别该伸长多少。

嗯，这就像你开着一台六轴机床，告诉它“刀尖给我到这儿，角度给我摆成这样”，然后伺服电机得知道每根丝杠转多少圈。反解，就是那个从“想”到“做”的桥梁。

4.1 为什么反解比正解“好欺负”？

我个人习惯，先讲清楚一个概念：反解通常有解析解，正解往往没有。

你想想看，串联机器人（比如常见的六轴工业臂）是正解容易、反解难。因为串联是“关节空间→末端位姿”，正向传递很直接；但反过来，从末端位姿反推关节角，往往要解高次方程，甚至多解。

并联机器人刚好反过来。它的结构是闭环的，支腿长度和平台位姿之间有明确的几何约束。你只要把上平台、下平台的铰点坐标写清楚，再用两点间距离公式一套，长度就出来了。说白了，就是纯几何计算，没有复杂的迭代。

我在项目中遇到过一位同事，他非要用数值法去解反解，结果迭代半天还不收敛。我告诉他：“你直接用几何法，三行公式就搞定。” 他试完之后，拍着桌子说：“原来这么简单！”

4.2 几何法推导：一步一步来

好，咱们开始推导。我尽量用最直观的方式讲，不堆砌矩阵。

4.2.1 先定义坐标系

我们需要两个坐标系：

• 固定坐标系 {B}：固结在下平台（基座）上，原点通常在下平台中心。
• 动坐标系 {P}：固结在上平台（动平台）上，原点在上平台中心。

下平台的六个铰点，在 {B} 系下的坐标是已知的，记作 B_i（i=1~6）。上平台的六个铰点，在 {P} 系下的坐标也是已知的，记作 P_i。

嗯，这里要注意：B_i 和 P_i 都是设计时就定好的，属于结构参数。你换一台机器，这些值就不一样了。

4.2.2 上平台位姿怎么表示？

上平台的位姿，由两部分组成：

• 位置：动坐标系原点在固定坐标系下的坐标，记作 O_p = [x, y, z]^T。
• 姿态：动坐标系相对于固定坐标系的旋转矩阵，记作 R。

旋转矩阵 R 可以用欧拉角、四元数或者方向余弦矩阵表示。我个人习惯用 ZYX 欧拉角（先绕Z转γ，再绕Y转β，最后绕X转α），因为直观。

4.2.3 关键一步：把上平台铰点坐标转到固定坐标系

上平台的铰点 P_i 是在动坐标系 {P} 下描述的。要算它到固定坐标系下对应铰点 B_i 的距离，得先把 P_i 转换到 {B} 系下。

这个转换公式很简单：

B_P_i = R * P_i + O_p

其中：

• B_P_i 是上平台第 i 个铰点在固定坐标系下的坐标。
• R 是旋转矩阵（3x3）。
• P_i 是上平台第 i 个铰点在动坐标系下的坐标（3x1）。
• O_p 是动坐标系原点在固定坐标系下的位置（3x1）。

说白了，就是先旋转、再平移。你想想看，这跟你在 CAD 里把一个零件先转个角度、再挪个位置，是一个道理。

4.2.4 最后一步：算支腿长度

现在，下平台铰点 B_i 和上平台铰点 B_P_i 都在同一个坐标系 {B} 下了。支腿长度就是这两点之间的欧氏距离：

L_i = || B_P_i - B_i ||

展开写就是：

L_i = sqrt( (B_P_ix - B_ix)^2 + (B_P_iy - B_iy)^2 + (B_P_iz - B_iz)^2 )

对 i = 1 到 6，重复这个计算，你就得到了六个支腿的长度。

对于 i = 1 到 6:
    B_P_i = R * P_i + O_p
    L_i = || B_P_i - B_i ||

4.3 一个具体的例子（Stewart 平台）

为了让你更清楚，我拿最常见的 Stewart 平台举个例子。

假设下平台六个铰点均匀分布在一个半径为 R_b 的圆上，角度间隔 60°，但相邻两个铰点之间错开 30°（这是典型的 Stewart 布局）。上平台类似，半径为 R_p。

那么，下平台铰点坐标（在 {B} 系下）为：

B_i = [R_b * cos(θ_bi), R_b * sin(θ_bi), 0]^T

上平台铰点坐标（在 {P} 系下）为：

P_i = [R_p * cos(θ_pi), R_p * sin(θ_pi), 0]^T

其中 θ_bi 和 θ_pi 是各个铰点的角度位置。

现在，给定上平台位姿 O_p = [0, 0, 1]（高度 1 米），姿态为绕 Z 轴转 10°（即 γ = 10°），其他角度为 0。那么旋转矩阵 R 就是：

R = [cos(10°)  -sin(10°)  0
     sin(10°)   cos(10°)  0
     0          0         1]

然后，对每个铰点，计算 B_P_i = R * P_i + O_p，再算 L_i = || B_P_i - B_i ||。

嗯，你可以在 MATLAB 或 Python 里写个循环，几行代码就搞定了。

4.4 几何法的本质：就是解三角形

你可能会问：“这跟几何法有什么关系？不就是在算坐标吗？”

其实，你仔细看，每个支腿、上平台铰点、下平台铰点，构成了一个空间三角形。已知两条边（上平台半径、下平台半径）和夹角（由位姿决定），求第三边（支腿长度）。这就是几何法的本质——空间三角形的边角关系。

只不过，我们用坐标法把这个问题统一处理了，避免了手动画图和解三角函数的麻烦。

4.5 反解的应用场景

反解在工程中无处不在：

• 运动控制：你给控制器一个目标位姿，它必须实时算出支腿长度，然后驱动电机。反解是控制循环的第一步。
• 轨迹规划：你要让上平台走一条平滑的轨迹，就得在轨迹上采样多个点，对每个点做反解，得到支腿长度随时间的变化曲线。
• 标定：在机器装配完后，实际铰点坐标和设计值有偏差。通过反解和实测数据，可以反推出真实的铰点位置。

4.6 本章小结

好，咱们把这一讲的核心捋一捋：

• 位置反解，就是已知上平台位姿，求六个支腿长度。
• 几何法的核心步骤：坐标变换 + 距离公式。
• 先定义固定坐标系和动坐标系，把上平台铰点从动系转到固定系，再算与下平台铰点的距离。
• 反解有解析解，计算简单、速度快，适合实时控制。

嗯，这一讲就到这里。你回去可以自己写个程序，随便给个位姿，算算支腿长度，感受一下几何法的简洁之美。

公众号:蓝海资料掘金营，微信deep3321