即MEMS惯性器件的校准方法,只在几分钟内的精度有效。
简化的捷联惯导算法及误差方程
在MEMS中,陀螺仪精度为0.1。/s0.1^。/s0.1。/s量级,加速度计为5mg5mg5mg量级,由于陀螺仪精度过低无法,获得地球自转信息,需要对陀螺仪捷联惯导更新算法进行简化:
简化的姿态更新算法为:
。:四元数乘法P。Q=[p0−p1−p2−p3p1p0−p3−p2p2p3p0−p1p3−p2p1−p0][q0q1q2q3]=MPQ。:四元数乘法\\ P。Q=\left[\begin{matrix} p_0&-p_1&-p_2&-p_3\\p_1&p_0&-p_3&-p_2\\ p_2&p_3&p_0&-p_1\\p_3&-p_2&p_1&-p_0\\ \end{matrix}\right]\left[\begin{matrix} q_0\\q_1\\q_2\\q_3 \end{matrix}\right]=M_PQ。:四元数乘法P。Q=⎣⎢⎢⎡p0p1p2p3−p1p0p3−p2−p2−p3p0p1−p3−p2−p1−p0⎦⎥⎥⎤⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤=MPQ
Qb(m)n:tm时刻的姿态变化四元数Q_{b{(m)}}^n:t_m时刻的姿态变化四元数Qb(m)n:tm时刻的姿态变化四元数
Qb(m)b(m−1):tm−1到tm时刻的姿态四元数变换Q_{b(m)}^{b(m-1)}:t_{m-1}到t_m时刻的姿态四元数变换Qb(m)b(m−1):tm−1到tm时刻的姿态四元数变换
Δθm=[tm−1,tm]时间内角增量\Delta\theta_m=[t_{m-1},t_m]时间内角增量Δθm=[tm−1,tm]时间内角增量
Qb(m)n=Qb(m−1)n。Qb(m)b(m−1)Qb(m)b(m−1)=[cosΔθm2Δ∣θm∣ΔθmsinΔθm2]Q_{b{(m)}}^n=Q_{b{(m-1)}}^n。Q_{b(m)}^{b(m-1)}\\ Q_{b(m)}^{b(m-1)}=\left[\begin{matrix} cos \frac{\Delta \theta_m}{2}\\\frac{\Delta| \theta_m|}{\Delta \theta_m}sin\frac{\Delta \theta_m}{2} \end{matrix}\right]Qb(m)n=Qb(m−1)n。Qb(m)b(m−1)Qb(m)b(m−1)=[cos2ΔθmΔθmΔ∣θm∣sin2Δθm]
对于中低速的运载体,一般地速v<100m/sv<100m/sv<100m/s,在惯导比力方程中,(2wien+wenn)×vn≈1×10−3(2w^n_{ie}+w^n_{en})×v^n\approx 1×10^{-3}(2wien+wenn)×vn≈1×10−3远小于传感器本身误差,因此将速度更新方程简化为:
vmn:为tm时刻的惯导速度Cb(m−1)n:与四元数Qb(m−1)n对应的姿态阵v^n_m:为t_m时刻的惯导速度\\ C^n_{b(m-1)}:与四元数Q^n_{b(m-1)}对应的姿态阵vmn:为tm时刻的惯导速度Cb(m−1)n:与四元数Qb(m−1)n对应的姿态阵
Δvm:在时间[tm−1,tm]时刻内的增量,加速度输出×采样周期\Delta v_m:在时间[t_{m-1},t_m]时刻内的增量,加速度输出×采样周期Δvm:在时间[tm−1,tm]时刻内的增量,加速度输出×采样周期
vmn=vm−1n+Δvsf(m)n+gnTsΔvsf(m)n=Cb(m−1)n(Δvm+12Δθm×Δvm)v^n_m=v^n_{m-1}+\Delta v^n_{sf(m)}+g^nT_s\\ \Delta v^n_{sf(m)}=C^n_{b(m-1)}(\Delta v_m+\frac{1}{2}\Delta \theta_m×\Delta v_m)vmn=vm−1n+Δvsf(m)n+gnTsΔvsf(m)n=Cb(m−1)n(Δvm+21Δθm×Δvm)
一般MEMS在小范围内移动(几千米内),可以选择当地直角坐标系作为导航参考坐标系(n系),与地球表面固联,导航起始点为坐标原点,oxn,oyn,oznox_n,oy_n,oz_noxn,oyn,ozn分别指向东向、北向、天向,得到导航定位的微分方程为:
pmn=pm−1n+vm−1n+vmn2Tsp˙n=vnpmn=[xmymzm]Tp^n_m=p^n_{m-1}+\frac{v^n_{m-1}+v^n_m}{2}T_s\\ \dot p^n=v^n\\ p^n_m=\left[\begin{matrix} x_m&y_m&z_m \end{matrix}\right]^T\\ pmn=pm−1n+2vm−1n+vmnTsp˙n=vnpmn=[xmymzm]T
得到低精度惯导系统方程为:
wgb:陀螺仪角速率白噪声w^b_g:陀螺仪角速率白噪声wgb:陀螺仪角速率白噪声
wab:加度计比力白噪声w^b_a:加度计比力白噪声wab:加度计比力白噪声
τai:时间常数i=x,y,z\tau_{ai}:时间常数i=x,y,zτai:时间常数i=x,y,z
τgi:时间常数i=x,y,z\tau_{gi}:时间常数i=x,y,zτgi:时间常数i=x,y,z
wraib:一阶马尔可夫过程白噪声w^b_{rai}:一阶马尔可夫过程白噪声wraib:一阶马尔可夫过程白噪声
wrgib:一阶马尔可夫过程白噪声w^b_{rgi}:一阶马尔可夫过程白噪声wrgib:一阶马尔可夫过程白噪声
ξrb=[ξrxbξrybξrzb]T:陀螺仪的一阶马尔可夫过程误差,allan方差分析\xi_r^b=\left[\begin{matrix} \xi_{rx}^b&\xi_{ry}^b&\xi_{rz}^b \end{matrix}\right]^T:陀螺仪的一阶马尔可夫过程误差,allan方差分析ξrb=[ξrxbξrybξrzb]T:陀螺仪的一阶马尔可夫过程误差,allan方差分析
ξrb=[▽rxb▽ryb▽rzb]T:加速仪的一阶马尔可夫过程误差\xi_r^b=\left[\begin{matrix} \bigtriangledown_{rx}^b&\bigtriangledown_{ry}^b&\bigtriangledown_{rz}^b \end{matrix}\right]^T:加速仪的一阶马尔可夫过程误差ξrb=[▽rxb▽ryb▽rzb]T:加速仪的一阶马尔可夫过程误差
ϕ˙=−Cbn(ξrb+wgb)δv˙n=fsfn×ϕ+Cbn(▽rb+wab)δp˙n=δvn\dot \phi=-C^n_b(\xi^b_r+w^b_g)\\ \delta \dot v^n=f^n_{sf}×\phi+C^n_b(\bigtriangledown^b_r+w^b_a)\\ \delta \dot p^n=\delta v^nϕ˙=−Cbn(ξrb+wgb)δv˙n=fsfn×ϕ+Cbn(▽rb+wab)δp˙n=δvn
ξ˙rib=−1τgiξrib+wrgib▽˙rxb=1τai▽rib+wraibi=x,y,z\dot \xi^b_{ri}=-\frac{1}{\tau_{gi}}\xi^b_{ri}+w^b_{rgi}\\ \dot\bigtriangledown_{rx}^b=\frac{1}{\tau_{ai}}\bigtriangledown_{ri}^b+w^b_{rai}\\ i=x,y,zξ˙rib=−τgi1ξrib+wrgib▽˙rxb=τai1▽rib+wraibi=x,y,z
一阶马尔可夫过程的作用:
(1):一阶马尔卡夫过程可在长时间组合滤波后,避免滤波器过度收敛,防止过渡收敛导致抗干扰性弱
(2):如果惯性传感器中误差存在较大随机常值成分,可通过滤波器的传感器误差反馈矫正,消除随机常值误差影响