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

四元数

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

三维旋转表示

1. 单位四元数 $\tilde{q}=cos(\theta /2)+\hat{w}sin(\theta /2)$ 可以表示在三维空间中绕（单位向量）$\hat{w}$ 旋转 $\theta$ 角的旋转算子。注意，$\hat{w}$ 在这里被看作三维空间的一个向量而不是一个超复数。
2. 向量的旋转：
   a. 将向量“四元化”：$\tilde{x}=0+\overrightarrow{x}$；
   b. 对向量进行绕轴 $\hat{w}$ 旋转 $\theta$ 角，得到新向量 ${\tilde{x}}^{{}^{\prime }}=\tilde{q}\tilde{x}{\tilde{q}}^{\ast }$.
3. 等价的旋转矩阵：
   对于四元数 $q=q_0+q_{1}i+q_{2}j+q_{3}k$
   将 ${\tilde{x}}^{{}^{\prime }}=\tilde{q}\tilde{x}{\tilde{q}}^{\ast }$ 展开，合并同类项得
   $$R=\left[\begin{array}{ccc}2(q_0^2+q_1^2)-1& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 2(q_0^2+q_2^2)-1& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& 2(q_0^2+q_3^2)-1\end{array}\right]$$
   以上是左手系下的表示，关于左/右手系的区分：左右手比“枪”的形状，食指指向对手（You），代表 Y 轴，大拇指是用来瞄准的（x 像不像准心），因此是 X 轴，剩下的中指代表 Z 轴。此时，左手对应的坐标系就是左手系，右手对应右手系。
   右手系表达：
   $$R=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& 1-2(q_1^2+q_2^2)\end{array}\right]$$
   后面的 code 用的是右手系，书本上讲的左手系，在其他地方也有用途，注意区分。

四元数的微分形式

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

姿态解算

主要结合一个四轴飞控项目进行理解。
首先说明一下导航坐标系 n 的定义： x/y/z 轴依次指向东、北、天，设航偏角为 $\psi$ （注意：顺时针为正，习惯上北偏东为正），俯仰角为 $\theta$，横滚角为 $\gamma$ ，机体坐标系 b 与 n 的关系为
$$T_n^b=\left[\begin{array}{ccc}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{array}\right]$$
变量定义：

	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;

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

	// 加速度归一化
	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;

2. 提取四元数等效旋转矩阵中的重力分量，就是通过旋转矩阵将重力（设为 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);

3. 向量叉积得出姿态误差，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);

4. 进行补偿；

	// 再做加速度积分补偿角速度的补偿值
	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;

5. 一阶 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;

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

	// 四元数归一化
	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;