移动机器人学（二）四元数

四元数

四元数的定义与基本运算
在飞行器姿态解算中一般采用四元数表达旋转，个人理解，旋转矩阵用3*3的矩阵以及附加的6个非线性约束（列向量之间相互正交且模长为1）描述了旋转，而四元数通过4个数以及一个约束（单位四元数）描述了同样的信息量，类似对于一个线性方程组，通过代换消元的方式减少约束，减少变量数，达到减少计算量的目的。实际上，惯性导航系统中是通过四元数实现快速循环迭代，它能够以1000Hz的速度更新运动对象的姿态。

三维旋转表示

单位四元数 q~=cos(θ/2)+w^sin(θ/2)\tilde{q}=cos(\theta/2)+\widehat{w}sin(\theta/2)q~=cos(θ/2)+wsin(θ/2) 可以表示在三维空间中绕（单位向量）w^\widehat{w}w 旋转 θ\thetaθ 角的旋转算子。注意，w^\widehat{w}w 在这里被看作三维空间的一个向量而不是一个超复数。
向量的旋转：
a. 将向量“四元化”：x~=0+x→\tilde{x}=0+\overrightarrow{x}x~=0+x；
b. 对向量进行绕轴 w^\widehat{w}w 旋转 θ\thetaθ 角，得到新向量 x~′=q~x~q~∗\tilde{x}^{'}=\tilde{q}\tilde{x}\tilde{q}^{*}x~′=q~x~q~∗.
等价的旋转矩阵：
对于四元数 q=q0+q1i+q2j+q3kq=q_0+q_1i+q_2j+q_3kq=q0+q1i+q2j+q3k
将 x~′=q~x~q~∗\tilde{x}^{'}=\tilde{q}\tilde{x}\tilde{q}^{*}x~′=q~x~q~∗ 展开，合并同类项得
R=[2(q02+q12)−12(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)2(q02+q22)−12(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)2(q02+q32)−1]R = \left [ \begin{matrix} 2(q_0^2+q_1^2)-1 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2) \\ 2(q_1q_2+q_0q_3) & 2(q_0^2+q_2^2)-1 & 2(q_2q_3-q_0q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_0q_1+q_2q_3) & 2(q_0^2+q_3^2)-1 \\ \end{matrix} \right ] R=⎣⎡2(q02+q12)−12(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)2(q02+q22)−12(q0q1+q2q3)2(q1q3+q0q2)2(q2q3−q0q1)2(q02+q32)−1⎦⎤
以上是左手系下的表示，关于左/右手系的区分：左右手比“枪”的形状，食指指向对手（You），代表 Y 轴，大拇指是用来瞄准的（x 像不像准心），因此是 X 轴，剩下的中指代表 Z 轴。此时，左手对应的坐标系就是左手系，右手对应右手系。
右手系表达：
R=[1−2(q22+q32)2(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)1−2(q12+q32)2(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)1−2(q12+q22)]R = \left [ \begin{matrix} 1-2(q_2^2+q_3^2) & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2) \\ 2(q_1q_2+q_0q_3) & 1-2(q_1^2+q_3^2) & 2(q_2q_3-q_0q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_0q_1+q_2q_3) & 1-2(q_1^2+q_2^2) \\ \end{matrix} \right ] R=⎣⎡1−2(q22+q32)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q12+q32)2(q0q1+q2q3)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q12+q22)⎦⎤
后面的 code 用的是右手系，书本上讲的左手系，在其他地方也有用途，注意区分。

四元数的微分形式

https://blog.csdn.net/qq_39554681/article/details/88909564

姿态解算

主要结合一个四轴飞控项目进行理解。
首先说明一下导航坐标系 n 的定义： x/y/z 轴依次指向东、北、天，设航偏角为 ψ\psiψ （注意：顺时针为正，习惯上北偏东为正），俯仰角为 θ\thetaθ，横滚角为 γ\gammaγ ，机体坐标系 b 与 n 的关系为
Tnb=[cγcψ+sγsψsθ−cγsψ+sγcψsθ−sγcθsψcθcψcθsθsγcψ−cγsψsθ−sγsψ−cγcψsθcγcθ]T^b_n=\left [ \begin{matrix} c\gamma{c}\psi+s\gamma{s}\psi{s}\theta & -c\gamma{s}\psi+s\gamma{c}\psi{s}\theta & -s\gamma{c}\theta \\ {s}\psi{c}\theta & {c}\psi{c}\theta & {s}\theta \\ s\gamma{c}\psi-c\gamma{s}\psi{s}\theta & -s\gamma{s}\psi-c\gamma{c}\psi{s}\theta & c\gamma{c}\theta \\ \end{matrix} \right ] Tnb=⎣⎡cγcψ+sγsψsθsψcθsγcψ−cγsψsθ−cγsψ+sγcψsθcψcθ−sγsψ−cγcψsθ−sγcθsθcγcθ⎦⎤
变量定义：

 volatile struct V{float x;float y;float z;} Gravity, Acc, Gyro, AccGravity;static struct V GyroIntegError = {0};static float KpDef = 0.8f;static float KiDef = 0.0003f;static Quaternion NumQ = {1, 0, 0, 0};float q0_t, q1_t, q2_t, q3_t;// float NormAcc;float NormQuat;float HalfTime = dt * 0.5f;

重力加速度归一化为单位向量；

 // 加速度归一化NormQuat = Q_rsqrt(squa(MPU6050.accX) + squa(MPU6050.accY) + squa(MPU6050.accZ));// Q_rsqrt:快速计算 1/Sqrt(x)Acc.x = pMpu->accX * NormQuat;Acc.y = pMpu->accY * NormQuat;Acc.z = pMpu->accZ * NormQuat;

提取四元数等效旋转矩阵中的重力分量，就是通过旋转矩阵将重力（设为 1，步骤 1 中加速度归一化做铺垫）转换至机体坐标系下；

 // 提取等效旋转矩阵中的重力分量Gravity.x = 2 * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);Gravity.y = 2 * (NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);Gravity.z = 1 - 2 * (NumQ.q1 * NumQ.q1 + NumQ.q2 * NumQ.q2);

向量叉积得出姿态误差，Acc 是通过加速度计测量得到的，AccGravity 是通过陀螺积分计算而来，本质上代表的是一个量，叉积仍是在机体坐标系上的，而且其大小与陀螺积分误差成正比，用于纠正陀螺，这里体现了传感器数据的融合；

 // 向量叉乘得出的值AccGravity.x = (Acc.y * Gravity.z - Acc.z * Gravity.y);AccGravity.y = (Acc.z * Gravity.x - Acc.x * Gravity.z);AccGravity.z = (Acc.x * Gravity.y - Acc.y * Gravity.x);

进行补偿；

 // 再做加速度积分补偿角速度的补偿值GyroIntegError.x += AccGravity.x * KiDef;GyroIntegError.y += AccGravity.y * KiDef;GyroIntegError.z += AccGravity.z * KiDef;// 角速度融合加速度积分补偿值Gyro.x = pMpu->gyroX * Gyro_Gr + KpDef * AccGravity.x + GyroIntegError.x; // 弧度制Gyro.y = pMpu->gyroY * Gyro_Gr + KpDef * AccGravity.y + GyroIntegError.y;Gyro.z = pMpu->gyroZ * Gyro_Gr + KpDef * AccGravity.z + GyroIntegError.z;

一阶 Runge-Kutta 更新四元数；

 // 一阶龙格库塔法, 更新四元数q0_t = (-NumQ.q1 * Gyro.x - NumQ.q2 * Gyro.y - NumQ.q3 * Gyro.z) * HalfTime;q1_t = (NumQ.q0 * Gyro.x - NumQ.q3 * Gyro.y + NumQ.q2 * Gyro.z) * HalfTime;q2_t = (NumQ.q3 * Gyro.x + NumQ.q0 * Gyro.y - NumQ.q1 * Gyro.z) * HalfTime;q3_t = (-NumQ.q2 * Gyro.x + NumQ.q1 * Gyro.y + NumQ.q0 * Gyro.z) * HalfTime;NumQ.q0 += q0_t;NumQ.q1 += q1_t;NumQ.q2 += q2_t;NumQ.q3 += q3_t;

四元数归一化，因为计算机中的浮点运算存在误差，计算过程中四元数会失去规范化特性（表征旋转我们使用单位四元数或者规范化的四元数）。

 // 四元数归一化NormQuat = Q_rsqrt(squa(NumQ.q0) + squa(NumQ.q1) + squa(NumQ.q2) + squa(NumQ.q3));NumQ.q0 *= NormQuat;NumQ.q1 *= NormQuat;NumQ.q2 *= NormQuat;NumQ.q3 *= NormQuat;