捷联惯导系统学习7.5(低成本组合导航系统模型)

低成本组合导航系统模型

低精度MEMS惯性/卫星/地磁组合导航系统中，选择惯导系统的姿态失准角ϕ\phiϕ、速度误差δvn\delta v^nδvn、定位误差δpn\delta p^nδpn、陀螺仪相关漂移ξrb\xi^b_rξrb、加速度计相关偏值▽rb\bigtriangledown_r^b▽rb、安装偏差角γ\gammaγ、磁倾角δηx\delta\eta_xδηx、磁偏角δηz\delta\eta_zδηz，作为状态(20维数据),如下：
βs=diag(1/τsx,1/τsy,1/τsz)(s=g,a)\beta_s=diag(1/\tau_{sx},1/\tau_{sy},1/\tau_{sz})(s=g,a)βs=diag(1/τsx,1/τsy,1/τsz)(s=g,a)
[(hn×)Cmn](:,[1,3]):右下标表示第1和第3列[(h^n×)C^n_m]_{(:,[1,3])}:右下标表示第1和第3列[(hn×)Cmn](:,[1,3]):右下标表示第1和第3列
Vv:接收机速度测量白噪声V_v:接收机速度测量白噪声Vv:接收机速度测量白噪声
Vp:接收机位置测量白噪声V_p:接收机位置测量白噪声Vp:接收机位置测量白噪声
VH:地磁测量白噪声V_H:地磁测量白噪声VH:地磁测量白噪声
{X˙=FX+GWbZ=[v~INSn−v~GNSSnp~INSn−p~GNSSnC~bnh~bm−Cm′ne2]=HX+VX=[ϕT(δvn)T(δpn)T(ξrb)T(▽rb)TγTδηxδηz]TF=[03×303×303×3−Cbn03×3fsfn×03×303×303×3Cbn03×3I3×303×303×303×3015×503×303×303×3−βg03×303×303×303×303×3−βa05×20],G=[−Cbn03×303×303×303×3Cbn03×303×3b03×1203×303×3I3×303×303×303×303×3I3×305×20]W=[wgbwabwrgbwrab],V=[VvVpVH],H=[HGHH],HG=[06×3I6×606×11]HH=[(hn×)03×12−(hn×)Cbn−[(hn×)Cmn](:,[1,3])]\begin{cases} \dot X=FX+GW^b\\ Z=\left[\begin{matrix} \tilde v^n_{INS}-\tilde v^n_{GNSS}\\ \tilde p^n_{INS}-\tilde p^n_{GNSS}\\ \tilde C^n_b \tilde h^{b_m}-C^n_{m'}e_2 \end{matrix}\right]=HX+V \end{cases}\\ X=\left[\begin{matrix} \phi^T&(\delta v^n)^T&(\delta p^n)^T&(\xi_r^b)^T&(\bigtriangledown_r^b)^T&\gamma^T&\delta\eta_x&\delta\eta_z \end{matrix}\right]^T\\ F=\left[\begin{matrix} 0_{3×3}&0_{3×3}&0_{3×3}&-C^n_b&0_{3×3}\\ f^n_{sf}×&0_{3×3}&0_{3×3}&0_{3×3}&C^n_b\\ 0_{3×3}&I_{3×3}&0_{3×3}&0_{3×3}&0_{3×3}&0_{15×5}\\ 0_{3×3}&0_{3×3}&0_{3×3}&-\beta_g&0_{3×3}\\ 0_{3×3}&0_{3×3}&0_{3×3}&0_{3×3}&-\beta_a\\ &&0_{5×20} \end{matrix}\right], G=\left[\begin{matrix} -C^n_b&0_{3×3}&0_{3×3}&0_{3×3}\\ 0_{3×3}&C^n_b&0_{3×3}&0_{3×3}b\\ &&0_{3×12}\\ 0_{3×3}&0_{3×3}&I_{3×3}&0_{3×3}\\ 0_{3×3}&0_{3×3}&0_{3×3}&I_{3×3}\\ &&0_{5×20} \end{matrix}\right]\\ W=\left[\begin{matrix} w^b_g\\w^b_a\\w^b_{rg}\\w^b_{ra} \end{matrix}\right],V=\left[\begin{matrix} V_v\\V_p\\V_H \end{matrix}\right],H=\left[\begin{matrix} H_G\\H_H \end{matrix}\right],H_G=\left[\begin{matrix} 0_{6×3}&I_{6×6}&0_{6×11} \end{matrix}\right]\\ H_H=\left[\begin{matrix} (h^n×)&0_{3×12}&-(h^n×)C^n_b&-[(h^n×)C^n_m]_{(:,[1,3])} \end{matrix}\right]⎩⎪⎪⎨⎪⎪⎧X˙=FX+GWbZ=⎣⎡v~INSn−v~GNSSnp~INSn−p~GNSSnC~bnh~bm−Cm′ne2⎦⎤=HX+VX=[ϕT(δvn)T(δpn)T(ξrb)T(▽rb)TγTδηxδηz]TF=⎣⎢⎢⎢⎢⎢⎢⎡03×3fsfn×03×303×303×303×303×3I3×303×303×303×303×303×303×303×305×20−Cbn03×303×3−βg03×303×3Cbn03×303×3−βa015×5⎦⎥⎥⎥⎥⎥⎥⎤,G=⎣⎢⎢⎢⎢⎢⎢⎡−Cbn03×303×303×303×3Cbn03×303×303×303×303×12I3×303×305×2003×303×3b03×3I3×3⎦⎥⎥⎥⎥⎥⎥⎤W=⎣⎢⎢⎡wgbwabwrgbwrab⎦⎥⎥⎤,V=⎣⎡VvVpVH⎦⎤,H=[HGHH],HG=[06×3I6×606×11]HH=[(hn×)03×12−(hn×)Cbn−[(hn×)Cmn](:,[1,3])]

低成本惯导的初始化对准

利用加速度计进行水平姿态对准

在静止条件下，运载体的线运动及其导数均为零：
0=Cbnfsb+gn0=C^n_bf^b_s+g^n0=Cbnfsb+gn
为了减小测量噪声和外界晃动干扰，常使用一小段时间内的平均比力进行运算,对式1进行移项可得：
fsfb=−(Cbn)Tgnf^b_{sf}=-(C^n_b)^Tg^nfsfb=−(Cbn)Tgn
展开为:
g:当地重力大小g:当地重力大小g:当地重力大小
[fsfxbfsfybfsfzb]=−[C11C12C13C21C22C23C31C32C33][00−g]\left[\begin{matrix} f^b_{sfx}\\ f^b_{sfy}\\ f^b_{sfz}\\ \end{matrix}\right]=-\left[\begin{matrix} C_{11}&C_{12}&C_{13}\\ C_{21}&C_{22}&C_{23}\\ C_{31}&C_{32}&C_{33}\\ \end{matrix}\right]\left[\begin{matrix} 0\\0\\-g \end{matrix}\right]⎣⎡fsfxbfsfybfsfzb⎦⎤=−⎣⎡C11C21C31C12C22C32C13C23C33⎦⎤⎣⎡00−g⎦⎤
得到:
[C31C32C33]=[fsfxb/gfsfyb/gfsfzb/g]\left[\begin{matrix} C_{31}\\C_{32}\\C_{33} \end{matrix}\right]=\left[\begin{matrix} f^b_{sfx}/g\\f^b_{sfy}/g\\f^b_{sfz}/g \end{matrix}\right]⎣⎡C31C32C33⎦⎤=⎣⎡fsfxb/gfsfyb/gfsfzb/g⎦⎤
CbnC^n_bCbn第三行向量为归一化重力矢量在载体坐标下的恶投影，前两行元素无法求取
构造CbnC^n_bCbn前两行的姿态阵，构造方法如下:
记姿态阵CbnC^n_bCbn的三个行向量分别为:C1,C2,C3C_1,C_2,C_3C1,C2,C3即有Cbn=[(C1)T(C2)T(C3)T]C^n_b=\left[\begin{matrix}(C_1)^T&(C_2)^T&(C_3)^T\end{matrix}\right]Cbn=[(C1)T(C2)T(C3)T]
(1)构造:C3=[C31C32C33]=(fsfb)/∣fsfb∣C_3=\left[\begin{matrix}C_{31}&C_{32}&C_{33}\end{matrix}\right]=(f^b_{sf})/|f^b_{sf}|C3=[C31C32C33]=(fsfb)/∣fsfb∣
(2)C3C_3C3为3维单位向量，其最大元素值应当不小于3/3≈0.5\sqrt{3}/3\approx 0.53/3≈0.5,当∣C31∣>0.5|C_{31}|>0.5∣C31∣>0.5时，构造C2′=[C32−C310]C_2'=\left[\begin{matrix}C_{32}&-C_{31}&0\end{matrix}\right]C2′=[C32−C310]，否则构造C2′=[0C33−C32]C_2'=\left[\begin{matrix}0&C_{33}&-C_{32}\end{matrix}\right]C2′=[0C33−C32],显然,C2′C3TC_2'C_3^TC2′C3T与原向量正交，再单位化C2=C2′/∣C2′∣C_2=C'_2/|C'_2|C2=C2′/∣C2′∣
(3)构造C1=C2×C3C_1=C_2×C_3C1=C2×C3
在地磁测量校准中会用到，Cbh=CbnC_b^h=C_b^nCbh=Cbnh表示当地水平坐标系

利用地磁测量进行方位校准
如果地磁信息可用，忽略小的磁偏角影响，以磁方位近似代替地理方位对准，若已知磁偏角参数，也可以做适当补偿提高方位精确度。

假设经过加速度计对准之后的水平姿态矩阵为CbhC_b^hCbh,真实姿态矩阵为CbnC_b^nCbn：
Chn=[cosϕ−sinϕ0sinϕcosϕ0001]C_h^n=\left[\begin{matrix} cos\phi&-sin\phi&0\\ sin\phi&cos\phi&0\\ 0&0&1 \end{matrix}\right]Chn=⎣⎡cosϕsinϕ0−sinϕcosϕ0001⎦⎤
根据地磁场关系有：
hn=CmnHm/H=[hEnhEnhUn]Th^n=C^n_mH^m/H=\left[\begin{matrix} h^n_E&h^n_E&h^n_U \end{matrix}\right]^Thn=CmnHm/H=[hEnhEnhUn]T
Cbh:水平方向投影C_b^h:水平方向投影Cbh:水平方向投影
Cbn:真实姿态阵C_b^n:真实姿态阵Cbn:真实姿态阵
h~n=Cbhh~b=[h~xhh~yhh~zh]T\tilde h^n=C^h_b\tilde h^b=\left[\begin{matrix} \tilde h^h_x&\tilde h^h_y&\tilde h^h_z \end{matrix}\right]^Th~n=Cbhh~b=[h~xhh~yhh~zh]T
ChnCbhh~b=hn即Chnh~h=hnC^n_hC_b^h\tilde h^b=h^n即C^n_h \tilde h^h=h^nChnCbhh~b=hn即Chnh~h=hn
求得：
sinϕ=−hEnh~yh+hNnh~xh(h~xh)2+(h~yh)2cosϕ=hEnh~xh+hNnh~yh(h~xh)2+(h~yh)2sin \phi=\frac{-h^n_E\tilde h^h_y+h^n_N\tilde h^h_x}{(\tilde h^h_x)^2+(\tilde h^h_y)^2}\\ cos\phi=\frac{h^n_E\tilde h^h_x+h^n_N\tilde h^h_y}{(\tilde h^h_x)^2+(\tilde h^h_y)^2}sinϕ=(h~xh)2+(h~yh)2−hEnh~yh+hNnh~xhcosϕ=(h~xh)2+(h~yh)2hEnh~xh+hNnh~yh
最后可求得重力和磁力两个矢量在不同坐标系（n系与b系）之间的测量转换关系，如下：
{−gn=Cbnfsfbhn=Cbnh~b\begin{cases} -g^n=C^n_bf^b_{sf}\\ h^n=C^n_b\tilde h^b \end{cases}{−gn=Cbnfsfbhn=Cbnh~b
在使用双矢量定姿法可同时实现水平姿态与方位对准计算

利用卫星导航进行方位校准
完成水平校准之后。如果卫星导航信息可用.
对于固定翼飞行器或者载车，其行驶方向一般沿载体纵轴方向（正前方），根据行驶轨迹的航向角或卫星测速的矢量，容易求得载体纵轴相对于地理北的方位角，再结合CbhC^h_bCbh计算获得的俯仰角和横滚角，求得完整的初始姿态阵。
对于多旋翼无人机，其飞行方向具有任意性，可以沿载体任意方向飞行，与固定翼方法不同，可以通过在水平方向上做短时直线加速度来实现。
原理如下：

比力方程可以近似为：
fsfh:水平方向投影f^h_{sf}:水平方向投影fsfh:水平方向投影
Cbh:水平方向投影，假设为已知量C_b^h:水平方向投影，假设为已知量Cbh:水平方向投影，假设为已知量
Cbn:真实姿态阵C_b^n:真实姿态阵Cbn:真实姿态阵
v˙n≈Cbnfsfb+gn=ChnCbhfsfb+gn=Chnfsfh+gn\dot v^n \approx C_b^nf^b_{sf}+g^n=C^n_hC^h_bf^b_{sf}+g^n=C^n_hf^h_{sf}+g^nv˙n≈Cbnfsfb+gn=ChnCbhfsfb+gn=Chnfsfh+gn
v˙n≠0\dot v^n \neq 0v˙n=0，用两个是时刻的卫星导航速度平均变换量求得
v‾˙≈vGNSSn(tk)−vGNSSn(tk−1)tk−tk−1\dot {\overline v}\approx\frac{v^n_{GNSS}(t_k)-v^n_{GNSS}(t_{k-1})}{t_k-t_{k-1}}v˙≈tk−tk−1vGNSSn(tk)−vGNSSn(tk−1)
求得sinϕ,cosϕsin\phi,cos\phisinϕ,cosϕ
f‾sfh=[f‾sfxhf‾sfyhf‾sfzh]在[tk−1,tk]时间内的水平坐标系比力投影\overline f^h_{sf}=\left[\begin{matrix} \overline f^h_{sfx}&\overline f^h_{sfy}&\overline f^h_{sfz} \end{matrix}\right]在[t_{k-1},t_k]时间内的水平坐标系比力投影fsfh=[fsfxhfsfyhfsfzh]在[tk−1,tk]时间内的水平坐标系比力投影
v˙n=[v‾˙Ev‾˙Nv‾˙U]\dot v^n=\left[\begin{matrix} \dot {\overline v}_{E}&\dot {\overline v}_{N}&\dot {\overline v}_{U} \end{matrix}\right]v˙n=[v˙Ev˙Nv˙U]
sinϕ=−v‾˙Ef‾sfyh+v‾˙Nf‾sfxh(f‾sfxh)2+(f‾sfyh)2cosϕ=v‾˙Ef‾sfyh+v‾˙Nf‾sfxh(f‾sfxh)2+(f‾sfyh)2sin\phi=\frac{-\dot {\overline v}_{E}\overline f^h_{sfy}+\dot {\overline v}_{N}\overline f^h_{sfx}}{(\overline f^h_{sfx})^2+(\overline f^h_{sfy})^2}\\ cos\phi=\frac{\dot {\overline v}_{E}\overline f^h_{sfy}+\dot {\overline v}_{N}\overline f^h_{sfx}}{(\overline f^h_{sfx})^2+(\overline f^h_{sfy})^2}sinϕ=(fsfxh)2+(fsfyh)2−v˙Efsfyh+v˙Nfsfxhcosϕ=(fsfxh)2+(fsfyh)2v˙Efsfyh+v˙Nfsfxh
求得ChnC_h^nChn
Chn=[cosϕ−sinϕ0sinϕcosϕ0001]C_h^n=\left[\begin{matrix} cos\phi&-sin\phi&0\\ sin\phi&cos\phi&0\\ 0&0&1 \end{matrix}\right]Chn=⎣⎡cosϕsinϕ0−sinϕcosϕ0001⎦⎤
得到;
Cbn=CnmCbhC^n_b=C^m_nC^h_bCbn=CnmCbh

捷联地平仪工作原理

运载体在大多数情况下运动平缓，利用水平加速度计计算和修正水平姿态的方法称为：地平仪/垂直陀螺仪工作模式，引入地磁，计算磁方位角，得到相对于地磁场的方位角能力，这种导航方式称为姿态航向参考系统。
低精度MEMS惯导/卫星组合导航系统的速度和位置主要依赖于卫星，如果卫星导航信号长时间不可用，MEMS则无法进行速度和位置导航，误差会发散。
在特定情况下，可对加速度计输出做判别，当飞行器处于悬停、匀速或低加速度状态时，经常性的对水平姿态进行修正，有可能长时间保持MEMS惯导系统姿态稳定可用，为运载体提供一定精度的水平姿态参考。

在低加速度机动条件下，将惯导比力方程及其误差方程式近似如下：
v˙n=fsfn+gnδv˙n=fsfn×ϕ\dot v^n=f^n_{sf}+g^n\\ \delta \dot v^n=f^n_{sf}×\phiv˙n=fsfn+gnδv˙n=fsfn×ϕ
低精度的MEMS无法长时间计算运载体的速度，只能将其视为平稳运行的，MEMS检测到水平加速度，一般是水平失准角引起的，反过来，就可以利用水平失准角计算加速度修正水平失准角，迫使计算速度平稳。
在速度平稳情况下，加速度与加速度误差含义一致，令等式1中两式相等，可得：
fsfn+gn=fsfn×ϕf^n_{sf}+g^n=f^n_{sf}×\phifsfn+gn=fsfn×ϕ
低机动条件下，fsfn≈[00g]Tf^n_{sf}\approx \left[\begin{matrix}0&0&g\end{matrix}\right]^Tfsfn≈[00g]T,得到分量形式如下：
gn=[00−g]g^n= \left[\begin{matrix} 0\\0\\-g \end{matrix}\right]gn=⎣⎡00−g⎦⎤
ϕ=[ϕEϕNϕU]\phi=\left[\begin{matrix} \phi_E\\\phi_N\\\phi_U \end{matrix}\right]ϕ=⎣⎡ϕEϕNϕU⎦⎤
[fEfNfU]+[00−g]=[00g]+[ϕEϕNϕU]\left[\begin{matrix} f_E\\ f_N\\ f_U \end{matrix}\right]+ \left[\begin{matrix} 0\\0\\-g \end{matrix}\right]=\left[\begin{matrix} 0\\0\\g \end{matrix}\right]+\left[\begin{matrix} \phi_E\\\phi_N\\\phi_U \end{matrix}\right] ⎣⎡fEfNfU⎦⎤+⎣⎡00−g⎦⎤=⎣⎡00g⎦⎤+⎣⎡ϕEϕNϕU⎦⎤
得到：
{fE=−gϕNfN=gϕEfU=g\begin{cases} f_E=-g\phi_N\\ f_N=g\phi_E\\ f_U=g \end{cases}⎩⎪⎨⎪⎧fE=−gϕNfN=gϕEfU=g
第4式可知，水平比力只提供水平失准角修正信息(ϕE,ϕN)\phi_E,\phi_N)ϕE,ϕN))，不能用于计算方位失准角ϕU\phi_UϕU,这里假设ϕU\phi_UϕU=0,因而有:
e=[001]:天向z轴单位矢量e=\left[\begin{matrix} 0\\0\\1 \end{matrix}\right]:天向z轴单位矢量e=⎣⎡001⎦⎤:天向z轴单位矢量
∣fsfn∣≈g:低机动时的比力模值|f^n_{sf}| \approx g:低机动时的比力模值∣fsfn∣≈g:低机动时的比力模值
{ϕE=fN/gϕN=−fE/gϕU=0即ϕ=1gfsfn×e3\begin{cases} \phi_E=f_N/g\\ \phi_N=-f_E/g\\ \phi_U=0\\ \end{cases}即 \phi=\frac{1}{g}f^n_{sf}×e_3⎩⎪⎨⎪⎧ϕE=fN/gϕN=−fE/gϕU=0即ϕ=g1fsfn×e3
对第5式等式左右两边同乘CnbC^b_nCnb,得到：
C3:Cbn的第三行向量C_3:C_b^n的第三行向量C3:Cbn的第三行向量
ϕb:失准角在b系的投影\phi^b:失准角在b系的投影ϕb:失准角在b系的投影
ϕb=Cnbϕ=Cnb(1gfsfn×e3)=1g(Cnbfsfn)×(Cnbe3)=1gfsfb×C3T=fsfb∣fsfb∣×C3T\phi^b=C^b_n \phi=C^b_n(\frac{1}{g}f^n_{sf}×e_3)=\frac{1}{g}(C_n^bf^n_{sf})×(C^b_ne_3)=\frac{1}{g}f^b_{sf}×C_3^T=\frac{f^b_{sf}}{|f^b_{sf}|}×C_3^Tϕb=Cnbϕ=Cnb(g1fsfn×e3)=g1(Cnbfsfn)×(Cnbe3)=g1fsfb×C3T=∣fsfb∣fsfb×C3T
结合失准角ϕb\phi^bϕb与陀螺仪角增量输出相结合，得到加权的失准角修正四元数姿态更新算法：
Δθ~m在时间段[tm−1,tm],内的陀螺仪角增量输出\Delta \tilde\theta_m在时间段[t_{m-1},t_m],内的陀螺仪角增量输出Δθ~m在时间段[tm−1,tm],内的陀螺仪角增量输出
Δθm′：表示修正后的角增量\Delta \theta'_m ：表示修正后的角增量Δθm′：表示修正后的角增量
α∈[0,1]:失准角修正系数,α越小抗干扰能力越小\alpha \in[0,1]:失准角修正系数,\alpha 越小抗干扰能力越小α∈[0,1]:失准角修正系数,α越小抗干扰能力越小
Qb(m)n=Qb(m−1)n。Qb(m)b(m−1)Qb(m)b(m−1)=[cos∣Δθm′∣2Δθm′∣Δθm′∣sinΔ∣θm′∣2]Δθm′=Δθ~m+αϕbQ^n_{b(m)}=Q^n_{b(m-1)}。Q^{b(m-1)}_{b(m)}\\ Q^{b(m-1)}_{b(m)}=\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]\\ \Delta \theta'_m=\Delta \tilde \theta_m+\alpha \phi^b Qb(m)n=Qb(m−1)n。Qb(m)b(m−1)Qb(m)b(m−1)=[cos2∣Δθm′∣∣Δθm′∣Δθm′sin2Δ∣θm′∣]Δθm′=Δθ~m+αϕb
在不存在加速度机动条件可以使用上述公式，求解，如何判断如下：
fsfb:加速度计输出的模值f^b_{sf}:加速度计输出的模值fsfb:加速度计输出的模值
β1:预设的加速度阈值\beta_1:预设的加速度阈值β1:预设的加速度阈值
β2:预设的模值阈值\beta_2:预设的模值阈值β2:预设的模值阈值
fH=fE2+fN2：水平加速度计模值f_H=\sqrt{f_E^2+f^2_N}：水平加速度计模值fH=fE2+fN2：水平加速度计模值
当∣∣fsfb∣−g∣<β1||f^b_{sf}|-g|<\beta_1∣∣fsfb∣−g∣<β1时，不存在加速度机动。
（1）∣∣fsfb∣−g∣<β1||f^b_{sf}|-g|<\beta_1∣∣fsfb∣−g∣<β1且fH<β2f_H<\beta_2fH<β2时，认定没有加速度机动，可以使用上述算法利用fsfbf^b_{sf}fsfb或估计失准角ϕ\phiϕ
（2）fH≥β2f_H \geq \beta_2fH≥β2时，姿态阵中ϕb\phi^bϕb比较大，或者运载体中存在较大的水平加速度机动：如果出现时间过长运载体可能处于连续转弯或盘旋运动，如果不是则是其失准角过大，需要对姿态进行快速矫正