-
摘要: 针对车载全球导航卫星系统/惯性导航系统（global navigation satellite system/inertial navigation system，GNSS/INS）组合导航中卫星信号中断，惯性导航系统单独导航误差积累较大的问题，提出了附加载体运动条件约束的卡尔曼（Kalman）滤波解算方法。通过利用载体固有的运动约束，包括近似高程约束、近似速度约束和近似姿态约束，减少载体自由度和模型参数；通过引入新的观测类型，增加观测冗余，可以加强Kalman滤波解，提高在GNSS信号中断时组合导航系统的定位精度，实现无缝导航。
-
关键词:
- GNSS/INS组合导航 /
- 附加观测约束 /
- Kalman滤波
Abstract: A Kalman filter with kinematic motion constraints is presented to address the problem of increasing position error from INS on s vehicle when the GNSS/INS integrated navigation system is without a GNSS signal. By using the inherent motion constraints, such as approximation height constraint, velocity constraint, and attitude constraint, the freedom of motion of a vehicle will be controlled and the unknown model parameters will be reduced. Both motion constraints and new kind measurements will strengthen the solution in constraint Kalman filtering. The positioning accuracy of the GNSS/INS integrated navigation system can be improved when the GNSS signal is blocked, achieving continuous navigation. -
目前车辆导航较常用的是全球卫星导航系统/惯性导航系统(global navigation satellite system/inertial navigation system，GNSS/INS)组合导航，这种方式可以结合卫星导航系统精度高与惯性导航系统自主连续的优点。但是当汽车载体进入高楼林立的街道，或进入地下车库，通过隧道，将会导致GNSS信号中断。利用低精度微机电系统(micro electro mechanical system，MEMS)的惯性导航系统可以实现连续导航，但是其误差漂移较大，常使组合导航系统定位精度随时间显著降低。例如北京的分水岭隧道，标称隧道长度3 400 m，采用低精度INS的车载导航的汽车通过隧道约有3~5 min的GNSS信号中断，这将导致组合导航定位误差达数千米。
GNSS信号中断必将导致组合导航系统定位精度显著降低。目前主要有两种改善方法，一种是根据载体的固有运动规律，构造具有运动约束(可以称之为内部约束)的组合导航滤波方程；另一种是利用冗余传感器的观测信息构造附加观测约束的滤波方程(可以称之为外部约束)。这两种约束具有提高导航定位精度和可用性的功能。具有约束条件的卡尔曼(Kalman)滤波[1-2]和具有约束条件的抗差滤波及富有条件的自适应滤波模型[3-4]已经有相应理论和计算方法，此外，具有内部约束和外部约束的参数估计及其性质也有论文做过讨论[5]，但是，针对具体近似运动约束条件的组合导航实际性能分析并不充分。
在内部约束方面，针对较低精度的全球定位系统/惯性导航系统组合导航系统，Dissanayake[6]提出了非完整速度约束的思想，即载体侧向和法向速度均为0的约束；Godha[7-8]基于短时间车辆高程变化较小提出了高程约束，但对水平定位精度改善不明显；Klein[9]针对城市环境，提出了姿态角速度约束，水平姿态角初值选取为0，但是由于惯性测量单元(inertial measurement unit，IMU)存在安装误差，初值为0会导致定位结果恶化；Niu[10-11]提出了航向角速度约束，可以改善低动态车辆导航精度，并分析了非完整约束观测值的可观测性，为不同的约束条件提供了观测值选取策略；柴艳菊[12]将速度约束和高程约束应用到GPS/INS紧组合中，使约束算法在卫星数目不足时可用，拓宽了其使用范围。
在外部约束方面，吴富梅[13]针对GNSS信号中断，应用里程计观测值附加速度约束，使定位精度得到较大改善；Thienel[14]提出了利用磁强计以测定飞行器航向角的方法，但并没有给出应用于车载组合导航的算法；Yu[15]利用地图匹配技术附加路网约束，并对GPS信号中断时地图匹配定位性能进行了验证。这些观测约束或外部约束均可有效提升定位精度。
本文充分利用外部约束验证各类约束对车载导航性能改善情况。约束包括附加里程计观测值的完整的速度约束、利用磁罗盘观测值构建的完整的姿态约束；提出了更完善的高程约束和姿态约束初值选取方案；最后利用实际观测数据，对多种约束方案进行了比较分析。
1 Kalman滤波
常用GNSS/INS松组合Kalman滤波模型一般为：
$$ \left\{ \begin{array}{l} {\mathit{\boldsymbol{X}}_k} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k,k - 1}}{\mathit{\boldsymbol{X}}_{k - 1}} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{\mathit{\boldsymbol{W}}_{k - 1}}\\ {\mathit{\boldsymbol{L}}_k} = {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{ \boldsymbol{\varDelta} }}_k} \end{array} \right. $$ (1) 式中，Xk=[φ3×1T δv3×1T δp3×1T ε3×1T d3×1T]kT方括号内分别对应INS的失准角、速度误差、位置误差、陀螺仪零漂和加速度计零偏；下标k, k-1分别为当前历元和前一历元；Φk, k-1=I+F(t)dt+F2(t)dt2/2，为状态转移函数Ft的二阶离散化[16]，F(t)表达式可参看文献[8]；Γk-1为系统噪声驱动矩阵；Wk-1为系统噪声向量；Lk为观测向量；Hk为观测矩阵；Δk观测噪声向量。
标准Kalman滤波解算模型如下：
$$ \left\{ \begin{array}{l} {{\mathit{\boldsymbol{\bar X}}}_k} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k,k - 1}}{{\mathit{\boldsymbol{\hat X}}}_{k - 1}}\\ {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar X}}}_k}}} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k,k - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{\mathit{\boldsymbol{X}}_k}}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k,k - 1}^{\rm{T}} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{\mathit{\boldsymbol{Q}}_k}\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}^{\rm{T}}\\ {\mathit{\boldsymbol{K}}_k} = {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar X}}}_k}}}\mathit{\boldsymbol{H}}_k^{\rm{T}}{\left( {{\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar X}}}_k}}}\mathit{\boldsymbol{H}}_k^{\rm{T}} + {\mathit{\boldsymbol{R}}_k}} \right)^{ - 1}}\\ {{\mathit{\boldsymbol{\hat X}}}_k} = {{\mathit{\boldsymbol{\bar X}}}_k} + {\mathit{\boldsymbol{K}}_k}\left( {{\mathit{\boldsymbol{L}}_k} - {\mathit{\boldsymbol{H}}_k}{{\mathit{\boldsymbol{\bar X}}}_k}} \right)\\ {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\hat X}}}_k}}} = \left( {\mathit{\boldsymbol{I}} - {\mathit{\boldsymbol{K}}_k}{\mathit{\boldsymbol{H}}_k}} \right){\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\bar X}}}_k}}} \end{array} \right. $$ (2) 式中，Xk和$\mathit{\boldsymbol{\hat X}}$k分别为状态预测向量和状态估值向量；ΣXk和Σ$\mathit{\boldsymbol{\hat X}}$k分别为与其下标对应的状态向量的协方差矩阵；Kk为滤波增益矩阵；Qk和Rk分别为系统噪声向量和观测噪声向量的协方差矩阵；I为对应阶数的单位矩阵。
构造附加的约束条件为：
$$ {\mathit{\boldsymbol{B}}_k}{{\mathit{\boldsymbol{\hat X}}}_k} - {\mathit{\boldsymbol{D}}_k} = 0 $$ (3) 式中，Bk为附加约束的观测矩阵；Dk为约束观测理论值。
文献[3]中，杨元喜指出附加约束的Kalman滤波分步递推解组合了两步滤波过程：第一步是没有约束条件的标准Kalman滤波；第二步是使用约束条件的修正过程。由此，我们得到附加约束的Kalman滤波分步递推解的第二步为：
$$ \left\{ \begin{array}{l} {{\mathit{\boldsymbol{\hat X}}}_k} = \mathit{\boldsymbol{\hat X}}_k^0 + {\mathit{\boldsymbol{K}}_k}\left( {{\mathit{\boldsymbol{D}}_k} - {\mathit{\boldsymbol{B}}_k}\mathit{\boldsymbol{\hat X}}_k^0} \right)\\ {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{{\mathit{\boldsymbol{\hat X}}}_k}}} = \left( {\mathit{\boldsymbol{I}} - {\mathit{\boldsymbol{K}}_k}{\mathit{\boldsymbol{B}}_k}} \right){\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{\mathit{\boldsymbol{\hat X}}_k^0}}\\ {\mathit{\boldsymbol{K}}_k} = {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{\mathit{\boldsymbol{\hat X}}_k^0}}\mathit{\boldsymbol{B}}_k^{\rm{T}}{\left( {{\mathit{\boldsymbol{B}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{\mathit{\boldsymbol{\hat X}}_k^0}}\mathit{\boldsymbol{B}}_k^{\rm{T}}} \right)^{ - 1}} \end{array} \right. $$ (4) 式中，$\mathit{\boldsymbol{\hat X}}$k0为第一步标准Kalman滤波估值；Σ$\mathit{\boldsymbol{\hat X}}$k0为与之对应的协方差矩阵。
若将约束条件当作具有零协方差矩阵Dk的观测方程，则式(4) 也是一个标准递推解[3]。
根据不同情况，选取不同的约束类型进行状态估计，而且若有附加传感器，还将引入新的观测量，构造新的观测方程，详见§2。
2 运动约束算法
2.1 高程约束(height constraint，HC)
本文中约束均为松约束，类似新增观测，约束方程含有误差。假设车载系统在平坦地区运动(尤其是城市)，其高程短时间内不会发生较大变化[7]。天向速度如图 1所示：(注：图 1中天向速度不为0，只为区别载体的天向速度和法向速度)。
则约束方程为：
$$ \left\{ \begin{array}{l} h_k^n = {h_0} + {\eta _h}\\ v_U^n = 0 + {\eta _U} \end{array} \right.,\left\{ \begin{array}{l} E\left( {{\eta _h}} \right) = 0\\ E\left( {{\eta _U}} \right) = 0 \end{array} \right. $$ (5) 式中，h为高程；下标0为初始时刻；U为天向上标；n为导航坐标系；ηh、ηU分别为载体在导航坐标系下的高程位置噪声和天向速度噪声；E ()表示期望值。
由式(5) 得到约束条件的观测矩阵：
$$ {\mathit{\boldsymbol{B}}_k} = \left[ {\begin{array}{*{20}{c}} {{{\bf{0}}_{1 \times 5}}}&1&{{{\bf{0}}_{1 \times 9}}}\\ {{{\bf{0}}_{1 \times 8}}}&1&{{{\bf{0}}_{1 \times 6}}} \end{array}} \right] $$ (6) 在构造约束观测向量时，可以采用固定高程(常值)进行约束[9]，但实际中高程的观测值并不是常值，因此以前一历元的高程及天向速度来约束当前历元[12]，则约束观测向量为：
$$ {\mathit{\boldsymbol{D}}_k} = \left[ {\begin{array}{*{20}{c}} {\delta v_U^n}\\ {\delta {h^n}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {v_{U,k}^n}\\ {h_k^n} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} {v_{U,k - 1}^n}\\ {h_{k - 1}^n} \end{array}} \right] $$ (7) 记ΣHC为高程约束观测向量的协方差矩阵，则：
$$ {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{HC}} = \left[ {{\rm{diag}}\left( {\begin{array}{*{20}{c}} {\eta _h^2}&{\eta _U^2} \end{array}} \right)} \right] $$ (8) 式中，diag()表示对角矩阵。
若载体装有气压计(barometer，简称为bar)，则载体的高程和高程变化率理论上可直接观测：
$$ \left\{ \begin{array}{l} h_k^n = h_{{\rm{bar}}}^n + {\eta _h}\\ v_U^n = \dot h_{{\rm{bar}}}^n + {\eta _U} \end{array} \right.,\left\{ \begin{array}{l} E\left( {{\eta _h}} \right) = 0\\ E\left( {{\eta _U}} \right) = 0 \end{array} \right. $$ (9) 式中，$\dot h$barn、hbarn分别为气压计输出的导航坐标系下当前历元的高程变化率和高程。
附加高程观测条件的Kalman滤波观测矩阵Bk不变，约束观测向量Dk为：
$$ {\mathit{\boldsymbol{D}}_k} = \left[ {\begin{array}{*{20}{c}} {\delta v_U^n}\\ {\delta {h^n}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {v_{U,k}^n}\\ {h_k^n} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} {\dot h_{{\rm{bar}}}^n}\\ {h_{{\rm{bar}}}^n} \end{array}} \right] $$ (10) 高程约束观测向量协方差ΣHC形式不变，取值理论上取决于气压计参数。
由于高程参数为直接可测参数，可直接修正高程。
2.2 速度约束
对于车载纯惯性导航，在载体坐标系下，认为汽车通常不出现侧滑，且无剧烈颠簸，即载体的侧向与法向速度为0[6]，如图 2所示(图中载体侧向速度未画出)。
则约束方程为：
$$ \left\{ \begin{array}{l} v_x^b = 0 + {\eta _x}\\ v_z^b = 0 + {\eta _z} \end{array} \right.,\left\{ \begin{array}{l} E\left( {{\eta _x}} \right) = 0\\ E\left( {{\eta _z}} \right) = 0 \end{array} \right. $$ (11) 式中，v为速度，下标x、z分别为侧向和法向，上标b为载体坐标系；ηx、ηz分别为载体在载体坐标系下的侧向与法向的速度噪声。
利用经典GNSS/INS组合导航滤波方程，需将导航坐标系中的速度误差转换到载体坐标系下：
$$ {\mathit{\boldsymbol{v}}^b} = \mathit{\boldsymbol{C}}_n^b{\mathit{\boldsymbol{v}}^n} $$ (12) 式中，Cnb指从导航系到载体系的方向余弦矩阵。
对式(12) 进行全微分，得到载体坐标系下速度误差方程：
$$ \begin{array}{l} \delta {\mathit{\boldsymbol{v}}^b} = \delta \mathit{\boldsymbol{C}}_n^b{\mathit{\boldsymbol{v}}^n} + \mathit{\boldsymbol{C}}_n^b\delta {\mathit{\boldsymbol{v}}^n} = \mathit{\boldsymbol{C}}_n^b{\mathit{\boldsymbol{ \boldsymbol{\varOmega} }}^n}{\mathit{\boldsymbol{v}}^n} + \mathit{\boldsymbol{C}}_n^b\delta {\mathit{\boldsymbol{v}}^n} = \\ \;\;\;\;\;\;\;\;\;\left( {\mathit{\boldsymbol{C}}_n^b \times {\mathit{\boldsymbol{v}}^n}} \right){\mathit{\boldsymbol{\varphi }}^n} + \mathit{\boldsymbol{C}}_n^b\delta {\mathit{\boldsymbol{v}}^n} \end{array} $$ (13) 式中，Ωn为φn的反对称矩阵。
由式(11) 得到约束条件的观测矩阵：
$$ {\mathit{\boldsymbol{B}}_k} = {\left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{C}}_n^b \times {\mathit{\boldsymbol{v}}^n}}&{\mathit{\boldsymbol{C}}_n^b}&{{{\bf{0}}_{3 \times 9}}} \end{array}} \right]_{\left( {1,3} \right)}} $$ (14) 式中，下标(1, 3) 代表矩阵的第1行和第3行。
速度的约束观测向量为：
$$ {\mathit{\boldsymbol{D}}_k} = \left[ {\begin{array}{*{20}{c}} {\delta v_x^b}\\ {\delta v_z^b} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {v_{{\rm{INS}},x}^b}\\ {v_{{\rm{INS}},z}^b} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} 0\\ 0 \end{array}} \right] $$ (15) 式中，vINS, xb、vINS, zb分别为车载INS解算的载体侧向与法向速度。
记ΣVC速度为约束观测向量的协方差矩阵，则：
$$ {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{\rm{VC}}}} = \left[ {{\rm{diag}}\left( {\begin{array}{*{20}{c}} {\eta _x^2}&{\eta _z^2} \end{array}} \right)} \right] $$ (16) 若载体装有里程计(odometer，简称为odo)，则可以测出载体的前向速度[13]：
$$ v_y^b = v_{{\rm{odo}}}^b + {\eta _{{\rm{odo}}}},E\left( {{\eta _{{\rm{odo}}}}} \right) = 0 $$ (17) 式中，vodob为里程计测得的载体前向速度; ηodo为前向速度的测量噪声。
附加速度条件的Kalman滤波观测矩阵Bk和约束观测向量Dk为：
$$ \left\{ \begin{array}{l} {\mathit{\boldsymbol{B}}_k} = \left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{C}}_n^b \times {\mathit{\boldsymbol{v}}^n}}&{\mathit{\boldsymbol{C}}_n^b}&{{{\bf{0}}_{3 \times 9}}} \end{array}} \right]\\ {\mathit{\boldsymbol{D}}_k} = \left[ {\begin{array}{*{20}{c}} {\delta v_x^b}\\ {\delta v_y^b}\\ {\delta v_z^b} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {v_{{\rm{INS}},x}^b}\\ {v_{{\rm{INS}},y}^b}\\ {v_{{\rm{INS}},z}^b} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} 0\\ {v_{{\rm{odo}}}^b}\\ 0 \end{array}} \right] \end{array} \right. $$ (19) 速度约束(velocity constraint，VC)观测向量协方差ΣVC为：
$$ {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{\rm{VC}}}} = \left[ {{\rm{diag}}\left( {\begin{array}{*{20}{c}} {\eta _x^2}&{\eta _{{\rm{odo}}}^2}&{\eta _z^2} \end{array}} \right)} \right] $$ (20) 其中前向速度噪声方差取值取决于里程计性能参数。
由于只有速度参数为直接可测参数，因此使用修正后的速度重新进行位置推算[13]。
2.3 姿态约束
对于载体在平坦地区运动，尤其是车辆，其俯仰角和滚转角变化较小[9]，即载体的水平姿态角变化率约为0。则约束方程为：
$$ \left\{ \begin{array}{l} {{\dot \theta }^n} = 0 + {\varepsilon _x} + {\eta _{{\rm{pit}}}}\\ {{\dot \gamma }^n} = 0 + {\varepsilon _y} + {\eta _{{\rm{rol}}}} \end{array} \right.,\left\{ \begin{array}{l} E\left( {{\eta _{{\rm{pit}}}}} \right) = 0\\ E\left( {{\eta _{{\rm{rol}}}}} \right) = 0 \end{array} \right. $$ (21) 式中，θ、γ分别为载体俯仰角和滚转角；εx、εy分别为载体俯仰和滚转陀螺仪零漂；ηpit、ηrol分别为载体在导航坐标系下的俯仰角和滚转角的角速度噪声(可以认为是陀螺仪的随机游走)，下标pit和rol分别为俯仰角和滚转角简称。
由式(21) 得到约束条件的观测矩阵：
$$ {\mathit{\boldsymbol{B}}_k} = \left[ {\begin{array}{*{20}{c}} {{\rm{diag}}\left( {\begin{array}{*{20}{c}} 1&1 \end{array}} \right)}&{{{\bf{0}}_{2 \times 13}}} \end{array}} \right] $$ (22) 构造约束观测时，通常采用固定水平姿态角(水平姿态角为0) 进行约束[9]，但实际中由于安装误差等原因，INS与载体之间存在常值误差角，因此本文以前一历元的水平姿态约束(attitude constraint，AC)当前历元，约束观测向量为：
$$ {\mathit{\boldsymbol{D}}_k} = \left[ {\begin{array}{*{20}{c}} {\delta {\theta ^n}}\\ {\delta {\gamma ^n}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\theta _k^n}\\ {\gamma _k^n} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} {\theta _{k - 1}^n}\\ {\gamma _{k - 1}^n} \end{array}} \right] $$ (23) 记ΣAC为姿态约束观测向量的协方差矩阵，则：
$$ {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{\rm{AC}}}} = \left[ {{\rm{diag}}\left( {\begin{array}{*{20}{c}} {\varepsilon _x^2}&{\varepsilon _y^2} \end{array}} \right)} \right] $$ (24) 若载体装有磁强计(magnetometer，简称mag)，则可以直接测出载体的航向角。
$$ {\varphi ^n} = \varphi _{{\rm{mag}}}^n + {\varepsilon _{{\rm{mag}}}} + {\eta _{{\rm{yaw}}}},E\left( {{\eta _{{\rm{yaw}}}}} \right) = 0 $$ (25) 式中，φmagn为磁罗盘测得的载体在导航坐标系下的偏航角；εmag为磁罗盘零偏；ηyaw为偏航角的测量噪声，下标yaw为偏航角简称。
附加姿态观测条件的Kalman滤波观测矩阵Bk和约束观测向量Dk为：
$$ {\mathit{\boldsymbol{B}}_k} = \left[ {\begin{array}{*{20}{c}} {{\rm{diag}}\left( {\begin{array}{*{20}{c}} 1&1&1 \end{array}} \right)}&{{{\bf{0}}_{3 \times 12}}} \end{array}} \right] $$ (26) $$ {\mathit{\boldsymbol{D}}_k} = \left[ {\begin{array}{*{20}{c}} {\delta {\theta ^n}}\\ {\delta {\gamma ^n}}\\ {\delta {\varphi ^n}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\theta _k^n}\\ {\gamma _k^n}\\ {\varphi _k^n} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} {\theta _{k - 1}^n}\\ {\gamma _{k - 1}^n}\\ {\varphi _{{\rm{mag}}}^n} \end{array}} \right] $$ (27) 姿态约束观测向量协方差ΣAC为：
$$ {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_{{\rm{AC}}}} = \left[ {{\rm{diag}}\left( {\begin{array}{*{20}{c}} {\varepsilon _x^2}&{\varepsilon _y^2}&{\varepsilon _{{\rm{mag}}}^2} \end{array}} \right)} \right] $$ (28) 其中航向角噪声方差取值取决于磁罗盘参数。
由于只有姿态参数为直接可测参数，因此使用修正后的姿态重新进行位置推算。
3 附加不同约束条件对定位结果的影响
本算例采用北京华力创通的一组松组合数据，时长637s。GNSS定位结果为GPS和北斗导航系统组合结果，GNSS更新频率为1 Hz；INS采用中低精度MEMS，采样频率为25 Hz，陀螺仪零漂约40°/h，随机游走约1°/Hz0.5，加速度计零偏约25mg，随机游走约7.5 mg/Hz0.5；载体附加传感器为里程计，测速误差约0.1 m/s，并附带仿真磁罗盘数据，航向角误差约1.5°；参考基准为高精度实时动态(real-time kinematic，RTK)定位结果。载体运动轨迹如图 3：
为了验证GNSS信号中断时，附加运动约束的组合导航性能，结合图 3，本文选取运动轨迹较平稳的180至400历元(图中两黑色圆圈之间路径)，人为中断GNSS信号，模拟载体通过隧道，中断时间为200至380历元，共180s。选取历元速度与高程如图 4所示。
由图 4可以看出，该段时间内载体运动速度较快，且高程较平稳，符合模拟条件。
本文采取如下方案：
方案1 无附加约束；
方案2 附加高程约束；
方案3 方案2基础上，附加速度约束；
方案4 方案3基础上，附加具有里程计观测值的速度约束；
方案5 方案4基础上，附加姿态约束；
方案6 方案5基础上，附加具有磁罗盘观测值的姿态约束。
各约束方案的方差如表 1。
表 1 附加约束方案Table 1. The Scheme of Constraint约束条件及观测量 方差值 天向速度约束 0.05 m/s×0.05 m/s 高程约束 0.1 m×0.1 m 侧向速度约束 0.05 m/s×0.05 m/s 法向速度约束 0.05 m/s×0.05 m/s 里程计观测量 0.1 m/s×0.1 m/s 俯仰姿态约束 0.2°×0.2° 横滚姿态约束 0.1°×0.1° 磁罗盘观测量 1.5°×1.5° 计算结果分别见图 5。从图中可以得出以下结论。
1) GNSS信号中断后，INS独立工作，位置误差随时间积累，发散速度快。180 s各方向误差分别达到：2450 m (E，东向)，610 m (N，北向)，265 m (U，天向)。
2) 附加高程约束后，水平误差无明显变化，高程方向得到明显改善，误差不再随时间发散。最大误差为10.12 m (U)，均方根误差为4.52 m (U)。
3) 同时附加高程约束和速度约束，在保持高程误差改善的前提下，水平误差随时间发散速度明显减小。最大误差分别为260.96 m (E)、53.52 m (N)、9.98 m (U)。其中高程误差出现细微差别是由于高程约束的天向速度改正和速度约束中的法向速度改正的共同影响。
4) 附加具有里程计观测值的速度约束后，水平位置误差得到显著改善，东向误差不再随时间发散，北向误差发散趋势进一步减小。最大误差分别为23.07 m (E)、28.76 m (N)、9.98 m (U)。均方根误差分别为16.89 m (N)、13.46 m (E)、4.52 m (U)。
5) 附加姿态约束后，在一定程度上减小姿态角发散，可以小幅改善水平位置精度。最大误差分别为19.09 m (E)、26.12 m (N)、9.78 m (U)；均方根误差分别为13.46 m (E)、16.37 m (N)、4.51 m (U)。
6) 附加具有磁罗盘观测值的姿态约束后，提升了航向角的精度，反映在位置误差进一步减小，尤其北向误差不再随时间发散且最大误差减小。最大误差分别为13.65 m (E)、4.89 m (N)、9.80 m (U)；均方根误差分别为8.46 m (E)、2.59 m (N)、4.51 m (U)。
4 结语
当组合导航系统GNSS信号中断，仅依靠INS进行导航时，定位误差随时间快速发散。对于本算例采用的MEMS级别惯导，180 s位置误差达数千米。
组合导航系统GNSS信号中断后，附加高程约束后，仅改善高程方向的位置误差，且效果明显；附加速度约束后，由于增加速度观测量，提高速度精度，采用修正速度重新进行位置推算，可大幅改善水平位置误差；附加姿态约束后，可小幅改善定位精度。引入磁罗盘观测值后，通过改善方位误差，可进一步减小位置误差。
附加约束条件以及引入新的观测值，均可改善组合导航定位精度。合理选取约束条件以及观测值，并组合使用，可以获得更好的定位精度。
约束条件的强度及引入新观测值的精度都会影响组合导航的定位精度。尤其是气压计能否准确敏感高程变化，引入气压计观测值对定位结果有何影响仍需进一步研究。
-
表 1 附加约束方案
Table 1 The Scheme of Constraint
约束条件及观测量 方差值 天向速度约束 0.05 m/s×0.05 m/s 高程约束 0.1 m×0.1 m 侧向速度约束 0.05 m/s×0.05 m/s 法向速度约束 0.05 m/s×0.05 m/s 里程计观测量 0.1 m/s×0.1 m/s 俯仰姿态约束 0.2°×0.2° 横滚姿态约束 0.1°×0.1° 磁罗盘观测量 1.5°×1.5° 
下载: 导出CSV
-
[1] Simon D, Chia T L. Kalman Filtering with State Equality Constraints[J]. IEEE, Transactions on Aerospace and Electronic Systems, 2002, 38(1):128-136 doi: 10.1109/7.993234
[2] Simon D, Simon D L. Aircraft Turbofan Engine Health Estimation Using Constrained Kalman Filtering[J]. Journal of Engineering for Gas Turbines and Power, 2004, 126(1):1-6 doi: 10.1115/1.1640361
[3] Yang Y, Gao W, Zhang X. Robust Kalman Filtering with Constraints:A Case Study in Application to Navigation[J].Journal of Geodesy, 2010, 84(6):373-381 doi: 10.1007/s00190-010-0374-6
[4] Yang Y, Zhang X, Xu J. Adaptively Constrained Kalman Filtering for Navigation Applications[J]. Survey Review, 2011, 43(322):370-381 doi: 10.1179/003962611X13055561708704
[5] 杨元喜, 景一帆, 曾安敏.自适应参数估计与内外部精度的关系[J].测绘学报, 2014, 43(5):441-445 http://www.cnki.com.cn/Article/CJFDTOTAL-CHXB201405002.htm Yang Yuanxi, Jing Yifan, Zeng Anmin. Adaptive Parameter Estimation and Inner and External Precision[J].ActaGeodastica et CartographicaSinica, 2014, 43(5):441-445 http://www.cnki.com.cn/Article/CJFDTOTAL-CHXB201405002.htm
[6] Dissanayake G, Sukkarieh S, Hugh D W. The Aiding of Alow-cost Strapdown Inertial Measurement Unit Using Vehicle Model Constraints for Land Vehicle Applications[J].IEEE Transactions on Robotics and Automation, 2001, 17(5):731-747 doi: 10.1109/70.964672
[7] Godha S, Cannon M E. GPS/MEMS INS Integrated System for Navigation in Urban Areas[J].GPS Solution, 2007, 11:193-203 doi: 10.1007/s10291-006-0050-8
[8] Godha S. Performance Evaluation of Low-Cost MEMS-Based IMU Integrated with GPS for Land Vehicle Navigation Application[D]. Calgary, AB:University of Calgary, 2006
[9] Klein I, Filin S, Toledo T. Vehicle Constraints Enhancement for Supporting INS Navigation in Urban Environments[J]. Navigation, Journal of the Instituteof Navigation, 2011, 58(1):7-15.
[10] Niu X, Zhang H P, Chiang K W, et al. Using Land-Vehicle Steering Constraint ToImprove the Heading Estimation of MEMSGPS/INS Georeferencing Systems:ISPRSVol. XXXVⅢ, Part 1, 2010[C]. The Geomatics Conference and Symposium of Commission I, SPRS Convergence in Geomatics-Shaping Canada's Competitive Landscape, Calgary, Alberta, Canada, 2010
[11] Niu X, Li Y, Zhang Q, et al. Observability Analysis of Non-Holonomic Constraints for LandVehicle Navigation Systems[J]. Journal of Global Positioning Systems, 2012, 11(1):80-88 doi: 10.5081/jgps
[12] 柴艳菊, 阳仁贵, 王海涛, 等.附加运动约束提高GPS/INS导航参数估计精度[J].中国惯性技术学报, 2011, 19(1):28-32 http://www.cnki.com.cn/Article/CJFDTOTAL-ZGXJ201101007.htm Chai Yanju, Yang Rengui, Wang Haitao, et al. Improving the Navigation Accuracy of GPS/INS by Adding Motion Information Constraints[J]. Journal of Chinese Inertial Technology, 2011, 19(1):28-32 http://www.cnki.com.cn/Article/CJFDTOTAL-ZGXJ201101007.htm
[13] 吴富梅, 杨元喜.附加速度先验信息的车载GPS/INS/Odometer组合导航算法[J].宇航学报, 2010, 31(10):2314-2320 doi: 10.3873/j.issn.1000-1328.2010.10.013 Wu Fumei, Yang Yuanxi. GPS/INS/Odometer Integrated Navigation Algorithm with Prior Velocityin Land Vehicle System[J]. Journal of Astronautics, 2010, 31(10):2314-2320 doi: 10.3873/j.issn.1000-1328.2010.10.013
[14] Thienel J, Harman R, Bar-Itzhack I. Results of the Magnetometer Navigation (MAGNAV) Inflight Experiment[J]. AIAA Journal, 2004
[15] Yu M, Li Z, Chen Y, et al. Improving Integrity and Reliability of Map Matching Techniques[J]. Journal of Global Positioning Systems, 2006, 5(1-2):40-46 http://www.gmat.unsw.edu.au/wang/jgps/v5n12/v5n12p07.pdf
[16] 张力军, 钱山, 蔡洪, 等. Kalman滤波中连续系统离散化的计算机实现[J].飞行器测控学报, 2010, 29(2):66-69 http://www.cnki.com.cn/Article/CJFDTOTAL-FXCK201002020.htm Zhang L, Qian S, Cai H, et al. Computer Implementation to Discretization of Continuous System for Kalman Filter[J]. Journal of Spacecraft TT & C Technology, 2010, 29(2):66-69 http://www.cnki.com.cn/Article/CJFDTOTAL-FXCK201002020.htm
-
期刊类型引用(13)
1. 杨忠荣. 低成本车载RTK高精度定位方法. 地理空间信息. 2025(03): 96-100 . 
百度学术
2. 程建华，陈思成，臧楠，程思翔，赵国晶，马子凡. 附加自适应短时高程变化率约束的PPP/INS紧组合增强模型. 测绘学报. 2024(09): 1761-1776 . 百度学术
3. 王勋，崔先强，高天杭. 动力学模型自适应滤波算法研究. 武汉大学学报(信息科学版). 2023(05): 741-748 . 百度学术
4. 张一，尹潇，宋海娜. 运动方向约束的自适应SRUKF目标跟踪算法. 导航定位学报. 2023(05): 53-59 . 百度学术
5. 李清泉，吕世望，陈智鹏，殷煜，张德津. 冬奥会国家速滑馆超大地坪平整度快速测量. 武汉大学学报(信息科学版). 2022(03): 325-333 . 百度学术
6. 辜声峰，戴春齐，何成鹏，方礼喆，王梓豪. 面向城市车载导航的多系统PPP-RTK/VIO半紧组合算法性能分析. 武汉大学学报(信息科学版). 2021(12): 1852-1861 . 百度学术
7. 陶晓晓，卢小平，路泽忠，周雨石，余振宝. WiFi融合环境光定位方法研究. 测绘科学. 2020(06): 57-61+72 . 百度学术
8. 张辰东，王兆瑞. 一种长隧道内高速列车实时高精度定位方法. 计算机仿真. 2020(09): 124-128+188 . 百度学术
9. 尹潇，柴洪洲，向民志，杜祯强. 附加运动学约束的BDS抗差UKF导航算法. 测绘学报. 2020(11): 1399-1406 . 百度学术
10. 王诒国，潘孝星. 基于马氏距离的抗差卡尔曼滤波算法在组合导航中的应用. 北京测绘. 2020(12): 1810-1814 . 百度学术
11. 吕大千，曾芳玲，欧阳晓凤，于合理. 时频传递的改进整数相位钟方法. 测绘学报. 2019(07): 889-897 . 百度学术
12. 邓中亮，尹露，唐诗浩，刘延旭，宋汶轩. 室内定位关键技术综述. 导航定位与授时. 2018(03): 14-23 . 百度学术
13. 张迪，施昆，李照永. 移动测量系统的GNSS/INS组合定位方法的对比研究. 软件. 2018(08): 110-116 . 百度学术
其他类型引用(18)
计量
- 文章访问数: 1946
- HTML全文浏览量: 215
- PDF下载量: 599
- 被引次数: 31
