3、运动学基础（二）：Stewart平台几何参数定义、位置反解（Inverse Kinematics）推导与实现

各位同学，欢迎来到第三讲。

上一讲我们聊了Stewart平台的空间自由度，以及它为什么能实现六自由度运动。今天咱们要动真格的了——把平台的几何参数定义清楚，然后推导位置反解。

位置反解，说白了就是：给定动平台的位置和姿态，算出六个电动缸的长度。这是所有控制算法的基础。你想想看，如果连缸长都算不准，后面的自适应控制就是空中楼阁。

3.1 几何参数定义——先把坐标系搭好

做机器人运动学，第一件事就是定义坐标系。我个人习惯把这件事放在最前面做，因为坐标系一旦定错，后面所有推导都是白费功夫。

Stewart平台涉及两个关键坐标系：

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

嗯，这里要注意：两个坐标系的原点不一定在同一个垂直线上，尤其是当平台处于初始零位时，我们通常让{P}的原点位于{B}的Z轴正方向上，距离为h₀。

接下来定义铰点位置：

  • 下平台铰点：B₁, B₂, ..., B₆，在{B}系下的坐标是已知常数。
  • 上平台铰点：P₁, P₂, ..., P₆，在{P}系下的坐标是已知常数。

我在项目中遇到过一个问题：有人把铰点坐标定义在平台边缘，结果算出来的缸长干涉了。所以我的建议是——铰点坐标一定要用极坐标形式定义，方便后续调整半径和角度。

关键参数表（我常用的定义方式）：

参数符号说明
基座铰点分布半径R_b下平台铰点所在圆的半径
动平台铰点分布半径R_p上平台铰点所在圆的半径
基座铰点角度偏移θ_b相邻铰点对的夹角（通常60°）
动平台铰点角度偏移θ_p同上，但上下平台通常错开30°
初始高度h₀动平台原点在基坐标系下的Z坐标

为什么上下平台要错开30°？说白了就是为了避免铰点重合，保证运动空间最大化。这是Stewart平台的经典设计，你记住就行。

3.2 位置反解推导——从几何到代数

好，坐标系和参数都定义好了，现在开始推导。

位置反解的核心思想其实很简单：每个电动缸的长度，等于上铰点与下铰点之间的空间距离。

但这里有个坑：上铰点是在动坐标系{P}下定义的，而下铰点是在基坐标系{B}下定义的。两个坐标系之间有相对运动，所以我们需要把上铰点坐标转换到基坐标系下。

转换公式长这样：

P_i_in_B = T * P_i_in_P

其中T是4×4的齐次变换矩阵，包含旋转矩阵R和平移向量t。

旋转矩阵R由三个欧拉角（通常用Z-Y-X顺序）决定，平移向量t就是动平台原点的位置(x, y, z)。

我曾经犯过一个低级错误：把旋转矩阵的顺序搞反了，结果算出来的缸长全是负数。嗯，从那以后我每次写代码都会先验证一下——给一个已知姿态，看看变换后的点坐标是否合理。

具体推导步骤如下：

  1. 给定动平台位姿：位置(x, y, z)，姿态(α, β, γ)。
  2. 构造变换矩阵T：根据欧拉角计算旋转矩阵R，再结合平移向量t。
  3. 计算上铰点在基系下的坐标：P_i_B = R * P_i_P + t。
  4. 计算缸长向量：L_i = P_i_B - B_i。
  5. 计算缸长标量：l_i = ||L_i||。

你看，就这么五步。但实际实现时，每一步都有细节要注意。

我的小技巧：在代码里，我习惯把铰点坐标存成6×3的矩阵，这样可以用矩阵运算一次性算出所有缸长，比循环快得多。尤其是在实时控制中，每毫秒都要算一次反解，效率很重要。

3.3 代码实现——把公式变成可运行的程序

理论讲完了，咱们看看代码怎么写。我用Python写了一个简单的实现，核心函数就几十行。

import numpy as np

def stewart_inverse_kinematics(pose, params):
    """
    位置反解函数
    :param pose: [x, y, z, alpha, beta, gamma]
    :param params: 包含铰点坐标等参数
    :return: 六个缸长
    """
    x, y, z, alpha, beta, gamma = pose
    
    # 构造旋转矩阵（Z-Y-X顺序）
    Rz = np.array([[np.cos(alpha), -np.sin(alpha), 0],
                   [np.sin(alpha),  np.cos(alpha), 0],
                   [0, 0, 1]])
    Ry = np.array([[np.cos(beta), 0, np.sin(beta)],
                   [0, 1, 0],
                   [-np.sin(beta), 0, np.cos(beta)]])
    Rx = np.array([[1, 0, 0],
                   [0, np.cos(gamma), -np.sin(gamma)],
                   [0, np.sin(gamma),  np.cos(gamma)]])
    R = Rz @ Ry @ Rx
    
    # 平移向量
    t = np.array([x, y, z])
    
    # 计算每个缸长
    leg_lengths = []
    for i in range(6):
        P_i_B = R @ params['P_i_P'][i] + t
        L_i = P_i_B - params['B_i'][i]
        l_i = np.linalg.norm(L_i)
        leg_lengths.append(l_i)
    
    return np.array(leg_lengths)

这段代码看起来简单，但有几个地方容易出错：

  • 旋转顺序：一定要和你的姿态定义一致。我见过有人用Z-Y-X，但代码里写成了X-Y-Z，结果对不上。
  • 角度单位：我建议统一用弧度，避免在转换中出错。
  • 数值稳定性：当平台接近奇异位形时，缸长计算可能不稳定。我一般会加一个检查——如果缸长超出物理范围，就报错。

注意：位置反解虽然简单，但它依赖于准确的几何参数。如果铰点坐标测量有误差，算出来的缸长就会不准。我曾经在调试时发现平台运动轨迹有偏差，查了两天才发现是铰点半径参数写错了。所以，参数标定是Stewart平台工程化中非常重要的一环，后面我们会专门讲。

3.4 知识体系总览

为了让你对本章内容有个整体把握，我画了一张图。这张图把几何参数、坐标系定义、反解推导和代码实现串在了一起。

Stewart平台位置反解知识体系 输入：动平台位姿 (x, y, z, α, β, γ) 几何参数定义 R_b, R_p, θ_b, θ_p, h₀ 铰点坐标 B_i, P_i (6组) 核心推导：位置反解 P_i_B = R · P_i_P + t l_i = ||P_i_B - B_i|| 代码实现 Python函数：stewart_inverse_kinematics() 输出：六个缸长 l₁ ~ l₆ 注意：旋转顺序为Z-Y-X 铰点坐标需提前标定

这张图从左到右、从上到下展示了位置反解的完整流程。你写代码的时候，可以对照这张图来组织你的函数结构。

3.5 避坑指南——我踩过的几个坑

最后，分享几个我实际项目中踩过的坑，希望能帮你省点时间：

  • 坑一：铰点编号顺序——上下平台的铰点编号必须一一对应。我见过有人把B₁对应到了P₂，结果算出来的缸长乱套了。
  • 坑二：初始高度h₀——这个值不是随便给的。它决定了平台的工作空间。我一般会先做一次正解验证，确保在零位时所有缸长相等。
  • 坑三：欧拉角的万向锁——当β接近±90°时，旋转矩阵会退化。虽然Stewart平台很少用到这么大的角度，但如果你做仿真，最好加一个保护。

好了，这一讲的内容就到这里。位置反解是Stewart平台运动学中最基础、最常用的工具。下一讲我们会聊位置正解——那个才是真正让人头疼的问题。

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

← 上一章 📖 返回目录 下一章 →