四元数微分方程的推导和代码实现

waihekor

已于 2022-09-24 22:03:23 修改

阅读量8.1k

点赞数 18

分类专栏：组合导航文章标签： EKF SLAM 四元数

于 2019-11-28 22:43:34 首次发布

本文链接：https://blog.csdn.net/waihekor/article/details/103297840

版权

组合导航专栏收录该内容

20 篇文章 36 订阅

订阅专栏

前言

最近在研究基于EKF的IMU姿态解算，阅读论文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》，遇到了四元数的微分方程，公式如下：

其中表示如下：

关于该公式的推导，之前在《惯性导航（第二版）》看过，现在将这部分内容整理下来，防止经常忘记，督促自己养成记笔记的好习惯。

推导过程：

首先定义坐标系：n系为导航坐标系，b系为载体坐标系，则n系到b系的旋转四元数可以表示为：

上式中为旋转轴，为旋转角，对两边求导可得：

根据哥氏定理可得：

由于刚体绕μ轴旋转，与刚体固联的b坐标系的各个轴在旋转的过程中分别位于三个不同的圆锥面上，三个圆锥面的定点即为b系的原点，μ为其共同的对称轴，这块大家可以想象一下，还是挺容易想象的，这样μ到b坐标系三个轴上的投影不变，长度为各自圆锥底面半径，所以有：

又有：

上式中的意思是：R系到b系的角速度在R系上的投影。

所以：

因此

又因为：

上面公式中是纯单位四元数相乘，根据四元数乘法法则容易推出，详细证明可见附录，所以

可得：

上面公式中的是在导航坐标系下的角速度，而IMU中的陀螺仪测量得到的角速度是在载体坐标系的，所以还需要一个转换关系,根据坐标变换的四元数乘法表示法：

上式中为的共轭四元数，所以

带入的公式得：

由于为单位四元数，所以

为陀螺仪的测量值，记

根据四元数的乘法定义，有两种表示形式，第一种如下所示：

即

或者也可以写成如下形式：

即为：

实现过程：

实际上就是四元数微分方程的解法，常用的有欧拉方法、中值法，毕卡算法，龙格库塔法。

常用的是经典4阶龙格库塔法，公式如下所示：

上面公式中的，，，都是微分方程的一阶导数，即为微分方程中的，同时可以看到一阶导数是关于的函数，即，所以在计算，，，时，只需要更新，，，就可以了，是陀螺仪数据更新周期。

参考代码如下：

% 四元数微分方程的4阶龙格库塔法
% q0:4*1
% gyro：陀螺仪数据
% T：更新周期
function [ q ] = Quaternion_RungeKutta4( q0,gyro,T)
    q0=Norm_Quaternion(q0); %归一化
    K1= Quaternion_Diff( gyro,q0);
    q1=Norm_Quaternion(q0+T/2*K1);
    K2 = Quaternion_Diff(gyro,q1);
    q2=Norm_Quaternion(q0+T/2*K2);
    K3 = Quaternion_Diff(gyro,q2);
    q3=Norm_Quaternion(q0+T*K3);
    K4 = Quaternion_Diff(gyro,q3);
    q = q0 + T/6*(K1+2*K2+2*K3+K4);
    q = Norm_Quaternion(q);
end

% 函数功能：四元数微分方程
% 输    出：四元数的一阶导数
% 备    注：连续域
function [ q_diff ] = Quaternion_Diff( gyro,q)

A = [       0, -gyro(1)/2, -gyro(2)/2, -gyro(3)/2;
    gyro(1)/2,          0,  gyro(3)/2, -gyro(2)/2;
    gyro(2)/2, -gyro(3)/2,          0,  gyro(1)/2;
    gyro(3)/2,  gyro(2)/2, -gyro(1)/2,         0];

q_diff = A*q;
end


function [ q ] = Norm_Quaternion( q )
 q = q/norm(q,2);
end