四元数

Posted 2020-08-12

旋转的表示方式

旋转矩阵

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

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

RBA$R_A^B$是正交矩阵，RAB$R_B^A$为RBA$R_A^B$的转置。det(RBA$R_A^B$)=1

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

quaternion

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

q=?????abcd?????=??????cos(0.5θ)vxsin(0.5θ)vysin(0.5θ)vzsin(0.5θ)??????$$q=\left(\begin{array}{c}a\\ b\\ c\\ d\end{array}\right)=\left(\begin{array}{c}\cos ?(0.5\theta )\\ v_x\sin ?(0.5\theta )\\ v_y\sin ?(0.5\theta )\\ v_z\sin ?(0.5\theta )\end{array}\right)$$

θ$\theta$为转过的角度，(vxvyvz)T${\left(\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right)}^T$定义了转轴，为单位矢量。
四元数的模为1。

四元数的动态方程：

如果q的第一项是标量项

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

?????abcd?????=?????q0q1q2q3?????$\left(\begin{array}{c}a\\ b\\ c\\ d\end{array}\right)=\left(\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right)$

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

由加速度计可以计算roll，pitch

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

Hx$H_x$等是地磁场在n‘坐标系中的表示，hx$h_x$等是在b坐标系中的表示。然后由Hx$H_x$, Hy$H_y$求反正切，再加上磁偏角修正，就得到了yaw。

axis-angle

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

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

rb=q?rnq(1)$$\begin{array}{}\text{(1)}& r^b=q^{?}{r}^{n}q\end{array}$$

其中r$r$是标量部分为0的四元数，q$q$是将n系转至b系的四元数。1

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

四元数乘法：

(r1,v1→)?(r2,v2→)=(r1r2?v1→?v2→r1v2→+r2v1→+v1→×v2→)$$(r_1,\overrightarrow{v_1})?(r_2,\overrightarrow{v_2})=\left(\begin{array}{c}{r}_1{r}_2?\overrightarrow{v_1}?\overrightarrow{v_2}\\ r_1\overrightarrow{v_2}+r_2\overrightarrow{v_1}+\overrightarrow{v_1}\times \overrightarrow{v_2}\end{array}\right)$$

四元数q=[q0q1q2q3]$q=\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]$(q0$q_0$是标量部分)对应的dcm

Rbn=(2q20?1)I+2QQT?2q0[Q×]$$R_n^b=(2q_0^2?1)I+2Q{Q}^T?2q_0[Q\times ]$$

2
其中Q$Q$是四元数的矢量部分，[Q×]$[Q\times ]$是矢量的反对称矩阵，

[Q×]=???0Q3?Q2?Q30Q1Q2?Q10???$$[Q\times ]=\left[\begin{array}{ccc}0& ?Q_3& Q_2\\ Q_3& 0& ?Q_1\\ ?Q_2& Q_1& 0\end{array}\right]$$

展开后为：

Rbn=????2(q20+q21)?12(q1q2?q0q3)2(q0q2+q1q3)2(q1q2+q0q3)2(q20+q22)?12(q2q3?q0q1)2(q1q3?q0q2)2(q0q1+q2q3)2(q20+q23)?1????(2)$$\begin{array}{}\text{(2)}& R_n^b=\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_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{array}\right]\end{array}$$

dcm->四元数

由(2)$\text{(2)}$可得：

4q20?1=tr(R)4q0q1=R23?R324q0q2=R31?R134q0q3=R12?R21$$\begin{gathered} 4q_0^2?1=tr(R) \\ 4q_0{q}_1=R_{23}?R_{32} \\ 4q_0{q}_2=R_{31}?R_{13} \\ 4q_0{q}_3=R_{12}?R_{21} \end{gathered}$$

于是：

q0=R11+R22+R33+1????????????????√2q1=R23?R324q0q2=R31?R134q0q3=R12?R214q0$$\begin{gathered} q_0=\frac{\sqrt{R_{11}+R_{22}+R_{33}+1}}{2} \\ {q}_1=\frac{R_{23}?R_{32}}{4q_0} \\ {q}_2=\frac{R_{31}?R_{13}}{4q_0} \\ {q}_3=\frac{R_{12}?R_{21}}{4q_0} \end{gathered}$$

四元数->欧拉角

绕X轴旋转

Rx=???1000cθ?sθ0sθcθ???$$R_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& c_{\theta }& s_{\theta }\\ 0& ?s_{\theta }& c_{\theta }\end{array}\right]$$

绕Y轴旋转

Ry=???cθ0sθ010?sθ0cθ???$$R_y=\left[\begin{array}{ccc}{c}_{\theta }& 0& ?s_{\theta }\\ 0& 1& 0\\ s_{\theta }& 0& c_{\theta }\end{array}\right]$$

绕Z轴旋转

Rz=???cθ?sθ0sθcθ0001???$$R_z=\left[\begin{array}{ccc}{c}_{\theta }& s_{\theta }& 0\\ ?s_{\theta }& c_{\theta }& 0\\ 0& 0& 1\end{array}\right]$$

sθ$s_{\theta }$放置位置记忆方法：看如下序列

XYZXY

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

R=RxRyRz=???cyczsxsycz?cxszcxsycz+sxszcyszsxsysz+cxczcxsysz?sxcz?sysxcycxcy???$$R=R_x{R}_y{R}_z=\left[\begin{array}{ccc}{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{array}\right]$$

tan(θz)=R12R11sin(θy)=?R13tan(θx)=R23R33$$\begin{gathered} \text{tan}({\theta }_z)=\frac{R_{12}}{R_{11}} \\ \text{sin}({\theta }_y)=?R_{13} \\ \text{tan}({\theta }_x)=\frac{R_{23}}{R_{33}} \end{gathered}$$

若是小角转动，

R=???1?θzθyθz1?θx?θyθx1???=I?Θ(3)$$\begin{array}{}\text{(3)}& R=\left[\begin{array}{ccc}1& {\theta }_z& ?{\theta }_y\\ ?{\theta }_z& 1& {\theta }_x\\ {\theta }_y& ?{\theta }_x& 1\end{array}\right]=I?\mathrm{\Theta }\end{array}$$

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

RyRxRz=???cycz?sxsysz?cxszsycz+sxcyszcysz+sxsyczcxczsysz?sxcycz?cxsysxcxcy???$$R_y{R}_x{R}_z=\left[\begin{array}{ccc}{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{array}\right]$$

运动学

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

ωbnb=Rbnωnnb$${\omega }_{nb}^b=R_n^b{\omega }_{nb}^n$$

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

Ωbnb=RbnΩnnbRnb$${\mathrm{\Omega }}_{nb}^b=R_n^b{\mathrm{\Omega }}_{nb}^n{R}_b^n$$

dcm微分方程推导：

Rbn˙=limδt→0Rbn(t+δt)?Rbn(t)δt$$\dot{R_n^b}=\underset{{\delta }_t\to 0}{lim}\frac{R_n^b(t+{\delta }_t)?R_n^b(t)}{{\delta }_t}$$

而Rbn(t+δt)$R_n^b(t+{\delta }_t)$是从Rbn(t)$R_n^b(t)$经过小转动(3)$\text{(3)}$而来，即

Rbn˙=(I?Ωbnbδt)Rbn(t)?Rbn(t)δt=?ΩbnbRbn$$\begin{gathered} \dot{R_n^b}=\frac{(I?{\mathrm{\Omega }}_{nb}^b{\delta }_t)R_n^b(t)?R_n^b(t)}{{\delta }_t} \\ =?{\mathrm{\Omega }}_{nb}^b{R}_n^b \end{gathered}$$

其中Ωbnb${\mathrm{\Omega }}_{nb}^b$是角速度ωbnb${\omega }_{nb}^b$的反对称矩阵。

四元数微分方程的推导

有如下等式：

cos(θ)+sin(θ)n=eθn$$cos(\theta )+sin(\theta )\mathbf{n}=e^{\theta \mathbf{n}}$$

其中n$\mathbf{n}$是单位矢量的四元数。证明在这

q˙(t)=limdt→0q(t+dt)?q(t)dt$$\dot{q}(t)=\underset{dt\to 0}{lim}\frac{q(t+dt)?q(t)}{dt}$$

而

q(t+dt)=q(t)eωdt/2$$q(t+dt)=q(t)e^{\omega dt/2}$$

其中ω$\omega$是角速度的四元数表示。
得

q˙=12qω=12??????0ωxωyωz?ωx0?ωzωy?ωyωz0?ωx?ωz?ωyωx0??????q$$\dot{q}=\frac{1}{2}q\omega =\frac{1}{2}\left[\begin{array}{cccc}0& ?{\omega }_x& ?{\omega }_y& ?{\omega }_z\\ {\omega }_x& 0& {\omega }_z& ?{\omega }_y\\ {\omega }_y& ?{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& ?{\omega }_x& 0\end{array}\right]q$$

4

欧拉角微分方程推导：

ωbnb=ωbn1+ωb12+ωb2b(4)$$\begin{array}{}\text{(4)}& {\omega }_{nb}^b={\omega }_{n1}^b+{\omega }_{12}^b+{\omega }_{2b}^b\end{array}$$

而

ω1n1=???001???θz˙$${\omega }_{n1}^1=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\dot{{\theta }_z}$$

ω212=???010???θy˙$${\omega }_{12}^2=\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]\dot{{\theta }_y}$$

ωb2b=???100???θx˙$${\omega }_{2b}^b=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]\dot{{\theta }_x}$$

ωbn1=Rb1ω1n1ωb12=Rb2ω212$${\omega }_{n1}^b=R_1^b{\omega }_{n1}^1{\omega }_{12}^b=R_2^b{\omega }_{12}^2$$

都带入式(4)$\text{(4)}$，可得

ωbnb=???1000cx?sx?sysxcycxcy???????θx˙θy˙θz˙????$${\omega }_{nb}^b=\left[\begin{array}{ccc}1& 0& ?s_y\\ 0& c_x& s_x{c}_y\\ 0& ?s_x& c_x{c}_y\end{array}\right]\left[\begin{array}{c}\dot{{\theta }_x}\\ \dot{{\theta }_y}\\ \dot{{\theta }_z}\end{array}\right]$$

可得欧拉角微分方程：

????θx˙θy˙θz˙????=????100sxtycxsxcycxty?sxcxcy????ωbnb$$\left[\begin{array}{c}\dot{{\theta }_x}\\ \dot{{\theta }_y}\\ \dot{{\theta }_z}\end{array}\right]=\left[\begin{array}{ccc}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{array}\right]{\omega }_{nb}^b$$

5

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

qk+1=Φkqk+wk$$q_{k+1}={\mathrm{\Phi }}_k{q}_k+w_k$$

其中

wk=?Δt2Ξk?k$$w_k=?\frac{\mathrm{\Delta }t}{2}{\mathrm{\Xi }}_k{?}_k$$

?k${?}_k$是角速度误差

Ξk=[?QT[Q×]+q0I3×3]$${\mathrm{\Xi }}_k=\left[\begin{array}{c}?Q^T\\ [Q\times ]+q_0{I}_{3\times 3}\end{array}\right]$$

---

1. 如果是同一坐标系下的矢量旋转，则是
   
   r′=qrq?$$r^{\prime }=qr{q}^{?}$$
   ?
2. 将式(1)$\text{(1)}$展开来证明。 ?
3. ωnb${\omega }_{nb}$表示b系相对于n的转动角速度。 ?
4. 对于表示坐标系旋转的四元数，如果先后进行了p,q旋转，则联合旋转是p*q，即后转的乘在右边。 ?
5. ty$t_y$表示tan(θy)$\tan ?({\theta }_y)$ ?

来自为知笔记(Wiz)