其他分享
首页 > 其他分享> > 四元数与其微分方程

四元数与其微分方程

作者:互联网

文章目录

注:这里讨论的是旋转过程的,也可以理解为两个坐标系之间的旋转关系。而不是单独地描述一个坐标系,这是不可能实现的。

四元数

这里要说明,四元数可以理解为四维超球面上的一个点,点坐标用四维坐标轴表示。
在这里插入图片描述
四元数是空间旋转的轴角表示法的进一步理解

四元数的三角表示法

上面介绍了四元数的定义,实际上是四元数的复数表示法。四元数还可以用三角法表示,其更能表达其物理含义。

推导

在这里插入图片描述
参考文献:范奎武. 用四元数描述飞行器姿态时的几个基本问题[J]. 航天控制, 2012, 30(4):49-53.

也可以分开来写:
在这里插入图片描述

几何含义

在这里插入图片描述

扩展:轴角表示法

旋转的轴角表示用两个值参数化了旋转: 一个轴或直线,和描述绕这个轴的旋转量的一个角。
在这里插入图片描述

四元数微分方程

构造目的

陀螺仪测量的是三轴的角速度,根据时间积分得到的是三轴欧拉角。那么现在问题来了,我们要计算总的姿态角,假如初始值是R0,P0,Y0,现在姿态角增量是δR,δP,δY。我们知道R0,P0,Y0是按照ZYX欧拉角描述法,机体依次绕自己的坐标系ZYX进行旋转的,而δR,δP,δY则可以理解为相对于前一个时间的坐标系按照XYZ固定角进行旋转的(也可以理解为机体依次绕自己的坐标系ZYX进行旋转,这两者是等效的)。
因此要积分求最终的姿态角,我们要么就对R0,P0,Y0按照ZYX顺序进行三次旋转,要么就得想其他办法。按照ZYX顺序进行三次旋转显然是十分繁琐的,据说还会遇到万向节死锁的问题。因此有了解四元数的方法。
解四元数的方法本质上就是利用δR,δP,δY按照XYZ固定角进行旋转来求解的。方法是导出δR,δP,δY与四元数的关系:
在这里插入图片描述

其中
在这里插入图片描述

推导过程:微分方程

首先定义坐标系: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
 
参考博客

https://blog.csdn.net/qq_45467083/article/details/107082439
https://zhuanlan.zhihu.com/p/101101455
https://blog.csdn.net/waihekor/article/details/103297840
https://blog.csdn.net/sinat_38245860/article/details/80036340

标签:q0,gyro,旋转,四元,Quaternion,微分方程,坐标系,与其
来源: https://blog.51cto.com/u_15262460/2883167