一种基于坐标变换的三维车载定位导航方法与流程

本发明涉及一种基于坐标变换的三维车载定位导航方法，使用此方法得到的车辆定位高度信息能够保证车辆基本的高度定位要求，结合gis将车辆定位到具体的某一车道，有效地解决了复杂立交桥等路况下车辆定位错误的问题，不仅提高了车辆高度方向的定位精度，也提高了车辆导航的整体定位精度，可用于三维空间复杂的路况中。

背景技术：

在现实条件下，实际的系统多数是是非线性的，车辆机动模型的系统方程和观测方程也都是非线性的。无迹卡尔曼滤波根据被估计量和量测量的协方差阵来确定最佳增益阵，对系统方程和量测方程没有多余的附加要求，算法既可以用于线性系统又可以用于非线性系统，但在非线性系统的条件下，无迹卡尔曼滤波更能表现出其优越性，非线性越强，效果越明显。

在实际的车载运动中，不可能是单一的运动模型，目标车辆的运动过程是由多种机动状态组成。交互多模型算法是一种软切换算法，目前在机动目标跟踪领域得到了广泛的应用。能有效的克服单一模型带来的估计误差较大的问题，通过有效的加权融合对系统进行状态估计。交互多模型算法是使用不同的运动模型对应不同的运动状态过程，并将其假设为一个马尔可夫过程，即认为车辆的机动过程是一个马尔可夫系数下的多个运动模型的融合，各个运动模型的切换时通过转移概率来控制，融合每个模型的输出，将其作为整个目标状态的估计。

传统的交互多模型无迹卡尔曼滤波针对车载平面运动有很好的滤波效果，但对于高度信息的定位精度误差较大，无法完全区别出车辆位于复杂立交桥的哪一层，容易出现导航地图匹配实际路况出错的现象。由于gps接收信号和海拔高度与地面高度换算等误差因素，gps给出的车辆高度数据误差太大，在几米到几十米之间不等，将这样的高度数据用于滤波，明显效果不可能太精确，车辆高度定位误差太大的问题依然存在于现实生活中。

坐标变换是对空间实体的位置描述，是从一种坐标系变换到另一种坐标系，通过两个坐标系之间的对应关系来实现。任何维的旋转可以表述为向量与合适尺寸的方阵的乘积。最终一个旋转等价于在另一个不同坐标系下对点位置的重新表述。在地理信息系统中，有两种意义的坐标变换，一是地图投影变换，即从一种地图投影转换到另一种地图投影，地图上各点坐标均发生变化；另一是量测系统坐标转换，即从大地坐标系到地图坐标系、数字化仪坐标系、绘图仪坐标系或显示器坐标系之间的坐标转换。

技术实现要素：

本发明针对现有技术的不足，提出一种基于坐标变换的三维车载定位导航方法，先通过交互多模型无迹卡尔曼滤波对水平方向的车辆状态进行估计，当车辆进入坡面路况后利用坐标变换的方法，建立坐标系变化前后车辆三维位置坐标之间的关系方程式，利用平面坐标与高度坐标的关系解出车辆高度信息值，提高车辆在高度方面的定位精度，再结合gis将车辆定位到具体的某一车道。

为解决上述技术问题，本发明提供基于坐标变换的三维车载定位导航方法，包括以下步骤：

步骤1，建立m个非线性运动模型j，模型j在k+1时刻的表达式为是模型j在k时刻时均值为零的白噪声序列，是模型j在k时刻的概率，运动模型之间通过马尔可夫链来控制转移；

步骤2，建立无迹卡尔曼滤波的非线性观测方程zk＝h(xk,vk)，vk为系统观测噪声序列；

步骤3，模型i到模型j的转移概率为pij，计算在k-1时刻模型i到模型j的混合概率μij[(k-1)/(k-1)]，计算模型j在k-1时刻的混合状态估计和混合协方差估计pj[(k-1)/(k-1)]，将计算后得到的混合估计作为循环的初始状态；

步骤4，将模型j在k-1时刻的混合状态估计经过u变换计算出sigma点值及其相应的一阶权值和二阶权值其中i表示sigma点的序号；

步骤5，将sigma点值通过模型j的状态方程，计算出状态的一步预测sigma点值用状态的一步预测sigma点值计算出状态的一步预测值和一步预测协方差矩阵其中，上标x表示此矩阵是状态值的协方差矩阵，一步预测值为混合状态估计在k时刻的值；

步骤6，将步骤5中的一步预测sigma点值通过系统观测方程，求出观测值的sigma点值计算观测预测值为zj[(k)/(k-1)]、观测协方差矩阵为及状态观测互协方差矩阵为其中上标z表示此矩阵是观测值的协方差矩阵，上标xz表示此矩阵是状态值和观测值的互协方差矩阵；

步骤7，测量更新，计算增益阵kj，计算模型j在k时刻的状态估计值以及误差协方差矩阵

步骤8，计算更新后的概率模型j的似然函数λj(k)，加入新的观测信息z(k)后，更新当前模型概率；

步骤9，输出交互，各个模型并行进行以上步骤，更新各自模型概率后计算总的状态估计和总的协方差估计p^x[(k)/(k)]；

步骤10，判断车辆是否处于坡面，若否，则返回步骤3继续执行，反之则建立平面坐标系s和坡面坐标系s1，根据两个坐标系变化前后的关系，构建变化前后车辆三维位置坐标之间的方程式，由此计算出车辆在坡面的高度信息，之后再返回步骤3继续循环执行。

进一步，步骤4中，sigma点值的具体计算方法如下：

式中，n为状态向量维数，pj[(k-1)/(k-1)]为混合协方差估计，λ为缩放比例参数，其中i表示sigma点的序号；

进一步，步骤5中，计算状态的一步预测值和一步预测协方差矩阵的具体方法如下：

其中，为步骤4中的sigma通过模型j的状态方程后的一步预测sigma点值。

进一步，步骤6中，计算观测预测值为zj[(k)/(k-1)]、观测协方差矩阵为及状态观测互协方差矩阵为的具体方法如下：

其中，qk-1为观测噪声协方差矩阵，观测值的sigma点值。

进一步，步骤9中，计算总的状态估计和总的协方差估计p^x[(k)/(k)]的具体方法如下：

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

进一步，步骤10中，计算车辆三维位置坐标之间方程式的具体方法如下：

其中，为坐标系s变换的方向角，θ为坐标系s变换的仰角，ex1、ey1、ez1是坐标系s1的坐标轴单位向量，ex、ey、ez是坐标系s的坐标轴单位向量。

本发明所达到的有益技术效果：

1、引入交互多模型使用不同的运动模型对应不同的运动状态过程，融合传统的无迹卡尔曼滤波，解决车载运动状态突然变化时定位效果不佳的问题，能够实现自适应滤波。

2、由于gps高度定位误差太大，导致车辆无法进行准确的高度定位。传统的气压计可以一定程度上辅助车辆进行高度的定位，但影响气压的因素比较多包括：温度，湿度，海拔高度，空气密度等，气压计的使用情况容易受到天气情况的影响，非常不稳定，高度定位精度也不能达到高架桥层高级别。通过坐标变换的方法来解决车辆的高度定位导航问题，不仅能够在高度方面有很好的精度提升，对于车辆整体的三维定位精度也有相应提升，且不受外界环境的影响。

附图说明

图1是本发明的实现流程图；

图2是坐标s和坐标s1变换的示意图

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

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

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

图6是本发明车辆在z方向定位误差图；

图7是使用immukf-3d算法的车辆在z方向定位误差图；

图8是本发明与使用immukf-3d算法的车辆在z方向定位误差上的对比图；

图9是本发明与使用immukf-3d算法的车辆在整体定位误差上的对比图。

具体实施方式

下面结合具体实施例对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

下面结合附图和实施例对本发明专利进一步说明。

一种基于坐标变换的三维车载定位导航方法，如图1所示，具体包含如下步骤：

步骤1，建立m个非线性运动模型j，模型j在k+1时刻的表达式为xk是系统的状态向量，f(·)是非线性的过程函数，是模型j在k时刻时均值为零的白噪声序列，是模型j在k时刻的概率，运动模型之间通过马尔可夫链来控制转移；

步骤2，建立无迹卡尔曼滤波的非线性观测方程zk＝h(xk,vk)，zk是观测向量，h(·)是观测的非线性函数，vk为系统观测噪声序列，观测向量维数必须小于或等于状态向量的维数。

步骤3，模型i到模型j的转移概率为pij，模型j的预测概率即归一化常数为计算在k-1时刻模型i到模型j的混合概率μij[(k-1)/(k-1)]，计算模型j在k-1时刻的混合状态估计和混合协方差估计pj[(k-1)/(k-1)]，将计算后得到的混合估计作为循环的初始状态；

步骤4，将模型j在k-1时刻的混合状态估计经过u变换计算出sigma点值及其相应的一阶权值和二阶权值其中i表示sigma点的序号；sigma点值的具体计算方法如下：

式中，n为状态向量维数，pj[(k-1)/(k-1)]为混合协方差估计，λ为缩放比例参数，其中i表示sigma点的序号；

步骤5，将sigma点值通过模型j的状态方程，计算出状态的一步预测sigma点值用状态的一步预测sigma点值计算出状态的一步预测值和一步预测协方差矩阵其中，上标x表示此矩阵是状态值的协方差矩阵，一步预测值为混合状态估计在k时刻的值；

计算状态的一步预测值和一步预测协方差矩阵的具体方法如下：

其中，为步骤4中的sigma通过模型j的状态方程后的一步预测sigma点值。

步骤6，将步骤5中的一步预测sigma点值通过系统观测方程，求出观测值的sigma点值计算观测预测值为zj[(k)/(k-1)]、观测协方差矩阵为及状态观测互协方差矩阵为其中上标z表示此矩阵是观测值的协方差矩阵，上标xz表示此矩阵是状态值和观测值的互协方差矩阵；

计算观测预测值为zj[(k)/(k-1)]、观测协方差矩阵为及状态观测互协方差矩阵为的具体方法如下：

其中，qk-1为观测噪声协方差矩阵，观测值的sigma点值。

步骤7，测量更新，计算增益阵kj，计算模型j在k时刻的状态估计值以及误差协方差矩阵

步骤8，计算更新后的概率模型j的似然函数λj(k)，加入新的观测信息z(k)后，更新当前模型概率；

步骤9，输出交互，各个模型并行进行以上步骤，更新各自模型概率后计算总的状态估计和总的协方差估计p^x[(k)/(k)]；计算总的状态估计和总的协方差估计p^x[(k)/(k)]的具体方法如下：

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

步骤10，判断车辆是否处于坡面，若否，则返回步骤3继续执行，反之则建立平面坐标系s和坡面坐标系s1，在坡面坐标系s1中取坡面法向量为z1轴，取坐标系s水平旋转方向为x1轴，坡面仰角方向为y1轴，根据两个坐标系变化前后的关系，构建变化前后车辆三维位置坐标之间的方程式，由此计算出车辆在坡面的高度信息。之后再返回步骤3继续循环执行。计算车辆三维位置坐标之间方程式的具体方法如下：

其中，为坐标系s变换的方向角，θ为坐标系s变换的仰角。ex1、ey1、ez1是坐标系s1的坐标轴单位向量，ex、ey、ez是坐标系s的坐标轴单位向量。

实施例

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

本发明设置的状态向量包括车辆的位移、速度和加速度信息，维数为6维，观测向量包括里程和位移，维数为3维。仿真1中车辆在三维空间内做s型运动，方向角45度，坡度45度，初始位置位于(100,500,0)坐标处，初始速度为(0,10,0)，经过直线匀速运动250滤波点之后，开始上坡进行变速s型运动。采样周期为0.1秒。坐标s和坐标s1变换的方法如图2所示。车辆三维运动轨迹估计图如图3所示。测试车辆运动状态突然变化时的速度跟踪情况，实验结果如图4和图5所示，由图可知，在车辆运动状态发生突然变化的时刻，本发明提出的滤波算法也能使估计速度很好地跟踪上标准速度。测量车辆在z方向的定位误差，实验结果如图6所示，由于前250滤波点时还在平地，坐标还未变换，所以无高度误差，之后的高度误差平均在2米左右，与图7相比定位误差明显减小。测量车辆使用immukf-3d算法在z方向的定位误差，实验结果如图7所示，高度误差平均在4米左右，在此误差范围内，车辆在高架等复杂路况处容易出现定位不准的现象。测量本发明与使用immukf-3d算法的车辆在z方向定位误差上的对比，实验数据单位为米(m)，实验结果如图8所,示，与immukf-3d方法相比，本发明方法在z方向定位误差上更小。测量本发明与使用immukf-3d算法的车辆在整体定位误差上的对比，实验数据单位为米(m)，实验结果如图9所示，与immukf-3d方法相比，本发明方法在整体定位误差上更小。实验对比了该方法与immukf-3d方法的性能。所述方法性能提升较大。

本发明引入交互多模型使用不同的运动模型对应不同的运动状态过程，融合传统的无迹卡尔曼滤波，解决车载运动状态突然变化时定位效果不佳的问题，能够实现自适应滤波。通过坐标变换的方法来解决车辆的高度定位导航问题，不仅能够在高度方面有很好的精度提升，对于车辆整体的三维定位精度也有相应提升，且不受外界环境的影响。

以上已以较佳实施例公布了本发明，然其并非用以限制本发明，凡采取等同替换或等效变换的方案所获得的技术方案，均落在本发明的保护范围内。