基于自适应里程计模型的车载定位导航方法与流程

本发明涉及车载定位导航，具体涉及一种基于自适应里程计模型的车载定位导航方法。

背景技术：

车载导航系统已经成为人们驾车出行的必备配置，目前的车载定位导航系统多数是是非线性的，车辆机动模型的系统方程和观测方程也都是非线性的。现有的扩展卡尔曼滤波算法在单点处对非线性函数进行线性化近似，对系统状态分布的近似比较简单，并且在系统状态发生骤变时，收敛比较慢，不能及时应变处理，在面对紧急情况的安全性方面仍无法满足。

现有的车载定位导航系统多通过装载gps接收装置，获得定位目标的速度、位置等信息。但gps定位也存在一定的缺点，当gps信号受到建筑物遮挡时，其定位的连续性就遭到了破坏，难以保证在特殊情况下定位导航功能的稳定性。且gps在进行车辆定位时的误差范围在14m左右，定位精确度仍有待提高。

技术实现要素：

发明目的：本发明目的是提供一种自适应里程计模型的车载定位导航方法，提高车载定位导航的精确度和连续性，提高障碍物环境下导航性能的稳定性。

技术方案：本发明提供了一种基于自适应里程计模型的车载定位导航方法，该方法包括：

(1)根据车辆可能存在的m个运动状态对应建立m个非线性运动模型以及m个模型状态向量之间的转移矩阵ti，j；

非线性运动模型的状态方程表示为

其中，i，j＝1，2，...m，i、j表示对应的非线性运动模型，ti，j即为从模型i到模型j的转移矩阵，k为车辆行驶过程中的采样时刻，是k时刻的状态向量，是k时刻里程计的输出数据，是均值为零的白噪声序列；

(2)采集k时刻车辆左右驱动轮里程计的数据，根据车辆行驶过程中的车辆状态的几何约束关系，分析左右驱动轮的里程计采集的运动距离和位置姿态数据，得出车辆中心点的运动距离和位置变化数据；

(3)融合容积卡尔曼滤波，加入gps观测值，并行计算各运动模型的输出，包括：

(31)建立容积卡尔曼滤波的非线性观测方程zk＝h(xk，vk)，xk是系统的状态向量，vk为系统观测噪声序列；

(32)将k-1时刻的误差协方差矩阵pj，k-1|k-1进行chol分解得到下三角矩阵sk-1|k-1；

(33)将k-1时刻的混合状态估计根据球面-半径的规则确定2n个cubature点，根据cubature点计算得到cubature点xi，k-1|k-1及其相应的权值wi，n为模型状态向量的维数；

(34)将cubature点xi，k-1|k-1通过模型j的状态方程，计算出状态的一步预测cubature点值x^*i，k|k-1，并计算出状态的一步预测值和一步预测协方差矩阵pj，k|k-1；

(35)将一步预测协方差矩阵pj，k|k-1进行chol分解，得到下三角矩阵sk|k-1；

(36)利用下三角矩阵sk|k-1计算得到2n个cubature点xi，k|k-1及其相应的权值wi；

(37)将cubature点xi，k|k-1通过系统观测方程，求出观察预计值并利用2n个观察预计值来计算此时整体的观察预计

(38)计算k时刻的观测协方差矩阵pzz，k|k-1及状态观测互协方差矩阵pxz，k|k-1；

(39)量测更新，计算容积卡尔曼滤波增益kj，计算模型j在k时刻的状态估计值以及误差协方差矩阵pj，k|k；

(4)分析车辆左右驱动轮里程计中的位移数据，判断当前车辆所处的运动状态，并匹配对应的运动模型；根据匹配的运动模型更新模型概率；

(5)融合输出交互，得到k时刻总的状态估计和总的协方差估计pk|k；判断车辆行驶是否停止，若停止，则结束滤波；否则，返回步骤(2)。

进一步地，步骤(1)中，几何约束关系为：

δda＝(δdb+δdc)/2

γa＝(-adb+δdc)/l

其中，δdb、δdc和δda分别是左右两轮里程计和车辆中心点在采样间隔内的位移，l是车辆的宽度，γa是车辆中心在采样间隔内的位置姿态偏移角度。

进一步地，步骤(33)中，cubature点的具体确定方法如下：

其中，向量集p由每个维度上只能为1、0、-1的单位向量构成，n为状态向量维数。

进一步地，步骤(34)中，一步预测值和一步预测协方差矩阵pj，k|k-1的具体方法如下：

其中，γk|k-1是系统的过程噪声驱动阵，qk-1是系统过程噪声wk-1的方差阵。

进一步地，步骤(38)(39)中，观测预测值为观测协方差矩阵为pzz，k|k-1及状态观测互协方差矩阵为pxz，k|k-1的具体方法如下：

其中，rk为观测噪声协方差矩阵。

进一步地，步骤(5)中，总的状态估计和总的协方差估计pk|k具体方法如下：

其中，μj(k)为k时刻运动模型j的概率。

有益效果：与现有技术相比，本发明的定位导航方法在车辆的左右驱动轮上均安装里程计，而这两个设备与车辆的相对位置始终保持不变，因而各个时刻总保持着固定的几何关系。利用该约束关系，可以降低gps对车辆位置坐标观测数据的观察误差。通过对左右轮里程计数据的分析，作为速度模型、加速度模型、转弯模型切换的依据，代替现有技术中的的协方差矩阵分析的概率方法，能够更准确的切换模型，从而提高卡尔曼滤波的精度、稳定性。

附图说明

图1是本发明的定位导航方法流程图；

图2是车辆左右驱动轮上里程计的几何关系图；

图3-1是本发明车辆二维运动轨迹估计图；

图3-2是本发明车辆二维运动轨迹估计图的局部放大图；

图4是本发明车辆在y方向速度估计图；

图5是本发明与使用ckf-imm算法的车辆在整体定位误差上的对比图；

图6是本发明与使用ckf-imm算法的车辆整体仿真定位结果的局部放大图。

具体实施方式

下面结合附图和实施例对本发明做进一步描述：

本发明提供了一种基于自适应里程计模型的车载定位导航方法，如图1所示，该方法包括：

(1)根据车辆可能存在的m个运动状态对应建立m个非线性运动模型以及m个模型状态向量之间的转移矩阵ti，j；

非线性运动模型表示为

其中，i，j＝1，2，...m，j表示对应的非线性运动模型，ti，j即为从模型i到模型j的转移矩阵，k为车辆行驶过程中的采样时刻，是k时刻的状态向量，是k时刻里程计的输出数据，是均值为零的白噪声序列；

(2)采集k时刻车辆左右驱动轮里程计的数据，根据车辆行驶过程中的车辆状态的几何约束关系，分析左右驱动轮的里程计采集的运动距离和位置姿态数据，得出车辆中心点的运动距离和位置变化数据；其中，如图2所示，车辆状态的几何约束关系为：

δda＝(δdb+δdc)/2(2)

γa＝(-δdb+δdc)/l(3)

式中，δdb、δdc和δda分别是左右两轮里程计和车辆中心点在采样间隔内的位移，l是车辆的宽度，γa是车辆中心在采样间隔内的位置姿态偏移角度。

(3)融合容积卡尔曼滤波，加入gps观测值，并行计算各运动模型的输出，包括以下步骤：

建立容积卡尔曼滤波的非线性观测方程zk＝h(xk，vk)，xk是系统的状态向量，vk为系统观测噪声序列；

将k-1时刻的误差协方差矩阵pj，k-1|k-1进行chol分解得到下三角矩阵sk-1|k-1；

将k-1时刻的混合状态估计根据球面-半径的规则确定2n个cubature点，根据cubature点计算得到cubature点xi，k-1|k-1及其相应的权值wi，n为模型状态向量的维数；cubature点的具体确定方法如下：

其中，向量集p由每个维度上只能为1、0、-1的单位向量构成，n为状态向量维数。

将cubature点xi，k-1|k-1通过模型j的状态方程，计算出状态的一步预测cubature点值x^*i，k|k-1，并计算出状态的一步预测值和一步预测协方差矩阵pj，k|k-1；二者通过以下方法计算：

其中，γk|k-1是系统的过程噪声驱动阵，qk-1是系统过程噪声wk-1的方差阵。

将一步预测协方差矩阵pj，k|k-1进行chol分解，得到下三角矩阵sk|k-1；

利用下三角矩阵sk|k-1计算得到2n个cubature点xi，k|k-1及其相应的权值wi；

将cubature点xi，k|k-1通过系统观测方程，求出观察预计值并利用2n个观察预计值来计算此时整体的观察预计计算此时的观测协方差矩阵pzz，k|k-1及状态观测互协方差矩阵pxz，k|k-1；其计算方法如式(9)～(12)：

其中，rk为观测噪声协方差矩阵。

量测更新，计算容积卡尔曼滤波增益kj，计算模型j在k时刻的状态估计值以及误差协方差矩阵pj，k|k；其计算方法如式(13)～(14)

(4)分析车辆左右驱动轮里程计中的位移数据，判断当前车辆所处的运动状态，并匹配对应的运动模型：

判断两里程计各自当前时刻与前一时刻的位移是否相同，即是否同时满足以下公式：

δdb，k-δdb，k-1＝＝0(15)

δdc，k-δdc，k-1＝＝0(16)

其中，δdb、δdc分别是左右里程计的位移数据；

若不满足，则判别为匀加速模型；

若里程计数据满足式(15)(16)，即当前时刻与前一时刻的位移相同；继续根据公式(17)比较左右里程计当前数值：

|δdb，k-δdc，k|≥a，a为常量(17)

其中，a表示机动车在转弯状态下，左右轮采样间隔内的位移差，在具体实施例中，a根据车辆的车宽来取值。

若左右里程计数值满足式(17)，判别为转弯模型，否则判别为匀速直线模型。

模型判别完成后，根据判别的运动模型更新模型概率。

(5)融合输出交互，得到k时刻总的状态估计和总的协方差估计pk|k；判断车辆行驶是否停止，若停止，则结束滤波；否则，返回步骤(2)。总的状态估计和总的协方差估计pk|k具体方法如下：

其中，μj(k)为k时刻运动模型j的概率。

本方法的方法性能和定位精度提高效果通过如下实施进一步说明：

本发明实施例中设置的状态向量包括车辆的位移、速度和加速度信息，维数为6维，观测向量包括x方向的位移和y方向的位移，维数为2维。仿真实验中设定车宽为3m，常量a取值0.03。仿真中车辆在二维空间初始方向角45度，初始位置位于(10，50)坐标处，初始速度为(20，20)。0-100个滤波点做匀速直线运动；100-200个滤波点做减速运动；200-700个点做匀速转弯运动；700-800个点作匀加速直线运动。车辆的运动轨迹估计图如图3所示。从图3可以看出，获取到的gps观测数据存在较大的噪声误差，使用里程计模型后的定位曲线显著提高了与实际运动轨迹的拟合程度。

测试车辆运动状态突然变化时的速度跟踪情况，实验结果如图4所示。相比起单独的容积卡尔曼滤波算法，加入里程计模型后，速度估计的精度得到显著提升且降低了时延。

测量本发明与ckf-imm算法的车辆在整体定位误差上的对比，实验数据单位为米(m)，实验结果如图5所示。可以看出车辆在进行匀速直线或变速直线或转弯运动时，使用里程计模型的定位误差均小于ckf-imm算法。

测量本发明与ckf-imm算法的车辆的定位仿真结果局部放大图如图6所示。实验对比了该方法与ckf-imm算法的性能，在遇到转弯等车辆机动状态持续改变的场景下，本发明提供的定位导航方法的定位误差明显小于ckf-imm算法。

本发明引入多个里程计设备的数据分析，使用不同的运动模型对应不同的运动状态，融合容积卡尔曼滤波，解决车载运动状态突然变化时定位效果不佳的问题，能够实现自适应滤波。且替换了原有的协方差矩阵的概率分析法，在算法的运行速度上也有较大的提升，并可在gps信号缺失的路况中使用。