文章目录

四元数四元数的三角表示法推导几何含义扩展：轴角表示法 四元数微分方程构造目的推导过程：微分方程数值解法 参考博客

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

四元数

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

四元数的三角表示法

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

推导

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