通过空间角速度积分更新旋转矩阵及四元数的公式推导

最近开始着手做硕士毕设课题，需要用到IMU采集姿态信息用以提取手势，本着亲身实践的目的，我决定自己写算法实现姿态的融合，由于IMU运动过程中的加速度计输出叠加了非重力项，然后这就必然绕不开使用原始传感器的角速度数据来对姿态进行积分运算。

这里值得一提的是，看到网络上好多博客的做法都是对单个欧拉角直接积分，事实上这种做法是错误的，四元数4个参数就可以表示姿态，欧拉角凭什么可以只用3个角度变量表示姿态，是因为欧拉角表示法还有旋转顺序这一条，同一组3个角度，旋转顺序不同，最终的姿态也不同，如果直接对单个欧拉角进行积分，顺序就会错乱，最终的效果无法保证，当然我没有试验过，这里只是从理论上说明一下。

正文.

一、旋转矩阵求导和积分

3维旋转矩阵表示的是空间物体的姿态，线速度不会改变物体姿态，只有角速度才会。我们先将旋转矩阵由求极限求导的公式列出来，如式(1):

$\dot{R}=\lim\limits_{\triangle{t}\to0}\frac{R(t+\triangle{t})-R(t)}{\triangle{t}} \tag{1}$

由于Δt前后矩阵仍然是旋转矩阵，所以必然存在另一个旋转变换矩阵使得该积分前的旋转矩阵左乘该变换矩阵可以得到积分后的旋转矩阵，公式表达如式(2)：

$R(t+\triangle{t})=R_{k}(\triangle{\theta})R(t) \tag{2}$

在上式(2)中变换矩阵用轴角的形式表示，即绕着转轴K(下标所示)旋转Δθ角。将(2)代入(1)中，我们得到式(3)：

$\dot{R}=(\lim\limits_{\triangle{t}\to0}\frac{R_{k}(\triangle{\theta})-E}{\triangle{t}})R(t) \tag{3}$

当Δt趋于0，即Δθ趋于0时，可以对轴角参数表示的姿态矩阵(《机器人学》.Craig原书公式 .2.80)进行简化，得到式(4):

$R_{k}(\triangle{\theta})=\left[ \begin{matrix} 1 & -k_z\triangle{\theta} & k_y\triangle{\theta} \\ k_z\triangle{\theta} & 1 & -k_x\triangle{\theta} \\ -k_y\triangle{\theta} & k_x\triangle{\theta} & 1 \end{matrix} \right] \tag{4}$

再将式(4)代入式(3)，得到式(5)：

$\dot{R}=\left[ \begin{matrix} 0 & -k_z\dot{\theta} & k_y\dot{\theta} \\ k_z\dot{\theta} & 0 & -k_x\dot{\theta} \\ -k_y\dot{\theta} & k_x\dot{\theta} & 0 \end{matrix} \right]R(t) \tag{5}$

上式(5)中的矩阵称为角速度反对称矩阵，矩阵的每一项分别表示角速度在X-Y-Z轴上的分量，可由角速度向量叉乘的形式得到，于是最终的由空间角速度和时间积分得到的旋转矩阵表示的姿态由式(6)得到：

二、四元数求导和积分

由公式(6)知道旋转矩阵的积分形式后，再根据四元数与旋转矩阵相互转化的公式(我目前用这种方法只算出了q₄，其它三个元素这样算很麻烦，后面有计算步骤是使用四元数直接微分算得，下面的计算就权当两种形式的积分之间的相互验证吧)，我们可以很快的推算出四元数的角速度积分形式，步骤如下：

首先我们列出旋转矩阵转化为四元数的公式(7)-(10)：

$q_{1}=\frac{r_{32}-r_{23}}{4q_{4}} \tag{7}$

$q_{2}=\frac{r_{13}-r_{31}}{4q_{4}} \tag{8}$

$q_{3}=\frac{r_{21}-r_{12}}{4q_{4}} \tag{9}$

$q_{4}=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}} \tag{10}$

上式中，q₄为四元数的实数项，其它为虚数项，接下来我们直接利用式(6)，将其矩阵相乘得到积分后的矩阵表示，即式(11):

$R(t+\triangle{t})=\left[ \begin{matrix} 1 & -\omega_{z}\triangle{t} & \omega_{y}\triangle{t} \\ \omega_{z}\triangle{t} & 1 & -\omega_{x}\triangle{t} \\ -\omega_{y}\triangle{t} & \omega_{x}\triangle{t} & 1 \end{matrix} \right]\cdot\left[\begin{matrix} m_{11} & m_{12} & m_{13} \\ m_{21} & m_{22} & m_{23} \\ m_{31} & m_{32} & m_{33} \end{matrix} \right] \tag{11}$

我以求取新的Q₄为例，说明计算的方法，其它三项原理相同，最后我会放出完整的积分公式。下式中，我将以小写字母m表示积分前的矩阵元素，小写字母r表示积分后的矩阵元素，大写字母Q表示新的四元数，小写q表示积分前的四元数。在式(10)中我们知道，计算q₄需要r₁₁,r₂₂,r₃₃,把式(11)乘开即可得到以上三项的表达式分别为：

$r_{11} = m_{11}+\triangle{t}(-\omega_{z}m_{21}+\omega_{y}m_{31}) \tag{12}$

$r_{22} = m_{22}+\triangle{t}(\omega_{z}m_{12}-\omega_{x}m_{32}) \tag{13}$

$r_{33} = m_{33}+\triangle{t}(-\omega_{y}m_{13}+\omega_{x}m_{23}) \tag{14}$

将以上三式代入式(10)可以得到：

$Q_{4}=\frac{1}{2}\sqrt{1+m_{11}+m_{22}+m_{33}-\triangle{t}\omega_{z}(m_{21}-m_{12})-\triangle{t}\omega_{y}(m_{13}-m_{31})-\triangle{t}\omega_{x}(m_{32}-m_{23})} \tag{15}$

进一步变形我们可以得到：

$Q_{4}=\frac{1}{2}\sqrt{1+m_{11}+m_{22}+m_{33}}\sqrt{1+\frac{-\triangle{t}\omega_{z}(m_{21}-m_{12})-\triangle{t}\omega_{y}(m_{13}-m_{31})-\triangle{t}\omega_{x}(m_{32}-m_{23})}{1+m_{11}+m_{22}+m_{33}}} \tag{16}$

将式(7)-(10)代入式(16)中，可得到：

$Q_{4} = q_{4}\sqrt{1+\frac{-\omega_{z}\triangle{t}q_{3}-\omega_{y}\triangle{t}q_{2}-\omega_{x}\triangle{t}q_{1}}{q_{4}}} \tag{17}$

进一步针对 $\sqrt{1+x}$ 形式的函数进行泰勒展开取前两项，并应用于式(17)得到近似的解(Δt趋于0时)：

$Q_{4} = q_{4}+\frac{1}{2}\triangle{t}(-\omega_{z}q_{3}-\omega_{y}q_{2}-\omega_{x}q_{1}) \tag{17}$

直接使用四元数微分的计算方法

有四元数的表示形式 $Q=cos(\frac{\theta}{2})+\hat{n}sin(\frac{\theta}{2})$ ，对其求导得到式(18):

$\frac{dQ}{dt}=-\frac{1}{2}sin(\frac{\theta}{2})\frac{d\theta}{dt}+\frac{d\hat{n}}{dt}sin(\frac{\theta}{2})+\hat{n}\frac{1}{2}cos(\frac{\theta}{2})\frac{d\theta}{dt} \tag{18}$

上式中 $\hat{n}$ 表示的为旋转轴，在积分或者微分的一瞬间，角速度为恒定值，故只有角度变换， $\frac{d\hat{n}}{dt}$ 即为0，同时有 $\hat{n}\frac{d\theta}{dt}=\omega^E$ ， $\omega^E$ 表示在世界坐标系中的物体转动的角速度。在上式中，提取 $\frac{1}{2}\omega^E$ ，两外两项相加即为四元数Q，于是有:

$\frac{dQ}{dt}= \frac{1}{2}\omega^E * Q \tag{19}$

上式中" * "表示4元数乘法。另外设在物体坐标系中的角速度表示为 $\omega^O$ ，由坐标变换的四元数表示法得到：

$\omega^E=Q*\omega^O*Q^* \tag{20}$

上式中Q^*为Q的共轭四元数，Q^**Q=1，将上式代入到式(19)得到

$\frac{dQ}{dt}= \frac{1}{2}Q*\omega^O \tag{21}$

将式(20)用四元数乘法展开可得微分的形式：

$\frac{dQ}{dt}=\frac{1}{2}\left[ \begin{matrix} 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{matrix} \right]\left[ \begin{matrix} q_4 \\ q_1 \\ q_2 \\ q_3 \end{matrix} \right] \tag{22}$

然后我们将式(22)代入到微分求极限中，最终可获得积分形式：

$Q=\left[ \begin{matrix} q_4 \\ q_1 \\ q_2 \\ q_3 \end{matrix} \right]+\frac{\triangle{t}}{2}\left[ \begin{matrix} -\omega_xq_1-\omega_yq_2-\omega_zq_3 \\ \omega_xq_4-\omega_yq_3+\omega_zq_2 \\ \omega_xq_3+\omega_yq_4-\omega_zq_1 \\ -\omega_xq_2+\omega_yq_1+\omega_zq_4 \end{matrix} \right] \tag{23}$