BetaFlight深入传感设计之六：四元数计算方法
BetaFlight深入传感设计之六：四元数计算方法
- 1. 四元数理论
- 1.1 定义
- 1.2 基本运算
- 1.2.1 矢量加减
- 1.2.2 标量乘法
- 1.3 矢量点叉乘
- 1.3.1 矢量点乘
- 1.3.2 矢量叉乘
- 1.4 除法求逆
- 2. 程序四元素
- 2.1 四元数定义
- 2.2 四元数初始化
- 2.2.1 单位标量初始化
- 2.2.2 矢量初始化
- 2.3 表达转换
- 2.3.1 复数式转三角式
- 2.3.2 三角式转复数式
- 2.4 基本运算
- 2.4.1 矢量加法
- 2.4.2 标量乘法
- 2.5 矢量点叉乘
- 2.5.1 矢量点乘
- 2.5.2 矢量叉乘
- 2.6 零标量四元数旋转
- 2.6.1 CW方向旋转
- 2.6.2 CCW方向旋转
- 2.7 其他运算
- 2.7.1 四元素模平方
- 2.7.2 四元素求厄
- 2.7.3 四元素归一化
- 3. 机体坐标系和地球坐标系转换
- 3.1 机体坐标系向地球坐标系旋转
- 3.2 地球坐标系向机体坐标系旋转
- 4. 总结
- 5. 补充资料
- 5.1 ENU Coordinates
- 5.2 NED Coordinates
- 5.3 Earth-Centered Earth-Fixed Coordinates
- 5.4 Azimuth-Elevation-Range Coordinates
- 6. 参考资料
BetaFlight和iNavFlight都是出自CleanFlight分支，两者的侧重点不太一样。从四元数应用的角度来看，iNav不仅代码比较完整，且对四元数操作进行了相应的数学逻辑封装。
我们这里针对四元数的应用，结合代码进行一次整理和理解。
1. 四元数理论
1.1 定义
- 矢量式
Q→=q0+q→\overrightarrow{Q} = q_0 + \overrightarrow{q}Q=q0+q
- 复数式
Q→=q0+q1⋅i+q2⋅j+q3⋅k\overrightarrow{Q} = q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot kQ=q0+q1⋅i+q2⋅j+q3⋅k
注：高中课本的复数就是超复数的一种特殊情况，其中q2=q3=0q_2 = q_3 = 0q2=q3=0。
- 三角式
Q→=cosθ2+u→sinθ2\overrightarrow{Q} = cos \cfrac{\theta}{2} + \overrightarrow{u} sin \cfrac{\theta}{2}Q=cos2θ+usin2θ
1.2 基本运算
1.2.1 矢量加减
P→±Q→=(p0+p1⋅i+p2⋅j+p3⋅k)±(q0+q1⋅i+q2⋅j+q3⋅k)=(p0±q0)+(p1±q1)⋅i+(p2±q2)⋅j+(p3±q3)⋅k\overrightarrow{P} \pm \overrightarrow{Q} = (p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k) \pm (q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot k) = (p_0 \pm q_0) + (p_1 \pm q_1) \cdot i + (p_2 \pm q_2) \cdot j + (p_3 \pm q_3) \cdot kP±Q=(p0+p1⋅i+p2⋅j+p3⋅k)±(q0+q1⋅i+q2⋅j+q3⋅k)=(p0±q0)+(p1±q1)⋅i+(p2±q2)⋅j+(p3±q3)⋅k
1.2.2 标量乘法
a⋅Q→=a⋅q0+a⋅q1⋅i+a⋅q2⋅j+a⋅q3⋅ka \cdot \overrightarrow{Q} = a \cdot q_0 + a \cdot q_1 \cdot i + a \cdot q_2 \cdot j + a \cdot q_3 \cdot ka⋅Q=a⋅q0+a⋅q1⋅i+a⋅q2⋅j+a⋅q3⋅k
1.3 矢量点叉乘
1.3.1 矢量点乘
P→⋅Q→=∣P∣∣Q∣cosθPQ\overrightarrow{P} \cdot \overrightarrow{Q} = \rvert P \rvert \rvert Q \rvert cos \theta_{PQ}P⋅Q=∣P∣∣Q∣cosθPQ
P→⋅Q→=(p0+p1⋅i+p2⋅j+p3⋅k)⋅(q0+q1⋅i+q2⋅j+q3⋅k)=p0q0+p1q1+p2q2+p3q3\overrightarrow{P} \cdot \overrightarrow{Q} = (p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k) \cdot (q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot k) = p_0 q_0 + p_1 q_1 + p_2 q_2 + p_3 q_3P⋅Q=(p0+p1⋅i+p2⋅j+p3⋅k)⋅(q0+q1⋅i+q2⋅j+q3⋅k)=p0q0+p1q1+p2q2+p3q3
1.3.2 矢量叉乘
P→×Q→=−Q→×P→\overrightarrow{P} \times \overrightarrow{Q} = -\overrightarrow{Q} \times \overrightarrow{P}P×Q=−Q×P
∣P→×Q→∣=∣P∣∣Q∣sinθPQ\rvert \overrightarrow{P} \times \overrightarrow{Q} \rvert =\rvert P \rvert \rvert Q \rvert sin \theta_{PQ}∣P×Q∣=∣P∣∣Q∣sinθPQ
P→×Q→=(P→×Q→)x+(P→×Q→)y+(P→×Q→)z\overrightarrow{P} \times \overrightarrow{Q} = (\overrightarrow{P} \times \overrightarrow{Q} )_x + (\overrightarrow{P} \times \overrightarrow{Q} )_y + (\overrightarrow{P} \times \overrightarrow{Q} )_zP×Q=(P×Q)x+(P×Q)y+(P×Q)z
(P→×Q→)x=PyQz−PzQy(\overrightarrow{P} \times \overrightarrow{Q} )_x = P_yQ_z - P_zQ_y(P×Q)x=PyQz−PzQy
(P→×Q→)y=PzQx−PxQz(\overrightarrow{P} \times \overrightarrow{Q} )_y = P_zQ_x - P_xQ_z(P×Q)y=PzQx−PxQz
(P→×Q→)z=PxQy−PyQx(\overrightarrow{P} \times \overrightarrow{Q} )_z = P_xQ_y - P_yQ_x(P×Q)z=PxQy−PyQx
P→×Q→=(p0+p1⋅i+p2⋅j+p3⋅k)×(q0+q1⋅i+q2⋅j+q3⋅k)=(p0q0−p1q1−p2q2−p3q3)+(p0q1+p1q0+p2q3−p3q2)⋅i+(p0q2+p2q0+p3q1−p1q3)⋅j+(p0q3+p3q0+p1q2−p2q1)⋅k=r0+r1⋅i+r2⋅j+r3⋅k\overrightarrow{P} \times \overrightarrow{Q} = (p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k) \times (q_0 + q_1 \cdot i + q_2 \cdot j + q_3 \cdot k) = (p_0q_0 - p_1q_1 - p_2q_2 - p_3q_3) + (p_0q_1 + p_1q_0 + p_2q_3 - p_3q_2) \cdot i + (p_0q_2 + p_2q_0 + p_3q_1 -p_1q_3) \cdot j +(p_0q_3 + p_3q_0 +p_1q_2 - p_2q_1) \cdot k = r_0 + r_1 \cdot i + r_2 \cdot j + r_3 \cdot kP×Q=(p0+p1⋅i+p2⋅j+p3⋅k)×(q0+q1⋅i+q2⋅j+q3⋅k)=(p0q0−p1q1−p2q2−p3q3)+(p0q1+p1q0+p2q3−p3q2)⋅i+(p0q2+p2q0+p3q1−p1q3)⋅j+(p0q3+p3q0+p1q2−p2q1)⋅k=r0+r1⋅i+r2⋅j+r3⋅k
换成矩阵表达式(便于后续计算机计算)：
R=M(P)Q=M′(Q)PR = M(P)Q = M\rq(Q)PR=M(P)Q=M′(Q)P
[r0r1r2r3]=M(P)Q=[p0−p1−p2−p3p1p0−p3p2p2p3p0−p1p3−p2p1p0][q0q1q2q3]\begin{bmatrix} r_0 \\ r_1 \\ r_2 \\ r_3 \end{bmatrix} = M(P)Q =\begin{bmatrix} p_0 & -p_1 & -p_2 & -p_3\\ p_1& p_0 & -p_3 & p_2 \\ p_2 & p_3 & p_0 & -p_1\\ p_3 & -p_2 & p_1 & p_0 \end{bmatrix}\begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}⎣⎡r0r1r2r3⎦⎤=M(P)Q=⎣⎡p0p1p2p3−p1p0p3−p2−p2−p3p0p1−p3p2−p1p0⎦⎤⎣⎡q0q1q2q3⎦⎤
[r0r1r2r3]=M′(Q)P=[q0−q1−q2−q3q1q0q3−q2q2−q3q0q1q3q2−q1q0][p0p1p2p3]\begin{bmatrix} r_0 \\ r_1 \\ r_2 \\ r_3 \end{bmatrix} = M\rq(Q)P = \begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3\\ q_1& q_0 & q_3 & -q_2 \\ q_2 & -q_3 & q_0 & q_1\\ q_3 & q_2 & -q_1 & q_0 \end{bmatrix}\begin{bmatrix} p_0 \\ p_1 \\ p_2 \\ p_3 \end{bmatrix}⎣⎡r0r1r2r3⎦⎤=M′(Q)P=⎣⎡q0q1q2q3−q1q0−q3q2−q2q3q0−q1−q3−q2q1q0⎦⎤⎣⎡p0p1p2p3⎦⎤
1.4 除法求逆
P→⊗R→=1\overrightarrow{P} \otimes \overrightarrow{R} = 1P⊗R=1
将记作：R→=P−1→=P∗→\overrightarrow{R} = \overrightarrow{P^{-1}}=\overrightarrow{P^*}R=P−1=P∗
P→⊗P∗→=（p0+p1⋅i+p2⋅j+p3⋅k）⊗（p0−p1⋅i−p2⋅j−p3⋅k）=q02+q12+q22+q32=∥P∥\overrightarrow{P} \otimes \overrightarrow{P^*} = （p_0 + p_1 \cdot i + p_2 \cdot j + p_3 \cdot k） \otimes （p_0 - p_1 \cdot i - p_2 \cdot j - p_3 \cdot k）= q_0^2 + q_1^2 + q_2^2 + q_3^2 = \rVert P \rVertP⊗P∗=（p0+p1⋅i+p2⋅j+p3⋅k）⊗（p0−p1⋅i−p2⋅j−p3⋅k）=q02+q12+q22+q32=∥P∥
所以: P−1→=P∗→∥P∥\overrightarrow{P^{-1}} =\cfrac{\overrightarrow{P^*}}{ \rVert P \rVert}P−1=∥P∥P∗
2. 程序四元素
2.1 四元数定义
typedef struct {float q0, q1, q2, q3;
} fpQuaternion_t;
2.2 四元数初始化
2.2.1 单位标量初始化
static inline fpQuaternion_t * quaternionInitUnit(fpQuaternion_t * result)
{result->q0 = 1.0f;result->q1 = 0.0f;result->q2 = 0.0f;result->q3 = 0.0f;return result;
}
2.2.2 矢量初始化
static inline fpQuaternion_t * quaternionInitFromVector(fpQuaternion_t * result, const fpVector3_t * v)
{result->q0 = 0.0f;result->q1 = v->x;result->q2 = v->y;result->q3 = v->z;return result;
}
2.3 表达转换
注：进行转换的输入参数必须归一化。
2.3.1 复数式转三角式
static inline void quaternionToAxisAngle(fpAxisAngle_t * result, const fpQuaternion_t * q)
{fpAxisAngle_t a = {.axis = {{1.0f, 0.0f, 0.0f}}, .angle = 0};a.angle = 2.0f * acos_approx(constrainf(q->q0, -1.0f, 1.0f));if (a.angle > M_PIf) {a.angle -= 2.0f * M_PIf;}const float sinVal = sqrt(1.0f - q->q0 * q->q0);// Axis is only valid when rotation is large enough sin(0.0057 deg) = 0.0001if (sinVal > 1e-4f) {a.axis.x = q->q1 / sinVal;a.axis.y = q->q2 / sinVal;a.axis.z = q->q3 / sinVal;} else {a.angle = 0;}*result = a;
}
2.3.2 三角式转复数式
static inline fpQuaternion_t * axisAngleToQuaternion(fpQuaternion_t * result, const fpAxisAngle_t * a)
{fpQuaternion_t q;const float s = sin_approx(a->angle / 2.0f);q.q0 = cos_approx(a->angle / 2.0f);q.q1 = -a->axis.x * s;q.q2 = -a->axis.y * s;q.q3 = -a->axis.z * s;*result = q;return result;
}
问题1：为什么q1q2q3q_1 q_2 q_3q1q2q3是负值??? 按照定义的角度看应该是正的呀，有知道的朋友请评论批注，谢谢！
2.4 基本运算
2.4.1 矢量加法
static inline fpQuaternion_t * quaternionAdd(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)
{fpQuaternion_t p;p.q0 = a->q0 + b->q0;p.q1 = a->q1 + b->q1;p.q2 = a->q2 + b->q2;p.q3 = a->q3 + b->q3;*result = p;return result;
}
2.4.2 标量乘法
static inline fpQuaternion_t * quaternionScale(fpQuaternion_t * result, const fpQuaternion_t * a, const float b)
{fpQuaternion_t p;p.q0 = a->q0 * b;p.q1 = a->q1 * b;p.q2 = a->q2 * b;p.q3 = a->q3 * b;*result = p;return result;
}
2.5 矢量点叉乘
2.5.1 矢量点乘
略.
注：貌似点乘在实际飞控应用中用的不多，代码中未见定义。
2.5.2 矢量叉乘
根据叉乘定义和右手螺旋法则，我们知道叉乘结果是两个矢量模与sinθsin \thetasinθ乘积，方向垂直于两个叉乘矢量。
- 当θ=0\theta = 0θ=0的时候， 叉乘结果为零；
- 当θ=90\theta = 90θ=90的时候，叉乘结果为两个矢量模，并垂直于两个叉乘矢量；
所以，在实际应用中，归一化的矢量叉乘结果往往用于表达两个矢量的相关性。当两个物理量表达的是同一个物理特性时，叉乘结果的值越趋于零，表示误差越小。
static inline fpQuaternion_t * quaternionMultiply(fpQuaternion_t * result, const fpQuaternion_t * a, const fpQuaternion_t * b)
{fpQuaternion_t p;p.q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;p.q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;p.q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;p.q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;*result = p;return result;
}
2.6 零标量四元数旋转
2.6.1 CW方向旋转
static inline fpVector3_t * quaternionRotateVector(fpVector3_t * result, const fpVector3_t * vect, const fpQuaternion_t * ref)
{fpQuaternion_t vectQuat, refConj;vectQuat.q0 = 0;vectQuat.q1 = vect->x;vectQuat.q2 = vect->y;vectQuat.q3 = vect->z;quaternionConjugate(&refConj, ref);quaternionMultiply(&vectQuat, &refConj, &vectQuat);quaternionMultiply(&vectQuat, &vectQuat, ref);result->x = vectQuat.q1;result->y = vectQuat.q2;result->z = vectQuat.q3;return result;
}
2.6.2 CCW方向旋转
static inline fpVector3_t * quaternionRotateVectorInv(fpVector3_t * result, const fpVector3_t * vect, const fpQuaternion_t * ref)
{fpQuaternion_t vectQuat, refConj;vectQuat.q0 = 0;vectQuat.q1 = vect->x;vectQuat.q2 = vect->y;vectQuat.q3 = vect->z;quaternionConjugate(&refConj, ref);quaternionMultiply(&vectQuat, ref, &vectQuat);quaternionMultiply(&vectQuat, &vectQuat, &refConj);result->x = vectQuat.q1;result->y = vectQuat.q2;result->z = vectQuat.q3;return result;
}
2.7 其他运算
2.7.1 四元素模平方
static inline float quaternionNormSqared(const fpQuaternion_t * q)
{return sq(q->q0) + sq(q->q1) + sq(q->q2) + sq(q->q3);
}
2.7.2 四元素求厄
static inline fpQuaternion_t * quaternionConjugate(fpQuaternion_t * result, const fpQuaternion_t * q)
{result->q0 = q->q0;result->q1 = -q->q1;result->q2 = -q->q2;result->q3 = -q->q3;return result;
}
2.7.3 四元素归一化
static inline fpQuaternion_t * quaternionNormalize(fpQuaternion_t * result, const fpQuaternion_t * q)
{float mod = fast_fsqrtf(quaternionNormSqared(q));if (mod < 1e-6f) {// Length is too small - re-initialize to zero rotationresult->q0 = 1;result->q1 = 0;result->q2 = 0;result->q3 = 0;}else {result->q0 = q->q0 / mod;result->q1 = q->q1 / mod;result->q2 = q->q2 / mod;result->q3 = q->q3 / mod;}return result;
}
3. 机体坐标系和地球坐标系转换
问题2：这里为什么NED (sensor frame) 和 NEU (navigation)是这个关系，逻辑在哪里？？？
3.1 机体坐标系向地球坐标系旋转
void imuTransformVectorBodyToEarth(fpVector3_t * v)
{// From body frame to earth framequaternionRotateVectorInv(v, v, &orientation);// HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)v->y = -v->y;
}
3.2 地球坐标系向机体坐标系旋转
void imuTransformVectorEarthToBody(fpVector3_t * v)
{// HACK: This is needed to correctly transform from NED (sensor frame) to NEU (navigation)v->y = -v->y;// From earth frame to body framequaternionRotateVector(v, v, &orientation);
}
4. 总结
四元数在四轴飞控方面关于姿态和导航方面的应用具有举足轻重的意义。尤其对PID的的误差计算带来了便捷，但是这个在物理概念上需要明确的符号推导和现实意义，才能在后续传感融合上得到长远的发展。
为此，整理了上述基本概念，但是截止目前为止，还存在以下两个问题：
【1】三角式转复数式的时候，什么q1q2q3q_1 q_2 q_3q1q2q3是负值??? 按照定义的角度看应该是正的，如何理解？？？
==》这里搜索了iNav的代码，没有看到axisAngleToQuaternion函数在代码中使用。因此即使有问题，也没有太多关系。
【2】NED (sensor frame) 和 NEU (navigation)的指向是如何定义的？
==》请参考补充资料
如果有同学知道原因，也请多多指点，谢谢先！
5. 补充资料
5.1 ENU Coordinates
5.2 NED Coordinates
5.3 Earth-Centered Earth-Fixed Coordinates
5.4 Azimuth-Elevation-Range Coordinates
6. 参考资料
【1】Comparison of 3-D Coordinate Systems
【2】About Aerospace Coordinate Systems
【3】Flight controller is different from the airframe coordinate system? #11903
BetaFlight深入传感设计之六：四元数计算方法相关推荐
- BetaFlight深入传感设计：传感模块设计框架
BetaFlight深入传感设计:传感模块设计框架 1. BetaFlight传感器简介 2. BetaFlight传感器嵌入式软件设计 3. HwPreInit/HwInit阶段 4. HwIo阶段 ...
- BetaFlight深入传感设计之九：传感坐标系/机体坐标系/导航坐标系/经纬度坐标系
BetaFlight深入传感设计之九:传感坐标系/机体坐标系/导航坐标系/经纬度坐标系 1. 问题症结 2. 入手分析 2.1 传感坐标系 2.2 机体坐标系 2.3 导航坐标系 2.4 经纬坐标系 ...
- BetaFlight深入传感设计之八：坐标系
BetaFlight深入传感设计之八:坐标系 1. 坐标系统应用 1.1 Geographic Coordinate System: LLH, Longitude-Latitude-Height 1. ...
- BetaFlight深入传感设计之十：传感器物理特性方向对齐
BetaFlight深入传感设计之十:传感器物理特性方向对齐 1. 对齐定义 2. 常见对齐方式 3. 自定义对齐方式 4. 总结 5. 参考资料 6. 补充:gyro + mag对齐方式 AHRS( ...
- BetaFlight深入传感设计之五：MahonyAHRS 方向余弦矩阵理论
BetaFlight深入传感设计之五:MahonyAHRS & 方向余弦矩阵理论 1. 基础预备知识 1.1 机体坐标系 1.2 欧拉角 1.2.1 概念解释 1.2.2 动态概念 1.2.3 ...
- BetaFlight深入传感设计之三：IMU传感模块
BetaFlight深入传感设计之三:IMU传感模块 1. HwPreInit/HwInit阶段 1.1 [业务HwPreInit]gyroPreInit 1.2 [业务HwInit]gyroInit ...
- BetaFlight深入传感设计之四：GPS传感模块
BetaFlight深入传感设计之四:GPS传感模块 1. HwPreInit/HwInit阶段 1.1 [业务HwPreInit]gpsInit 1.2 [业务HwInit]gpsInitHardw ...
- BetaFlight深入传感设计之七：GPSBaro高度数据融合
BetaFlight深入传感设计之七:GPS&Baro高度数据融合 1. 现象 2. 分析 2.1 程序逻辑 2.2 GPS精度 2.3 数值分析 3. 总结 传感器数据融合最主要的目的是为了 ...
- COM载板设计之六：VGA和音频AC97/HDA接口
2.11 VGA 2.11.1 信号定义 所有类型的COM Express模块都应当定义一个模拟VGA RGB接口,接口由3个模拟彩色信号(Red.Green.Blue)组成,数字水平和垂直同步信号, ...
最新文章
- Linux Watchdog 机制
- nginx.pid failed (2: The system cannot find the file specified
- 从零开始的AI·朴素贝叶斯？拿来吧你（附实例代码）
- 机器人学习--栅格地图（occupancy grid map）构建
- linux使用VNC服务轻松远程安装oracle
- Python多线程豆瓣影评API接口爬虫
- jboss7 关闭日志打印_使用自定义日志记录处理程序在JBoss AS 7中跟踪SQL语句
- 简单的Spring Memcached – Spring缓存抽象和Memcached
- python 函数 过程_python之函数篇
- 处理器指令编码可重定义的方法_RISC-V学习笔记1 《基于FPGA与RISC-V的嵌入式系统设计》第3章 RISC-V指令集...
- 年终感想——财务自由的程序员，你见过吗？
- linux服务器视频转换,将动画gif转换为linux服务器上的视频,同时保留帧速率
- 学习记录1——vissim4.3安装和vissim4.3时间修改工具使用
- 有了HTML5，Flash还能走多远？
- Xshell、MobaXterm、Secure CRT等工具用法
- halcon学习笔记4-字符识别（包括汉字识别）
- VDA6.3认证辅导,VDA6.3认证以保证汽车零部件生产过程中的质量保证
- 循环神经网络--RNN GRU LSTM 对比分析
- 指纹传感器安全性究竟有多少？
- RocketMq之消费方式