惯导机械编排算法
预备知识
![image-20221102204801385](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221102204801385.png)
![image-20221102204928595](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221102204928595.png)
![image-20221102205644140](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221102205644140.png)
![image-20221102212123016](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221102212123016.png)
欧拉角法
![image-20221102212739448](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221102212739448.png)
![image-20221102212955069](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221102212955069.png)
![image-20221102220857730](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221102220857730.png)
这里务必注意:欧拉角正方向定义是从R系->b系 ,即从参考系->当前的载体系
![image-20221102220835356](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221102220835356.png)
欧拉角微分方程
建立 陀螺输出的w与欧拉角微分的关系 (后续因欧拉角限制不会使用)
![image-20221104191104431](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221104191104431.png)
方向余弦矩阵
![image-20221104204609060](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221104204609060.png)
![image-20221104211322950](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221104211322950.png)
方向余弦阵性质
![image-20221104211425823](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221104211425823.png)
![image-20221104222130231](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221104222130231.png)
理解:
DCM小角度时,
![image-20221105104155205](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105104155205.png)
C b R = I + ( α R b × ) C_b^R = I + (\alpha_{Rb} \times) CbR=I+(αRb×)
C b ( t k ) t ( k − 1 ) = I + Δ θ b t ( k − 1 ) b t k × C_{b(tk)}^{t(k-1)} = I + \Delta \theta_{b_{t(k-1)}b_{t_{k}}}\times Cb(tk)t(k−1)=I+Δθbt(k−1)btk×
方向余弦矩阵的微分方程
![image-20221105110828531](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105110828531.png)
但实际完成姿态更新时,由于陀螺输出的角速度是 ω i b b \omega_{ib}^{b} ωibb非 ω R b b \omega_{Rb}^{b} ωRbb,需要补偿掉i系与n系之间的地球自转和牵连角速度
微分方程的求解
![image-20221105210014487](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105210014487.png)
![image-20221105214159395](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105214159395.png)
以上计算前提是假设b系相对于R系角速度方向保持不变
四元数
![image-20221105221455105](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105221455105.png)
![image-20221105221942499](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105221942499.png)
![image-20221105222043072](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105222043072.png)
![image-20221105222130362](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105222130362.png)
![image-20221105222257288](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221105222257288.png)
注意:
纯虚四元数乘法,分块矩阵方法:(实部 + 虚部)
![image-20221106093006903](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106093006903.png)
![image-20221106093448197](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106093448197.png)
![image-20221106093632379](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106093632379.png)
![image-20221106093815450](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106093815450.png)
四元数共轭 <==> 矩阵转置
四元数逆 <==> 矩阵的逆
四元数的三角函数表示
![image-20221106100632895](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106100632895.png)
![image-20221106100808025](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106100808025.png)
模长为1的四元数 可以表示 三维空间的姿态旋转
可以等价理解为 模长为1的复数可以 表示二维空间的旋转
p = ∣ ∣ l e n g t h ∣ ∣ e i θ , w h e n l e n g t h = 1 , p c o u l d o n l y d e s c r i b e p o s e i n 2 − d i m e n s i o n s p a c e p = ||length||e^{i\theta}, when length = 1, p could only describe pose in 2-dimension space p=∣∣length∣∣eiθ,whenlength=1,pcouldonlydescribeposein2−dimensionspace
四元数算子证明*
![]()
- 欧拉旋转定理
![image-20221106101121401](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106101121401.png)
姿态四元数
![image-20221106103122759](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106103122759.png)
一般做姿态投影都转化为 方向余弦阵,考虑到向量旋转投影变换四元数乘法需要做两次
四元数的微分方程
![image-20221106194405232](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106194405232.png)
![image-20221106194611882](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106194611882.png)
![image-20221106194934204](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106194934204.png)
四元数和DCM更新时的 不可交换误差 问题
![image-20221106195246560](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106195246560.png)
![image-20221106195707702](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221106195707702.png)
等效旋转矢量
![image-20221108232536856](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221108232536856.png)
圆锥误差补偿:载体在做圆锥运动时,不可交换误差补偿项较大
姿态更新算法小结
![image-20221113085240825](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221113085240825.png)
更正: 蓝色字体第二点 增量应为w_in^{n},因为是n系的更新。
![image-20221113090050457](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221113090050457.png)
- 这里边用到了w_ie^{n},需要载体速度V_E,V_N
- 初始姿态由初始对准算法给定
![image-20221113091619547](https://qixi-1314016585.cos.ap-shanghai.myqcloud.com/image-20221113091619547.png)
姿态表达根本上是为了向量投影,实际常用方向余弦矩阵和四元数
RV仅用在多子样算法补偿不可交换误差的情况