CN105427739A - 一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法 - Google Patents

Abstract

本发明公开了一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法，本方法通过多传感器信息采集车辆，采集选定道路的位置信息、车辆信息；然后对采集到的道路位置信息进行初步处理，将经纬度坐标转化为平面坐标，并将转化后平面坐标表示的位置作为道路的节点；通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出每个节点位置道路的纵向坡度；最后将道路的节点位置信息以及纵向坡度信息，利用数字地图制作软件制作成增强型数字地图。

Description

一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法

技术领域

本发明属于地理信息系统领域，涉及一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法。

背景技术

数字地图是通过数字化的方法，把城市地理信息以一定的格式存储起来，并能以连续地图的形式呈现出来，本质上是一个城市地理数据库。数字地图可提供丰富的基于位置的服务，为人们生活带来了极大的便利。然而目前的数字地图存在以下问题：数字地图精度较低，无法通过已知信息进一步计算，获得精度较高的道路纵向坡度等信息；地图信息匮乏，一般仅包含位置信息，而对于一些人们关注的重要信息，例如道路的航向、曲率、坡度、附近楼层的高度等信息并没有包含；数字地图目前制作复杂、专业性要求很高，需要掌握空间变换与几何变换算法、矢量与栅格数据模型等；除了对专业性有很高的要求，传统数字地图的制作过程中很多测绘工作需要由人工来完成，工作量大且效率不高，制作成本相对较高。

针对上述问题，近几年国内外开始关注增强型数字矢量地图，其不仅包含了普通地图的位置信息，还包含了一些特有的信息，例如道路的航向、坡度、路边房屋的高度等，增强型数字矢量地图相对于普通数字地图具有更加广泛的运用前景。本发明提出的基于卡尔曼滤波的道路坡度增强型数字地图的制作方法就是一种增强型的数字矢量地图的制作方法。目前的坡度测量仪、全站仪、水平仪等传统道路纵向坡度测量设备，当需要测量道路里程较长时，往往因为测量工作量太大、数据采集方式复杂，难以满足大规模应用的需求。因此本发明使用了多传感器信息采集车辆进行道路及车辆状态信息的采集，简单易行、效率高。本发明所制作的基于卡尔曼滤波的道路坡度增强型数字地图，具体包含了道路的纵向坡度信息，在车辆主动安全等领域有着广泛的应用。例如，近几年客车在山区道路翻车事故频繁发生，造成巨大的人员财产损失，司机对当前路况判断失误以及缺乏对复杂环境中路况的提前预警是事故发生的主要原因。道路纵向坡度是一种非常重要的道路信息，道路纵向坡度对于车辆档位控制、速度控制有着重要的参考价值，司机提前获得准确的道路纵向坡度信息可以有效避免车辆出现翻车等一系列事故。因此制作基于卡尔曼滤波的道路坡度增强型数字地图，有着重要的现实意义。

发明内容

本发明提出了一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法，已解决现有数字地图的制作工作量大、过程复杂、精度不够高、缺乏道路坡度信息(道路坡度具体指道路纵向坡度，其在车辆主动安全领域有着广泛的应用)的问题。

本发明提出一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法。首先选定道路，确定道路起始点与终点；通过多传感器信息采集车辆，采集选定道路的位置信息、车辆的状态信息；然后对采集到的道路位置信息进行初步处理，将经纬度坐标转化为平面坐标，并将转化后平面坐标表示的位置作为道路的节点；通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出每个节点位置道路的纵向坡度；最后将道路的节点位置信息以及纵向坡度信息，利用数字地图制作软件制作成基于卡尔曼滤波的道路坡度增强型数字矢量地图。

具体实施步骤包括：

步骤一、首先选定道路；

首先将需要测绘的道路进行分割，选定需要进行量测的部分，确定需要测绘部分的起始点与终点。本专利所适用的道路为高速公路以及一级、二级公路，且所适用的道路光滑连续，不包含交叉路口。鉴于步骤二所采用的高斯-克吕格投影在局部范围精度较高，且随着范围增大误差也随之增大，因此选择的道路长度应不超过5km。

步骤二、通过多传感器信息采集车辆，采集选定道路位置以及车辆状态等信息；

本方法采用了多传感器信息采集车辆，其搭载的卫星定位系统可以实时输出道路位置R_i(L_iB_i)、车辆的垂直速度V_Z,i、车辆的水平速度V_XY,i以及卫星定位系统接收到的卫星数N_sat,i，其中L_i、B_i分别表示经度、纬度；纵向加速度传感器输出车辆纵向加速度信息A_i，其中i表示开始采集后接收到的信息的序号，i＝1,2,3….。通过同时开始采集多种传感器信息并统一各个传感器信息输出频率(输出频率均为20Hz)，保证采集序号相同的信息一一对应。同一时刻所采集的信息有：车辆所在位置的经纬度信息R_i(L_iB_i)、车辆的垂直速度V_Z,i、车辆的水平速度V_XY,i、卫星定位系统接收到的卫星数N_sat,i以及车辆纵向的加速度A_i。车辆在信息采集的过程中需要保持车辆平稳运行，以保证车身与地面尽量平行，减小在估计道路纵向坡度时因为车身倾斜而产生的误差。同时采集车辆的胎压需要保持一致，避免因车辆胎压不同导致的道路坡度估计误差。道路信息采集过程中为了保证采集的信息密度均匀，车速要保持匀速，且车速在55-65km/h范围以内，这样保证采集的道路位置信息的间距适中，所采集的位置信息间距在0.764-0.903m之间。由于道路中车道之间基本平行，所以本专利选取行进方向左侧车道来提取道路的纵向坡度信息，且采集过程中采集车辆沿车道中心行驶。

步骤三、将采集到经纬度坐标转化成平面坐标，并将转化后平面坐标表示的位置作为道路的节点；

由于制作地图需要平面直角坐标系坐标，本发明采用较为成熟的3度带高斯-克吕格投影方法，将经纬度坐标R_i(L_iB_i)投影为高斯平面直角坐标系坐标P_i(x_iy_i)，x_i为坐标转换后对应的平面直角坐标系的纵坐标(北向位置)，y_i为坐标转换后对应的平面直角坐标系的横坐标(东向位置)。根据起始点R₁(L₁B₁)选定R₀(L₀B₀)作为高斯-克吕格投影的原点，其中L₀＝3D，D为(L₁/3)四舍五入取整的值，B₀＝0°。经纬度坐标R_i(L_iB_i)转换公式如下所示：

$\left\{\begin{array}{c}{x}_i=X_{B_i}+\frac{1}{2}{\mathrm{Ntl}}^2{\cos }^2{B}_i+\frac{1}{24}Nt(5-t^2+9{\mathrm{\η}}^2+4{\mathrm{\η}}^4)l^4{B}_i\\ +\frac{1}{720}Nt(61-58t^2+t^4+270{\mathrm{\η}}^2-330{\mathrm{\η}}^2{t}^2)l^6{\cos }^6{B}_i+...\\ y_i=Nl\cos B_i+\frac{1}{6}N(1-t^2-{\mathrm{\η}}^2)l^3{\cos }^3{B}_i\\ +\frac{1}{120}N(5-18t^2+t^4+14{\mathrm{\η}}^4-58{\mathrm{\η}}^2{t}^2)l^5{\cos }^5{B}_i+...\end{array}\right.---\left(1\right)$

式(1)为高斯投影公式的泰勒级数展开式，式中省去了7次以上高次项，其中为赤道至纬度B_i的子午线弧长，且 $X_{B_i}=C_0{B}_i-\cos B_i(C_1\sin B_i+C_2{\sin }^3{B}_i+C_3{\sin }^5{B}_i+C_4{\sin }^7{B}_i),$ l为所求点的经度L_i与L₀之差，t＝tanB_i，η＝e′cosB_i，e′为椭球第二偏心率，N为通过所求点的卯酉圈曲率半径，C₀，C₁，C₂，C₃，C₄为与点位无关的系数，仅有椭球体长半轴、短半轴、第一偏心率等参数确定。目前矢量地图多采用点的连线来表示道路，所以本发明将转换后平面坐标P_i(x_iy_i)所表示的位置作为道路的节点N_i(x_iy_i)，通过节点的连线表示道路。

步骤四、通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出节点处的道路纵向坡度信息；

本发明提出了一种基于卡尔曼滤波的道路纵向坡度估计算法，该算法精度高、实时性好、适用范围广，能够实现复杂环境下的道路纵向坡度估计。

具体估计算法如下：

1、卡尔曼滤波观测量计算模型；

本发明提出了一种基于多传感器信息的卡尔曼滤波观测量计算模型。该模型采用高精度卫星定位系统和车辆纵向加速度传感器，可以得出比单一传感器精度更高、鲁棒性更好的道路纵向坡度观测值。本发明中坡度采用百分比法表示。

1)利用高精度卫星定位系统的数据解算出道路纵向坡度。

具体的解算方式：通过高精度卫星定位系统获取车辆的垂直速度V_Z,i和水平速度V_XY,i信息，然后根据公式

得出道路纵向坡度θ_1,i；

2)利用车辆纵向加速度传感器数据解算出道路纵向坡度。

具体的解算方式：根据采集的车辆纵向加速度A_i，考虑到车辆行驶在匀速状态，车辆的纵向加速度可以忽略不计，然后通过公式

${\mathrm{\θ}}_{2,i}=tan\left({\sin }^{-1}\right(\frac{A_i}{g}\left)\right)\×100\%---\left(3\right)$

得出道路纵向坡度θ_2,i其中g为重力加速度g＝9.8m/s²。

本算法依据卫星定位系统接收到的卫星数N_sat,i，将两种道路纵向坡度计算值的结果进行融合，得出精度更高、鲁棒性更好的道路纵向坡度观测值；最终的道路纵向坡度观测值θ_m(i)可由融合公式如下所示：

${\mathrm{\&theta;}}_m\left(i\right)=\left\{\begin{array}{cc}\frac{V_{Z,i}}{V_{XY,i}}\&times;100\%& N_{sat,i}\&GreaterEqual;6\\ tan\left({\sin }^{-1}\right(\frac{A_i}{g}\left)\right)\&times;100\%& N_{sat,i}<6\end{array}\right.---\left(4\right)$

2、建立卡尔曼滤波的状态方程和观测方程。

本算法的卡尔曼滤波的状态方程：X＝[θ]，X为系统状态向量，θ为道路的纵向坡度；W表示零均值的系统高斯白噪声向量且W＝[w₁]，其中w₁系统高斯白噪声分量；W对应的系统噪声协方差阵为Q，其中表示系统高斯白噪声w₁对应的方差；状态转移矩阵这是由于道路的坡度都是连续缓慢变化的，可以认为当前的坡度等于下一采样点的坡度。

观测方程：Z(i)＝H(i)X(i)+V(i)式中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。由于观测向量与状态向量都是道路纵向坡度，所以H＝[1]、Z＝[θ_m]、其中θ_m为根据卡尔曼滤波观测量计算模型得出的道路纵向坡度。表示道路纵向坡度的观测噪声且是均值为0、方差为的高斯白噪声，V对应的观测噪声方差阵R可表示为

对于上述所描述的系统状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程：

一步预测误差方差阵：

测量更新：

滤波增益矩阵：K(i)＝P(i,i-1)H′(i)[H(i)P(i,i-1)H′(i)+R(i)]^-1(7)

状态估计：

估计误差方差阵：P(i)＝[I-K(i)H(i)]P(i,i-1)(9)

其中为的转置、H′(i)为H(i)的转置。

经过上述递推计算后，即可估计出节点N_i(x_iy_i)位置的纵向坡度θ_i，其中

步骤五、将道路的节点位置信息N_i(x_iy_i)以及对应的道路纵向坡度信息θ_i通过数字地图制作软件制作成基于卡尔曼滤波的道路坡度增强型数字矢量地图。

根据获得到道路的节点位置信息N_i(x_iy_i)以及该节点的道路纵向坡度信息θ_i通过数字地图自作软件生成基于卡尔曼滤波的道路坡度增强型数字矢量地图。首先利用节点的连线来表示所选取的道路，然后通过增加节点的道路信息列表将对应节点的道路纵向坡度信息增加到地图上。

有益效果如下:

本方法采用的基于卡尔曼滤波的道路纵向坡度估计算法，估计出道路的纵向坡度，该算法结合了多种传感器的优点，避免了单一传感器的不足，精度高、鲁棒性好；通过信息采集车辆进行道路以及车辆状态信息的采集，不需要大量的人工测绘工作，实施方便；本发明所制作的数字矢量地图在原有位置信息的基础上增加了道路纵向坡度信息，在车辆主动安全领域有着广泛的应用。

附图说明

图1为本发明所提方法的流程框图；

图2为本发明采用的多传感器信息采集车辆示意图。

具体实施方式

数字地图是通过数字化的方法，把城市地理信息以一定的格式存储起来，并能以连续地图的形式呈现出来，本质上是一个城市地理数据库。数字地图可提供丰富的基于位置的服务，为人们生活带来了极大的便利。然而目前的数字地图存在以下问题：数字地图精度较低，无法通过已知信息进一步计算，获得精度较高的道路纵向坡度等信息；地图信息匮乏，一般仅包含位置信息，而对于一些人们关注的重要信息，例如道路的航向、曲率、坡度、附近楼层的高度等信息并没有包含；数字地图目前制作复杂、专业性要求很高，需要掌握空间变换与几何变换算法、矢量与栅格数据模型等；除了对专业性有很高的要求，传统数字地图的制作过程中很多测绘工作需要由人工来完成，工作量大且效率不高，制作成本相对较高。

针对上述问题，近几年国内外开始关注增强型数字矢量地图，其不仅包含了普通地图的位置信息，还包含了一些特有的信息，例如道路的航向、坡度、路边房屋的高度等，增强型数字矢量地图相对于普通数字地图具有更加广泛的运用前景。本发明提出的基于卡尔曼滤波的道路坡度增强型数字地图的制作方法就是一种增强型的数字矢量地图的制作方法。目前的坡度测量仪、全站仪、水平仪等传统道路纵向坡度测量设备，当需要测量道路里程较长时，往往因为测量工作量太大、数据采集方式复杂，难以满足大规模应用的需求。因此本发明使用了多传感器信息采集车辆进行道路及车辆状态信息的采集，简单易行、效率高。本发明所制作的基于卡尔曼滤波的道路坡度增强型数字地图，具体包含了道路的纵向坡度信息，在车辆主动安全等领域有着广泛的应用。例如，近几年客车在山区道路翻车事故频繁发生，造成巨大的人员财产损失，司机对当前路况判断失误以及缺乏对复杂环境中路况的提前预警是事故发生的主要原因。道路纵向坡度是一种非常重要的道路信息，道路纵向坡度对于车辆档位控制、速度控制有着重要的参考价值，司机提前获得准确的道路纵向坡度信息可以有效避免车辆出现翻车等一系列事故。因此制作基于卡尔曼滤波的道路坡度增强型数字地图，有着重要的现实意义。

本发明提出一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法。首先选定道路，确定道路起始点与终点；通过搭载了卫星定位系统和纵向加速度传感器的多传感器信息采集车辆，采集选定道路的位置信息、车辆的状态信息；然后对采集到的道路位置信息进行初步处理，将经纬度坐标转化为平面坐标，并将转化后平面坐标表示的位置作为道路的节点；通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出每个节点位置道路的纵向坡度；最后将道路的节点位置信息以及纵向坡度信息，利用数字地图制作软件制作成基于卡尔曼滤波的道路坡度增强型数字矢量地图。本方法采用基于卡尔曼滤波的道路纵向坡度估计算法，该算法结合了多种传感器的优点，避免了单一传感器的不足，精度高、鲁棒性好。通过信息采集车辆进行道路位置信息以及车辆状态信息的采集，不需要大量的人工测绘工作，实施方便。

实施方式中卫星定位系统为高精度(水平定位精度的圆概率误差[CEP]小于0.02m)、高频(输出频率大于等于20Hz)、多模(兼容全球定位系统、北斗导航系统)的卫星定位系统，系统可以输出位置信息，水平速度和垂直速度、接收机收到的卫星数；纵向加速度传感器采用高精度(随机偏差1mg以内)、高频(输出频率大于等于20Hz)的加速度传感器，传感器可以输出纵向加速度。卫星定位系统天线安装在车顶中心位置；纵向加速度传感器安装在车辆的质心位置，方向与车辆纵轴一致。

具体实施步骤包括：

步骤一、首先选定道路；

首先将需要测绘的道路进行分割，选定需要进行量测的部分，确定需要测绘部分的起始点与终点。本专利所适用的道路为高速公路以及一级、二级公路，且所适用的道路光滑连续，不包含交叉路口。鉴于步骤二所采用的高斯-克吕格投影在局部范围精度较高，且随着范围增大误差也随之增大，因此选择的道路长度应不超过5km。

步骤二、通过多传感器信息采集车辆，采集选定道路位置以及车辆状态等信息；

本方法采用了多传感器信息采集车辆，其搭载的卫星定位系统可以实时输出道路位置R_i(L_iB_i)、车辆的垂直速度V_Z,i、车辆的水平速度V_XY,i以及卫星定位系统接收到的卫星数N_sat,i，其中L_i、B_i分别表示经度、纬度；纵向加速度传感器输出车辆纵向加速度信息A_i，其中i表示开始采集后接收到的信息的序号，i＝1,2,3….。通过同时开始采集多种传感器信息并统一各个传感器信息输出频率(输出频率均为20Hz)，保证采集序号相同的信息一一对应。同一时刻所采集的信息有：车辆所在位置的经纬度信息R_i(L_iB_i)、车辆的垂直速度V_Z,i、车辆的水平速度V_XY,i、卫星定位系统接收到的卫星数N_sat,i以及车辆纵向的加速度A_i。车辆在信息采集的过程中需要保持车辆平稳运行，以保证车身与地面尽量平行，减小在估计道路纵向坡度时因为车身倾斜而产生的误差。同时采集车辆的胎压需要保持一致，避免因车辆胎压不同导致的道路坡度估计误差。道路信息采集过程中为了保证采集的信息密度均匀，车速要保持匀速，且车速在55-65km/h范围以内，这样保证采集的道路位置信息的间距适中，所采集的位置信息间距在0.764-0.903m之间。由于道路中车道之间基本平行，所以本专利选取行进方向左侧车道来提取道路的纵向坡度信息，且采集过程中采集车辆沿车道中心行驶。本发明采用了多传感器信息采集车辆进行道路信息的采集，相对于利用坡度测量仪、全站仪、电磁波测距仪等设备(需要技术人员在目标道路进行大量测量工作)，该方法只需要将车辆在选定道路驶过就可以通过传感器采集对应的信息，简单易行、效率较高。

步骤三、将采集到经纬度坐标转化成平面坐标，并将转化后平面坐标表示的位置作为道路的节点；

由于制作地图需要平面直角坐标系坐标，本发明采用较为成熟的3度带高斯-克吕格投影方法，将经纬度坐标R_i(L_iB_i)投影为高斯平面直角坐标系坐标P_i(x_iy_i)，x_i为坐标转换后对应的平面直角坐标系的纵坐标(北向位置)，y_i为坐标转换后对应的平面直角坐标系的横坐标(东向位置)。根据起始点R₁(L₁B₁)选定R₀(L₀B₀)作为高斯-克吕格投影的原点，其中L₀＝3D，D为(L₁/3)四舍五入取整的值，B₀＝0°。经纬度坐标R_i(L_iB_i)转换公式如下所示：

$\left\{\begin{array}{c}{x}_i=X_{B_i}+\frac{1}{2}{\mathrm{Ntl}}^2{\cos }^2{B}_i+\frac{1}{24}Nt(5-t^2+9{\mathrm{\&eta;}}^2+4{\mathrm{\&eta;}}^4)l^4{B}_i\\ +\frac{1}{720}Nt(61-58t^2+t^4+270{\mathrm{\&eta;}}^2-330{\mathrm{\&eta;}}^2{t}^2)l^6{\cos }^6{B}_i+...\\ y_i=Nl\cos B_i+\frac{1}{6}N(1-t^2-{\mathrm{\&eta;}}^2)l^3{\cos }^3{B}_i\\ +\frac{1}{120}N(5-18t^2+t^4+14{\mathrm{\&eta;}}^4-58{\mathrm{\&eta;}}^2{t}^2)l^5{\cos }^5{B}_i+...\end{array}\right.---\left(1\right)$

式(1)为高斯投影公式的泰勒级数展开式，式中省去了7次以上高次项，其中为赤道至纬度B_i的子午线弧长，且 $X_{B_i}=C_0{B}_i-\cos B_i(C_1\sin B_i+C_2{\sin }^3{B}_i+C_3{\sin }^5{B}_i+C_4{\sin }^7{B}_i),$ l为所求点的经度L_i与L₀之差，t＝tanB_i，η＝e′cosB_i，e′为椭球第二偏心率，N为通过所求点的卯酉圈曲率半径，C₀，C₁，C₂，C₃，C₄为与点位无关的系数，仅有椭球体长半轴、短半轴、第一偏心率等参数确定。目前矢量地图多采用点的连线来表示道路，所以本发明将转换后平面坐标P_i(x_iy_i)所表示的位置作为道路的节点N_i(x_iy_i)，通过节点的连线表示道路。具体的坐标转化步骤以及参数详见参考文献(刘基余.卫星定位系统卫星导航定位原理与方法.北京:科学出版社,2003.229-379)、(胡伍生,高成发.卫星定位系统测量原理及其应用.北京:人民交通出版社，2004.1-101.)。

步骤四、通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出节点处的道路纵向坡度信息；

本发明提出了一种基于卡尔曼滤波的道路纵向坡度估计算法，该算法精度高、实时性好、适用范围广，能够实现复杂环境下的道路纵向坡度估计。

具体估计算法如下：

1、卡尔曼滤波观测量计算模型；

本发明提出了一种基于多传感器信息的卡尔曼滤波观测量计算模型。该模型采用高精度卫星定位系统和车辆纵向加速度传感器，可以得出比单一传感器精度更高、鲁棒性更好的道路纵向坡度观测值。本发明中坡度采用百分比法表示。

1)利用高精度卫星定位系统的数据解算出道路纵向坡度。

具体的解算方式：通过高精度卫星定位系统获取车辆的垂直速度V_Z,i和水平速度V_XY,i信息，然后根据公式

${\mathrm{\&theta;}}_{1,i}=\frac{V_{Z,i}}{V_{XY,i}}\&times;100\%---\left(2\right)$

得出道路纵向坡度θ_1,i；

2)利用车辆纵向加速度传感器数据解算出道路纵向坡度。

具体的解算方式：根据采集的车辆纵向加速度A_i，考虑到车辆行驶在匀速状态，车辆的纵向加速度可以忽略不计，然后通过公式

${\mathrm{\&theta;}}_{2,i}=tan\left({\sin }^{-1}\right(\frac{A_i}{g}\left)\right)\&times;100\%---\left(3\right)$

得出道路纵向坡度θ_2,i，其中g为重力加速度g＝9.8m/s²。

本算法依据卫星定位系统接收到的卫星数N_sat,i，将两种道路纵向坡度计算值的结果进行融合，得出精度更高、鲁棒性更好的道路纵向坡度观测值；最终的道路纵向坡度观测值θ_m(i)可由融合公式如下所示：

${\mathrm{\&theta;}}_m\left(i\right)=\left\{\begin{array}{cc}\frac{V_{Z,i}}{V_{XY,i}}\&times;100\%& N_{sat,i}\&GreaterEqual;6\\ tan\left({\sin }^{-1}\right(\frac{A_i}{g}\left)\right)\&times;100\%& N_{sat,i}<6\end{array}\right.---\left(4\right)$

考虑到本算法需要卫星定位系统信号达到一定的精度定位水平精度，而通常卫星数越多卫星定位系统解算的数据精度、可靠性越高，所以当接收到卫星数大于等于6颗卫星时，利用高精度卫星定位系统的数据解算出道路纵向坡度；如果小于6颗卫星时则利用车辆纵向加速度传感器数据解算出道路的坡度，加速度传感器具有不受环境干扰等优点，但是易受到温漂、零漂的影响，在卫星定位系统受到遮挡的情况下可以提供更加准确的坡度解算。

2、建立卡尔曼滤波的状态方程和观测方程。

本算法的卡尔曼滤波的状态方程：X＝[θ]，X为系统状态向量，θ为道路的纵向坡度；W表示零均值的系统高斯白噪声向量且W＝[w₁]，其中w₁系统高斯白噪声分量；W对应的系统噪声协方差阵为Q，其中表示系统高斯白噪声w₁对应的方差；状态转移矩阵这是由于道路的坡度都是连续缓慢变化的，可以认为当前的坡度等于下一采样点的坡度。

观测方程：Z(i)＝H(i)X(i)+V(i)式中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。由于观测向量与状态向量都是道路纵向坡度，所以H＝[1]、Z＝[θ_m]、其中θ_m为根据卡尔曼滤波观测量计算模型得出的道路纵向坡度。表示道路纵向坡度的观测噪声且是均值为0、方差为的高斯白噪声，V对应的观测噪声方差阵R可表示为

对于上述所描述的系统状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程：

一步预测误差方差阵：

测量更新：

滤波增益矩阵：K(i)＝P(i,i-1)H′(i)[H(i)P(i,i-1)H′(i)+R(i)]^-1(7)

状态估计：

估计误差方差阵：P(i)＝[I-K(i)H(i)]P(i,i-1)(9)

其中为的转置、H′(i)为H(i)的转置。

经过上述递推计算后，即可估出计节点N_i(x_iy_i)位置的纵向坡度θ_i，其中

步骤五、将道路的节点位置信息N_i(x_iy_i)以及对应的道路纵向坡度信息θ_i通过数字地图制作软件制作成基于卡尔曼滤波的道路坡度增强型数字矢量地图。

根据获得到道路的节点位置信息N_i(x_iy_i)以及该节点的道路纵向坡度信息θ_i通过数字地图自作软件生成基于卡尔曼滤波的道路坡度增强型数字矢量地图。首先利用节点的连线来表示所选取的道路，然后通过增加节点的道路信息列表将对应节点的道路纵向坡度信息增加到地图上。例如，数字地图制作软件MapInfo以列表的形式组织所有图形和信息数据，每一个信息在地图中都可以被理解成一个图层。本实施方式通过MapInfo制作基于卡尔曼滤波的道路坡度增强型数字矢量地图，具体步骤如下：首先将得到的节点位置信息和节点处纵向坡度信息分别制作成信息列表，并转换成MapInfo可以打开的文件格式，然后用MapInfo打开节点位置的信息列表，并根据位置信息创建节点，最后将节点处的纵向坡度信息列表添加进地图中，生成包含道路纵向坡度的增强型矢量数字地图。具体的数字地图的制作可以参考文献(王家耀,李志林,武芳.数字地图综合进展.北京:科学出版社，2011)、(王家耀,孙群,王光霞,江南,吕晓华.地图学原理与方法.北京:科学出版社,2006)、(吴秀琳,刘永革,王利军.Mapinfo9.5中文版标准教程.北京:清华大学出版,2009)。

Claims (1)

1.一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法；首先选定道路，确定道路起始点与终点；通过多传感器信息采集车辆，采集选定道路的位置信息、车辆的状态信息；然后对采集到的道路位置信息进行初步处理，将经纬度坐标转化为平面坐标，并将转化后平面坐标表示的位置作为道路的节点；通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出每个节点位置道路的纵向坡度；最后将道路的节点位置信息以及纵向坡度信息，利用数字地图制作软件制作成基于卡尔曼滤波的道路坡度增强型数字地图；

具体实施步骤包括：

步骤一、首先选定道路；

首先将需要测绘的道路进行分割，选定需要进行量测的部分，确定需要测绘部分的起始点与终点；本方法所适用的道路为高速公路以及一级、二级公路，且所适用的道路光滑连续，不包含交叉路口。鉴于步骤二所采用的高斯-克吕格投影在局部范围精度较高，且随着范围增大误差也随之增大，因此选择的道路长度应不超过5km；

步骤二、通过多传感器信息采集车辆，采集选定道路位置以及车辆状态等信息；

本方法采用了多传感器信息采集车辆，其搭载的卫星定位系统可以实时输出道路位置R_i(L_iB_i)、车辆的垂直速度V_Z,i、车辆的水平速度V_XY,i以及卫星定位系统接收到的卫星数N_sat,i，其中L_i、B_i分别表示经度、纬度；纵向加速度传感器输出车辆纵向加速度信息A_i，其中i表示开始采集后接收到的信息的序号，i＝1,2,3....；通过同时开始采集多种传感器信息并统一各个传感器信息输出频率(输出频率均为20Hz)，保证采集序号相同的信息一一对应；同一时刻所采集的信息有：车辆所在位置的经纬度信息R_i(L_iB_i)、车辆的垂直速度V_Z,i、车辆的水平速度V_XY,i、卫星定位系统接收到的卫星数N_sat,i以及车辆纵向的加速度A_i；车辆在信息采集的过程中需要保持车辆平稳运行，以保证车身与地面尽量平行，减小在估计道路纵向坡度时因为车身倾斜而产生的误差；同时采集车辆的胎压需要保持一致，避免因车辆胎压不同导致的道路坡度估计误差；道路信息采集过程中为了保证采集的信息密度均匀，车速要保持匀速，且车速在55-65km/h范围以内，这样保证采集的道路位置信息的间距适中，所采集的位置信息间距在0.764-0.903m之间；由于道路中车道之间基本平行，所以本方法选取行进方向左侧车道来提取道路的纵向坡度信息，且采集过程中采集车辆沿车道中心行驶；

步骤三、将采集到经纬度坐标转化成平面坐标，并将转化后平面坐标表示的位置作为道路的节点；

由于制作地图需要平面直角坐标系坐标，本方法采用较为成熟的3度带高斯-克吕格投影方法，将经纬度坐标R_i(L_iB_i)投影为高斯平面直角坐标系坐标P_i(x_iy_i)，x_i为坐标转换后对应的平面直角坐标系的纵坐标(北向位置)，y_i为坐标转换后对应的平面直角坐标系的横坐标(东向位置)；根据起始点R₁(L₁B₁)选定R₀(L₀B₀)作为高斯-克吕格投影的原点，其中L₀＝3D，D为(L₁/3)四舍五入取整的值，B₀＝0°；经纬度坐标R_i(L_iB_i)转换公式如下所示：

$\left\{\begin{array}{c}{x}_i=X_{B_i}+\frac{1}{2}{\mathrm{Ntl}}^2{\cos }^2{B}_i+\frac{1}{24}Nt(5-t^2+9{\mathrm{\&eta;}}^2+4{\mathrm{\&eta;}}^4)l^4{B}_i\\ +\frac{1}{720}Nt(61-58t^2+t^4+270{\mathrm{\&eta;}}^2-330{\mathrm{\&eta;}}^2{t}^2)l^6{\cos }^6{B}_i+...\\ y_i=Nl\cos B_i+\frac{1}{6}N(1-t^2+{\mathrm{\&eta;}}^2)l^3{\cos }^3{B}_i\\ +\frac{1}{120}N(5-18t^2+t^4+14{\mathrm{\&eta;}}^4-58{\mathrm{\&eta;}}^2{t}^2)l^5{\cos }^5{B}_i+...\end{array}\right.---\left(1\right)$

式(1)为高斯投影公式的泰勒级数展开式，式中省去了7次以上高次项，其中为赤道至纬度B_i的子午线弧长，且 $X_{B_i}=C_0{B}_i-\cos B_i(C_1\sin B_i+C_2{\sin }^3{B}_i+C_3{\sin }^5{B}_i+C_4{\sin }^7{B}_i),$ l为所求点的经度L_i与L₀之差，t＝tanB_i，η＝e′cosB_i，e′为椭球第二偏心率，N为通过所求点的卯酉圈曲率半径，C₀，C₁，C₂，C₃，C₄为与点位无关的系数，仅有椭球体长半轴、短半轴、第一偏心率等参数确定；目前矢量地图多采用点的连线来表示道路，所以本方法将转换后平面坐标P_i(x_iy_i)所表示的位置作为道路的节点N_i(x_iy_i)，通过节点的连线表示道路；

步骤四、通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出节点处的道路纵向坡度信息；

本方法提出了一种基于卡尔曼滤波的道路纵向坡度估计算法，该算法精度高、实时性好、适用范围广，能够实现复杂环境下的道路纵向坡度估计；

具体估计算法如下：

1、卡尔曼滤波观测量计算模型；

本方法提出了一种基于多传感器信息的卡尔曼滤波观测量计算模型；该模型采用高精度卫星定位系统和车辆纵向加速度传感器，可以得出比单一传感器精度更高、鲁棒性更好的道路纵向坡度观测值；本方法中坡度采用百分比法表示；

1)利用高精度卫星定位系统的数据解算出道路纵向坡度；

具体的解算方式：通过高精度卫星定位系统获取车辆的垂直速度V_Z,i和水平速度V_XY,i信息，然后根据公式

${\mathrm{\&theta;}}_{1,i}=\frac{V_{Z,i}}{V_{XY,i}}\&times;100\%---\left(2\right)$

得出道路纵向坡度θ_1,i；

2)利用车辆纵向加速度传感器数据解算出道路纵向坡度；

具体的解算方式：根据采集的车辆纵向加速度A_i，考虑到车辆行驶在匀速状态，

车辆的纵向加速度可以忽略不计，然后通过公式

${\mathrm{\&theta;}}_{2i}=tan\left({\sin }^{-1}\right(\frac{A_i}{g}\left)\right)\&times;100\%---\left(3\right)$

得出道路纵向坡度θ_2,i，其中g为重力加速度g＝9.8m/s²；

本算法依据卫星定位系统接收到的卫星数N_sat,i，将两种道路纵向坡度计算值的结果进行融合，得出精度更高、鲁棒性更好的道路纵向坡度观测值；最终的道路纵向坡度观测值θ_m(i)可由融合公式如下所示：

${\mathrm{\&theta;}}_m\left(i\right)=\left\{\begin{array}{cc}\frac{V_{Z,i}}{V_{XY,i}}\&times;100\%& N_{sat,i}\&GreaterEqual;6\\ \tan \left({\sin }^{-1}\right(\frac{A_i}{g}\left)\right)\&times;100\%& N_{sat,i}<6\end{array}\right.---\left(4\right)$

2、建立卡尔曼滤波的状态方程和观测方程；

本算法的卡尔曼滤波的状态方程：X＝[θ]，X为系统状态向量，θ为道路的纵向坡度；W表示零均值的系统高斯白噪声向量且W＝[w₁]，其中w₁系统高斯白噪声分量；W对应的系统噪声协方差阵为Q，其中表示系统高斯白噪声w₁对应的方差；状态转移矩阵这是由于道路的坡度都是连续缓慢变化的，可以认为当前的坡度等于下一采样点的坡度；

观测方程：Z(i)＝H(i)X(i)+V(i)式中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。由于观测向量与状态向量都是道路纵向坡度，所以H＝[1]、Z＝[θ_m]、其中θ_m为根据卡尔曼滤波观测量计算模型得出的道路纵向坡度。表示道路纵向坡度的观测噪声且是均值为0、方差为的高斯白噪声，V对应的观测噪声方差阵R可表示为

对于上述所描述的系统状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新：

时间更新：

状态一步预测方程：

一步预测误差方差阵：

测量更新：

滤波增益矩阵：K(i)＝P(i,i-1)H′(i)[H(i)P(i,i-1)H′(i)+R(i)]^-1(7)

状态估计：

估计误差方差阵：P(i)＝[I-K(i)H(i)]P(i,i-1)(9)

其中为的转置、H′(i)为H(i)的转置。

经过上述递推计算后，即可估计出节点N_i(x_iy_i)位置的纵向坡度θ_i，其中

步骤五、将道路的节点位置信息N_i(x_iy_i)以及对应的道路纵向坡度信息θ_i通过数字地图制作软件制作成基于卡尔曼滤波的道路坡度增强型数字矢量地图；

根据获得到道路的节点位置信息N_i(x_iy_i)以及该节点的道路纵向坡度信息θ_i通过数字地图自作软件生成基于卡尔曼滤波的道路坡度增强型数字矢量地图；首先利用节点的连线来表示所选取的道路，然后通过增加节点的道路信息列表将对应节点的道路纵向坡度信息增加到地图上。