四元数

首页 > 代码库 > 四元数

2024-08-15 10:20:46 217人阅读

旋转的表示方式

旋转矩阵

$R_{A}^{B}$ 表示从A坐标系到B坐标系的旋转矩阵，那么 $r^{B} = R_{A}^{B} r^{A}$ 。

要注意的是坐标系的旋转和矢量的旋转的区别。矢量的旋转是在同一坐标系内， $r^{'} = R_{v} r$ 。

$R_{A}^{B}$ 是正交矩阵， $R_{B}^{A}$ 为 $R_{A}^{B}$ 的转置。det( $R_{A}^{B}$ )=1

$R_{A}^{B}$ 的行向量是A坐标系的基矢在B坐标系中的表示，列向量是B坐标系的基矢在A坐标系中的表示。

quaternion

根据欧拉转动定理，任意多次的旋转可等效为一次旋转。用四元数来表示，标量部分表示转过的角度，矢量部分表示转轴的方向。

q = (\begin{matrix} a \\ b \\ c \\ d \end{matrix}) = (\begin{matrix} \cos (0.5 θ) \\ v_{x} \sin (0.5 θ) \\ v_{y} \sin (0.5 θ) \\ v_{z} \sin (0.5 θ) \end{matrix})

θ

为转过的角度，

{(\begin{matrix} v_{x} & v_{y} & v_{z} \end{matrix})}^{T}

定义了转轴，为单位矢量。
四元数的模为1。

四元数的动态方程：

如果q的第一项是标量项
技术分享

如果q的最后一项是标量项
$\dot{q} = Ω q$

技术分享 $(\begin{matrix} a \\ b \\ c \\ d \end{matrix}) = (\begin{matrix} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{matrix})$

用加速度计和磁力计来计算欧拉角

由加速度计可以计算roll，pitch
技术分享

技术分享

也就是根据 $C_{b}^{n}$ 的第三行，第三行是重力加速度在b坐标系中的表示。
由磁力计可以计算yaw。设由n坐标系转过yaw后的坐标系为n‘。我们需要知道地磁场在n‘坐标系中的表示。而n‘到b坐标系是通过pitch，roll转动达到的，所以 $C_{b}^{n^{'}}$ 只要把 $C_{b}^{n}$ 中的 $ψ$ 设为0就行了。
技术分享

$H_{x}$ 等是地磁场在n‘坐标系中的表示， $h_{x}$ 等是在b坐标系中的表示。然后由 $H_{x}$ , $H_{y}$ 求反正切，再加上磁偏角修正，就得到了yaw。

axis-angle

根据欧拉转动定理，刚体在三维空间的任意次序转动可等效于一次定轴转动。于是有了axis-angle表示。转动定轴又称Euler axis.
技术分享

四元数下的矢量坐标变换（与秦永元的书也一致）

\begin{matrix} (1) & r^{b} = q^{*} r^{n} q \end{matrix}

其中

r

是标量部分为0的四元数，

q

是将n系转至b系的四元数。1

四元数->方向余弦矩阵(dcm)

四元数乘法：

(r_{1}, \vec{v_{1}}) \otimes (r_{2}, \vec{v_{2}}) = (\begin{matrix} r_{1} r_{2} - \vec{v_{1}} \cdot \vec{v_{2}} \\ r_{1} \vec{v_{2}} + r_{2} \vec{v_{1}} + \vec{v_{1}} \times \vec{v_{2}} \end{matrix})

四元数

q = [\begin{matrix} q_{0} & q_{1} & q_{2} & q_{3} \end{matrix}]

(

q_{0}

是标量部分)对应的dcm

R_{n}^{b} = (2 q_{0}^{2} - 1) I + 2 Q Q^{T} - 2 q_{0} [Q \times]

2
其中

Q

是四元数的矢量部分，

[Q \times]

是矢量的反对称矩阵，

[Q \times] = [\begin{matrix} 0 & - Q_{3} & Q_{2} \\ Q_{3} & 0 & - Q_{1} \\ - Q_{2} & Q_{1} & 0 \end{matrix}]

展开后为：

\begin{matrix} (2) & R_{n}^{b} = [\begin{matrix} 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_{0} q_{1} + q_{2} q_{3}) \\ 2 (q_{0} q_{2} + q_{1} q_{3}) & 2 (q_{2} q_{3} - q_{0} q_{1}) & 2 (q_{0}^{2} + q_{3}^{2}) - 1 \end{matrix}] \end{matrix}

dcm->四元数

由 $(2)$ 可得：

4 q_{0}^{2} - 1 = t r (R) 4 q_{0} q_{1} = R_{23} - R_{32} 4 q_{0} q_{2} = R_{31} - R_{13} 4 q_{0} q_{3} = R_{12} - R_{21}

于是：

q_{0} = \frac{\sqrt{R_{11} + R_{22} + R_{33} + 1}}{2} q_{1} = \frac{R_{23} - R_{32}}{4 q_{0}} q_{2} = \frac{R_{31} - R_{13}}{4 q_{0}} q_{3} = \frac{R_{12} - R_{21}}{4 q_{0}}

四元数->欧拉角

绕X轴旋转

R_{x} = [\begin{matrix} 1 & 0 & 0 \\ 0 & c_{θ} & s_{θ} \\ 0 & - s_{θ} & c_{θ} \end{matrix}]

绕Y轴旋转

R_{y} = [\begin{matrix} c_{θ} & 0 & - s_{θ} \\ 0 & 1 & 0 \\ s_{θ} & 0 & c_{θ} \end{matrix}]

绕Z轴旋转

R_{z} = [\begin{matrix} c_{θ} & s_{θ} & 0 \\ - s_{θ} & c_{θ} & 0 \\ 0 & 0 & 1 \end{matrix}]

s_{θ}

放置位置记忆方法：看如下序列

XYZXY

绕X轴旋转90度，将Y轴转到了Z轴。那么 $s_{θ}$ 对应了Y轴（矩阵第二行），- $s_{θ}$ 对应Z轴（第三行）。
同理，绕Y轴旋转90度，将Z轴转到了X轴。那么 $s_{θ}$ 对应了Z轴（第三行），- $s_{θ}$ 对应X轴（第一行）。
最后，绕Z轴旋转90度，将X轴转到了Y轴。那么 $s_{θ}$ 对应了X轴（第一行），- $s_{θ}$ 对应Y轴（第二行）。

R = R_{x} R_{y} R_{z} = [\begin{matrix} c_{y} c_{z} & c_{y} s_{z} & - s_{y} \\ s_{x} s_{y} c_{z} - c_{x} s_{z} & s_{x} s_{y} s_{z} + c_{x} c_{z} & s_{x} c_{y} \\ c_{x} s_{y} c_{z} + s_{x} s_{z} & c_{x} s_{y} s_{z} - s_{x} c_{z} & c_{x} c_{y} \end{matrix}]

tan (θ_{z}) = \frac{R_{12}}{R_{11}} sin (θ_{y}) = - R_{13} tan (θ_{x}) = \frac{R_{23}}{R_{33}}

若是小角转动，

\begin{matrix} (3) & R = [\begin{matrix} 1 & θ_{z} & - θ_{y} \\ - θ_{z} & 1 & θ_{x} \\ θ_{y} & - θ_{x} & 1 \end{matrix}] = I - Θ \end{matrix}

其中

Θ

为反对称矩阵。可见小角度转动与转动顺序无关（即只与绕xyz各轴转过的角度有关，如果不是所有3轴都出现呢？）。

R_{y} R_{x} R_{z} = [\begin{matrix} c_{y} c_{z} - s_{x} s_{y} s_{z} & c_{y} s_{z} + s_{x} s_{y} c_{z} & - c_{x} s_{y} \\ - c_{x} s_{z} & c_{x} c_{z} & s_{x} \\ s_{y} c_{z} + s_{x} c_{y} s_{z} & s_{y} s_{z} - s_{x} c_{y} c_{z} & c_{x} c_{y} \end{matrix}]

运动学

角速度也是矢量，因此也适用于在坐标系间的变换。

ω_{n b}^{b} = R_{n}^{b} ω_{n b}^{n}

3
对于角速度的反对称矩阵表示，则是

Ω_{n b}^{b} = R_{n}^{b} Ω_{n b}^{n} R_{b}^{n}

dcm微分方程推导：

\dot{R_{n}^{b}} = lim_{δ_{t} \to 0} \frac{R_{n}^{b} (t + δ_{t}) - R_{n}^{b} (t)}{δ_{t}}

而

R_{n}^{b} (t + δ_{t})

是从

R_{n}^{b} (t)

经过小转动

(3)

而来，即

\dot{R_{n}^{b}} = \frac{(I - Ω_{n b}^{b} δ_{t}) R_{n}^{b} (t) - R_{n}^{b} (t)}{δ_{t}} = - Ω_{n b}^{b} R_{n}^{b}

其中

Ω_{n b}^{b}

是角速度

ω_{n b}^{b}

的反对称矩阵。

四元数微分方程的推导

有如下等式：

c o s (θ) + s i n (θ) n = e^{θ n}

其中

n

是单位矢量的四元数。证明在这

\dot{q} (t) = lim_{d t \to 0} \frac{q (t + d t) - q (t)}{d t}

而

q (t + d t) = q (t) e^{ω d t / 2}

其中

ω

是角速度的四元数表示。
得

\dot{q} = \frac{1}{2} q ω = \frac{1}{2} [\begin{matrix} 0 & - ω_{x} & - ω_{y} & - ω_{z} \\ ω_{x} & 0 & ω_{z} & - ω_{y} \\ ω_{y} & - ω_{z} & 0 & ω_{x} \\ ω_{z} & ω_{y} & - ω_{x} & 0 \end{matrix}] q

欧拉角微分方程推导：

\begin{matrix} (4) & ω_{n b}^{b} = ω_{n 1}^{b} + ω_{12}^{b} + ω_{2 b}^{b} \end{matrix}

而

ω_{n 1}^{1} = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] \dot{θ_{z}}

ω_{12}^{2} = [\begin{matrix} 0 \\ 1 \\ 0 \end{matrix}] \dot{θ_{y}}

ω_{2 b}^{b} = [\begin{matrix} 1 \\ 0 \\ 0 \end{matrix}] \dot{θ_{x}}

ω_{n 1}^{b} = R_{1}^{b} ω_{n 1}^{1} ω_{12}^{b} = R_{2}^{b} ω_{12}^{2}

都带入式

(4)

，可得

ω_{n b}^{b} = [\begin{matrix} 1 & 0 & - s_{y} \\ 0 & c_{x} & s_{x} c_{y} \\ 0 & - s_{x} & c_{x} c_{y} \end{matrix}] [\begin{matrix} \dot{θ_{x}} \\ \dot{θ_{y}} \\ \dot{θ_{z}} \end{matrix}]

可得欧拉角微分方程：

[\begin{matrix} \dot{θ_{x}} \\ \dot{θ_{y}} \\ \dot{θ_{z}} \end{matrix}] = [\begin{matrix} 1 & s_{x} t_{y} & c_{x} t_{y} \\ 0 & c_{x} & - s_{x} \\ 0 & \frac{s_{x}}{c_{y}} & \frac{c_{x}}{c_{y}} \end{matrix}] ω_{n b}^{b}

考虑陀螺仪角速度误差的四元数更新方程：

q_{k + 1} = Φ_{k} q_{k} + w_{k}

其中

w_{k} = - \frac{Δ t}{2} Ξ_{k} ϵ_{k}

ϵ_{k}

是角速度误差

Ξ_{k} = [\begin{matrix} - Q^{T} \\ [Q \times] + q_{0} I_{3 \times 3} \end{matrix}]

如果是同一坐标系下的矢量旋转，则是
$r^{'} = q r q^{*}$
?
将式 $(1)$ 展开来证明。 ?
$ω_{n b}$ 表示b系相对于n的转动角速度。 ?
对于表示坐标系旋转的四元数，如果先后进行了p,q旋转，则联合旋转是p*q，即后转的乘在右边。 ?
$t_{y}$ 表示 $\tan (θ_{y})$ ?

来自为知笔记(Wiz)

四元数

声明：以上内容来自用户投稿及互联网公开渠道收集整理发布，本网站不拥有所有权，未作人工编辑处理，也不承担相关法律责任，若内容有误或涉及侵权可进行投诉：投诉/举报工作人员会在5个工作日内联系你，一经查实，本站将立刻删除涉嫌侵权内容。

联系
我们