注:这里讨论的是旋转过程的,也可以理解为两个坐标系之间的旋转关系。而不是单独地描述一个坐标系,这是不可能实现的。
四元数这里要说明,四元数可以理解为四维超球面上的一个点,点坐标用四维坐标轴表示。
四元数是空间旋转的轴角表示法的进一步理解
上面介绍了四元数的定义,实际上是四元数的复数表示法。四元数还可以用三角法表示,其更能表达其物理含义。
参考文献:范奎武. 用四元数描述飞行器姿态时的几个基本问题[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