CN114889606B
有效
一种基于多传感融合的低成本高精定位方法
技术领域
[0001]本发明涉及自动驾驶领域的定位方法，特别涉及一种基于多传感融合的低成本高精定位方法。
背景技术
[0002]自动驾驶的架构可以用五个系统来描述，包括定位、感知、规划、控制和系统管理。感知规划和控制等模块要求对车辆的位置有准确的了解，以执行正确的驾驶决策和行动。例如几个分米的误差可能让车辆定位到错误的车道，导致错误的驾驶行为进而引发交通事故。因此，自动驾驶汽车需要强大且精准的定位系统，精度达到分米甚至厘米级。
[0003]全球定位系统(GPS)为全球定位提供了一种方便的解决方案，是车辆应用中最常用的定位系统。然而，GPS只能提供较低的定位精度(～10m)，且受信号遮挡、多路径等因素的影响可靠性较差，单独运用于自动驾驶车辆定位不能满足要求。因此，GPS通常与惯性测量单元(IMU)、实时运动学(RTK)等集成以获得更高的精度。然而，为了实现高精度定位，需要使用价格昂贵的IMU或者RTK。一些学者使用激光雷达创建环境的特征地图，并在地图中定位车辆。虽然激光雷达技术可以提供精确和稳定的定位方法，但缺点是它们的高功率，高计算代价，以及高实现成本，给自动驾驶汽车的大规模生产和营销带来了问题。
[0004]随着计算机视觉技术的进步，摄像头作为一种低成本的环境感知传感器，已经成为自动驾驶不可或缺的感知设备。有学者提出仅使用摄像头的低成本定位方法，首先通过将图像分割成网格并提取每个单元的方向直方图来估计大致的位置，然后使用包含环境地标的地图进行定位，但是精度不高。另一种使用摄像头的方法是视觉里程计，它通过比较左右摄像头在不同时间帧下跟踪特征点的相对运动和方向，增量计算车辆的姿态，以进行相对定位，但视觉里程计存在严重的漂移误差问题，尤其在开阔的道路场景。
[0005]为了进一步提高基于摄像头的定位技术的性能，研究人员利用摄像头识别的车道线与摄像头的相对侧向距离(下文简称为线侧距离)与其他传感器进行融合来实现定位。车辆行驶时车辆所在车道以及车辆所在的左右车道线边界点会发生变化，必须准确的知道当前车辆所处的车道和边界点才能有效利用摄像头测量值进行定位，然而，现有研究利用线侧距离与其他传感器进行融合定位时，由于车辆所匹配的车道和边界点不准确，尤其是在车辆换道行驶靠近车道线时匹配精度较差，造成定位误差。
发明内容
[0006]为了解决上述技术问题，本发明提供一种基于多传感融合的低成本高精定位方法，包括以下步骤：
[0007]步骤1、创建地图模型：
[0008]由于高精度地图包含大量复杂的细节和冗余的信息，建立大范围的高精度地图费时费力且成本高，为节约成本，本发明建立一种轻量化的车道级地图模型；
[0009]步骤2、基于GPS、轮式里程计和底盘信号的融合：
[0010](1)基于扩展卡尔曼滤波EKF对GPS、轮式里程计和底盘信号进行融合，利用车轮里程计相关测量值对状态变量进行修正；
[0011](2)挖掘车辆底盘的StandStill信号来进行运动学约束，包括零速修正、零角速度约束；
[0012]步骤3、换道识别：
[0013]采用一种鲁棒的换道识别方法，通过建立多重准则来进行换道识别，当能识别到车道线且车道线质量较好时，直接根据C0判断准则判断是否换道；当能识别到车道线但车道线质量较差时或者识别不到车道线时，则采用横摆角速度、航向角、变道时长多个准则进行综合判断；
[0014]步骤4、地图匹配：
[0015]步骤4.1、道路-路口匹配：
[0016]在车辆行驶过程中，判断车辆三种位置状态：在路口、在道路、离开路口进入道路；当车辆在道路时，使用道路内车道匹配方法确定车道；当车辆从路口离开进入道路时，及时启用摄像头辅助定位方法；
[0017]步骤4.2、道路内车道匹配：
[0018]当识别出在道路内行驶时，首先以车辆当前位置为中心，搜索一定范围内的候选车道，然后利用数个评价函数对候选车道进行综合评价；计算各个候选车道的综合评分，选定综合评分最高的车道为匹配结果；
[0019]步骤4.3、确定左右边界点：
[0020]当车辆离开路口进入道路，以当前车道起始点作为车辆的行驶起点，并根据行驶起点确定车辆所在的初始左右边界点；在车辆行驶过程中进行换道识别，若没有发生换道，则根据轮式里程计的车轮转数推算车辆走过距离与车道线的各离散点间距进行比较，结合初始边界点对边界点进行更新；当发生换道时，根据道路匹配方法重新匹配车道，然后在新车道上重新匹配车辆行驶起点和初始左右边界点，继续沿车道行驶时更新左右边界点的步骤与不换道时相同；
[0021]步骤5、基于摄像头的辅助定位：
[0022]在得到车辆所在车道和左右边界点基础上进行基于摄像头的辅助定位；
[0023]在过去的研究中，常常将线侧距离融入EKF中进行定位，但是EKF是对非线性系统的线性化近似，会带来精度损失；针对这个问题，本发明提出一种新的基于摄像头的辅助定位方法：求出过车辆后轴中心垂直于航向角θ的直线与左边界线和右边界线的交点、车辆到左右边界线距离；利用左边界线定位后的车辆后轴中心坐标和利用右边界线定位后的车辆后轴中心坐标，确定最终定位坐标。
[0024]进一步的，步骤1中，本发明建立一种轻量化的车道级地图模型，单条车道l表示为：
[0025]l＝{id,pre,suc,pc,pl,pr,q} (1)
[0026]其中id表示车道l的序号，从靠近道路中心线开始，左侧序号从1开始依次递增，右侧序号从-1开始依次递减；pre表示与车道起点相连的交叉路口或车道；suc定义为与车道末端连接的交叉路口或车道；pc,pl和pr分别代表车道中心线、左车道线和右车道线的点集；q包含车道对应的属性，包括宽度、限速、左右车道线类型；
[0027]其中pl和pr表示为：
[0028]
[0029]
[0030]进一步的，步骤2(1)中，本步骤采用一种基于扩展卡尔曼滤波EKF对GPS、轮式里程计和底盘信号进行融合的方法，利用车轮里程计相关测量值对状态变量进行修正；所述的状态变量表示如下：
[0031]x＝[px,py,θ,ω]T (4)
[0032]其中px和py为ENU坐标系下车辆的横纵坐标，θ为ENU坐标系下车辆的航向角，ω为车辆横摆角速度；
[0033]状态转移方程为：
[0034]x'＝Fx+Bu+λ (5)
[0035]此处，控制输入Bu为0，λ为过程噪声；
[0036]本发明使用两轮里程计模型，设车辆的质心侧偏角为β，状态转移方程具体如下：
[0037]
[0038]
[0039]θk＝θk-1+ωk-1T (8)
[0040]ωk＝ωk-1 (9)
[0041]在正常驾驶和天气良好条件下，车轮打滑忽略不计，速度计算如下：
[0042]
[0043]其中nRL和nRR为每秒左右车轮转数，cRL和cRR是实际的左右车轮周长；横摆角速度ω由底盘ESC提供；
[0044]状态转移矩阵F表示为：
[0045]
[0046]当GPS信号良好时，利用GPS系统建立的观测矩阵为：
[0047]
[0048]利用轮式里程计信号建立的横摆角速度观测值如下：
[0049]
[0050]其中tR是后轴轮距，对应的观测矩阵为：
[0051]H＝(0 0 0 1) (14)；
[0052]步骤2(2)中，挖掘车辆底盘的StandStill信号来进行运动学约束，包括零速修正、零角速度约束，当StandStill为0时表示车辆处于行驶状态，为1时表示车辆处于静止，由于这个信号很准确，可以进行状态变量的约束，消除停车时噪声带来的积分误差。
[0053]当StandStill为1时，表示车辆处于静止状态，车速和横摆角速度均为0，可得：
[0054]px,k＝px,k-1 (15)
[0055]py,k＝py,k-1 (16)
[0056]θk＝θk-1 (17)
[0057]ωk＝0 (18)
[0058]此时F为：
[0059]
[0060]进一步的，步骤3中，本发明提出一种鲁棒的换道识别方法：通过建立多重准则来进行换道识别，当能识别到车道线且车道线质量较好时，直接根据C0判断准则判断是否换道；当能识别到车道线但车道线质量较差时或者识别不到车道线时，采用横摆角速度、航向角、变道时长多个准则进行综合判断，这样能保证在车道线遮挡或者磨损情况下都能识别到换道。
[0061]所述的C0判断准则如下：
[0062]假设摄像头坐标系原点已标定在车辆后轴中心，C0代表线侧距离，车辆实际后轴中心位置到左右车道线的线侧距离分别为
和
左侧车道线的C0值
大于0，右侧车道线的C0值
小于0；
[0063]不管向左还是向右换道跨越车道线时，
逐渐减小接近于0，
增大接近于0；设t时刻的左右车道线C0值分别为
和
判断变量ξ；则换道判断条件为：
[0064]
[0065]在识别到换道状态之后启动换道结束状态检测，设定C0值的变化量阈值
当满足条件(21)时，判断已结束换道；
[0066]
[0067]当不满足C0判断准则时，使用横摆角速度、航向角、变道时长判断准则进行综合判断；
[0068]横摆角速度判断准则如下：
[0069]利用横摆角速度的变化识别车辆是否换道，通过在预定义的窗口长度M上观察横摆角速度的变化来识别换道：左换道和右换道过程中，横摆角速度变化曲线经历一个波峰和波谷，对于左换道，先有波峰再有波谷；右换道则先有波谷再有波峰；
[0070]航向角判断准则如下：
[0071]如果满足横摆角速度判断准则，则可能发生换道，也可能在弯道上行驶；使用比窗口长度M稍长的滑动窗口检测时间段内航向角始末变化量δ＝|θend-θstart|，如果δ≈0，则车辆在直道上变道；否则沿弯道行驶；
[0072]变道时长判断准则如下：
[0073]设完整的变道时长为TLC,若横摆角速度从波峰到波谷的时间大于TLC，则判断车辆沿弯道行驶而不是换道。
[0074]进一步的，步骤4.1中，道路-路口匹配的流程如下：
[0075]S01、开始；
[0076]S02、点对点匹配，找到最近点为匹配点；
[0077]S03、判断此刻匹配点是否在路口内，如果是则进入S04，否则进入S05；
[0078]S04、如果当前摄像头没有识别出车道线，则判断此刻车辆在路口内，并进入S08，否则进入S05；
[0079]S05、判断车辆是否上一时刻在路口，此刻不在路口，如果是则进入S06，否则进行道路内车道匹配，并进入S08；
[0080]S06、判断当前匹配点所在道路是否为路口的后续道路，如果是则进入步骤S07，否则进入S08；
[0081]S07、判断当前摄像头是否能识别出车道线，如果是则判断此刻车辆离开路口进入道路，并进入S08，否则进入S08；
[0082]S08、结束匹配。
[0083]进一步的，步骤4.2道路内车道匹配过程中，在道路内行驶时，换道靠近车道线时由于定位误差会出现来回匹配到本车道和相邻车道的结果。结合提出的换道识别方法，本发明提出一种改进的多指标加权评价地图匹配方法，提高道路内车道匹配的准确性和鲁棒性。
[0084]所述的数个评价函数包括：
[0085](1)基于距离的评价，设评价函数为：
[0086]
[0087]其中d为定位坐标至候选车道中心线的距离；
[0088](2)基于拓扑关系的评价：设评价函数为F(link)，如果当前匹配车道与上一时刻匹配车道具有拓扑上的连接关系或者为同一车道，则F(link)＝1，否则F(link)＝0；
[0089](3)基于航向的评价：设候选车道中心线航向角为θ1，车辆航向角为θ2，考虑到车辆航向角及弯道的中心线方向是变化的，且车辆行驶时并不一定与车道平齐，构建的评价函数为：
[0090]
[0091](4)基于车道线类型的评价：设摄像头识别的车道线总数为M，将摄像头识别的车道线类型与地图上对应位置的车道线类型相比较，类型相同的车道线数目记为N，考虑到定位的不确定性及同一车道可能存在的车道线类型的变化，使用系数弱化基于车道线类型的评价，评价函数为：
[0092]
[0093](5)基于换道识别的评价：设评价函数为F(lc)，以上一时刻所在车道lcur及左右相邻车道lleft和lright三条车道为例，检测到换道时：
[0094]
[0095]综合评分为：
[0096]Fsum＝F(link)gF(θ)g(F(d)+F(lanetype)+F(lc)) (26)。
[0097]计算各个候选车道的综合评分，选定综合评分最高的车道为匹配结果。
[0098]进一步的，步骤5基于摄像头的辅助定位中，车道线模型为：
[0099]y＝C3x3+C2x2+C1x+C0 (27)
[0100]其中C0、C1、C2和C3分别代表线侧距离、坡度、曲率和曲率导数；
[0101]设车辆的后轴中心为(xM,yM)，以它为车辆坐标系原点，横轴指向车辆前方，纵轴指向车辆左侧；摄像头坐标系RC的原点为点C(xC,yC)，横纵轴方向与车辆坐标系相同；摄像头坐标系RC的原点为点C(xC,yC)标定在车辆后轴中心pM(xM,yM),车辆的左边界点为
和
右边界点为
和
过后轴中心pM(xM,yM)，方向与航向角θ方向垂直的直线与左边界线和右边界线交点分别为pL(xL,yL)、pR(xR,yR)；车辆定位坐标到左右边界线距离分别为dl和dr，设车辆实际后轴中心位置为preal，摄像头测得的车辆相对于左右边界线的线侧距离分别为
和
[0102]从后轴中心pM出发，方向与航向角θ方向垂直的直线方程表示为：
[0103]
[0104]求得交点坐标pL和pR后，dl和dr表示为：
[0105]
[0106]
[0107]利用左边界线定位后的车辆后轴中心坐标为(xML,yML)：
[0108]
[0109]利用右边界线定位后的车辆后轴中心坐标为(xMR,yMR)：
[0110]
[0111]使用左右车道线定位的平均值作为最后的定位坐标(xM',yM')；
[0112]考虑到线侧距离具有一定的误差，误差接近于一个常值，设为Δdcam，最终定位坐标见式(33)：
[0113]
[0114]本发明的有益效果：
[0115]针对现有研究利用线侧距离定位车辆相对车道线的侧向位置时，存在的因车道匹配错误或者车道左右边界点匹配错误造成定位误差的问题，本发明首先提出一种稳健的换道识别算法，结合该算法设计了一种改进的多指标加权评价地图匹配算法和车道左右边界点确定方法；在利用匹配得到车辆所在准确车道和左右边界点的基础上，提出一种基于摄像头的辅助定位方法，提升车辆相对于车道的侧向精度；本发明不需要激光雷达等高成本设备，且与现有低成本融合定位技术相比，定位的精度更高，稳定性更强，为自动驾驶定位提供了一种低成本、高精度的解决方案。
附图说明
[0116]图1车辆行驶时车辆所在车道以及车辆所在的左右车道线边界点示意图；车辆后轴中心在左车道线的投影位于
和
之间，简称为左边界点，左边界点连线简称左边界线；同理，右边界点为
和
右边界点连线简称右边界线；
[0117]图2为本发明整体框架示意图；
[0118]图3为本发明两轮里程计模型示意图；
[0119]图4为本发明换道识别流程示意图；
[0120]图5为本发明实车实验换道过程的横摆角速度及相对车道的侧向位移变化示意图；(a)左换道(b)右换道；
[0121]图6为本发明道路-路口匹配方法流程示意图；
[0122]图7为本发明车道左右边界点确定方法流程示意图；
[0123]图8为本发明基于摄像头的辅助定位示意图；
[0124]图9为实验验证中道路上行驶侧向误差对比示意图；(a)本发明方法侧向误差(b)文献方法侧向误差。
具体实施方式
[0125]车辆行驶时车辆所在车道以及车辆所在的左右车道线边界点如图1所示，车辆后轴中心在左车道线的投影位于
和
之间，将此两点简称为左边界点，左边界点连线简称左边界线；同理，右边界点为
和
右边界点连线简称右边界线。
[0126]如图2所示，本发明提供一种基于多传感融合的低成本高精定位方法，包括以下步骤：
[0127]步骤1、创建地图模型：
[0128]本发明建立一种轻量化的车道级地图模型，单条车道l表示为：
[0129]l＝{id,pre,suc,pc,pl,pr,q} (1)
[0130]其中id表示车道l的序号，从靠近道路中心线开始，左侧序号从1开始依次递增，右侧序号从-1开始依次递减；pre表示与车道起点相连的交叉路口或车道；suc定义为与车道末端连接的交叉路口或车道；pc,pl和pr分别代表车道中心线、左车道线和右车道线的点集；q包含车道对应的属性，包括宽度、限速、左右车道线类型；
[0131]其中pl和pr表示为：
[0132]
[0133]
[0134]步骤2、基于GPS、轮式里程计和底盘信号的融合：
[0135](1)本步骤采用一种基于扩展卡尔曼滤波EKF对GPS、轮式里程计和底盘信号进行融合的方法，利用车轮里程计相关测量值对状态变量进行修正；所述的状态变量表示如下：
[0136]x＝[px,py,θ,ω]T (4)
[0137]其中px和py为ENU坐标系下车辆的横纵坐标，θ为ENU坐标系下车辆的航向角，ω为车辆横摆角速度；
[0138]状态转移方程为：
[0139]x'＝Fx+Bu+λ (5)
[0140]此处，控制输入Bu为0，λ为过程噪声；
[0141]本发明使用两轮里程计模型，如图3所示，设车辆的质心侧偏角为β，状态转移方程具体如下：
[0142]
[0143]
[0144]θk＝θk-1+ωk-1T (8)
[0145]ωk＝ωk-1 (9)
[0146]在正常驾驶和天气良好条件下，车轮打滑忽略不计，速度计算如下：
[0147]
[0148]其中nRL和nRR为每秒左右车轮转数，cRL和cRR是实际的左右车轮周长；横摆角速度ω由底盘ESC提供；
[0149]状态转移矩阵F表示为：
[0150]
[0151]当GPS信号良好时，利用GPS系统建立的观测矩阵为：
[0152]
[0153]利用轮式里程计信号建立的横摆角速度观测值如下：
[0154]
[0155]其中tR是后轴轮距，对应的观测矩阵为：
[0156]H＝(0 0 0 1) (14)；
[0157](2)挖掘车辆底盘的StandStill信号来进行运动学约束，包括零速修正、零角速度约束；当StandStill为0时表示车辆处于行驶状态，为1时表示车辆处于静止，由于这个信号很准确，可以进行状态变量的约束，消除停车时噪声带来的积分误差。
[0158]当StandStill为1时，表示车辆处于静止状态，车速和横摆角速度均为0，可得：
[0159]px,k＝px,k-1 (15)
[0160]py,k＝py,k-1 (16)
[0161]θk＝θk-1 (17)
[0162]ωk＝0 (18)
[0163]此时F为：
[0164]
[0165]步骤3、换道识别：
[0166]通常，交通规则仅允许单次变道时车辆转移到相邻车道，所以本发明不考虑单次横跨多条车道的情况。
[0167]本发明提出采用一种鲁棒的换道识别方法，如图4所示，通过建立多重准则来进行换道识别，当能识别到车道线且车道线质量较好时，直接根据C0判断准则判断是否换道；当能识别到车道线但车道线质量较差时或者识别不到车道线时，则采用横摆角速度、航向角、变道时长多个准则进行综合判断，保证车辆在车道线遮挡或者磨损情况下能够识别出是否换道；
[0168]所述的C0判断准则如下：
[0169]假设摄像头坐标系原点已标定在车辆后轴中心，C0代表线侧距离，车辆实际后轴中心位置到左右车道线的线侧距离分别为
和
左侧车道线的C0值
大于0，右侧车道线的C0值
小于0；
[0170]通过实车实验观察分析可知，不管向左还是向右换道跨越车道线时，
逐渐减小接近于0，
增大接近于0；设t时刻的左右车道线C0值分别为
和
判断变量ξ，此处ξ取值为0.3米；则换道判断条件为：
[0171]
[0172]在识别到换道状态之后启动换道结束状态检测，设定C0值的变化量阈值
这里取为0.1，当满足条件(21)时，判断已结束换道；
[0173]
[0174]当不满足C0判断准则时，使用横摆角速度、航向角、变道时长判断准则进行综合判断；
[0175]横摆角速度判断准则如下：
[0176]该准则利用横摆角速度的变化识别车辆是否换道。我们进行了实车换道实验，车辆所处车道位于正东方向，因此X坐标沿车道方向，Y坐标沿车道侧向，Y坐标的变化能表现车辆相对车道的侧向位移的变化，图5描绘了左换道和右换道过程中Y轴轨迹和横摆角速度的变化，从图中可以看出，车辆正在换道时，Y轴坐标发生较大变化，整个换道过程中横摆角速度变化曲线经历一个波峰和波谷，对于左换道，先有波峰再有波谷；右换道则先有波谷再有波峰。因此，可以通过在预定义的窗口长度M上观察横摆角速度的变化，来识别换道。
[0177]航向角判断准则如下：
[0178]如果满足横摆角速度判断准则，则可能发生换道，也可能在弯道上行驶；使用比窗口长度M稍长的滑动窗口检测时间段内航向角始末变化量δ＝|θend-θstart|，如果δ≈0，则车辆在直道上变道；否则沿弯道行驶；
[0179]变道时长判断准则如下：
[0180]设完整的变道时长为TLC,若横摆角速度从波峰到波谷的时间大于TLC，则判断车辆沿弯道行驶而不是换道。
[0181]步骤4、地图匹配：
[0182]步骤4.1、道路-路口匹配：
[0183]在车辆行驶过程中，判断车辆三种位置状态：在路口、在道路、离开路口进入道路；当车辆在道路时，进行道路内车道匹配确定所在车道；当车辆从路口离开进入道路时，及时启用摄像头辅助定位方法；
[0184]如图6所示，道路-路口匹配的流程如下：
[0185]S01、开始；
[0186]S02、点对点匹配，找到最近点为匹配点；
[0187]S03、判断此刻匹配点是否在路口内，如果是则进入S04，否则进入S05；
[0188]S04、如果当前摄像头没有识别出车道线，则判断此刻车辆在路口内，并进入S08，否则进入S05；
[0189]S05、判断车辆是否上一时刻在路口，此刻不在路口，如果是则进入S06，否则进行道路匹配，并进入S08；
[0190]S06、判断当前匹配点所在道路是否为路口的后续道路，如果是则进入步骤S07，否则进入S08；
[0191]S07、判断当前摄像头是否能识别出车道线，如果是则判断此刻车辆离开路口进入道路，并进入S08，否则进入S08；
[0192]S08、结束匹配。
[0193]步骤4.2、道路内车道匹配：
[0194]在道路内行驶时，换道靠近车道线时由于定位误差会出现来回匹配到本车道和相邻车道的结果。结合提出的换道识别方法，本发明提出一种改进的多指标加权评价地图匹配方法，提高道路内车道匹配的准确性和鲁棒性。
[0195]当识别出在道路内行驶时，首先以车辆当前位置为中心，搜索一定范围内的候选车道，然后利用数个评价函数对候选车道进行综合评价；计算各个候选车道的综合评分，选定综合评分最高的车道为匹配结果；
[0196]所述的数个评价函数包括：
[0197](1)基于距离的评价，设评价函数为：
[0198]
[0199]其中d为定位坐标至候选车道中心线的距离；
[0200](2)基于拓扑关系的评价：设评价函数为F(link)，如果当前匹配车道与上一时刻匹配车道具有拓扑上的连接关系或者为同一车道，则F(link)＝1，否则F(link)＝0；
[0201](3)基于航向的评价：设当前车道中心线行驶方向航向角为θ1，车辆航向角为θ2，考虑到车辆航向角及弯道的中心线方向是变化的，且车辆行驶时并不一定与车道平齐，构建的评价函数为：
[0202]
[0203](4)基于车道线类型的评价：设摄像头识别的车道线总数为M，将摄像头识别的车道线类型与地图上对应位置的车道线类型相比较，类型相同的车道线数目记为N，考虑到定位的不确定性及同一车道可能存在的车道线类型的变化，使用系数弱化基于车道线类型的评价，评价函数为：
[0204]
[0205](5)基于换道识别的评价：设评价函数为F(lc)，以上一时刻所在车道lcur及左右相邻车道lleft和lright三条车道为例，检测到换道时：
[0206]
[0207]综合评分为：
[0208]Fsum＝F(link)gF(θ)g(F(d)+F(lanetype)+F(lc)) (26)。
[0209]计算各个候选车道的综合评分，选定综合评分最高的车道为匹配结果。
[0210]步骤4.3、确定左右边界点：
[0211]除了将车辆位置匹配到地图对应的正确车道上，为了有效利用车道线进行辅助定位，地图匹配需要确定左右边界点，如图7所示，车道左右边界点确定方法如下：
[0212]当车辆离开路口进入道路，以当前车道起始点作为车辆的行驶起点，并根据行驶起点确定车辆所在的初始左右边界点；在车辆行驶过程中进行换道识别，若没有发生换道，则根据轮式里程计的车轮转数推算车辆走过距离与车道线的各离散点间距进行比较，结合初始边界点对边界点进行更新；当发生换道时，根据道路匹配方法重新匹配车道，然后在新车道上重新匹配车辆行驶起点和初始左右边界点，继续沿车道行驶时更新左右边界点的步骤与不换道时相同；
[0213]步骤5、基于摄像头的辅助定位：
[0214]如图8所示，在得到车辆所在车道和左右边界点基础上进行基于摄像头的辅助定位；
[0215]建立车道线模型：
[0216]y＝C3x3+C2x2+C1x+C0 (27)
[0217]其中C0、C1、C2和C3分别代表线侧距离、坡度、曲率和曲率导数；
[0218]设车辆的后轴中心为(xM,yM)，以它为车辆坐标系原点，横轴指向车辆前方，纵轴指向车辆左侧；摄像头坐标系RC的原点为点C(xC,yC)，横纵轴方向与车辆坐标系相同；摄像头坐标系RC的原点为点C(xC,yC)标定在车辆后轴中心pM(xM,yM),车辆的左边界点为
和
右边界点为
和
过后轴中心pM(xM,yM)，方向与航向角θ方向垂直的直线与左边界线和右边界线交点分别为pL(xL,yL)、pR(xR,yR)；车辆定位坐标到左右边界线距离分别为dl和dr，设车辆实际后轴中心位置为preal，摄像头测得的车辆相对于左右边界线的线侧距离分别为
和
[0219]从后轴中心pM出发，方向与航向角θ方向垂直的直线方程表示为：
[0220]
[0221]求得交点坐标pL和pR后，dl和dr表示为：
[0222]
[0223]
[0224]利用左边界线定位后的车辆后轴中心坐标为(xML,yML)：
[0225]
[0226]利用右边界线定位后的车辆后轴中心坐标为(xMR,yMR)：
[0227]
[0228]使用左右边界线定位的平均值作为最后的定位坐标(xM',yM')，并且考虑到线侧距离具有一定的误差，误差接近于一个常值，设为Δdcam，最终定位坐标见式(33)：
[0229]
[0230]实验验证：
[0231]本发明使用SCANeR studio构建逼真的虚拟驾驶场景；考虑到摄像头C0观测值在实际中偏差较稳定且较小，设置该观测值偏差的均值为0.2米，标准差分别为0.05米,同时Δdcam设置为0.2米。通过搭建四条道路交汇于十字路口的驾驶场景，参照实车配备的传感器参数，设置仿真中的参数见表1，后续仿真实验参数均按照表1进行设置。
[0232]表1.仿真中的参数设置
[0233]
[0234]在两条道路上均进行了换道到相邻车道然后再换回原车道的双换道，中间右转行驶过路口，前一条道路沿东西方向，后一条道路沿南北方向。我们同时运行文献《Road-Centered Map-Aided Localization for Driverless Cars UsingSingle-Frequency GNSS Receivers》中方法与本发明方法，保证两种方法接收的传感器输入和车辆状态信号输入完全一致，然后将两种方法输出的定位结果进行对比。
[0235]图9对比了两种方法在两条道路上行驶时相对车道的侧向误差。文献方法侧向误差存在较大波动，且在换道时会产生1米以上较大的侧向误差，相比而言，本发明方法在大部分时候误差较小接近于0，偶尔会出现波动，最大侧向误差也是接近于1米，因此，相比于文献，本发明方法在侧向定位方面精度更高。
[0236]文献提出方法与本发明方法的定位结果对比如表2所示。
[0237]从表2中可以看出，本发明方法与文献提出方法相比，相对车道的侧向精度，整体精度，和航向角精度三个指标的均值和标准差均更小，即在这三方面的定位精度都有提升，且更加稳定，更加符合无人驾驶对定位系统的要求。
[0238]表2.定位结果对比
[0239]
[0240]
[0029]
[0030]进一步的，步骤2(1)中，本步骤采用一种基于扩展卡尔曼滤波EKF对GPS、轮式里程计和底盘信号进行融合的方法，利用车轮里程计相关测量值对状态变量进行修正；所述的状态变量表示如下：
[0031]x＝[px,py,θ,ω]T (4)
[0032]其中px和py为ENU坐标系下车辆的横纵坐标，θ为ENU坐标系下车辆的航向角，ω为车辆横摆角速度；
[0033]状态转移方程为：
[0034]x'＝Fx+Bu+λ (5)
[0035]此处，控制输入Bu为0，λ为过程噪声；
[0036]本发明使用两轮里程计模型，设车辆的质心侧偏角为β，状态转移方程具体如下：
[0037]
[0038]
[0039]θk＝θk-1+ωk-1T (8)
[0040]ωk＝ωk-1 (9)
[0041]在正常驾驶和天气良好条件下，车轮打滑忽略不计，速度计算如下：
[0042]
[0043]其中nRL和nRR为每秒左右车轮转数，cRL和cRR是实际的左右车轮周长；横摆角速度ω由底盘ESC提供；
[0044]状态转移矩阵F表示为：
[0045]
[0046]当GPS信号良好时，利用GPS系统建立的观测矩阵为：
[0047]
[0048]利用轮式里程计信号建立的横摆角速度观测值如下：
[0049]
[0050]其中tR是后轴轮距，对应的观测矩阵为：
[0051]H＝(0 0 0 1) (14)；
[0052]步骤2(2)中，挖掘车辆底盘的StandStill信号来进行运动学约束，包括零速修正、零角速度约束，当StandStill为0时表示车辆处于行驶状态，为1时表示车辆处于静止，由于这个信号很准确，可以进行状态变量的约束，消除停车时噪声带来的积分误差。
[0053]当StandStill为1时，表示车辆处于静止状态，车速和横摆角速度均为0，可得：
[0054]px,k＝px,k-1 (15)
[0055]py,k＝py,k-1 (16)
[0056]θk＝θk-1 (17)
[0057]ωk＝0 (18)
[0058]此时F为：
[0059]
[0060]进一步的，步骤3中，本发明提出一种鲁棒的换道识别方法：通过建立多重准则来进行换道识别，当能识别到车道线且车道线质量较好时，直接根据C0判断准则判断是否换道；当能识别到车道线但车道线质量较差时或者识别不到车道线时，采用横摆角速度、航向角、变道时长多个准则进行综合判断，这样能保证在车道线遮挡或者磨损情况下都能识别到换道。
[0061]所述的C0判断准则如下：
[0062]假设摄像头坐标系原点已标定在车辆后轴中心，C0代表线侧距离，车辆实际后轴中心位置到左右车道线的线侧距离分别为和左侧车道线的C0值大于0，右侧车道线的C0值小于0；
[0063]不管向左还是向右换道跨越车道线时，逐渐减小接近于0，增大接近于0；设t时刻的左右车道线C0值分别为和判断变量ξ；则换道判断条件为：
[0064]
[0065]在识别到换道状态之后启动换道结束状态检测，设定C0值的变化量阈值当满足条件(21)时，判断已结束换道；
[0066]
[0067]当不满足C0判断准则时，使用横摆角速度、航向角、变道时长判断准则进行综合判断；
[0068]横摆角速度判断准则如下：
[0069]利用横摆角速度的变化识别车辆是否换道，通过在预定义的窗口长度M上观察横摆角速度的变化来识别换道：左换道和右换道过程中，横摆角速度变化曲线经历一个波峰和波谷，对于左换道，先有波峰再有波谷；右换道则先有波谷再有波峰；
[0070]航向角判断准则如下：
[0071]如果满足横摆角速度判断准则，则可能发生换道，也可能在弯道上行驶；使用比窗口长度M稍长的滑动窗口检测时间段内航向角始末变化量δ＝|θend-θstart|，如果δ≈0，则车辆在直道上变道；否则沿弯道行驶；
[0072]变道时长判断准则如下：
[0073]设完整的变道时长为TLC,若横摆角速度从波峰到波谷的时间大于TLC，则判断车辆沿弯道行驶而不是换道。
[0074]进一步的，步骤4.1中，道路-路口匹配的流程如下：
[0075]S01、开始；
[0076]S02、点对点匹配，找到最近点为匹配点；
[0077]S03、判断此刻匹配点是否在路口内，如果是则进入S04，否则进入S05；
[0078]S04、如果当前摄像头没有识别出车道线，则判断此刻车辆在路口内，并进入S08，否则进入S05；
[0079]S05、判断车辆是否上一时刻在路口，此刻不在路口，如果是则进入S06，否则进行道路内车道匹配，并进入S08；
[0080]S06、判断当前匹配点所在道路是否为路口的后续道路，如果是则进入步骤S07，否则进入S08；
[0081]S07、判断当前摄像头是否能识别出车道线，如果是则判断此刻车辆离开路口进入道路，并进入S08，否则进入S08；
[0082]S08、结束匹配。
[0083]进一步的，步骤4.2道路内车道匹配过程中，在道路内行驶时，换道靠近车道线时由于定位误差会出现来回匹配到本车道和相邻车道的结果。结合提出的换道识别方法，本发明提出一种改进的多指标加权评价地图匹配方法，提高道路内车道匹配的准确性和鲁棒性。
[0084]所述的数个评价函数包括：
[0085](1)基于距离的评价，设评价函数为：
[0086]
[0087]其中d为定位坐标至候选车道中心线的距离；
[0088](2)基于拓扑关系的评价：设评价函数为F(link)，如果当前匹配车道与上一时刻匹配车道具有拓扑上的连接关系或者为同一车道，则F(link)＝1，否则F(link)＝0；
[0089](3)基于航向的评价：设候选车道中心线航向角为θ1，车辆航向角为θ2，考虑到车辆航向角及弯道的中心线方向是变化的，且车辆行驶时并不一定与车道平齐，构建的评价函数为：
[0090]
[0091](4)基于车道线类型的评价：设摄像头识别的车道线总数为M，将摄像头识别的车道线类型与地图上对应位置的车道线类型相比较，类型相同的车道线数目记为N，考虑到定位的不确定性及同一车道可能存在的车道线类型的变化，使用系数弱化基于车道线类型的评价，评价函数为：
[0092]
[0093](5)基于换道识别的评价：设评价函数为F(lc)，以上一时刻所在车道lcur及左右相邻车道lleft和lright三条车道为例，检测到换道时：
[0094]
[0095]综合评分为：
[0096]Fsum＝F(link)gF(θ)g(F(d)+F(lanetype)+F(lc)) (26)。
[0097]计算各个候选车道的综合评分，选定综合评分最高的车道为匹配结果。
[0098]进一步的，步骤5基于摄像头的辅助定位中，车道线模型为：
[0099]y＝C3x3+C2x2+C1x+C0 (27)
[0100]其中C0、C1、C2和C3分别代表线侧距离、坡度、曲率和曲率导数；
[0101]设车辆的后轴中心为(xM,yM)，以它为车辆坐标系原点，横轴指向车辆前方，纵轴指向车辆左侧；摄像头坐标系RC的原点为点C(xC,yC)，横纵轴方向与车辆坐标系相同；摄像头坐标系RC的原点为点C(xC,yC)标定在车辆后轴中心pM(xM,yM),车辆的左边界点为和右边界点为和过后轴中心pM(xM,yM)，方向与航向角θ方向垂直的直线与左边界线和右边界线交点分别为pL(xL,yL)、pR(xR,yR)；车辆定位坐标到左右边界线距离分别为dl和dr，设车辆实际后轴中心位置为preal，摄像头测得的车辆相对于左右边界线的线侧距离分别为和
[0102]从后轴中心pM出发，方向与航向角θ方向垂直的直线方程表示为：
[0103]
[0104]求得交点坐标pL和pR后，dl和dr表示为：
[0105]
[0106]
[0107]利用左边界线定位后的车辆后轴中心坐标为(xML,yML)：
[0108]
[0109]利用右边界线定位后的车辆后轴中心坐标为(xMR,yMR)：
[0110]
[0111]使用左右车道线定位的平均值作为最后的定位坐标(xM',yM')；
[0112]考虑到线侧距离具有一定的误差，误差接近于一个常值，设为Δdcam，最终定位坐标见式(33)：
[0113]
[0114]本发明的有益效果：
[0115]针对现有研究利用线侧距离定位车辆相对车道线的侧向位置时，存在的因车道匹配错误或者车道左右边界点匹配错误造成定位误差的问题，本发明首先提出一种稳健的换道识别算法，结合该算法设计了一种改进的多指标加权评价地图匹配算法和车道左右边界点确定方法；在利用匹配得到车辆所在准确车道和左右边界点的基础上，提出一种基于摄像头的辅助定位方法，提升车辆相对于车道的侧向精度；本发明不需要激光雷达等高成本设备，且与现有低成本融合定位技术相比，定位的精度更高，稳定性更强，为自动驾驶定位提供了一种低成本、高精度的解决方案。
附图说明
[0116]图1车辆行驶时车辆所在车道以及车辆所在的左右车道线边界点示意图；车辆后轴中心在左车道线的投影位于和之间，简称为左边界点，左边界点连线简称左边界线；同理，右边界点为和右边界点连线简称右边界线；
[0117]图2为本发明整体框架示意图；
[0118]图3为本发明两轮里程计模型示意图；
[0119]图4为本发明换道识别流程示意图；
[0120]图5为本发明实车实验换道过程的横摆角速度及相对车道的侧向位移变化示意图；(a)左换道(b)右换道；
[0121]图6为本发明道路-路口匹配方法流程示意图；
[0122]图7为本发明车道左右边界点确定方法流程示意图；
[0123]图8为本发明基于摄像头的辅助定位示意图；
[0124]图9为实验验证中道路上行驶侧向误差对比示意图；(a)本发明方法侧向误差(b)文献方法侧向误差。
具体实施方式
[0125]车辆行驶时车辆所在车道以及车辆所在的左右车道线边界点如图1所示，车辆后轴中心在左车道线的投影位于和之间，将此两点简称为左边界点，左边界点连线简称左边界线；同理，右边界点为和右边界点连线简称右边界线。
[0126]如图2所示，本发明提供一种基于多传感融合的低成本高精定位方法，包括以下步骤：
[0127]步骤1、创建地图模型：
[0128]本发明建立一种轻量化的车道级地图模型，单条车道l表示为：
[0129]l＝{id,pre,suc,pc,pl,pr,q} (1)
[0130]其中id表示车道l的序号，从靠近道路中心线开始，左侧序号从1开始依次递增，右侧序号从-1开始依次递减；pre表示与车道起点相连的交叉路口或车道；suc定义为与车道末端连接的交叉路口或车道；pc,pl和pr分别代表车道中心线、左车道线和右车道线的点集；q包含车道对应的属性，包括宽度、限速、左右车道线类型；
[0131]其中pl和pr表示为：
[0132]
[0133]
[0134]步骤2、基于GPS、轮式里程计和底盘信号的融合：
[0135](1)本步骤采用一种基于扩展卡尔曼滤波EKF对GPS、轮式里程计和底盘信号进行融合的方法，利用车轮里程计相关测量值对状态变量进行修正；所述的状态变量表示如下：
[0136]x＝[px,py,θ,ω]T (4)
[0137]其中px和py为ENU坐标系下车辆的横纵坐标，θ为ENU坐标系下车辆的航向角，ω为车辆横摆角速度；
[0138]状态转移方程为：
[0139]x'＝Fx+Bu+λ (5)
[0140]此处，控制输入Bu为0，λ为过程噪声；
[0141]本发明使用两轮里程计模型，如图3所示，设车辆的质心侧偏角为β，状态转移方程具体如下：
[0142]
[0143]
[0144]θk＝θk-1+ωk-1T (8)
[0145]ωk＝ωk-1 (9)
[0146]在正常驾驶和天气良好条件下，车轮打滑忽略不计，速度计算如下：
[0147]
[0148]其中nRL和nRR为每秒左右车轮转数，cRL和cRR是实际的左右车轮周长；横摆角速度ω由底盘ESC提供；
[0149]状态转移矩阵F表示为：
[0150]
[0151]当GPS信号良好时，利用GPS系统建立的观测矩阵为：
[0152]
[0153]利用轮式里程计信号建立的横摆角速度观测值如下：
[0154]
[0155]其中tR是后轴轮距，对应的观测矩阵为：
[0156]H＝(0 0 0 1) (14)；
[0157](2)挖掘车辆底盘的StandStill信号来进行运动学约束，包括零速修正、零角速度约束；当StandStill为0时表示车辆处于行驶状态，为1时表示车辆处于静止，由于这个信号很准确，可以进行状态变量的约束，消除停车时噪声带来的积分误差。
[0158]当StandStill为1时，表示车辆处于静止状态，车速和横摆角速度均为0，可得：
[0159]px,k＝px,k-1 (15)
[0160]py,k＝py,k-1 (16)
[0161]θk＝θk-1 (17)
[0162]ωk＝0 (18)
[0163]此时F为：
[0164]
[0165]步骤3、换道识别：
[0166]通常，交通规则仅允许单次变道时车辆转移到相邻车道，所以本发明不考虑单次横跨多条车道的情况。
[0167]本发明提出采用一种鲁棒的换道识别方法，如图4所示，通过建立多重准则来进行换道识别，当能识别到车道线且车道线质量较好时，直接根据C0判断准则判断是否换道；当能识别到车道线但车道线质量较差时或者识别不到车道线时，则采用横摆角速度、航向角、变道时长多个准则进行综合判断，保证车辆在车道线遮挡或者磨损情况下能够识别出是否换道；
[0168]所述的C0判断准则如下：
[0169]假设摄像头坐标系原点已标定在车辆后轴中心，C0代表线侧距离，车辆实际后轴中心位置到左右车道线的线侧距离分别为和左侧车道线的C0值大于0，右侧车道线的C0值小于0；
[0170]通过实车实验观察分析可知，不管向左还是向右换道跨越车道线时，逐渐减小接近于0，增大接近于0；设t时刻的左右车道线C0值分别为和判断变量ξ，此处ξ取值为0.3米；则换道判断条件为：
[0171]
[0172]在识别到换道状态之后启动换道结束状态检测，设定C0值的变化量阈值这里取为0.1，当满足条件(21)时，判断已结束换道；
[0173]
[0174]当不满足C0判断准则时，使用横摆角速度、航向角、变道时长判断准则进行综合判断；
[0175]横摆角速度判断准则如下：
[0176]该准则利用横摆角速度的变化识别车辆是否换道。我们进行了实车换道实验，车辆所处车道位于正东方向，因此X坐标沿车道方向，Y坐标沿车道侧向，Y坐标的变化能表现车辆相对车道的侧向位移的变化，图5描绘了左换道和右换道过程中Y轴轨迹和横摆角速度的变化，从图中可以看出，车辆正在换道时，Y轴坐标发生较大变化，整个换道过程中横摆角速度变化曲线经历一个波峰和波谷，对于左换道，先有波峰再有波谷；右换道则先有波谷再有波峰。因此，可以通过在预定义的窗口长度M上观察横摆角速度的变化，来识别换道。
[0177]航向角判断准则如下：
[0178]如果满足横摆角速度判断准则，则可能发生换道，也可能在弯道上行驶；使用比窗口长度M稍长的滑动窗口检测时间段内航向角始末变化量δ＝|θend-θstart|，如果δ≈0，则车辆在直道上变道；否则沿弯道行驶；
[0179]变道时长判断准则如下：
[0180]设完整的变道时长为TLC,若横摆角速度从波峰到波谷的时间大于TLC，则判断车辆沿弯道行驶而不是换道。
[0181]步骤4、地图匹配：
[0182]步骤4.1、道路-路口匹配：
[0183]在车辆行驶过程中，判断车辆三种位置状态：在路口、在道路、离开路口进入道路；当车辆在道路时，进行道路内车道匹配确定所在车道；当车辆从路口离开进入道路时，及时启用摄像头辅助定位方法；
[0184]如图6所示，道路-路口匹配的流程如下：
[0185]S01、开始；
[0186]S02、点对点匹配，找到最近点为匹配点；
[0187]S03、判断此刻匹配点是否在路口内，如果是则进入S04，否则进入S05；
[0188]S04、如果当前摄像头没有识别出车道线，则判断此刻车辆在路口内，并进入S08，否则进入S05；
[0189]S05、判断车辆是否上一时刻在路口，此刻不在路口，如果是则进入S06，否则进行道路匹配，并进入S08；
[0190]S06、判断当前匹配点所在道路是否为路口的后续道路，如果是则进入步骤S07，否则进入S08；
[0191]S07、判断当前摄像头是否能识别出车道线，如果是则判断此刻车辆离开路口进入道路，并进入S08，否则进入S08；
[0192]S08、结束匹配。
[0193]步骤4.2、道路内车道匹配：
[0194]在道路内行驶时，换道靠近车道线时由于定位误差会出现来回匹配到本车道和相邻车道的结果。结合提出的换道识别方法，本发明提出一种改进的多指标加权评价地图匹配方法，提高道路内车道匹配的准确性和鲁棒性。
[0195]当识别出在道路内行驶时，首先以车辆当前位置为中心，搜索一定范围内的候选车道，然后利用数个评价函数对候选车道进行综合评价；计算各个候选车道的综合评分，选定综合评分最高的车道为匹配结果；
[0196]所述的数个评价函数包括：
[0197](1)基于距离的评价，设评价函数为：
[0198]
[0199]其中d为定位坐标至候选车道中心线的距离；
[0200](2)基于拓扑关系的评价：设评价函数为F(link)，如果当前匹配车道与上一时刻匹配车道具有拓扑上的连接关系或者为同一车道，则F(link)＝1，否则F(link)＝0；
[0201](3)基于航向的评价：设当前车道中心线行驶方向航向角为θ1，车辆航向角为θ2，考虑到车辆航向角及弯道的中心线方向是变化的，且车辆行驶时并不一定与车道平齐，构建的评价函数为：
[0202]
[0203](4)基于车道线类型的评价：设摄像头识别的车道线总数为M，将摄像头识别的车道线类型与地图上对应位置的车道线类型相比较，类型相同的车道线数目记为N，考虑到定位的不确定性及同一车道可能存在的车道线类型的变化，使用系数弱化基于车道线类型的评价，评价函数为：
[0204]
[0205](5)基于换道识别的评价：设评价函数为F(lc)，以上一时刻所在车道lcur及左右相邻车道lleft和lright三条车道为例，检测到换道时：
[0206]
[0207]综合评分为：
[0208]Fsum＝F(link)gF(θ)g(F(d)+F(lanetype)+F(lc)) (26)。
[0209]计算各个候选车道的综合评分，选定综合评分最高的车道为匹配结果。
[0210]步骤4.3、确定左右边界点：
[0211]除了将车辆位置匹配到地图对应的正确车道上，为了有效利用车道线进行辅助定位，地图匹配需要确定左右边界点，如图7所示，车道左右边界点确定方法如下：
[0212]当车辆离开路口进入道路，以当前车道起始点作为车辆的行驶起点，并根据行驶起点确定车辆所在的初始左右边界点；在车辆行驶过程中进行换道识别，若没有发生换道，则根据轮式里程计的车轮转数推算车辆走过距离与车道线的各离散点间距进行比较，结合初始边界点对边界点进行更新；当发生换道时，根据道路匹配方法重新匹配车道，然后在新车道上重新匹配车辆行驶起点和初始左右边界点，继续沿车道行驶时更新左右边界点的步骤与不换道时相同；
[0213]步骤5、基于摄像头的辅助定位：
[0214]如图8所示，在得到车辆所在车道和左右边界点基础上进行基于摄像头的辅助定位；
[0215]建立车道线模型：
[0216]y＝C3x3+C2x2+C1x+C0 (27)
[0217]其中C0、C1、C2和C3分别代表线侧距离、坡度、曲率和曲率导数；
[0218]设车辆的后轴中心为(xM,yM)，以它为车辆坐标系原点，横轴指向车辆前方，纵轴指向车辆左侧；摄像头坐标系RC的原点为点C(xC,yC)，横纵轴方向与车辆坐标系相同；摄像头坐标系RC的原点为点C(xC,yC)标定在车辆后轴中心pM(xM,yM),车辆的左边界点为和右边界点为和过后轴中心pM(xM,yM)，方向与航向角θ方向垂直的直线与左边界线和右边界线交点分别为pL(xL,yL)、pR(xR,yR)；车辆定位坐标到左右边界线距离分别为dl和dr，设车辆实际后轴中心位置为preal，摄像头测得的车辆相对于左右边界线的线侧距离分别为和
[0219]从后轴中心pM出发，方向与航向角θ方向垂直的直线方程表示为：
[0220]
[0221]求得交点坐标pL和pR后，dl和dr表示为：
[0222]
[0223]
[0224]利用左边界线定位后的车辆后轴中心坐标为(xML,yML)：
[0225]
[0226]利用右边界线定位后的车辆后轴中心坐标为(xMR,yMR)：
[0227]
[0228]使用左右边界线定位的平均值作为最后的定位坐标(xM',yM')，并且考虑到线侧距离具有一定的误差，误差接近于一个常值，设为Δdcam，最终定位坐标见式(33)：
[0229]
[0230]实验验证：
[0231]本发明使用SCANeR studio构建逼真的虚拟驾驶场景；考虑到摄像头C0观测值在实际中偏差较稳定且较小，设置该观测值偏差的均值为0.2米，标准差分别为0.05米,同时Δdcam设置为0.2米。通过搭建四条道路交汇于十字路口的驾驶场景，参照实车配备的传感器参数，设置仿真中的参数见表1，后续仿真实验参数均按照表1进行设置。
[0232]表1.仿真中的参数设置
[0233]
[0234]在两条道路上均进行了换道到相邻车道然后再换回原车道的双换道，中间右转行驶过路口，前一条道路沿东西方向，后一条道路沿南北方向。我们同时运行文献《Road-Centered Map-Aided Localization for Driverless Cars UsingSingle-Frequency GNSS Receivers》中方法与本发明方法，保证两种方法接收的传感器输入和车辆状态信号输入完全一致，然后将两种方法输出的定位结果进行对比。
[0235]图9对比了两种方法在两条道路上行驶时相对车道的侧向误差。文献方法侧向误差存在较大波动，且在换道时会产生1米以上较大的侧向误差，相比而言，本发明方法在大部分时候误差较小接近于0，偶尔会出现波动，最大侧向误差也是接近于1米，因此，相比于文献，本发明方法在侧向定位方面精度更高。
[0236]文献提出方法与本发明方法的定位结果对比如表2所示。
[0237]从表2中可以看出，本发明方法与文献提出方法相比，相对车道的侧向精度，整体精度，和航向角精度三个指标的均值和标准差均更小，即在这三方面的定位精度都有提升，且更加稳定，更加符合无人驾驶对定位系统的要求。
[0238]表2.定位结果对比
[0239]
[0240]
