Q
→
=
c
o
s
θ
2
+
u
→
s
i
n
θ
2
\overrightarrow{Q} = cos \cfrac{\theta}{2} + \overrightarrow{u} sin \cfrac{\theta}{2}
Q=cos2θ+usin2θ
1.2 基本运算
1.2.1 矢量加减
P
→
±
Q
→
=
(
p
0
+
p
1
⋅
i
+
p
2
⋅
j
+
p
3
⋅
k
)
±
(
q
0
+
q
1
⋅
i
+
q
2
⋅
j
+
q
3
⋅
k
)
=
(
p
0
±
q
0
)
+
(
p
1
±
q
1
)
⋅
i
+
(
p
2
±
q
2
)
⋅
j
+
(
p
3
±
q
3
)
⋅
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 k
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
1.2.2 标量乘法
a
⋅
Q
→
=
a
⋅
q
0
+
a
⋅
q
1
⋅
i
+
a
⋅
q
2
⋅
j
+
a
⋅
q
3
⋅
k
a \cdot \overrightarrow{Q} = a \cdot q_0 + a \cdot q_1 \cdot i + a \cdot q_2 \cdot j + a \cdot q_3 \cdot k
a⋅Q=a⋅q0+a⋅q1⋅i+a⋅q2⋅j+a⋅q3⋅k
1.3 矢量点叉乘
1.3.1 矢量点乘
P
→
⋅
Q
→
=
∣
P
∣
∣
Q
∣
c
o
s
θ
P
Q
\overrightarrow{P} \cdot \overrightarrow{Q} = \rvert P \rvert \rvert Q \rvert cos \theta_{PQ}
P⋅Q=∣P∣∣Q∣cosθPQ
P
→
⋅
Q
→
=
(
p
0
+
p
1
⋅
i
+
p
2
⋅
j
+
p
3
⋅
k
)
⋅
(
q
0
+
q
1
⋅
i
+
q
2
⋅
j
+
q
3
⋅
k
)
=
p
0
q
0
+
p
1
q
1
+
p
2
q
2
+
p
3
q
3
\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_3
P⋅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
∣
s
i
n
θ
P
Q
\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} )_z
P×Q=(P×Q)x+(P×Q)y+(P×Q)z
(
P
→
×
Q
→
)
x
=
P
y
Q
z
−
P
z
Q
y
(\overrightarrow{P} \times \overrightarrow{Q} )_x = P_yQ_z - P_zQ_y
(P×Q)x=PyQz−PzQy
(
P
→
×
Q
→
)
y
=
P
z
Q
x
−
P
x
Q
z
(\overrightarrow{P} \times \overrightarrow{Q} )_y = P_zQ_x - P_xQ_z
(P×Q)y=PzQx−PxQz
(
P
→
×
Q
→
)
z
=
P
x
Q
y
−
P
y
Q
x
(\overrightarrow{P} \times \overrightarrow{Q} )_z = P_xQ_y - P_yQ_x
(P×Q)z=PxQy−PyQx
P
→
×
Q
→
=
(
p
0
+
p
1
⋅
i
+
p
2
⋅
j
+
p
3
⋅
k
)
×
(
q
0
+
q
1
⋅
i
+
q
2
⋅
j
+
q
3
⋅
k
)
=
(
p
0
q
0
−
p
1
q
1
−
p
2
q
2
−
p
3
q
3
)
+
(
p
0
q
1
+
p
1
q
0
+
p
2
q
3
−
p
3
q
2
)
⋅
i
+
(
p
0
q
2
+
p
2
q
0
+
p
3
q
1
−
p
1
q
3
)
⋅
j
+
(
p
0
q
3
+
p
3
q
0
+
p
1
q
2
−
p
2
q
1
)
⋅
k
=
r
0
+
r
1
⋅
i
+
r
2
⋅
j
+
r
3
⋅
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 k
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
换成矩阵表达式(便于后续计算机计算):
R
=
M
(
P
)
Q
=
M
′
(
Q
)
P
R = M(P)Q = M\rq(Q)P
R=M(P)Q=M′(Q)P
[
r
0
r
1
r
2
r
3
]
=
M
(
P
)
Q
=
[
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
]
[
q
0
q
1
q
2
q
3
]
\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⎦⎤
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 rotation
result->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 frame
quaternionRotateVectorInv(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 frame
quaternionRotateVector(v, v, &orientation);
}
【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