基于GPS信息的道路地图更新方法

摘要

本发明属于信息科学工程领域，特别是涉及一种基于GPS信息的道路预测与修正方法研究。该方法是将已有道路看做一个状态空间，利用EM算法和Kalman算法对其状态空间模型中的未知参数进行估计，最后利用估计出的状态空间模型和Kalman滤波算法对新修道路进行预测与修正。本发明首次将这一方法运用到道路预测与修正这一研究方向，解决了地图更新速度较慢的问题，该算法具有及时有效的优点。

主权项

一种基于GPS信息的道路地图的更新方法，其特征在于它包括如下步骤：步骤1：对于地图上已标示道路的经纬度信息样本总量为T，样本(即观测变量)记为y_t，t＝1,2,3,…,T，设初始状态x₀服从均值为x_0|0、方差为P_0|0的高斯分布，传递噪声η_t独立同分布，服从均值为0、方差为Q的高斯分布，观测噪声ε_t独立同分布，服从均值为0、方差为R的高斯分布，且初始状态x₀、传递噪声η_t和观测噪声ε_t相互独立，F为传递矩阵，H为观测矩阵，a_t,b_t分别表示道路的经纬度信息，状态变量$x_t=\left[\begin{array}{c}{a}_t\\ b_t\end{array}\right],$其中未知参数记为θ＝{F,H,Q,R,x_0|0,P_0|0}，将其初始化，记$F^{\mathrm{old}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$$H^{\mathrm{old}}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$$Q^{\mathrm{old}}=\left[\begin{array}{cc}0.1& 0\\ 0& 0.1\end{array}\right],$$R^{\mathrm{old}}=\left[\begin{array}{cc}10^{-2}& 0\\ 0& 10^{-5}\end{array}\right],$$x_{0|0}^{\mathrm{old}}=\left[\begin{array}{c}0\\ 0\end{array}\right],$$P_{0|0}^{\mathrm{old}}=\left[\begin{array}{cc}0.1& 0\\ 0& 0.1\end{array}\right],$在以下的步骤中，涉及参数F,H,Q,R,x_0|0,P_0|0的计算时均使用θ^old中的参数的对应值，对已知的道路信息进行状态空间建模$\left\{\begin{array}{c}{x}_t={\mathrm{Fx}}_{t-1}+{\eta }_t\\ y_t={\mathrm{Hx}}_t+{ϵ}_t\end{array}\right.;$步骤2：记x_0:T＝{x₀,x₁,…,x_T}，y_1:T＝{y₁,y₂,…,y_T}，n＝2为观测变量的维度，m＝2为状态变量的维度，公式中涉及的撇号表示矩阵的装置，根据模型假设，求出完全数据(x_0:T,y_1:T)的对数似然函数的表达式$\begin{array}{c}\ln p(x_{0:T},y_{1:T}|\theta )=-\frac{1}{2}\ln \left|P_{0|0}\right|-\frac{1}{2}{(x_0-x_{0|0})}^{\prime }{P}_{0|0}^{-1}(x_0-x_{0|0})-\frac{T}{2}\ln \left|Q\right|\\ -\frac{1}{2}\underset{t=1}{\overset{T}{\Sigma }}{(x_t-{\mathrm{Fx}}_{t-1})}^{\prime }{Q}^{-1}(x_t-{\mathrm{Fx}}_{t-1})-\frac{T}{2}\ln \left|R\right|\\ -\frac{1}{2}\underset{t=1}{\overset{T}{\Sigma }}{(y_t-{\mathrm{Hx}}_t)}^{\prime }{R}^{-1}(y_t-H_{\mathrm{xt}})-\frac{(x+m)T+m}{2}\ln \left(2\pi \right)\end{array};$步骤3：利用θ^old中的F，Q，x_0|0和P_0|0的值计算状态空间中的一步预测值x_1|0和P_1|0，具体实施公式如下x_1|0＝Fx_0|0P_1|0＝FP_0|0F′+Q；步骤4：利用卡尔曼滤波算法和x_1|0和P_1|0求出各个时刻对应的状态滤波值x_t|t、状态方差滤波值P_t|t、状态的一步预测值x_t+1|t和状态方差的一步预测值P_t+1|t，其中t＝1,2,3,…,T；步骤5：利用步骤4得到的各个时刻的预测值x_t+1|t、P_t+1|t和卡尔曼平滑算法求出各个时刻对应的状态平滑值x_t|T和方差平滑值P_t|T，其中t＝0,1,2,3,…,T；步骤6：利用t时刻方差的滤波值P_t|t和t+1时刻方差的预测值P_t+1|t，计算P_t,t‑1|T；步骤7：求出步骤2中对数似然函数lnp(x_0:T,y_1:T|θ)关于p(x_0:T|y_1:T,θ^old)的期望其中，${\Gamma }_t=\underset{t=1}{\overset{T}{\Sigma }}(x_{t|T}{x_{t|T}}^{\prime }+P_{t|T}),$步骤8：对E[lnp(x_0:T,y_1:T|θ)]关于参数求最大值，具体实施方式为对E[lnp(x_0:T,y_1:T|θ)]关于参数F,H,R^‑1,Q^‑1，x_0|0,求偏导并令其为0，解方程组后得到参数的更新结果如下：$H=\left(\underset{t=1}{\overset{T}{\Sigma }}{y}_t{x_{t|T}}^{\prime }\right){\Gamma }^{-1},$$R=\frac{1}{T}\underset{t=1}{\overset{T}{\Sigma }}((y_t-{\mathrm{Hx}}_{t|T}){(y_t-{\mathrm{Hx}}_{t|T})}^{\prime }+{\mathrm{HP}}_{t|T}{H}^{\prime }),$x_0|0＝x_0|T，P_0|0＝P_0|T，记更新后的参数为θ^new＝{F,Q,R,x_0|0,P_0|0}；步骤9：检验参数是否收敛。具体实施方式是设定一个阈值，比如10^‑6,检验|θ^new‑θ^old|是否小于该阈值，如果小于该阈值，进入下一步骤，否则，令θ^old＝θ^new，返回到步骤3；步骤10：得到相应的参数估计θ^new后，更新状态空间的初始值x_1|0和P_1|0，具体实施如下x_1|0＝Fx_0|0P_1|0＝FP_0|0F′+Q；步骤11：利用所得的状态空间模型和卡尔曼滤波算法对新的道路信息进行预测和修正，初始值取为原有道路信息的一步预测值。具体实施方式为利用θ^new，重新计算x_T+1|T和P_T+1|T，利用所得的x_T+1|T和P_T+1|T作为新的道路的初始值，再将θ^new代入建立的状态空间模型，作为与该已知道路相连接的新的道路的状态空间模型，在实际应用时按相同的时间间隔读取新的道路的经纬度信息，利用卡尔曼滤波对读取到的经纬度信息进行预测与修正。