四元数与其微分方程

2024-01-06 16:52:10

文章目录

四元数
四元数的三角表示法
四元数微分方程
参考博客

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

四元数

这里要说明，四元数可以理解为四维超球面上的一个点，点坐标用四维坐标轴表示。

四元数是空间旋转的轴角表示法的进一步理解

四元数的三角表示法

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

推导

参考文献：范奎武. 用四元数描述飞行器姿态时的几个基本问题[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

码农公寓