CN112558087B - 一种自动驾驶车辆的定位系统及方法 - Google Patents

一种自动驾驶车辆的定位系统及方法 Download PDF

Info

Publication number
CN112558087B
CN112558087B CN202011306720.2A CN202011306720A CN112558087B CN 112558087 B CN112558087 B CN 112558087B CN 202011306720 A CN202011306720 A CN 202011306720A CN 112558087 B CN112558087 B CN 112558087B
Authority
CN
China
Prior art keywords
state
confidence
positioning
laser slam
navigation
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
CN202011306720.2A
Other languages
English (en)
Other versions
CN112558087A (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.)
Dongfeng Motor Corp
Original Assignee
Dongfeng Motor Corp
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 Dongfeng Motor Corp filed Critical Dongfeng Motor Corp
Priority to CN202011306720.2A priority Critical patent/CN112558087B/zh
Publication of CN112558087A publication Critical patent/CN112558087A/zh
Application granted granted Critical
Publication of CN112558087B publication Critical patent/CN112558087B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • 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
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

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

Abstract

本发明公开了一种自动驾驶车辆的定位系统及方法，属于汽车自动驾驶技术领域，在GPS信号良好的地方，使用组合导航模块提供车辆定位结果，保障车辆能够沿预定轨迹正常行驶；在组合导航状态不好的时候，使用多角度覆盖激光雷达SLAM定位算法提供车辆状态信息；组合导航以及激光SLAM状态都不太好的场景下进行使用时，通过当前数据获取到的车道线信息进行短距离定位；采用三种定位方案进行安全冗余，充分保证车辆定位安全与定位可靠性。

Description

一种自动驾驶车辆的定位系统及方法
技术领域
本发明属于汽车自动驾驶技术领域，更具体地，涉及一种自动驾驶车辆的定位系统及方法。
背景技术
近年来，L4级自动驾驶的潮流越来越热，而作为自动驾驶的核心模块--定位系统，其结果的重要性自是不言而喻。当前的定位系统有基于GPS+IMU的组合定位，基于激光/视觉的SLAM(即时定位与建图)等形式，各自有用武之地确也有各自的局限性。
参考专利申请CN109470240A，公开了一种自动驾驶定位方法，该自动驾驶定位方法包括根据需求分别自动切换以下三种自动驾驶的定位技术：在感应到基站的情况下，采用卫星定位和捷联惯导组合的定位技术；在未感应到基站的情况下，采用激光雷达点云和高精度地图匹配的定位技术；在隧道或夜间外界环境光线稳定的情况下，采用视觉里程算法的定位技术。上述定位方法根据不同环境切换不同的定位技术，但是定位的稳定可靠性存在局限性。
发明内容
针对现有技术的以上缺陷或改进需求，本发明提出了一种自动驾驶车辆的定位系统及方法，将GPS+IMU的组合导航，激光SLAM以及视觉辅助划分定位优先级，判断各自的状态优良情况，根据状态的优良情况进行切换定位，为自动驾驶提供一个稳定可靠的定位结果。
为实现上述目的，按照本发明的一个方面，提供了一种自动驾驶车辆的定位方法，包括：
若组合导航定位状态为优，则选择组合导航进行定位，若组合导航定位状态非优，且激光SLAM定位状态为优，则选择激光SLAM进行定位；
若组合导航定位状态与激光SLAM定位状态均为差，则采用视觉辅助定位，通过车道线信息进行短距离定位；
若组合导航定位状态与激光SLAM定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式。
在一些可选的实施方案中，所述若组合导航定位状态与激光SLAM定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式，包括：
基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，在组合导航定位状态为优时，选择组合导航进行定位，在组合导航定位状态不为优时，基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，在激光SLAM定位状态为优时，选择激光SLAM进行定位，在激光SLAM定位状态为非优时，采用视觉辅助定位，通过车道线信息进行短距离定位，其中，组合导航本身给出的状态置信度值越高、自身置信度跳变延时越低、组合导航的位置与GPS的Odom轨迹推算的偏差越小，组合导航定位状态越好；激光SLAM本身给出的状态置信度值越高、自身置信度跳变延时越低、激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差越小，激光SLAM定位状态越好。
在一些可选的实施方案中，所述基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，包括：
在组合导航本身给出的状态置信度值高，并且置信度高的状态跳变小于第一时间阈值时，确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值高表示组合导航本身给出的状态置信度值大于第一组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，则确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值居中表示组合导航本身给出的状态置信度值大于第二组合导航置信度阈值，小于等于第一组合导航置信度阈值，置信度低表示组合导航本身给出的状态置信度值小于等于第二组合导航置信度阈值，第一组合导航置信度阈值大于第二组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差大于等于预设组合导航偏差阈值，或跳变为置信度低的时间大于等于第二时间阈值，则确定组合导航定位状态为非优。
在一些可选的实施方案中，所述基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，包括：
在激光SLAM本身给出的状态置信度值高，并且置信度高的状态跳变小于第三时间阈值时，确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值高表示激光SLAM本身给出的状态置信度值大于第一激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差小于预设SLAM偏差阈值，且跳变为置信度低的时间小于第四时间阈值，则确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值居中表示激光SLAM本身给出的状态置信度值大于第二激光SLAM置信度阈值，小于等于第一激光SLAM置信度阈值，置信度低表示激光SLAM本身给出的状态置信度值小于等于第二激光SLAM置信度阈值，第一激光SLAM置信度阈值大于第二激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差大于等于预设SLAM偏差阈值，或跳变为置信度低的时间大于等于第四时间阈值，则确定激光SLAM定位状态为非优。
在一些可选的实施方案中，在采用视觉辅助定位，通过车道线信息进行短距离定位后，所述方法还包括：
等待激光SLAM与组合导航状态恢复，若预设距离内未恢复，则控制车辆停车。
按照本发明的另一方面，提供了一种自动驾驶车辆的定位系统，包括：
组合导航模块，用于在组合导航定位状态为优时，进行定位；
激光SLAM定位模块，用于在组合导航定位状态非优，且激光SLAM定位状态为优时，进行定位；
视觉辅助模块，用于在组合导航定位状态与激光SLAM定位状态均为差时，采用视觉辅助定位，通过车道线信息进行短距离定位；
控制模块，用于在组合导航定位状态与激光SLAM定位状态均处于一般时，结合odom轨迹推算值确定采用的定位方式。
在一些可选的实施方案中，所述控制模块，用于基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，在组合导航定位状态为优时，选择组合导航进行定位，在组合导航定位状态不为优时，基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，在激光SLAM定位状态为优时，选择激光SLAM进行定位，在激光SLAM定位状态为非优时，采用视觉辅助定位，通过车道线信息进行短距离定位，其中，组合导航本身给出的状态置信度值越高、自身置信度跳变延时越低、组合导航的位置与GPS的Odom轨迹推算的偏差越小，组合导航定位状态越好；激光SLAM本身给出的状态置信度值越高、自身置信度跳变延时越低、激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差越小，激光SLAM定位状态越好。
在一些可选的实施方案中，所述基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，包括：
在组合导航本身给出的状态置信度值高，并且置信度高的状态跳变为置信度非高的时间小于第一时间阈值时，确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值高表示组合导航本身给出的状态置信度值大于第一组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，则确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值居中表示组合导航本身给出的状态置信度值大于第二组合导航置信度阈值，小于等于第一组合导航置信度阈值，置信度低表示组合导航本身给出的状态置信度值小于等于第二组合导航置信度阈值，第一组合导航置信度阈值大于第二组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差大于等于预设组合导航偏差阈值，或跳变为置信度低的时间大于等于第二时间阈值，则确定组合导航定位状态为非优。
在一些可选的实施方案中，所述基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，包括：
在激光SLAM本身给出的状态置信度值高，并且置信度高的状态跳变为置信度非高的时间小于第三时间阈值时，确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值高表示激光SLAM本身给出的状态置信度值大于第一激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差小于预设SLAM偏差阈值，且跳变为置信度低的时间小于第四时间阈值，则确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值居中表示激光SLAM本身给出的状态置信度值大于第二激光SLAM置信度阈值，小于等于第一激光SLAM置信度阈值，置信度低表示激光SLAM本身给出的状态置信度值小于等于第二激光SLAM置信度阈值，第一激光SLAM置信度阈值大于第二激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差大于等于预设SLAM偏差阈值，或跳变为置信度低的时间大于等于第四时间阈值，则确定激光SLAM定位状态为非优。
在一些可选的实施方案中，所述控制模块，还用于在采用视觉辅助定位，通过车道线信息进行短距离定位后，等待激光SLAM与组合导航状态恢复，若预设距离内未恢复，则控制车辆停车。
总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
(1)针对高度自动驾驶系统，本发明提供一种自动驾驶车辆的定位系统及方法，在GPS信号良好的地方，使用组合导航模块提供车辆定位结果，保障车辆能够沿预定轨迹正常行驶；在组合导航状态不好的时候，使用多角度覆盖激光雷达SLAM定位算法提供车辆状态信息；在组合导航以及激光SLAM状态都不太好的场景下，通过当前数据获取到的车道线信息进行短距离定位；定位精度更准确，定位结果更稳定可靠。采用三种定位方案进行安全冗余，充分保证车辆定位安全与定位可靠性。
(2)考虑定位方式可靠性中，不仅考虑定位自身状态置信度，同时引入odom作为第三者判断，加强了对于自身状态判断的可靠性，同时引入状态跳变判断，避免了定位方式的切换频繁，保障了车辆行驶舒适性与安全性。
(3)采用组合导航与激光SLAM算法进行精确定位，将视觉定位作为短距离定位进行补充，采用算力需求不高的方法进行定位保障，保障了安全可靠的同时对于车载控制器算力需求不会产生较大影响。
附图说明
图1是本发明实施例提供的一种自动驾驶车辆的定位方法的流程示意图；
图2是本发明实施例提供的一种定位状态图；
图3是本发明实施例提供的一种定位备份切换逻辑示意图；
图4是本发明实施例提供的一种系统结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
在本发明实例中，“第一”、“第二”等是用于区别不同的对象，而不必用于描述特定的顺序或先后次序。
以下对本发明用到的技术术语进行说明：
GPS：(Global Positioning System)全球定位系统；
IMU：(Inertial Measurement Unit)惯性测量单元；
SLAM：(simultaneous localization and mapping)即时定位与地图构建；
odom：(odometry)里程计坐标系。
如图1所示是本发明实施例提供的一种自动驾驶车辆的定位方法的流程示意图，包括：
S1：若组合导航定位状态为优，则选择组合导航进行定位，若组合导航定位状态非优，且激光SLAM定位状态为优，则选择激光SLAM进行定位；
S2：若组合导航定位状态与激光SLAM定位状态均为差，则采用视觉辅助定位，通过车道线信息进行短距离定位；
S3：若组合导航定位状态与激光SLAM定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式。
在本发明实施例中，组合导航可以为含GPS+IMU的导航方式。
在本发明实施例中，可以将组合导航定位状态和激光SLAM定位状态分为差、一般和优三个状态，可以根据环境条件或者自身的状态置信度值进行划分，划分标准可以根据实际需要确定。
在本发明实施例中，如图2所示，组合导航定位作为主要定位方式，在GPS信号良好的地方提供车辆定位结果，同时需要实时提供组合导航状态信息并结合运动估计计算两者之间的偏差，激光SLAM作为一种备选定位方式，在组合导航状态不好的时候进行替补，并且实时提供激光SLAM的状态信息并结合运动估计计算两者之间的偏差。并且根据组合导航状态信息和激光SLAM的状态信息，将各自定位状态分为3个等级判断(即差、一般和优)，此等级为初始状态等级，车载控制器将结合状态判断与跳变情况，以及将odom拟合作为2者定位状态均一般的第三判断条件，从而综合判断输出对于组合导航与激光定位导航状态的最终状态等级，根据等级选择合适的定位方式。视觉辅助是最后一道定位手段，当组合导航以及激光SLAM状态都不太好的场景下进行使用，通过当前数据获取到的车道线信息进行短距离定位。
在本发明实施例中，如图3所示，若组合导航定位状态与激光SLAM定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式，可以通过以下方式实现：
基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，在组合导航定位状态为优时，选择组合导航进行定位，在组合导航定位状态不为优时，基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，在激光SLAM定位状态为优时，选择激光SLAM进行定位，在激光SLAM定位状态为非优时，采用视觉辅助定位，通过车道线信息进行短距离定位，其中，组合导航本身给出的状态置信度值越高、自身置信度跳变延时越低、组合导航的位置与GPS的Odom轨迹推算的偏差越小，组合导航定位状态越好；激光SLAM本身给出的状态置信度值越高、自身置信度跳变延时越低、激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差越小，激光SLAM定位状态越好。
在本发明实施例中，根据车速信息以及组合导航当前位置信息对轨迹进行推算，得到组合导航的odom轨迹推算值，其中轨迹推算值通过运动模型计算得出，其相关公式如下所示：
Figure BDA0002788542670000091
Figure BDA0002788542670000092
Figure BDA0002788542670000093
Figure BDA0002788542670000094
Figure BDA0002788542670000095
其中相关参数定义及量纲如下：
m-车体质量，kg；
I-车体转动惯量，kg·m2；
v-车体速度，m/s2；
ω-车体横摆角速度，rad/s；
β-质心侧滑角，rad；
kf，kr-前后轮侧偏刚度；
δ-转向角，rad；
lf-前轮到车体质心的距离，m；
lr-后轮到车体质心的距离，m；
α-道路横向坡度，rad；
Pay-侧向空气动力，N；
lpx-气动力中心到质心距离，m；
x，y，θ-车辆在平面上的位置和方位角，m，m，rad。
在本发明实施例中，根据车速信息以及激光SLAM当前位置信息对轨迹进行推算，得到激光SLAM的odom轨迹推算值，轨迹推算公式如上(1)～(5)所示，其中SLAM在定位地图信息匹配时需要给定位置初始信息，其公式如下所示：
Figure BDA0002788542670000101
Figure BDA0002788542670000102
Δx＝rsin(Δθ) (8)
Δy＝r(1-cos(Δθ)) (9)
θ1＝θ0+Δθ (10)
x1＝x0+Δx cos(θ0)-Δy sin(θ0) (11)
y1＝y0+Δy sin(θ0)-Δy cos(θ0) (12)
其中，相关参数定义及量纲如下：
Δsr-右轮走过的距离，m；
Δsl-左轮走过的距离，m；
r-车辆转弯半径，m；
L-车辆后轴轮距，m；
x0，y0，θ0表示t0时刻车辆在平面上的位置和方位角，m，m，rad；
x1，y1，θ1表示t1时刻车辆在平面上的位置和方位角，m，m，rad。
在一些可选的实施方案中，上述基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，可以通过以下方式实现：
在组合导航本身给出的状态置信度值高，并且组合导航在一段时间内其跳变为状态置信度非高的时间小于第一时间阈值时，确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值高表示组合导航本身给出的状态置信度值大于第一组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，则确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值居中表示组合导航本身给出的状态置信度值大于第二组合导航置信度阈值，小于等于第一组合导航置信度阈值，置信度低表示组合导航本身给出的状态置信度值小于等于第二组合导航置信度阈值，第一组合导航置信度阈值大于第二组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差大于等于预设组合导航偏差阈值，或跳变为置信度低的时间大于等于第二时间阈值，则确定组合导航定位状态为非优。
其中，第一时间阈值、第一组合导航置信度阈值、预设组合导航偏差阈值、第二时间阈值及第二组合导航置信度阈值可以通过实际需要确定。
在本发明实施例中，上述基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，可以通过以下方式实现：
在激光SLAM本身给出的状态置信度值高，并且SLAM定位在一段时间内其跳变为状态置信度非高的时间小于第三时间阈值时，确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值高表示激光SLAM本身给出的状态置信度值大于第一激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差小于预设SLAM偏差阈值，且跳变为置信度低的时间小于第四时间阈值，则确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值居中表示激光SLAM本身给出的状态置信度值大于第二激光SLAM置信度阈值，小于等于第一激光SLAM置信度阈值，置信度低表示激光SLAM本身给出的状态置信度值小于等于第二激光SLAM置信度阈值，第一激光SLAM置信度阈值大于第二激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差大于等于预设SLAM偏差阈值，或跳变为置信度低的时间大于等于第四时间阈值，则确定激光SLAM定位状态为非优。
其中，第三时间阈值、第一激光SLAM置信度阈值、预设SLAM偏差阈值、第四时间阈值及第二激光SLAM置信度阈值可以通过实际需要确定。
在本发明实施例中，在采用视觉辅助定位，通过车道线信息进行短距离定位后，等待激光SLAM与组合导航状态恢复，若预设距离内未恢复，则控制车辆停车。
如图4所示，本发明实施例提供了一种自动驾驶车辆的定位系统，其构成包含；组合导航模块(含GPS+IMU)，激光SLAM定位模块(如多角度覆盖激光雷达，可是多视场拼接或者360°激光雷达)，视觉辅助模块(如前向相机)及控制模块(如车载控制器)。组合导航模块能够接受卫星消息实现自车定位，并向车载控制器传递定位信息，多角度覆盖激光雷达通过构建激光雷达全局地图与SLAM定位算法也可实现自车精确定位，并向车载控制器传递定位信息，前向相机可通过视觉识别周围环境信息实现对于车辆的大致定位，向车载控制器传递定位信息和相对车道偏离信息。车载控制器融合所有传感器定位信息，选择最优定位方式并确保车辆定位安全。
其中，组合导航模块为优先定位模式，在室外无遮挡的环境下组合导航状态优良的情况下，可以进行有效定位，当组合导航状态不佳，激光SLAM状态优良的情况下，切换到激光SLAM进行定位，当组合导航以及激光SLAM定位状态都不稳定的情况下，获取当前感知车道线信息，根据车道线位置进行短距离车道保持滑行，同时检测组合导航及激光SLAM定位状态，待状态恢复，及时切回到组合导航及激光SLAM定位，若短距离滑行时间状态未恢复，车辆即停车。
如图2及图3所示，定位的安全备份冗余机制会实时监控判断组合导航定位以及激光SLAM定位状态，当组合导航定位状态为优时，以组合导航定位为主；当组合导航状态非优且激光SLAM定位状态为优时，以激光SLAM定位为主；当组合导航与激光SLAM导航状态都差时，通过感知车道线信息进行短距离行驶辅助定位，等待激光SLAM与组合导航状态恢复；当激光SLAM定位和组合导航定位状态为一般时，需要进一步进行如下判断：
对组合导航的定位状态判断基于三个条件：组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时判断。当置信度高时，并且置信度高的状态跳变小于第一时间阈值，使用组合导航定位；当置信度居中时，判断组合导航的位置与GPS的Odom轨迹推算的偏差，偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，可继续考虑使用组合导航定位，若偏差大于等于预设组合导航偏差阈值或跳变为置信度低时间大于等于第二时间阈值，需要进一步判断激光SLAM定位状态以及与其的偏差：若激光SLAM定位状态优，并且置信度高的状态跳变小于第三时间阈值，即使用激光SLAM定位，若激光SLAM状态一般，且激光SLAM的位置与激光SLAM Odom轨迹推算偏差小于预设SLAM偏差阈值，且跳变为置信度低的时间小于第四时间阈值，使用激光SLAM定位；若激光SLAM的位置与激光SLAM Odom轨迹推算偏差大于预设SLAM偏差阈值或虽然组合导航与激光SLAM状态均一般但跳变时间大于各自对应的时间阈值，则启用感知车道线信息进行短距离行驶辅助定位，等待激光SLAM与组合导航状态恢复。
需要指出，根据实施的需要，可将本申请中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，以实现本发明的目的。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种自动驾驶车辆的定位方法，其特征在于，包括：
若组合导航定位状态为优，则选择组合导航进行定位，若组合导航定位状态非优，且激光SLAM定位状态为优，则选择激光SLAM进行定位；
若组合导航定位状态与激光SLAM定位状态均为差，则采用视觉辅助定位，通过车道线信息进行短距离定位；
若组合导航定位状态与激光SLAM定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式；
所述若组合导航定位状态与激光SLAM定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式，包括：
基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，在组合导航定位状态为优时，选择组合导航进行定位，在组合导航定位状态不为优时，基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，在激光SLAM定位状态为优时，选择激光SLAM进行定位，在激光SLAM定位状态为非优时，采用视觉辅助定位，通过车道线信息进行短距离定位，其中，组合导航本身给出的状态置信度值越高、自身置信度跳变延时越低、组合导航的位置与GPS的Odom轨迹推算的偏差越小，组合导航定位状态越好；激光SLAM本身给出的状态置信度值越高、自身置信度跳变延时越低、激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差越小，激光SLAM定位状态越好；
所述基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，包括：
在组合导航本身给出的状态置信度值高，并且置信度高的状态跳变为置信度非高的时间小于第一时间阈值时，确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值高表示组合导航本身给出的状态置信度值大于第一组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，则确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值居中表示组合导航本身给出的状态置信度值大于第二组合导航置信度阈值，小于等于第一组合导航置信度阈值，置信度低表示组合导航本身给出的状态置信度值小于等于第二组合导航置信度阈值，第一组合导航置信度阈值大于第二组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差大于等于预设组合导航偏差阈值，或跳变为置信度低的时间大于等于第二时间阈值，则确定组合导航定位状态为非优。
2.根据权利要求1所述的定位方法，其特征在于，所述基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，包括：
在激光SLAM本身给出的状态置信度值高，并且置信度高的状态跳变为置信度非高的时间小于第三时间阈值时，确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值高表示激光SLAM本身给出的状态置信度值大于第一激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差小于预设SLAM偏差阈值，且跳变为置信度低的时间小于第四时间阈值，则确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值居中表示激光SLAM本身给出的状态置信度值大于第二激光SLAM置信度阈值，小于等于第一激光SLAM置信度阈值，置信度低表示激光SLAM本身给出的状态置信度值小于等于第二激光SLAM置信度阈值，第一激光SLAM置信度阈值大于第二激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差大于等于预设SLAM偏差阈值，或跳变为置信度低的时间大于等于第四时间阈值，则确定激光SLAM定位状态为非优。
3.根据权利要求1所述的定位方法，其特征在于，在采用视觉辅助定位，通过车道线信息进行短距离定位后，所述方法还包括：
等待激光SLAM与组合导航状态恢复，若预设距离内未恢复，则控制车辆停车。
4.一种自动驾驶车辆的定位系统，其特征在于，包括：
组合导航模块，用于在组合导航定位状态为优时，进行定位；
激光SLAM定位模块，用于在组合导航定位状态非优，且激光SLAM定位状态为优时，进行定位；
视觉辅助模块，用于在组合导航定位状态与激光SLAM定位状态均为差时，采用视觉辅助定位，通过车道线信息进行短距离定位；
控制模块，用于在组合导航定位状态与激光SLAM定位状态均处于一般时，结合odom轨迹推算值确定采用的定位方式；
所述控制模块，用于基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，在组合导航定位状态为优时，选择组合导航进行定位，在组合导航定位状态不为优时，基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，在激光SLAM定位状态为优时，选择激光SLAM进行定位，在激光SLAM定位状态为非优时，采用视觉辅助定位，通过车道线信息进行短距离定位，其中，组合导航本身给出的状态置信度值越高、自身置信度跳变延时越低、组合导航的位置与GPS的Odom轨迹推算的偏差越小，组合导航定位状态越好；激光SLAM本身给出的状态置信度值越高、自身置信度跳变延时越低、激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差越小，激光SLAM定位状态越好；
所述基于组合导航本身给出的状态置信度值、GPS的Odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，包括：
在组合导航本身给出的状态置信度值高，并且置信度高的状态跳变小于第一时间阈值时，确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值高表示组合导航本身给出的状态置信度值大于第一组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，则确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值居中表示组合导航本身给出的状态置信度值大于第二组合导航置信度阈值，小于等于第一组合导航置信度阈值，置信度低表示组合导航本身给出的状态置信度值小于等于第二组合导航置信度阈值，第一组合导航置信度阈值大于第二组合导航置信度阈值；
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与GPS的Odom轨迹推算的偏差大于等于预设组合导航偏差阈值，或跳变为置信度低的时间大于等于第二时间阈值，则确定组合导航定位状态为非优。
5.根据权利要求4所述的定位系统，其特征在于，所述基于激光SLAM本身给出的状态置信度值、激光SLAM的Odom轨迹推算及自身置信度跳变延时确定激光SLAM定位状态，包括：
在激光SLAM本身给出的状态置信度值高，并且置信度高的状态跳变小于第三时间阈值时，确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值高表示激光SLAM本身给出的状态置信度值大于第一激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差小于预设SLAM偏差阈值，且跳变为置信度低的时间小于第四时间阈值，则确定激光SLAM定位状态为优，其中，激光SLAM本身给出的状态置信度值居中表示激光SLAM本身给出的状态置信度值大于第二激光SLAM置信度阈值，小于等于第一激光SLAM置信度阈值，置信度低表示激光SLAM本身给出的状态置信度值小于等于第二激光SLAM置信度阈值，第一激光SLAM置信度阈值大于第二激光SLAM置信度阈值；
在激光SLAM本身给出的状态置信度值居中时，若激光SLAM的位置与激光SLAM的Odom轨迹推算的偏差大于等于预设SLAM偏差阈值，或跳变为置信度低的时间大于等于第四时间阈值，则确定激光SLAM定位状态为非优。
6.根据权利要求4所述的定位系统，其特征在于，所述控制模块，还用于在采用视觉辅助定位，通过车道线信息进行短距离定位后，等待激光SLAM与组合导航状态恢复，若预设距离内未恢复，则控制车辆停车。
CN202011306720.2A 2020-11-20 2020-11-20 一种自动驾驶车辆的定位系统及方法 Active CN112558087B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011306720.2A CN112558087B (zh) 2020-11-20 2020-11-20 一种自动驾驶车辆的定位系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011306720.2A CN112558087B (zh) 2020-11-20 2020-11-20 一种自动驾驶车辆的定位系统及方法

Publications (2)

Publication Number Publication Date
CN112558087A CN112558087A (zh) 2021-03-26
CN112558087B true CN112558087B (zh) 2023-06-23

Family

ID=75043983

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011306720.2A Active CN112558087B (zh) 2020-11-20 2020-11-20 一种自动驾驶车辆的定位系统及方法

Country Status (1)

Country Link
CN (1) CN112558087B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113296512B (zh) * 2021-05-24 2022-10-04 福建盛海智能科技有限公司 一种基于激光雷达和gps的无人循迹驾驶方法与终端
CN113282090A (zh) * 2021-05-31 2021-08-20 三一专用汽车有限责任公司 工程车辆无人驾驶控制方法、装置、工程车辆及电子设备
CN113900517B (zh) * 2021-09-30 2022-12-20 北京百度网讯科技有限公司 线路导航方法和装置、电子设备、计算机可读介质
CN115856979B (zh) * 2023-02-16 2023-06-02 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE3910912A1 (de) * 1989-04-05 1990-10-18 Ar Autonome Roboter Gmbh Navigationsverfahren mit einem koppelnavigationssystem und einem magnetsensor fuer frei navigierende transportfahrzeuge im industriellen bereich
CN107402005A (zh) * 2016-05-20 2017-11-28 北京自动化控制设备研究所 一种基于惯性/里程计/rfid的高精度组合导航方法
CN109931926A (zh) * 2019-04-04 2019-06-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航算法
EP3629057A2 (en) * 2018-09-27 2020-04-01 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for calibrating relative pose and medium
CN111751852A (zh) * 2020-06-17 2020-10-09 北京联合大学 基于点云配准的无人车gnss定位可靠性评价方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1500907B1 (en) * 2003-07-21 2014-11-12 LG Electronics, Inc. Apparatus and method for detecting vehicle location in navigation system
US8374785B2 (en) * 2010-01-28 2013-02-12 Eride, Inc. Tightly coupled GPS and dead-reckoning vehicle navigation
FR3057348B1 (fr) * 2016-10-06 2019-07-19 Thales Procede de localisation ponctuelle d'un vehicule evoluant sur une trajectoire contrainte et systeme associe
WO2019119289A1 (zh) * 2017-12-20 2019-06-27 深圳前海达闼云端智能科技有限公司 一种定位方法、装置及电子设备、计算机程序产品
CN109084732B (zh) * 2018-06-29 2021-01-12 北京旷视科技有限公司 定位与导航方法、装置及处理设备
CN109470240A (zh) * 2018-11-15 2019-03-15 安徽酷哇机器人有限公司 自动驾驶定位方法
CN111522003B (zh) * 2019-01-31 2022-11-11 广州汽车集团股份有限公司 车辆定位方法及系统、计算机设备、车辆、存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE3910912A1 (de) * 1989-04-05 1990-10-18 Ar Autonome Roboter Gmbh Navigationsverfahren mit einem koppelnavigationssystem und einem magnetsensor fuer frei navigierende transportfahrzeuge im industriellen bereich
CN107402005A (zh) * 2016-05-20 2017-11-28 北京自动化控制设备研究所 一种基于惯性/里程计/rfid的高精度组合导航方法
EP3629057A2 (en) * 2018-09-27 2020-04-01 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for calibrating relative pose and medium
CN109931926A (zh) * 2019-04-04 2019-06-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航算法
CN111751852A (zh) * 2020-06-17 2020-10-09 北京联合大学 基于点云配准的无人车gnss定位可靠性评价方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Casual video watching during sensor guided navigation;Euijai Ahn et al;《 Proceedings of the 12th ACM SIGGRAPH International Conference on virtual-reality continuum and its applications in industry》;全文 *
车载GPS/DR/电子地图组合定位导航研究;宫雨生 等;辽宁工程技术大学学报;第24卷增刊;全文 *

Also Published As

Publication number Publication date
CN112558087A (zh) 2021-03-26

Similar Documents

Publication Publication Date Title
CN112558087B (zh) 一种自动驾驶车辆的定位系统及方法
US11951975B1 (en) Handling sensor occlusions for autonomous vehicles
CN107908186B (zh) 用于控制无人驾驶车辆运行的方法及系统
US10179588B2 (en) Autonomous vehicle control system
JP7199163B2 (ja) 位置推定方法、装置及びコンピュータプログラム
US8558758B2 (en) Information display apparatus
JP2019532292A (ja) 車両位置特定の自律走行車両
JP2022502722A (ja) 自律運転車両の屋外運転システム
EP3770549B1 (en) Information processing device, movement device, method, and program
KR20220033477A (ko) 자동 발렛 파킹 시스템의 위치 추정 장치 및 방법
US20120303222A1 (en) Driver assistance system
CN105008857A (zh) 用于汽车的智能视频导航
US11100675B2 (en) Information processing apparatus, information processing method, program, and moving body
CN111959499B (zh) 一种车辆控制方法及装置
Hu et al. Real-time data fusion on tracking camera pose for direct visual guidance
JP2005098853A (ja) 地図データ更新方法および地図データ更新装置
CN108334078A (zh) 一种基于高精度地图领航的自动驾驶方法及系统
US11585945B2 (en) Method for the satellite-supported determination of a position of a vehicle
CN111413990A (zh) 一种车道变更轨迹规划系统
JP2019061603A (ja) 情報処理装置、移動装置、および方法、並びにプログラム
JPWO2004048895A1 (ja) 移動体ナビゲート情報表示方法および移動体ナビゲート情報表示装置
Michalek et al. Precise docking at charging stations for large-capacity vehicles: An advanced driver-assistance system for drivers of electric urban buses
Katriniok Optimal vehicle dynamics control and state estimation for a low-cost GNSS-based collision avoidance system
US11148684B2 (en) Vehicle control device
CN113655801B (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