旋转、角速度与四元数：先把方向这件事说清楚

本节要解决什么问题

机器人转了以后，同一条速度向量在机体系与导航系的三个分量不同。机械臂每个关节也在改变坐标系。若把“物体真的动了”和“只是换一种坐标写法”混为一谈，后面的 IMU 积分、DVL 融合和机械臂正运动学都会出错。

本节只讨论姿态和运动学：描述怎么运动，不讨论需要多大力才能这样运动。力、质量、惯量将在动力学章出现。

先统一约定：$R=R_{nb}$ 把机体系 $b$ 的列向量分量变成导航系 $n$ 的分量；两个系均为右手系。本节角速度表示机体相对导航系的转动，局部导航系近似惯性；高精度长航时惯导还需从陀螺对惯性空间的角速度中扣除地球自转与导航系转率。四元数采用 Hamilton 乘法、标量在前的 $q=[w,x,y,z]^T$。角度参与三角函数时用 rad；角速度用 rad/s。软件库的存储顺序可能不同，不能靠变量名字猜。

1. 为什么旋转矩阵的列就是三个坐标轴

设机体系的三个单位轴在导航系中写成 $e_x^n,e_y^n,e_z^n$。机体系向量 $a^b=[a_x,a_y,a_z]^T$ 的含义是“沿这三根轴各走多少”。因此

这个式子不是凭空定义的矩阵乘法，而是三次轴向分解相加。轴是单位向量且互相垂直，于是每列与自身点乘为一、不同列点乘为零：

因此反向坐标变换只需转置。还需 $\det R=+1$ 保持右手性；满足正交性但行列式为负一的矩阵含镜像，不能当刚体旋转。

例如机体绕共同的正 $z$ 轴按右手方向转 $\alpha$，其 $x$ 轴在导航系中为 $[\cos\alpha,\sin\alpha,0]^T$，$y$ 轴为 $[-\sin\alpha,\cos\alpha,0]^T$。把列拼起来：

取 $\alpha=\pi/2$，机体系的前向速度 $a^b=[1,0,0]^T\ \mathrm{m/s}$ 变为 $a^n=[0,1,0]^T\ \mathrm{m/s}$。这是一条很适合放进单元测试的方向检查。

坐标变换为何按右到左复合

若 $a^n=R_{nb}a^b$ 且 $a^b=R_{bc}a^c$，代入得到 $a^n=R_{nb}R_{bc}a^c$。所以 $R_{nc}=R_{nb}R_{bc}$，靠右的变换先执行。一般 $R_xR_z\ne R_zR_x$，因为两次旋转的轴在空间中的含义不同。机械臂的变换链、相机外参链都应先写清上下标，再计算乘积。

2. 叉乘矩阵为什么会出现在所有旋转公式里

对任意 $a=[a_x,a_y,a_z]^T$ 定义

这个矩阵把“与 $a$ 做叉乘”改写成线性算子；它满足 $[a]_\times^T=-[a]_\times$，因此称反对称矩阵。三个独立数刚好对应一个三维向量。还应记住 $a\times b=-b\times a$，所以 $[a]_\times b=-[b]_\times a$。后续雅可比里的负号常来自这一步，而不是需要死记的额外规定。

固定在刚体上的点，离转轴的矢量为 $r$。在很短的 $dt$ 内绕单位轴 $u$ 转过 $d\theta$，一阶位移为

为什么是叉乘？位移方向同时垂直于轴与半径，长度为到轴距离乘转角；右手方向由叉乘给出。这就是速度雅可比、刚体杆臂补偿的几何来源。

3. 旋转矩阵的时间导数与角速度

设 $a^b$ 是刚体上固定向量，故 $\dot a^b=0$。由 $a^n=Ra^b$ 得到 $\dot a^n=\dot R a^b$；而刚才的刚体运动关系给出 $\dot a^n=[\omega^n]_\times Ra^b$。对所有 $a^b$ 都成立，因此

$\omega^b$ 与 $\omega^n$ 是同一个物理角速度在不同坐标系中的分量，不是两个角速度。机载陀螺仪通常输出本体轴向分量，所以积分常用右侧乘法 $R[\omega^b]_\times$。

还可对 $R^TR=I$ 求导，得 $\dot R^TR+R^T\dot R=0$，这证明 $R^T\dot R$ 必然反对称。因此它能用三个数表示，正是 $[\omega^b]_\times$。矩阵有九个数，但旋转瞬时自由度只有三个。

不要把 $\omega$ 直接当成滚转、俯仰、偏航角的逐分量导数。对于约定 $R=R_z(\psi)R_y(\theta)R_x(\phi)$，先把三个瞬时转轴写到机体系。滚转轴为 $e_x$；俯仰轴经过已有滚转的逆变换，为 $R_x^Te_y=(0,\cos\phi,-\sin\phi)^T$；偏航轴还须逆转俯仰，为 $R_x^TR_y^Te_z=(-\sin\theta,\sin\phi\cos\theta,\cos\phi\cos\theta)^T$。每根轴分别乘 $\dot\phi,\dot\theta,\dot\psi$ 后相加，把三根轴拼成三列便得到

三列分别是滚转、俯仰和偏航转轴在当前机体系的方向。这个变换的行列式为 $\cos\theta$；当俯仰为 $\pm\pi/2$ 时两种欧拉参数变化无法区分，逆映射不存在。这是参数化奇异，不是机器人在物理上突然少了一个自由度。

4. 从轴角到单位四元数

绕单位向量 $u$ 旋转角度 $\theta$，用

表示。这里 $v\in\mathbb R^3$ 是四元数向量部分，不是机器人线速度。它满足 $w^2+v^Tv=1$，四个存储分量受一个约束，故局部仍是三个自由度。

为什么是半角？下面的旋转采用 $q\otimes[0,a]\otimes q^*$，同一四元数从左右各作用一次。把乘法展开会产生 $\cos^2(\theta/2)-\sin^2(\theta/2)=\cos\theta$ 和 $2\sin(\theta/2)\cos(\theta/2)=\sin\theta$，恰好得到角度 $\theta$ 的旋转，而不是 $2\theta$。

Hamilton 乘法规定

上半部分是标量，下半部分是三维向量。叉乘项使其通常不交换。单位四元数的共轭与逆为 $q^*=q^{-1}=[w,-v]^T$；非单位四元数的逆还需除以模长平方，不能省略。

把 $a$ 写成纯四元数 $[0,a]^T$。先只做左边一次乘法：

再右乘 $q^*=[w,-v]^T$，其向量部分为

使用 $a\times v=-v\times a$ 和 $(v\times a)\times v=(v^Tv)a-v(v^Ta)$ 收集同类项，标量部分相消，向量部分得到

所以对应的旋转矩阵是

这一步既解释四元数怎么真正作用于向量，也提供独立于“库函数”的验证办法。代入轴角表达式，就得到下一节推导的 Rodrigues 公式。

例：$q=[\sqrt2/2,0,0,\sqrt2/2]^T$，即绕正 $z$ 轴转九十度，应该把 $[1,0,0]^T$ 变为 $[0,1,0]^T$。$-q$ 代入 $R(q)$ 也完全一样，因为所有项都含两个四元数分量。于是 $q$ 与 $-q$ 表示同一个姿态，四元数对旋转是双覆盖，不能把四元数分量跳号误判为物体瞬间转了一圈。

5. 从陀螺仪读数到姿态更新

设陀螺测量 $\omega_m$、偏置估计 $\hat b_g$，短区间内的角速度近似常量，先算 $\omega=\omega_m-\hat b_g$ 和转角向量 $\delta\theta=\omega\Delta t$。它们分别是 rad/s 与 rad；不能把角速度直接塞进轴角。

令 $a=\|\delta\theta\|$，对应的小旋转四元数为

右乘是因为 $\delta\theta$ 在机体系表达。若角速度在导航系表达，应构造导航系小旋转并左乘，不能只换乘法方向而不换分量。

当 $a\rightarrow0$，$\sin(a/2)/a\rightarrow1/2$，程序应采用级数 $1/2-a^2/48+\cdots$，避免零除。使用极限得到微分形式

简单 Euler 步进 $q+\dot q\Delta t$ 会产生模长漂移，应归一化；使用指数增量也建议为浮点舍入作轻量归一化。归一化只能修正单位模约束，不能消除陀螺偏置，也不能替代高动态下的圆锥运动补偿。

6. 为什么姿态误差用三个数，而不是四元数相减

两个四元数直接相减不尊重单位球面，而且 $q$ 与 $-q$ 的差很大却表示同姿态。定义右局部误差：

$\hat q$ 是名义姿态，$\delta\theta$ 是围绕该姿态的小三维转角误差。小角下 $\delta q\simeq[1,\delta\theta^T/2]^T$，因此可以在三维空间为误差建立高斯近似，而名义姿态仍保存在单位四元数里。

惯导常用的名义存储是位置三维、速度三维、四元数四维、加速度偏置三维、陀螺偏置三维，总共十六个数；误差状态是

这不是漏了一维，而是避免把四元数的单位模约束当成独立的不确定方向。不能将一个 $15\times15$ 的协方差直接按普通加法更新十六维名义状态；姿态必须通过小旋转注入。

7. 插值、误差角与实际实现边界

若要比较两个姿态，先求 $q_e=q_1^{-1}\otimes q_2$；为了取不超过 $\pi$ 的主旋转角，可把其符号选为标量部分非负，然后

不要只算四元数欧氏距离。刚好在 $\pi$ 附近，最短轴的符号并不唯一，算法需要一致的分支约定。

对单位四元数插值，先在 $q_1^Tq_2<0$ 时将 $q_2$ 取负，避免绕长路。令 $\beta=\arccos(q_1^Tq_2)$，球面插值为

$\beta$ 是四维单位球上的夹角，不是直接的物理旋转角；两者相差因子二。$\beta$ 很小时用归一化线性插值，点积进入 $\arccos$ 前截断到 $[-1,1]$ 以应对舍入。四元数避免了欧拉角那种局部积分奇异，但仍有双覆盖、单位模约束及全局分支问题，不是“从此没有任何数学特殊情况”。

8. 手写练习：先用方向测试抓出乘法错误

实现标量在前四元数乘法、旋转向量到四元数、向量旋转。测试至少包括零旋转、九十度绕轴、正负号等价、组合不交换、接近零的增量。输入都要求有限数值、四元数单位模或显式归一化；生产版本应拒绝零范数四元数。

#include <array>
#include <cmath>
#include <cassert>
#include <stdexcept>
using V = std::array<double,3>;
struct Q { double w; V v; };
V add(V a,V b){return {a[0]+b[0],a[1]+b[1],a[2]+b[2]};}
V mul(double s,V a){return {s*a[0],s*a[1],s*a[2]};}
double dot(V a,V b){return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];}
V cross(V a,V b){return {a[1]*b[2]-a[2]*b[1],
 a[2]*b[0]-a[0]*b[2],a[0]*b[1]-a[1]*b[0]};}
Q product(Q a,Q b){return {a.w*b.w-dot(a.v,b.v),
 add(add(mul(a.w,b.v),mul(b.w,a.v)),cross(a.v,b.v))};}
Q unit(Q q){double n=std::sqrt(q.w*q.w+dot(q.v,q.v));
 if(!std::isfinite(n)||n<1e-15)throw std::invalid_argument("bad q");
 return {q.w/n,mul(1/n,q.v)};}
Q fromRotvec(V x){double a=std::sqrt(dot(x,x));
 double s=a<1e-6?0.5-a*a/48.0:std::sin(a/2)/a;
 return unit({std::cos(a/2),mul(s,x)});}
V rotate(Q q,V x){q=unit(q);
 return add(add(mul(q.w*q.w-dot(q.v,q.v),x),
 mul(2*dot(q.v,x),q.v)),mul(2*q.w,cross(q.v,x)));}
int main(){const double pi=std::acos(-1.0);V ex{1,0,0};
 Q q=fromRotvec({0,0,pi/2});V y=rotate(q,ex);
 assert(std::abs(y[0])<1e-12&&std::abs(y[1]-1)<1e-12);
 V yn=rotate({-q.w,mul(-1,q.v)},ex);
 for(int i=0;i<3;i++)assert(std::abs(y[i]-yn[i])<1e-12);
 Q z=fromRotvec({0,0,0});assert(std::abs(z.w-1)<1e-12);
 Q tiny=fromRotvec({1e-12,0,0});assert(std::isfinite(tiny.v[0]));
 Q a=fromRotvec({pi/2,0,0}),b=fromRotvec({0,0,pi/2});
 V ab=rotate(product(a,b),ex),ba=rotate(product(b,a),ex);
 assert(std::abs(ab[2]-ba[2])>0.9);
}

9. 如何连接 ArduSub、机械臂与 SLAM

• 读飞控姿态接口时先核查输出四元数表示哪个方向的变换，以及数组顺序；不要根据一个库的约定推断另一个库。
• 对 DVL 速度做机体到导航系变换需要 $R_{nb}$；反向预测机体系测量需要 $R_{nb}^T$，两者的姿态雅可比符号不同。
• 对机械臂而言，每个关节改变一个相对坐标系，再将各段变换相乘；关节角向量不是四元数，末端整体姿态才需要旋转表示。
• 对 SLAM 而言，四元数存储姿态，优化器在局部三维转角上求增量，不能无约束优化四个分量后假装约束不存在。

原始资料与继续阅读

本节的数例、展开步骤和练习为独立教学推导；外部资料用于核对定义与约定，不照搬论文段落。

• Solà：Quaternion kinematics for the error-state Kalman filter：系统区分四元数约定、旋转与误差状态，建议对照第 2、4、5 节阅读。
• Modern Robotics 3.2.2：Angular Velocities：核对角速度、反对称矩阵与左/右表达。
• NASA：Attitude Representations for Kalman Filtering：阅读全局四元数与局部三分量姿态误差的设计动机。