[发明专利]基于GPS信息的道路地图更新方法

摘要

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

主权项

1.一种基于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分别表示道路的经纬度信息，状态变量其中未知参数记为θ＝{F,H,Q,R,x_0|0,P_0|0}，将其初始化，记在以下的步骤中，涉及参数F,H,Q,R,x_0|0,P_0|0的计算时初始状态使用θ^old中的参数的对应值，对已知的道路信息进行状态空间建模步骤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)的对数似然函数的表达式：步骤3：利用θ^old中的F，Q，x_0|0和P_0|0的值计算状态空间中的一步预测值x_1|0和P_1|0，具体实施公式如下：步骤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；具体的迭代计算步骤为：(401)令t＝1；(402)利用t时刻的观测值y_t和状态的一步预测值x_t|t‑1，计算(403)利用t时刻状态的方差的一步预测值P_t|t‑1，计算D_t＝HP_t|t‑1H′+R；(404)利用x_t|t‑1、P_t|t‑1和(402)、(403)步中得到的D_t，计算该时刻状态的滤波值(405)利用P_t|t‑1和(402)、(403)步中得到的D_t，计算该时刻方差的滤波值(406)利用P_t|t‑1和(403)中得到的D_t，计算K_t＝FP_t|t‑1H′D_t^‑1(407)利用x_t|t‑1和(402)、(406)中得到的K_t，计算下一时刻的预测值(408)利用P_t|t‑1和(403)、(406)中得到的D_t、K_t，计算下一时刻方差的预测值P_t+1|t＝FP_t|t‑1F′+Q‑K_tD_tK_t′(409)若t＝T，迭代计算结束，否则，令t＝t+1，返回到(402)；步骤5：利用步骤4得到的各个时刻的预测值x_t+1|t、P_t+1|t和卡尔曼平滑算法求出各个时刻对应的状态平滑值x_t|T和方差平滑值P_t|T，其中t＝0,1,2,3,…,T；具体的迭代计算步骤为：(501)令t＝T，且初始化u_t＝U_t＝0；(502)利用t时刻的观测值y_t和状态预测值x_t|t‑1，计算(503)利用t时刻方差的预测值P_t|t‑1，计算D_t＝HP_t|t‑1H′+R；(504)利用t时刻方差的预测值P_t|t‑1和(503)步中得到的D_t，计算K_t＝FP_t|t‑1H′D_t^‑1；(505)利用(504)得到的K_t，计算L_t＝F‑K_tH；(506)利用(502)、(503)、(505)中得到的D_t和L_t，计算(507)利用(503)、(505)中得到的D_t、L_t，计算(508)利用t时刻状态和方差的预测值x_t|t‑1、P_t|t‑1和(506)中得到的u_t‑1，计算t时刻的状态平滑值x_t|T＝x_t|t‑1+P_t|t‑1u_t‑1；(509)利用t时刻方差的预测值P_t|t‑1和(507)中得到的U_t‑1，计算t时刻的方差平滑值P_t|T＝P_t|t‑1‑P_t|t‑1U_t‑1P_t|t‑1；(510)若t＝1，迭代计算结束，否则，令t＝t‑1，返回到(502)；(511)令t＝0，利用x_1|0，P_1|0和(508)、(509)所得的x_1|T、P_1|T，计算步骤6：利用t时刻方差的滤波值P_t|t和t+1时刻方差的预测值P_t+1|t，计算P_t,t‑1|T；具体的迭代计算步骤为：(601)令t＝T，计算P_T,T‑1|T＝[I‑P_T|T‑1H′(HP_T|T‑1H′+R)^‑1H]FP_T‑1|T‑1；(602)令t＝t‑1，计算(603)循环执行(602)，直至t＝1；步骤7：求出步骤2中对数似然函数lnp(x_0:T,y_1:T|θ)关于p(x_0:T|y_1:T,θ^old)的期望其中，步骤8：对E[ln p(x_0:T,y_1:T|θ)]关于参数求最大值，具体实施方式为对E[ln p(x_0:T,y_1:T|θ)]关于参数F,H,R^‑1,Q^‑1，x_0|0,求偏导并令其为0，解方程组后得到参数的更新结果如下：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，具体实施如下步骤11：利用所得的状态空间模型和卡尔曼滤波算法对新的道路信息进行预测和修正，初始值取为原有道路信息的一步预测值；具体实施方式为利用θ^new，重新计算x_T+1|T和P_T+1|T，利用所得的x_T+1|T和P_T+1|T作为新的道路的初始值，再将θ^new代入建立的状态空间模型，作为与该已知道路相连接的新的道路的状态空间模型，在实际应用时按相同的时间间隔读取新的道路的经纬度信息，利用卡尔曼滤波对读取到的经纬度信息进行预测与修正。