[发明专利]一种面向隧道环境的车辆高可靠组合定位方法

权利要求书

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)：

d1(k)=aeuI1(k)+cerI1(k)---(1)]]>

d2(k)=aeuI2(k)+cerI2(k)---(2)]]>

d3(k)=aeuI3(k)+cerI3(k)---(3)]]>

d4(k)=aeuI4(k)+cerI4(k)---(4)]]>

其中，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）利用平面上两点间的距离公式，建立车辆位置方程组

[xm(k)-x1(k)]2+[ym(k)-y1(k)]2=d1(k)[xm(k)-x2(k)]2+[ym(k)-y2(k)]2=d2(k)[xm(k)-x3(k)]2+[ym(k)-y3(k)]2=d3(k)[xm(k)-x4(k)]2+[ym(k)-y4(k)]2=d4(k)---(5)]]>

其中，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)=δ(k-1)axb(k-1)′,]]>其中δk-1)表示在离散时刻k-1车辆的前轮转向角，由方向盘转向角传感器测得的方向盘转角信息除以方向盘与前轮的转向传动比来确定，表示在离散时刻k-1车辆的纵向加速度，通过纵向加速度传感器测得，δ(k-1)axb(k-1)′]]>表示向量δ(k-1)axb(k-1)]]>的转置；

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系统外输入对应的零均值高斯白噪声向量且γ(k-1)=wδwaxb′,]]>其中w_δ、分别表示直测的系统外输入δ(k-1)、对应的零均值高斯白噪声，这些噪声包含在系统状态函数向量f的两个系统外输入里面，wδwaxb′]]>表示向量wδwaxb]]>的转置，非线性的离散化后的系统状态函数向量为：

f(X(k-1),U((k-1),W(k-1),γ(k-1))=f1(X(k-1),U(k-1),W(k-1),γ(k-1))f2(X(k-1),U(k-1),W(k-1),γ(k-1))f3(X(k-1),U(k-1),W(k-1),γ(k-1))f4(X(k-1),U(k-1),W(k-1),γ(k-1))f5(X(k-1),U(k-1),W(k-1),γ(k-1))f6(X(k-1),U(k-1),W(k-1),γ(k-1))]]>

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

式（7）中，β₁、β₂、β₃、β₄、β₅、β₆分别表示的中间变量，即

β1=-2(Caf+Car)M]]>β2=2(-Caflfront+Carlrear)M]]>β3=2CafM]]>，在式（7）

β4=2(-Caflfront+Carlrear)Jzb]]>β5=-2(Caflfront2+Carlrear2)Jzb]]>β6=2CaflfrontJzb]]>

及β₁、β₂、β₃、β₄、β₅、β₆的表达式中，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)=σw12000000σw22000000σw32000000σw42000000σw52000000σw62,]]>其中

及分别表示系统高斯白噪声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(k)=xm(k)ym(k)vxbm(k)ωzbm(k)′,]]>xm(k)ym(k)vxbm(k)ωzbm(k)′]]>表示向量xm(k)ym(k)vxbm(k)ωzbm(k)]]>的转置，H(k)=100000010000000100000001,]]>V(k)=nxnynvxbnωzb,]]>其中n_x是车辆沿正东方向位置的观测噪声且n_x是均值为0、方差为的高斯白噪声，表示n_x的方差，n_y是车辆沿正北方向的位置的观测噪声且n_y是均值为0、方差为的高斯白噪声，表示n_y的方差，是车辆纵向前进速度的观测噪声且是均值为0、方差为的高斯白噪声，表示的方差，是车辆横摆角速度的观测噪声且是均值为0、方差为的高斯白噪声，表示的方差，V(k)对应的观测噪声方差阵R(k)可表示为：R(k)=σx20000σy20000σvxb20000σωzb2;]]>

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

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

时间更新：

状态一步预测方程：

X^(k,k-1)=f(X^(k-1),U(k-1),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]=∂fi∂xj(X^(k,k-1),U(k-1),0,0)]]>

B[i,t]=∂fi∂ut(X^(k,k-1),U(k-1),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[5,5]=1+Tβ1vxb]]>A[5,6]=T(β2vxb-vxb)]]>A_[6，1]＝A_[6,2]＝A_[6,3]＝0

A[6,4]=T(-β4vxb2vyb-β5vxb2ωzb)]]>A[6,5]=Tβ4vxb]]>A[6,6]=1+Tβ5vxb]]>

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

状态估计：X^(k)=X^(k,k-1)+K(k)[Z(k)-H(k)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的单位阵。