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

摘要

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

说明书

技术领域：

本发明提出了一种新的道路地图的更新方法，更具体的说是一种通过对状态空间的模型估计提高道路 地图准确性的方法，属于信息工程领域。

背景技术：

近年来，随着城市化的发展，越来越多的人通过导航软件来寻找快捷方便的出行路线。在导航软件中， 交通地图的及时更新对导航软件的使用效果起到关键作用。目前，我国很多城市都在进行道路重建，随之 会带来道路形状的变化、道路消失和新道路的产生等情况。对于这些情况，国家权威机构至少需要半年至 一年的时间来完成测绘与确定，在此之后，导航软件才会更新交通地图。因此，面对城市中大量的道路建 设，如何及时更新交通地图是个需要迫切解决的问题。

本发明所采用的数据是由车辆反馈的GPS经纬度信息。当车辆行驶在高楼林立的市区或隧道或立交桥 下时，GPS的定位效果较差甚至是无法定位，为此，研究学者们利用车辆的行驶方向和距离信息来推算车 辆的瞬时位置，将GPS定位和航位推算(Dead—Reckoning，DR)结合，确保车辆定位系统的精度，但是 定位传感器也存在着测量误差，GPS/DR组合定位还是无法提供足够高的定位精度。这就需要借助地图匹配 定位修正方法来提高定位精度，目前常用的地图匹配算法有：基于代价函数的地图匹配算法、基于D—S 证据推理的地图匹配算法、基于模糊逻辑的地图匹配算法等，这些算法在减少系统定位误差方面各有优缺 点。传统的方法对误差处理不足，而本发明提出的方法能够对状态空间中的观测噪声和传递噪声进行更精 确的估计，进一步提高GPS的定位精度，保证道路匹配的准确度。

发明内容：

基于GPS信息的道路地图更新方法包括：对地图上已经标示的道路建立状态空间模型，模型中的未知 参数包括传递矩阵、观测矩阵、传递噪声及其观测噪声的方差，初始状态的均值及其方差，对模型中的所 有未知参数进行初始化，利用卡尔曼滤波算法求出各个时刻对应的状态的滤波值、预测值及其对应的方差 的滤波值、预测值，再利用各个时刻的状态、方差的预测值和卡尔曼平滑算法求出各个时刻对应的平滑值， 然后计算状态变量和观测变量的对数似然函数及其关于在给定参数和观测变量下的状态变量的密度函数 的期望，对该期望关于参数求偏导，令其为0，求出使其期望达到最大的参数，更新参数直至收敛，这样 就可得到观测矩阵、传递矩阵、观测噪声的方差、传递噪声的方差、初始状态的均值及其方差的估计，再 将估计出的参数代入状态空间模型中，利用卡尔曼滤波算法求出一步预测值，作为与之连接的未标识道路 的初始值，最后利用得到的状态空间模型和卡尔曼滤波算法对未标识的道路进行预测及修正，及时更新道 路地图信息。

该发明的优点是所使用的信息是时域上的量，可对非平稳的随机过程进行处理，将信号看成是白噪声 作用下的一个线性系统的输出，用状态方程来描述输入与输出的关系，然后从期望最大化的角度估计出状 态方程中的观测矩阵、传递矩阵、观测噪声的方差、传递噪声的方差、初始状态的均值及其方差，采用的 迭代算法能实时、快速地计算道路的经纬度。

附图说明：

图1是该方法的流程图

具体实施方式：

实例1，本发明包含如下步骤：

步骤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}^{-5}& 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),$${\theta }^{\mathrm{old}}=\{F^{\mathrm{old}},H^{\mathrm{old}},R^{\mathrm{old}},x_{0|0}^{\mathrm{old}},P_{0|0}^{\mathrm{old}}\},$在以下的步骤中，涉及参数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为状态 变量的维度，公式中涉及的撇号表示矩阵的转置，根据步骤1中的模型假设，求出完全数据(x_0:T,y_1:T)的 对数似然函数的表达式

$\left(\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-{\mathrm{Hx}}_t)-\frac{(n+m)T+m}{2}\ln \left(2\pi \right)\end{array}\right)$

步骤3：利用θ^old中的F，Q，x_0|0和P_0|0的值计算状态空间中的一步预测值x_1|0和P_1|0，具体实施公 式如下

x_1|0＝Fx_0|0

P_1|0＝FP_0|0F′+Q

步骤4：利用卡尔曼滤波算法和步骤3中得到的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，具体的迭代 计算步骤为：

(1)令t＝1；

(2)利用t时刻的观测值y_t和状态的一步预测值x_t|t-1，计算

(3)利用t时刻状态的方差的一步预测值P_t|t-1，计算D_t＝HP_t|t-1H′+R；

(4)利用x_t|t-1、P_t|t-1和(2)、(3)步中得到的D_t，计算该时刻状态的滤波值

(5)利用P_t|t-1和(2)、(3)步中得到的D_t，计算该时刻方差的滤波值

(6)利用P_t|t-1和(3)中得到的D_t，计算K_t＝FP_t|t-1H′D_t^-1

(7)利用x_t|t-1和(2)、(6)中得到的K_t，计算下一时刻的预测值

(8)利用P_t|t-1和(3)、(6)中得到的D_t、K_t，计算下一时刻方差的预测值P_t+1|t＝FP_t|t-1F′+Q-K_tD_tK_t′

(9)若t＝T，迭代计算结束，否则，令t＝t+1，返回到(2)

步骤5：利用步骤4得到的各个时刻的预测值x_t+1|t、P_t+1|t和卡尔曼平滑算法求出各个时刻对应的状态 平滑值x_t|T和方差平滑值P_t|T，其中t＝0,1,2,3,…,T，具体的迭代计算步骤为：

(1)令t＝T，且初始化u_t＝U_t＝0；

(2)利用t时刻的观测值y_t和步骤4得到的状态预测值x_t|t-1，计算

(3)利用步骤4得到的t时刻方差的预测值P_t|t-1，计算D_t＝HP_t|t-1H′+R；

(4)利用步骤4得到的P_t|t-1和(3)步中得到的D_t，计算K_t＝FP_t|t-1H′D_t^-1

(5)利用(4)得到的K_t，计算L_t＝F-K_tH

(6)利用(2)、(3)、(5)中得到的D_t和L_t，计算

(7)利用(3)、(5)中得到的D_t、L_t，计算

(8)利用步骤4中得到的x_t|t-1、P_t|t-1和(6)中得到的u_t-1，计算t时刻的状态平滑值x_t|T＝x_t|t-1+P_t|t-1u_t-1

(9)利用步骤4中得到的P_t|t-1和(7)中得到的U_t-1，计算t时刻的方差平滑值P_t|T＝P_t|t-1-P_t|t-1U_t-1P_t|t-1

(10)若t＝1，迭代计算结束，否则，令t＝t-1，返回到(2)

(11)令t＝0，利用步骤3所得的x_1|0，P_1|0和(8)、(9)所得的x_1|T、P_1|T，计算$P_0^{*}=P_{0|0}{F}^{\prime }{P}_{1|0}^{-1},$$P_{0|T}=P_{0|0}+P_0^{*}(P_{1|T}-P_{1|0}){P_0^{*}}^{\prime },T_{0|T}=x_{0|0}+P_0^{*}(x_{1|T}-x_{1|0})$

步骤6：利用步骤4得到的方差的滤波值P_t|t和预测值P_t+1|t，计算P_t,t-1|T，t＝1,2,3,…,T具体的迭代 计算步骤为：

(1)令t＝T，计算P_T,T-1|T＝[I-P_T|T-1H′(HP_T|T-1H′+R)^-1H]FP_T-1|T-1

(2)令t＝t-1，计算$P_t^{*}=P_{t|t}{F}^{\prime }{P}_{t+1|t}^{-1},P_{t,t-1|T}=P_{t|t}{P_{t-1}^{*}}^{\prime }{P}_t^{*}(P_{t+1,t|T}-{\mathrm{FP}}_{t|t}){P_{t-1}^{*}}^{\prime },$循环执行(2)，直至t＝1

步骤7：求出步骤2中对数似然函数lnp(x_0:T,y_1:T|θ)关于p(x_0:T|y_1:T,θ^old)的期望

$\left(\begin{array}{c}E\left[\ln p(x_{0:T},y_{1:T}|\theta )\right]=-\frac{1}{2}\ln \left|P_{0|0}\right|-\frac{T}{2}\ln \left|Q\right|-\frac{T}{2}\ln \left|R\right|-\frac{(n+m)T+m}{2}\ln \left(2\pi \right)\\ -\frac{1}{2}\mathrm{tr}\left(P_{0|0}^{-1}E\right[{(x_{0|T}-x_{0|0})(x_{0|T}-x_{0|0})}^{\prime }\left]\right)\\ -\frac{1}{2}\underset{t=1}{\overset{T}{\Sigma }}\mathrm{tr}\left(Q^{-1}E\right[(x_t-F_t{x}_{t-1}){(x_t-F_t{x}_{t-1})}^{\prime }\left]\right)\\ -\frac{1}{2}\underset{t=1}{\overset{T}{\Sigma }}\mathrm{tr}\left(R^{-1}E\right[(y_t-{\mathrm{Hx}}_{t|T}){(y_t-{\mathrm{Hx}}_{t|T})}^{\prime }\left]\right)\end{array}\right)$

记${\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，解方程组后得到参数的更新结果如下：

解得：

$\frac{\partial }{\partial H}E\left[\ln p(x_{0:T},y_{1:T}|\theta )\right]=-\frac{1}{2}{R}^{-1}\underset{t=1}{\overset{T}{\Sigma }}[-2(y_t-H_t{x}_{t|T}){x_{t|T}}^{\prime }+{2\mathrm{HP}}_{t|T}]=0$

解得：$H=\left(\underset{t=1}{\overset{T}{\Sigma }}{y}_t{x_{t|T}}^{\prime }\right){\Gamma }^{-1}$

$\frac{\partial }{{\partial R}^{-1}}E\left[\ln p(x_{0:T},y_{1:T}|\theta )\right]=\frac{T}{2}R-\frac{1}{2}\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 })=0$

解得：$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 })$

解得：

$\frac{\partial }{{\partial x}_{0|0}}E\left[\ln p(x_{0:T},y_{1:T})\right|\theta ]=P_{0|0}^{-1}(x_{0|T}-x_{0|0})=0$

解得：x_0|0＝x_0|T

$\frac{\partial }{{\partial P}_{0|0}^{-1}}\left[\ln p(x_{0:T},y_{1:T}|\theta )\right]=\frac{P_{0|0}}{2}-\frac{1}{2}\left((x_{0|T}-x_{0|0}){(x_{0|T}-x_{0|0})}^{\prime }{+P}_{0|T}\right)=0$

解得：P_0|0＝P_0|T

则记更新后的参数为θ^new＝{F,Q,R,x_0|0,P_0|0}；

步骤9：检验参数是否收敛。具体实施方式是设定一个阈值，比如10^-6,检验|θ^new-θ^old|是否小于该阈 值，如果小于该阈值，进入步骤10，否则，令θ^old＝θ^new，返回到步骤3；

步骤10：得到相应的参数估计θ^new后，更新状态空间的初始值x_1|0和P_1|0，具体实施如下

x_1|0＝Fx_0|0

P_1|0＝FP_0|0F′+Q

步骤11：利用所得的状态空间模型和卡尔曼滤波算法对新的道路信息进行预测和修正，初始值取为原 有道路信息的一步预测值。具体实施方式为利用θ^new，执行步骤4，利用步骤4所得的x_T+1|T和P_T+1|T作为 新的道路的初始值，再将θ^new代入步骤1中建立的状态空间模型，作为与该已知道路相连接的新的道路的 状态空间模型，在实际应用时按相同的时间间隔读取新的道路的经纬度信息，利用卡尔曼滤波对读取到的 经纬度信息进行预测与修正，迭代计算公式与步骤4中的公式相同。