首页 > 其他分享> 文章详细

四元数与其微分方程

2021-06-08 17:57:32 阅读：675 来源： 互联网

标签：q0 gyro 旋转四元 Quaternion 微分方程坐标系与其

文章目录

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

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

四元数

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

四元数的三角表示法

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

推导

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

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9