[发明专利]车载导航系统误差协方差阵部分参数自适应调节的方法

权利要求书

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进行统计，获得一定时间段内由步骤4)获得的距离差序列C内元素的均值与方差；

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

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

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

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

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

状态误差协方差阵：

其中，q₁₁表示北向位置状态噪声方差，q₂₂表示东向位置状态噪声方差；q₃₃表示GPS航向状态噪声方差；q₄₄表示GPS速度状态噪声方差；q₅₅表示里程仪状态噪声方差；q₆₆表示陀螺角速率状态噪声方差；w表示为状态变量估计值的状态误差向量；

观测向量

观测方程为：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₄

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

观测误差协方差阵：

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