CN102928816B - 一种面向隧道环境的车辆高可靠组合定位方法 - Google Patents

Abstract

本发明公开了一种面向隧道环境的车辆高可靠组合定位方法，首先利用射频识别定位算法进行车辆的初步定位，获取含有噪声的位置信息，为了进一步提高初步定位的定位精度，针对前轮转向四轮车辆，结合隧道环境建立车辆定位的扩展卡尔曼滤波模型，以方向盘转角和纵向加速度传感器所输出的信息作为系统状态方程外部输入量，以初步定位获取的含有噪声的车辆位置信息以及利用轮速传感器测量与计算得到的车辆前向速度、车辆横摆角速度作为系统观测量，建立扩展卡尔曼滤波模型的观测方程，最终通过扩展卡尔曼滤波递推算法准确、实时、可靠的推算出隧道环境内车辆的精确位置信息，实现车辆在隧道环境下的准确、实时、可靠的定位。

Description

一种面向隧道环境的车辆高可靠组合定位方法

技术领域

本发明涉及一种面向隧道环境的车辆高可靠组合定位方法，其目的在于当全球定位系统GPS在隧道环境中因信号遮挡而无法对车辆进行准确定位时，利用射频识别定位算法进行车辆的初步定位，获取含有噪声的初步位置信息，利用扩展卡尔曼滤波算法对获取的初步位置信息进行滤波、递推，实现车辆位置信息的精确估计，解决车辆的准确、高可靠的定位问题，具有可靠性高、准确性高、实时性好，成本低的优点，属于车辆导航定位领域。

背景技术

随着交通行业的迅速发展，随之出现的交通问题也日趋明显，智能运输系统ITS成为了世界公认的解决交通问题的有效途径。车辆定位技术是智能运输系统的关键技术之一，如何准确、可靠对车辆进行定位成为研究的重点。

目前，车辆定位领域应用最多的是GPS技术。GPS能为动载体实时全天候地提供三维位置、速度和时间等信息，因而得到了广泛的应用。但当车辆行驶在隧道、立交桥等区域时，GPS因信号受到遮挡会出现定位不准甚至失效的问题。

为克服GPS的不足，各种组合导航的研究引起了广泛的重视。GPS/INS组合定位系统以及GPS/DR组合定位系统在GPS信号受遮挡区域主要依靠INS（即惯性导航）或DR（即航位推算，由陀螺仪和里程计构成）来推算定位。但当GPS出现长时间的定位不准或失效时，无论INS还是DR将累积大的误差。针对INS和DR的不足，MM（即地图匹配技术）近年来引入到GPS/INS或GPS/DR组合导航系统中，但是，MM的精度取决于数字地图的质量和地图匹配算法的精度，在道路密集区域当GPS发生长时间的失效，MM常会导致错误的定位结果。

总体看来，这些基于GPS的组合定位方法依然无法满足隧道（尤其是长隧道）等GPS信号受遮挡环境内车辆准确、可靠定位的需要。

除GPS外，近年来其它无线定位技术也得到了快速的发展，如射频识别（Radio FrequencyIDentification，RFID）定位、蜂窝移动定位（Cellular-based）、超宽带无线（Ultra Wide Band，UWB）、无线局域网络（Wireless Local AreaNetworks，WLAN）以及蓝牙（Bluetooth）等，并主要应用于室内定位。特别是随着主动式RFID技术的成熟，RFID在室内定位中的运用比较成熟，各种定位算法和定位技术的研究也取得了很大的进展，但尚未应用于户外隧道环境下的定位导航，同时由于传感器误差和测量误差，所获取的位置信息往往含有噪声。

发明内容

本发明的目的是克服现有技术的不足，提出一种面向隧道环境的车辆高可靠组合定位方法，本方法在GPS信号受遮挡的隧道环境下利用射频识别定位算法进行车辆的初步定位，得到含有噪声的车辆的初步位置信息，同时融合车载方向盘转角传感器、纵向加速度传感器以及轮速传感器信息，利用扩展卡尔曼滤波对初步定位得到初步位置信息进行滤波、递推得到车辆位置信息的准确估计，具有可靠性高、准确性高、实时性好，成本低的优点。

本发明采取的技术方案如下：一种面向隧道环境的车辆高可靠组合定位方法，其特征在于：利用射频识别定位算法进行车辆的初步定位，获取含有噪声的初步位置信息，为了进一步提高初步定位的定位精度，针对前轮转向四轮车辆，结合隧道环境建立车辆定位的扩展卡尔曼滤波模型，以方向盘转角和加速度传感器所输出的信息作为系统状态方程外部输入量，以初步定位获取的含有噪声的车辆位置信息以及利用轮速传感器测量与计算得到的车辆前向速度、车辆横摆角速度作为系统观测量，通过扩展卡尔曼滤波递推算法实时、准确、可靠的推算出隧道环境内车辆的精确位置信息，具体步骤如下：

步骤1）在隧道环境下布置有源主动式射频识别电子标签tag，在车辆上布置射频识别读写器reader

在隧道壁体两侧无遮挡位置，每距离10-20米等间隔地布置有源主动式射频识别电子标签tag，以正东为ox指向，以正北为oy指向，原点为o，建立地理坐标系xoy，原点o选取在地球表面一个固定点上，得到地理坐标系xoy下的所有有源主动式射频识别电子标签tag的坐标，将射频识别读写器reader固定在行驶的车辆的车顶，此时射频识别读写器reader在地理坐标系的位置坐标即被认为是车辆在地理坐标系中的位置坐标，并近似认为有源主动式射频识别电子标签tag和射频识别读写器reader处于同一平面上；

步骤2）选择用于实时定位的有效标签

根据射频识别读写器reader实时接收到的有源主动式射频识别电子标签tag在每个离散时刻k发出的信号，k=1,2,3,…，k为正整数，将最大的四个接收信号强度值所对应的四个有源主动式射频识别电子标签tag作为用于实时定位的四个有效标签，四个有效标签在地理坐标系xoy中的坐标分别为(x₁(k)，y₁(k))、（x₂(k)，y₂(k)）、（x₃(k)，y₃(k)）、（x₄(k)，y₄(k)），且x₁(k)、y₁(k)、x₂(k)、y₂(k)、x₃(k)、y₃(k)、x₄(k)、y₄(k)分别表示离散时刻k这四个有效标签在地理坐标系中对应的坐标值，这些坐标值为已知；

步骤3）利用射频识别定位算法进行车辆初步定位

步骤3.1）拟合出射频识别读写器reader和有源主动式射频识别电子标签tag之间距离d和接收信号强度值I的关系

d＝ae^uI+ce^rI，

其中，d表示射频识别读写器reader与有源主动式射频识别电子标签tag之间的距离，其单位为米，I表示由射频识别读写器reader接收并得到的来自相应有源主动式射频识别电子标签tag的接收信号强度值，无量纲，e为自然底数且e=2.71828，a、u、c、r分别表示相应的系数常量，a=171.8，u=-1.27，c=83.05，r=-0.05354；

分别求得离散时刻k的射频识别读写器reader和四个有效标签之间的距离d₁(k)、d₂(k)、d₃(k)、d₄(k)：

$d_1\left(k\right)={\mathrm{ae}}^{{\mathrm{uI}}_1\left(k\right)}+{\mathrm{ce}}^{{\mathrm{rI}}_1\left(k\right)}---\left(1\right)$

$d_2\left(k\right)={\mathrm{ae}}^{{\mathrm{uI}}_2\left(k\right)}+{\mathrm{ce}}^{{\mathrm{rI}}_2\left(k\right)}---\left(2\right)$

$d_3\left(k\right)={\mathrm{ae}}^{{\mathrm{uI}}_3\left(k\right)}+{\mathrm{ce}}^{r{I}_3\left(k\right)}---\left(3\right)$

$d_4\left(k\right)={\mathrm{ae}}^{{\mathrm{uI}}_4\left(k\right)}+{\mathrm{ce}}^{{\mathrm{rI}}_4\left(k\right)}---\left(4\right)$

其中，d₁(k)、d₂(k)、d₃(k)、d₄(k)分别表示离散时刻k的射频识别读写器reader和四个有效标签之间的距离，其单位为米，I₁(k)、I₂(k)、I₃(k)、I₄(k)分别表示由射频识别读写器reader接收并得到的来自四个有效标签的离散时刻k的接收信号强度值，无量纲，e为自然底数且e=2.71828，a、u、c、r分别表示相应的系数常量，具体取值同上；

步骤3.2）利用平面上两点间的距离公式，建立车辆位置方程组

$\left\{\begin{array}{c}\sqrt{{[x_m\left(k\right)-x_1\left(k\right)]}^2+{[y_m\left(k\right)-y_1\left(k\right)]}^2}=d_1\left(k\right)\\ \sqrt{{[x_m\left(k\right)-x_2\left(k\right)]}^2+{[y_m\left(k\right)-y_2\left(k\right)]}^2}=d_2\left(k\right)\\ \sqrt{{[x_m\left(k\right)-x_3\left(k\right)]}^2+{[y_m\left(k\right)-y_3\left(k\right)]}^2}=d_3\left(k\right)\\ \sqrt{{[x_m\left(k\right)-x_4\left(k\right)]}^2+{[y_m\left(k\right)-y_4\left(k\right)]}^2}=d_4\left(k\right)\end{array}\right.---\left(5\right)$

其中，x_m(k)和y_m(k)分别表示离散时刻k射频识别读写器reader在地理坐标系中的坐标值，为未知待求量；

步骤3.3）利用最小二乘法求解位置方程组，计算出射频识别读写器reader在地理坐标系中的坐标值x_m(k)，y_m(k)；

步骤4）利用扩展卡尔曼滤波算法进行车辆精确定位

步骤4.1）建立隧道环境下车辆定位的扩展卡尔曼滤波模型的状态方程

针对隧道环境下的车辆定位，建立车辆载体坐标系，其原点o_b选取在车辆质心处，o_bx_b轴沿车辆的纵向轴并与车辆纵向前进方向一致，o_bz_b轴垂直于车辆运行平面并指向地面的反方向，而o_by_b轴按右手螺旋规则可确定，将方向盘转角传感器安装在待定位车辆的方向盘上，将纵向加速度传感器安装在待定位车辆的车内，且将两个轮速传感器分别安装在车辆的左后轮和右后轮上，在离散时刻k，取系统状态变量

其中，X(k)表示离散时刻k的系统状态变量，x(k)、y(k)分别表示离散时刻k，在地理坐标系中待定位车辆的坐标值，

分别表示在离散时刻k车辆航向角、车辆纵向前进速度、车辆侧向速度、车辆横摆角速度，表示向量

的转置，根据运动学和动力学原理，离散化后的扩展卡尔曼滤波模型的状态方程表示为：

X(k)＝f(X(k-1),U(k-1),W(k-1),γ(k-1))    （6）

式（6）中，f表示系统状态函数向量，X(k-1)表示离散时刻k-1的系统状态变量；

U(k-1)表示离散时刻k-1系统状态方程的外输入向量，且 $U(k-1)={\left[\begin{array}{cc}\mathrm{\δ}(k-1)& a_{x_b}(k-1)\end{array}\right]}^{\′},$ 其中δ(k-1)表示在离散时刻k-1车辆的前轮转向角，由方向盘转向角传感器测得的方向盘转角信息除以方向盘与前轮的转向传动比来确定，

表示在离散时刻k-1车辆的纵向加速度，通过纵向加速度传感器测得， ${\left[\begin{array}{cc}\mathrm{\δ}(k-1)& a_{x_b}(k-1)\end{array}\right]}^{\′}$ 表示向量 $\left[\begin{array}{cc}\mathrm{\δ}(k-1)& a_{x_b}(k-1)\end{array}\right]$ 的转置；

W(k-1)表示在离散时刻k-1的零均值系统高斯白噪声向量且W(k-1)＝[w₁ w₂ w₃ w₄ w₅ w₆]′，其中w₁、w₂、w₃、w₄、w₅及w₆分别表示六个系统高斯白噪声分量，[w₁ w₂ w₃ w₄ w₅ w₆]′表示向量[w₁ w₂ w₃ w₄ w₅ w₆]的转置；

γ(k-1)表示在离散时刻k-1系统外输入对应的零均值高斯白噪声向量且 $\mathrm{\γ}(k-1)={\left[\begin{array}{cc}{w}_{\mathrm{\δ}}& w_{a_{x_b}}\end{array}\right]}^{\′},$ 其中w_δ、

分别表示直测的系统外输入δ(k-1)、

对应的零均值高斯白噪声，这些噪声包含在系统状态函数向量f的两个系统外输入里面， ${\left[\begin{array}{cc}{w}_{\mathrm{\δ}}& w_{a_{x_b}}\end{array}\right]}^{\′}$ 表示向量 $\left[\begin{array}{cc}{w}_{\mathrm{\δ}}& w_{a_{x_b}}\end{array}\right]$ 的转置，非线性的离散化后的系统状态函数向量为：

$f(X(k-1),U((k-1),W(k-1),\mathrm{\γ}(k-1))=\left[\begin{array}{c}{f}_1(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_2(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_3(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_4(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_5(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_6(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\end{array}\right]$

其中，f₁、f₂、f₃、f₄、f₅、f₆分别表示系统状态函数向量f的六个系统状态函数向量分量，且

式（7）中，β₁、β₂、β₃、β₄、β₅、β₆分别表示

的中间变量，即

${\mathrm{\β}}_1=\frac{-2(C_{\mathrm{af}}+C_{\mathrm{ar}})}{M}$ ${\mathrm{\β}}_2=\frac{2(-C_{\mathrm{af}}{l}_{\mathrm{front}}+C_{\mathrm{ar}}{l}_{\mathrm{rear}})}{M}$ ${\mathrm{\β}}_3=\frac{2C_{\mathrm{af}}}{M}$

${\mathrm{\β}}_4=\frac{2(-C_{\mathrm{af}}{l}_{\mathrm{front}}+C_{\mathrm{ar}}{l}_{\mathrm{rear}})}{J_{z_b}}$ ${\mathrm{\β}}_5=\frac{-2(C_{\mathrm{af}}{l_{\mathrm{front}}}^2+C_{\mathrm{ar}}{l_{\mathrm{rear}}}^2)}{J_{z_b}}$ ${\mathrm{\β}}_6=\frac{2C_{\mathrm{af}}{l}_{\mathrm{front}}}{J_{z_b}}$ ，在式(7)

及β₁、β₂、β₃、β₄、β₅、β₆的表达式中，M和

分别表示车辆的质量和车辆绕o_bz_b轴的转动惯量，l_front是车辆前轮轮轴中心到质心的距离，l_rear是车辆后轮轮轴中心到质心的距离，C_af、C_ar分别表示车辆前、后轮胎的侧偏刚度，T表示离散的周期（T为500毫秒、1000毫秒或2000毫秒），W(k-1)对应的系统噪声协方差阵Q(k-1)为：

$Q(k-1)=\left[\begin{array}{cccccc}{{\mathrm{\σ}}_{w_1}}^2& 0& 0& 0& 0& 0\\ 0& {{\mathrm{\σ}}_{w_2}}^2& 0& 0& 0& 0\\ 0& 0& {{\mathrm{\σ}}_{w_3}}^2& 0& 0& 0\\ 0& 0& 0& {{\mathrm{\σ}}_{w_4}}^2& 0& 0\\ 0& 0& 0& 0& {{\mathrm{\σ}}_{w_5}}^2& 0\\ 0& 0& 0& 0& 0& {{\mathrm{\σ}}_{w_6}}^2\end{array}\right],$ 其中

及

分别表示系统高斯白噪声w₁、w₂、w₃、w₄、w₅及w₆对应的方差，γ(k-1)对应的系统外部输入噪声的协方差阵为Γ(k-1)且

其中σ_δ ²、

分别表示w_δ、

对应的方差；

步骤4.2）建立隧道环境下车辆定位的扩展卡尔曼滤波模型的观测方程以射频定位算法计算输出的地理坐标系下的车辆位置x_m(k)、y_m(k)以及利用轮速传感器测量与计算得到的离散时刻k车辆纵向前进速度

车辆横摆角速度

作为扩展卡尔曼滤波模型的观测量，且

其中，v_rr(k)和v_lr(k)分别表示车辆右后轮和左后轮的线速度，可以分别通过安装在车辆右后轮和左后轮的轮速传感器测量值乘以相应的轮胎半径得到，D表示车辆右后轮和左后轮之间的距离。离散化后的扩展卡尔曼滤波模型的观测方程为：

Z(k)＝H(k)·X(k)+V(k)       （8）

其中，Z(k)、H(k)分别为离散时刻k扩展卡尔曼滤波模型的观测向量和观测阵，V(k)表示离散时刻k的零均值观测白噪声向量，且其与离散时刻k的零均值系统高斯白噪声向量W(k)互不相关，且 $Z\left(k\right)={\left[\begin{array}{cccc}{x}_m\left(k\right)& y_m\left(k\right)& v_{x_{b_m}}\left(k\right)& {\mathrm{\ω}}_{z_{b_m}}\left(k\right)\end{array}\right]}^{\′},$ ${\left[\begin{array}{cccc}{x}_m\left(k\right)& y_m\left(k\right)& v_{x_{b_m}}\left(k\right)& {\mathrm{\ω}}_{z_{b_m}}\left(k\right)\end{array}\right]}^{\′}$ 表示向量 $\left[\begin{array}{cccc}{x}_m\left(k\right)& y_m\left(k\right)& v_{x_{b_m}}\left(k\right)& {\mathrm{\ω}}_{z_{b_m}}\left(k\right)\end{array}\right]$ 的转置， $H\left(k\right)=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],$ $V\left(k\right)=\left[\begin{array}{c}{n}_x\\ n_y\\ n_{v_{x_b}}\\ n_{{\mathrm{\ω}}_{z_b}}\end{array}\right],$ 其中n_x是车辆沿正东方向位置的观测噪声且n_x是均值为0、方差为的高斯白噪声，

表示n_x的方差，n_y是车辆沿正北方向的位置的观测噪声且n_y是均值为0、方差为的高斯白噪声，表示n_y的方差，

是车辆纵向前进速度的观测噪声且

是均值为0、方差为的高斯白噪声，

表示

的方差，

是车辆横摆角速度的观测噪声且

是均值为0、方差为

的高斯白噪声，

表示

的方差，V(k)对应的观测噪声方差阵R(k)可表示为： $R\left(k\right)=\left[\begin{array}{cccc}{{\mathrm{\σ}}_x}^2& 0& 0& 0\\ 0& {{\mathrm{\σ}}_y}^2& 0& 0\\ 0& 0& {\mathrm{\σ}}_{v_{x_b}}^2& 0\\ 0& 0& 0& {\mathrm{\σ}}_{{\mathrm{\ω}}_{z_b}}^2\end{array}\right];$

步骤4.3）进行扩展卡尔曼滤波的时间更新和测量更新，得到车辆的实时、精确的位置信息

对于式（6）和式（8）所描述的状态方程和观测方程，运用扩展卡尔曼滤波算法，建立标准滤波递推过程，该过程包括时间更新和测量更新：

时间更新：

状态一步预测方程：

$\hat{X}(k,k-1)=f(\hat{X}(k-1),U(k-1),\mathrm{0,0}),$ 其中

表示状态X的滤波计算值；

一步预测误差方差阵P(k,k-1)：

P(k,k-1)＝A(k,k-1)P(k-1)A′(k,k-1)+B(k,k-1)Γ(k-1)B′(k,k-1)+Q(k-1)

其中，A是系统状态函数向量f对状态向量X求偏导数的雅克比矩阵，A′表示矩阵A的转置，B是系统状态函数向量f对外部输入向量U求偏导数的雅克比矩阵，B′表示矩阵B的转置，矩阵A的第i行第j列元素A_[i，j]，i＝1,2,3,4,5,6，j=1,2,3,4,5,6，和矩阵B的第i行第t列元素B_[i，t]，i＝1,2,3,4,5,6，t＝1,2可分别通过下式求得：

$A_{[i,i]}=\frac{\∂f_i}{\∂x_j}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0})$

$B_{[i,t]}=\frac{\∂f_i}{\∂u_t}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0})$

各矩阵元素的取值如下：

A_[1，1]＝1A_[1，2]＝0

A_[1,6]＝0  A_[2，1]＝0  A_[2,2]＝1

A_[2,6]＝0 A_[3,1]＝A_[3,2]＝A_[3,4]＝A_[3,5]＝0 A_[3,3]＝1 A_[3,6]＝T

A_[4,1]＝A_[4,2]＝A_[4,3]＝A_[4,5]＝A_[4,6]＝0 A_[4,4]＝1

A_[5,1]＝A_[5,2]＝A_[5,3]＝0 $A_{\left[\mathrm{5,4}\right]}=T[-\frac{{\mathrm{\β}}_1}{{v_{x_b}}^2}{v}_{y_b}+(-\frac{{\mathrm{\β}}_2}{{v_{x_b}}^2}-1){\mathrm{\ω}}_{z_b}]$

$A_{\left[\mathrm{5,5}\right]}=1+T\frac{{\mathrm{\β}}_1}{v_{x_b}}$ $A_{\left[\mathrm{5,6}\right]}=T(\frac{{\mathrm{\β}}_2}{v_{x_b}}-v_{x_b})$ A_[6，1]＝A_[6,2]＝A_[6,3]＝0

$A_{\left[\mathrm{6,4}\right]}=T(-\frac{{\mathrm{\β}}_4}{{v_{x_b}}^2}{v}_{y_b}-\frac{{\mathrm{\β}}_5}{{v_{x_b}}^2}{\mathrm{\ω}}_{z_b})$ $A_{\left[\mathrm{6,5}\right]}=T\frac{{\mathrm{\β}}_4}{v_{x_b}}$ $A_{\left[\mathrm{6,6}\right]}=1+T\frac{{\mathrm{\β}}_5}{v_{x_b}}$

B_[1,1]＝B_[1,2]＝B_[2,1]＝B_[2,2]=B_[3,1]=B_[3,2]=B_[4,1]=B_[5,2]=B_[6,2]=0

B_[4,2]＝T    B_[5,1]＝Tβ₃    B_[6,1]＝Tβ₆

测量更新：

滤波增益矩阵K(k)：K(k)＝P(k,k-1)·H′(k)·[H(k)P(k,k-1)H′(k)+R(k)]^-1状态估计： $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}(k,k-1)]$

估计误差方差阵P(k)：P(k)＝[E-K(k)·H(k)]·P(k,k-1)

其中，H′(k)表示矩阵H(k)的转置，上角标-1表示对矩阵求逆，E为6×6的单位阵。

本发明的优点及显著效果：

1.本发明针对隧道环境的车辆定位提出，在GPS信号被遮挡的隧道环境内利用射频识别定位算法进行车辆的初步定位，为了进一步提高初步定位的定位精度，利用扩展卡尔曼滤波算法进行车辆的进一步准确定位。射频识别定位算法使车辆在隧道环境内能够可靠、实时的定位，扩展卡尔曼滤波算法保证了车辆准确、实时的定位，解决了隧道环境内车辆准确、可靠、实时定位的问题。

2.本发明针对隧道环境，提出一种基于低成本的射频识别芯片，车载方向盘转角传感器、纵向加速度传感器以及轮速传感器同时融合扩展卡尔曼滤波算法的组合导航定位方法，组合算法简便易行，准确性和可靠性高，实时性强。

3.本发明通过实验，经过比较和统计其它几种常用函数，发现指数函数式具有最好的拟合度，最终选用指数函数拟合出射频识别读写器reader和有源主动式射频识别电子标签tag之间距离和接收信号强度值的关系，并利用最小二乘法求解出车辆所在地理坐标系的位置信息，提高了计算的精度。

4.本发明所用的射频识别芯片，车载方向盘转角传感器，纵向加速度传感器以及轮速传感器，成本低，组合定位效果好，有利于推广应用。

附图说明

图1是本发明的流程图。

图2是本发明利用射频识别定位算法进行车辆初步定位的流程图。

图3是本发明利用扩展卡尔曼滤波算法进行车辆进一步准确定位流程图。

图4是有源主动式射频识别电子标签tag和射频识别读写器reader在隧道环境下的布局图。

图5是射频识别读写器reader与有源主动式射频识别电子标签tag之间的距离和接收信号强度值关系的常见拟合函数的拟合曲线比较图。

图6是车辆动力学模型图。

图4中主要元件及符号说明：

…,q,q+1,…,…,p,p+1,…固定在隧道壁体两侧的有源主动式射频识别电子标签tag；

s    安装在车辆顶部的射频识别读写器reader；

g    行驶在隧道下的车辆；

h    隧道壁体的两侧；

d₁(k)、d₂(k)、d₃(k)、d₄(k)离散时刻k射频识别读写器reader分别和四个有效电子标签的距离。

具体实施方式

随着人民生活水平的提高，车辆已经越来越多的进入普通家庭，同时，交通拥挤加剧，交通事故频发，交通环境恶化等交通问题也随之而来。随着这些问题的产生，智能交通系统（ITS）应运而生。车辆导航定位在智能交通系统中起着重要的作用，如何准确地、实时地、可靠地确定车辆的位置成为研究的热点。目前常用的几种车辆导航定位系统有地图匹配（MM），航位推算（DR），惯性导航系统（INS），卫星导航（GNSS）等。

地图匹配（MM）是一种通过纯软件的方法来校正卫星定位或航位推算定位等定位方法的定位误差的技术。其应用是基于用于匹配的数字化地图包含高精度的道路信息以及被定位的车辆始终在道路上行驶两种假设为基础的。这种方法在假设成立的条件下能够提高车辆定位的精度，但是如果上述假设不成立，地图匹配将产生错误的输出，并导致系统性能的严重下降，而且这种方法不能单独使用，必须结合其它定位方法进行使用。航位推算（DR），采用罗盘仪、陀螺仪、里程计等传感设备测量出车辆的距离、方位来推算出位置和速度信息，在短时间内这些传感器所获得的位置相对精度较高，但由于陀螺仪的漂移及里程计的系统误差等因素的影响，定位误差会随着推算时间的增加而变大。惯性导航系统（INS），由加速度计和陀螺仪组成，是一种自主导航方法。它完全依靠设备自主地完成导航任务，和外界不发生任何光电联系，因此隐蔽性好，工作不受天气条件的限制，由于导航信息经过积分而产生，定位误差随时间增大，长时间工作精度和可靠性逐步下降，每次使用之前都需要长时间的初始对准。在卫星导航（GNSS）中，可选择的卫星导航系统主要有美国的全球定位系统（GPS），俄罗斯的全球卫星导航系统（GLONASS）以及中国的北斗系统等。

以上各种定位方法中以GPS的应用最为广泛和成熟，这种定位方法具有全天候、全球性以及能实时定位，定位精度高等优点但它同时也存在着致命的弱点,它只有在条件理想的户外环境才能提供准确、连续的信息，一旦信号被隧道、立交桥等遮挡时，GPS信号受到严重干扰，不能准确输出有效信息，定位精度不能满足要求。在这种情况下，研究如何利用其他辅助导航技术手段进行组合导航定位，解决单一GPS导航定位系统的定位盲区问题，提高车辆导航定位的可持续导航定位能力及可靠性成为当今车辆导航定位的热点研究方向。

目前常见的组合导航定位主要有GPS/MM（地图匹配）组合导航定位，GPS/DR（航位推算）组合导航定位，GPS/DR/MM组合导航定位，GPS/INS（惯性导航）组合导航定位等。在GPS/MM组合导航定位中，地图匹配可以通过纯软件的方法来校正卫星定位或航位推算定位等定位方法定位的误差，但是这种组合的缺点是当GPS信号丢失时，系统无法正常工作，定位系统的可靠性低。对于GPS/DR组合导航定位，当GPS信号丢失时，航位推算可以继续工作，但是航位推算系统由于本身存在系统误差，短期内航位推算精度高，但是长期推算精度将降低。GPS/DR/MM组合导航定位综合了GPS/MM和GPS/DR导航定位的优点，短期内可靠性和定位精度都得到了提高，但是由于航位推算系统的限制，随着时间的累积，定位精度降低。GPS/INS组合导航定位，短期内提高了导航和定位的精度，增强了整个系统的可靠性，但是INS长时间工作精度降低，且价格昂贵，不能大范围推广，一般应用于精度要求高的导航定位领域。

总体看来，这些基于GPS的组合定位方法依然无法满足隧道（尤其是长隧道）等GPS信号受遮挡环境内车辆准确、可靠定位的需要。

除GPS外，近年来其它无线定位技术也得到了快速的发展，如射频识别（Radio FrequencyIDentification，RFID）定位、蜂窝移动定位（Cellular-based）、超宽带无线（Ultra Wide Band，UWB）、无线局域网络（Wireless Local AreaNetworks，WLAN）以及蓝牙（Bluetooth）等，并主要应用于室内定位。

射频识别技术（RFID）是一种非接触式的自动识别技术，它可以自动识别目标对象，可以工作于各种恶劣环境，具有非接触、成本低、定位精度高、技术成熟等优点。RFID系统主要由射频识别读写器reader，射频识别电子标签tag，收发天线组成。根据射频识别电子标签tag工作所需能量的供给方式，可以将射频识别电子标签tag分为有源射频、无源射频两类。有源射频识别电子标签tag使用芯片内电池的能量、识别距离长，可达几十甚至上百米；无源射频识别电子标签tag不含电池，利用射频识别读写器reader发射的电磁波提供能量，它的发射距离受限制，一般是几十厘米，且需要射频识别读写器reader的发射功率大。根据射频识别调制方式可将射频识别电子标签tag分为主动式、被动式两类。主动式的射频识别电子标签tag用自身的能量主动地发送数据给射频识别读写器reader，且自带电池供电，它的电能充足，工作可靠性高，信号传输距离远，被动式的射频识别电子标签tag必须利用射频识别读写器reader的载波来调制自己的信号，发送距离短。通常情况下有源射频识别电子标签tag是主动式的，无源射频识别电子标签tag是被动式的。当前国内外对RFID定位技术的研究主要集中在定位算法和定位方案两方面，特别是随着主动式RFID技术的成熟，RFID在室内定位中的运用比较成熟，各种定位算法和定位技术的研究也取得了很大的进展，但尚未运用在室外隧道环境下的定位。

综上所述，现有的组合导航定位方法在对于隧道等GPS信号遮挡环境内的车辆导航定位都存在着各种不足，而无线射频识别技术具有诸多优点，同时其在室内定位方面具有相当的成熟度，为在隧道环境下实现对车辆的可靠、准确的定位，考虑到本发明的识别距离，本发明选取有源主动式射频识别电子标签tag，将其运用在隧道环境中，提出一种面向隧道环境的车辆高可靠组合定位方法，即基于低成本的射频识别芯片、车载方向盘转角传感器、纵向加速度传感器以及轮速传感器同时融合扩展卡尔曼滤波算法的组合导航定位方法，利用射频识别定位算法进行车辆初步定位，获取含有噪声的车辆初步位置信息，为了进一步提高初步定位的定位精度，通过建立扩展卡尔曼滤波模型，利用方向盘转角传感器和加速度传感器的输出值作为系统状态方程外输入值，利用射频识别定位算法初步定位获得的含有噪声的位置量以及利用轮速传感器测量与计算得到的车辆前向速度、车辆横摆角速度作为扩展卡尔曼滤波模型的观测量，通过滤波、递推，进一步提高车辆位置的估计精度，从而解决GPS信号被遮挡时车辆的准确、可靠的定位问题，具体步骤如下：

步骤1）在隧道环境下布置有源主动式射频识别电子标签tag，在车辆上布置射频识别读写器reader

本发明以正东为ox指向，以正北为oy指向，原点为o，建立地理坐标系xoy，原点o选取在地球表面一个固定点上，将有源主动式射频识别电子标签tag以10-20米的距离等间隔地布置在隧道壁体的两侧，得到地理坐标系xoy下的所有有源主动式射频识别电子标签tag的坐标，为了防止射频信号受到遮挡，将射频识别读写器reader（本发明选用北京博讯公司的JN5139型号的射频识别读写器reader和有源主动式射频识别电子标签tag，在无遮挡环境下通讯距离可以达到500米）安装在行驶的车辆的顶部，如图4所示，此时射频识别读写器reader在地理坐标系的位置坐标即被认为是车辆在地理坐标系中的位置坐标，并使有源主动式射频识别电子标签tag的安装高度略高于射频识别读写器reader距地面高度，使其信号传输不受遮挡，并可近似认为有源主动式射频识别电子标签tag和射频识别读写器reader处于同一平面上。通常情况下，有源主动式射频识别电子标签tag以一定时间间隔（可以人为设定，如100毫秒）以广播形式向外发送射频信号，在有源主动式射频识别电子标签tag通讯范围内的射频识别读写器reader都会读到该标签的相关数据，当安装有射频识别读写器reader的车辆行驶到布置有有源主动式射频识别电子标签tag的隧道时，射频识别读写器reader会接收到在其读写范围内有源主动式射频识别电子标签tag的信息，包括该有源主动式射频识别电子标签tag的位置坐标编码信息和接收信号强度RSS（Received SignalStrength）编码信息。

步骤2）选择用于实时定位的有效标签

根据射频识别读写器reader和有源主动式射频识别电子标签tag的通讯距离，结合有源主动式射频识别电子标签tag在隧道下的布局特点，通常情况下，射频识别读写器reader至少能够稳定的读到距其最近的四个有源主动式射频识别电子标签tag的信息，若读到的有源主动式射频识别电子标签tag的个数大于四个，则根据射频识别读写器reader实时接收到的有源主动式射频识别电子标签tag在每个离散时刻k发出的信号，k=1,2,3,…，k为正整数，最大值为10¹⁰，将最大的四个接收信号强度值所对应的四个有源主动式射频识别电子标签tag作为用于实时定位的四个有效标签，离散时刻k，四个有效标签在地理坐标系xoy中的坐标分别为(x₁(k)，y₁(k))、（x₂(k)，y₂(k)）、（x₃(k)，y₃(k)）、（x₄(k)，y₄(k)），且x₁(k)、y₁(k)、x₂(k)、y₂(k)、x₃(k)、y₃(k)、x₄(k)、y₄(k)分别表示离散时刻k这四个有效标签在地理坐标系中对应的坐标值，这些坐标值为已知；

步骤3）利用射频识别定位算法进行车辆初步定位

步骤3.1）拟合出射频识别读写器reader和有源主动式射频识别电子标签tag之间距离d和接收信号强度值I的关系

在射频识别定位中，通过大量的实验和多次统计（采用北京博讯公司的JN5139型号的射频芯片进行），本发明事先拟合出了射频识别读写器reader与有源主动式射频识别电子标签tag之间距离d和射频识别读写器reader接收的来自有源主动式射频识别电子标签tag的接收信号强度值I之间的关系，在拟合的过程中，本发明利用常见的一元二次函数，幂函数和指数函数来拟合距离d和接收信号强度值I之间的关系，综合比较这三个函数的残差平方和（SSE），均方根误差（RMSE），方程的确定系数（R-square）以及调整后的方程确定系数（AdjustedR-square），其中，SSE和RMSE的值越趋向于0表示函数拟合度越好，AdjustedR-square和R-square的值越趋向于1表示函数拟合度越好，具体的比较如表1所示，三个函数的具体拟合情况如图5所示，

表1三个常见函数的拟合情况对比表

最终，经过数据比较和统计分析，发现指数函数式具有最好的拟合度，因此本发明采用指数函数式

d＝ae^uI+ce^rI，

来确定d的值，其中，I表示由射频识别读写器reader接收并得到的来自有源主动式射频识别电子标签tag的接收信号强度值，无量纲，d表示射频识别读写器reader和相应有源主动式射频识别电子标签tag之间的距离，其单位为米，e为自然底数且e=2.71828，a、u、c、r分别表示相应的系数常量，由具体的实验确定，通过大量的实验和多次统计，最终取a=171.8，u=-1.27，c=83.05，r=-0.05354。

分别求得离散时刻k的射频识别读写器reader和四个有效标签之间的距离d₁(k)、d₂(k)、d₃(k)、d₄(k)：

$d_1\left(k\right)={\mathrm{ae}}^{{\mathrm{uI}}_1\left(k\right)}+{\mathrm{ce}}^{{\mathrm{rI}}_1\left(k\right)}---\left(1\right)$

$d_2\left(k\right)={\mathrm{ae}}^{{\mathrm{uI}}_2\left(k\right)}+{\mathrm{ce}}^{{\mathrm{rI}}_2\left(k\right)}---\left(2\right)$

$d_3\left(k\right)={\mathrm{ae}}^{{\mathrm{uI}}_3\left(k\right)}+{\mathrm{ce}}^{{\mathrm{rI}}_3\left(k\right)}---\left(3\right)$

$d_4\left(k\right)={\mathrm{ae}}^{{\mathrm{uI}}_4\left(k\right)}+{\mathrm{ce}}^{{\mathrm{rI}}_4\left(k\right)}---\left(4\right)$

其中，d₁(k)、d₂(k)、d₃(k)、d₄(k)分别表示离散时刻k的射频识别读写器reader和四个有效标签之间的距离，其单位为米，I₁(k)、I₂(k)、I₃(k)、I₄(k)分别表示由射频识别读写器reader接收并得到的来自四个有效标签的离散时刻k的接收信号强度值，无量纲，e为自然底数且e=2.71828，a、u、c、r分别表示相应的系数常量，具体取值同上；

步骤3.2）利用平面上两点间的距离公式，建立车辆位置方程组

在离散时刻k，设射频识别读写器reader所在地理坐标系的位置坐标为（x_m(k)，y_m(k)），x_m(k)和y_m(k)分别表示离散时刻k射频识别读写器reader在地理坐标系中的坐标值，为未知待求量，因为射频识别读写器reader和有源主动式射频识别电子标签tag近似处于同一个平面，由平面上两点间的距离公式可以得到：

$\left\{\begin{array}{c}\sqrt{{[x_m\left(k\right)-x_1\left(k\right)]}^2+{[y_m\left(k\right)-y_1\left(k\right)]}^2}=d_1\left(k\right)\\ \sqrt{{[x_m\left(k\right)-x_2\left(k\right)]}^2+{[y_m\left(k\right)-y_2\left(k\right)]}^2}=d_2\left(k\right)\\ \sqrt{{[x_m\left(k\right)-x_3\left(k\right)]}^2+{[y_m\left(k\right)-y_3\left(k\right)]}^2}=d_3\left(k\right)\\ \sqrt{{[x_m\left(k\right)-x_4\left(k\right)]}^2+{[y_m\left(k\right)-y_4\left(k\right)]}^2}=d_4\left(k\right)\end{array}\right.---\left(5\right)$

步骤3.3）利用最小二乘法求解位置方程组，计算出射频识别读写器reader在地理坐标系中的坐标值x_m(k)，y_m(k)

在式（5）中，d₁(k)、d₂(k)、d₃(k)、d₄(k)的值可以分别通过式（1）、式（2）、式（3）、式（4）求得，而x₁(k)、y₁(k)、x₂(k)、y₂(k)、x₃(k)、y₃(k)、x₄(k)、y₄(k)的值也可事先确定，因此式（5）的未知量为x_m(k)和y_m(k)，只要完成对式（5）的求解，就可以确定x_m(k)和y_m(k)的值，又因为式（5）是一个关于x_m(k)、y_m(k)的超定方程（方程个数大于未知量个数），由于用最小二乘法求解超定方程算法简单，误差小，精度高，因此本发明利用最小二乘法求解超定方程的解，具体解法如下：

首先将（5）式进行等价变形如下：

然后①-②、①-③、①-④、②-③、②-④、③-④，对（6）式处理得到下式：

$\left\{\begin{array}{c}[x_2\left(k\right)-x_1\left(k\right)]x_m\left(k\right)+[y_2\left(k\right)-y_1\left(k\right)]y_m\left(k\right)=\frac{1}{2}[{d_1}^2\left(k\right)-{d_2}^2\left(k\right)+{x_2}^2\left(k\right)-{x_1}^2\left(k\right)+{y_2}^2\left(k\right)-{y_1}^2\left(k\right)]\\ [x_3\left(k\right)-x_1\left(k\right)]x_m\left(k\right)+[y_3\left(k\right)-y_1\left(k\right)]y_m\left(k\right)=\frac{1}{2}[{d_1}^2\left(k\right)-{d_3}^2\left(k\right)+{x_3}^2\left(k\right)-{x_1}^2\left(k\right)+{y_3}^2\left(k\right)-{y_1}^2\left(k\right)]\\ [x_4\left(k\right)-x_1\left(k\right)]x_m\left(k\right)+[y_4\left(k\right)-y_1\left(k\right)]y_m\left(k\right)=\frac{1}{2}[{d_1}^2\left(k\right)-{d_4}^2\left(k\right)+{x_4}^2\left(k\right)-{x_1}^2\left(k\right)+{y_4}^2\left(k\right)-{y_1}^2\left(k\right)]\\ [x_3\left(k\right)-x_2\left(k\right)]x_m\left(k\right)+[y_3\left(k\right)-y_2\left(k\right)]y_m\left(k\right)=\frac{1}{2}[{d_2}^2\left(k\right)-{d_3}^2\left(k\right)+{x_3}^2\left(k\right)-{x_2}^2\left(k\right)+{y_3}^2\left(k\right)-{y_2}^2\left(k\right)]\\ {[x}_4\left(k\right)-x_2\left(k\right)]x_m\left(k\right)+[y_4\left(k\right)-y_2\left(k\right)]y_m\left(k\right)=\frac{1}{2}[{d_2}^2\left(k\right)-{d_4}^2\left(k\right)+{x_4}^2\left(k\right)-{x_2}^2\left(k\right)+{y_4}^2\left(k\right)-{y_2}^2\left(k\right)]\\ [x_4\left(k\right)-x_3\left(k\right)]x_m\left(k\right)+[y_4\left(k\right)-y_3\left(k\right)]y_m\left(k\right)=\frac{1}{2}[{d_3}^2\left(k\right)-{d_4}^2\left(k\right)+{x_4}^2\left(k\right)-{x_3}^2\left(k\right)+{y_4}^2\left(k\right)-{y_3}^2\left(k\right)]\end{array}\right.---\left(7\right)$

将（7）写成矩阵形式即：

GY=η    （8）

在式（8）中，G、Y、η为中间变量且

$G=\left[\begin{array}{cc}{x}_2\left(k\right)-x_1\left(k\right)& y_2\left(k\right)-y_1\left(k\right)\\ x_3\left(k\right)-x_1\left(k\right)& y_3\left(k\right)-y_1\left(k\right)\\ x_4\left(k\right)-x_1\left(k\right)& y_4\left(k\right)-y_1\left(k\right)\\ x_3\left(k\right)-x_2\left(k\right)& y_3\left(k\right)-y_2\left(k\right)\\ x_4\left(k\right)-x_2\left(k\right)& y_4\left(k\right)-y_2\left(k\right)\\ x_4\left(k\right)-x_3\left(k\right)& y_4\left(k\right)-y_3\left(k\right)\end{array}\right],$ $Y=\left[\begin{array}{c}{x}_m\left(k\right)\\ y_m\left(k\right)\end{array}\right]$

$\mathrm{\η}=\left[\begin{array}{c}\frac{1}{2}[{d_1}^2\left(k\right)-{d_2}^2\left(k\right)+{x_2}^2\left(k\right)-{x_1}^2\left(k\right)+{y_2}^2\left(k\right)-{y_1}^2\left(k\right)]\\ \frac{1}{2}[{d_1}^2\left(k\right)-{d_3}^2\left(k\right)+{x_3}^2\left(k\right)-{x_1}^2\left(k\right)+{y_3}^2\left(k\right)-{y_1}^2\left(k\right)]\\ \frac{1}{2}[{d_1}^2\left(k\right)-{d_4}^2\left(k\right)+{x_4}^2\left(k\right)-{x_1}^2\left(k\right)+{y_4}^2\left(k\right)-{y_1}^2\left(k\right)]\\ \frac{1}{2}[{d_2}^2\left(k\right)-{d_3}^2\left(k\right)+{x_3}^2\left(k\right)-{x_2}^2\left(k\right)+{y_3}^2\left(k\right)-{y_2}^2\left(k\right)]\\ \frac{1}{2}[{d_2}^2\left(k\right)-{d_4}^2\left(k\right)+{x_4}^2\left(k\right)-{x_2}^2\left(k\right)+{y_4}^2\left(k\right)-{y_2}^2\left(k\right)]\\ \frac{1}{2}[{d_3}^2\left(k\right)-{d_4}^2\left(k\right)+{x_4}^2\left(k\right)-{x_3}^2\left(k\right)+{y_4}^2\left(k\right)-{y_3}^2\left(k\right)]\end{array}\right]$

再求使残差η-GY的2-范数取得极小值的解，即

${\left|\right|\mathrm{\η}-{\mathrm{GY}}_0\left|\right|}_2=\underset{Y\∈N^2}{\min }{\left|\right|\mathrm{\η}-\mathrm{GY}\left|\right|}_2---\left(9\right)$

其中，Y₀表示满足式（9）的解，|| ||₂表示矩阵的2-范数，N²表示2维的实数向量全体，对于式（9），此时Y=Y₀是方程组GY=η的最小二乘解，求解GY=η的最小二乘解也就是求解正规方程组：

G′GY＝G′η        （10）

的解，G′表示矩阵G的转置，当G′G可逆时，方程组GY=η的极小最小二乘解（即所有最小二乘解中本身长度最小的解，也称为最佳逼近解）唯一且

$Y=Y_0={\left(G^{\′}G\right)}^{-1}{G}^{\′}\mathrm{\η}=\left[\begin{array}{c}{x}_m\left(k\right)\\ y_m\left(k\right)\end{array}\right]---\left(11\right)$

其中，上角标^-1表示对矩阵求逆，当G′G不可逆时，方程组GY=η的唯一极小最小二乘解为

$Y=Y_0={\left(G^{\′}G\right)}^{+}{G}^{\′}\mathrm{\η}=\left[\begin{array}{c}{x}_m\left(k\right)\\ y_m\left(k\right)\end{array}\right]---\left(12\right)$

其中，(G′G)⁺表示对矩阵(G′G)求广义逆（本发明中矩阵的广义逆是指矩阵的Moore-Penrose逆）；

对于式（11）或式（12）解出了Y即得到了x_m(k)、y_m(k)的值，x_m(k)、y_m(k)是读写器在离散时刻k所在地理坐标系的坐标值，因为读写器固定在车辆上，读写器所在地理坐标系的位置也即车辆所在地理坐标系的位置，因此也就初步确定了离散时刻k车辆所在地理坐标系的位置。

步骤4）利用扩展卡尔曼滤波算法进行车辆精确定位

步骤4.1）建立隧道环境下车辆定位的扩展卡尔曼滤波模型的状态方程

由于利用射频识别定位算法定位得到的车辆位置量仍然含有噪声，为了更准确的获知车辆在隧道下的位置，本发明利用扩展卡尔曼滤波进行滤波递推来获得车辆在隧道下的位置的精确估计。首先建立扩展卡尔曼滤波模型的系统状态方程，针对本发明的应用领域，本发明以前轮转向的四轮车辆为研究对象，将方向盘转角传感器安装在待定位车辆的方向盘上，将纵向加速度传感器安装在待定位车辆的车内，且将两个轮速传感器分别安装在车辆非转向的左后轮和右后轮上，忽略车辆的俯仰、侧倾和上下弹跳运动，认为车辆前、后轴上左右两个轮胎的转向角、侧偏角、纵向力及侧向力相同，采用如图6所示的经典的车辆动力学Bicycle模型（经等效简化后相当于前、后车轮被分别集中在车辆前、后轴中点而构成一假想的Bicycle模型）。F_yf是作用在单个前轮上的侧向力，F_yr是作用在单个后轮上的侧向力，α_front、α_rear分别表示前、后轮胎的侧偏角。

本发明采用该2个自由度的Bicycle模型（该模型在车辆平稳行驶过程中已被证明是充分有效的），忽略转向系统的影响，直接以前轮转向角作为输入，忽略悬架的作用，认为车辆只做平行于地面的运动。图6中定义了两个坐标系，xoy是地理坐标系，在本发明中地理坐标系的原点o选取在车辆附近地球表面的某一固定点，ox指向东，oy指向北。b系是车辆载体坐标系，其原点o_b选取在车辆质心处，o_bx_b轴沿车辆的纵向轴并与车辆纵向前进方向一致，o_bz_b轴垂直于车辆运行平面并指向地面的反方向（即向上，绕o_bz_b轴的横摆角速度

的正方向定义如图6所示），而o_by_b轴按右手螺旋规则可确定。

是车辆的纵向前进速度、

是车辆侧向速度、是车辆横摆角速度、

是车辆纵向加速度，可通过纵向加速度传感器测得，它们都是定义在车辆载体坐标系下且相对于车辆质心的。

是车辆航向角即车辆纵向轴相对于地理坐标系正东方向的角度。δ是前轮转向角，可由方向盘转向角除以从方向盘到前轮的转向传动比来确定，方向盘转向角可以通过方向盘转角传感器测得。根据运动学和动力学原理，车辆的运动模型可描述为：

其中，x、y分别表示车辆在地理坐标系下的东向和北向位置，β₁、β₂、β₃、β₄、β₅、β₆分别表示

的中间变量，即

${\mathrm{\β}}_1=\frac{-2(C_{\mathrm{af}}+C_{\mathrm{ar}})}{M}$ ${\mathrm{\β}}_2=\frac{2(-C_{\mathrm{af}}{l}_{\mathrm{front}}+C_{\mathrm{ar}}{l}_{\mathrm{rear}})}{M}$ ${\mathrm{\β}}_3=\frac{2C_{\mathrm{af}}}{M}$ ，在式

${\mathrm{\β}}_4=\frac{2(-C_{\mathrm{af}}{l}_{\mathrm{front}}+C_{\mathrm{ar}}{l}_{\mathrm{rear}})}{J_{z_b}}$ ${\mathrm{\β}}_5=\frac{-2(C_{\mathrm{af}}{l_{\mathrm{front}}}^2+C_{\mathrm{ar}}{l_{\mathrm{rear}}}^2)}{J_{z_b}}$ ${\mathrm{\β}}_6=\frac{2C_{\mathrm{af}}{l}_{\mathrm{front}}}{J_{z_b}}$

（13）及β₁、β₂、β₃、β₄、β₅、β₆的表达式中，M和

分别是车辆的质量和车辆绕质心垂向轴o_bz_b的转动惯量，l_front是车辆前轮轮轴中心到质心的距离，l_rear是车辆后轮轮轴中心到质心的距离，C_af、C_ar分别表示前、后轮胎的侧偏刚度。

式（13）是一个非线性连续方程，在应用扩展卡尔曼滤波计算时需要进行离散化处理。为此，对式（13）进行离散化处理，建立隧道环境下车辆定位的扩展卡尔曼滤波模型的系统状态方程，其矩阵形式可表示为：

X(k)＝f(X(k-1),U(k-1),W(k-1),γ(k-1))      （14）

系统状态变量为其中，X(k)表示离散时刻k的系统状态变量，x(k)、y(k)分别表示离散时刻k，在地理坐标系中待定位车辆的坐标值，

分别表示离散时刻k车辆航向角、即车辆纵向轴相对于地理坐标系正东方向的角度，车辆纵向前进速度、车辆侧向速度、车辆横摆角速度，

表示向量

的转置；

f表示系统状态函数向量，X(k-1)表示离散时刻k-1的系统状态变量；

U(k-1)表示离散时刻k-1系统状态方程的外输入向量，且 $U(k-1)={\left[\begin{array}{cc}\mathrm{\δ}(k-1)& a_{x_b}(k-1)\end{array}\right]}^{\′},$ δk-1)表示在离散时刻k-1车辆的前轮转向角，由方向盘转向角传感器测得的方向盘转角信息除以方向盘与前轮的转向传动比来确定，

表示在离散时刻k-1车辆的纵向加速度，可通过加速度传感器测得， ${\left[\begin{array}{cc}\mathrm{\δ}(k-1)& a_{x_b}(k-1)\end{array}\right]}^{\′}$ 表示向量 $\left[\begin{array}{cc}\mathrm{\δ}(k-1)& a_{x_b}(k-1)\end{array}\right]$ 的转置；

W(k-1)表示在离散时刻k-1零均值的系统高斯白噪声向量且W(k-1)＝[w₁w₂ w₃ w₄ w₅ w₆]′，其中w₁、w₂、w₃、w₄、w₅及w₆分别表示六个系统高斯白噪声分量，[w₁ w₂ w₃ w₄ w₅ w₆]′表示向量[w₁ w₂ w₃ w₄ w₅ w₆]的转置；

γ(k-1)表示在离散时刻k-1系统外输入对应的零均值高斯白噪声向量且 $\mathrm{\γ}(k-1)={\left[\begin{array}{cc}{w}_{\mathrm{\δ}}& w_{a_{x_b}}\end{array}\right]}^{\′},$ 其中w_δ、分别表示直测的系统外输入δ(k-1)、对应的零均值高斯白噪声，这些噪声包含在系统状态函数向量f的两个系统外输入里面， ${\left[\begin{array}{cc}{w}_{\mathrm{\δ}}& w_{a_{x_b}}\end{array}\right]}^{\′}$ 表示向量 $\left[\begin{array}{cc}{w}_{\mathrm{\δ}}& w_{a_{x_b}}\end{array}\right]$ 的转置，非线性的系统状态函数向量离散化形式为：

$f(X(k-1),U((k-1),W(k-1),\mathrm{\γ}(k-1))=\left[\begin{array}{c}{f}_1(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_2(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_3(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_4(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_5(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\\ f_6(X(k-1),U(k-1),W(k-1),\mathrm{\γ}(k-1))\end{array}\right],$

其中，f₁、f₂、f₃、f₄、f₅、f₆分别表示系统状态函数向量f的六个系统状态函数向量分量，且

式（15）中，T表示离散的周期（T为500毫秒、1000毫秒或2000毫秒），其它变量的含义同前。W(k-1)对应的系统噪声协方差阵Q(k-1)为：

$Q(k-1)=\left[\begin{array}{cccccc}{{\mathrm{\σ}}_{w_1}}^2& 0& 0& 0& 0& 0\\ 0& {{\mathrm{\σ}}_{w_2}}^2& 0& 0& 0& 0\\ 0& 0& {{\mathrm{\σ}}_{w_3}}^2& 0& 0& 0\\ 0& 0& 0& {{\mathrm{\σ}}_{w_4}}^2& 0& 0\\ 0& 0& 0& 0& {{\mathrm{\σ}}_{w_5}}^2& 0\\ 0& 0& 0& 0& 0& {{\mathrm{\σ}}_{w_6}}^2\end{array}\right],$ 其中

及

分别表示系统高斯白噪声w₁、w₂、w₃、w₄、w₅及w₆对应的方差；γ(k-1)对应的系统外部输入噪声的协方差阵为Γ(k-1)且

其中σ_δ ²、

分别表示w_δ、

对应的方差；

步骤4.2）建立隧道环境下车辆定位的扩展卡尔曼滤波模型的观测方程

根据扩展卡尔曼滤波理论，车辆运动的扩展卡尔曼滤波模型除包括系统状态方程公式（14）外，还应包括系统观测方程，本发明选择射频定位算法计算输出的车辆在地理坐标系中沿正东方向的位置和沿正北方向的位置x_m(k)、y_m(k)以及利用轮速传感器测量与计算得到的离散时刻k车辆纵向前进速度

车辆横摆角速度

作为系统观测量，由于图6所示的车辆运动实际上是由车辆纵向、侧向、横摆运动组成的平面复合运动，根据平面复合运动原理可以得到

其中，v_rr(k)和v_lr(k)分别表示车辆右后轮和左后轮的线速度，可以分别通过安装在车辆右后轮和左后轮的轮速传感器测量值乘以相应的轮胎半径得到，D表示车辆右后轮和左后轮之间的距离。离散化后的扩展卡尔曼滤波模型的观测方程的矩阵形式可表示为：

Z(k)＝H(k)·X(k)+V(k)        （16）

其中，Z(k)、H(k)分别为离散时刻k扩展卡尔曼滤波模型的观测向量和观测阵，V(k)表示离散时刻k的零均值观测白噪声向量，且其与离散时刻k的零均值系统高斯白噪声向量W(k)互不相关，且 $Z\left(k\right)={\left[\begin{array}{cccc}{x}_m\left(k\right)& y_m\left(k\right)& v_{x_{b_m}}\left(k\right)& {\mathrm{\ω}}_{z_{b_m}}\left(k\right)\end{array}\right]}^{\′},$ ${\left[\begin{array}{cccc}{x}_m\left(k\right)& y_m\left(k\right)& v_{x_{b_m}}\left(k\right)& {\mathrm{\ω}}_{z_{b_m}}\left(k\right)\end{array}\right]}^{\′}$ 表示向量 $\left[\begin{array}{cccc}{x}_m\left(k\right)& y_m\left(k\right)& v_{x_{b_m}}\left(k\right)& {\mathrm{\ω}}_{z_{b_m}}\left(k\right)\end{array}\right]$ 的转置， $H\left(k\right)=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],$ $V\left(k\right)=\left[\begin{array}{c}{n}_x\\ n_y\\ n_{v_{x_b}}\\ n_{{\mathrm{\ω}}_{z_b}}\end{array}\right],$ 其中n_x是车辆沿正东方向位置的观测

噪声且n_x是均值为0、方差为

的高斯白噪声，

表示n_x的方差，n_y是车辆沿正北方向的位置的观测噪声且n_y是均值为0、方差为

的高斯白噪声，

表示n_y的方差，是车辆纵向前进速度的观测噪声且

是均值为0、方差为的高斯白噪声，

表示

的方差，

是车辆横摆角速度的观测噪声且

是均值为0、方差为的高斯白噪声，表示

的方差，V(k)对应的观测噪声方差阵R(k)可表示为：

步骤4.3）进行扩展卡尔曼滤波的时间更新和测量更新，得到车辆的精确、实时的位置信息

对于式（14）和式（16）所描述的状态方程和观测方程，运用扩展卡尔曼滤波算法，建立标准滤波递推过程，该过程包括时间更新和测量更新：时间更新：

状态一步预测方程：

$\hat{X}(k,k-1)=f(\hat{X}(k-1),U(k-1),\mathrm{0,0}),$ 其中

表示状态X的滤波计算值；

一步预测误差方差阵P(k,k-1)：

P(k,k-1)＝A(k,k-1)P(k-1)A′(k,k-1)+B(k,k-1)Γ(k-1)B′(k,k-1)+Q(k-1)

其中，A是系统状态函数向量f对状态向量X求偏导数的雅克比矩阵，A′表示矩阵A的转置，B是系统状态函数向量f对外部输入向量U求偏导数的雅克比矩阵，B′表示矩阵B的转置，矩阵A的第i行第j列元素A_[i，j]，i＝1,2,3,4,5,6，j=1,2,3,4,5,6，和矩阵B的第i行第t列元素B_[i，t]，i＝1,2,3,4,5,6，t＝1,2可分别通过下面的式子求得：

$A_{[i,i]}=\frac{\∂f_i}{\∂x_j}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0})$

$B_{[i,t]}=\frac{\∂f_i}{\∂u_t}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0})$

具体而言，各矩阵元素的取值如下：

A_[1，1]＝1  A_[1，2]＝0

A_[1,6]＝0  A_[2,1]＝0  A_[2,2]＝1

A_[2,6]＝0  A_[3,1]＝A_[3,2]＝A_[3,4]＝A_[3,5]＝0  A_[3,3]＝1  A_[3,6]＝T

A_[4,1]＝A_[4,2]＝A_[4,3]＝A_[4,5]＝A_[4,6]＝0  A_[4,4]＝1

A_[5,1]＝A_[5,2]＝A_[5,3]＝0 $A_{\left[\mathrm{5,4}\right]}=T[-\frac{{\mathrm{\β}}_1}{{v_{x_b}}^2}{v}_{y_b}+(-\frac{{\mathrm{\β}}_2}{{v_{x_b}}^2}-1){\mathrm{\ω}}_{z_b}]$

$A_{\left[\mathrm{5,5}\right]}=1+T\frac{{\mathrm{\β}}_1}{v_{x_b}}$ $A_{\left[\mathrm{5,6}\right]}=T(\frac{{\mathrm{\β}}_2}{v_{x_b}}-v_{x_b})$ A_[6，1]＝A_[6,2]＝A_[6,3]＝0

$A_{\left[\mathrm{6,4}\right]}=T(-\frac{{\mathrm{\β}}_4}{{v_{x_b}}^2}{v}_{y_b}-\frac{{\mathrm{\β}}_5}{{v_{x_b}}^2}{\mathrm{\ω}}_{z_b})$ $A_{\left[\mathrm{6,5}\right]}=T\frac{{\mathrm{\β}}_4}{v_{x_b}}$ $A_{\left[\mathrm{6,6}\right]}=1+T\frac{{\mathrm{\β}}_5}{v_{x_b}}$

B_[1,1]＝B_[1,2]＝B_[2,1]＝B_[2,2]=B_[3,1]=B_[3,2]=B_[4,1]=B_[5,2]=B_[6,2]=0

B_[4,2]＝T    B_[5,1]＝Tβ₃    B_[6,1]＝Tβ₆

测量更新：

滤波增益矩阵K(k)：K(k)＝P(k,k-1)·H′(k)·[H(k)P(k,k-1)H′(k)+R(k)]^-1状态估计： $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}(k,k-1)]$

估计误差方差阵P(k)：P(k)=[E-K(k)·H(k)]·P(k，k-1)

其中，H′(k)表示矩阵H(k)的转置，上角标^-1表示对矩阵求逆，E为6×6的单位阵。

在上述滤波递推计算过程中，可确定车辆在每个离散时刻的位置x(k)、y(k),由于观测量x_m(k)、y_m(k)含有噪声，通过扩展卡尔曼滤波可以进一步有效地降低噪声，提高车辆位置估计的精度，从而解决GPS信号被遮挡时，车辆的准确、实时、可靠的定位问题。

Claims (1)

1.一种面向隧道环境的车辆高可靠组合定位方法，其特征在于：利用射频识别定位算法进行车辆的初步定位，获取含有噪声的位置信息，为了进一步提高初步定位的定位精度，针对前轮转向四轮车辆，结合隧道环境建立车辆定位的扩展卡尔曼滤波模型，以方向盘转角和加速度传感器所输出的信息作为系统状态方程外部输入量，以初步定位获取的含有噪声的车辆位置信息以及利用轮速传感器测量与计算得到的车辆前向速度、车辆横摆角速度作为系统观测量，通过扩展卡尔曼滤波算法准确、实时、可靠的推算出隧道环境内车辆的精确位置信息，具体步骤如下： 

步骤1)在隧道环境下布置有源主动式射频识别电子标签tag，在车辆上布置射频识别读写器reader 

在隧道壁体两侧无遮挡位置，每距离10-20米等间隔地布置有源主动式射频识别电子标签tag，以正东为ox指向，以正北为oy指向，原点为o，建立地理坐标系xoy，原点o选取在地球表面一个固定点上，得到地理坐标系xoy下的所有有源主动式射频识别电子标签tag的坐标，将射频识别读写器reader固定在行驶的车辆的车顶，此时射频识别读写器reader在地理坐标系的位置坐标即被认为是车辆在地理坐标系中的位置坐标，并近似认为有源主动式射频识别电子标签tag和射频识别读写器reader处于同一平面上； 

步骤2)选择用于实时定位的有效标签 

根据射频识别读写器reader实时接收到的有源主动式射频识别电子标签tag在每个离散时刻k发出的信号，k=1，2，3，…，k为正整数，将最大的四个接收信号强度值所对应的四个有源主动式射频识别电子标签tag作为用于实时定位的四个有效标签，四个有效标签在地理坐标系xoy中的坐标分别为(x₁(k)，y₁(k))、(x₂(k)，y₂(k))、(x₃(k)，y₃(k))、(x₄(k)，y₄(k))，且x₁(k)、y₁(k)、x₂(k)、y₂(k)、x₃(k)、y₃(k)、x₄(k)、y₄(k)分别表示离散时刻k这四个有效标签在地理坐标系中对应的坐标值，这些坐标值为已知； 

步骤3)利用射频识别定位算法进行车辆初步定位 

步骤3.1)拟合出射频识别读写器reader和有源主动式射频识别电子标签tag之间距离d和接收信号强度值I的关系 

d＝ae^uI+ce^rI， 

其中，d表示射频识别读写器reader与有源主动式射频识别电子标签tag之间的距离，其单位为米，I表示由射频识别读写器reader接收并得到的来自相应有源主动式射频识别电子标签tag的接收信号强度值，无量纲，e为自然底数且e=2.71828，a、u、c、r分别表示相应的系数常量，a=171.8，u=-1.27，c=83.05，r=-0.05354； 

分别求得离散时刻k的射频识别读写器reader和四个有效标签之间的距离d₁(k)、d₂(k)、d₃(k)、d₄(k)： 

其中，d₁(k)、d₂(k)、d₃(k)、d₄(k)分别表示离散时刻k的射频识别读写器reader和四个有效标签之间的距离，其单位为米，I₁(k)、I₂(k)、I₃(k)、I₄(k)分别表示由射频识别读写器reader接收并得到的来自四个有效标签的离散时刻k的接收信号强度值，无量纲，e为自然底数且e=2.71828，a、u、c、r分别表示相应的系数常量，具体取值同上； 

步骤3.2)利用平面上两点间的距离公式，建立车辆位置方程组 

其中，x_m(k)和y_m(k)分别表示离散时刻k射频识别读写器reader在地理坐标系中的坐标值，为未知待求量； 

步骤3.3)利用最小二乘法求解位置方程组，计算出射频识别读写器reader在地理坐标系中的坐标值x_m(k)，y_m(k)； 

步骤4)利用扩展卡尔曼滤波算法进行车辆精确定位 

步骤4.1)建立隧道环境下车辆定位的扩展卡尔曼滤波模型的状态方程 

针对隧道环境下的车辆定位，建立车辆载体坐标系，其原点o_b选取在车辆质心处，o_bx_b轴沿车辆的纵向轴并与车辆纵向前进方向一致，o_bz_b轴垂直于车辆运行平面并指向地面的反方向，而o_by_b轴按右手螺旋规则可确定，将方向盘转角传感器安装在待定位车辆的方向盘上，将纵向加速度传感器安装在待定位车辆的车内，且将两个轮速传感器分别安装在车辆的左后轮和右后轮上，在离散时刻k，取系统状态变量

其中，X(k)表示离散时刻k的系统状态变量，x(k)、y(k)分别表示离散时刻k，在地理坐标系中待定位车辆的坐标值，

分别表示在离散时刻k车辆航向角、车辆纵向前进速度、车辆侧向速度、车辆横摆角速度， 表示向量

的转置，根据运动学和动力学原理，离散化后的扩展卡尔曼滤波模型的状态方程表示为： 

X(k)=f(X(k-1)，U(k-1)，W(k-1)，γ(k-1))   (6) 

式(6)中，f表示系统状态函数向量，X(k-1)表示离散时刻k-1的系统状态变量； 

U(k-1)表示离散时刻k-1系统状态方程的外输入向量，且 

其中δ(k-1)表示在离散时刻k-1车辆的前轮转向角，由方向盘转向角传感器测得的方向盘转角信息除以方向盘与前轮的转向传动比来确定，

表示在离散时刻k-1车辆的纵向加速度，通过纵向加速度传感器测得，

表示向量

的转置； 

W(k-1)表示在离散时刻k-1的零均值系统高斯白噪声向量且 

其中w₁、w₂、w₃、w₄、w₅及w₆分别表示六个系统高斯白噪声分量，

表示向量

的转置； 

γ(k-1)表示在离散时刻k-1系统外输入对应的零均值高斯白噪声向量且 

其中

分别表示直测的系统外输入δ(k-1)、对应的零均值高斯白噪声，这些噪声包含在系统状态函数向量f的两个系统外输入里面，

表示向量

的转置，非线性的离散化后的系统状态函数向量为： 

其中，f₁、f₂、f₃、f₄、f₅、f₆分别表示系统状态函数向量f的六个系统状态函数向量分量，且 

式(7)中，β₁、β₂、β₃、β₄、β₅、β₆分别为表示

的中间变量，即 在式(7)及β₁、β₂、β₃、β₄、β₅、β₆的表达式中，M和

分别表示车辆的质量和车辆绕o_bZ_b轴的转动惯量，l_front是车辆前轮轮轴中心到质心的距离，l_rear是车辆后轮轮轴中心到质心的距离，C_af、C_ar分别表示车辆前、后轮胎的侧偏刚度，T表示离散的周期，T为500毫秒、1000毫秒或2000毫秒，W(k-1)对应的系统噪声协方差阵Q(k-1)为： 

其中

及

分别表示系统高斯白噪声w₁、w₂、w₃、w₄、w₅及w₆对应的方差，γ(k-1)对应的系统外部输入噪声的协方差阵为Γ(k-1)且

其中σ_δ ²、

分别表示w_δ、对应的方差； 

步骤4.2)建立隧道环境下车辆定位的扩展卡尔曼滤波模型的观测方程 

以射频定位算法计算输出的地理坐标系下的车辆位置x_m(k)、y_m(k)以及利用轮速传感器测量与计算得到的离散时刻k车辆纵向前进速度

车辆横摆角速度

作为扩展卡尔曼滤波模型的观测量，且

其中，v_rr(k)和v_lr(k)分别表示车辆右后轮和左后轮的线速度，可以分别通过安装在车辆右后轮和左后轮的轮速传感器测量值乘以相应的轮胎半径得到，D表示车辆右后轮和左后轮之间的距离，离散化后的扩展卡尔曼滤波模型的观测方程为： 

Z(k)=H(k)·X(k)+V(k)   (8) 

其中，Z(k)、H(k)分别为离散时刻k扩展卡尔曼滤波模型的观测向量和观测阵，V(k)表示离散时刻k的零均值观测白噪声向量，且其与离散时刻k的零均值系统高斯白噪声向量W(k)互不相关，且

表示向量

的转置， 

其中n_x是车辆沿正东方向位置的观测噪声且n_x是均值为0、方差为

的高斯白噪声，

表示n_x的方差，n_y是车辆沿正北方向的位置的观测噪声且n_y是均值为0、方差为

的高斯白噪声，

表示nx的方差，

是车辆纵向前进速度的观测噪声且

是均值为0、方差为

的高斯白噪声，

表示

的方差，

是车辆横摆角速度的观测噪声且

是均值为0、方差为

的高斯白噪声，

表示

的方差，V(k)对应的观测噪声方 差阵R(k)可表示为：

步骤4.3)进行扩展卡尔曼滤波的时间更新和测量更新，得到车辆的实时、精确的位置信息 

对于式(6)和式(8)所描述的状态方程和观测方程，运用扩展卡尔曼滤波算法，建立标准滤波递推过程，该过程包括时间更新和测量更新： 

时间更新： 

状态一步预测方程： 

其中

表示状态X的滤波计算值； 

一步预测误差方差阵P(k，k-1)： 

P(k，k-1)=A(k，k-1)P(k-1)A′(k，k-1)+B(k，k-1)Γ(k-1)B′(k，k-1)+Q(k-1) 

其中，A是系统状态函数向量f对状态向量X求偏导数的雅克比矩阵，A′表示矩阵A的转置，B是系统状态函数向量f对外部输入向量U求偏导数的雅克比矩阵，B′表示矩阵B的转置，矩阵A的第i行第j列元素A[i，j]，i＝1，2，3，4，5，6，j＝1，2，3，4，5，6，和矩阵B的第i行第t列元素B[i，t]，i=1，2，3，4，5，6，t＝1，2可分别通过下式求得： 

各矩阵元素的取值如下： 

A_[2，6]=0 A_[3，1]=A_[3，2]=A_[3，4]=A_[3，5]=0 A_[3，3]=1 A_[3，6]=T 

A_[4，1]=A_[4，2]=A_[4，3]=A_[4，5]=A_[4，6]=0 A_[4，4]=1 

B_[1，1]=B_[1，2]=B_[2，1]=B_[2，2]＝B_[3，1]＝B_[3，2]＝B_[4，1]＝B_[5，2]＝B_[6，2]＝0 

测量更新： 

滤波增益矩阵K(k)：K(k)=P(k，k-1)·H′(k)·[H(k)P(k，k-1)H′(k)+R(k)]^-1

状态估计：

估计误差方差阵P(k)：P(k)=[E-K(k)·H(k)]·P(k，k-1) 

其中，H′(k)表示矩阵H(k)的转置，上角标^-1表示对矩阵求逆，E为6×6的单位阵。 

Images