龙格库塔（Runge-Kutta）法求四元数微分方程

Posted 2022-11-21 herr_edoc

文章目录

• 一、背景知识
• • 1. 坐标系
  • 2. 四元数
  • • 四元数的矩阵形式
    • 四元数与旋转的关系
• 二、数学模型
• • 1. 四元数微分方程
  • 2. 四元数微分方程的矩阵形式
• 三、常微分方程的初值问题
• • 1. 欧拉法
  • 2. 显式梯形法
  • 3. 中点法
  • 4. 泰勒法
  • 5. 龙格库塔法
  • • 龙格库塔法的一般形式
    • 四阶龙格库塔法
• 四、龙格库塔法求解四元数微分方程
• 五、四元数转欧拉角

一、背景知识

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

1. 坐标系

陀螺仪输出的是角速度，那么对角速度积分是不是就能得到物体在空间中的姿态呢？事实上并非如此，在这里需要理解三个坐标系：世界坐标系、载体坐标系和惯性坐标系。
世界坐标系又称作宇宙坐标系或者全局坐标系，是其他坐标系的参考系，我们可以在世界坐标系下去描述其他坐标系或者物体的位置和姿态。
载体坐标系，又称作机体坐标系、物体坐标系等，顾名思义，这个坐标系是物体上的坐标系，会跟随物体的运动而运动。以手机为例，一般会在手机上建立如下所示的右手坐标系，这个坐标系就是一个载体坐标系。陀螺仪安装在手机上，如果忽略芯片本身和安装带来的非正交误差的话，可以认为陀螺仪的坐标系和手机的坐标系是同一个坐标系。所以，陀螺仪输出的角速度实际上是手机载体坐标系的角速度，直接对陀螺仪角速度积分得到的不是我们从世界坐标系看到的手机的姿态角度。需要将陀螺仪角速度转换到惯性坐标系，然后再积分才能得到物体的姿态。

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

2. 四元数

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

四元数的矩阵形式

类似复数，四元数也有模长或者称为2-范数，即$\Vert \begin{array}{c}q\end{array}\Vert =\sqrt{w^2+x^2+y^2+z^2}$，如果$\Vert \begin{array}{c}q\end{array}\Vert =1$，则称这个四元数位单位四元数，单位四元数有如下性质：$q^{-1}=q^{\ast }$。
假设两个四元数相乘$q_1{q}_2$，其中$q_1=a+b\ast i+c\ast j+d\ast k$，$q_2=e+f\ast i+g\ast j+h\ast k$，则
$$\begin{array}{rl}{q}_1\ast q_2=& ae+af\ast i+ag\ast j+ah\ast k+\\ & be\ast i-bf+bg\ast k-bh\ast j+\\ & ce\ast j-cf\ast k-cg+ch\ast i+\\ & dek+df\ast j-dg\ast i-dh\\ =& (ae-bf-cg-dh)+\\ & (be+af-dg+ch)\ast i\\ & (ce+df+ag-bh)\ast j\\ & (de-cf+bg+ah)\ast k\end{array}$$
上面的结果可以写成一个方阵和一个列矩阵相乘：
$$q_1\ast q_2=|\begin{array}{cccc}a& -b& -c& -d\\ b& a& -d& c\\ c& d& a& -b\\ d& -c& b& \end{array}$$