本发明属于智能交通技术领域，具体涉及一种用于智能车辆的基于视觉信息与gis信息融合的方法。
背景技术：
随着汽车工业高速发展，世界汽车保有量不断攀升，与之而来的能源短缺、环境污染、交通拥堵等问题逐渐凸显。伴随着计算机、通信、自动控制等技术的提升，智能交通系统(intelligenttransportationsystem,its)应运而生，传感器技术、信息融合技术和控制算法的不断改进大大提升了车辆的智能化程度，智能汽车将改善道路交通环境。
智能车辆获取交通环境和自车状态信息是车辆运动控制的前提和基础，车载传感器在信息获取中有着不可或缺的地位。
目前智能车辆主要通过摄像机和雷达等传感器获取道路信息。摄像机可以获取道路环境的图像信息，然后通过图像处理技术提取所需信息，但是容易受光照等条件的影响。车辆需要获取交通环境中的车道、信号灯和车距等信息，因此多传感器融合的车辆感知方案成为研究热点。
车辆定位导航系统由gps和gis组成，gps通过卫星定位可以获取车辆位置坐标信息，gis的电子地图数据库中存储着城市交通道路信息。因此车辆定位导航系统可以获取当前车辆位置信息，根据用户给出目的地，利用路径规划策略得到行驶路径，并且在行驶过程中实时提供车辆全局位置信息。
智能车辆是智能交通系统的重要组成部分，车辆控制既要考虑局部交通环境，又要考虑城市道路交通系统的全局交通环境，从而保证车辆安全、高效的在道路上行驶。
技术实现要素：
根据以上现有技术的不足，本发明提供一种用于智能车辆的基于视觉信息与gis信息融合的方法，以期实现gis提供全局路径信息引导车辆行驶，并且提供车载传感器感知范围外的先知信息指导车辆局部信息检测，减少系统信息处理量，提高智能车辆安全运行的准确性和实时性。
本发明所述的一种用于智能车辆的基于视觉信息与gis信息融合的方法，其特征在于：通过视觉信息与gis信息融合的方式实现车辆在多车道的定位、路口信号灯检测、路口定位及车辆导航三个方面。
其中，优选方案如下：
所述的视觉信息首先要对图像进行预处理，包括相近标定、感兴趣区域划分、图像灰度化处理以及边缘检测；其中感兴趣区域划分是根据图像信息分布特点和所要提取的信息，图像划分为不同的感兴趣区域，即图像下半部分为检测车道线、交通标线感兴趣区域；图像上半部分为检测信号灯感兴趣区域；将感兴趣区域又划分为近区域和远区域。
所述的车辆在多车道的定位具体实现过程如下：
(1)设定车道线包含实现和虚线的路段称为常规路段，车道线均为实线的路段称为非常规路段；
(2)根据gps定位和电子地图确定车辆所在道路；
(3)根据定位、位置匹配，确定车辆所在路段是否为常规路段；当车辆在非常规路段时，分别检测本车道两侧是否存在车道线，根据检测结果判断车辆所在车道；当车辆在常规路段时，利用视觉信息判断车辆所在车道的两侧车道线类型，根据检测结果判断车辆所在车道。若车道左侧为实线右侧为虚线，则车辆在左侧车道，若车道左、右侧均为虚线，则车辆在中间车道；若车道左侧为虚线右侧为实线，则车辆在右侧车道。
所述过程(3)中，当车辆处于非常规路段，受周围车辆影响可能导致无法检测到相邻车道或者车道数量多于3时，无法判断车辆在车道中的具体位置，需要根据gps位置坐标计算车辆与道路边缘的距离，根据距离辅助定位车辆所在车道，具体过程如下：
假设车辆位置坐标为gi，车辆与道路左、右边缘最近点距离表示为dli(gi，li)，dri(gi，ri)，则取n个采样点计算车辆位置与道路左、右边缘最近点平均距离：
记车辆位置与道路左、右边缘最近点平均距离比值为：
ra的值反映了车辆在道路中的相对位置。
所述过程(3)中利用视觉信息技术判断车辆所在车道的两侧车道线类型根据以下方法实现：车道线存在于感兴趣区域，所以只需在感兴趣区域检测直线，因此对车道线所在的感兴趣区域的近、远区域分别执行hough变换；hough变换检测到图像中存在的线段，获得的线段参数包括起点、终点、长度，因为检测的对象为车道两侧的车道线，根据车道线平行的特点，计算每条线段的斜率，并计算两条线段之间的距离，由于我国规定了车道的宽度，根据距离排除不是车道线的线段，得到数量较少的候选车道线，候选车道线包括实线与虚线，先设定两个阈值lmax，lmin分别为线段长度上限阈值和下限阈值，当车道线长度大于上限阈值lmax时，则判断为实线；当车道线长度大于下限阈值lmin且小于上限阈值lmax时，判断车道线为虚线；当车道线长度小于下限阈值lmin时，重新获取车道线在按照上述步骤进行比较，结果比例较大者为最终结果，从而判断车道线为实线或虚线。
所述的路口信号灯检测具体实现过程如下：
(a)通过gps获取车辆地理位置，计算车辆距离路口距离，当距离大于100米时，信号灯检测程序不启动；距离小于100米时，启动信号灯检测程序，进入步骤(b)；
(b)对车辆进行车道中的定位，确定车辆所在车道；
(c)确定车辆所在车道信号灯检测感兴趣区域；
(d)通过电子地图获取信号灯类型和安装方式；
(e)通过视觉信息对信号灯检测，输出车辆所在车道信号灯状态。
所述步骤(e)中视觉信息对信号灯检测根据以下方法实现：
a)车辆行驶至路口时，一是正常行驶通过路口，二是在路口处减速停车，两种工况受交通流和信号灯的影响，因此通过路口时，需要对停车线以及交通信号灯进行检测。
b)检测停车线时，考虑到停车线有两个特征，一是停车线存在于两车道线之间且与之垂直，二是停车线在车辆前方基本呈水平方向。在感兴趣区域内利用hough变换方法进行直线检测，通过对直线角度约束会检测到满足停车线特征的所有直线，然后，由国标可知车道宽度lr，停车线长度不大于车道宽度，设置停车线长度下限阈值ls，当检测到的线段长度l满足条件ls＜l＜lr时，即判断为停车线；
c)检测信号灯时，基于颜色和形状特征相结合的方法检测信号灯。具体为：
根据信号灯三通道分布范围可得到阈值分割范围：
20＜r-g＜150&20＜r-b＜120为红色区域；
20＜g-r＜150&10＜g-b＜50为绿色区域；
20＜r-g＜80&0＜g-b＜90为黄色区域。
通过颜色分割之后图形中仍有大量干扰因素，为了排除干扰因素缩小信号灯检测范围，利用形状特征进一步过滤图像。以绿灯为例，当信号灯排列顺序为纵向时，从上到下排列顺序为红、黄、绿；当信号灯排列顺序为横向时，从左到右排列顺序为红、黄、绿。由此可以得到信号灯和背板的相对位置；
检测信号灯背板，当检测到信号板且满足与信号灯的关系，则认为该区域为信号灯区域。当信号灯为圆形信号灯时，上述过程即可满足信号灯检测需求，但是信号灯为箭头型信号灯时，还需要判断箭头的方向。
观察该箭头型信号灯可以发现其上下对称左右不对称的特点，将该信号灯区域分为上下两部分和左右两部分，且上下相等左右相等。然后统计各部分信号灯像素个数，上部分像素个数记为au，下部分像素个数记为ad，左部分像素个数记为a1，右部分像素个数记为ar。以上述信号灯为例，记
0.9＜pud＜1.1&plr＞pud箭头方向向左
0.9＜pud＜1.1&plr＜pud箭头方向向右
0.9＜plr＜1.1&pud＞plr箭头方向向上
0.9＜plr＜1.1&pud＜plr箭头方向向下
以上即为圆形信号灯和箭头型信号灯的检测方法。
所述的路口定位及车辆导航包括以视觉信息确定车辆初始位姿和以视觉信息融合gps和dr数据确定车辆最终位姿。在车辆导航系统中，根据起始点和给定终点，地图规划出的行驶路径由路段和节点组成，即由道路和路口组成。车辆在道路行驶时，根据视觉检测车道线和前车距离信息沿规划路径行驶，但是车辆行至路口时没有车道线等信息，单靠视觉信息无法实现车辆控制，因此还需要运用航位推算(dr)技术、gps定位技术和数据融合技术等。
电子地图数据库中存储着路口的位置、坐标信息，根据车辆运动特性和路口几何特征可以计算出车辆在路口处的期望行驶轨迹，控制车辆沿期望轨迹行驶应具备以下三个条件：获得车辆由道路进入路口时的初始位姿；根据车辆当前位姿可以推算出下一时刻车辆位姿；获得车辆由路口进入道路时的最终位姿。
本发明所具有的优点是：
(1)设计了车辆视觉检测模块，实现了对车道线、信号灯和前方车辆的检测。设计了基于hough变换的车道线虚线与实线检测方法和基于相机标定的车辆航向角计算方法，实现了车道线实现与虚线实时检测和车辆航向角的实时计算，分别用于车辆车道定位和车辆横向运动控制；设计了停车线检测方法和基于颜色和特征的信号灯检测方法，实现了停车线和圆型、箭头型信号快速检测，为车辆路口控制提供了信息。
(2)基于mapinfo平台，gis电子地图数据库，为车辆导航和与视觉信息融合提供支持。
(3)设计了基于gis电子地图和车道线检测相结合的车辆在多车道中的定位方法，实现了快速、准确车道定位。根据地图中存储信号灯类型和安装方式信息，设计了圆型和箭头型信号灯检测策略，设计了基于车辆所在车道的信号灯检测感兴趣区域确定方法，使信号灯检测效率和准确性得到提高。设计了基于扩展卡尔曼滤波的车辆路口导航方法，由几何法得到车辆在路口处的初始位姿和最终位姿，由扩展卡尔曼滤波结果得到车辆在路口中的实时位姿，实现车辆在路口处的控制。
附图说明
图1为实施例1中车辆在多车道中的定位算法流程图；
图2为实施例1中路口信号灯检测流程图；
图3为实施例1中车辆坐标系和局部坐标系关系图；
图4为实施例1中世界坐标系、相机坐标系、图像坐标系关系图。
具体实施方式
以下结合附图和实施例对本发明做进一步说明。
实施例1：
一种用于智能车辆的基于视觉信息与gis信息融合的方法，通过视觉信息与gis信息融合的方式实现车辆在多车道的定位、路口信号灯检测、路口定位及车辆导航三个方面。
所述的视觉信息首先要对图像进行预处理，包括相近标定、感兴趣区域划分、图像灰度化处理以及边缘检测；具体如下：
(1)相机标定：为了便于表示相机获取图像中各参数间的关系，需要建立世界坐标系、相机坐标系和图像坐标系。
通过迭代梯度下降和雅可比矩阵校正优化相机内外参数。
(2)感兴趣区域划分
观察图像可知，车道线、交通标线等道路信息，集中在图像的下半部分区域，交通信号灯存在于图像上半部分。根据图像信息分布特点和所要提取的信息，图像可以划分为不同的感兴趣区域：图像下半部分为检测车道线、交通标线感兴趣区域；图像上半部分为检测信号灯感兴趣区域。
将感兴趣区域划分为两个区域，即近区域和远区域。
感兴趣区域划分：
si＝(k，v)＝kiv2(i＝1，2)
式中，si为感兴趣区域距离，k为计算系数，v是道路允许最大车速。
(3)图像灰度化处理
由于本文获取的图像为彩色图像，所以包含信息量较大，直接进行处理计算量大，降低系统实时性。在车道线和交通标线检测中，既需要保留有用信息，又要提高图像处理速度，因此需要对原图像进行灰度化处理。
本文采用的颜色模型为rgb模型，分别赋予三原色不同的权值进行计算，最终得到灰度图像的像素值，其计算公式为：
利用灰度化公式处理图像。
(4)边缘检测算法
经过灰度化处理后的图像存在灰度不连续的特点，由一个区域过渡到另一个区域时灰度发生突变，边缘检测就是要找到灰度突变的边缘。采用canny算子对图像进行边缘检测。
得到边缘点集后，通过双阈值方法将边缘点连接起来。
如图1所示，车道线检测模块为视觉信息技术的硬件载体，所述的车辆在多车道的定位具体实现过程如下：
(1)设定车道线包含实现和虚线的路段称为常规路段，车道线均为实线的路段称为非常规路段；
(2)根据gps定位和电子地图确定车辆所在道路；
(3)根据定位、位置匹配，确定车辆所在路段是否为常规路段；当车辆在非常规路段时，分别检测本车道两侧是否存在车道线，根据检测结果判断车辆所在车道；当车辆在常规路段时，利用视觉信息判断车辆所在车道的两侧车道线类型，根据检测结果判断车辆所在车道。若车道左侧为实线右侧为虚线，则车辆在左侧车道，若车道左、右侧均为虚线，则车辆在中间车道；若车道左侧为虚线右侧为实线，则车辆在右侧车道。
所述过程(3)中，当车辆处于非常规路段，受周围车辆影响可能导致无法检测到相邻车道或者车道数量多于3时，无法判断车辆在车道中的具体位置，需要根据gps位置坐标计算车辆与道路边缘的距离，根据距离辅助定位车辆所在车道，具体过程如下：
假设车辆位置坐标为gi，车辆与道路左、右边缘最近点距离表示为dli(gi，li)，dri(gi，ri)，则取n个采样点计算车辆位置与道路左、右边缘最近点平均距离：
记车辆位置与道路左、右边缘最近点平均距离比值为：
ra的值反映了车辆在道路中的相对位置。以三车道为例，考虑gps定位误差为2m，当车辆在中间车道行驶时，ra的取值取值范围应该在0.5-2之间，当ra的值小于0.5时则车辆在左侧车道行驶，当ra的值大于2时车辆在右侧车道行驶。ra的值受gps定位精度影响较大，因此需要去除无效的gps定位点，当车辆位置与任一道路边缘距离大于两倍的道路宽度时则去除该gps定位点。
所述过程(3)中利用视觉信息技术判断车辆所在车道的两侧车道线类型根据以下方法实现：车道线存在于感兴趣区域，所以只需在感兴趣区域检测直线，因此对车道线所在的感兴趣区域的近、远区域分别执行hough变换；hough变换检测到图像中存在的线段，获得的线段参数包括起点、终点、长度，因为检测的对象为车道两侧的车道线，根据车道线平行的特点，计算每条线段的斜率，并计算两条线段之间的距离，由于我国规定了车道的宽度，根据距离排除不是车道线的线段，得到数量较少的候选车道线，候选车道线包括实线与虚线，先设定两个阈值lmax，lmin分别为线段长度上限阈值和下限阈值，当车道线长度大于上限阈值lmax时，则判断为实线；当车道线长度大于下限阈值lmin且小于上限阈值lmax时，判断车道线为虚线；当车道线长度小于下限阈值lmin时，重新获取车道线在按照上述步骤进行比较，结果比例较大者为最终结果，从而判断车道线为实线或虚线。
如图2所示，信号灯检测是车辆顺利通过路口的前提。虽然信号灯的类型和安装方式较多，但是对于一个具体的路口，信号灯类型和安装方式是确定的，所以在检测信号灯之前如果能获得其类型和安装方式，将会提高检测精度和检测效率。gis地图数据库可以提供信号灯检测所需的类型和安装方式信息。
信号灯一般只存在于交通路口，所以只有车辆行驶至路口附近时信号灯检测程序才开始运行，不仅可以减少无信号灯时的误检测，而且可以提高系统效率。为提高信号灯检测效率和准确性，首先确定信号灯存在的感兴趣区域，然后检测信号灯。车辆在某一车道驶向路口时，信号灯在图像中的位置范围基本是固定的，信号灯的位置范围即为感兴趣区域，它受车辆距信号灯距离、车辆位姿以及车辆所在车道影响。感兴趣区域应满足以下条件：车辆接近路口时，始终沿所在车道中线平稳行驶；感兴趣区域应包含从开始检测位置到停车线位置信号灯所有存在区域。
车辆在不同车道时，信号灯感兴趣区域会发生变化，所以车辆在不同车道驶进路口时，信号灯感兴趣区域的位置是不同的，各车道信号灯感兴趣区域由实验标定获得并存储至地图数据库中，根据上问车辆在多车道中的定位可得到车辆所在具体车道，然后即可得到所在车道的信号灯检测感兴趣区域，再确定车辆所在车道的信号灯状态。
所述的路口信号灯检测具体实现过程如下：
(a)通过gps获取车辆地理位置，计算车辆距离路口距离，当距离大于100米时，信号灯检测程序不启动；距离小于100米时，启动信号灯检测程序，进入步骤(b)；
(b)对车辆进行车道中的定位，确定车辆所在车道；
(c)确定车辆所在车道信号灯检测感兴趣区域；
(d)通过电子地图获取信号灯类型和安装方式；
(e)通过视觉信息对信号灯检测，输出车辆所在车道信号灯状态。
gis可以计算车辆距信号灯的距离，信号灯检测算法只有在车辆距离路口小于100米时才开始执行，避免了信号灯检测算法过多占用系统资源。在信号灯检测过程中，感兴趣区域可以提高信号灯检测的准确性，降低信息处理量，提高系统实时性，因此感兴趣区域对于信号灯检测算法至关重要。基于地图与视觉的信号灯检测算法，多数是根据地图提供的信号灯位置信息以及车辆在地图中的坐标信息，然后通过坐标转换计算得到信号灯在图像坐标系中坐标，以此确定感兴趣区域。此方法可以准确计算出车辆与车道线的横向位置，但是无法准确计算车辆纵向位置，而且该方法计算量较大，对硬件系统性能要求较高。本文在确定感兴趣区域时采用查表的方法，即通过计算车辆与路口(停车线)的距离查找与之对应的图像中感兴趣区域坐标。感兴趣区域坐标用两组坐标表示：感兴趣区域形状为矩形，矩形用左上角坐标(x1，y2)及右下角坐标(x2，y2)来表示。车辆在距离路口停车线100时开启信号灯检测算法，此时距离由gis根据gps定位车辆坐标计算得出，当视觉可以检测到停车线时，距离即有视觉检测结果计算得出以提高车辆纵向定位精度。以某一路口为例，通过实车标定得到多组车辆距路口停车线距离与信号灯感兴趣区域坐标关系，感兴趣区域查询表如下表所示。
由上表可以看出，感兴趣区域查询点之间的距离间隔不同，是因为车辆距离较远时信号灯在图像中所占像素很少，检测效果不理想，随着距离越近，查询点的间隔距离也越来越小，可以提供更为准确的感兴趣区域，当车辆距离停车线30米以内时，视觉检测可以准确计算出车辆距离停车线的距离。计算出车辆与停车线的实际距离后，取与实际距离最接近的查询距离进行查表，确定感兴趣区域。上表中感兴趣区域坐标并不是实验中信号灯实际坐标取值，而是考虑误差因素调整后的坐标值。
所述步骤(e)中视觉信息对信号灯检测根据以下方法实现：
a)车辆行驶至路口时，一是正常行驶通过路口，二是在路口处减速停车，两种工况受交通流和信号灯的影响，因此通过路口时，需要对停车线以及交通信号灯进行检测。
b)检测停车线时，考虑到停车线有两个特征，一是停车线存在于两车道线之间且与之垂直，二是停车线在车辆前方基本呈水平方向。在感兴趣区域内利用hough变换方法进行直线检测，通过对直线角度约束会检测到满足停车线特征的所有直线，然后，由国标可知车道宽度lr，停车线长度不大于车道宽度，设置停车线长度下限阈值ls，当检测到的线段长度l满足条件ls＜l＜lr时，即判断为停车线；
c)检测信号灯时，基于颜色和形状特征相结合的方法检测信号灯。具体为：
根据信号灯三通道分布范围可得到阈值分割范围：
20＜r-g＜150&20＜r-b＜120为红色区域；
20＜g-r＜150&10＜g-b＜50为绿色区域；
20＜r-g＜80&0＜g-b＜90为黄色区域。
通过颜色分割之后图形中仍有大量干扰因素，为了排除干扰因素缩小信号灯检测范围，利用形状特征进一步过滤图像。以绿灯为例，当信号灯排列顺序为纵向时，从上到下排列顺序为红、黄、绿；当信号灯排列顺序为横向时，从左到右排列顺序为红、黄、绿。由此可以得到信号灯和背板的相对位置；
检测信号灯背板，当检测到信号板且满足与信号灯的关系，则认为该区域为信号灯区域。当信号灯为圆形信号灯时，上述过程即可满足信号灯检测需求，但是信号灯为箭头型信号灯时，还需要判断箭头的方向。
观察该箭头型信号灯可以发现其上下对称左右不对称的特点，将该信号灯区域分为上下两部分和左右两部分，且上下相等左右相等。然后统计各部分信号灯像素个数，上部分像素个数记为au，下部分像素个数记为ad，左部分像素个数记为al，右部分像素个数记为ar。以上述信号灯为例，记
0.9＜pud＜1.1&plr＞pud箭头方向向左
0.9＜pud＜1.1&plr＜pud箭头方向向右
0.9＜plr＜1.1&pud＞plr箭头方向向上
0.9＜plr＜1.1&pud＜plr箭头方向向下
以上即为圆形信号灯和箭头型信号灯的检测方法。
所述的路口定位及车辆导航具体实现过程如下：
在车辆导航系统中，根据起始点和给定终点，地图规划出的行驶路径由路段和节点组成，即由道路和路口组成。车辆在道路行驶时，根据视觉检测车道线和前车距离信息沿规划路径行驶，但是车辆行至路口时没有车道线等信息，单靠视觉信息无法实现车辆控制，因此还需要运用航位推算(dr)技术、gps定位技术和数据融合技术等。
电子地图数据库中存储着路口的位置、坐标信息，根据车辆运动特性和路口几何特征可以计算出车辆在路口处的期望行驶轨迹，控制车辆沿期望轨迹行驶应具备以下三个条件：获得车辆由道路进入路口时的初始位姿；根据车辆当前位姿可以推算出下一时刻车辆位姿；获得车辆由路口进入道路时的最终位姿。
(i)如图3所示，定义车辆坐标系和路口局部坐标系；车辆坐标系就是以车辆为参考建立坐标系，车辆前轮轴中心为坐标原点oc，车辆轴线向前方向为yc轴，垂直于yc轴向右方向为xc轴；路口局部坐标系以车辆所在车道右侧车道线与停车线交点为原点o1，垂直停车线向前为y1轴，垂直于y1轴向右方向为x1轴。
(ⅱ)车辆初始位姿确定：车辆到路口附近开始检测停车线，检测到停车线后则开始计算车辆的初始位姿，通过视觉检测路口局部坐标系原点o1位置信息，然后通过逆透视变换可以计算出局部坐标系在车辆坐标系的位姿(xc，1，yc，1，θ)，其中θ是车辆航向角，车辆航向角θ可以通过视觉信息技术检测计算得到，具体如下：
车辆航向角就是车辆行驶方向与道路的夹角，通过计算车辆行驶方向与车道线夹角即可得到车辆航向角，车辆航向角计算方法如下：
建立世界坐标系、相机坐标系和图像坐标系，坐标系关系图如图4所示。假设车道线一端点p，在世界坐标系下的坐标为(xw,yw,zw),在相机坐标系下的坐标为(xc,yc,zc),在图像坐标系下的坐标为p(u,v)，其中主点坐标为o0(u0,v0)。
由坐标系关系图可得：
式中，f为相机的焦距；pb可以由标定得到的平移向量和标定板左上角点实际高度计算得到。
本文假设相机光轴与地面平行，则端点坐标在相机坐标系下的表示为：
相机标定得到外部参数旋转矩阵和平移向量r和t，由坐标系之间的转换关系可得：
pw＝r-1·pc-r-1·t
式中，pw，pc为p点的世界坐标和相机坐标。通过坐标转换得到车道线端点在世界坐标系下的坐标，选取世界坐标系yoz平面与地面平行，计算世界坐标系下表示车道线向量与z轴的夹角，即得到车辆航向角。
由此可以得到车辆在路口局部坐标系中的初始位姿：
全局坐标系下的点(xw，yw)与路口局部坐标系下的点(x1，y1)，转换关系：
式中，m，d，α分别为坐标系的旋转矩阵、平移向量和旋转角，选取地图上两点b,c分别将其全局坐标和局部坐标带入关系式，求解即可得到m,d。则全局坐标系下任意一点对应的局部坐标系坐标为：
得到车辆初始位姿后，根据车辆运动特性可知车辆当前时刻与下一时刻位姿关系为：
式中：x1，i，y1，i，θi为路口局部坐标系下i时刻车辆坐标和航向角；δsi，δθi分别是i时刻到i+1时刻车辆行驶距离和航向角变化量；
(ⅲ)车辆最终位姿的确定：采用扩展卡尔曼滤波方法对gps和dr数据进行信息融合，扩展卡尔曼滤波过程方程为：
xi＝f(xi-1，ui-1，w)
其中xi为第i时刻的状态；ui-1为控制输入；w为里程计与编码器输入偏差，w＝(w1，w2)。
输入误差协方差：
卡尔曼测量方程：
zi＝h(xi，e)
e＝[ex，ey]t
观测误差协方差：
车辆在路口处过程方程：
路口处观测方程：
式中，ex，ey分别是gps纵向和横向定位误差。上述就是基于扩展卡尔曼滤波得到的过程方程和测量方程，不断更新时间和更新测量数据就构成了扩展卡尔曼滤波的基本步骤。
车辆由路口进入车道时，期望车辆位姿为车辆在车道中间且与道路平行进入，即车辆轴线与道路中线一致，设定车辆进入道路的目标点为m(x1，m，y1，m)点，点h(x1，h，y1，h)，k(x1，k，y1，k)为停车线与车道线的交点，假设车道线宽度为b，则车辆由路口进入道路的最终位姿为：
以上就是车辆在路口处的控制方法，车辆在路口处的初始位姿和最终位姿由坐标计算可以获得，车辆在路口中的实时位姿由扩展卡尔曼滤波结果获得。由于存在误差，所以车辆由路口进入道路时，进入点可能会存在偏差，但是车辆检测到车道线后对车辆位姿进行调整即可。