声明:本人水平有限,博客可能存在部分错误的地方,请广大读者谅解并向本人反馈错误。
最近需要使用AEKF对姿态进行结算,所以又对四元数进了深入的学习,本篇博客仅对四元数进行推导,后续会对基于四元数的AEKF算法进行实现。
一、 坐标变化
1.1 基本概念
首先,我对参考坐标系进行了定义,即选择“东-北-天”(或者叫做“右-前-上”),如下图:
并且记:
- p(pitch):俯仰角(绕X轴旋转);
- r(roll):横滚角(绕Y轴旋转);
- y(yaw):航向角(绕Z轴旋转);
并且设角度逆时针旋转为正。
1.2 绕X轴旋转
由上图可以得出由X-Y-Z–>X’-Y’-Z’的变换矩阵:
[ 1 0 0 0 c o s p s i n p 0 − s i n p c o s p ] \begin{bmatrix} 1&0&0 \\ 0&cosp&sinp\\0&-sinp&cosp \end{bmatrix} 1000cosp−sinp0sinpcosp
1.3 绕Y轴旋转
变换矩阵:
[ c o s r 0 − s i n r 0 1 0 s i n r 0 c o s r ] \begin{bmatrix} cosr&0&-sinr \\ 0&1&0\\sinr&0&cosr \end{bmatrix} cosr0sinr010−sinr0cosr
1.4 绕Z轴旋转
变换矩阵:
[ c o s y s i n y 0 − s i n y c o s y 0 0 0 1 ] \begin{bmatrix} cosy&siny&0\\ -siny&cosy&0\\0&0&1 \end{bmatrix} cosy−siny0sinycosy0001
1.5 坐标变换矩阵
在此假设旋转的顺序为:Z–>X–>Y,那么最后的坐标变换矩阵为(注意:一定要把矩阵相乘的顺序搞对):
C n b = C 2 b ( 绕 Y 轴旋转矩阵 ) ∗ C 1 2 ( 绕 X 轴旋转矩阵 ) ∗ C n 1 ( 绕 Z 轴旋转矩阵 ) C_n^b=C_2^b(绕Y轴旋转矩阵)*C_1^2(绕X轴旋转矩阵)*C_n^1(绕Z轴旋转矩阵) Cnb=C2b(绕Y轴旋转矩阵)∗C12(绕X轴旋转矩阵)∗Cn1(绕Z轴旋转矩阵)
= [ c o s r 0 − s i n r 0 1 0 s i n r 0 c o s r ] ∗ [ 1 0 0 0 c o s p s i n p 0 − s i n p c o s p ] ∗ [ c o s y s i n y 0 − s i n y c o s y 0 0 0 1 ] =\begin{bmatrix} cosr&0&-sinr \\ 0&1&0\\sinr&0&cosr \end{bmatrix}*\begin{bmatrix} 1&0&0 \\ 0&cosp&sinp\\0&-sinp&cosp \end{bmatrix}*\begin{bmatrix} cosy&siny&0\\ -siny&cosy&0\\0&0&1 \end{bmatrix} = cosr0sinr010−sinr0cosr ∗ 1000cosp−sinp0sinpcosp ∗ cosy−siny0sinycosy0001
= [ c o s r ∗ c o s y − s i n p ∗ s i n r ∗ s i n y c o s y ∗ s i n y + s i n p ∗ s i n r ∗ c o s y − c o s p ∗ s i n r − c o s p ∗ s i n y c o s p ∗ c o s y s i n p s i n r ∗ c o s y + s i n p ∗ c o s r ∗ s i n y s i n y ∗ s i n r − s i n p ∗ c o s r ∗ c o s y c o s r c o s p ] =\begin{bmatrix} cosr*cosy-sinp*sinr*siny&cosy*siny+sinp*sinr*cosy&-cosp*sinr \\ -cosp*siny&cosp*cosy&sinp\\ sinr*cosy+sinp*cosr*siny&siny*sinr-sinp*cosr*cosy&cosrcosp \end{bmatrix} = cosr∗cosy−sinp∗sinr∗siny−cosp∗sinysinr∗cosy+sinp∗cosr∗sinycosy∗siny+sinp∗sinr∗cosycosp∗cosysiny∗sinr−sinp∗cosr∗cosy−cosp∗sinrsinpcosrcosp
注意: C n b C_n^b Cnb是n系到b系的坐标变换矩阵,而在求解时,是将b系的姿态变换到n系上求解,所以最后要对 C n b C_n^b Cnb转置变为 C b n C_b^n Cbn 。
C b n = ( C n b ) T C_b^n = (C_n^b)^T Cbn=(Cnb)T
= [ c o s r ∗ c o s y − s i n p ∗ s i n r ∗ s i n y − c o s p ∗ s i n y s i n r ∗ c o s y + s i n p ∗ c o s r ∗ s i n y c o s y ∗ s i n y + s i n p ∗ s i n r ∗ c o s y c o s p ∗ c o s y s i n y ∗ s i n r − s i n p ∗ c o s r ∗ c o s y − c o s p ∗ s i n r s i n p c o s r ∗ c o s p ] =\begin{bmatrix} cosr*cosy-sinp*sinr*siny&-cosp*siny&sinr*cosy+sinp*cosr*siny \\ cosy*siny+sinp*sinr*cosy&cosp*cosy&siny*sinr-sinp*cosr*cosy\\ -cosp*sinr&sinp&cosr*cosp \end{bmatrix} = cosr∗cosy−sinp∗sinr∗sinycosy∗siny+sinp∗sinr∗cosy−cosp∗sinr−cosp∗sinycosp∗cosysinpsinr∗cosy+sinp∗cosr∗sinysiny∗sinr−sinp∗cosr∗cosycosr∗cosp
= [ T 11 T 12 T 13 T 21 T 22 T 23 T 31 T 32 T 33 ] =\begin{bmatrix} T_{11}&T_{12}&T_{13}\\ T_{21}&T_{22}&T_{23}\\ T_{31}&T_{32}&T_{33} \end{bmatrix} = T11T21T31T12T22T32T13T23T33
二、 四元数
2.1 基础概念回忆
在课题学习(十七)----姿态更新的四元数算法总结博客中,已经对四元数进行了比较详细的讲解,在这里只对四元数的物理意义重述一次:
其中, u → R = [ l , m , n ] \overrightarrow{u}^R=[l,m,n] uR=[l,m,n],且四元数定义为:
{ q 0 = c o s θ 2 q 1 = l s i n θ 2 q 2 = m s i n θ 2 q 3 = n s i n θ 2 \begin{cases}q_0=cos\frac{\theta}{2}\\q_1=lsin\frac{\theta}{2}\\q_2=msin\frac{\theta}{2}\\q_3=nsin\frac{\theta}{2}\end{cases} ⎩ ⎨ ⎧q0=cos2θq1=lsin2θq2=msin2θq3=nsin2θ
2.2 绕X轴旋转
把 u → R = [ l , m , n ] = [ 1 , 0 , 0 ] \overrightarrow{u}^R=[l,m,n]=[1,0,0] uR=[l,m,n]=[1,0,0]代入 { q 0 = c o s θ 2 q 1 = l s i n θ 2 q 2 = m s i n θ 2 q 3 = n s i n θ 2 \begin{cases}q_0=cos\frac{\theta}{2}\\q_1=lsin\frac{\theta}{2}\\q_2=msin\frac{\theta}{2}\\q_3=nsin\frac{\theta}{2}\end{cases} ⎩ ⎨ ⎧q0=cos2θq1=lsin2θq2=msin2θq3=nsin2θ,那么就可以得到绕X轴的四元数 q x = [ c o s p 2 , s i n p 2 , 0 , 0 ] q_x=[cos\frac{p}{2},sin\frac{p}{2},0,0] qx=[cos2p,sin2p,0,0]
2.3 绕Y、Z轴旋转
绕Y轴旋转:把 u → R = [ l , m , n ] = [ 0 , 1 , 0 ] \overrightarrow{u}^R=[l,m,n]=[0,1,0] uR=[l,m,n]=[0,1,0]代入 { q 0 = c o s θ 2 q 1 = l s i n θ 2 q 2 = m s i n θ 2 q 3 = n s i n θ 2 \begin{cases}q_0=cos\frac{\theta}{2}\\q_1=lsin\frac{\theta}{2}\\q_2=msin\frac{\theta}{2}\\q_3=nsin\frac{\theta}{2}\end{cases} ⎩ ⎨ ⎧q0=cos2θq1=lsin2θq2=msin2θq3=nsin2θ,那么就可以得到绕Y轴的四元数 q y = [ c o s r 2 , 0 , s i n r 2 , 0 ] q_y=[cos\frac{r}{2},0,sin\frac{r}{2},0] qy=[cos2r,0,sin2r,0]
绕Y轴旋转:把 u → R = [ l , m , n ] = [ 0 , 0 , 1 ] \overrightarrow{u}^R=[l,m,n]=[0,0,1] uR=[l,m,n]=[0,0,1]代入 { q 0 = c o s θ 2 q 1 = l s i n θ 2 q 2 = m s i n θ 2 q 3 = n s i n θ 2 \begin{cases}q_0=cos\frac{\theta}{2}\\q_1=lsin\frac{\theta}{2}\\q_2=msin\frac{\theta}{2}\\q_3=nsin\frac{\theta}{2}\end{cases} ⎩ ⎨ ⎧q0=cos2θq1=lsin2θq2=msin2θq3=nsin2θ,那么就可以得到绕Z轴的四元数 q z = [ c o s y 2 , 0 , 0 , s i n y 2 ] q_z=[cos\frac{y}{2},0,0,sin\frac{y}{2}] qz=[cos2y,0,0,sin2y]
2.4 用四元数表示旋转矩阵(坐标变换矩阵)
在第一节中,我们规定了旋转顺序为:Z–>X–>Y,那么最后四元数的旋转矩阵为(参考博客:《欧拉角和四元数之间转换公式推导》): q = q z ⨂ q x ⨂ q y q = q_z \bigotimes q_x\bigotimes q_y q=qz⨂qx⨂qy
" ⨂ \bigotimes ⨂"运算在《课题学习(十七)----姿态更新的四元数算法总结 》博客中也有介绍,大家可以参考:
在这部分,我有一点未搞清楚,就是四元数的乘法不满足交换律,即“ P ⨂ Q ≠ Q ⨂ P \bold P \bigotimes \bold Q \neq \bold Q \bigotimes \bold P P⨂Q=Q⨂P” ,那么公式 q = q z ⨂ q x ⨂ q y q = q_z \bigotimes q_x\bigotimes q_y q=qz⨂qx⨂qy不能更换相乘的顺序,所以…我搞不懂这个顺序是否正确。
反正最后按照 q = q z ⨂ q x ⨂ q y q = q_z \bigotimes q_x\bigotimes q_y q=qz⨂qx⨂qy得到的四元数为:
三、四元数与坐标变换矩阵的关系
在秦永元老师的《惯性导航(第二版)》9.2.2节(P248-P253)中对四元数与坐标变换矩阵的关系进行了推导,大家可以参考学习。
最后得到的四元数表示的坐标变换矩阵为:
C b n = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 3 q 0 ) 2 ( q 1 q 3 + q 2 q 0 ) 2 ( q 1 q 2 + q 3 q 0 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 1 q 0 ) 2 ( q 1 q 3 − q 2 q 0 ) 2 ( q 2 q 3 + q 1 q 0 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] C_b^n=\begin{bmatrix} q^2_{0}+q^2_{1}-q^2_{2}-q^2_{3} &2(q_{1}q_{2}-q_{3}q_{0})&2(q_{1}q_{3}+q_{2}q_{0})\\ 2(q_{1}q_{2}+q_{3}q_{0}) &q^2_{0}-q^2_{1}+q^2_{2}-q^2_{3}&2(q_{2}q_{3}-q_{1}q_{0})\\ 2(q_{1}q_{3}-q_{2}q_{0})&2(q_{2}q_{3}+q_{1}q_{0})&q^2_{0}-q^2_{1}-q^2_{2}+q^2_{3} \end{bmatrix} Cbn= q02+q12−q22−q322(q1q2+q3q0)2(q1q3−q2q0)2(q1q2−q3q0)q02−q12+q22−q322(q2q3+q1q0)2(q1q3+q2q0)2(q2q3−q1q0)q02−q12−q22+q32
同时,大家可以对本博客2.4节推导出来的四元数进行反推,比如我计算了 2 ( q 2 q 3 + q 1 q 0 ) 2(q_{2}q_{3}+q_{1}q_{0}) 2(q2q3+q1q0),最后得到的结果就是 s i n p sinp sinp:
四、往期回顾
课题学习(一)----静态测量
课题学习(二)----倾角和方位角的动态测量方法(基于磁场的测量系统)
课题学习(三)----倾角和方位角的动态测量方法(基于陀螺仪的测量系统)
课题学习(四)----四元数解法
课题学习(五)----阅读论文《抗差自适应滤波的导向钻具动态姿态测量方法》
课题学习(六)----安装误差校准、实验方法
课题学习(七)----粘滑运动的动态算法
课题学习(八)----卡尔曼滤波动态求解倾角、方位角
课题学习(九)----阅读《导向钻井工具姿态动态测量的自适应滤波方法》论文笔记
课题学习(十)----阅读《基于数据融合的近钻头井眼轨迹参数动态测量方法》论文笔记
课题学习(十一)----阅读《Attitude Determination with Magnetometers and Accelerometers to Use in Satellite》
课题学习(十二)----阅读《Extension of a Two-Step Calibration Methodology to Include Nonorthogonal Sensor Axes》
课题学习(十三)----阅读《Calibration of Strapdown Magnetometers in Magnetic Field Domain》论文笔记
课题学习(十四)----三轴加速度计+三轴陀螺仪传感器-ICM20602
课题学习(十五)----阅读《测斜仪旋转姿态测量信号处理方法》论文
课题学习(十六)----阅读《Continuous Wellbore Surveying While Drilling Utilizing MEMS Gyroscopes Based…》论文
课题学习(十七)----姿态更新的四元数算法总结
课题学习(十八)----捷联测试电路设计与代码实现(基于MPU6050和QMC5883L)
课题学习(十九)----Allan方差:陀螺仪噪声分析
课题学习(二十)----阅读《近钻头井斜动态测量重力加速度信号提取方法研究》论文