[发明专利]一种基于坐标变换的三维车载定位导航方法

说明书

本发明公开了一种基于坐标变换的三维车载定位导航方法，该方法首先建立多个非线性运动模型；接着对车辆的各个运动模型概率进行混合估计，各个运动模型分别计算自己的滤波初始值；然后融合无迹卡尔曼滤波，加入GPS观测值，并行计算各模型的滤波输出，在滤波结束后更新各模型的概率，找到最符合当前车辆运动的模型进行输出交互，得到总的状态估计和总的协方差估计；最后当车辆是在坡面状态时，利用坐标变换的方法建立平面和坡面两个不同的坐标系，得出车辆位置三维坐标变化前后的关系，算出其高度信息。本发明确实有效地提高了车辆在高度方向上的定位精度以及整体的定位精度，具有较强的实用价值。

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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