1.本发明属于自动驾驶融合定位技术领域，具体涉及一种车道级融合定位方法及系统。
背景技术：2.定位是实现自动驾驶的关键技术。结合高精度地图的定位使自动驾驶汽车获取了超感知的能力，极大地提升了自动驾驶的安全性和效率。在自动驾驶感知融合技术中，定位融合是实现车辆自动控制的必要过程。目前自动驾驶车辆的定位方案主要有组合惯导(gnss和ins)定位、激光雷达定位、视觉定位及融合不同设备的组合定位。传统的单一传感器或简单组合定位方案往往精度不高且鲁棒性不好。融合定位，如公开号为cn106767853a(申请号201611261781.5)的专利，一种基于多信息融合的无人驾驶车辆高精度定位方法，提出基于视觉、gnss/ins、激光雷达及高精度地图的融合定位方案虽然稳定性较好，定位精度也足够高，但价格高昂的激光雷达也使得此方案下的自动驾驶迟迟无法量产。
3.现有的定位方法主要存在以下缺陷：
4.1)依赖于单一传感器的定位方案或简单组合定位方案定位精度不高且鲁棒性不好；
5.2)结合高精度地图及激光雷达等多传感器融合的定位方案定位效果好，但成本较高，短期内难以应用到量产车型上。
技术实现要素：6.针对现有技术的上述不足，本发明要解决的技术问题是提供一种车道级融合定位方法及系统，避免为了达到控制所要求的定位性能而带来高昂成本的问题。
7.为解决上述技术问题，本发明采用如下技术方案：
8.一种车道级融合定位方法，具体包括以下步骤：
9.s1：初始定位，在定位初始化模块内采用三帧初始化定位以获得初始定位结果以及所述的一种车道级融合定位系统运行开始第一帧的局部高精度地图，并判断所述初始定位结果是否有效，如果有效，表示初始化成功并执行s2；如果无效，则持续进行初始化定位和判断直至初始化成功；
10.s2：航迹粗推算，基于所述初始定位结果，在航迹粗推算模块内利用本车轮速信息计算本车的粗定位位置，并通过所述粗定位位置获取每一帧本车周围的局部高精度地图；
11.s3：坐标转换，通过坐标转换模块将全局gps坐标转换为本车坐标系下的笛卡尔坐标；
12.s4：车道匹配，利用当前帧的局部高精度地图确定本车所处的车道，并将该局部高精度地图上与视觉车道线匹配的地图段的车道进行编号，得到本车所在车道的车道编号；
13.s5：车道级精配准定位，利用所述步骤s4获得的车道编号对所述步骤s2的当前帧的航迹粗推算所得的粗定位结果进行修正，输出修正后的定位结果，执行步骤s6；
14.s6：坐标逆转换，在坐标逆转换模块内将所述步骤s5得到的修正后的定位结果转换为全局的gps坐标并输出；同时将该gps坐标输入到航迹粗推算模块内以对所述步骤s2的粗定位结果进行校正。
15.进一步完善上述技术方案，所述步骤s1中，对连续三帧之间的定位结果的差距来判断所述初始定位结果是否有效，如果连续三帧的定位结果差别不大，认定为有效；如果差别过大，则认定为无效。
16.进一步地，所述步骤s2基于认定在航迹粗推算模块内前后两帧本车的位姿不会发现巨大的变化；
17.然后通过下式计算本车车速v、航向角速度ω及转弯半径r：
[0018][0019][0020][0021]
其中，vr为右后轮轮速，单位为m/s；v
l
为左后轮轮速，单位为m/s；l为后轮轮距；
[0022]
再通过下式计算当前时刻t本车的位姿：
[0023]
x
t
＝x
t-1
+δt*v*cos(θ
t
)
[0024]yt
＝y
t-1
+δt*v*sin(θ
t
)
[0025]
θ
t
＝θ
t-1
+δt*ω
[0026]
其中，x、y为位置,θ为航向角。
[0027]
进一步地，所述步骤s3中的全局gps坐标基于所述步骤s2所得的每一帧本车的粗定位结果来获取对应的局部高精度地图，该局部高精度地图内的车道线及车道中心线点即为所述全局gps坐标。
[0028]
进一步地，所述步骤s4还包括：
[0029]
s4.1：根据所述步骤s1和s2获得的当前帧的局部高精度地图，通过车道线数量比对获取本车在当前帧的局部高精度地图上所处的地段；
[0030]
s4.2：将当前帧的局部高精度地图上的本车两边车道线的横向距离、两边车道线类型、颜色与前视摄像头所给的对应量进行比较，从而确定本车在当前帧的局部高精度地图上所处的车道，完成车道匹配；
[0031]
s4.3：对所述步骤s4.2得到的车道进行编号，从左到右依次对车道进行编号，输出本车所在车道的车道编号n。
[0032]
进一步地，所述步骤s5中，
[0033]
通过所述车道编号n来获取当前帧局部高精度地图上本车道中心线距离本车最近的轨迹点，并将该轨迹点添加到计算队列q1中，同时将所述步骤s2计算所得的航迹粗推算的粗定位点添加到计算队列q2中；
[0034]
固定计算队列q1及q2的容量大小，每次q1和q2满时，删除最先进入计算队列的轨迹点和粗定位点，并将当前帧的点分别添加到计算队列的末尾；
[0035]
根据计算队列q1内的轨迹点和q2内的粗定位点进行计算，得到航迹粗推算的修正
量，再通过所述修正量对当前帧航迹粗推算的粗定位结果进行修正，即可得到修正后的定位结果。
[0036]
进一步地，所述计算队列q1及q2的容量大小均为10；
[0037]
即保存历史10帧局部高精度地图上本车道中心线距离本车最近的轨迹点，记为点集a；历史10帧的本车粗定位点，记为点集b；
[0038]
通过下式计算得到航迹粗推算的粗定位点的修正量：
[0039][0040][0041]
其中，θ为旋转量，t为平移量，为点集a的第i帧轨迹点的x坐标，为点集a的第i帧轨迹点的y坐标，为点集b的第i帧粗定位点的x坐标，为点集b的第i帧粗定位点的y坐标，为点集a的中心点的x坐标，为点集a的中心点的y坐标，为点集b的中心点的x坐标,为点集b的中心点的y坐标，r为旋转矩阵。
[0042]
进一步地，所述步骤s2中，若来自坐标逆转换模块的全局gps定位点有效，即可通过该全局gps定位点对航迹粗推算模块内的粗定位结果进行校正。
[0043]
本发明还涉及一种车道级融合定位系统，包括：
[0044]
定位初始化模块，用于自动驾驶功能开启后进行定位初始化，获取整个定位系统的初始结果以及运行开始时的局部高精度地图；
[0045]
航迹粗推算模块，用于在所述初始结果的基础上根据本车轮速计算本车的粗定位位置，并根据所述粗定位位置获取每个时刻本车周围的局部高精度地图；
[0046]
坐标转换模块，用于将全局的gps坐标转换为本车坐标系下的局部笛卡尔坐标；
[0047]
车道匹配模块，用于确定本车在每个时刻的局部高精度地图上所处的车道；
[0048]
车道级精配准定位模块，用于对当前帧的航迹粗推算所得的粗定位结果进行修正，获得最终的定位结果；
[0049]
坐标逆转换模块，用于将本车坐标系下的局部笛卡尔坐标转换为全局gps定位坐标，为坐标转换模块的逆操作。
[0050]
相比现有技术，本发明具有如下有益效果：
[0051]
1、本发明的一种车道级融合定位方法，假设自动驾驶车辆大体是沿着道路中心线行驶，实际情况也如此。因此可以通过gps做定位初始化，后续在初始化基础上做航迹粗推算，通过得到的粗定位结果获取周围一段范围高精度地图，再利用高精度地图车道线数量、类型、颜色等信息与前视摄像头识别到的车道线的对应信息做匹配，获取本车在高精度地图上所在车道，最后利用一段时间内本车历史行驶轨迹点与高精度地图上所在车道中心线做车道级精配准，即可得到精度较高的车道级定位结果。
[0052]
2、本发明的一种车道级融合定位系统，采用低成本传感器，通过定位初始化、航迹粗推算、gps到局部笛卡尔坐标转换、车道匹配、车道级精配准、笛卡尔局部坐标到gps全局坐标逆转换等模块实现车道级高精度定位，可用于量产车上。
附图说明
[0053]
图1为实施例的一种车道级融合定位方法的流程图。
具体实施方式
[0054]
下面结合附图对本发明的具体实施方式作进一步的详细说明。
[0055]
具体实施例的一种车道级融合定位系统，包括：
[0056]
定位初始化模块，用于自动驾驶功能开启后进行定位初始化，获取整个定位系统的初始结果以及运行开始时的局部高精度地图；
[0057]
航迹粗推算模块，用于在所述初始结果的基础上根据本车轮速计算本车的粗定位位置，并根据所述粗定位位置获取每个时刻本车周围的局部高精度地图；
[0058]
坐标转换模块，用于将全局的gps坐标转换为本车坐标系下的局部笛卡尔坐标；
[0059]
车道匹配模块，用于确定本车在每个时刻的局部高精度地图上所处的车道；
[0060]
车道级精配准定位模块，用于对当前帧的航迹粗推算所得的粗定位结果进行修正，获得最终的定位结果；
[0061]
坐标逆转换模块，用于将本车坐标系下的局部笛卡尔坐标转换为全局gps定位坐标，为坐标转换模块的逆操作。
[0062]
实施例的一种车道级融合定位系统，采用低成本传感器，通过定位初始化、航迹粗推算、gps到局部笛卡尔坐标转换、车道匹配、车道级精配准、笛卡尔局部坐标到gps全局坐标逆转换等模块实现车道级高精度定位，可用于量产车上。
[0063]
请参见图1，本发明还提供一种车道级融合定位方法，本方法基于上述的一种车道级融合定位系统而进行，具体包括以下步骤：
[0064]
s1：初始定位，在定位初始化模块内采用三帧初始化定位以获得初始定位结果以及所述的一种车道级融合定位系统运行开始第一帧的局部高精度地图，并判断所述初始定位结果是否有效，如果有效，表示初始化成功并执行s2；如果无效，则持续进行初始化定位和判断直至初始化成功；
[0065]
s2：航迹粗推算，基于所述初始定位结果，在航迹粗推算模块内利用本车轮速信息计算本车的粗定位位置，并通过所述粗定位位置获取每一帧本车周围的局部高精度地图；
[0066]
s3：坐标转换，通过坐标转换模块将全局gps坐标转换为本车坐标系下的笛卡尔坐标；
[0067]
s4：车道匹配，利用当前帧的局部高精度地图确定本车所处的车道，并将该局部高精度地图上与视觉车道线匹配的地图段的车道进行编号，得到本车所在车道的车道编号；
[0068]
s4.1：根据所述步骤s1和s2获得的当前帧的局部高精度地图，通过车道线数量比对获取本车在当前帧的局部高精度地图上所处的地段；
[0069]
s4.2：将当前帧的局部高精度地图上的本车两边车道线的横向距离、两边车道线类型、颜色与前视摄像头所给的对应量进行比较，从而确定本车在当前帧的局部高精度地图上所处的车道，完成车道匹配；
[0070]
s4.3：对所述步骤s4.2得到的车道进行编号，从左到右依次对车道进行编号，输出本车所在车道的车道编号n；
[0071]
s5：车道级精配准定位，利用所述步骤s4获得的车道编号对所述步骤s2的当前帧
的航迹粗推算所得的粗定位结果进行修正，输出修正后的定位结果，执行步骤s6；
[0072]
s6：坐标逆转换，在坐标逆转换模块内将所述步骤s5得到的修正后的定位结果转换为全局的gps坐标并输出；同时将该gps坐标输入到航迹粗推算模块内以对所述步骤s2的粗定位结果进行校正。
[0073]
实施时，本发明假设自动驾驶车辆大体是沿着道路中心线行驶，实际情况也如此。因此可以通过gps做定位初始化，后续在初始化基础上做航迹粗推算，通过得到的粗定位结果获取周围一段范围高精度地图，再利用高精度地图车道线数量、类型、颜色等信息与前视摄像头识别到的车道线的对应信息做匹配，获取本车在高精度地图上所在车道，最后利用一段时间内本车历史行驶轨迹点与高精度地图上所在车道中心线做车道级精配准，即可得到精度较高的车道级定位结果。
[0074]
进一步地，这里可以理解的是，步骤s4.1中所提及的当前帧的局部高精度地图，应涵盖步骤s1和s2中所提及的局部高精度地图，如果处于本车道级融合定位系统开始运行的第一帧，那么所需的局部高精度地图应为步骤s1中的局部高精度地图；如果系统运行后，即是步骤s2中的每一帧的局部高精度地图。
[0075]
请继续参见图1，其中，所述步骤s1中，对连续三帧之间的定位结果的差距来判断所述初始定位结果是否有效，如果连续三帧的定位结果差别不大，认定为有效；如果差别过大，则认定为无效。
[0076]
这样，以便后续的定位算法运行。
[0077]
其中，所述步骤s2基于认定在航迹粗推算模块内前后两帧本车的位姿不会发现巨大的变化；
[0078]
然后通过下式计算本车车速v、航向角速度ω及转弯半径r：
[0079][0080][0081][0082]
其中，vr为右后轮轮速，单位为m/s；v
l
为左后轮轮速，单位为m/s；l为后轮轮距；
[0083]
再通过下式计算当前时刻t本车的位姿：
[0084]
x
t
＝x
t-1
+δt*v*cos(θ
t
)
[0085]yt
＝y
t-1
+δt*v*sin(θ
t
)
[0086]
θ
t
＝θ
t-1
+δt*ω
[0087]
其中，x、y为位置,θ为航向角。
[0088]
这样，用于后续车道匹配时计算本车所在车道。
[0089]
其中，所述步骤s3中的全局gps坐标基于所述步骤s2所得的每一帧本车的粗定位结果来获取对应的局部高精度地图，该局部高精度地图内的车道线及车道中心线点即为所述全局gps坐标。
[0090]
实施时，此处所获取的局部高精度地图，本实施例所选取的范围为本车前150m及后50m。
[0091]
其中，所述步骤s5中，
[0092]
通过所述车道编号n来获取当前帧局部高精度地图上本车道中心线距离本车最近的轨迹点，并将该轨迹点添加到计算队列q1中，同时将所述步骤s2计算所得的航迹粗推算的粗定位点添加到计算队列q2中；
[0093]
固定计算队列q1及q2的容量大小，每次q1和q2满时，删除最先进入计算队列的轨迹点和粗定位点，并将当前帧的点分别添加到计算队列的末尾；
[0094]
根据计算队列q1内的轨迹点和q2内的粗定位点进行计算，得到航迹粗推算的修正量，再通过所述修正量对当前帧航迹粗推算的粗定位结果进行修正，即可得到修正后的定位结果。
[0095]
实施时，固定计算队列q1和q2的大小均为10，即保存历史10帧局部高精度地图上本车道中心线距离本车最近的轨迹点，记为点集a；历史10帧的本车粗定位点，记为点集b；通过下式即可计算得到航迹粗推算的粗定位点的修正量：
[0096][0097][0098]
其中，θ为旋转量，t为平移量，为点集a的第i帧轨迹点的x坐标，为点集a的第i帧轨迹点的y坐标，为点集b的第i帧粗定位点的x坐标，为点集b的第i帧粗定位点的y坐标，为点集a的中心点的x坐标(即点集a所有点的x坐标的均值)，为点集a的中心点的y坐标(即点集a所有点的y坐标的均值)，为点集b的中心点的x坐标(即点集b所有点的x坐标的均值),为点集b的中心点的y坐标(即点集b所有点的y坐标的均值)，r为旋转矩阵。
[0099]
将当前帧的航迹粗推算的粗定位结果通过上述修正量进行旋转平移，即可获得修正后的定位结果，并输出到坐标逆转换模块进行坐标逆转换。
[0100]
其中，所述步骤s2中，若来自坐标逆转换模块的全局gps定位点有效，即可通过该全局gps定位点对航迹粗推算模块内的粗定位结果进行校正。
[0101]
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。
技术特征：1.一种车道级融合定位方法，其特征在于：具体包括以下步骤：s1：初始定位，在定位初始化模块内采用三帧初始化定位以获得初始定位结果以及所述的一种车道级融合定位系统运行开始第一帧的局部高精度地图，并判断所述初始定位结果是否有效，如果有效，表示初始化成功并执行s2；如果无效，则持续进行初始化定位和判断直至初始化成功；s2：航迹粗推算，基于所述初始定位结果，在航迹粗推算模块内利用本车轮速信息计算本车的粗定位位置，并通过所述粗定位位置获取每一帧本车周围的局部高精度地图；s3：坐标转换，通过坐标转换模块将全局gps坐标转换为本车坐标系下的笛卡尔坐标；s4：车道匹配，利用当前帧的局部高精度地图确定本车所处的车道，并将该局部高精度地图上与视觉车道线匹配的地图段的车道进行编号，得到本车所在车道的车道编号；s5：车道级精配准定位，利用所述步骤s4获得的车道编号对所述步骤s2的当前帧的航迹粗推算所得的粗定位结果进行修正，输出修正后的定位结果，执行步骤s6；s6：坐标逆转换，在坐标逆转换模块内将所述步骤s5得到的修正后的定位结果转换为全局的gps坐标并输出；同时将该gps坐标输入到航迹粗推算模块内以对所述步骤s2的粗定位结果进行校正。2.根据权利要求1所述一种车道级融合定位方法，其特征在于：所述步骤s1中，对连续三帧之间的定位结果的差距来判断所述初始定位结果是否有效，如果连续三帧的定位结果差别不大，认定为有效；如果差别过大，则认定为无效。3.根据权利要求1所述一种车道级融合定位方法，其特征在于：所述步骤s2基于认定在航迹粗推算模块内前后两帧本车的位姿不会发现巨大的变化；然后通过下式计算本车车速ν、航向角速度ω及转弯半径r：下式计算本车车速ν、航向角速度ω及转弯半径r：下式计算本车车速ν、航向角速度ω及转弯半径r：其中，v
r
为右后轮轮速，单位为m/s；v
l
为左后轮轮速，单位为m/s；l为后轮轮距；再通过下式计算当前时刻t本车的位姿：x
t
＝x
t-1
+δt*v*cos(θ
t
)y
t
＝y
t-1
+δt*v*sin(θ
t
)θ
t
＝θ
t-1
+δt*ω其中，x、y为位置,θ为航向角。4.根据权利要求1所述一种车道级融合定位方法，其特征在于：所述步骤s3中的全局gps坐标基于所述步骤s2所得的每一帧本车的粗定位结果来获取对应的局部高精度地图，该局部高精度地图内的车道线及车道中心线点即为所述全局gps坐标。5.根据权利要求1所述一种车道级融合定位方法，其特征在于：所述步骤s4还包括：s4.1：根据所述步骤s1和s2获得的当前帧的局部高精度地图，通过车道线数量比对获取本车在当前帧的局部高精度地图上所处的地段；
s4.2：将当前帧的局部高精度地图上的本车两边车道线的横向距离、两边车道线类型、颜色与前视摄像头所给的对应量进行比较，从而确定本车在当前帧的局部高精度地图上所处的车道，完成车道匹配；s4.3：对所述步骤s4.2得到的车道进行编号，从左到右依次对车道进行编号，输出本车所在车道的车道编号n。6.根据权利要求5所述一种车道级融合定位方法，其特征在于：所述步骤s5中，通过所述车道编号n来获取当前帧局部高精度地图上本车道中心线距离本车最近的轨迹点，并将该轨迹点添加到计算队列q1中，同时将所述步骤s2计算所得的航迹粗推算的粗定位点添加到计算队列q2中；固定计算队列q1及q2的容量大小，每次q1和q2满时，删除最先进入计算队列的轨迹点和粗定位点，并将当前帧的点分别添加到计算队列的末尾；根据计算队列q1内的轨迹点和q2内的粗定位点进行计算，得到航迹粗推算的修正量，再通过所述修正量对当前帧航迹粗推算的粗定位结果进行修正，即可得到修正后的定位结果。7.根据权利要求6所述一种车道级融合定位方法，其特征在于：所述计算队列q1及q2的容量大小均为10；即保存历史10帧局部高精度地图上本车道中心线距离本车最近的轨迹点，记为点集a；历史10帧的本车粗定位点，记为点集b；通过下式计算得到航迹粗推算的粗定位点的修正量：通过下式计算得到航迹粗推算的粗定位点的修正量：其中，θ为旋转量，t为平移量，为点集a的第i帧轨迹点的x坐标，为点集a的第i帧轨迹点的y坐标，为点集b的第i帧粗定位点的x坐标，为点集b的第i帧粗定位点的y坐标，为点集a的中心点的x坐标，为点集a的中心点的y坐标，为点集b的中心点的x坐标,为点集b的中心点的y坐标，r为旋转矩阵。8.根据权利要求6所述一种车道级融合定位方法，其特征在于：所述步骤s2中，若来自坐标逆转换模块的全局gps定位点有效，即可通过该全局gps定位点对航迹粗推算模块内的粗定位结果进行校正。9.一种车道级融合定位系统，其特征在于：包括：定位初始化模块，用于自动驾驶功能开启后进行定位初始化，获取整个定位系统的初始结果以及运行开始时的局部高精度地图；航迹粗推算模块，用于在所述初始结果的基础上根据本车轮速计算本车的粗定位位置，并根据所述粗定位位置获取每个时刻本车周围的局部高精度地图；坐标转换模块，用于将全局的gps坐标转换为本车坐标系下的局部笛卡尔坐标；车道匹配模块，用于确定本车在每个时刻的局部高精度地图上所处的车道；
车道级精配准定位模块，用于对当前帧的航迹粗推算所得的粗定位结果进行修正，获得最终的定位结果；坐标逆转换模块，用于将本车坐标系下的局部笛卡尔坐标转换为全局gps定位坐标，为坐标转换模块的逆操作。
技术总结本发明涉及一种车道级融合定位方法及系统，系统包括定位初始化模块、航迹粗推算模块、坐标转换模块、车道匹配模块、车道级精配准定位模块和坐标逆转换模块。本发明通过GPS做定位初始化，后续在初始化基础上做航迹粗推算，通过得到的粗定位结果获取周围一段范围高精度地图，再利用高精度地图车道线数量、类型、颜色等信息与前视摄像头识别到的车道线的对应信息做匹配，获取本车在高精度地图上所在车道，最后利用一段时间内本车历史行驶轨迹点与高精度地图上所在车道中心线做车道级精配准，即可得到精度较高的车道级定位结果。即可得到精度较高的车道级定位结果。即可得到精度较高的车道级定位结果。
技术研发人员：万满 任凡 王宽 杨钊
受保护的技术使用者：重庆长安汽车股份有限公司
技术研发日：2022.03.31
技术公布日：2022/7/5
