专门做特产的网站,网站开发所需经费,seo百度优化排名,建网站到底需要多少钱声明#xff1a;本人水平有限#xff0c;博客可能存在部分错误的地方#xff0c;请广大读者谅解并向本人反馈错误。 最近需要使用AEKF对姿态进行结算#xff0c;所以又对四元数进了深入的学习#xff0c;本篇博客仅对四元数进行推导#xff0c;后续会对基于四元数的… 声明本人水平有限博客可能存在部分错误的地方请广大读者谅解并向本人反馈错误。 最近需要使用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} 100 \\ 0cospsinp\\0-sinpcosp \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} cosr0-sinr \\ 010\\sinr0cosr \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} cosysiny0\\ -sinycosy0\\001 \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^bC_2^b(绕Y轴旋转矩阵)*C_1^2(绕X轴旋转矩阵)*C_n^1(绕Z轴旋转矩阵) CnbC2b(绕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} cosr0-sinr \\ 010\\sinr0cosr \end{bmatrix}*\begin{bmatrix} 100 \\ 0cospsinp\\0-sinpcosp \end{bmatrix}*\begin{bmatrix} cosysiny0\\ -sinycosy0\\001 \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*sinycosy*sinysinp*sinr*cosy-cosp*sinr \\ -cosp*sinycosp*cosysinp\\ sinr*cosysinp*cosr*sinysiny*sinr-sinp*cosr*cosycosrcosp \end{bmatrix} cosr∗cosy−sinp∗sinr∗siny−cosp∗sinysinr∗cosysinp∗cosr∗sinycosy∗sinysinp∗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*sinysinr*cosysinp*cosr*siny \\ cosy*sinysinp*sinr*cosycosp*cosysiny*sinr-sinp*cosr*cosy\\ -cosp*sinrsinpcosr*cosp \end{bmatrix} cosr∗cosy−sinp∗sinr∗sinycosy∗sinysinp∗sinr∗cosy−cosp∗sinr−cosp∗sinycosp∗cosysinpsinr∗cosysinp∗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] u R[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_0cos\frac{\theta}{2}\\q_1lsin\frac{\theta}{2}\\q_2msin\frac{\theta}{2}\\q_3nsin\frac{\theta}{2}\end{cases} ⎩ ⎨ ⎧q0cos2θq1lsin2θq2msin2θq3nsin2θ
2.2 绕X轴旋转 把 u → R [ l , m , n ] [ 1 , 0 , 0 ] \overrightarrow{u}^R[l,m,n][1,0,0] u R[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_0cos\frac{\theta}{2}\\q_1lsin\frac{\theta}{2}\\q_2msin\frac{\theta}{2}\\q_3nsin\frac{\theta}{2}\end{cases} ⎩ ⎨ ⎧q0cos2θq1lsin2θq2msin2θq3nsin2θ,那么就可以得到绕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] u R[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_0cos\frac{\theta}{2}\\q_1lsin\frac{\theta}{2}\\q_2msin\frac{\theta}{2}\\q_3nsin\frac{\theta}{2}\end{cases} ⎩ ⎨ ⎧q0cos2θq1lsin2θq2msin2θq3nsin2θ,那么就可以得到绕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] u R[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_0cos\frac{\theta}{2}\\q_1lsin\frac{\theta}{2}\\q_2msin\frac{\theta}{2}\\q_3nsin\frac{\theta}{2}\end{cases} ⎩ ⎨ ⎧q0cos2θq1lsin2θq2msin2θq3nsin2θ,那么就可以得到绕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 qqz⨂qx⨂qy ⨂ \bigotimes ⨂运算在《课题学习(十七)----姿态更新的四元数算法总结 》博客中也有介绍大家可以参考 在这部分我有一点未搞清楚就是四元数的乘法不满足交换律即“ P ⨂ Q ≠ Q ⨂ P \bold P \bigotimes \bold Q \neq \bold Q \bigotimes \bold P P⨂QQ⨂P” ,那么公式 q q z ⨂ q x ⨂ q y q q_z \bigotimes q_x\bigotimes q_y qqz⨂qx⨂qy不能更换相乘的顺序所以…我搞不懂这个顺序是否正确。 反正最后按照 q q z ⨂ q x ⨂ q y q q_z \bigotimes q_x\bigotimes q_y qqz⨂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 q02q12−q22−q322(q1q2q3q0)2(q1q3−q2q0)2(q1q2−q3q0)q02−q12q22−q322(q2q3q1q0)2(q1q3q2q0)2(q2q3−q1q0)q02−q12−q22q32 同时大家可以对本博客2.4节推导出来的四元数进行反推比如我计算了 2 ( q 2 q 3 q 1 q 0 ) 2(q_{2}q_{3}q_{1}q_{0}) 2(q2q3q1q0)最后得到的结果就是 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方差陀螺仪噪声分析 课题学习(二十)----阅读《近钻头井斜动态测量重力加速度信号提取方法研究》论文
