数学基础：向量、矩阵、坐标系变换、四元数、欧拉角及其在运动控制中的应用

运动控制算法的本质，是对“物体在空间中的位置与姿态”进行精确的描述、计算与驱动。无论是六轴工业机器人的关节插补，还是无人机飞控中的姿态解算，亦或是移动机器人底盘的速度分解，其底层都依赖向量、矩阵、坐标系变换、欧拉角与四元数这五大数学工具。本章将系统梳理这些数学概念在运动控制中的工程映射，帮助读者建立从“公式”到“代码”的思维桥梁。

上图展示了本章的知识框架：向量与矩阵构成描述运动的基本代数工具，坐标系变换解决“在哪个参考系下描述”的问题，而欧拉角与四元数则提供姿态的两种主流表达方式。四者共同支撑起机器人运动学、飞控姿态解算与移动机器人导航等核心应用。下面我们逐一深入。

2.1 向量：运动控制中的位置、速度与力的统一描述

核心原理

向量是既有大小又有方向的量，在运动控制中通常用列向量表示。三维空间中，位置向量 p = [x, y, z]^T 描述物体在参考系中的坐标；速度向量 v = [v_x, v_y, v_z]^T 描述线速度；角速度向量 ω = [ω_x, ω_y, ω_z]^T 描述旋转速率。向量的点积（内积）用于计算投影与能量，叉积（外积）用于计算力矩与旋转轴。在代码中，向量通常以 float[3] 或 Eigen::Vector3d 形式存储。

判断方法或操作流程

在运动控制中，使用向量时需遵循以下流程：

1. 明确参考系：向量必须标注其所在坐标系，如 p_base 表示基座标系下的位置。
2. 选择运算规则：点积用于角度计算（cosθ = a·b / (|a||b|)），叉积用于求法向量或力矩。
3. 归一化处理：当向量仅表示方向时（如机器人末端朝向），需归一化为单位向量。
4. 数值稳定性检查：避免除以接近零的模长，防止浮点溢出。

案例拆解：移动机器人速度分解

假设一个差速驱动机器人，左右轮速度分别为 v_L = 0.3 m/s，v_R = 0.5 m/s，轮距 d = 0.4 m。我们需要计算机器人本体的线速度 v 和角速度 ω。

根据运动学模型：

v = (v_L + v_R) / 2 = 0.4 m/s
ω = (v_R - v_L) / d = 0.5 rad/s

若机器人当前位姿为 (x=1.0, y=2.0, θ=30°)，则世界坐标系下的速度向量为：

v_x = v * cos(θ) = 0.4 * cos(30°) ≈ 0.346 m/s
v_y = v * sin(θ) = 0.4 * sin(30°) = 0.2 m/s

这里，v 和 ω 构成一个二维运动向量，而 [v_x, v_y] 是其在世界系下的投影。实际调试中，若发现机器人走弧线而非直线，应检查左右轮速度是否一致，以及轮距参数是否准确。

2.2 矩阵：旋转矩阵与雅可比矩阵的工程应用

核心原理

矩阵在运动控制中承担两大核心角色：旋转矩阵描述坐标系之间的姿态变换，雅可比矩阵建立关节速度与末端速度之间的线性映射。旋转矩阵 R ∈ SO(3) 是正交矩阵，满足 R^TR = I 且 det(R) = 1。雅可比矩阵 J 则是一个 6×n 矩阵（n 为关节数），其每一列对应一个关节的速度贡献。

判断方法或操作流程

使用矩阵时，需注意以下操作要点：

• 矩阵乘法顺序：旋转矩阵左乘表示绕固定轴旋转，右乘表示绕自身轴旋转。例如 R = R_z(γ) · R_y(β) · R_x(α) 表示先绕 X 轴转 α，再绕新 Y 轴转 β，最后绕新 Z 轴转 γ。
• 矩阵求逆：对于旋转矩阵，逆矩阵即转置矩阵（R^-1 = R^T），可大幅简化计算。
• 雅可比矩阵的奇异性：当 det(J·J^T) = 0 时，机器人处于奇异位形，此时某些方向的速度无法实现。

案例拆解：六轴机器人雅可比矩阵应用

以典型的六轴关节机器人为例，其雅可比矩阵 J 将关节速度向量 q̇ = [q̇₁, ..., q̇₆]^T 映射到末端笛卡尔速度 v = [v_x, v_y, v_z, ω_x, ω_y, ω_z]^T。在调试中，若发现末端在某个方向运动迟缓，可检查 J 矩阵对应列的元素是否接近零，这往往意味着该关节对末端该方向运动贡献不足。

2.3 坐标系变换：从基座到工具的统一描述

核心原理

坐标系变换是运动控制的“通用语言”。一个完整的位姿由位置向量 p 和姿态矩阵 R 组成，通常用齐次变换矩阵 T = [R p; 0 1] 表示。变换链 T_base→tool = T_base→link1 · T_{link1→link2} · ... · T_linkN→tool 描述了从机器人基座到末端工具的总变换。

判断方法或操作流程

进行坐标系变换时，遵循以下步骤：

1. 建立变换树：明确所有坐标系之间的父子关系（如世界→机体→相机→目标）。
2. 确定变换参数：每个变换包含平移向量和旋转矩阵（或等效的欧拉角/四元数）。
3. 选择变换方向：正向变换（父→子）与逆向变换（子→父）互为逆矩阵。
4. 链式相乘：从根坐标系开始，依次左乘子变换矩阵。

案例拆解：视觉引导抓取中的坐标变换

在视觉引导机器人抓取场景中，相机固定于机器人上方，目标物体在相机坐标系下的位姿为 T_cam→obj。我们需要将其转换到机器人基座坐标系下：

T_base→obj = T_base→cam · T_cam→obj

其中 T_base→cam 通过手眼标定获得。假设标定结果为：

T_base→cam = [R_cam, p_cam; 0, 1]
R_cam = [[0, -1, 0], [1, 0, 0], [0, 0, 1]]  # 绕Z轴旋转90°
p_cam = [0.5, 0.2, 1.0]  # 相机在基座系下的位置

若相机检测到物体在相机系下的位置为 [0.1, 0.05, 0.8]，则物体在基座系下的位置为：

p_base = R_cam * p_cam_obj + p_cam
       = [0*(-0.1) + (-1)*0.05 + 0*0.8 + 0.5,
          1*0.1 + 0*0.05 + 0*0.8 + 0.2,
          0*0.1 + 0*0.05 + 1*0.8 + 1.0]
       = [0.45, 0.3, 1.8]

实际调试中，若抓取位置偏差较大，应优先检查标定矩阵的精度，而非盲目调整 PID 参数。

2.4 欧拉角与四元数：姿态表示的权衡与转换

核心原理

欧拉角用三个角度（如滚转 φ、俯仰 θ、偏航 ψ）描述姿态，直观但存在万向锁问题（当 θ = ±90° 时，φ 与 ψ 的旋转轴重合，丢失一个自由度）。四元数 q = [w, x, y, z]^T 是一种超复数表示，无奇异性，且插值平滑，适合计算与滤波。两者可相互转换：

// 欧拉角(RPY)转四元数
w = cos(φ/2)cos(θ/2)cos(ψ/2) + sin(φ/2)sin(θ/2)sin(ψ/2)
x = sin(φ/2)cos(θ/2)cos(ψ/2) - cos(φ/2)sin(θ/2)sin(ψ/2)
y = cos(φ/2)sin(θ/2)cos(ψ/2) + sin(φ/2)cos(θ/2)sin(ψ/2)
z = cos(φ/2)cos(θ/2)sin(ψ/2) - sin(φ/2)sin(θ/2)cos(ψ/2)

判断方法或操作流程

选择姿态表示方式时，可参考以下准则：

• 人机交互界面：使用欧拉角，便于操作者理解。
• 姿态解算与融合：使用四元数，避免万向锁且计算效率高。
• 轨迹插补：使用四元数球面线性插值（Slerp），保证旋转路径最短。
• 存储与传输：四元数仅需 4 个浮点数，比旋转矩阵（9 个）更紧凑。

案例拆解：无人机姿态解算中的四元数互补滤波

在无人机飞控中，陀螺仪积分得到姿态，但存在漂移；加速度计可测量重力方向，但噪声大。互补滤波将两者融合：

// 简化互补滤波（Mahony算法核心）
// 1. 用加速度计估计重力方向误差
error = cross(accel_normalized, estimated_gravity)
// 2. 将误差经PI补偿后叠加到陀螺仪角速度
gyro_corrected = gyro + Kp * error + Ki * integral(error)
// 3. 四元数积分更新
q_dot = 0.5 * q ⊗ [0, gyro_corrected]
q += q_dot * dt
// 4. 归一化四元数

实际调试中，若无人机悬停时姿态缓慢漂移，应增大 Ki 积分系数；若高频抖动，则需降低 Kp。通过四元数表示，避免了欧拉角在俯仰接近 90° 时的奇异问题。

2.5 综合实战：从传感器数据到执行器指令的数学链路

案例拆解：移动机器人航向推算

假设一个四轮移动机器人，配备惯性测量单元（IMU）和轮式编码器。我们需要融合两者数据，实时估计机器人位姿 (x, y, θ)。

初始条件：机器人起始位姿 (0, 0, 0)，IMU 输出偏航角速度 ω_imu = 0.1 rad/s，编码器测得线速度 v = 0.5 m/s，运行 10 秒。

分析过程：

1. 使用编码器积分位置：x += v·cos(θ)·dt，y += v·sin(θ)·dt。
2. 使用 IMU 积分航向：θ += ω_imu·dt。
3. 每 100ms 用加速度计修正横滚与俯仰（此处忽略）。

计算结果：10 秒后，θ = 1.0 rad，x = 0.5·sin(1.0) ≈ 0.42 m，y = 0.5·(1-cos(1.0)) ≈ 0.23 m（圆弧轨迹）。

调试要点：若实际轨迹与计算不符，检查编码器安装方向是否与 IMU 的 Z 轴一致，以及时间同步是否准确。这里，向量（位置）、矩阵（旋转）、四元数（姿态）全部参与运算，体现了数学工具的综合应用。