Runge-Kutta法求四元数微分方程

Runge-Kutta法求四元数微分方程

文章目录一、背景知识1. 坐标系

2. 四元数四元数的矩阵形式

四元数与旋转的关系

二、数学模型1. 四元数微分方程

2. 四元数微分方程的矩阵形式

三、常微分方程的初值问题1. 欧拉法

2. 显式梯形法

3. 中点法

4. 泰勒法

5. 龙格库塔法龙格库塔法的一般形式

四阶龙格库塔法

四、龙格库塔法求解四元数微分方程

五、四元数转欧拉角

陀螺仪是一种测量角运动的装置，在导航、运动检测、姿态检测等方面有着非常广泛的应用。在惯性测量单元(IMU)中，陀螺仪起主要作用，加速度计往往用来辅助，通过滤波算法和融合算法可以由IMU输出的数据得到物体的姿态。事实上，如果陀螺仪的性能足够好，就可以只通过对陀螺仪得到准确的姿态了。这篇文章要解决的问题就是如何用陀螺仪的数据得到物体的姿态。

1. 坐标系

陀螺仪输出的是角速度，那么对角速度积分是不是就能得到物体在空间中的姿态呢？事实上并非如此，在这里需要理解三个坐标系：世界坐标系、载体坐标系和惯性坐标系。

世界坐标系又称作宇宙坐标系或者全局坐标系，是其他坐标系的参考系，我们可以在世界坐标系下去描述其他坐标系或者物体的位置和姿态。

载体坐标系，又称作机体坐标系、物体坐标系等，顾名思义，这个坐标系是物体上的坐标系，会跟随物体的运动而运动。以手机为例，一般会在手机上建立如下所示的右手坐标系，这个坐标系就是一个载体坐标系。陀螺仪安装在手机上，如果忽略芯片本身和安装带来的非正交误差的话，可以认为陀螺仪的坐标系和手机的坐标系是同一个坐标系。所以，陀螺仪输出的角速度实际上是手机载体坐标系的角速度，直接对陀螺仪角速度积分得到的不是我们从世界坐标系看到的手机的姿态角度。需要将陀螺仪角速度转换到惯性坐标系，然后再积分才能得到物体的姿态。

惯性坐标系是为了简化世界坐标系和载体坐标系的转化而产生的。两个直角坐标系的关系包括旋转和位移。惯性坐标系的原点和载体坐标系的原点是同一个，所以通过旋转关系，两个坐标系就能够重合。而惯性坐标系通过位移就能够和世界坐标系重合。

2. 四元数

表示一个物体的姿态有很多种方法，最主流的方法有欧拉角、旋转矩阵、轴角，以及四元数。这些表示方法各有优缺点，但综合来说，四元数是一种非常适合在工程上表示姿态的方法。四元数可以理解成复数的扩展，它有一个实部和三个虚部，表示成q=w+x?i+y?j+z?kq=w+x\ast{i}+y\ast{j}+z\ast{k}q=w+x?i+y?j+z?k，其中i?i=?1,j?j=?1,k?k=?1,i?j?k=?1i\ast{i}=-1,j\ast{j}=-1,k\ast{k}=-1,i\ast{j}\ast{k}=-1i?i=?1,j?j=?1,k?k=?1,i?j?k=?1。四元数有自己一套运算法则和性质，后面在其他文章具体阐述，这里介绍几个重要的点。

四元数的矩阵形式

类似复数，四元数也有模长或者称为2-范数，即∥q∥=w2+x2+y2+z2\begin{Vmatrix}q\end{Vmatrix}=\sqrt {w^2+x^2+y^2+z^2}∥∥?q?∥∥?=w2+x2+y2+z2?，如果∥q∥=1\begin{Vmatrix}q\end{Vmatrix}=1∥∥?q?∥∥?=1，则称这个四元数位单位四元数，单位四元数有如下性质：q?1=q?q^{-1}=q^{\ast}q?1=q?。

假设两个四元数相乘q1q2q_1q_2q1?q2?，其中q1=a+b?i+c?j+d?kq_1=a+b\ast{i}+c\ast{j}+d\ast{k}q1?=a+b?i+c?j+d?k，q2=e+f?i+g?j+h?kq_2=e+f\ast{i}+g\ast{j}+h\ast{k}q2?=e+f?i+g?j+h?k，则

q1?q2=ae+af?i+ag?j+ah?k+be?i?bf+bg?k?bh?j+ce?j?cf?k?cg+ch?i+dek+df?j?dg?i?dh=(ae?bf?cg?dh)+(be+af?dg+ch)?i(ce+df+ag?bh)?j(de?cf+bg+ah)?k

\begin{aligned}

q_1 * q_2 = & ae + af * i + ag * j + ah * k + \\

& be * i - bf + bg * k - bh * j + \\

& ce * j - cf * k - cg + ch * i + \\

& dek + df * j - dg * i - dh \\

=& (ae - bf - cg - dh) + \\

& (be + af - dg + ch) * i \\

& (ce + df + ag - bh) * j \\

& (de - cf + bg + ah) * k

\end{aligned}q1??q2?==?ae+af?i+ag?j+ah?k+be?i?bf+bg?k?bh?j+ce?j?cf?k?cg+ch?i+dek+df?j?dg?i?dh(ae?bf?cg?dh)+(be+af?dg+ch)?i(ce+df+ag?bh)?j(de?cf+bg+ah)?k?

上面的结果可以写成一个方阵和一个列矩阵相乘：

q1?q2=∣a?b?c?dba?dccda?bd?cba∣∣efgh∣

q_1 * q_2 = \begin{vmatrix}a&-b&-c&-d\\b&a&-d&c\\c&d&a&-b\\d&-c&b&a\end{vmatrix}

\begin{vmatrix}e\\f\\g\\h\end{vmatrix}q1??q2?=∣∣∣∣∣∣∣∣?abcd??bad?c??c?dab??dc?ba?∣∣∣∣∣∣∣∣?∣∣∣∣∣∣∣∣?efgh?∣∣∣∣∣∣∣∣?

如果矩阵q2=1+0?i+0?j+0?kq_2 = 1 + 0 * i + 0 * j + 0 * kq2?=1+0?i+0?j+0?k，则q1?q2=q1q_1 * q_2 = q_1q1??q2?=q1?

四元数与旋转的关系

四元数可以表示三维空间中物体的旋转或姿态，从q=w+x?i+y?j+z?kq = w + x * i + y * j + z * kq=w+x?i+y?j+z?k很难想象出来，但是如果换成q=[cos?θ2,sin?θ2?n^]q = [\cos{\frac{\theta}{2}}, \sin{\frac{\theta}{2}} * \hat{n}]q=[cos2θ?,sin2θ??n^]就好理解多了，这个四元数表示一个绕轴n^\hat{n}n^旋转θ\thetaθ度的旋转动作。对一个向量(或者坐标系)进行旋转可以表示为v′=q?vqv^{'}=q^{\ast}vqv′=q?vq。

为了用陀螺仪数据求出物体的姿态，我们需要将物理原理抽象成数学模型，然后用数学方法求解。假设惯性坐标系位EEE，载体坐标系位bbb，陀螺仪输出的角速度记作ωEbb\omega_{Eb}^{b}ωEbb?，物体在惯性坐标系的角速度记作ωEbE\omega_{Eb}^{E}ωEbE?，通过坐标系旋转qqq将载体坐标系下的角速度转换为惯性坐标系下的角速度，即q?ω→Ebbq=ω→EbEq^{\ast} \overrightarrow\omega_{Eb}^{b} q = \overrightarrow\omega_{Eb}^{E}q?ωEbb?q=ωEbE?。只要求出qqq就能知道物体相对的姿态变化情况。

1. 四元数微分方程

令Q=[cos?θ2,n^sin?θ2]Q = [\cos{\frac{\theta}{2}}, \hat{n}\sin{\frac{\theta}{2}}]Q=[cos2θ?,n^sin2θ?]，则QQQ对时间ttt的微分方程为：

Q˙=?12sin?θ2dθdt+dn^dtsin?θ2+12n^cos?θ2dθdt=12dθdt(cos?θ2n^?sin?θ2),becausedn^dt=0=12n^dθdt(cos?θ2+n^sin?θ2),becausen^?n^=?1=12n^ω→EbE(cos?θ2+n^sin?θ2)=12n^ω→EbEQ=12n^Qω→Ebb,becauseω→Ebb=Q?ω→EbEQ,Q?Q=1=12(q0+q1i+q2j+q3k)(0+ωxi+ωyi+ωzk)

\begin{aligned}

\dot{Q} & = - \frac{1}{2}\sin{\frac{\theta}{2}} \frac{{\rm d}\theta}{{\rm d}t} + \frac{{\rm d}\hat{n}}{{\rm d}t} \sin{\frac{\theta}{2}} + \frac{1}{2}\hat{n}\cos{\frac{\theta}{2}}\frac{{\rm d}\theta}{{\rm d}t} \\

& = \frac{1}{2}\frac{{\rm d}\theta}{{\rm d}t}(\cos{\frac{\theta}{2}}\hat{n}-\sin{\frac{\theta}{2}}),& \text{because }\frac{{\rm d}\hat{n}}{{\rm d}t}=0 \\

& = \frac{1}{2}\hat{n}\frac{{\rm d}\theta}{{\rm d}t}(\cos{\frac{\theta}{2}}+\hat{n}\sin{\frac{\theta}{2}}), &\text{because }\hat{n}\ast\hat{n}=-1 \\

& = \frac{1}{2}\hat{n}\overrightarrow\omega_{Eb}^{E}(\cos{\frac{\theta}{2}}+\hat{n}\sin{\frac{\theta}{2}}) \\

& = \frac{1}{2}\hat{n}\overrightarrow\omega_{Eb}^{E}Q \\

& = \frac{1}{2}\hat{n}Q\overrightarrow\omega_{Eb}^{b}, & \text{because }\overrightarrow\omega_{Eb}^{b}=Q^{\ast}\overrightarrow\omega_{Eb}^{E}Q,Q^{\ast}Q=1 \\

& = \frac{1}{2}(q_0+q_1i+q_2j+q_3k)(0+\omega_xi+\omega_yi+\omega_zk)

\end{aligned}Q˙??=?21?sin2θ?dtdθ?+dtdn^?sin2θ?+21?n^cos2θ?dtdθ?=21?dtdθ?(cos2θ?n^?sin2θ?),=21?n^dtdθ?(cos2θ?+n^sin2θ?),=21?n^ωEbE?(cos2θ?+n^sin2θ?)=21?n^ωEbE?Q=21?n^QωEbb?,=21?(q0?+q1?i+q2?j+q3?k)(0+ωx?i+ωy?i+ωz?k)?becausedtdn^?=0becausen^?n^=?1becauseωEbb?=Q?ωEbE?Q,Q?Q=1?

2. 四元数微分方程的矩阵形式

根据两个四元数相乘的矩阵形式，上式改写为：

Q˙=12∣q0?q1?q2?q3q1q0?q3q2q2q3q0?q3q3?q2q1q0∣∣0ωxωyωz∣=∣0?ωx?ωy?ωzωx0?ωz?ωyωy?ωz0ωxωzωy?ωx0∣∣q0q1q2q3∣

\begin{aligned}

\dot{Q} = & \frac{1}{2}\begin{vmatrix}q_0&-q_1&-q_2&-q_3\\q_1&q_0&-q_3&q_2\\q_2&q_3&q_0&-q_3\\q_3&-q_2&q_1&q_0\end{vmatrix} \begin{vmatrix}0\\\omega_x\\\omega_y\\\omega_z\end{vmatrix} \\

= & \begin{vmatrix}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{vmatrix}\begin{vmatrix}q_0\\q_1\\q_2\\q_3\end{vmatrix}

\end{aligned}Q˙?==?21?∣∣∣∣∣∣∣∣?q0?q1?q2?q3???q1?q0?q3??q2???q2??q3?q0?q1???q3?q2??q3?q0??∣∣∣∣∣∣∣∣?∣∣∣∣∣∣∣∣?0ωx?ωy?ωz??∣∣∣∣∣∣∣∣?∣∣∣∣∣∣∣∣?0ωx?ωy?ωz???ωx?0?ωz?ωy???ωy??ωz?0?ωx???ωz??ωy?ωx?0?∣∣∣∣∣∣∣∣?∣∣∣∣∣∣∣∣?q0?q1?q2?q3??∣∣∣∣∣∣∣∣??

工程上很多问题建成数学模型时都是微分方程的形式，而ordinary differential equation(ODE)，即常微分方程是微分方程中较为简单的一类，常微分方程初值问题则是常微分方程理论研究和实际应用的一种基本定解问题，可以描述为以下形式：

{y′=f(t,y)y(0)=y0t∈[a,b]

\begin{cases}

y^{'}=f(t,y) \\

y(0)=y_0 \\

t\in{[a,b]}

\end{cases}??????y′=f(t,y)y(0)=y0?t∈[a,b]?

直接求解常微分方程难度较大，所以伟大的数学家们想出了各种近似求法，能够在一定误差范围内求出方程(组)的解。粗略来说，这些近似求解法的基本思想是已知上一个值求下一个值，下一个值等于上一个值加上一个增量，而增量约等于步长乘以斜率。本文讲到的求解方法基本上都是在“斜率”上做文章，也有在步长上改进算法的，如“可变步长法”。

1. 欧拉法

欧拉法是很多求解方法的基础，它表示为：

{ω0=y0ωi+1=ωi+hf(ti,ωi)

\begin{cases}

\omega_0=y_0 \\

\omega_{i+1}=\omega_i+hf(t_i,\omega_i)

\end{cases}{ω0?=y0?ωi+1?=ωi?+hf(ti?,ωi?)?

其中hhh为时间ttt轴上的步长，f(ti,ωi)f(t_i,\omega_i)f(ti?,ωi?)为该点的斜率。从ω0\omega_0ω0?开始不断迭代就能够求出点tit_iti?处的近似解。

2. 显式梯形法

对于常微分方程求解法，工程上一般需要考虑截断误差、时间复杂度和空间复杂度(在其他文章中阐述)，综合这些指标选择最合适的算法。欧拉法能够求出近似解，而且计算量比较小，但是误差比较大。显式梯形法是对欧拉法的一种改进，它表示为：

{ω0=y0ωi+1=ωi+h2(f(ti,ωi)+f(ti+h,ωi+hf(ti,ωi)))

\begin{cases}

\omega_0=y_0 \\

\omega_{i+1}=\omega_i+\frac{h}{2}(f(t_i,\omega_i)+f(t_i+h,\omega_i+hf(t_i,\omega_i)))

\end{cases}{ω0?=y0?ωi+1?=ωi?+2h?(f(ti?,ωi?)+f(ti?+h,ωi?+hf(ti?,ωi?)))?

3. 中点法

中点法也是欧拉法的一种改进方法，它表示为：

{ω0=y0ωi+1=ωi+hf(ti+h2,f(ti,ωi+h2f(ti,ωi)))

\begin{cases}

\omega_0=y_0 \\

\omega_{i+1}=\omega_i+hf(t_i+\frac{h}{2},f(t_i, \omega_i+\frac{h}{2}f(t_i,\omega_i)))

\end{cases}{ω0?=y0?ωi+1?=ωi?+hf(ti?+2h?,f(ti?,ωi?+2h?f(ti?,ωi?)))?

4. 泰勒法

(待补充)

5. 龙格库塔法

龙格库塔法(Runge-Kutta，简称RK法)是一组常微分方程求解器，它可以根据要求调整阶数，输出误差级别不一样的结果。龙格库塔法的基本思想囊括了以上讲到的欧拉法、显式梯形法、中点法和泰勒法，是一种较为灵活的求解方法。

龙格库塔法的一般形式

(待补充)

四阶龙格库塔法

四阶龙格库塔法是最经典的龙格库塔法，它的截断误差、时间复杂度和空间复杂度都很好，常用于工程上求解常微分方程问题，它的形式为：

{ω0=y0ωi+1=ωi+16(s1+2s2+2s3+s4)s1=f(ti,ωi)s2=f(ti+12h,ωi+12s1)s3=f(ti+12h,ωi+12s2)s4=f(ti+h,ωi+hs3)

\begin{cases}

\omega_0=y_0 \\

\omega_{i+1}=\omega_i+\frac{1}{6}(s_1+2s_2+2s_3+s_4) \\

s_1=f(t_i,\omega_i) \\

s_2=f(t_i+\frac{1}{2}h,\omega_i+\frac{1}{2}s_1) \\

s_3=f(t_i+\frac{1}{2}h,\omega_i+\frac{1}{2}s_2) \\

s_4=f(t_i+h,\omega_i+hs_3)

\end{cases}????????????????????ω0?=y0?ωi+1?=ωi?+61?(s1?+2s2?+2s3?+s4?)s1?=f(ti?,ωi?)s2?=f(ti?+21?h,ωi?+21?s1?)s3?=f(ti?+21?h,ωi?+21?s2?)s4?=f(ti?+h,ωi?+hs3?)?

我们已经知道四元数表示的微分方程：

Q˙=∣0?ωx?ωy?ωzωx0?ωz?ωyωy?ωz0ωxωzωy?ωx0∣∣q0q1q2q3∣，

\dot{Q} = \begin{vmatrix}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{vmatrix}\begin{vmatrix}q_0\\q_1\\q_2\\q_3\end{vmatrix}，Q˙?=∣∣∣∣∣∣∣∣?0ωx?ωy?ωz???ωx?0?ωz?ωy???ωy??ωz?0?ωx???ωz??ωy?ωx?0?∣∣∣∣∣∣∣∣?∣∣∣∣∣∣∣∣?q0?q1?q2?q3??∣∣∣∣∣∣∣∣?，

同时也知道了四阶龙格库塔法的公式，那么就可以将四元数微分方程带入四阶龙格库塔公式进行求解。需要注意的是初值条件，一般会设初始角度θ\thetaθ为0，则Q0=∣1000∣Q_0=\begin{vmatrix}1&0&0&0\end{vmatrix}Q0?=∣∣?1?0?0?0?∣∣?。

以上求解可以得到一个表示iii时刻旋转状态的四元数QiQ_iQi?，再通过四元数和欧拉角的转换关系即可得到物体旋转后的姿态。

(未完待续)

Runge-Kutta法求四元数微分方程相关教程