CN103512575A - 一种测绘车用惯导系统零速修正方法 - Google Patents

Abstract

本发明属于惯性技术领域，具体涉及一种测绘车用惯导系统零速修正方法。本发明包括以下步骤：滤波参数初始化；惯性导航和滤波器状态转移矩阵离散化；计算速度积分观测量和量测矩阵；UD分解卡尔曼滤波时间更新计算；发送零速修正申请；UD分解卡尔曼滤波量测更新；惯导系统误差修正；重复以上步骤直到定位定向系统工作结束。本发明需要解决的技术问题为：现有技术中以速度误差为观测量的速度匹配零速修正方法易受外界干扰速度的影响，降低了惯导误差估计精度。本发明实现了对量测速度噪声的有效滤波平滑，减小了干扰速度的影响，提高了滤波计算精度和滤波工作稳定性，克服了估计方差阵计算误差引起的奇异问题，从而提高了零速修正精度和可靠性。

Description

一种测绘车用惯导系统零速修正方法

技术领域

本发明属于惯性技术领域，具体涉及一种测绘车用惯导系统零速修正方法。

背景技术

目前，典型的陆用定位定向系统工作模式为惯导/里程计组合导航，并兼容零速修正，获得较高的自主定位定向精度。其中，零速修正是测绘用自主定位定向系统所普遍采用的方法，通过车辆的间歇性短时停车来修正惯导系统误差，从而实现惯导系统的高精度定位，具有完全自主性。

零速修正的主要方法有曲线拟合法和卡尔曼滤波法。由于曲线拟合法忽略了惯导系统各个速度通道之间的耦合关系，因此该方法的精度相对较低；卡尔曼滤波方法可以跟踪惯导内部状态变化过程，对观测速度误差具有滤波作用，估计精度相对较高，然而，实际应用中由于车辆发动机、风扰、人员走动等干扰速度的影响，停车状态下并非完全零速，常规的以速度误差为观测量的速度匹配零速修正方法将受干扰速度的影响，导致零速修正精度降低。

发明内容

本发明需要解决的技术问题为：现有技术中以速度误差为观测量的速度匹配零速修正方法易受外界干扰速度的影响，降低了惯导误差估计精度。

本发明的技术方案如下所述：

一种测绘车用惯导系统零速修正方法

采用的坐标系定义如下：

n：导航坐标系（oxyz），北天东地理坐标系，x轴指北，y轴指天，z轴指东；

n′：计算导航坐标系（o′x′y′z′），x′轴指北，y′轴指天，z′轴指东；

b：惯导载体系（o″x″y″z″），与陀螺坐标系重合，前上右坐标系，x″轴指向前，y″轴朝上，z″轴指右；

本方法包括以下步骤：

步骤1）滤波参数初始化；

步骤2）惯性导航和滤波器状态转移矩阵离散化；

步骤3）计算速度积分观测量和量测矩阵；

步骤4）UD分解卡尔曼滤波时间更新计算；

步骤5）发送零速修正申请；

步骤6）UD分解卡尔曼滤波量测更新；

步骤7）惯导系统误差修正；

步骤8）重复以上步骤2）~7），直到定位定向系统工作结束。

步骤1）具体包括以下步骤：

惯导系统完成初始对准后，对零速修正UD分解卡尔曼滤波器

参数进行初始化，转入导航状态，设计惯导系统误差模型如下：

式中，

$F=\left[\begin{array}{ccc}{F}_{11}& F_{12}& F_{13}\\ F_{21}& F_{22}& F_{23}\\ F_{31}& F_{32}& F_{33}\end{array}\right],$

$F_{12}=\left[\begin{array}{ccc}0& {-f}_E& f_U\\ f_E& 0& {-f}_N\\ {-f}_U& f_N& 0\end{array}\right],$

F₃₂＝[0_3×3]，

建立如下零速修正卡尔曼滤波器模型：

$\left\{\begin{array}{c}X\left(k\right)=\mathrm{\Φ}(k,k-1)X(k-1)+\mathrm{\Γ}(k-1)w(k-1)\\ Z\left(k\right)=H\left(k\right)X\left(k\right)+\mathrm{\γ}\left(k\right)\end{array}\right.---\left(2\right)$

式（1）、式（2）中，

状态向量

ω_ie为地球自转角速率；

V_N、V_U、V_E分别为惯导北向、天向、东向速度；

f_N、f_U、f_E分别为惯导北向、天向、东向加速度；

λ、

h分别为惯导经度、纬度、高度；

R_M、R_N分别为地球子午面、卯酉面半径；

δV_N、δV_U、δV_E分别为惯导北向、天向、东向速度误差；

φ_N、φ_U、φ_E分别为惯导北向、天向、东向失准角；

δh、δλ分别为惯导纬度误差、高度误差、经度误差；

分别为惯导载体系x轴、y轴、z轴加速度计零位误差；

ε_x、ε_y、ε_z分别为惯导载体系x轴、y轴、z轴陀螺常值漂移；

z(k)为观测量；

H(k)为量测矩阵；

w(k)为系统噪声；

γ(k)为观测噪声；

Γ(k)为系统噪声矩阵；

Φ(k,k-1)为离散状态转移矩阵；

分别为惯导载体系x轴、y轴、z轴加速度计测量噪声；

δε_x、δε_y、δε_z分别为惯导载体系x轴、y轴、z轴陀螺测量噪声；

为惯导姿态阵；

卡尔曼滤波估计误差方差P_k和一步预测估计误差方差P_k/k-1分解成UDU^T的形式，即

$P_k=U_k{D}_k{U}_k^T,$ 记为 $\hat{P}=\hat{U}\hat{D}\widehat{U^T}---\left(3\right)$

$P_{k/k-1}=U_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T,$ 记为 $\tilde{P}=\tilde{U}\tilde{D}\widetilde{U^T}---\left(4\right)$

UD自适应卡尔曼滤波器的初始参数设置包括：

一步预测误差方差分解阵

一步预测误差方差分解阵

估计误差方差分解阵

根据所采用的惯导系统精度设置估计误差方差分解阵

系统噪声方差阵Q＝E[w(k)w^T(k)]、噪声阵Γ；

量测噪声方差阵R＝E[γ(k)γ^T(k)]，

E表示数学期望；

w(k)为系统噪声；

γ(k)为观测噪声。

步骤2）具体包括以下步骤：

以T_n为周期进行捷联惯导姿态、速度、位置更新，同时计算离散状态转移矩阵Φ_k,k-1；

离散状态转移矩阵计算公式如下：

$\mathrm{\Φ}(k+{\mathrm{iT}}_n,k)=I_{15\×15}+\underset{j=1}{\overset{i}{\mathrm{\Σ}}}{A}_{k+j{T}_n}{T}_n---\left(5\right)$

其中， $A=\left[\begin{array}{cc}F& G\\ 0_{6\×9}& 0_{6\×6}\end{array}\right],$ $G=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& -C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],$

为惯导姿态阵，I_15×15为15维单位阵，T_n为导航周期。

步骤3）具体包括以下步骤：

以T_z为周期，计算速度积分观测量和量测矩阵：

观测量：

$Z\left(k\right)=\underset{i=1}{\overset{50}{\mathrm{\Σ}}}{V}^{\mathrm{INS}}(k-1+{\mathrm{iT}}_z)T_z---\left(6\right)$

量测矩阵为：

$H\left(k\right)=\left[\underset{i=1}{\overset{50}{\mathrm{\Σ}}}\hat{H}\mathrm{\Φ}(k+{\mathrm{iT}}_n,k)\right]{\mathrm{\Φ}}^{-1}(k+1,k)T_z---\left(7\right)$

其中，V^INS＝[V_N V_U V_E]^T，

步骤4）具体包括以下步骤：

卡尔曼滤波时间更新计算周期设置为T_f，导航计算时间达到T_f的整倍数时，进行UD分解卡尔曼滤波时间更新计算，计算公式如下：

状态一步预测计算：

$\tilde{X}(k,k-1)=\mathrm{\Φ}(k,k-1)\hat{X}(k-1),\hat{X}\left(0\right)=0_{1\×15}\left(8\right)$

一步预测均方差计算：

为主对角元全为1的上三角阵，为对角阵，则

和

按以下各式确定：

${\tilde{D}}_j=W_j^{n-j}D{\left[W_j^{n-j}\right]}^T,j=\mathrm{15,14},...2---\left(9\right)$

${\tilde{U}}_{i,j}=\left\{\begin{array}{cc}\frac{W_i^{(n-j)}D{\left[W_i^{(n-j)}\right]}^T}{{\tilde{D}}_j}& {\tilde{D}}_j\&GreaterEqual;1.0e-25\\ 0& {\tilde{D}}_j<1.0e-25\end{array}\right.,j=\mathrm{15,14},...2,i=\mathrm{1,2},...j-1---\left(10\right)$

$W_i^{n-j+1}=W_i^{n-j}-{\tilde{U}}_{i,j}{W}_j^{n-j},j=\mathrm{15,14},...2,i=\mathrm{1,2},...j-1---\left(11\right)$

${\tilde{D}}_1=W_1^{n-1}D{\left[W_1^{n-1}\right]}^T---\left(12\right)$

式中

$D=\mathrm{diag}{\left[\begin{array}{cc}\hat{D}& Q\end{array}\right]}_{15\&times;21}---\left(13\right)$

$W^{\left(0\right)}=\left[\begin{array}{cc}\mathrm{\&Phi;}\hat{U}& \mathrm{\&Gamma;}\end{array}\right]={\left[\begin{array}{c}{W}_1^{\left(0\right)}\\ W_2^{\left(0\right)}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ W_n^{\left(0\right)}\end{array}\right]}_{15\&times;21}---\left(14\right)$

为15维行向量；

令 $\hat{X}\left(k\right)=\tilde{X}(k,k-1),$ $\hat{U}=\tilde{D},\hat{D}=\tilde{D},$ 则卡尔曼滤波时间更新结束。

步骤5）中，距离上次零速修正或对准结束的时间达到T_zc后，向用户申请零速修正。

步骤6）具体包括以下步骤：

当惯导系统处于零速状态，且允许零速修正后，进行UD卡尔

曼滤波量测更新，量测更新周期为T_f；

按公式（15）~（23）进行量测更新计算；

$f^{\left(n\right)}={\left({\hat{U}}^{(n-1)}\right)}^T{\left(H^{\left(n\right)}\right)}^T,n=\mathrm{1,2,3},$ 量测矩阵 $H=\left[\begin{array}{c}{H}^{\left(1\right)}\\ H^{\left(2\right)}\\ H^{\left(3\right)}\end{array}\right]---\left(15\right)$

$g_i^{\left(n\right)}={\hat{D}}_i^{(n-1)}{f}_i^{\left(n\right)},i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;15,n=\mathrm{1,2,3}---\left(16\right)$

${\mathrm{\&alpha;}}_j^{\left(n\right)}={\mathrm{\&alpha;}}_{j-1}^{\left(n\right)}+f_i{g}_i^{\left(n\right)},j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,{\mathrm{\&alpha;}}_0^{\left(n\right)}=r\left(n\right),n=\mathrm{1,2,3}---\left(17\right)$

${\mathrm{\&lambda;}}_j^{\left(n\right)}={-f}_i^{\left(n\right)}/{\mathrm{\&alpha;}}_{j-1}^{\left(n\right)},j=1,2,\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(18\right)$

则：

${\hat{D}}_j^{\left(n\right)}={\hat{D}}_j^{(n-1)}\frac{{\mathrm{\&alpha;}}_{j-1}^{\left(n\right)}}{{\mathrm{\&alpha;}}_j^{\left(n\right)}},j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(19\right)$

${\hat{U}}_{\mathrm{ij}}^{\left(n\right)}={\hat{U}}_{\mathrm{ij}}^{(n-1)}+{\mathrm{\&lambda;}}_j^{\left(n\right)}(g_i^{\left(n\right)}+\underset{k=i+1}{\overset{j-1}{\mathrm{\&Sigma;}}}{\hat{U}}_{\mathrm{ik}}^{(n-1)}{g}_k^{\left(n\right)}),j=\mathrm{2,3},\&CenterDot;\&CenterDot;\&CenterDot;,15,i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,j-1,n=\mathrm{1,2,3}---\left(20\right)$

${\hat{U}}_{\mathrm{ii}}^{\left(n\right)}=1,i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(21\right)$

其中，H⁽ⁿ⁾为1×15的行向量，

$R=\left[\begin{array}{ccc}r\left(1\right)& 0& 0\\ 0& r\left(2\right)& 0\\ 0& 0& r\left(3\right)\end{array}\right];$

滤波增益K_k按照下式进行计算：

$K_k^{\left(n\right)}=\frac{{\hat{U}}^{(n-1)}{\hat{D}}^{(n-1)}{f}^{\left(n\right)}}{{\mathrm{\&alpha;}}_{\mathrm{IN}\_\mathrm{DX}}^{\left(n\right)}},n=\mathrm{1,2,3}---\left(22\right)$

状态估计计算：

$X_k^{\left(n\right)}=X_k^{(n-1)}+K_k^{\left(n\right)}(Z_k^{\left(n\right)}-H_k^{\left(n\right)}{X}_k^{(n-1)}),Z_k=\left[\begin{array}{c}{Z}_k^{\left(1\right)}\\ Z_k^{\left(2\right)}\\ Z_k^{\left(3\right)}\end{array}\right]n=\mathrm{1,2,3}---\left(23\right)$

其中， ${\hat{X}}_k^{\left(0\right)}={\hat{X}}_{k/k-1};$

滤波最优估计结果为： ${\hat{X}}_k={\hat{X}}_k^{\left(3\right)},$ $K_k=K_k^{\left(3\right)},$ $\hat{U}={\hat{U}}^{\left(3\right)},$ $\hat{D}={\hat{D}}^{\left(3\right)};$

式中：Q—系统噪声的协方差矩阵；R—观测噪声的协方差矩阵。

步骤7）具体包括以下步骤：

当卡尔曼滤波量测更新时间大于T_gx后，停止卡尔曼滤波量测更新，并利用滤波估计结果

对惯导水平姿态、速度、位置误差进行修正，告知用户本次零速修正结束，车辆可以开动；

速度修正：

$V_N={\hat{V}}_N-{\hat{X}}_k\left(1\right),$ $V_N={\hat{V}}_U-{\hat{X}}_k\left(2\right),$ $V_N={\hat{V}}_E-{\hat{X}}_k\left(3\right);$

为修正前惯导北速、天速、东速；

位置修正：

$\mathrm{\&lambda;}=\widehat{\mathrm{\&lambda;}}-{\hat{X}}_k\left(9\right),$

$h=\hat{h}-{\hat{X}}_k\left(8\right);$

为修正前惯导北速、天速、东速；

水平姿态修正：

$C_b^n=\left[\begin{array}{ccc}1& -{\hat{X}}_k& 0\\ {\hat{X}}_k& 1& -{\hat{X}}_k\left(3\right)\\ 0& {\hat{X}}_k\left(3\right)& 1\end{array}\right]C_b^{n^{\&prime;}}.$

作为优选方案，R＝E[γ(k)γ^T(k)]＝diag{(0.01)²,(0.01)²,(0.01)²}；

5ms≤T_n≤20ms；

50ms≤T_f≤1s；

T_zc=10min；

T_f＝1s；

10s≤T_gx≤30s。

本发明的有益效果为：

本发明开发了一种基于UD分解卡尔曼滤波的速度积分匹配零速修正方法，利用停车状态下平均速度为零的原理，对惯导速度进行积分，以此速度积分为观测量，实现对量测速度噪声的有效滤波平滑，减小干扰速度的影响；同时采用UD分解卡尔曼滤波提高滤波计算精度和滤波工作稳定性，克服估计方差阵计算误差引起的奇异问题，从而提高零速修正精度和可靠性。以50Hz的量测信息采样频率为例，相对传统的速度匹配方法方法，量测噪声方差可减小50倍，实现了量测信息的平滑压缩，减小零速状态下干扰噪声的影响，提高了量测信息信噪比。

附图说明

图1为本发明的测绘车用惯导系统零速修正方法流程图；

图2为惯导系统零速修正点定位误差示意图。

具体实施方式

下面结合附图和实施例对本发明的测绘车用惯导系统零速修正方法进行详细说明。

本发明的测绘车用惯导系统零速修正方法采用的坐标系定义如下：

n：导航坐标系（oxyz），北天东地理坐标系，x轴指北，y轴指天，z轴指东；

n′：计算导航坐标系（o′x′y′z′），x′轴指北，y′轴指天，z′轴指东；

b：惯导载体系（o"x″y″z″），与陀螺坐标系重合，前上右坐标系，x″轴指向前，y″轴朝上，z″轴指右。

如图1所示，本发明的测绘车用惯导系统零速修正方法包括以下步骤：

步骤1）滤波参数初始化

惯导系统完成初始对准后，对零速修正UD分解卡尔曼滤波器参数进行初始化，转入导航状态。

本发明的方法中，设计惯导系统误差模型如下：

式中，

$F=\left[\begin{array}{ccc}{F}_{11}& F_{12}& F_{13}\\ F_{21}& F_{22}& F_{23}\\ F_{31}& F_{32}& F_{33}\end{array}\right],$

$F_{12}=\left[\begin{array}{ccc}0& {-f}_E& f_U\\ f_E& 0& {-f}_N\\ {-f}_U& f_N& 0\end{array}\right],$

b

F₃₂＝[0_3×3]，

建立如下零速修正卡尔曼滤波器模型：

$\left\{\begin{array}{c}X\left(k\right)=\mathrm{\&Phi;}(k,k-1)X(k-1)+\mathrm{\&Gamma;}(k-1)w(k-1)\\ Z\left(k\right)=H\left(k\right)X\left(k\right)+\mathrm{\&gamma;}\left(k\right)\end{array}\right.---\left(2\right)$

式（1）、式（2）中，

状态向量

ω_ie为地球自转角速率；

V_N、V_U、V_E分别为惯导北向、天向、东向速度；

f_N、f_U、f_E分别为惯导北向、天向、东向加速度；

λ、

h分别为惯导经度、纬度、高度；

R_M、R_N分别为地球子午面、卯酉面半径；

δV_N、δV_U、δV_E分别为惯导北向、天向、东向速度误差；

φ_N、φ_U、φ_E分别为惯导北向、天向、东向失准角；

δh、δλ分别为惯导纬度误差、高度误差、经度误差；

分别为惯导载体系x轴、y轴、z轴加速度计零位误差；

ε_x、ε_y、ε_z分别为惯导载体系x轴、y轴、z轴陀螺常值漂移；

z(k)为观测量；

H(k)为量测矩阵；

w(k)为系统噪声；

γ(k)为观测噪声；

Γ(k)为系统噪声矩阵；

Φ(k,k-1)为离散状态转移矩阵。

分别为惯导载体系x轴、y轴、z轴加速度计测量噪声；

δε_x、δε_y、δε_z分别为惯导载体系x轴、y轴、z轴陀螺测量噪声；

为惯导姿态阵。

本算法中采用UD分解卡尔曼滤波算法。设卡尔曼滤波估计误差方差P_k和一步预测估计误差方差P_k/k-1可分解成UDU^T的形式，即

$P_k=U_k{D}_k{U}_k^T,$ 简记为 $\hat{P}=\hat{U}\hat{D}\widehat{U^T}---\left(3\right)$

$P_{k/k-1}=U_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T,$ 简记为 $\tilde{P}=\tilde{U}\tilde{D}\widetilde{U^T}---\left(4\right)$

UD自适应卡尔曼滤波器的主要初始参数设置包括：

一步预测误差方差分解阵

一步预测误差方差分解阵

估计误差方差分解阵

根据所采用的惯导系统精度设置估计误差方差分解阵

系统噪声方差阵Q＝E[w(k)w^T(k)]、噪声阵Γ；本实施例中，Q为6x6维矩阵，Γ为15x6维矩阵；

量测噪声方差阵R＝E[γ(k)γ^T(k)]，本实施例中，

R＝E[γ(k)γ^T(k)]＝diag{(0.01)²,(0.01)²,(0.01)²}。

E表示数学期望；

w(k)为系统噪声；

γ(k)为观测噪声。

步骤2）惯性导航和滤波器状态转移矩阵离散化

以T_n为周期(5ms≤T_n≤20ms)，进行捷联惯导姿态、速度、位置更新，即惯性导航计算，可采用常规的定时增量导航算法。同时计算离散状态转移矩阵Φ_k,k-1。

离散状态转移矩阵计算公式如下：

$\mathrm{\&Phi;}(k+{\mathrm{iT}}_n,k)=I_{15\&times;15}+\underset{j=1}{\overset{i}{\mathrm{\&Sigma;}}}{A}_{k+j{T}_n}{T}_n,$ 本实施例中，T_n＝5ms        （5）

其中， $A=\left[\begin{array}{cc}F& G\\ 0_{6\&times;9}& 0_{6\&times;6}\end{array}\right],$ $G=\left[\begin{array}{cc}{C}_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& -C_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right],$

为惯导姿态阵，I_15×15为15维单位阵，T_n为导航周期。

步骤3）计算速度积分观测量和量测矩阵

以T_z为周期(20ms≤T_z≤80ms)，计算速度积分观测量和量测矩阵：

观测量：

$Z\left(k\right)=\underset{i=1}{\overset{50}{\mathrm{\&Sigma;}}}{V}^{\mathrm{INS}}(k-1+{\mathrm{iT}}_z)T_z,$ 本实施例中，T_z＝20ms       （6）

量测矩阵为：

$H\left(k\right)=\left[\underset{i=1}{\overset{50}{\mathrm{\&Sigma;}}}\hat{H}\mathrm{\&Phi;}(k+{\mathrm{iT}}_n,k)\right]{\mathrm{\&Phi;}}^{-1}(k+1,k)T_z---\left(7\right)$

其中，V^INS＝[V_N V_U V_E]^T，

步骤4）UD分解卡尔曼滤波时间更新计算

卡尔曼滤波时间更新计算周期设置为T_f，(50ms≤T_f≤1s)，本实施例中T_f＝1s，导航计算时间达到1s的整倍数时，进行UD分解卡尔曼滤波时间更新计算。计算公式如下。

状态一步预测计算：

$\tilde{X}(k,k-1)=\mathrm{\&Phi;}(k,k-1)\hat{X}(k-1),\hat{X}\left(0\right)=0_{1\&times;15}---\left(8\right)$

一步预测均方差计算

为主对角元全为1的上三角阵，

为对角阵，则

和

按以下各式确定：

${\tilde{D}}_j=W_j^{n-j}D{\left[W_j^{n-j}\right]}^T,j=\mathrm{15,14},...2---\left(9\right)$

${\tilde{U}}_{i,j}=\left\{\begin{array}{cc}\frac{W_i^{(n-j)}D{\left[W_i^{(n-j)}\right]}^T}{{\tilde{D}}_j}& {\tilde{D}}_j\&GreaterEqual;1.0e-25\\ 0& {\tilde{D}}_j<1.0e-25\end{array}\right.,j=\mathrm{15,14},...2,i=\mathrm{1,2},...j-1---\left(10\right)$

$W_i^{n-j+1}=W_i^{n-j}-{\tilde{U}}_{i,j}{W}_j^{n-j},j=\mathrm{15,14},...2,i=\mathrm{1,2},...j-1---\left(11\right)$

${\tilde{D}}_1=W_1^{n-1}D{\left[W_1^{n-1}\right]}^T---\left(12\right)$

式中

$D=\mathrm{diag}{\left[\begin{array}{cc}\hat{D}& Q\end{array}\right]}_{15\&times;21}---\left(13\right)$

$W^{\left(0\right)}=\left[\begin{array}{cc}\mathrm{\&Phi;}\hat{U}& \mathrm{\&Gamma;}\end{array}\right]={\left[\begin{array}{c}{W}_1^{\left(0\right)}\\ W_2^{\left(0\right)}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ W_n^{\left(0\right)}\end{array}\right]}_{15\&times;21}---\left(14\right)$

为15维行向量。

令 $\hat{X}\left(k\right)=\tilde{X}(k,k-1),$ $\hat{U}=\tilde{D},$ $\hat{D}=\tilde{D},$ 则卡尔曼滤波时间更新结束。

步骤5）发送零速修正申请

距离上次零速修正或对准结束的时间达到T_zc后，向用户申请零速修正。本实施例中，T_zc=10min。

步骤6）UD分解卡尔曼滤波量测更新

当惯导系统处于零速状态，且允许零速修正后，进行UD卡尔曼滤波量测更新，量测更新周期为T_f，本实施例中T_f＝1s。

按公式（15）~（23）进行量测更新计算。

$f^{\left(n\right)}={\left({\hat{U}}^{(n-1)}\right)}^T{\left(H^{\left(n\right)}\right)}^T,n=\mathrm{1,2,3},$ 量测矩阵 $H=\left[\begin{array}{c}{H}^{\left(1\right)}\\ H^{\left(2\right)}\\ H^{\left(3\right)}\end{array}\right]---\left(15\right)$

$g_i^{\left(n\right)}={\hat{D}}_i^{(n-1)}{f}_i^{\left(n\right)},i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;15,n=\mathrm{1,2,3}---\left(16\right)$

${\mathrm{\&alpha;}}_j^{\left(n\right)}={\mathrm{\&alpha;}}_{j-1}^{\left(n\right)}+f_i{g}_i^{\left(n\right)},j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,{\mathrm{\&alpha;}}_0^{\left(n\right)}=r\left(n\right),n=\mathrm{1,2,3}---\left(17\right)$

${\mathrm{\&lambda;}}_j^{\left(n\right)}={-f}_i^{\left(n\right)}/{\mathrm{\&alpha;}}_{j-1}^{\left(n\right)},j=1,2,\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(18\right)$

则：

${\hat{D}}_j^{\left(n\right)}={\hat{D}}_j^{(n-1)}\frac{{\mathrm{\&alpha;}}_{j-1}^{\left(n\right)}}{{\mathrm{\&alpha;}}_j^{\left(n\right)}},j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(19\right)$

${\hat{U}}_{\mathrm{ij}}^{\left(n\right)}={\hat{U}}_{\mathrm{ij}}^{(n-1)}+{\mathrm{\&lambda;}}_j^{\left(n\right)}(g_i^{\left(n\right)}+\underset{k=i+1}{\overset{j-1}{\mathrm{\&Sigma;}}}{\hat{U}}_{\mathrm{ik}}^{(n-1)}{g}_k^{\left(n\right)}),j=\mathrm{2,3},\&CenterDot;\&CenterDot;\&CenterDot;,15,i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,j-1,n=\mathrm{1,2,3}---\left(20\right)$

${\hat{U}}_{\mathrm{ii}}^{\left(n\right)}=1,i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(21\right)$

其中，H⁽ⁿ⁾为1×15的行向量，

$R=\left[\begin{array}{ccc}r\left(1\right)& 0& 0\\ 0& r\left(2\right)& 0\\ 0& 0& r\left(3\right)\end{array}\right].$

滤波增益K_k的计算可按照下式进行：

$K_k^{\left(n\right)}=\frac{{\hat{U}}^{(n-1)}{\hat{D}}^{(n-1)}{f}^{\left(n\right)}}{{\mathrm{\&alpha;}}_{\mathrm{IN}\_\mathrm{DX}}^{\left(n\right)}},n=\mathrm{1,2,3}---\left(22\right)$

状态估计计算：

$X_k^{\left(n\right)}=X_k^{(n-1)}+K_k^{\left(n\right)}(Z_k^{\left(n\right)}-H_k^{\left(n\right)}{X}_k^{(n-1)}),Z_k=\left[\begin{array}{c}{Z}_k^{\left(1\right)}\\ Z_k^{\left(2\right)}\\ Z_k^{\left(3\right)}\end{array}\right]n=\mathrm{1,2,3}---\left(23\right)$

其中， ${\hat{X}}_k^{\left(0\right)}={\hat{X}}_{k/k-1}.$

滤波最优估计结果为： ${\hat{X}}_k={\hat{X}}_k^{\left(3\right)},$ $K_k=K_k^{\left(3\right)},$ $\hat{U}={\hat{U}}^{\left(3\right)},$ $\hat{D}={\hat{D}}^{\left(3\right)};$

式中：Q—系统噪声的协方差矩阵；R—观测噪声的协方差矩阵。

步骤7）惯导系统误差修正

当卡尔曼滤波量测更新时间大于T_gx后，停止卡尔曼滤波量测更新，并利用滤波估计结果

对惯导水平姿态、速度、位置误差进行修正，告知用户本次零速修正结束，车辆可以开动。本实施例中，10s≤T_gx≤30s。

速度修正：

$V_N={\hat{V}}_N-{\hat{X}}_k\left(1\right),$ $V_N={\hat{V}}_U-{\hat{X}}_k\left(2\right),$ $V_N={\hat{V}}_E-{\hat{X}}_k\left(3\right).$

为修正前惯导北速、天速、东速。

位置修正：

$\mathrm{\&lambda;}=\widehat{\mathrm{\&lambda;}}={\hat{X}}_k\left(9\right),$

$h=\hat{h}-{\hat{X}}_k\left(8\right).$

为修正前惯导北速、天速、东速。

水平姿态修正：

$C_b^n=\left[\begin{array}{ccc}1& -{\hat{X}}_k\left(5\right)& 0\\ {\hat{X}}_k\left(5\right)& 1& -{\hat{X}}_k\left(3\right)\\ 0& {\hat{X}}_k\left(3\right)& 1\end{array}\right]C_b^{n^{\&prime;}}.$

步骤8）重复以上步骤2）~7），直到定位定向系统工作结束。

利用本发明的方法进行实际车载试验。采用陀螺精度优于0.01°/h的激光捷联惯导系统，共进行了13组零速修正试验，每组试验的时间为2h。停车状态下系统准备时间为5min，对准结束后车辆行驶，转入导航状态，零速修正间隔时间为10min，每次零速修正时间为30s，试验结果见表1，图2为其中一条试验曲线图。

定位精度为零速修正后惯导位置与真实路标点位置的差值。

表1惯导/里程计组合导航试验结果统计

从13个条次的零速修正车载试验统计结果来看，相对常规的速度匹配零速修正方法，本发明的基于UD分解卡尔曼滤波的速度积分匹配零速修正方法的精度更高，13组试验的水平定位精度为8.67m（CEP），高程定位精度为4.91m（PE）。

Claims (9)

1.一种测绘车用惯导系统零速修正方法，其特征在于：

采用的坐标系定义如下：

n：导航坐标系（oxyz），北天东地理坐标系，x轴指北，y轴指天，z轴指东；

n′：计算导航坐标系（o′x′y′z′），x′轴指北，y′轴指天，z′轴指东；

b：惯导载体系（o"x″y″z″），与陀螺坐标系重合，前上右坐标系，x″轴指向前，y″轴朝上，z″轴指右；

本方法包括以下步骤：

步骤1）滤波参数初始化；

步骤2）惯性导航和滤波器状态转移矩阵离散化；

步骤3）计算速度积分观测量和量测矩阵；

步骤4）UD分解卡尔曼滤波时间更新计算；

步骤5）发送零速修正申请；

步骤6）UD分解卡尔曼滤波量测更新；

步骤7）惯导系统误差修正；

步骤8）重复以上步骤2）~7），直到定位定向系统工作结束。

2.根据权利要求1所述的测绘车用惯导系统零速修正方法，其特征在于：步骤1）具体包括以下步骤：

惯导系统完成初始对准后，对零速修正UD分解卡尔曼滤波器参数进行初始化，转入导航状态，设计惯导系统误差模型如下：

式中，

$F=\left[\begin{array}{ccc}{F}_{11}& F_{12}& F_{13}\\ F_{21}& F_{22}& F_{23}\\ F_{31}& F_{32}& F_{33}\end{array}\right],$

$F_{12}=\left[\begin{array}{ccc}0& {-f}_E& f_U\\ f_E& 0& -f_N\\ -f_U& f_N& 0\end{array}\right],$

F₃₂＝[0_3×3]，

建立如下零速修正卡尔曼滤波器模型：

$\left\{\begin{array}{c}X\left(k\right)=\mathrm{\&Phi;}(k,k-1)X(k-1)+\mathrm{\&Gamma;}(k-1)w(k-1)\\ Z\left(k\right)=H\left(k\right)X\left(k\right)+\mathrm{\&gamma;}\left(k\right)\end{array}\right.---\left(2\right)$

式（1）、式（2）中，

状态向量

ω_ie为地球自转角速率；

V_N、V_U、V_E分别为惯导北向、天向、东向速度；

f_N、f_U、f_E分别为惯导北向、天向、东向加速度；

λ、

h分别为惯导经度、纬度、高度；

R_M、R_N分别为地球子午面、卯酉面半径；

δV_N、δV_U、δV_E分别为惯导北向、天向、东向速度误差；

φ_N、φ_U、φ_E分别为惯导北向、天向、东向失准角；

δh、δλ分别为惯导纬度误差、高度误差、经度误差；

分别为惯导载体系x轴、y轴、z轴加速度计零位误差；

ε_x、ε_y、ε_z分别为惯导载体系x轴、y轴、z轴陀螺常值漂移；

z(k)为观测量；

H(k)为量测矩阵；

w(k)为系统噪声；

γ(k)为观测噪声；

Γ(k)为系统噪声矩阵；

Φ(k,k-1)为离散状态转移矩阵；

分别为惯导载体系x轴、y轴、z轴加速度计测量噪声；

δε_x、δε_y、δε_z分别为惯导载体系x轴、y轴、z轴陀螺测量噪声；

为惯导姿态阵；

卡尔曼滤波估计误差方差P_k和一步预测估计误差方差P_k/k-1分解成UDU^T的形式，即

$P_k=U_k{D}_k{U}_k^T,$ 记为 $\hat{P}=\hat{U}\hat{D}{\hat{U}}^T---\left(3\right)$

$P_{k/k-1}=U_{k/k-1}{D}_{k/k-1}{U}_{k/k-1}^T,$ 记为 $\tilde{P}=\tilde{U}\tilde{D}{\tilde{U}}^T---\left(4\right)$

UD自适应卡尔曼滤波器的初始参数设置包括：

一步预测误差方差分解阵

一步预测误差方差分解阵

估计误差方差分解阵

根据所采用的惯导系统精度设置估计误差方差分解阵

系统噪声方差阵Q＝E[w(k)w^T(k)]、噪声阵Γ；

量测噪声方差阵R＝E[γ(k)γ^T(k)]，

E表示数学期望；

w(k)为系统噪声；

γ(k)为观测噪声。

3.根据权利要求2所述的测绘车用惯导系统零速修正方法，其特征在于：步骤2）具体包括以下步骤：

以T_n为周期进行捷联惯导姿态、速度、位置更新，同时计算离散状态转移矩阵Φ_k,k-1；

离散状态转移矩阵计算公式如下：

$\mathrm{\&Phi;}(k+{\mathrm{iT}}_n,k)=I_{15\&times;15}+\underset{j=1}{\overset{i}{\mathrm{\&Sigma;}}}{A}_{k+j{T}_n}{T}_n---\left(5\right)$

其中， $A=\left[\begin{array}{cc}F& G\\ 0_{6\&times;9}& 0_{6\&times;6}\end{array}\right],$ $G=\left[\begin{array}{cc}{G}_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& -C_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right],$

为惯导姿态阵，I_15×15为15维单位阵，T_n为导航周期。

4.根据权利要求3所述的测绘车用惯导系统零速修正方法，其特征在于：步骤3）具体包括以下步骤：

以T_z为周期，计算速度积分观测量和量测矩阵：

观测量：

$Z\left(k\right)=\underset{i=1}{\overset{50}{\mathrm{\&Sigma;}}}{V}^{\mathrm{INS}}(k-1+{\mathrm{iT}}_z)T_z---\left(6\right)$

量测矩阵为：

$H\left(k\right)=\left[\underset{i=1}{\overset{50}{\mathrm{\&Sigma;}}}\hat{H}\mathrm{\&Phi;}(k+i{T}_n,k)\right]{\mathrm{\&Phi;}}^{-1}(k+1,k)T_z---\left(7\right)$

其中，V^INS＝[V_N V_U V_E]^T，

5.根据权利要求4所述的测绘车用惯导系统零速修正方法，其特征在于：步骤4）具体包括以下步骤：

卡尔曼滤波时间更新计算周期设置为T_f，导航计算时间达到T_f的整倍数时，进行UD分解卡尔曼滤波时间更新计算，计算公式如下：

状态一步预测计算：

$\tilde{X}(k,k-1)=\mathrm{\&Phi;}(k,k-1)\hat{X}(k-1),\hat{X}\left(0\right)=0_{1\&times;15}---\left(8\right)$

一步预测均方差计算：

为主对角元全为1的上三角阵，

为对角阵，则和

按以下各式确定：

${\tilde{D}}_j=W_j^{n-j}D{\left[W_j^{n-j}\right]}^T,j=\mathrm{15,14},...2---\left(9\right)$

${\tilde{U}}_{i,j}=\left\{\begin{array}{cc}\frac{W_i^{(n-j)}D{\left[W_i^{(n-j)}\right]}^T}{{\tilde{D}}_j}& {\tilde{D}}_j\&GreaterEqual;1.0e-25\\ 0& {\tilde{D}}_j<1.0e-25\end{array}\right.,j=\mathrm{15,14},...2,i=\mathrm{1,2},...j-1---\left(10\right)$

$W_i^{n-j+1}=W_i^{n-j}-{\tilde{U}}_{i,j}{W}_j^{n-j},j=\mathrm{15,14},...2,i=\mathrm{1,2},...j-1---\left(11\right)$

${\tilde{D}}_1=W_1^{n-1}D{\left[W_1^{n-1}\right]}^T---\left(12\right)$

式中

$D=\mathrm{diag}{\left[\begin{array}{cc}\hat{D}& Q\end{array}\right]}_{15\&times;21}---\left(13\right)$

$W^{\left(0\right)}=\left[\begin{array}{cc}\mathrm{\&Phi;}\hat{U}& \mathrm{\&Gamma;}\end{array}\right]={\left[\begin{array}{c}{W}_1^{\left(0\right)}\\ W_2^{\left(0\right)}\\ .\\ .\\ .\\ W_n^{\left(0\right)}\end{array}\right]}_{15\&times;21}---\left(14\right)$

为15维行向量；

令 $\hat{X}\left(k\right)=\tilde{X}(k,k-1),$ $\hat{U}=\tilde{D},$ $\hat{D}=\tilde{D},$ 则卡尔曼滤波时间更新结束。

6.根据权利要求5所述的测绘车用惯导系统零速修正方法，其特征在于：步骤5）中，距离上次零速修正或对准结束的时间达到T_zc后，向用户申请零速修正。

7.根据权利要求6所述的测绘车用惯导系统零速修正方法，其特征在于：步骤6）具体包括以下步骤：

当惯导系统处于零速状态，且允许零速修正后，进行UD卡尔曼滤波量测更新，量测更新周期为T_f；

按公式（15）~（23）进行量测更新计算；

$f^{\left(n\right)}={\left({\hat{U}}^{(n-1)}\right)}^T{\left(H^{\left(n\right)}\right)}^T,n=\mathrm{1,2,3},$ 量测矩阵 $H=\left[\begin{array}{c}{H}^{\left(1\right)}\\ H^{\left(2\right)}\\ H^{\left(3\right)}\end{array}\right]---\left(15\right)$

$g_i^{\left(n\right)}={\hat{D}}_i^{(n-1)}{f}_i^{\left(n\right)},i=\mathrm{1,2},...15,n=\mathrm{1,2,3}---\left(16\right)$

${\mathrm{\&alpha;}}_j^{\left(n\right)}={\mathrm{\&alpha;}}_{j-1}^{\left(n\right)}+f_i{g}_i^{\left(n\right)},j=\mathrm{1,2},...,15,{\mathrm{\&alpha;}}_0^{\left(n\right)}=r\left(n\right),n=\mathrm{1,2,3}---\left(17\right)$

${\mathrm{\&lambda;}}_j^{\left(n\right)}=-f_i^{\left(n\right)}/{\mathrm{\&alpha;}}_{j-1}^{\left(n\right)},j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(18\right)$

则：

${\hat{D}}_j^{\left(n\right)}={\hat{D}}_j^{(n-1)}\frac{{\mathrm{\&alpha;}}_{j-1}^{\left(n\right)}}{{\mathrm{\&alpha;}}_j^{\left(n\right)}},j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(19\right)$

${\hat{U}}_{\mathrm{ij}}^{\left(n\right)}={\hat{U}}_{\mathrm{ij}}^{(n-1)}+{\mathrm{\&lambda;}}_j^{\left(n\right)}(g_i^{\left(n\right)}+\underset{k=i+1}{\overset{j-1}{\mathrm{\&Sigma;}}}{\hat{U}}_{\mathrm{ik}}^{(n-1)}{g}_k^{\left(n\right)}),j=\mathrm{2,3},\&CenterDot;\&CenterDot;\&CenterDot;,15,i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,j-1,n=\mathrm{1,2,3}---\left(20\right)$

${\hat{U}}_{\mathrm{ii}}^{\left(n\right)}=1,i=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,15,n=\mathrm{1,2,3}---\left(21\right)$

其中，H⁽ⁿ⁾为1×15的行向量， ${\hat{U}}^{\left(0\right)}=\tilde{U},$ ${\hat{D}}^{\left(0\right)}=\tilde{D},$ $R=\left[\begin{array}{ccc}r\left(1\right)& 0& 0\\ 0& r\left(2\right)& 0\\ 0& 0& r\left(3\right)\end{array}\right];$

滤波增益K_k按照下式进行计算：

$K_k^{\left(n\right)}=\frac{{\hat{U}}^{(n-1)}{\hat{D}}^{(n-1)}{f}^{\left(n\right)}}{{\mathrm{\&alpha;}}_{\mathrm{IN}\_\mathrm{DX}}^{\left(n\right)}},n=\mathrm{1,2,3}---\left(22\right)$

状态估计计算：

$X_k^{\left(n\right)}=X_k^{(n-1)}+K_k^{\left(n\right)}(Z_k^{\left(n\right)}-H_k^{\left(n\right)}{X}_k^{(n-1)}),Z_k=\left[\begin{array}{c}{Z}_k^{\left(1\right)}\\ Z_k^{\left(2\right)}\\ Z_k^{\left(3\right)}\end{array}\right]n=\mathrm{1,2,3}---\left(23\right)$

其中， ${\hat{X}}_k^{\left(0\right)}={\hat{X}}_{k/k-1};$

滤波最优估计结果为： ${\hat{X}}_k={\hat{X}}_k^{\left(3\right)},$ $K_k=K_k^{\left(3\right)},$ $\hat{U}={\hat{U}}^{\left(3\right)}$ $\hat{D}={\hat{D}}^{\left(3\right)};$

式中：Q—系统噪声的协方差矩阵；R—观测噪声的协方差矩阵。

8.根据权利要求7所述的测绘车用惯导系统零速修正方法，其特征在于：步骤7）具体包括以下步骤：

当卡尔曼滤波量测更新时间大于T后，停止卡尔曼滤波量测更新，并利用滤波估计结果

对惯导水平姿态、速度、位置误差进行修正，告知用户本次零速修正结束，车辆可以开动；

速度修正：

$V_N={\hat{V}}_N-{\hat{X}}_k\left(1\right),$ $V_N={\hat{V}}_U-{\hat{X}}_k\left(2\right),$ $V_N={\hat{V}}_E-{\hat{X}}_k\left(3\right);$

为修正前惯导北速、天速、东速；

位置修正：

$\mathrm{\&lambda;}=\widehat{\mathrm{\&lambda;}}-{\hat{X}}_k\left(9\right),$

$h=\hat{h}-{\hat{X}}_k\left(8\right);$

为修正前惯导北速、天速、东速；

水平姿态修正：

$C_b^n=\left[\begin{array}{ccc}1& -{\hat{X}}_k\left(5\right)& 0\\ {\hat{X}}_k\left(5\right)& 1& -{\hat{X}}_k\left(3\right)\\ 0& {\hat{X}}_k\left(3\right)& 1\end{array}\right]C_b^{n^{\&prime;}}.$

9.根据权利要求8所述的测绘车用惯导系统零速修正方法，其特征在于：

R＝E[γ(k)γ^T(k)]＝diag{(0.01)²,(0.01)²,(0.01)²}；

5ms≤T_n≤20ms；

50ms≤T_f≤1s；

T_zc=10min；

T_f＝1s；

10s≤T_gx≤30s。

Images