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

摘要

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

主权项

1.一种基于卡尔曼滤波的道路坡度增强型数字地图制作方法；首先选定道路，确定道路起始点与终点；通过多传感器信息采集车辆，采集选定道路的位置信息、车辆的状态信息；然后对采集到的道路位置信息进行初步处理，将经纬度坐标转化为平面坐标，并将转化后平面坐标表示的位置作为道路的节点；通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出每个节点位置道路的纵向坡度；最后将道路的节点位置信息以及纵向坡度信息，利用数字地图制作软件制作成基于卡尔曼滤波的道路坡度增强型数字地图；具体实施步骤包括：步骤一、首先选定道路；首先将需要测绘的道路进行分割，选定需要进行量测的部分，确定需要测绘部分的起始点与终点；本方法所适用的道路为高速公路以及一级、二级公路，且所适用的道路光滑连续，不包含交叉路口；选择的道路长度不超过5km；步骤二、通过多传感器信息采集车辆，采集选定道路位置以及车辆状态信息；本方法采用了多传感器信息采集车辆，多传感器包括卫星定位系统和车辆纵向加速度传感器；其搭载的卫星定位系统可以实时输出道路位置Ri(Li Bi)、车辆的垂直速度VZ，i、车辆的水平速度VXY，i以及卫星定位系统接收到的卫星数Nsat，i，其中Li、Bi分别表示经度、纬度；纵向加速度传感器输出车辆纵向加速度信息Ai，其中i表示开始采集后接收到的信息的序号，i＝1，2，3....；通过同时开始采集多种传感器信息并统一各个传感器信息输出频率为20Hz，保证采集序号相同的信息一一对应；同一时刻所采集的信息有：车辆所在位置的经纬度信息Ri(Li Bi)、车辆的垂直速度VZ，i、车辆的水平速度VXY，i、卫星定位系统接收到的卫星数Nsat，i以及车辆纵向的加速度Ai；车辆在信息采集的过程中需要保持车辆平稳运行，以保证车身与地面尽量平行，减小在估计道路纵向坡度时因为车身倾斜而产生的误差；同时采集车辆的胎压需要保持一致，避免因车辆胎压不同导致的道路坡度估计误差；道路信息采集过程中车速要保持匀速，且车速在55‑65km/h范围以内，这样保证采集的道路位置信息的间距适中，所采集的位置信息间距在0.764‑0.903m之间；本方法选取行进方向左侧车道来提取道路的纵向坡度信息，且采集过程中采集车辆沿车道中心行驶；步骤三、将采集到经纬度坐标转化成平面坐标，并将转化后平面坐标表示的位置作为道路的节点；本方法采用3度带高斯‑克吕格投影方法，将经纬度坐标Ri(Li Bi)投影为高斯平面直角坐标系坐标Pi(xi yi)，xi为坐标转换后对应的平面直角坐标系的纵坐标，yi为坐标转换后对应的平面直角坐标系的横坐标；根据起始点R1(L1 B1)选定R0(L0 B0)作为高斯‑克吕格投影的原点，其中L0＝3D，D为(L1/3)四舍五入取整的值，B0＝0°；经纬度坐标Ri(Li Bi)转换公式如下所示：式(1)为高斯投影公式的泰勒级数展开式，式中省去了7次以上高次项，其中为赤道至纬度B_i的子午线弧长，且l为所求点的经度L_i与L₀之差，t＝tan B_i，η＝e′cos B_i，e′为椭球第二偏心率，N为通过所求点的卯酉圈曲率半径，C₀，C₁，C₂，C₃，C₄为与点位无关的系数，仅由椭球体长半轴、短半轴、第一偏心率确定；本方法将转换后平面坐标P_i(x_i y_i)所表示的位置作为道路的节点N_i(x_i y_i)，通过节点的连线表示道路；步骤四、通过基于卡尔曼滤波的道路纵向坡度估计算法，估计出节点处的道路纵向坡度信息；具体估计算法如下：步骤1、卡尔曼滤波观测量计算模型；本方法中坡度采用百分比法表示；1)利用高精度卫星定位系统的数据解算出道路纵向坡度；具体的解算方式：通过高精度卫星定位系统获取车辆的垂直速度VZ，i和水平速度VXY，i信息，然后根据公式得出道路纵向坡度θ1，i；2)利用车辆纵向加速度传感器数据解算出道路纵向坡度；具体的解算方式：根据采集的车辆纵向加速度Ai，考虑到车辆行驶在匀速状态，车辆的纵向加速度可以忽略不计，然后通过公式得出道路纵向坡度θ2，i，其中g为重力加速度g＝9.8m/s2；本算法依据卫星定位系统接收到的卫星数Nsat，i，将两种道路纵向坡度计算值的结果进行融合，得出精度更高、鲁棒性更好的道路纵向坡度观测值；最终的道路纵向坡度观测值θm(i)可由融合公式如下所示：步骤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_i y_i)位置的纵向坡度θ_i，其中步骤五、将道路的节点位置信息Ni(xi yi)以及对应的道路纵向坡度信息θi通过数字地图制作软件制作成基于卡尔曼滤波的道路坡度增强型数字矢量地图；根据获得到道路的节点位置信息Ni(xi yi)以及该节点的道路纵向坡度信息θi通过数字地图自作软件生成基于卡尔曼滤波的道路坡度增强型数字矢量地图；首先利用节点的连线来表示所选取的道路，然后通过增加节点的道路信息列表将对应节点的道路纵向坡度信息增加到地图上。