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

摘要

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

主权项

1.一种基于坐标变换的三维车载定位导航方法，其特征在于包括以下步骤：步骤1，建立m个非线性运动模型j，模型j在k+1时刻的表达式为是模型j在k时刻时均值为零的白噪声序列，是模型j在k时刻的概率，运动模型之间通过马尔可夫链来控制转移；步骤2，建立无迹卡尔曼滤波的非线性观测方程Zk＝h(Xk,Vk)，Vk为系统观测噪声序列；步骤3，模型i到模型j的转移概率为p_ij，计算在k‑1时刻模型i到模型j的混合概率μ_ij[(k‑1)/(k‑1)]，计算模型j在k‑1时刻的混合状态估计和混合协方差估计P_j[(k‑1)/(k‑1)]，将计算后得到的混合估计作为循环的初始状态；步骤4，将模型j在k‑1时刻的混合状态估计经过U变换计算出sigma点值及其相应的一阶权值和二阶权值其中i表示sigma点的序号；步骤5，将sigma点值通过模型j的状态方程，计算出状态的一步预测sigma点值用状态的一步预测sigma点值计算出状态的一步预测值和一步预测协方差矩阵其中，上标x表示此矩阵是状态值的协方差矩阵，一步预测值为混合状态估计在k时刻的值；步骤6，将步骤5中的一步预测sigma点值通过系统观测方程，求出观测值的sigma点值计算观测预测值为Z_j[(k)/(k‑1)]、观测协方差矩阵为及状态观测互协方差矩阵为其中上标z表示此矩阵是观测值的协方差矩阵，上标xz表示此矩阵是状态值和观测值的互协方差矩阵；步骤7，测量更新，计算增益阵K_j，计算模型j在k时刻的状态估计值以及误差协方差矩阵步骤8，计算更新后的概率模型j的似然函数Λj(k)，加入新的观测信息Z(k)后，更新当前模型概率；步骤9，输出交互，各个模型并行进行以上步骤，更新各自模型概率后计算总的状态估计和总的协方差估计P^X[(k)/(k)]；步骤10，判断车辆是否处于坡面，若否，则返回步骤3继续执行，反之则建立平面坐标系S和坡面坐标系S1，根据两个坐标系变化前后的关系，构建变化前后车辆三维位置坐标之间的方程式，由此计算出车辆在坡面的高度信息，之后再返回步骤3继续循环执行。