Beihang作为第二大作业.docVIP

《惯性导航原理》第二次大作业 姓名:袁二凯 学号:SY0917145 联系方式 642101136@ 2011年11月8日 一、 原理分析 在利用方向余弦方法对惯性导航系统进行测算时，刚体空间位置用固连于刚体的动坐标系对固定参考系各轴的九个方向余弦来确定，九个方向余弦角存在六个约束条件，计算比较繁琐，模型也比较复杂。如果在计算过程中引入四元数，则可以通过坐标系的一次转动，实现方向余弦方法中的三次坐标旋转。原理图如下： fb ft λ、L、h Vx、Vy、Vz Witt Wibb + — Witb 从原理图可以清楚的看出，通过捷联姿态矩阵Cbt可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法即可以得到任意时刻载体的位置和速度信息。关键在于捷联姿态矩阵的求解。在这里应用四元数知识进行解算。 1. 捷联姿态矩阵的求解 因为平台的初始姿态角都是已知的，则可以先求解四元数中各元的初值。平台坐标系相对于地理坐标系的三次旋转可以由四元数的乘积得到。用四元数表示这三次转动为（逆时针为正）： Q1=cosφ2+zsinφ2 Q2=cosθ2+xb1sinθ2 Q3=cosγ2+ybsinγ2 对三者进行四元素乘法运算： Q=Q3∘Q2∘Q1 结果与四元数Λ=λ0+λ1x+λ2y+λ3z中的各元素相对应，就可以从已知的平台初始姿态求解出四元数的各元初值。 λ0=cosφ2cosθ2cosγ2-sinφ2sinθ2sinγ2λ1=cosφ2sinθ2cosγ2-sinφ2cosθ2sinγ2λ2=cosφ2cosθ2sinγ2+sinφ2sinθ2cosγ2λ3=sinφ2cosθ2cosγ2+cosφ2sinθ2sinγ2 带入四元数姿态矩阵即可得到捷联姿态矩阵： Ctb=λ02+λ12+λ22+λ322(λ1λ2+λ0λ3)2(λ1λ3-λ0λ2)2(λ1λ2-λ0λ3)λ02-λ12+λ22-λ322(λ2λ3+λ0λ1)2(λ1λ3+λ0λ2)2(λ2λ3-λ0λ1)λ02-λ12-λ22+λ32 通过此矩阵可以将地理坐标系的参数转移到平台坐标系。此矩阵的逆矩阵Cbt就是将比力信息从平台坐标系转移到地理坐标系的姿态矩阵。此外此矩阵还要在四元数的迭代计算中使用。下面进行四元数的迭代计算。 四元数微分方程的矩阵形式为 λ0λ1λ2λ3=120ωtbxb-ωtbxb0-ωtbyb-ωtbzbωtbzb -ωtbybωtbyb-ωtbzb 0 ωtbxbωtbzbωtbyb-ωtbxb 0λ0λ1λ2λ3 式中 ωtbb=ωibb-Ctbωitt 其中ωibb即为陀螺仪所测量的角速度值，ωitt为平台的指令角速度，为地球的自转角速率与地理坐标系相对于地球坐标系的角速率之和，即 ωitt=ωiet+ωett 地球的自转角速率为： ωiet=ωiextωieytωiezt=0ωiecosLωiesinL 地理坐标系相对于地球坐标系的角速率为： ωett=ωetxtωetytωetzt = -VetytRytVetxtRxtVetxtRxttanL 上述四元数方程的解和下面矩阵方程的解类似： Q（λ）=12M*ωtbbQ（λ） 记q(t)=λ0λ1λ2λ3T，由角增量法可得迭代公式（取四阶算法）： q(n+1)=1-∆θ028+∆θ04384I+12-∆θ0248∆θ q(n) （n=0,1,2，……） 式中 ∆θ=t1t2M*(ωtbb)dt=0∆θx-∆θx0-∆θy-∆θz∆θz -∆θy∆θy-∆θz 0 ∆θx∆θz∆θy-∆θx 0 ∆θ02=∆θx2+∆θy2+∆θz2 q(0)即为解算出的初值，带入迭代公式即可得到任意时刻的捷联姿态矩阵Cbt。通过捷联姿态矩阵Cbt可以将任意姿态的平台坐标系下的比力数据转换到地理坐标系下，然后通过指北方位平台式惯导解算的方法求解任意时刻载体的位置和速度信息即可。 2、指北方位平台式惯导求解方位和速度 载体相对地球运动时，加速度计测得的比力表达式，称为比力方程，方程如下： 在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得：