CN101592492A - 车载导航系统误差协方差阵部分参数自适应调节的方法 - Google Patents

Abstract

本发明公开了一种车载导航系统误差协方差阵部分参数自适应调节的方法，该方法需要建立车载导航系统的数学模型，根据模型系统能够进行滤波，采用了基于一段时间内轨迹的均值和方差的统计特性构建了GPS轨迹的光滑程度，用于判断是否需要调节误差协方差阵，利用GPS每秒的位移量与DR每秒的位移量构造观测噪声序列，建立了此噪声序列与误差协方差阵之间的关系，使得误差协方差阵在滤波系统中跟随噪声的变化而动态调节。本发明方法实现简单，易于操作；可以对误差协方差阵Q，R参数进行自适应调节；减小试凑法的工作量；提高导航系统的定位精度，与取经验值时直接滤波相比提高7％。

Description

车载导航系统误差协方差阵部分参数自适应调节的方法

技术领域

本发明属于车载导航技术领域，具体涉及一种车载卫星/惯性组合导航系统中卡尔曼滤波器误差协方差阵Q，R部分参数自适应调节的方法，用于提高车载导航定位精度。

背景技术

卡尔曼(Kalman)滤波是现代组合导航系统中应用十分广泛的数据融合技术，在准确获得不同测量系统误差与系统状态估计误差的信息的基础上，依据各种物理量之间的状态转移关系，合理利用不同测量系统的测量信息，能够获得对待估计状态的最优估计。在Kalman滤波中，状态误差协方差阵Q、观测误差协方差阵R的准确程度对估计结果精度的影响很大，是Kalman滤波的重要参数。

实际应用中Q-状态误差协方差阵、R-观测误差协方差阵参数的选择多采用经验性的试凑方法以及自适应滤波法。经验性试凑法依据测量设备的测量特性、组合滤波能力确定Q、R初步范围，并通过调整Q、R的仿真分析给出较为合适的协方差阵，工作量较大且不能适应观测精度变化的需要；现有的自适应滤波方法虽然具有Q、R的自适应调节能力，但其调节思想是从系统滤波方程结构关系出发，没有充分考虑组合滤波系统中不同测量手段的测量特性进行协方差阵的直接估计，影响了kalman滤波的精度。

发明内容

本发明的目的是为了解决导航系统卡尔曼(kalman)滤波方法中对误差协方差阵参数进行自适应调节的问题，提供了一种车载导航系统误差协方差阵部分参数自适应调节的方法，该方法的主要思想是充分考虑GPS、里程仪的测量与误差性质，构造了一种可以有效反映GPS测量误差大小的估计方法，利用短时测量精度较高的里程仪信息对相对测量精度较低的GPS误差进行估计，实现对GPS测量误差、滤波后车辆位置误差自适应估计，并在此基础上合理自适应修正Q、R，从而达到自适应滤波的目的。

本发明提供的自适应调节方法是建立在kalman滤波的基础上进行的，在不考虑GPS系统误差的情况下仅对随机误差进行估计。利用GPS直接测量信息，依据相邻时刻的位置差可以获得每秒内车辆行驶的位移；里程仪测量的信息也可以获得每秒内车辆行驶的位移，即航位推算(DR)；因此依据这两种测量方法构造相同时间段内每秒车辆行驶的位移序列具有很强的可比性。考虑到车辆的机动性，一般情况下，短时间内车辆行驶的路线大都为直线，并且航位推算(DR)的相对定位精度较高，这样可以利用DR的位置信息作为参考信息，使GPS每秒的位移量与DR每秒的位移量作差可获得GPS位置信息的观测误差，构造一段时间内的观测误差序列，建立观测误差与误差协方差阵之间的关系，从而可以实现对误差协方差阵进行估计调节。观测误差序列的构造利用的是轨迹的相对位置，因此不需要考虑车辆行驶的初始航向。本发明方法主要采用了基于一段时间内轨迹的均值和方差的统计特性构建了GPS轨迹的光滑程度简称光滑度，用于判断是否需要调节误差协方差阵，利用GPS每秒的位移量与DR每秒的位移量构造观测噪声序列，建立了此噪声序列与误差协方差阵之间的关系，使得误差协方差阵在滤波系统中跟随噪声的变化而动态调节，提高了车载定位精度，与选取Q，R为经验固定值相比定位精度提高了7％以上。

主要的技术方案分为四个模块，一是对车载导航系统进行数学建模，采用的是常用的kalman滤波的方法；二是对系统滤波，获得状态信息；三是对GPS输出的轨迹进行光滑度的判定，根据短时间内GPS的定位误差均值和方差的统计特性来判定轨迹的光滑程度；四是依据第三个模块得出的结果，构建GPS光滑度与kalman滤波误差协方差阵之间的关系，对Q，R误差协方差阵进行自适应调节，达到实时更新的效果。

具体过程以及相应的步骤如下所示：

步骤一、建立车载导航系统的数学模型

根据车辆导航系统的实际数据情况，选取GPS北向距离N_y、GPS东向距离E_x、GPS航向ψ、GPS速度v、里程仪的刻度因子S，陀螺角速率

作为状态变量，构建Kalman滤波模型状态方程，具体的数学模型如下：

状态向量x^T： $x^T=\left[\begin{array}{cccccc}{N}_y& E_x& \mathrm{\ψ}& v& S& {\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\end{array}\right]$

状态方程为：x(k+1)＝φ(k+1，k)x(k)+w(k)

状态误差向量为：w^T(k)＝[w₁ w₂ w₃ w₄ w₅ w₆]

GPS/DR导航系统的状态方程组如下：

N_y(k+1)＝N_y(k)+v(k)Tcos(ψ(k))+w₁

E_x(k+1)＝E_x(k)+v(k)Tsin(ψ(k))+w₂

$\mathrm{\ψ}(k+1)=\mathrm{\ψ}\left(k\right)+T{\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+w_3$

v(k+1)＝v(k)+w₄

S(k+1)＝S(k)+w₅

${\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}(k+1)={\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+w_6$

k表示当前时刻，k+1为下一时刻，w₁，w₂，w₃，w₄，w₅，w₆分别表示各个状态变量的状态误差，T为周期，本发明方法取为1s。状态转移矩阵经过泰勒展开得：

$\mathrm{\φ}(k+1,k)=\left[\begin{array}{cccccc}1& 0& -v\left(k\right)T\sin \left(\mathrm{\ψ}\left(k\right)\right)& T\cos \left(\mathrm{\ψ}\left(k\right)\right)& 0& 0\\ 0& 1& v\left(k\right)T\cos \left(\mathrm{\ψ}\left(k\right)\right)& T\sin \left(\mathrm{\ψ}\left(k\right)\right)& 0& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

状态误差协方差阵：

$Q=E\left[{\mathrm{ww}}^T\right]=\left[\begin{array}{cccccc}{q}_{11}& 0& 0& 0& 0& 0\\ 0& q_{22}& 0& 0& 0& 0\\ 0& 0& q_{33}& 0& 0& 0\\ 0& 0& 0& q_{44}& 0& 0\\ 0& 0& 0& 0& q_{55}& 0\\ 0& 0& 0& 0& 0& q_{66}\end{array}\right]$

w表示为状态变量估计值的状态误差向量。

选取GPS北向距离N_gps、GPS东向距离E_gps、GPS航向ψ_gps、GPS速度v_speed，陀螺角速率

作为观测变量，构建Kalman滤波模型观测方程，具体的数学模型如下：

观测向量y^T： $y^T=\left[\begin{array}{ccccc}{N}_{\mathrm{gps}}& E_{\mathrm{gps}}& {\mathrm{\ψ}}_{\mathrm{gps}}& v_{\mathrm{speed}}& {\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gyro}}\end{array}\right]$

观测方程为：y(k+1)＝H(k)x(k)+e(k)

观测误差向量为：e^T(k)＝[e₁ e₂ e₃ e₄ e₅]

GPS/DR导航系统的观测方程组如下：

N_gps(k)＝N_y(k)+e₁

E_gps(k)＝E_x(k)+e₂

ψ_gps(k)＝ψ(k)+e₃

v_speed(k)＝m(k)S(k)+e₄

${\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gyro}}\left(k\right)={\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+e_5$

e₁，e₂，e₃，e₄，e₅分别表示各个观测变量的观测误差，m(k)表示的是当前周期内里程仪累计的脉冲数。观测矩阵为：

$H\left(k\right)=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& m\left(k\right)& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

观测误差协方差阵：

$R=E\left[{\mathrm{ee}}^T\right]=\left[\begin{array}{ccccc}{r}_{11}& 0& 0& 0& 0\\ 0& r_{22}& 0& 0& 0\\ 0& 0& r_{33}& 0& 0\\ 0& 0& 0& r_{44}& 0\\ 0& 0& 0& 0& r_{55}\end{array}\right]$

e表示为观测变量估计值的观测误差向量。

根据状态与观测向量的定义，状态协方差阵的说明如下表所示：

表1状态误差协方差阵各元素定义表

序号	名称	意义	单位
				1	q11	北向位置状态噪声方差	M2
2	q22	东向位置状态噪声方差	M2
				3	q33	GPS航向状态噪声方差	rad2
4	q44	GPS速度状态噪声方差	(m/s)2
				5	q55	里程仪状态噪声方差	M2
6	q66	陀螺角速率状态噪声方差	(rad/s)2

表2观测误差协方差阵各元素定义表

序号	名称	意义	单位
				1	r11	北向位置观测噪声方差	M2
2	r22	东向位置观测噪声方差	M2
				3	r33	GPS航向观测噪声方差	rad2
4	r44	GPS速度观测噪声方差	(m/s)2
				5	r55	陀螺角速率观测噪声方差	(rad/s)2

依据上述建立的数学模型进入步骤二进行系统滤波，系统滤波的输出值即为状态变量。针对此车载导航系统分析，本发明仅对状态误差协方差阵Q和观测误差协方差阵R中的q₁₁、q₂₂、r₁₁、r₂₂进行自适应调节。

步骤二、系统滤波。

依据步骤一建立的数学模型对车辆导航系统进行系统滤波，滤波是一个循环的过程，以周期T(本发明方法取为1s)为单位，根据上一时刻对当前时刻的估计以及当前时刻的观测变量对当前时刻的状态变量进行滤波，获得当前时刻更为精确的状态变量，即为系统滤波的输出值。根据所获得的观测变量和状态变量对q₁₁、q₂₂、r₁₁、r₂₂进行调节。

步骤三、GPS光滑度判定。

光滑度是指输出轨迹的光滑程度。以短时间内航位推算(DR)轨迹作为理想轨迹对GPS轨迹光滑度进行估计判定。具体的判定步骤以及判定的方法如下：

1)设定轨迹的时间长度。

车辆的运动轨迹随着时间的变化而变化，把运动轨迹根据相等的时间长度分成很多不同的轨迹段，设定时间长度H，一般取15s～20s，并且不同轨迹段之间不可有时间重叠。时间长度可以根据车辆行驶的路况进行调整。

2)DR相对运动序列A的构造。

依据步骤1)中设定的时间长度H，利用DR解算公式获得每段轨迹内相邻两点之间的距离L：

L＝S(k)*(m(k-1)+m(k))/2

S(k)表示里程仪的刻度因素，m(k-1)表示前一周期内里程仪累计的脉冲数，m(k)表示的是当前周期内里程仪累计的脉冲数；根据获得的相邻两点之间的距离L即步骤1)中所设时间长度H内每秒的移动直线距离，构造序列A。

3)GPS相对运动序列B的构造。

依据步骤1)中设定的时间长度H，利用GPS相邻时刻输出的位置信息做差，求出每一秒的位移，得出相邻两点间距离，构造序列B。

4)构造距离差序列C。

将序列A和序列B相减得到一个距离差序列C，C中各元素为相邻两秒中GPS测量位移与DR测量位移之差，距离差序列C中的元素有正负之分。

5)误差统计。

距离差序列C进行统计，获得一定时间段内如15s内由步骤4)获得的距离差序列C内元素的均值与方差。

6)设定比较的阈值判断光滑度。

设定均值阈值为5～10，方差阈值为15～20，如果一定时间段H内距离差序列C中元素的均值和方差均小于设定的均值阈值和方差阈值，则判定这段时间H内的运动轨迹是光滑的，则不需要调节q₁₁、q₂₂、r₁₁、r₂₂四个参数，否则认为是不光滑的，进入步骤四。

步骤四、依据GPS光滑度调节协方差阵。

距离差序列C的均值代表了GPS定位误差的均值、距离差序列C的方差代表了GPS定位误差的方差，而在滤波系统中，观测误差协方差阵R和状态误差方差协阵Q也反映的是误差的信息，因此可以根据获得的距离差统计特性对Kalman滤波中的协方差阵进行调节更新。滤波中的协方差阵也包括了航向、速度等信息，由于光滑度的判定只是就距离而言的，因此光滑度的判定仅调节协方差阵中的四个参数，东向距离状态误差q₁₁、东向距离观测误差r₁₁、北向距离状态误差q₂₂、北向距离观测误差r₂₂。

所述调节协方差阵的步骤如下：

1)统计特性的确定。

一个距离差序列C的均值与方差数值不能作为统计特性看待，而应对连续多个序列的统计特性综合考虑后才能够给出近似的统计特性，因此通常情况(水平坐标精度因子-HDOP数值变化较小的情况下，根据GPS的测量性质在HDOP波动幅度大于0.5的情况下)下需要3次序列统计后结果都比较近似才可以认为具有一定的统计意义。

●根据GPS的测量性质在HDOP波动幅度大于0.5的情况下：

POS_{mean_now}＝(POS_{mean_n}+POS_{mean_n+1}+POS_{mean_n+2})/3

POS_{var_now}＝(POS_{var_n}+POS_{var_n+1}+POS_{var_n+2})/3

POS_{mean_now}表示序列差的均值，POS_{mean_n}、POS_{mean_n+1}、POS_{mean_n+2}表示连续三个距离差序列C的均值。

POS_{var_now}表示序列差的方差，POS_{var_n}、POS_{var_n+1}、POS_{var_n+2}表示连续三个距离差序列C的方差。

●根据GPS的测量性质在HDOP波动幅度小于0.5的情况下：

在保证一次序列都是针对同一级别HDOP统计的结果情况下，可以不进行3次序列的平均，而使用当前序列的方差或者均值数据，从而使得系统滤波性能可以快速跟踪噪声的变化。

2)协方差阵元素的更新。

根据GPS定位误差情况，需要更新的参数主要有q₁₁、q₂₂、r₁₁、r₂₂，由于在步骤三的光滑度判定部分获得是均值和方差数据，并没有将其分解到东向、北向分别考虑，因此依据光滑度判定对协方差阵中元素进行调整时，将东向、北向误差简化认为是具有相同的统计特性。

在kalman滤波系统中，协方差阵的调节主要用于对滤波的最优增益矩阵K，估计误差方差阵P，以及状态向量的估计，滤波方程的解如下：

$\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+K(k+1)[y(k+1)-H(k+1)\hat{x}(k+1|k)]$

$\hat{x}(k+1|k)=\mathrm{\φ}(k+1,k)\hat{x}\left(k\right|k)$

K(k+1)＝P(k+1|k)H^T(k+1)[H(k+1)P(k+1|k)H^T(k+1)+R(k+1)]^-1

P(k+1|k)＝φ(k+1，k)P(k|k)φ^T(k+1，k)+Q(k)

P(k+1|k+1)＝P(k+1|k)-K(k+1)H(k+1)P(k+1|k)

表示第k+1时刻对x(k+1)的估计；

表示第k时刻对x(k+1)的估计值；K(k+1)表示第k+1时刻的最优增益矩阵；y(k+1)表示第k+1时刻的观测向量；H(k+1)表示k+1时刻的观测矩阵；φ(k+1，k)表示第k时刻向第k+1时刻转换的状态转移矩阵；P(k+1|k)表示估计误差方差阵，k+1|k表示第k时刻对第k+1时刻的估计，k|k表示第k时刻对第k时刻的估计；R(k+1)表示的是第k+1时刻观测误差协方差阵，Q(k)表示的是第k时刻的状态误差协方差阵。本发明方法即是对不同时刻的Q(k)和R(k)进行自适应调节，从而实时调节误差估计方差阵以及最优增益矩阵，如公式所示，进而达到对状态进行精确估计的效果。

本发明方法采用的两个序列的差测出的误差信息可以描述R(k)的大小，R(k)物理含义上表示的观测误差信息，通过对POS_{var_now}的统计，可以获得实际观测误差的值。本发明方法采用了GPS位置的位置差信息，对于GPS位置系统误差不能做出适当的估计；在实际的导航系统中，由于构建模型的不准确，也会带来误差，根据衰减记忆滤波思想，可以通过增大观测误差的方式抑制模型误差的影响；同时由于统计样本数量少，观测误差不能完全保证误差的统计信息；基于以上三种考虑，可知获得的观测误差信息比实际的误差要小，在Kalman滤波过程中需要对观测噪声方差进行必要的放大，对于R中的r₁₁，r₂₂给予以下设定：

r₁₁＝r₂₂＝α*POS_{var_now}

α的取值范围对精度的影响存在着一定的变化趋势，α的取值为0.5～3时逐渐变化时，定位精度的提高程度是从小逐渐变大，再逐渐变小的一个过程，α存在着一个真值，对α的取值偏离真值越多，误差越大，针对本发明所采用的Kalman滤波模型，当α取为1～2时能够获得相比其他值时更高的定位精度。对于不同的模型也存在这样一个变化的趋势，只是依据不同的模型，α取值的大小有差异。通过大量的系统实际运行验证，也获得了相同的结论。

Kalman滤波方程中容易推导出：

$E\left(\tilde{y}\left(k\right|k-1){\tilde{y}}^T\left(k\right|k-1)\right)=H\left(k\right)P\left(k\right|k-1)H^T\left(k\right)+R\left(k\right)$

在滤波估计较准确的情况下，可以近似为R(k)。在滤波估计中位置信息即是状态变量，也是观测变量，在滤波模型准确时，对状态变量的估计也准确，此时P阵较小。

在kalman滤波的解算公式中可以得知：

P(k+1|k)＝φ(k+1，k)P(k|k)φ^T(k+1，k)+Q(k)

可知估计误差协方差阵P与Q存在以上的数学关系，而根据

$E\left(\tilde{y}\left(k\right|k-1){\tilde{y}}^T\left(k\right|k-1)\right)=H\left(k\right)P\left(k\right|k-1)H^T\left(k\right)+R\left(k\right)$ 同样可以知道Q与

也存在一定的数学关系，

可以近似为R(k)，本发明方法的R(k)根据POS_{var_now}的统计信息可以获得，因此本发明对于Q中的q₁₁，q₂₂给予以下的设定：

q₁₁＝q₂₂＝β*POS_{var_now}

Q反映的是状态误差信息，在模型准确的情况下，值较小，针对本发明所采用的滤波模型，对β进行了反复的调试，得出当β取为10％～40％时，均能提高系统的定位精度，相比Q，R取经验值时的定位精度高，自适应调节能力强。对于不同的滤波模型，H、P也不同，β的取值范围也因似情况而定。

步骤1)统计特性的确定可以得到实时的POS_{var_now}的值，运用以上的两个公式从而实现对于协方差阵Q、R中的参数q₁₁、q₂₂、r₁₁、r₂₂的自适应调节。

本发明的优点在于：

(1)本发明方法实现简单，易于操作；

(2)该方法可以对误差协方差阵Q，R参数进行自适应调节；

(3)减小试凑法的工作量；

(4)提高导航系统的定位精度，与直接滤波相比提高7％。

附图说明

图1是本发明自适应调节方法的流程图；

图2是本发明的GPS光滑度判定示意图；

图3a是本发明的仿真实验轨迹1结果图；

图3b是本发明的仿真实验轨迹2结果图；

图3c是本发明的仿真实验轨迹3结果图；

图3d仿真实验位置误差结果图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明的车载导航系统误差协方差阵参数自适应调节的方法的流程如附图1所示，具体实现过程如下：

步骤一、建立车载导航系统的数学模型。

根据车辆导航系统的的实际数据情况，选取GPS北向距离N_y、GPS东向距离E_x、GPS航向ψ、GPS速度v、里程仪的刻度因子S，陀螺角速率作为状态变量，构建Kalman滤波模型状态方程，具体的数学模型如下：

状态向量x^T： $x^T=\left[\begin{array}{cccccc}{N}_y& E_x& \mathrm{\ψ}& v& S& {\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\end{array}\right]$

状态方程为：x(k+1)＝φ(k+1，k)x(k)+w(k)

状态误差向量为：w^T(k)＝[w₁ w₂ w₃ w₄ w₅ w₆]

GPS/DR导航系统的状态方程组如下：

N_y(k+1)＝N_y(k)+v(k)Tcos(ψ(k))+w₁

E_x(k+1)＝E_x(k)+v(k)Tsin(ψ(k))+w₂

$\mathrm{\ψ}(k+1)=\mathrm{\ψ}\left(k\right)+T{\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+w_3$

v(k+1)＝v(k)+w₄

S(k+1)＝S(k)+w₅

${\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}(k+1)={\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+w_6$

k表示当前时刻，k+1为下一时刻，w₁，w₂，w₃，w₄，w₅，w₆分别表示各个状态变量的状态误差，T为周期，本发明方法取为1s。状态转移矩阵经过泰勒展开得：

$\mathrm{\φ}(k+1,k)=\left[\begin{array}{cccccc}1& 0& -v\left(k\right)T\sin \left(\mathrm{\ψ}\left(k\right)\right)& T\cos \left(\mathrm{\ψ}\left(k\right)\right)& 0& 0\\ 0& 1& v\left(k\right)T\cos \left(\mathrm{\ψ}\left(k\right)\right)& T\sin \left(\mathrm{\ψ}\left(k\right)\right)& 0& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

状态误差协方差阵：

$Q=E\left[{\mathrm{ww}}^T\right]=\left[\begin{array}{cccccc}{q}_{11}& 0& 0& 0& 0& 0\\ 0& q_{22}& 0& 0& 0& 0\\ 0& 0& q_{33}& 0& 0& 0\\ 0& 0& 0& q_{44}& 0& 0\\ 0& 0& 0& 0& q_{55}& 0\\ 0& 0& 0& 0& 0& q_{66}\end{array}\right]$

w表示为状态变量估计值的状态误差向量。

选取GPS北向距离N_gps、GPS东向距离E_gps、GPS航向ψ_gps、GPS速度v_speed，陀螺角速率

-作为观测变量，构建Kalman滤波模型观测方程，具体的数学模型如下：

观测向量y^T： $y^T=\left[\begin{array}{ccccc}{N}_{\mathrm{gps}}& E_{\mathrm{gps}}& {\mathrm{\ψ}}_{\mathrm{gps}}& v_{\mathrm{speed}}& {\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gyro}}\end{array}\right]$

观测方程为：y(k+1)＝H(k)x(k)+e(k)

观测误差向量为：e^T(k)＝[e₁ e₂ e₃ e₄ e₅]

GPS/DR导航系统的观测方程组如下：

N_gps(k)＝N_y(k)+e₁

E_gps(k)＝E_x(k)+e₂

${\mathrm{\ψ}}_{\mathrm{gps}}\left(k\right)=\stackrel{\·}{\mathrm{\ψ}}\left(k\right)+e_3$

v_speed(k)＝m(k)S(k)+e₄

${\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gyro}}\left(k\right)={\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+e_5$

e₁，e₂，e₃，e₄，e₅分别表示各个观测变量的观测误差，m(k)表示的是当前周期内里程仪累计的脉冲数。经过泰勒展开：

$H\left(k\right)=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& m\left(k\right)& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

观测误差协方差阵：

$R=E\left[{\mathrm{ee}}^T\right]=\left[\begin{array}{ccccc}{r}_{11}& 0& 0& 0& 0\\ 0& r_{22}& 0& 0& 0\\ 0& 0& r_{33}& 0& 0\\ 0& 0& 0& r_{44}& 0\\ 0& 0& 0& 0& r_{55}\end{array}\right]$

e表示为观测变量估计值的观测误差。

根据状态与观测向量的定义，状态协方差阵的说明如下表1，表2所示：

依据上述建立的数学模型进入步骤二进行系统滤波，系统滤波的输出值即为状态变量。针对此车载导航系统分析，本发明仅对状态误差协方差阵Q和观测误差协方差阵R中的q₁₁、q₂₂、r₁₁、r₂₂进行自适应调节。

步骤二、依据步骤一建立的数学模型进行系统滤波。

依据步骤一建立的数学模型对车辆导航系统进行系统滤波，滤波是一个循环的过程，以周期T(本发明方法取为1s)为单位，根据上一时刻对当前时刻的估计以及当前时刻的观测变量对当前时刻的状态变量进行滤波，获得当前时刻更为精确的状态变量，作为系统滤波的输出值。根据所获得的观测变量和状态变量对Q，R进行调节。

步骤三、GPS光滑度判定。

光滑度是指输出轨迹的光滑程度。以短时间内航位推算(DR)轨迹作为理想轨迹对GPS轨迹光滑度进行估计判定。具体的判定步骤以及判定的方法如下：

1)设定轨迹的时间长度。

车辆的运动轨迹随着时间的变化而变化，把运动轨迹根据相等的时间长度分成很多不同的轨迹段，设定时间长度H，一般取15s～20s，并且不同轨迹段之间不可有时间重叠。时间长度可以根据车辆行驶的路况进行调整。

2)DR相对运动序列A的构造。

依据步骤1)中设定的时间长度，利用DR解算公式获得每段轨迹内相邻两点之间的距离L：

L＝S(k)*(m(k-1)+m(k))/2

S(k)表示里程仪的刻度因素，m(k-1)表示前一时刻里程仪的脉冲数，m(k)表示当前周期内里程仪累计的脉冲数，依据以上公式构造序列A，A中的每个元素表示一个周期1s内车辆行驶的位移量。

3)GPS相对运动序列B的构造。

依据步骤1)中设定的时间长度，利用GPS相邻时刻输出的位置信息做差，求出每一秒的位移，得出相邻两点间距离，构造序列B。

4)构造距离差序列C。

将序列A和序列B相减得到一个距离差序列C，获得相邻两秒中GPS测量位移与DR测量位移之差，距离差序列C中的元素有正负之分。

5)误差统计。

对距离差序列C进行统计，获得一定时间段内如15s内由步骤4)获得的距离差序列C内元素的均值与方差。

6)设定比较的阈值判断光滑度。

设定均值阈值为5～10，方差阈值为15～20，如果一定时间段H内如15s内的距离差序列C中元素的均值和方差均小于设定的均值阈值和方差阈值，则判定这段时间段H内的运动轨迹是光滑的，则不需要调节q₁₁、q₂₂、r₁₁、r₂₂四个参数，否则认为是不光滑的，进入步骤四。

示意说明图如图2所示，标志为T的点代表的是陀螺、里程仪获得的位置信息，标志为G的点代表的是GPS的定位信息，箭头表示车辆行驶的方向。从示意图中可以看出，由陀螺和里程仪获得的位置信息较为平稳，位置的波动较小，输出的轨迹与GPS定位的轨迹相比较为光滑。这样短时间内可以以陀螺和里程仪获得的轨迹作为参照，而GPS获得的轨迹与之进行比较，得出GPS定位的光滑度。

步骤四、依据GPS光滑度调节协方差阵。

在不考虑GPS定位系统误差的情况下，距离差序列C的均值代表了GPS定位误差的均值、距离差序列C的方差代表了GPS定位误差的方差，而在滤波系统中，观测误差协方差阵R和状态误差协方差阵Q也反映的是误差的信息，因此可以根据获得的距离差统计特性对Kalman滤波中的协方差阵进行调节更新。滤波中的协方差阵也包括了航向，速度等信息，由于光滑度的判定只是就距离而言的，因此光滑度的判定仅调节协方差阵中的四个参数，东向距离状态误差q₁₁、东向距离观测误差r₁₁、北向距离状态误差q₂₂、北向距离观测误差r₂₂。

所述调节协方差阵的步骤如下：

1)统计特性的确定。

一个距离差序列C的均值与方差数值不能作为统计特性看待，而应对连续多个序列的统计特性综合考虑后才能够给出近似的统计特性，因此通常情况(水平坐标精度因子-HDOP数值变化较小的情况下，根据GPS的测量性质在HDOP波动幅度大于0.5的情况下)下需要3次序列统计后结果都比较近似才可以认为具有一定的统计意义。

●根据GPS的测量性质在HDOP波动幅度大于0.5的情况下：

POS_{mean_now}＝(POS_{mean_n}+POS_{mean_n+1}+POS_{mean_n+2})/3

POS_{var_now}＝(POS_{var_n}+POS_{var_n+1}+POS_{var_n+2})/3

POS_{mean_now}表示序列差的均值POS_{mean_n}、POS_{mean_n+1}、POS_{mean_n+2}表示连续三个距离差序列C的均值。

POS_{var_now}表示序列差的方差，POS_{var_n}、POS_{var_n+1}、POS_{var_n+2}表示连续三个距离差序列C的方差。

●根据GPS的测量性质在HDOP波动幅度小于0.5的情况下：

在保证一次序列都是针对同一级别HDOP统计的结果情况下，可以不进行3次序列的平均，而使用当前序列的方差或者均值数据，从而使得系统滤波性能可以快速跟踪噪声的变化。

2)协方差阵元素的更新。

根据GPS定位误差情况需要更新的参数主要有q₁₁、q₂₂、r₁₁、r₂₂，由于在步骤三的光滑度判定部分获得是均值和方差数据，并没有将其分解到东向、北向分别考虑，因此依据光滑度判定对协方差阵中元素进行调整时，将东向、北向误差简化认为是具有相同的统计特性。

在kalman滤波系统中，协方差阵的调节主要用于对滤波的最优增益矩阵K，估计误差方差阵P，以及状态向量的估计，滤波方程的解如下：

$\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+K(k+1)[y(k+1)-H(k+1)\hat{x}(k+1|k)]$

$\hat{x}(k+1|k)=\mathrm{\φ}(k+1,k)\hat{x}\left(k\right|k)$

K(k+1)＝P(k+1|k)H^T(k+1)[H(k+1)P(k+1|k)H^T(k+1)+R(k+1)]^-1

P(k+1|k)＝φ(k+1，k)P(k|k)φ^T(k+1，k)+Q(k)

P(k+1|k+1)＝P(k+1|k)-K(k+1)H(k+1)P(k+1|k)

表示第k+1时刻对x(k+1)的估计；

表示第k时刻对x(k+1)的估计值；K(k+1)表示第k+1时刻的最优增益矩阵；y(k+1)表示第k+1时刻的观测向量；H(k+1)表示k+1时刻的观测矩阵；φ(k+1，k)表示第k时刻向第k+1时刻转换的状态转移矩阵；P(k+1|k)表示估计误差方差阵，k+1|k表示第k时刻对第k+1时刻的估计，k|k表示第k时刻对第k时刻的估计；R(k+1)表示的是第k+1时刻观测误差协方差阵，Q(k)表示的是第k时刻的状态误差协方差阵。本发明方法即是对不同时刻的Q(k)和R(k)进行自适应调节，从而实时调节误差估计方差阵以及最优增益矩阵，如公式所示，进而达到对状态进行精确估计的效果。

本发明方法采用的两个序列的差测出的误差信息可以描述R(k)的大小，R(k)物理含义上表示的观测误差信息，通过对POS_{var_now}的统计，可以获得实际观测误差的值。本发明方法采用了GPS位置的位置差信息，对于GPS位置系统误差不能做出适当的估计；在实际的导航系统中，由于构建模型的不准确，也会带来误差，根据衰减记忆滤波思想，可以通过增大观测误差的方式抑制模型误差的影响；同时由于统计样本数量少，观测误差不能完全保证误差的统计信息；基于以上三种考虑，可知获得的观测误差信息比实际的误差要小，在Kalman滤波过程中需要对观测噪声方差进行必要的放大，对于R中的r₁₁，r₂₂给予以下设定：

r₁₁＝r₂₂＝α*POS_{var_now}

α的取值范围对精度的影响存在着一定的变化趋势，α的取值为0.5～3时逐渐变化时，定位精度的提高程度是从小逐渐变大，再逐渐变小的一个过程，α存在着一个真值，对α的取值偏离真值越多，误差越大，针对本发明所采用的Kalman滤波模型，当α取为1～2时能够获得相比其他值时更高的定位精度。对于不同的模型也存在这样一个变化的趋势，只是依据不同的模型，α取值的大小有差异。通过大量的系统实际运行验证，也获得了相同的结论。

Kalman滤波方程中容易推导出：

$E\left(\tilde{y}\left(k\right|k-1){\tilde{y}}^T\left(k\right|k-1)\right)=H\left(k\right)P\left(k\right|k-1)H^T\left(k\right)+R\left(k\right)$

在滤波估计较准确的情况下，

可以近似为R(k)。在滤波估计中位置信息即是状态变量，也是观测变量，在滤波模型准确时，对状态变量的估计也准确，此时P阵较小。

在kalman滤波的解算公式中可以得知：

P(k+1|k)＝φ(k+1，k)P(k|k)φ^T(k+1，k)+Q(k)

可知估计误差协方差阵P与Q存在以上的数学关系，而根据

$E\left(\tilde{y}\left(k\right|k-1){\tilde{y}}^T\left(k\right|k-1)\right)=H\left(k\right)P\left(k\right|k-1)H^T\left(k\right)+R\left(k\right)$ 同样可以知道Q与

也存在一定的数学关系，

可以近似为R(k)，本发明方法的R(k)根据POS_{var_now}的统计信息可以获得，因此本发明对于Q中的q₁₁，q₂₂给予以下的设定：

q₁₁＝q₂₂＝β*POS_{var_now}

Q反映的是状态误差信息，在模型准确的情况下，Q值较小，针对本发明所采用的滤波模型，对β进行了反复的调试，得出当β取为10％～40％时，均能提高系统的定位精度，相比Q，R取经验值时的定位精度高，自适应调节能力强。对于不同的滤波模型，H、P也不同，β的取值范围也因此情况而定。

步骤1)统计特性的确定可以得到实时的POS_{var_now}的值，运用以上的两个公式从而实现对与协方差阵Q、R中参数q₁₁、q₂₂、r₁₁、r₂₂的自适应调节。

本发明方法通过真实的数据进行了仿真实验，依据以上的步骤对q₁₁、q₂₂、r₁₁、r₂₂四个参数值进行了自适应调节，相比不调节误差协方差阵的定位精度提高了7％。实验的结果如图3a、图3b、图3c所示，截取了不同时间段的轨迹输出，图3d为仿真实验位置误差结果图。

图中横坐标表示的是车辆行驶的东向位置信息，纵坐标表示的是北向位置信息。同时图中的短点虚线表示的是无噪声时GPS输出的位置信息，长点虚线是加噪声后滤波系统输出的GPS的位置信息，点线交叉虚线表示的是采用协方差阵取经验值时GPS的位置信息，实线表示的是采用了本发明方法的GPS的位置信息。从图中可以看出实线比长点线明显的接近无噪声时的位置信息，相比取去经验值时也更接近没有误差的位置信息，表明采用本发明的方法可以提高GPS的定位精度。图3d为仿真实验位置误差结果图，横坐标表示时刻，纵坐标表示位置误差，点线交叉虚线表示Q，R取经验值时的定位误差，实线表示Q，R采用本发明方法自适应调节的定位误差，选取了两个不同的轨迹段，图中可以看到实线表示的定位误差明显较小，提高了定位精度。

仿真过程中，不同轨迹段的噪声的大小也不一样，本发明方法可以适应噪声的变化，达到实时动态调节的效果。同时表3统计了不同轨迹段内的位置误差的均值，均值1表示的是Q中的q₁₁、q₂₂取16，R中的r₁₁、r₂₂取36经验值时的情况，均值2表示Q，R采用本发明方法α取2，β取0.1自适应调节时的情况。表4统计了不同轨迹段内的位置误差的方差，同理方差1表示的是q₁₁、q₂₂取16，r₁₁、r₂₂取36时的情况，方差2表示α取2，β取0.1时的情况。从表中可以看出，均值2和方差2分别小于均值1和方差1，表明本发明方法相比误差协方差阵取经验值时可以减小定位误差，提高定位精度。

表3不同轨迹段位置误差均值

表4不同轨迹段位置误差方差

Claims (2)

1、车载导航系统误差协方差阵参数自适应调节的方法，包括如下步骤：

步骤一、建立车载导航系统的数学模型；

步骤二、系统滤波；

依据步骤一建立的数学模型对车辆导航系统进行系统滤波，滤波是一个循环的过程，以周期T为单位，根据上一时刻对当前时刻的估计以及当前时刻的观测变量对当前时刻的状态变量进行滤波估计，获得当前时刻更为精确的状态变量，做为系统滤波的输出值；

其特征在于：还包括如下步骤：

步骤三、GPS光滑度判定；

1)设定轨迹的时间长度；

把车辆的运动轨迹根据相等时间长度分成很多不同的轨迹段，设定时间长度H取15s～20s，并且不同轨迹段之间不可有时间重叠；

2)DR相对运动序列A的构造；

依据步骤1)中设定的时间长度，利用DR解算公式获得每段轨迹内相邻两点之间的距离L：

L＝S(k)*(m(k-1)+m(k))/2

S(k)表示里程仪的刻度因素，m(k-1)表示前一周期内里程仪累计的脉冲数，m(k)表示当前周期内累计的脉冲数，依据以上公式构造序列A，A中的每个元素表示一个周期1s内车辆行驶的位移量；

3)GPS相对运动序列B的构造；

依据步骤1)中设定的时间长度，利用GPS相邻时刻输出的位置信息做差，求出每一秒的位移，得出相邻两点间距离，构造序列B；

4)构造距离差序列；

将两序列A和序列B相减得到一个距离差序列C，获得相邻两秒中GPS测量位移与DR测量位移之差，距离差序列C中的元素有正负之分；

5)误差统计；

对距离差序列C进行统计，获得一定时间段内如15s内由步骤4)获得的距离差序列C内元素的均值与方差；

6)设定比较的阈值判断光滑度；

设定均值阈值为5～10，方差阈值为15～20，如果一定时间长度H内距离差序列C中元素的均值和方差均小于设定的均值阈值和方差阈值，则判定这段时间H内的运动轨迹是光滑的，则不需要调节q₁₁、q₂₂、r₁₁、r₂₂四个参数，否则认为是不光滑的，进入步骤四；

步骤四、依据GPS光滑度调节协方差阵；

所述调节协方差阵的步骤如下：

1)统计特性的确定

●根据GPS的测量性质在HDOP波动幅度大于0.5的情况下：

POS_{mean_now}＝(POS_{mean_n}+POS_{mean_n+1}+POS_{mean_n+2})/3

POS_{var_now}＝(POS_{var_n}+POS_{var_n+1}+POS_{var_n+2})/3

POS_{mean_now}表示序列差的均值POS_{mean_n}、POS_{mean_n+1}、POS_{mean_n+2}表示连续三个距离差序列C的均值；

POS_{var_now}表示序列差的方差，POS_{var_n}、POS_{var_n+1}、POS_{var_n+2}表示连续三个距离差序列C的方差；

·根据GPS的测量性质在HDOP波动幅度小于0.5的情况下：

在保证一次序列都是针对同一级别HDOP统计的结果情况下，使用当前序列的方差或者均值数据对协方差阵元素进行更新，从而使得系统滤波性能可以快速跟踪噪声的变化；

2)协方差阵元素的更新

r₁₁＝r₂₂＝α*POS_{var_now}    其中α为1～2；

q₁₁＝q₂₂＝β*POS_{var_now}    其中β为10％～40％。

2、根据权利要求1所述的车载导航系统误差协方差阵参数自适应调节的方法，其特征在于：

步骤一、建立车载导航系统的数学模型；

根据车辆导航系统的的实际数据情况，选取GPS北向距离N_y、GPS东向距离E_x、GPS航向ψ、GPS速度v、里程仪的刻度因子S，陀螺角速率作为状态变量，选取GPS北向距离N_gps、GPS东向距离E_gps、GPS航向ψ_gps、GPS速度v_speed，陀螺角速率

作为观测变量，构建Kalman滤波模型，包括状态方程与观测方程，具体的数学模型如下：

状态向量 $x^T:x^T=\left[\begin{array}{cccccc}{N}_y& E_x& \mathrm{\ψ}& v& S& {\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\end{array}\right]$

状态方程为：x(k+1)＝φ(k+1，k)x(k)+w(k)

状态误差向量为：w^T(k)＝[w₁ w₂ w₃ w₄ w₅ w₆]

GPS/DR导航系统的状态方程组如下：

N_y(k+1)＝N_y(k)+v(k)Tcos(ψ(k))+w₁

E_x(k+1)＝E_x(k)+v(k)Tsin(ψ(k))+w₂

$\mathrm{\ψ}(k+1)=\mathrm{\ψ}\left(k\right)+T{\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+w_3$

v(k+1)＝v(k)+w₄

S(k+1)＝S(k)+w₅

${\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}(k+1)={\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+w_6$

k表示当前时刻，k+1为下一时刻，w₁，w₂，w₃，w₄，w₅，w₆分别表示各个状态变量的状态误差，T为周期，取为1s，状态转移矩阵经过泰勒展开得：

$\mathrm{\φ}(k+1,k)=\left[\begin{array}{cccccc}1& 0& -v\left(k\right)T\sin \left(\mathrm{\ψ}\left(k\right)\right)& T\cos \left(\mathrm{\ψ}\left(k\right)\right)& 0& 0\\ 0& 1& v\left(k\right)T\cos \left(\mathrm{\ψ}\left(k\right)\right)& T\sin \left(\mathrm{\ψ}\left(k\right)\right)& 0& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

状态误差协方差阵：

$Q=E\left[w{w}^T\right]=\left[\begin{array}{cccccc}{q}_{11}& 0& 0& 0& 0& 0\\ 0& q_{22}& 0& 0& 0& 0\\ 0& 0& q_{33}& 0& 0& 0\\ 0& 0& 0& q_{44}& 0& 0\\ 0& 0& 0& 0& q_{55}& 0\\ 0& 0& 0& 0& 0& q_{66}\end{array}\right]$

w表示为状态变量估计值的状态误差向量；

观测向量 $y^T:y^T=\left[\begin{array}{ccccc}{N}_{\mathrm{gps}}& E_{\mathrm{gps}}& {\mathrm{\ψ}}_{\mathrm{gps}}& v_{\mathrm{speed}}& {\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gyro}}\end{array}\right]$

观测方程为：y(k+1)＝H(k)x(k)+e(k)

观测误差向量为：e^T(k)＝[e₁ e₂ e₃ e₄ e₅]

GPS/DR导航系统的观测方程组如下：

N_gps(k)＝N_y(k)+e₁

E_gps(k)＝E_x(k)+e₂

ψ_gps(k)＝ψ(k)+e₃

v_speed(k)＝m(k)S(k)+e₄

${\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gyro}}\left(k\right)={\stackrel{\·}{\mathrm{\ψ}}}_{\mathrm{gy}}\left(k\right)+e_5$

e₁，e₂，e₃，e₄，e₅分别表示各个观测变量的观测误差，m(k)表示的是当前周期内里程仪累计的脉冲数，观测矩阵为：

$H\left(k\right)=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& m\left(k\right)& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

观测误差协方差阵：

$R=E\left[e{e}^T\right]=\left[\begin{array}{ccccc}{r}_{11}& 0& 0& 0& 0\\ 0& r_{22}& 0& 0& 0\\ 0& 0& r_{33}& 0& 0\\ 0& 0& 0& r_{44}& 0\\ 0& 0& 0& 0& r_{55}\end{array}\right]$

e表示为观测变量估计值的观测误差向量；r₁₁、r₂₂、r₃₃、r₄₄，r₅₅分别为北向位置观测噪声方差、东向位置观测噪声方差、GPS航向观测噪声方差和GPS速度观测噪声方差，陀螺角速率观测误差。

Images