CN112710301B - 一种自动驾驶车辆高精度定位方法和系统 - Google Patents

一种自动驾驶车辆高精度定位方法和系统 Download PDF

Info

Publication number
CN112710301B
CN112710301B CN202011445401.XA CN202011445401A CN112710301B CN 112710301 B CN112710301 B CN 112710301B CN 202011445401 A CN202011445401 A CN 202011445401A CN 112710301 B CN112710301 B CN 112710301B
Authority
CN
China
Prior art keywords
vehicle
coordinate system
lane line
frenet
lane
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.)
Active
Application number
CN202011445401.XA
Other languages
English (en)
Other versions
CN112710301A (zh
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.)
SAIC Volkswagen Automotive Co Ltd
Original Assignee
SAIC Volkswagen Automotive 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 SAIC Volkswagen Automotive Co Ltd filed Critical SAIC Volkswagen Automotive Co Ltd
Priority to CN202011445401.XA priority Critical patent/CN112710301B/zh
Publication of CN112710301A publication Critical patent/CN112710301A/zh
Application granted granted Critical
Publication of CN112710301B publication Critical patent/CN112710301B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation 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
    • 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/34—Route searching; Route guidance
    • 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/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Electromagnetism (AREA)
  • Pure & Applied Mathematics (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种自动驾驶车辆高精度定位方法，其包括步骤：100：由自动驾驶目标路径获得Frenet坐标系下自动驾驶路径的参考线；200：获取车辆在直角坐标系下的粗略位置P(x，y)；300：采用直角坐标系到Frenet坐标系的转换函数XYToSL()将P(x，y)转换为车辆在Frenet坐标系下的车辆位置坐标P(s，l)；400：获取车道线在直角坐标系下的车道线位置方程；500：基于车道线位置方程计算得到车辆后轴中心偏离车道中心的距离dc；600：基于Frenet坐标系下的车辆位置坐标P(s，l)的纵坐标s以及dc建立Frenet坐标系下的坐标点
Figure DDA0002824299350000011
700：采用Frenet坐标系到直角坐标系的转换函数SLToXY()将Frenet坐标系下的坐标点
Figure DDA0002824299350000012
转换为直角坐标系下的坐标点
Figure DDA0002824299350000013
得到车辆的精确定位。此外，本发明还公开了一种自动驾驶车辆高精度定位系统。

Description

一种自动驾驶车辆高精度定位方法和系统
技术领域
本发明涉及一种车辆定位方法和系统，尤其涉及一种自动驾驶车辆的定位方法和系统。
背景技术
近年来，随着自动驾驶技术的快速发展，自动驾驶系统的逐步完善和改进，自动驾驶车辆运用在日常生活中的可能性也越来越大。
自动驾驶是一个非常庞大的系统，目前现有技术中的车辆自动驾驶技术在本质上其实是一个车辆循迹控制过程，其中车辆的位置、姿态(以下简称位姿)信息对于车辆自动驾驶的实现至关重要，车辆的横向定位性能关系到车辆的安全行驶，需要特别关注。
在自动驾驶过程中，系统的输入是车辆的实际位姿以及规划出来的参考路径，控制器根据这两者之间的差值对车辆的速度(油门)和航向(方向盘)进行调整，以使车辆沿规划好的轨迹运动。
目前，所采用的用于车辆自动驾驶的定位定向系统和技术主要有如下几种：1.惯导/卫星组合定位定向系统；2.视觉SLAM(VSLAM)；3.激光雷达SLAM(LSLAM)；4.里程计(Odometer)；5.磁罗盘。
其中，卫星导航系统的可用性受卫星可见性的影响很大，高性能的惯导系统价格昂贵，价格便宜的惯导系统只能与卫星导航系统组合使用。VSLAM和LSLAM都需要事先建立专门的特征地图，且只能给出局部定位，对于开放道路的自动驾驶不适用。里程计只能给出沿车身纵轴的速度信息，需要结合其它系统给出的航向信息才能用于定位，且累积误差较大。磁罗盘很容易受到环境磁场的干扰，从而影响其准确度。虽然目前现有的自动驾驶技术将各种定位定向系统组合使用，但是这种组合使用的方案在隧道、地下车库、城市峡谷、林木茂密路段等场景下仍然很难满足自动驾驶的需求。
基于此，针对现有技术中存在的缺陷，本发明期望获得一种自动驾驶车辆高精度定位方法及系统，采用该自动驾驶车辆高精度定位方法及系统适用于所有具有车道线的道路，其适用性广泛且定位精度高，可以有效适用于对车辆横向定位精度要求较高的场合，具有良好的推广前景和应用价值。
发明内容
本发明的目的之一在于提供一种自动驾驶车辆高精度定位方法，该自动驾驶车辆高精度定位方法适用于所有具有车道线的道路，其适用性广泛且定位精度高，可以有效适用于对车辆横向定位精度要求较高的场合，具有良好的推广前景和应用价值。
为了实现上述目的，本发明提出了一种自动驾驶车辆高精度定位方法，其包括步骤：
100：基于自动驾驶目标路径获得Frenet坐标系下的自动驾驶路径的参考线；
200：获取车辆在直角坐标系下的粗略位置P(x,y)；
300：采用直角坐标系到Frenet坐标系的转换函数XYToSL()将车辆在直角坐标系下的粗略位置P(x,y)转换为Frenet坐标系下的车辆位置坐标P(s,l)；
400：获取车道线在直角坐标系下的车道线位置方程；
500：基于所述车道线位置方程计算得到车辆后轴中心偏离车道中心的距离dc；
600：基于Frenet坐标系下的车辆位置坐标P(s,l)的纵坐标s以及dc建立Frenet坐标系下的坐标点
Figure BDA0002824299330000021
700：采用Frenet坐标系到直角坐标系的转换函数SLToXY()将Frenet坐标系下的坐标点
Figure BDA0002824299330000022
转换为直角坐标系下的坐标点
Figure BDA0002824299330000023
得到车辆的精确定位。
在本发明的上述技术方案中，本发明所述的自动驾驶车辆高精度定位方法在现有的定位定向系统及其组合基础上，可以综合使用高精度地图、车道线检测、参考线、SL-XY坐标转换等技术，能够有效地提高车辆的横向定位性能，对于自动驾驶技术在开放道路上的落地实现具有重要意义。
进一步地，在本发明所述的自动驾驶车辆高精度定位方法中，在步骤100中，基于高精度地图获得所述自动驾驶目标路径。
进一步地，在本发明所述的自动驾驶车辆高精度定位方法中，在步骤200中，从下述各项的至少其中一种定位系统获得车辆在直角坐标系下的粗略位置P(x,y)：惯导/卫星组合定位定向系统、GPS导航系统、北斗导航系统、里程计和磁罗盘。
进一步地，在本发明所述的自动驾驶车辆高精度定位方法中，在步骤400中，基于摄像机检测的车道线图像数据和/或激光雷达检测的车道线点云数据获取车道线在直角坐标系下的车道线位置方程。
本技术方案可以给出车道线在车身坐标系中的代数表达式，利用这个信息可以计算出车辆后轴中心相对于车道线或车道中心的横向偏移量，Frenet坐标系的横向坐标l。在某些实施方式下，可以使用检测到的所有车道线信息；在另一些实施方式下，也可以选择一条精度较好车道线。优选地，可以选择道路内侧的车道线信息。
车道线位置方程可以通过下述步骤获得：以车辆后轴中心为原点建立车辆坐标系V，设定X轴指向车头方向，Y轴指向车辆左侧，Z轴指向车顶方向。那么，车辆左侧车道线Ll在车辆坐标系V中的方程可以表达为：
Ax+By+C＝0 (1)
在上述公式(1)中，A，B，C表示方程的系数，其根据不同具体情况下车道线的具体位置而拟合得到。
基于上述公式(1)，就可以得到直线外某点P(x0,y0)到直线Ll的距离d为：
Figure BDA0002824299330000031
根据上述公式(2)可求得车辆后轴中心(即车辆坐标系原点)与左车道线Ll的距离为：
Figure BDA0002824299330000032
设车道宽度为W(该值可由高精度地图结合粗略位置P(x,y)得到)，则车辆后轴中心与车道中心线的距离dc(设定偏左为正)为：
Figure BDA0002824299330000033
如果系统能够检测到右车道线的信息，则由式(3)还可以得到车辆后轴中心与右车道线的距离dr，则dc也可由dl和dr按下式算出：
Figure BDA0002824299330000034
进一步地，在本发明所述的自动驾驶车辆高精度定位方法中，还包括步骤800：获得坐标点
Figure BDA0002824299330000041
与坐标点P(x,y)的偏差ΔP；将ΔP作为位置误差观测信息，使用卡尔曼滤波器对车辆位姿进行滤波，以输出进一步精确定位的车辆位置坐标。
相应地，本发明的另一目的在于提供一种自动驾驶车辆高精度定位系统，该多自动驾驶车辆高精度定位系统可以用于实施本发明上述的自动驾驶车辆高精度定位方法。
为了实现上述目的，本发明提出了一种自动驾驶车辆高精度定位系统，其包括：
路径规划模块，其生成自动驾驶目标路径；
参考线生成模块，其生成Frenet坐标系下的自动驾驶路径的参考线；
定位模块，其获取车辆在直角坐标系下的粗略位置P(x,y)，获取车道线在直角坐标系下的车道线位置方程；采用直角坐标系到Frenet坐标系的转换函数XYToSL()将车辆在直角坐标系下的粗略位置P(x,y)转换为Frenet坐标系下的车辆位置坐标P(s,l)；基于所述车道线位置方程计算得到车辆后轴中心偏离车道中心的距离dc；基于Frenet坐标系下的车辆位置坐标P(s,l)的纵坐标s以及dc建立Frenet坐标系下的坐标点
Figure BDA0002824299330000042
采用Frenet坐标系到直角坐标系的转换函数SLToXY()将Frenet坐标系下的坐标点
Figure BDA0002824299330000043
转换为直角坐标系下的坐标点
Figure BDA0002824299330000044
得到车辆的精确定位。
进一步地，在本发明所述的自动驾驶车辆高精度定位系统中，所述路径规划模块基于高精度地图生成自动驾驶目标路径。
进一步地，在本发明所述的自动驾驶车辆高精度定位系统中，车辆在直角坐标系下的粗略位置P(x,y)从下述各项的至少其中一种定位系统获得：惯导/卫星组合定位定向系统、GPS导航系统、北斗导航系统、里程计和磁罗盘。
进一步地，在本发明所述的自动驾驶车辆高精度定位系统中，基于摄像机检测的车道线图像数据和/或激光雷达检测的车道线点云数据获取车道线在直角坐标系下的车道线位置方程。
进一步地，在本发明所述的自动驾驶车辆高精度定位系统中，还包括卡尔曼滤波器，其将坐标点
Figure BDA0002824299330000045
与坐标点P(x,y)的偏差ΔP；ΔP作为位置误差观测信息，以对车辆位姿进行滤波，输出进一步精确定位的车辆位置坐标。
本发明所述的自动驾驶车辆高精度定位方法和系统相较于现有技术具有如下所述的优点和有益效果：
(1)传统的地图-定位融合算法需要进行复杂的地图匹配处理，而本发明所述的自动驾驶车辆高精度定位方法则不需要进行这么复杂的处理。
(2)本发明所述的自动驾驶车辆高精度定位方法将车道线检测、参考线生成、坐标转换功能以及常规的定位定向系统结合起来，能够给出车辆横向位置的约束信息，有利于提高车辆横向定位性能。
(3)本发明所述的自动驾驶车辆高精度定位方法对所有具有车道线的道路都适用(前提是具有该路段的高精度地图)，尤其适用于卫星导航系统无法正常工作的区域，如隧道、城市峡谷、林木茂密路段，地下车库等场景，对于其它需要精确横向定位精度的应用场景也具有重要参考意义。
(4)本发明所述的自动驾驶车辆高精度定位方法提供的思路可以与现有技术中的其它定向定位方法进行灵活组合，从而满足不同的应用需要。
相应地，本发明所述的自动驾驶车辆高精度定位系统可以用于实施本发明上述的自动驾驶车辆高精度定位方法，其同样具有上述优点以及有益效果。
附图说明
图1示意性地显示了本发明所述的自动驾驶车辆高精度定位系统在一种实施方式下的步骤流程图。
图2示意性地显示了Frenet坐标系下的自动驾驶路径的参考线。
具体实施方式
下面将结合说明书附图和具体的实施例对本发明所述的自动驾驶车辆高精度定位方法和系统做进一步的解释和说明，然而该解释和说明并不对本发明的技术方案构成不当限定。
图1示意性地显示了本发明所述的自动驾驶车辆高精度定位系统在一种实施方式下的步骤流程图。
如图1所示，在本实施方式中，在本发明中，本发明公开了一种自动驾驶车辆高精度定位系统，其可以包括：路径规划模块、参考线生成模块和定位模块。需要说明的是，本发明所述的自动驾驶车辆高精度定位系统可以用于实施本发明的自动驾驶车辆高精度定位方法。
该自动驾驶车辆高精度定位方法可以包括步骤：
100：基于自动驾驶目标路径获得Frenet坐标系下的自动驾驶路径的参考线。
200：获取车辆在直角坐标系下的粗略位置P(x,y)。
在上述步骤200中，车辆在直角坐标系下的粗略位置P(x,y)可以通过现有技术中的其他定位系统获得，例如：惯导/卫星组合定位定向系统、GPS导航系统、北斗导航系统、里程计和磁罗盘。当然，在某些实施方式中，也可以将上述定位系统中的几种进行组合，从而获得车辆在直角坐标系下的粗略位置P(x,y)。
300：采用直角坐标系到Frenet坐标系的转换函数XYToSL()将车辆在直角坐标系下的粗略位置P(x,y)转换为Frenet坐标系下的车辆位置坐标P(s,l)。
400：获取车道线在直角坐标系下的车道线位置方程。
500：基于所述车道线位置方程计算得到车辆后轴中心偏离车道中心的距离dc。
600：基于Frenet坐标系下的车辆位置坐标P(s,l)的纵坐标s以及dc建立Frenet坐标系下的坐标点
Figure BDA0002824299330000061
700：采用Frenet坐标系到直角坐标系的转换函数SLToXY()将Frenet坐标系下的坐标点
Figure BDA0002824299330000062
转换为直角坐标系下的坐标点
Figure BDA0002824299330000063
得到车辆的精确定位。
在本发明中，如果对定位性能的要求不是非常的高，则可以直接使用本发明上述步骤700所获得的车辆在直角坐标系下的坐标点
Figure BDA0002824299330000064
但若对定位的要求较高，则可以进一步地获得坐标点
Figure BDA0002824299330000065
与坐标点P(x,y)的偏差ΔP，并将ΔP作为位置误差观测信息，同时使用卡尔曼滤波器对车辆位姿进行滤波和平滑，从而输出进一步精确定位的车辆位置坐标。
需要说明的是，在本实施方式中，在本发明上述自动驾驶车辆高精度定位方法的步骤100中，自动驾驶目标路径可以基于高精度地图获得，并基于自动驾驶目标路径可以获得Frenet坐标系下的自动驾驶路径的参考线，如图2所示。
图2示意性地显示了Frenet坐标系下的自动驾驶路径的参考线。
如图2所示，同时结合参考图1，在本发明中，自动驾驶车辆的路径规划、车辆控制律生成都是在Frenet坐标系中进行的，Frenet坐标系基于自动驾驶路径的参考线M建立，参考线M一般就是车道中心。
设图2所示的P点为车辆位置，则车辆在直角坐标系下的坐标为P(s,l)，车辆在直角坐标系下的坐标为P(x,y)。其中，P(s,l)中的l(lateral)称为Frenet横坐标，其表示P点偏离参考线M的距离，当车辆位置位于参考线M上S点时，则此时l＝0。相应地，P点位于参考线M前进方向左侧时为正，P点位于参考线M前进方向右侧时为负。在坐标P(s,l)中，s称为Frenet纵坐标，其表示从参考线M起点到P点在参考线M上投影点之间的参考线长度。
在本发明中，自动驾驶路径规划在Frenet坐标系中进行，而在本发明基于现有技术的定位系统获得的车辆粗略位置P(x,y)以及地图中的固定路标等信息都是表示在直角坐标系中的，基于此，本发明需要采用转换函数XYToSL()和转换函数SLToXY()，实现图1所示的Frenet坐标系和直角坐标系之间的相互转换。
因此，在本发明上述自动驾驶车辆高精度定位方法的步骤300中，需要采用直角坐标系到Frenet坐标系的转换函数XYToSL()将车辆在直角坐标系下的粗略位置P(x,y)转换为Frenet坐标系下的车辆位置坐标P(s,l)。
此外，在本发明上述自动驾驶车辆高精度定位方法的步骤400中，本发明可以基于摄像机检测的车道线图像数据和/或激光雷达(即图1中的感知模块)检测的车道线点云数据获取车道线在直角坐标系下的车道线位置方程。其中，在采用摄像机和/或激光雷达检测前，需要先对传感器相对于车辆的安全位姿进行精确标定。
相应地，在本技术方案中，可以给出车道线在车身坐标系中的代数表达式，利用这个信息可以计算出车辆后轴中心相对于车道线或车道中心的横向偏移量，Frenet坐标系的横向坐标l。在某些实施方式下，可以使用检测到的所有车道线信息；在另一些实施方式下，也可以选择一条精度较好车道线。优选地，可以选择道路内侧的车道线信息。
车道线位置方程可以通过下述步骤获得：以车辆后轴中心为原点建立车辆坐标系V，设定X轴指向车头方向，Y轴指向车辆左侧，Z轴指向车顶方向。那么，车辆左侧车道线Ll在车辆坐标系V中的方程可以表达为：
Ax+By+C＝0 (1)
在上述公式(1)中，A，B，C表示方程的系数，其根据不同具体情况下车道线的具体位置而拟合得到。
相应地，在本发明所述自动驾驶车辆高精度定位方法的步骤500中，可以基于上述车道线位置方程，计算得到车辆后轴中心偏离车道中心的距离dc，dc可以通过下述步骤获得：
基于上述公式(1)，就可以得到直线外某点P(x0,y0)到直线Ll的距离d为：
Figure BDA0002824299330000081
根据上述公式(2)可求得车辆后轴中心(即车辆坐标系V原点)与左车道线Ll的距离为：
Figure BDA0002824299330000082
设车道宽度为W(该值可由高精度地图结合粗略位置P(x,y)得到)，则车辆后轴中心与车道中心线的距离dc(设定偏左为正)为：
Figure BDA0002824299330000083
如果系统能够检测到右车道线的信息，则由式(3)还可以得到车辆后轴中心与右车道线的距离dr，则dc也可由dl和dr按下式算出：
Figure BDA0002824299330000084
在本发明中，基于Frenet坐标系下的车辆位置坐标P(s,l)的纵坐标s以及dc可以建立Frenet坐标系下的坐标点
Figure BDA0002824299330000085
而后采用Frenet坐标系到直角坐标系的转换函数SLToXY()将Frenet坐标系下的坐标点
Figure BDA0002824299330000086
转换为直角坐标系下的坐标点
Figure BDA0002824299330000087
即可得到车辆的精确定位。
综上所述，并结合参考图1和图2可知，在本发明中，本发明所述的自动驾驶车辆高精度定位系统中的路径规划模块可以用于生成自动驾驶目标路径，并基于高精度地图生成自动驾驶目标路径。自动驾驶车辆高精度定位系统中的参考线生成模块可以用于生成Frenet坐标系下的自动驾驶路径的参考线。
相应地，在本发明所述自动驾驶车辆高精度定位系统中，获取车辆在直角坐标系下的粗略位置P(x,y)，获取车道线在直角坐标系下的车道线位置方程；采用直角坐标系到Frenet坐标系的转换函数XYToSL()将车辆在直角坐标系下的粗略位置P(x,y)转换为Frenet坐标系下的车辆位置坐标P(s,l)；基于所述车道线位置方程计算得到车辆后轴中心偏离车道中心的距离dc；基于Frenet坐标系下的车辆位置坐标P(s,l)的纵坐标s以及dc建立Frenet坐标系下的坐标点
Figure BDA0002824299330000091
采用Frenet坐标系到直角坐标系的转换函数SLToXY()将Frenet坐标系下的坐标点
Figure BDA0002824299330000092
转换为直角坐标系下的坐标点
Figure BDA0002824299330000093
得到车辆的精确定位。
综上所述可以看出，本发明所述的自动驾驶车辆高精度定位方法不同于现有技术中的定向定位方法，其适用性广泛且定位精度高，可以适用于所有具有车道线的道路，能够适用于对车辆横向定位精度要求较高的场合，具有良好的推广前景和应用价值。
本发明所述的自动驾驶车辆高精度定位方法在现有的定位定向方法及其组合基础上，可以综合使用高精度地图、车道线检测、参考线、SL-XY坐标转换等技术，能够有效地提高车辆的横向定位性能，对于自动驾驶技术在开放道路上的落地实现具有重要意义。
相应地，本发明所述的自动驾驶车辆高精度定位系统可以用于实施本发明上述的自动驾驶车辆高精度定位方法，其同样具有上述的优点以及有益效果。
需要说明的是，本发明保护范围中现有技术部分并不局限于本申请文件所给出的实施例，所有不与本发明的方案相矛盾的现有技术，包括但不局限于在先专利文献、在先公开出版物，在先公开使用等等，都可纳入本发明的保护范围。
此外，本案中各技术特征的组合方式并不限本案权利要求中所记载的组合方式或是具体实施例所记载的组合方式，本案记载的所有技术特征可以以任何方式进行自由组合或结合，除非相互之间产生矛盾。
还需要注意的是，以上所列举的实施例仅为本发明具体实施例。显然本发明不局限于以上实施例，随之做出的类似变化或变形是本领域技术人员能从本发明公开的内容直接得出或者很容易便联想到的，均应属于本发明的保护范围。

Claims (10)

1.一种自动驾驶车辆高精度定位方法，其特征在于，包括步骤：
100：基于自动驾驶目标路径获得Frenet坐标系下的自动驾驶路径的参考线；
200：获取车辆在直角坐标系下的粗略位置P(x，y)；
300：采用直角坐标系到Frenet坐标系的转换函数XYToSL()将车辆在直角坐标系下的粗略位置P(x，y)转换为Frenet坐标系下的车辆位置坐标P(s，l)；
400：获取车道线在直角坐标系下的车道线位置方程；其中车道线位置方程通过下述步骤获得：
以车辆后轴中心为原点建立车辆坐标系V，设定X轴指向车头方向，Y轴指向车辆左侧，Z轴指向车顶方向，车辆左侧车道线Ll在车辆坐标系V中的方程表达为：
Ax+By+C＝0 (1)
在上述公式(1)中，A，B，C表示方程的系数，其根据不同具体情况下车道线的具体位置而拟合得到；
500：基于所述车道线位置方程计算得到车辆后轴中心偏离车道中心的距离dc：
基于上述公式(1)，得到直线外某点P(x0，y0)到直线Ll的距离d为：
Figure FDA0003748547790000011
根据上述公式(2)求得车辆后轴中心与左车道线Ll的距离为：
Figure FDA0003748547790000012
设车道宽度为W，则车辆后轴中心与车道中心线的距离dcc为：
Figure FDA0003748547790000013
600：基于Frenet坐标系下的车辆位置坐标P(s，l)的纵坐标s以及dc建立Frenet坐标系下的坐标点
Figure FDA0003748547790000014
700：采用Frenet坐标系到直角坐标系的转换函数SLToXY()将Frenet坐标系下的坐标点
Figure FDA0003748547790000015
转换为直角坐标系下的坐标点
Figure FDA0003748547790000016
得到车辆的精确定位。
2.如权利要求1所述的自动驾驶车辆高精度定位方法，其特征在于，在步骤100中，基于高精度地图获得所述自动驾驶目标路径。
3.如权利要求1所述的自动驾驶车辆高精度定位方法，其特征在于，在步骤200中，从下述各项的至少其中一种定位系统获得车辆在直角坐标系下的粗略位置P(x，y)：惯导/卫星组合定位定向系统、GPS导航系统、北斗导航系统、里程计和磁罗盘。
4.如权利要求1所述的自动驾驶车辆高精度定位方法，其特征在于，在步骤400中，基于摄像机检测的车道线图像数据和/或激光雷达检测的车道线点云数据获取车道线在直角坐标系下的车道线位置方程。
5.如权利要求1-4中任意一项所述的自动驾驶车辆高精度定位方法，其特征在于，还包括步骤800：获得坐标点
Figure FDA0003748547790000021
与坐标点P(x，y)的偏差ΔP；将ΔP作为位置误差观测信息，使用卡尔曼滤波器对车辆位姿进行滤波，以输出进一步精确定位的车辆位置坐标。
6.一种自动驾驶车辆高精度定位系统，其特征在于，包括：
路径规划模块，其生成自动驾驶目标路径；
参考线生成模块，其生成Frenet坐标系下的自动驾驶路径的参考线；
定位模块，其获取车辆在直角坐标系下的粗略位置P(x，y)，获取车道线在直角坐标系下的车道线位置方程；采用直角坐标系到Frenet坐标系的转换函数XYToSL()将车辆在直角坐标系下的粗略位置P(x，y)转换为Frenet坐标系下的车辆位置坐标P(s，l)；基于所述车道线位置方程计算得到车辆后轴中心偏离车道中心的距离dc；基于Frenet坐标系下的车辆位置坐标P(s，l)的纵坐标s以及dc建立Frenet坐标系下的坐标点
Figure FDA0003748547790000022
采用Frenet坐标系到直角坐标系的转换函数SLToXY()将Frenet坐标系下的坐标点
Figure FDA0003748547790000023
转换为直角坐标系下的坐标点
Figure FDA0003748547790000024
得到车辆的精确定位；
其中车道线位置方程通过下述步骤获得：
以车辆后轴中心为原点建立车辆坐标系V，设定X轴指向车头方向，Y轴指向车辆左侧，Z轴指向车顶方向，车辆左侧车道线Ll在车辆坐标系V中的方程表达为：
Ax+By+C＝0 (1)
在上述公式(1)中，A，B，C表示方程的系数，其根据不同具体情况下车道线的具体位置而拟合得到；
其中距离dc基于下述步骤获得：
基于上述公式(1)，得到直线外某点P(x0，y0)到直线Ll的距离d为：
Figure FDA0003748547790000031
根据上述公式(2)求得车辆后轴中心与左车道线Ll的距离为：
Figure FDA0003748547790000032
设车道宽度为W，则车辆后轴中心与车道中心线的距离dc为：
Figure FDA0003748547790000033
7.如权利要求6所述的自动驾驶车辆高精度定位系统，其特征在于，所述路径规划模块基于高精度地图生成自动驾驶目标路径。
8.如权利要求6所述的自动驾驶车辆高精度定位系统，其特征在于，车辆在直角坐标系下的粗略位置P(x，y)从下述各项的至少其中一种定位系统获得：惯导/卫星组合定位定向系统、GPS导航系统、北斗导航系统、里程计和磁罗盘。
9.如权利要求6所述的自动驾驶车辆高精度定位系统，其特征在于，基于摄像机检测的车道线图像数据和/或激光雷达检测的车道线点云数据获取车道线在直角坐标系下的车道线位置方程。
10.如权利要求6-9中任意一项所述的自动驾驶车辆高精度定位系统，其特征在于，还包括卡尔曼滤波器，其将坐标点
Figure FDA0003748547790000034
与坐标点P(x，y)的偏差ΔP；ΔP作为位置误差观测信息，以对车辆位姿进行滤波，输出进一步精确定位的车辆位置坐标。
CN202011445401.XA 2020-12-09 2020-12-09 一种自动驾驶车辆高精度定位方法和系统 Active CN112710301B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011445401.XA CN112710301B (zh) 2020-12-09 2020-12-09 一种自动驾驶车辆高精度定位方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011445401.XA CN112710301B (zh) 2020-12-09 2020-12-09 一种自动驾驶车辆高精度定位方法和系统

Publications (2)

Publication Number Publication Date
CN112710301A CN112710301A (zh) 2021-04-27
CN112710301B true CN112710301B (zh) 2023-01-06

Family

ID=75542997

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011445401.XA Active CN112710301B (zh) 2020-12-09 2020-12-09 一种自动驾驶车辆高精度定位方法和系统

Country Status (1)

Country Link
CN (1) CN112710301B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114354209A (zh) * 2021-12-07 2022-04-15 重庆长安汽车股份有限公司 自动驾驶车道线与目标联合模拟方法及系统
CN116504068A (zh) * 2023-06-26 2023-07-28 创辉达设计股份有限公司江苏分公司 车道级车流量的统计方法、装置、计算机设备及存储介质

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110647151A (zh) * 2019-10-16 2020-01-03 北京京东乾石科技有限公司 坐标转换方法及装置、计算机可读存储介质、电子设备

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3483855B2 (ja) * 2001-01-16 2004-01-06 川崎重工業株式会社 車両系の走行制御方法及び装置
JP5799784B2 (ja) * 2011-12-06 2015-10-28 富士通株式会社 道路形状推定装置及びプログラム
CN106828489B (zh) * 2017-02-14 2019-04-26 中国科学院自动化研究所 一种车辆行驶控制方法及装置
KR102541561B1 (ko) * 2018-02-12 2023-06-08 삼성전자주식회사 차량의 주행을 위한 정보를 제공하는 방법 및 그 장치들
CN111912416B (zh) * 2019-05-07 2022-07-29 北京市商汤科技开发有限公司 用于设备定位的方法、装置及设备
CN110834644B (zh) * 2019-10-30 2021-01-19 中国第一汽车股份有限公司 一种车辆控制方法、装置、待控制车辆及存储介质
CN111338335B (zh) * 2019-12-31 2021-02-26 清华大学 一种结构化道路场景下的车辆局部轨迹规划方法
CN111231950B (zh) * 2020-03-05 2021-08-10 北京百度网讯科技有限公司 规划车辆变道路径的方法、装置、设备及可读存储介质
CN111898804B (zh) * 2020-07-10 2021-11-23 东风商用车有限公司 局部规划的道路自适应采样方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110647151A (zh) * 2019-10-16 2020-01-03 北京京东乾石科技有限公司 坐标转换方法及装置、计算机可读存储介质、电子设备

Also Published As

Publication number Publication date
CN112710301A (zh) 2021-04-27

Similar Documents

Publication Publication Date Title
US11307040B2 (en) Map information provision system
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
CN109946732B (zh) 一种基于多传感器数据融合的无人车定位方法
Lee et al. Development of a self-driving car that can handle the adverse weather
Rose et al. An integrated vehicle navigation system utilizing lane-detection and lateral position estimation systems in difficult environments for GPS
Gehrig et al. Dead reckoning and cartography using stereo vision for an autonomous car
CN104061899B (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN107015238A (zh) 基于三维激光雷达的无人车自主定位方法
CN105716617B (zh) 基于车辆数据绘制驾驶轨迹的系统及方法
CN112710301B (zh) 一种自动驾驶车辆高精度定位方法和系统
EP3936822A1 (en) Vehicle positioning method and apparatus, and vehicle, and storage medium
Weiss et al. Precise ego-localization in urban areas using laserscanner and high accuracy feature maps
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN111176298B (zh) 一种无人车轨迹录制与轨迹跟踪方法
CN112433531A (zh) 一种自动驾驶车辆的轨迹跟踪方法、装置及计算机设备
WO2016157428A1 (ja) 計測装置、計測方法、及び、プログラム
CN107228663A (zh) 一种自动导引运输车的定位系统和方法
CN111857121A (zh) 基于惯导和激光雷达的巡逻机器人行走避障方法及系统
JP2021092508A (ja) 走行軌跡推定方法及び走行軌跡推定装置
CN114563795A (zh) 基于激光里程计和标签融合算法的定位追踪方法及系统
Deusch et al. Improving localization in digital maps with grid maps
CN114889606B (zh) 一种基于多传感融合的低成本高精定位方法
CN114475581B (zh) 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法
CN115542925A (zh) 一种无人车横向控制用的偏差精准估计方法
WO2022252220A1 (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
GR01 Patent grant
GR01 Patent grant