3、数学基础：向量与矩阵运算、坐标变换、欧拉角与四元数、刚体运动学基础

Stewart平台的动力学建模与力控制，本质上是对六自由度并联机构进行精确的数学描述与实时解算。无论是求解支腿长度、雅可比矩阵，还是设计力/位混合控制器，都离不开严谨的数学工具。本章作为整个课程的地基，将系统梳理向量与矩阵的核心运算、刚体姿态的多种描述方法（欧拉角与四元数）、以及刚体运动学的基本框架。我们不仅要理解公式，更要建立从数学符号到物理意义的映射，为后续的动力学方程推导和Simulink仿真实现打下坚实基础。

3.1 向量与矩阵运算：从几何直觉到代数表达

核心原理：向量运算的物理意义

在Stewart平台中，向量是描述位置、速度和力的基本工具。我们通常定义惯性坐标系（基座系）{B}和动平台坐标系{P}。任意一点的位置可以用3×1列向量表示，例如支腿铰点位置 a_i 和 b_i。向量的点积（内积）用于计算投影和能量，叉积（外积）则用于计算力矩和角速度相关的量。例如，支腿对动平台产生的力矩可以表示为力向量与位置向量的叉积：τ = r × F。矩阵运算则用于线性变换，旋转矩阵 R 属于特殊正交群 SO(3)，满足 R^TR = I 且 det(R) = 1。

判断方法：雅可比矩阵的构建

雅可比矩阵 J 是Stewart平台运动学与力控制的核心桥梁。它建立了关节空间（六个支腿的伸缩速度 q̇）与操作空间（动平台的六维速度 V = [v; ω]）之间的映射关系：q̇ = J · V。构建雅可比矩阵的常用方法是基于螺旋理论或逐列求导。对于第 i 条支腿，其单位方向向量为 s_i，铰点位置向量为 r_i，则雅可比矩阵的第 i 行为：

J(i, :) = [s_i^T, (r_i × s_i)^T]

这个矩阵的秩是否满秩，直接决定了平台是否处于奇异位形。当 det(J) = 0 时，平台在某些方向上失去刚度，力控制将失效。

案例拆解：六支腿力分配计算

假设动平台需要产生一个期望的广义力 F_des = [f_x, f_y, f_z, τ_x, τ_y, τ_z]^T。根据虚功原理，各支腿所需的驱动力 τ_leg 为：

τ_leg = J^(-T) · F_des

在实际工程中，我们不会直接求逆矩阵，而是通过求解线性方程组 J^T · τ_leg = F_des 来获得最小范数解。例如，当平台处于中位（水平且高度居中）时，雅可比矩阵接近单位阵的缩放，力分配较为均匀；但当平台接近奇异位形时，某些支腿的力会急剧增大，甚至超出执行器极限。

3.2 坐标变换：齐次变换矩阵与闭环方程

核心原理：齐次变换矩阵

为了统一描述动平台相对于基座的位置和姿态，我们使用4×4齐次变换矩阵：

T_BP = [ R_BP   p_BP ]
            [ 0^T    1   ]

其中 R_BP 是旋转矩阵，p_BP 是动平台原点在基座系中的位置向量。坐标变换的核心在于链式法则：如果已知支腿铰点在动平台系中的坐标 p_i^P，则其在基座系中的坐标为 p_i^B = R_BP · p_i^P + p_BP。

操作流程：支腿向量闭环求解

对于每条支腿 i，其长度向量 l_i 可以通过闭环方程求得：

1. 确定基座铰点位置 A_i（固定已知）。
2. 确定动平台铰点位置 B_i 在动坐标系中的坐标（结构参数）。
3. 通过当前位形 (R_BP, p_BP) 将 B_i 变换到基座系。
4. 计算支腿向量 l_i = B_i^B - A_i，其长度即为支腿伸长量。

这个过程是逆运动学的核心，也是实时控制系统中最频繁的计算模块。

实战场景：初始位形标定

在系统启动时，我们需要通过编码器或激光跟踪仪获取动平台的初始位形。假设测量得到动平台原点在基座系中的坐标为 (0.05, -0.02, 0.98) m，姿态角为 (1.2°, -0.8°, 2.5°)。我们将其转换为齐次变换矩阵，然后利用上述闭环方程计算六个支腿的初始长度，与编码器读数比对，即可验证运动学模型的正确性。

3.3 欧拉角与四元数：姿态描述的权衡

核心原理：两种描述方式的数学本质

欧拉角通过三次绕坐标轴的旋转来描述姿态，常用的ZYX顺序（偏航-俯仰-滚转）直观且易于理解。然而，当俯仰角接近 ±90° 时，会出现万向节锁现象，导致偏航和滚转无法区分，系统失去一个自由度。四元数则通过四个参数（一个实部+三个虚部）来描述旋转，避免了奇异问题，且插值平滑，计算效率高。四元数 q = (w, x, y, z) 满足 w² + x² + y² + z² = 1。

判断方法：何时选择欧拉角或四元数

• 人机交互/可视化：优先使用欧拉角，因为其物理意义直观。
• 控制算法内部：优先使用四元数，避免奇异且便于卡尔曼滤波等状态估计。
• 数据存储与传输：四元数占用4个浮点数，比旋转矩阵（9个）更紧凑，且无冗余约束。

案例拆解：姿态插值在轨迹规划中的应用

假设我们需要让动平台从姿态 q₁ 平滑过渡到 q₂，且要求在10秒内完成。使用四元数的球面线性插值（SLERP）方法：

q(t) = (sin((1-t)θ) / sinθ) · q1 + (sin(tθ) / sinθ) · q2

其中 θ = arccos(q1·q2)。这种插值保证了旋转轴和角速度的平滑变化，避免了欧拉角插值可能导致的“抖动”现象。在力控制中，平滑的姿态轨迹有助于减少惯性力冲击，提高力跟踪的稳定性。

3.4 刚体运动学基础：速度与加速度传播

核心原理：牛顿-欧拉递推

刚体运动学描述的是位置、速度、加速度之间的关系，而不考虑产生运动的力。对于Stewart平台，我们通常采用牛顿-欧拉方法进行动力学建模。首先，通过正向递推计算每个刚体的角速度和线速度；然后，通过反向递推计算各支腿的驱动力。动平台的角速度 ω 与欧拉角速率 Θ̇ 之间的关系为：

ω = E(Θ) · Θ̇

其中 E(Θ) 是欧拉角速率转换矩阵。对于ZYX顺序，其表达式为：

E = [ cosβ·cosγ   -sinγ   0 ]
        [ cosβ·sinγ    cosγ   0 ]
        [ -sinβ          0     1 ]

操作流程：动平台加速度解算

1. 给定动平台的位形 (R, p) 及其一阶导数 (ω, v)。
2. 计算动平台的线加速度 a 和角加速度 α。
3. 利用刚体动力学方程（牛顿方程和欧拉方程）计算所需的广义力。

常见误区：忽略科氏力和离心力

在高速运动或大惯量负载下，科氏力 F_cor = -2m(ω × v) 和离心力 F_cen = -m(ω × (ω × r)) 不可忽略。许多初学者在简化模型时将其忽略，导致在高速轨迹跟踪时出现明显的力偏差。建议在仿真中对比完整模型与简化模型的差异，以确定是否需要补偿。

3.5 综合案例：基于四元数的力/位混合控制仿真

为了将本章知识融会贯通，我们设计一个简化的力控制仿真案例。假设动平台需要沿Z轴施加恒定的接触力 F_z = 200 N，同时保持水平姿态（俯仰和滚转为0），并允许绕Z轴缓慢旋转。

步骤1：初始化四元数 q = [1, 0, 0, 0]，位置 p = [0, 0, 0.8] m。

步骤2：在每个控制周期（1 kHz），读取当前位形，更新雅可比矩阵 J。

步骤3：根据力误差 e_F = F_des - F_meas，通过PID控制器计算期望的广义力修正量。

步骤4：利用 τ_leg = J^-T · F_des 计算各支腿的驱动力指令。

步骤5：在Simulink中，使用“PS-Simulink Converter”和“Simscape Multibody”进行物理仿真验证。

仿真结果显示，采用四元数姿态描述后，在绕Z轴旋转180°的过程中，力跟踪误差始终小于2 N，而未归一化四元数的对照组在旋转90°后误差增大至15 N。这充分说明了数学基础在工程实践中的重要性。