INS/GNSS组合导航（三）-SINS的机械编排

上节介绍了GNSS/INS组合时常用的几种坐标系及其之间的转换，这节就介绍一下SINS的机械编排。SINS即为捷联惯导系统，所谓的机械编排，即为从INS的加速度计和陀螺仪的真实输出值计算推导载体所在位置、速度和姿态的过程。

前言

在进行正式解算之前，我们需要明确，INS的三轴加速度计和三轴陀螺仪到底输出的什么信息。三轴加速度计输出的信息叫比力。何为比力？即假如载体自由落体时，真正的加速度是载体z方向上的重力加速度，但是实际上的加速度计并不能测量中立加速度。所以将这种不包含重力加速度的加速度测量值就称为比力。三轴陀螺仪输出的则是三个轴向的角速度值，这个角速度确切的说即为载体坐标系相对于惯性坐标系的角速度在载体坐标系下的投影。

在惯性导航解算过程中，姿态解算都是非常重要且十分基础的一步。这是因为，INS的输出值都是相对于惯性坐标系而言的，但是在INS导航中，常常需要获得相对于当地导航坐标系的位置和速度等信息，这就需要根据车辆的姿态进行必要的转换，所以姿态的解算精度直接影响到位置和速度的解算精度。因此在接下来，首先介绍姿态的解算。

1 姿态解算

用四元数表示的姿态微分方程可表示如下：
q˙bn=12qbn×ωnbb=12(q0+q1i+q2j+q3k)×(ωxi+ωyj+ωzk)=12[0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0][q0q1q2q3]=Ωnbb⋅qbn\dot q^n_b=\frac{1}{2}q^n_b\times \omega^b_{nb}\\ =\frac{1}{2}(q_0+q_1i+q_2j+q_3k)\times(\omega_xi+\omega_yj+\omega_zk)\\=\frac{1}{2}\begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \begin{bmatrix}q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} \\ =\Omega^b_{nb} \cdot q^n_bq˙bn=21qbn×ωnbb=21(q0+q1i+q2j+q3k)×(ωxi+ωyj+ωzk)=21⎣⎢⎢⎡0ωxωyωz−ωx0−ωzωy−ωyωz0−ωx−ωz−ωyωx0⎦⎥⎥⎤⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤=Ωnbb⋅qbn

其中，qbnq^n_bqbn描述的是载体坐标系到导航坐标系的四元数表示的姿态关系，ωnbb\omega^b_{nb}ωnbb是相对旋转角速度。ωnbb\omega^b_{nb}ωnbb并不是IMU的直接输出量，但是可以经过转换得到。
ωnbb=ωibb−ωinb=ωibb−Cnb(ωien+ωenn)\omega^b_{nb}=\omega^b_{ib}-\omega^b_{in}\\=\omega^b_{ib}-C^b_n(\omega^n_{ie}+\omega^n_{en})ωnbb=ωibb−ωinb=ωibb−Cnb(ωien+ωenn)
式中，ωibb\omega^b_{ib}ωibb是IMU的直接输出，即为载体坐标系相对i系（惯性系）的角速度在b系（载体系）下的投影。ωinb\omega^b_{in}ωinb是导航系相对于惯性系的角速度在载体坐标系下的投影。ωenn\omega^n_{en}ωenn是导航坐标系相对于地球系的角速度在导航坐标系中的投影，ωien\omega^n_{ie}ωien是地球自转角速度在导航系中的投影。ωenn\omega^n_{en}ωenn和ωien\omega^n_{ie}ωien的计算公式如下：
ωien=[ωie⋅cosL0−ωie⋅sinL]\omega^n_{ie}=\begin{bmatrix} \omega_{ie} \cdot cosL \\ 0 \\ -\omega_{ie} \cdot sinL \end{bmatrix}ωien=⎣⎡ωie⋅cosL0−ωie⋅sinL⎦⎤
ωien=[vynRE+h−vxnRN+h−vyntanLRE+h]\omega^n_{ie}=\begin{bmatrix} \frac{v^n_y}{R_E+h} \\ -\frac{v^n_x}{R_N+h} \\ -\frac{v^n_ytanL}{R_E+h} \end{bmatrix}ωien=⎣⎢⎡RE+hvyn−RN+hvxn−RE+hvyntanL⎦⎥⎤
ωie\omega_{ie}ωie是地球角速度，是个很小的常量。L和h分别表示载体的维度和海拔高。vxnv^n_xvxn和vynv^n_yvyn表示载体在导航n系下的速度。RER_ERE和RNR_NRN分别表示地球在东西方向和南北方向的曲率半径，可通过下式计算：
RE=R01−e2sin2LR_E=\frac{R_0}{\sqrt{1-e^2sin^2L}}RE=1−e2sin2LR0
RN=R0(1−e2)(1−e2sin2L)32R_N=\frac{R_0(1-e^2)}{(1-e^2sin^2L)^{\frac{3}{2}}}RN=(1−e2sin2L)23R0(1−e2)
其中，R0R_0R0和e分别是地球赤道半径和地球偏心率。
根据这些公式，便可以对微分方程进行求解从而获得姿态。

2 速度和位置解算

由于IMU输出的比力信息是沿着载体坐标系的三个分量，因此需要将比力信息通过载体的姿态信息CbnC^n_bCbn转换到导航坐标系。
fn=Cbnfbf^n=C^n_bf^bfn=Cbnfb
其中，fbf^bfb和fnf^nfn分别表示的是在载体系和导航系下的比力。

而IMU输出的比力和角速度都是在一个离散的采样间隔内的均值，一般在求解时，在对比力进行坐标系的转换时，应该利用两个采样点输出的姿态矩阵的均值进行转换，即
fn=12(Cbn(−)+Cbn(+))fbf^n=\frac{1}{2}(C^n_b(-)+C^n_b(+))f^bfn=21(Cbn(−)+Cbn(+))fb
这里不加推导的给出IMU系统的速度微分方程
fn+g=v˙n+(ωien+ωenn)×vnf^n+g=\dot v^n+(\omega^n_{ie}+\omega^n_{en})\times v^nfn+g=v˙n+(ωien+ωenn)×vn
利用上式和IMU输出的比力信息可以解算车辆速度，进而确定车辆位置。该方程中中g是导航坐标系下的重力加速度。vnv^nvn是载体速度在导航坐标系下的投影。v˙n\dot v^nv˙n是车辆相对于地固系的相对加速度(ωien+ωenn)×vn(\omega^n_{ie}+\omega^n_{en})\times v^n(ωien+ωenn)×vn是地球的自转和载体相对于地球转动而引起的牵连加速度以及科氏加速度。

将上式中的各矢量表示成列向量和矩阵的形式，将矢量叉乘表示成反对称矩阵的形式：
[v˙xnv˙ynv˙zn]=[fxnfynfzn]+[00g]−[0−2ωie,zn−ωen,zn2ωie,yn+ωen,yn2ωie,zn+ωen,zn0−2ωie,xn−ωen,xn−2ωie,yn−ωen,yn2ωie,xn+ωen,xn0][vxnvynvzn]\begin{bmatrix} \dot v^n_x \\ \dot v^n_y \\ \dot v^n_z \end{bmatrix}=\begin{bmatrix} f^n_x \\ f^n_y \\ f^n_z \end{bmatrix}+\begin{bmatrix} 0 \\ 0 \\ g \end{bmatrix}-\begin{bmatrix} 0 & -2\omega^n_{ie,z}-\omega^n_{en,z} & 2\omega^n_{ie,y}+\omega^n_{en,y} \\ 2\omega^n_{ie,z}+\omega^n_{en,z} & 0 & -2\omega^n_{ie,x}-\omega^n_{en,x} \\ -2\omega^n_{ie,y}-\omega^n_{en,y} & 2\omega^n_{ie,x}+\omega^n_{en,x} & 0 \end{bmatrix} \begin{bmatrix} v^n_x \\ v^n_y \\ v^n_z \end{bmatrix} ⎣⎡v˙xnv˙ynv˙zn⎦⎤=⎣⎡fxnfynfzn⎦⎤+⎣⎡00g⎦⎤−⎣⎡02ωie,zn+ωen,zn−2ωie,yn−ωen,yn−2ωie,zn−ωen,zn02ωie,xn+ωen,xn2ωie,yn+ωen,yn−2ωie,xn−ωen,xn0⎦⎤⎣⎡vxnvynvzn⎦⎤
一般情况下，车辆的运行都是南北或东西向，因此可以减少向量的阶次。仅考虑二阶。对上式的微分方程进行求解即可得到速度信息。

上式可以求解每个采样点车辆的速度情况，如果位置采用经纬度的方式表述，位置的时间倒数就是沿着曲线切线方向的速度和曲线曲率半径的比值，由此便可以得到车辆的位置微分函数。
L˙=vxnRN+h\dot L = \frac{v^n_x}{R_N+h}L˙=RN+hvxn
λ˙=vyn(RE+h)cosL\dot \lambda=\frac{v^n_y}{(R_E+h)cosL}λ˙=(RE+h)cosLvyn
上式各项都已经介绍过了，利用上式即可进行微分求解获得载体的位置。

3 重力模型计算

在速度微分方程中，我们需要知道当地的重力加速度。而地球上的重力加速和地理位置以及地面的高度有关。通常对于一般的惯性导航模型来说，我们常采用正常重力模型进行计算。其严格公式如下：
g(L)=agecos2L+bgpsin2La2cos2L+b2sin2Lg(L)=\frac{ag_ecos^2L+bg_psin^2L}{\sqrt{a^2cos^2L+b^2sin^2L}}g(L)=a2cos2L+b2sin2Lagecos2L+bgpsin2L
其中a为椭球长半轴，b为椭球短半轴，geg_ege和gpg_pgp分别为赤道和极点的正常重力。对于不同的参考椭球，上式的参数会有略微的差别，但是差异很小，一般都可以忽略。其中，在CGCS2000参考椭球下的相应参数为

a=6378137ma=6378137ma=6378137m
b=6356752.3141mb=6356752.3141mb=6356752.3141m
ge=9.7803253361m/s2g_e=9.7803253361m/s^2ge=9.7803253361m/s2
gp=9.8321849379m/s2g_p=9.8321849379m/s^2gp=9.8321849379m/s2
上式只考虑了地理纬度的影响，实际上还要考虑高程的影响：
g(h,L)=g(L)(1−2a(1+f+m−2fsin2L)h+3a2h2)g(h,L)=g(L)(1-\frac{2}{a}(1+f+m-2fsin^2L)h+\frac{3}{a^2}h^2)g(h,L)=g(L)(1−a2(1+f+m−2fsin2L)h+a23h2)
其中
m=ωie2a2bkMm=\frac{\omega^2_{ie}a^2b}{kM}m=kMωie2a2b
f为椭球变率，kM为引力常数，其值为：
f=1/298.257222101f=1/298.257222101f=1/298.257222101
kM=3.986004418x1014m3/s2kM=3.986004418 x 10^14m^3/s^2kM=3.986004418x1014m3/s2
经过正常重力计算计算的重力是相对于正常椭球来说的，对于大地水准面，地球上一点的垂线与参考椭球面上的垂线并不一致，从而引起垂线偏差。因此对于高精度的INS或重力测量，需要对重力误差进行修正。