CN114705199A - 一种车道级融合定位方法及*** - Google Patents

一种车道级融合定位方法及*** Download PDF

Info

Publication number
CN114705199A
CN114705199A CN202210356880.0A CN202210356880A CN114705199A CN 114705199 A CN114705199 A CN 114705199A CN 202210356880 A CN202210356880 A CN 202210356880A CN 114705199 A CN114705199 A CN 114705199A
Authority
CN
China
Prior art keywords
positioning
lane
vehicle
point
coarse
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210356880.0A
Other languages
English (en)
Inventor
万满
任凡
王宽
杨钊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing Changan Automobile Co Ltd
Original Assignee
Chongqing Changan Automobile Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chongqing Changan Automobile Co Ltd filed Critical Chongqing Changan Automobile Co Ltd
Priority to CN202210356880.0A priority Critical patent/CN114705199A/zh
Publication of CN114705199A publication Critical patent/CN114705199A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明涉及一种车道级融合定位方法及***，***包括定位初始化模块、航迹粗推算模块、坐标转换模块、车道匹配模块、车道级精配准定位模块和坐标逆转换模块。本发明通过GPS做定位初始化，后续在初始化基础上做航迹粗推算，通过得到的粗定位结果获取周围一段范围高精度地图，再利用高精度地图车道线数量、类型、颜色等信息与前视摄像头识别到的车道线的对应信息做匹配，获取本车在高精度地图上所在车道，最后利用一段时间内本车历史行驶轨迹点与高精度地图上所在车道中心线做车道级精配准，即可得到精度较高的车道级定位结果。

Description

一种车道级融合定位方法及***
技术领域
本发明属于自动驾驶融合定位技术领域，具体涉及一种车道级融合定位方法及***。
背景技术
定位是实现自动驾驶的关键技术。结合高精度地图的定位使自动驾驶汽车获取了超感知的能力，极大地提升了自动驾驶的安全性和效率。在自动驾驶感知融合技术中，定位融合是实现车辆自动控制的必要过程。目前自动驾驶车辆的定位方案主要有组合惯导(GNSS和INS)定位、激光雷达定位、视觉定位及融合不同设备的组合定位。传统的单一传感器或简单组合定位方案往往精度不高且鲁棒性不好。融合定位，如公开号为CN106767853A(申请号201611261781.5)的专利，一种基于多信息融合的无人驾驶车辆高精度定位方法，提出基于视觉、GNSS/INS、激光雷达及高精度地图的融合定位方案虽然稳定性较好，定位精度也足够高，但价格高昂的激光雷达也使得此方案下的自动驾驶迟迟无法量产。
现有的定位方法主要存在以下缺陷：
1)依赖于单一传感器的定位方案或简单组合定位方案定位精度不高且鲁棒性不好；
2)结合高精度地图及激光雷达等多传感器融合的定位方案定位效果好，但成本较高，短期内难以应用到量产车型上。
发明内容
针对现有技术的上述不足，本发明要解决的技术问题是提供一种车道级融合定位方法及***，避免为了达到控制所要求的定位性能而带来高昂成本的问题。
为解决上述技术问题，本发明采用如下技术方案：
一种车道级融合定位方法，具体包括以下步骤：
S1：初始定位，在定位初始化模块内采用三帧初始化定位以获得初始定位结果以及所述的一种车道级融合定位***运行开始第一帧的局部高精度地图，并判断所述初始定位结果是否有效，如果有效，表示初始化成功并执行S2；如果无效，则持续进行初始化定位和判断直至初始化成功；
S2：航迹粗推算，基于所述初始定位结果，在航迹粗推算模块内利用本车轮速信息计算本车的粗定位位置，并通过所述粗定位位置获取每一帧本车周围的局部高精度地图；
S3：坐标转换，通过坐标转换模块将全局GPS坐标转换为本车坐标系下的笛卡尔坐标；
S4：车道匹配，利用当前帧的局部高精度地图确定本车所处的车道，并将该局部高精度地图上与视觉车道线匹配的地图段的车道进行编号，得到本车所在车道的车道编号；
S5：车道级精配准定位，利用所述步骤S4获得的车道编号对所述步骤S2的当前帧的航迹粗推算所得的粗定位结果进行修正，输出修正后的定位结果，执行步骤S6；
S6：坐标逆转换，在坐标逆转换模块内将所述步骤S5得到的修正后的定位结果转换为全局的GPS坐标并输出；同时将该GPS坐标输入到航迹粗推算模块内以对所述步骤S2的粗定位结果进行校正。
进一步完善上述技术方案，所述步骤S1中，对连续三帧之间的定位结果的差距来判断所述初始定位结果是否有效，如果连续三帧的定位结果差别不大，认定为有效；如果差别过大，则认定为无效。
进一步地，所述步骤S2基于认定在航迹粗推算模块内前后两帧本车的位姿不会发现巨大的变化；
然后通过下式计算本车车速v、航向角速度ω及转弯半径r：
Figure BDA0003575731170000021
Figure BDA0003575731170000022
Figure BDA0003575731170000023
其中，vr为右后轮轮速，单位为m/s；vl为左后轮轮速，单位为m/s；l为后轮轮距；
再通过下式计算当前时刻t本车的位姿：
xt＝xt-1+Δt*v*cos(θt)
yt＝yt-1+Δt*v*sin(θt)
θt＝θt-1+Δt*ω
其中，x、y为位置,θ为航向角。
进一步地，所述步骤S3中的全局GPS坐标基于所述步骤S2所得的每一帧本车的粗定位结果来获取对应的局部高精度地图，该局部高精度地图内的车道线及车道中心线点即为所述全局GPS坐标。
进一步地，所述步骤S4还包括：
S4.1：根据所述步骤S1和S2获得的当前帧的局部高精度地图，通过车道线数量比对获取本车在当前帧的局部高精度地图上所处的地段；
S4.2：将当前帧的局部高精度地图上的本车两边车道线的横向距离、两边车道线类型、颜色与前视摄像头所给的对应量进行比较，从而确定本车在当前帧的局部高精度地图上所处的车道，完成车道匹配；
S4.3：对所述步骤S4.2得到的车道进行编号，从左到右依次对车道进行编号，输出本车所在车道的车道编号N。
进一步地，所述步骤S5中，
通过所述车道编号N来获取当前帧局部高精度地图上本车道中心线距离本车最近的轨迹点，并将该轨迹点添加到计算队列Q1中，同时将所述步骤S2计算所得的航迹粗推算的粗定位点添加到计算队列Q2中；
固定计算队列Q1及Q2的容量大小，每次Q1和Q2满时，删除最先进入计算队列的轨迹点和粗定位点，并将当前帧的点分别添加到计算队列的末尾；
根据计算队列Q1内的轨迹点和Q2内的粗定位点进行计算，得到航迹粗推算的修正量，再通过所述修正量对当前帧航迹粗推算的粗定位结果进行修正，即可得到修正后的定位结果。
进一步地，所述计算队列Q1及Q2的容量大小均为10；
即保存历史10帧局部高精度地图上本车道中心线距离本车最近的轨迹点，记为点集A；历史10帧的本车粗定位点，记为点集B；
通过下式计算得到航迹粗推算的粗定位点的修正量：
Figure BDA0003575731170000031
Figure BDA0003575731170000032
其中，θ为旋转量，t为平移量，
Figure BDA0003575731170000033
为点集A的第i帧轨迹点的x坐标，
Figure BDA0003575731170000034
为点集A的第i帧轨迹点的y坐标，
Figure BDA0003575731170000035
为点集B的第i帧粗定位点的x坐标，
Figure BDA0003575731170000036
为点集B的第i帧粗定位点的y坐标，
Figure BDA0003575731170000037
为点集A的中心点的x坐标，
Figure BDA0003575731170000038
为点集A的中心点的y坐标，
Figure BDA0003575731170000039
为点集B的中心点的x坐标,
Figure BDA00035757311700000310
为点集B的中心点的y坐标，R为旋转矩阵。
进一步地，所述步骤S2中，若来自坐标逆转换模块的全局GPS定位点有效，即可通过该全局GPS定位点对航迹粗推算模块内的粗定位结果进行校正。
本发明还涉及一种车道级融合定位***，包括：
定位初始化模块，用于自动驾驶功能开启后进行定位初始化，获取整个定位***的初始结果以及运行开始时的局部高精度地图；
航迹粗推算模块，用于在所述初始结果的基础上根据本车轮速计算本车的粗定位位置，并根据所述粗定位位置获取每个时刻本车周围的局部高精度地图；
坐标转换模块，用于将全局的GPS坐标转换为本车坐标系下的局部笛卡尔坐标；
车道匹配模块，用于确定本车在每个时刻的局部高精度地图上所处的车道；
车道级精配准定位模块，用于对当前帧的航迹粗推算所得的粗定位结果进行修正，获得最终的定位结果；
坐标逆转换模块，用于将本车坐标系下的局部笛卡尔坐标转换为全局GPS定位坐标，为坐标转换模块的逆操作。
相比现有技术，本发明具有如下有益效果：
1、本发明的一种车道级融合定位方法，假设自动驾驶车辆大体是沿着道路中心线行驶，实际情况也如此。因此可以通过GPS做定位初始化，后续在初始化基础上做航迹粗推算，通过得到的粗定位结果获取周围一段范围高精度地图，再利用高精度地图车道线数量、类型、颜色等信息与前视摄像头识别到的车道线的对应信息做匹配，获取本车在高精度地图上所在车道，最后利用一段时间内本车历史行驶轨迹点与高精度地图上所在车道中心线做车道级精配准，即可得到精度较高的车道级定位结果。
2、本发明的一种车道级融合定位***，采用低成本传感器，通过定位初始化、航迹粗推算、GPS到局部笛卡尔坐标转换、车道匹配、车道级精配准、笛卡尔局部坐标到GPS全局坐标逆转换等模块实现车道级高精度定位，可用于量产车上。
附图说明
图1为实施例的一种车道级融合定位方法的流程图。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步的详细说明。
具体实施例的一种车道级融合定位***，包括：
定位初始化模块，用于自动驾驶功能开启后进行定位初始化，获取整个定位***的初始结果以及运行开始时的局部高精度地图；
航迹粗推算模块，用于在所述初始结果的基础上根据本车轮速计算本车的粗定位位置，并根据所述粗定位位置获取每个时刻本车周围的局部高精度地图；
坐标转换模块，用于将全局的GPS坐标转换为本车坐标系下的局部笛卡尔坐标；
车道匹配模块，用于确定本车在每个时刻的局部高精度地图上所处的车道；
车道级精配准定位模块，用于对当前帧的航迹粗推算所得的粗定位结果进行修正，获得最终的定位结果；
坐标逆转换模块，用于将本车坐标系下的局部笛卡尔坐标转换为全局GPS定位坐标，为坐标转换模块的逆操作。
实施例的一种车道级融合定位***，采用低成本传感器，通过定位初始化、航迹粗推算、GPS到局部笛卡尔坐标转换、车道匹配、车道级精配准、笛卡尔局部坐标到GPS全局坐标逆转换等模块实现车道级高精度定位，可用于量产车上。
请参见图1，本发明还提供一种车道级融合定位方法，本方法基于上述的一种车道级融合定位***而进行，具体包括以下步骤：
S1：初始定位，在定位初始化模块内采用三帧初始化定位以获得初始定位结果以及所述的一种车道级融合定位***运行开始第一帧的局部高精度地图，并判断所述初始定位结果是否有效，如果有效，表示初始化成功并执行S2；如果无效，则持续进行初始化定位和判断直至初始化成功；
S2：航迹粗推算，基于所述初始定位结果，在航迹粗推算模块内利用本车轮速信息计算本车的粗定位位置，并通过所述粗定位位置获取每一帧本车周围的局部高精度地图；
S3：坐标转换，通过坐标转换模块将全局GPS坐标转换为本车坐标系下的笛卡尔坐标；
S4：车道匹配，利用当前帧的局部高精度地图确定本车所处的车道，并将该局部高精度地图上与视觉车道线匹配的地图段的车道进行编号，得到本车所在车道的车道编号；
S4.1：根据所述步骤S1和S2获得的当前帧的局部高精度地图，通过车道线数量比对获取本车在当前帧的局部高精度地图上所处的地段；
S4.2：将当前帧的局部高精度地图上的本车两边车道线的横向距离、两边车道线类型、颜色与前视摄像头所给的对应量进行比较，从而确定本车在当前帧的局部高精度地图上所处的车道，完成车道匹配；
S4.3：对所述步骤S4.2得到的车道进行编号，从左到右依次对车道进行编号，输出本车所在车道的车道编号N；
S5：车道级精配准定位，利用所述步骤S4获得的车道编号对所述步骤S2的当前帧的航迹粗推算所得的粗定位结果进行修正，输出修正后的定位结果，执行步骤S6；
S6：坐标逆转换，在坐标逆转换模块内将所述步骤S5得到的修正后的定位结果转换为全局的GPS坐标并输出；同时将该GPS坐标输入到航迹粗推算模块内以对所述步骤S2的粗定位结果进行校正。
实施时，本发明假设自动驾驶车辆大体是沿着道路中心线行驶，实际情况也如此。因此可以通过GPS做定位初始化，后续在初始化基础上做航迹粗推算，通过得到的粗定位结果获取周围一段范围高精度地图，再利用高精度地图车道线数量、类型、颜色等信息与前视摄像头识别到的车道线的对应信息做匹配，获取本车在高精度地图上所在车道，最后利用一段时间内本车历史行驶轨迹点与高精度地图上所在车道中心线做车道级精配准，即可得到精度较高的车道级定位结果。
进一步地，这里可以理解的是，步骤S4.1中所提及的当前帧的局部高精度地图，应涵盖步骤S1和S2中所提及的局部高精度地图，如果处于本车道级融合定位***开始运行的第一帧，那么所需的局部高精度地图应为步骤S1中的局部高精度地图；如果***运行后，即是步骤S2中的每一帧的局部高精度地图。
请继续参见图1，其中，所述步骤S1中，对连续三帧之间的定位结果的差距来判断所述初始定位结果是否有效，如果连续三帧的定位结果差别不大，认定为有效；如果差别过大，则认定为无效。
这样，以便后续的定位算法运行。
其中，所述步骤S2基于认定在航迹粗推算模块内前后两帧本车的位姿不会发现巨大的变化；
然后通过下式计算本车车速v、航向角速度ω及转弯半径r：
Figure BDA0003575731170000061
Figure BDA0003575731170000062
Figure BDA0003575731170000063
其中，vr为右后轮轮速，单位为m/s；vl为左后轮轮速，单位为m/s；l为后轮轮距；
再通过下式计算当前时刻t本车的位姿：
xt＝xt-1+Δt*v*cos(θt)
yt＝yt-1+Δt*v*sin(θt)
θt＝θt-1+Δt*ω
其中，x、y为位置,θ为航向角。
这样，用于后续车道匹配时计算本车所在车道。
其中，所述步骤S3中的全局GPS坐标基于所述步骤S2所得的每一帧本车的粗定位结果来获取对应的局部高精度地图，该局部高精度地图内的车道线及车道中心线点即为所述全局GPS坐标。
实施时，此处所获取的局部高精度地图，本实施例所选取的范围为本车前150m及后50m。
其中，所述步骤S5中，
通过所述车道编号N来获取当前帧局部高精度地图上本车道中心线距离本车最近的轨迹点，并将该轨迹点添加到计算队列Q1中，同时将所述步骤S2计算所得的航迹粗推算的粗定位点添加到计算队列Q2中；
固定计算队列Q1及Q2的容量大小，每次Q1和Q2满时，删除最先进入计算队列的轨迹点和粗定位点，并将当前帧的点分别添加到计算队列的末尾；
根据计算队列Q1内的轨迹点和Q2内的粗定位点进行计算，得到航迹粗推算的修正量，再通过所述修正量对当前帧航迹粗推算的粗定位结果进行修正，即可得到修正后的定位结果。
实施时，固定计算队列Q1和Q2的大小均为10，即保存历史10帧局部高精度地图上本车道中心线距离本车最近的轨迹点，记为点集A；历史10帧的本车粗定位点，记为点集B；通过下式即可计算得到航迹粗推算的粗定位点的修正量：
Figure BDA0003575731170000071
Figure BDA0003575731170000072
其中，θ为旋转量，t为平移量，
Figure BDA0003575731170000073
为点集A的第i帧轨迹点的x坐标，
Figure BDA0003575731170000074
为点集A的第i帧轨迹点的y坐标，
Figure BDA0003575731170000075
为点集B的第i帧粗定位点的x坐标，
Figure BDA0003575731170000076
为点集B的第i帧粗定位点的y坐标，
Figure BDA0003575731170000077
为点集A的中心点的x坐标(即点集A所有点的x坐标的均值)，
Figure BDA0003575731170000078
为点集A的中心点的y坐标(即点集A所有点的y坐标的均值)，
Figure BDA0003575731170000079
为点集B的中心点的x坐标(即点集B所有点的x坐标的均值),
Figure BDA00035757311700000710
为点集B的中心点的y坐标(即点集B所有点的y坐标的均值)，R为旋转矩阵。
将当前帧的航迹粗推算的粗定位结果通过上述修正量进行旋转平移，即可获得修正后的定位结果，并输出到坐标逆转换模块进行坐标逆转换。
其中，所述步骤S2中，若来自坐标逆转换模块的全局GPS定位点有效，即可通过该全局GPS定位点对航迹粗推算模块内的粗定位结果进行校正。
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (9)

1.一种车道级融合定位方法，其特征在于：具体包括以下步骤：
S1：初始定位，在定位初始化模块内采用三帧初始化定位以获得初始定位结果以及所述的一种车道级融合定位***运行开始第一帧的局部高精度地图，并判断所述初始定位结果是否有效，如果有效，表示初始化成功并执行S2；如果无效，则持续进行初始化定位和判断直至初始化成功；
S2：航迹粗推算，基于所述初始定位结果，在航迹粗推算模块内利用本车轮速信息计算本车的粗定位位置，并通过所述粗定位位置获取每一帧本车周围的局部高精度地图；
S3：坐标转换，通过坐标转换模块将全局GPS坐标转换为本车坐标系下的笛卡尔坐标；
S4：车道匹配，利用当前帧的局部高精度地图确定本车所处的车道，并将该局部高精度地图上与视觉车道线匹配的地图段的车道进行编号，得到本车所在车道的车道编号；
S5：车道级精配准定位，利用所述步骤S4获得的车道编号对所述步骤S2的当前帧的航迹粗推算所得的粗定位结果进行修正，输出修正后的定位结果，执行步骤S6；
S6：坐标逆转换，在坐标逆转换模块内将所述步骤S5得到的修正后的定位结果转换为全局的GPS坐标并输出；同时将该GPS坐标输入到航迹粗推算模块内以对所述步骤S2的粗定位结果进行校正。
2.根据权利要求1所述一种车道级融合定位方法，其特征在于：所述步骤S1中，对连续三帧之间的定位结果的差距来判断所述初始定位结果是否有效，如果连续三帧的定位结果差别不大，认定为有效；如果差别过大，则认定为无效。
3.根据权利要求1所述一种车道级融合定位方法，其特征在于：所述步骤S2基于认定在航迹粗推算模块内前后两帧本车的位姿不会发现巨大的变化；
然后通过下式计算本车车速ν、航向角速度ω及转弯半径r：
Figure FDA0003575731160000011
Figure FDA0003575731160000012
Figure FDA0003575731160000013
其中，vr为右后轮轮速，单位为m/s；vl为左后轮轮速，单位为m/s；l为后轮轮距；
再通过下式计算当前时刻t本车的位姿：
xt＝xt-1+Δt*v*cos(θt)
yt＝yt-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；
通过下式计算得到航迹粗推算的粗定位点的修正量：
Figure FDA0003575731160000021
Figure FDA0003575731160000022
其中，θ为旋转量，t为平移量，
Figure FDA0003575731160000023
为点集A的第i帧轨迹点的x坐标，
Figure FDA0003575731160000024
为点集A的第i帧轨迹点的y坐标，
Figure FDA0003575731160000031
为点集B的第i帧粗定位点的x坐标，
Figure FDA0003575731160000036
为点集B的第i帧粗定位点的y坐标，
Figure FDA0003575731160000032
为点集A的中心点的x坐标，
Figure FDA0003575731160000034
为点集A的中心点的y坐标，
Figure FDA0003575731160000035
为点集B的中心点的x坐标,
Figure FDA0003575731160000033
为点集B的中心点的y坐标，R为旋转矩阵。
8.根据权利要求6所述一种车道级融合定位方法，其特征在于：所述步骤S2中，若来自坐标逆转换模块的全局GPS定位点有效，即可通过该全局GPS定位点对航迹粗推算模块内的粗定位结果进行校正。
9.一种车道级融合定位***，其特征在于：包括：
定位初始化模块，用于自动驾驶功能开启后进行定位初始化，获取整个定位***的初始结果以及运行开始时的局部高精度地图；
航迹粗推算模块，用于在所述初始结果的基础上根据本车轮速计算本车的粗定位位置，并根据所述粗定位位置获取每个时刻本车周围的局部高精度地图；
坐标转换模块，用于将全局的GPS坐标转换为本车坐标系下的局部笛卡尔坐标；
车道匹配模块，用于确定本车在每个时刻的局部高精度地图上所处的车道；
车道级精配准定位模块，用于对当前帧的航迹粗推算所得的粗定位结果进行修正，获得最终的定位结果；
坐标逆转换模块，用于将本车坐标系下的局部笛卡尔坐标转换为全局GPS定位坐标，为坐标转换模块的逆操作。
CN202210356880.0A 2022-03-31 2022-03-31 一种车道级融合定位方法及*** Pending CN114705199A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210356880.0A CN114705199A (zh) 2022-03-31 2022-03-31 一种车道级融合定位方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210356880.0A CN114705199A (zh) 2022-03-31 2022-03-31 一种车道级融合定位方法及***

Publications (1)

Publication Number Publication Date
CN114705199A true CN114705199A (zh) 2022-07-05

Family

ID=82173359

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210356880.0A Pending CN114705199A (zh) 2022-03-31 2022-03-31 一种车道级融合定位方法及***

Country Status (1)

Country Link
CN (1) CN114705199A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115061479A (zh) * 2022-08-03 2022-09-16 国汽智控(北京)科技有限公司 车道关系确定方法、装置、电子设备及存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115061479A (zh) * 2022-08-03 2022-09-16 国汽智控(北京)科技有限公司 车道关系确定方法、装置、电子设备及存储介质
CN115061479B (zh) * 2022-08-03 2022-11-04 国汽智控(北京)科技有限公司 车道关系确定方法、装置、电子设备及存储介质

Similar Documents

Publication Publication Date Title
CN109946732B (zh) 一种基于多传感器数据融合的无人车定位方法
US11307040B2 (en) Map information provision system
US11024055B2 (en) Vehicle, vehicle positioning system, and vehicle positioning method
US11802769B2 (en) Lane line positioning method and apparatus, and storage medium thereof
CN107246868B (zh) 一种协同导航定位***及导航定位方法
CN102208035B (zh) 图像处理***及位置测量***
CN108628324B (zh) 基于矢量地图的无人车导航方法、装置、设备及存储介质
Lee et al. Development of a self-driving car that can handle the adverse weather
CN113916242A (zh) 车道定位方法和装置、存储介质及电子设备
WO2022147924A1 (zh) 车辆定位方法和装置、存储介质及电子设备
CN114396957B (zh) 基于视觉与地图车道线匹配的定位位姿校准方法及汽车
JP4596566B2 (ja) 自車情報認識装置及び自車情報認識方法
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN112819711B (zh) 一种基于单目视觉的利用道路车道线的车辆反向定位方法
CN113252022A (zh) 一种地图数据处理方法及装置
CN111413990A (zh) 一种车道变更轨迹规划***
JP7418196B2 (ja) 走行軌跡推定方法及び走行軌跡推定装置
Hara et al. Vehicle localization based on the detection of line segments from multi-camera images
CN113566817B (zh) 一种车辆定位方法及装置
CN113405555B (zh) 一种自动驾驶的定位传感方法、***及装置
CN114705199A (zh) 一种车道级融合定位方法及***
JP6790951B2 (ja) 地図情報学習方法及び地図情報学習装置
CN112710301B (zh) 一种自动驾驶车辆高精度定位方法和***
CN115790613A (zh) 一种视觉信息辅助的惯性/里程计组合导航方法及装置
CN112530270B (zh) 一种基于区域分配的建图方法及装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination