一种自动驾驶车辆的定位系统及方法与流程

1.本发明属于汽车自动驾驶技术领域，更具体地，涉及一种自动驾驶车辆的定位系统及方法。


背景技术：

2.近年来，l4级自动驾驶的潮流越来越热，而作为自动驾驶的核心模块
‑‑
定位系统，其结果的重要性自是不言而喻。当前的定位系统有基于gps+imu的组合定位，基于激光/视觉的slam(即时定位与建图)等形式，各自有用武之地确也有各自的局限性。
3.参考专利申请cn109470240a，公开了一种自动驾驶定位方法，该自动驾驶定位方法包括根据需求分别自动切换以下三种自动驾驶的定位技术：在感应到基站的情况下，采用卫星定位和捷联惯导组合的定位技术；在未感应到基站的情况下，采用激光雷达点云和高精度地图匹配的定位技术；在隧道或夜间外界环境光线稳定的情况下，采用视觉里程算法的定位技术。上述定位方法根据不同环境切换不同的定位技术，但是定位的稳定可靠性存在局限性。


技术实现要素：

4.针对现有技术的以上缺陷或改进需求，本发明提出了一种自动驾驶车辆的定位系统及方法，将gps+imu的组合导航，激光slam以及视觉辅助划分定位优先级，判断各自的状态优良情况，根据状态的优良情况进行切换定位，为自动驾驶提供一个稳定可靠的定位结果。
5.为实现上述目的，按照本发明的一个方面，提供了一种自动驾驶车辆的定位方法，包括：
6.若组合导航定位状态为优，则选择组合导航进行定位，若组合导航定位状态非优，且激光slam定位状态为优，则选择激光slam进行定位；
7.若组合导航定位状态与激光slam定位状态均为差，则采用视觉辅助定位，通过车道线信息进行短距离定位；
8.若组合导航定位状态与激光slam定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式。
9.在一些可选的实施方案中，所述若组合导航定位状态与激光slam定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式，包括：
10.基于组合导航本身给出的状态置信度值、gps的odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，在组合导航定位状态为优时，选择组合导航进行定位，在组合导航定位状态不为优时，基于激光slam本身给出的状态置信度值、激光slam的odom轨迹推算及自身置信度跳变延时确定激光slam定位状态，在激光slam定位状态为优时，选择激光slam进行定位，在激光slam定位状态为非优时，采用视觉辅助定位，通过车道线信息进行短距离定位，其中，组合导航本身给出的状态置信度值越高、自身置信度跳变延时越低、组合
导航的位置与gps的odom轨迹推算的偏差越小，组合导航定位状态越好；激光slam本身给出的状态置信度值越高、自身置信度跳变延时越低、激光slam的位置与激光slam的odom轨迹推算的偏差越小，激光slam定位状态越好。
11.在一些可选的实施方案中，所述基于组合导航本身给出的状态置信度值、gps的odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，包括：
12.在组合导航本身给出的状态置信度值高，并且置信度高的状态跳变小于第一时间阈值时，确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值高表示组合导航本身给出的状态置信度值大于第一组合导航置信度阈值；
13.在组合导航本身给出的状态置信度值居中时，若组合导航的位置与gps的odom轨迹推算的偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，则确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值居中表示组合导航本身给出的状态置信度值大于第二组合导航置信度阈值，小于等于第一组合导航置信度阈值，置信度低表示组合导航本身给出的状态置信度值小于等于第二组合导航置信度阈值，第一组合导航置信度阈值大于第二组合导航置信度阈值；
14.在组合导航本身给出的状态置信度值居中时，若组合导航的位置与gps的odom轨迹推算的偏差大于等于预设组合导航偏差阈值，或跳变为置信度低的时间大于等于第二时间阈值，则确定组合导航定位状态为非优。
15.在一些可选的实施方案中，所述基于激光slam本身给出的状态置信度值、激光slam的odom轨迹推算及自身置信度跳变延时确定激光slam定位状态，包括：
16.在激光slam本身给出的状态置信度值高，并且置信度高的状态跳变小于第三时间阈值时，确定激光slam定位状态为优，其中，激光slam本身给出的状态置信度值高表示激光slam本身给出的状态置信度值大于第一激光slam置信度阈值；
17.在激光slam本身给出的状态置信度值居中时，若激光slam的位置与激光slam的odom轨迹推算的偏差小于预设slam偏差阈值，且跳变为置信度低的时间小于第四时间阈值，则确定激光slam定位状态为优，其中，激光slam本身给出的状态置信度值居中表示激光slam本身给出的状态置信度值大于第二激光slam置信度阈值，小于等于第一激光slam置信度阈值，置信度低表示激光slam本身给出的状态置信度值小于等于第二激光slam置信度阈值，第一激光slam置信度阈值大于第二激光slam置信度阈值；
18.在激光slam本身给出的状态置信度值居中时，若激光slam的位置与激光slam的odom轨迹推算的偏差大于等于预设slam偏差阈值，或跳变为置信度低的时间大于等于第四时间阈值，则确定激光slam定位状态为非优。
19.在一些可选的实施方案中，在采用视觉辅助定位，通过车道线信息进行短距离定位后，所述方法还包括：
20.等待激光slam与组合导航状态恢复，若预设距离内未恢复，则控制车辆停车。
21.按照本发明的另一方面，提供了一种自动驾驶车辆的定位系统，包括：
22.组合导航模块，用于在组合导航定位状态为优时，进行定位；
23.激光slam定位模块，用于在组合导航定位状态非优，且激光slam定位状态为优时，进行定位；
24.视觉辅助模块，用于在组合导航定位状态与激光slam定位状态均为差时，采用视
觉辅助定位，通过车道线信息进行短距离定位；
25.控制模块，用于在组合导航定位状态与激光slam定位状态均处于一般时，结合odom轨迹推算值确定采用的定位方式。
26.在一些可选的实施方案中，所述控制模块，用于基于组合导航本身给出的状态置信度值、gps的odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，在组合导航定位状态为优时，选择组合导航进行定位，在组合导航定位状态不为优时，基于激光slam本身给出的状态置信度值、激光slam的odom轨迹推算及自身置信度跳变延时确定激光slam定位状态，在激光slam定位状态为优时，选择激光slam进行定位，在激光slam定位状态为非优时，采用视觉辅助定位，通过车道线信息进行短距离定位，其中，组合导航本身给出的状态置信度值越高、自身置信度跳变延时越低、组合导航的位置与gps的odom轨迹推算的偏差越小，组合导航定位状态越好；激光slam本身给出的状态置信度值越高、自身置信度跳变延时越低、激光slam的位置与激光slam的odom轨迹推算的偏差越小，激光slam定位状态越好。
27.在一些可选的实施方案中，所述基于组合导航本身给出的状态置信度值、gps的odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，包括：
28.在组合导航本身给出的状态置信度值高，并且置信度高的状态跳变为置信度非高的时间小于第一时间阈值时，确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值高表示组合导航本身给出的状态置信度值大于第一组合导航置信度阈值；
29.在组合导航本身给出的状态置信度值居中时，若组合导航的位置与gps的odom轨迹推算的偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，则确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值居中表示组合导航本身给出的状态置信度值大于第二组合导航置信度阈值，小于等于第一组合导航置信度阈值，置信度低表示组合导航本身给出的状态置信度值小于等于第二组合导航置信度阈值，第一组合导航置信度阈值大于第二组合导航置信度阈值；
30.在组合导航本身给出的状态置信度值居中时，若组合导航的位置与gps的odom轨迹推算的偏差大于等于预设组合导航偏差阈值，或跳变为置信度低的时间大于等于第二时间阈值，则确定组合导航定位状态为非优。
31.在一些可选的实施方案中，所述基于激光slam本身给出的状态置信度值、激光slam的odom轨迹推算及自身置信度跳变延时确定激光slam定位状态，包括：
32.在激光slam本身给出的状态置信度值高，并且置信度高的状态跳变为置信度非高的时间小于第三时间阈值时，确定激光slam定位状态为优，其中，激光slam本身给出的状态置信度值高表示激光slam本身给出的状态置信度值大于第一激光slam置信度阈值；
33.在激光slam本身给出的状态置信度值居中时，若激光slam的位置与激光slam的odom轨迹推算的偏差小于预设slam偏差阈值，且跳变为置信度低的时间小于第四时间阈值，则确定激光slam定位状态为优，其中，激光slam本身给出的状态置信度值居中表示激光slam本身给出的状态置信度值大于第二激光slam置信度阈值，小于等于第一激光slam置信度阈值，置信度低表示激光slam本身给出的状态置信度值小于等于第二激光slam置信度阈值，第一激光slam置信度阈值大于第二激光slam置信度阈值；
34.在激光slam本身给出的状态置信度值居中时，若激光slam的位置与激光slam的odom轨迹推算的偏差大于等于预设slam偏差阈值，或跳变为置信度低的时间大于等于第四
时间阈值，则确定激光slam定位状态为非优。
35.在一些可选的实施方案中，所述控制模块，还用于在采用视觉辅助定位，通过车道线信息进行短距离定位后，等待激光slam与组合导航状态恢复，若预设距离内未恢复，则控制车辆停车。
36.总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
37.(1)针对高度自动驾驶系统，本发明提供一种自动驾驶车辆的定位系统及方法，在gps信号良好的地方，使用组合导航模块提供车辆定位结果，保障车辆能够沿预定轨迹正常行驶；在组合导航状态不好的时候，使用多角度覆盖激光雷达slam定位算法提供车辆状态信息；在组合导航以及激光slam状态都不太好的场景下，通过当前数据获取到的车道线信息进行短距离定位；定位精度更准确，定位结果更稳定可靠。采用三种定位方案进行安全冗余，充分保证车辆定位安全与定位可靠性。
38.(2)考虑定位方式可靠性中，不仅考虑定位自身状态置信度，同时引入odom作为第三者判断，加强了对于自身状态判断的可靠性，同时引入状态跳变判断，避免了定位方式的切换频繁，保障了车辆行驶舒适性与安全性。
39.(3)采用组合导航与激光slam算法进行精确定位，将视觉定位作为短距离定位进行补充，采用算力需求不高的方法进行定位保障，保障了安全可靠的同时对于车载控制器算力需求不会产生较大影响。
附图说明
40.图1是本发明实施例提供的一种自动驾驶车辆的定位方法的流程示意图；
41.图2是本发明实施例提供的一种定位状态图；
42.图3是本发明实施例提供的一种定位备份切换逻辑示意图；
43.图4是本发明实施例提供的一种系统结构示意图。
具体实施方式
44.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
45.在本发明实例中，“第一”、“第二”等是用于区别不同的对象，而不必用于描述特定的顺序或先后次序。
46.以下对本发明用到的技术术语进行说明：
47.gps：(global positioning system)全球定位系统；
48.imu：(inertial measurement unit)惯性测量单元；
49.slam：(simultaneous localization and mapping)即时定位与地图构建；
50.odom：(odometry)里程计坐标系。
51.如图1所示是本发明实施例提供的一种自动驾驶车辆的定位方法的流程示意图，包括：
52.s1：若组合导航定位状态为优，则选择组合导航进行定位，若组合导航定位状态非优，且激光slam定位状态为优，则选择激光slam进行定位；
53.s2：若组合导航定位状态与激光slam定位状态均为差，则采用视觉辅助定位，通过车道线信息进行短距离定位；
54.s3：若组合导航定位状态与激光slam定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式。
55.在本发明实施例中，组合导航可以为含gps+imu的导航方式。
56.在本发明实施例中，可以将组合导航定位状态和激光slam定位状态分为差、一般和优三个状态，可以根据环境条件或者自身的状态置信度值进行划分，划分标准可以根据实际需要确定。
57.在本发明实施例中，如图2所示，组合导航定位作为主要定位方式，在gps信号良好的地方提供车辆定位结果，同时需要实时提供组合导航状态信息并结合运动估计计算两者之间的偏差，激光slam作为一种备选定位方式，在组合导航状态不好的时候进行替补，并且实时提供激光slam的状态信息并结合运动估计计算两者之间的偏差。并且根据组合导航状态信息和激光slam的状态信息，将各自定位状态分为3个等级判断(即差、一般和优)，此等级为初始状态等级，车载控制器将结合状态判断与跳变情况，以及将odom拟合作为2者定位状态均一般的第三判断条件，从而综合判断输出对于组合导航与激光定位导航状态的最终状态等级，根据等级选择合适的定位方式。视觉辅助是最后一道定位手段，当组合导航以及激光slam状态都不太好的场景下进行使用，通过当前数据获取到的车道线信息进行短距离定位。
58.在本发明实施例中，如图3所示，若组合导航定位状态与激光slam定位状态均处于一般，则结合odom轨迹推算值确定采用的定位方式，可以通过以下方式实现：
59.基于组合导航本身给出的状态置信度值、gps的odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，在组合导航定位状态为优时，选择组合导航进行定位，在组合导航定位状态不为优时，基于激光slam本身给出的状态置信度值、激光slam的odom轨迹推算及自身置信度跳变延时确定激光slam定位状态，在激光slam定位状态为优时，选择激光slam进行定位，在激光slam定位状态为非优时，采用视觉辅助定位，通过车道线信息进行短距离定位，其中，组合导航本身给出的状态置信度值越高、自身置信度跳变延时越低、组合导航的位置与gps的odom轨迹推算的偏差越小，组合导航定位状态越好；激光slam本身给出的状态置信度值越高、自身置信度跳变延时越低、激光slam的位置与激光slam的odom轨迹推算的偏差越小，激光slam定位状态越好。
60.在本发明实施例中，根据车速信息以及组合导航当前位置信息对轨迹进行推算，得到组合导航的odom轨迹推算值，其中轨迹推算值通过运动模型计算得出，其相关公式如下所示：
[0061][0062][0063]
[0064][0065][0066]
其中相关参数定义及量纲如下：
[0067]
m
‑
车体质量，kg；
[0068]
i
‑
车体转动惯量，kg
·
m2；
[0069]
v
‑
车体速度，m/s2；
[0070]
ω
‑
车体横摆角速度，rad/s；
[0071]
β
‑
质心侧滑角，rad；
[0072]
k
f
，k
r
‑
前后轮侧偏刚度；
[0073]
δ
‑
转向角，rad；
[0074]
l
f
‑
前轮到车体质心的距离，m；
[0075]
l
r
‑
后轮到车体质心的距离，m；
[0076]
α
‑
道路横向坡度，rad；
[0077]
p
ay
‑
侧向空气动力，n；
[0078]
l
px
‑
气动力中心到质心距离，m；
[0079]
x，y，θ
‑
车辆在平面上的位置和方位角，m，m，rad。
[0080]
在本发明实施例中，根据车速信息以及激光slam当前位置信息对轨迹进行推算，得到激光slam的odom轨迹推算值，轨迹推算公式如上(1)～(5)所示，其中slam在定位地图信息匹配时需要给定位置初始信息，其公式如下所示：
[0081][0082][0083]
δx＝rsin(δθ)
ꢀꢀꢀ
(8)
[0084]
δy＝r(1
‑
cos(δθ))
ꢀꢀꢀ
(9)
[0085]
θ1＝θ0+δθ
ꢀꢀꢀ
(10)
[0086]
x1＝x0+δx cos(θ0)
‑
δy sin(θ0)
ꢀꢀꢀ
(11)
[0087]
y1＝y0+δy sin(θ0)
‑
δy cos(θ0)
ꢀꢀꢀ
(12)
[0088]
其中，相关参数定义及量纲如下：
[0089]
δs
r
‑
右轮走过的距离，m；
[0090]
δs
l
‑
左轮走过的距离，m；
[0091]
r
‑
车辆转弯半径，m；
[0092]
l
‑
车辆后轴轮距，m；
[0093]
x0，y0，θ0表示t0时刻车辆在平面上的位置和方位角，m，m，rad；
[0094]
x1，y1，θ1表示t1时刻车辆在平面上的位置和方位角，m，m，rad。
[0095]
在一些可选的实施方案中，上述基于组合导航本身给出的状态置信度值、gps的odom轨迹推算及自身置信度跳变延时确定组合导航定位状态，可以通过以下方式实现：
[0096]
在组合导航本身给出的状态置信度值高，并且组合导航在一段时间内其跳变为状
态置信度非高的时间小于第一时间阈值时，确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值高表示组合导航本身给出的状态置信度值大于第一组合导航置信度阈值；
[0097]
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与gps的odom轨迹推算的偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，则确定组合导航定位状态为优，其中，组合导航本身给出的状态置信度值居中表示组合导航本身给出的状态置信度值大于第二组合导航置信度阈值，小于等于第一组合导航置信度阈值，置信度低表示组合导航本身给出的状态置信度值小于等于第二组合导航置信度阈值，第一组合导航置信度阈值大于第二组合导航置信度阈值；
[0098]
在组合导航本身给出的状态置信度值居中时，若组合导航的位置与gps的odom轨迹推算的偏差大于等于预设组合导航偏差阈值，或跳变为置信度低的时间大于等于第二时间阈值，则确定组合导航定位状态为非优。
[0099]
其中，第一时间阈值、第一组合导航置信度阈值、预设组合导航偏差阈值、第二时间阈值及第二组合导航置信度阈值可以通过实际需要确定。
[0100]
在本发明实施例中，上述基于激光slam本身给出的状态置信度值、激光slam的odom轨迹推算及自身置信度跳变延时确定激光slam定位状态，可以通过以下方式实现：
[0101]
在激光slam本身给出的状态置信度值高，并且slam定位在一段时间内其跳变为状态置信度非高的时间小于第三时间阈值时，确定激光slam定位状态为优，其中，激光slam本身给出的状态置信度值高表示激光slam本身给出的状态置信度值大于第一激光slam置信度阈值；
[0102]
在激光slam本身给出的状态置信度值居中时，若激光slam的位置与激光slam的odom轨迹推算的偏差小于预设slam偏差阈值，且跳变为置信度低的时间小于第四时间阈值，则确定激光slam定位状态为优，其中，激光slam本身给出的状态置信度值居中表示激光slam本身给出的状态置信度值大于第二激光slam置信度阈值，小于等于第一激光slam置信度阈值，置信度低表示激光slam本身给出的状态置信度值小于等于第二激光slam置信度阈值，第一激光slam置信度阈值大于第二激光slam置信度阈值；
[0103]
在激光slam本身给出的状态置信度值居中时，若激光slam的位置与激光slam的odom轨迹推算的偏差大于等于预设slam偏差阈值，或跳变为置信度低的时间大于等于第四时间阈值，则确定激光slam定位状态为非优。
[0104]
其中，第三时间阈值、第一激光slam置信度阈值、预设slam偏差阈值、第四时间阈值及第二激光slam置信度阈值可以通过实际需要确定。
[0105]
在本发明实施例中，在采用视觉辅助定位，通过车道线信息进行短距离定位后，等待激光slam与组合导航状态恢复，若预设距离内未恢复，则控制车辆停车。
[0106]
如图4所示，本发明实施例提供了一种自动驾驶车辆的定位系统，其构成包含；组合导航模块(含gps+imu)，激光slam定位模块(如多角度覆盖激光雷达，可是多视场拼接或者360
°
激光雷达)，视觉辅助模块(如前向相机)及控制模块(如车载控制器)。组合导航模块能够接受卫星消息实现自车定位，并向车载控制器传递定位信息，多角度覆盖激光雷达通过构建激光雷达全局地图与slam定位算法也可实现自车精确定位，并向车载控制器传递定位信息，前向相机可通过视觉识别周围环境信息实现对于车辆的大致定位，向车载控制器
传递定位信息和相对车道偏离信息。车载控制器融合所有传感器定位信息，选择最优定位方式并确保车辆定位安全。
[0107]
其中，组合导航模块为优先定位模式，在室外无遮挡的环境下组合导航状态优良的情况下，可以进行有效定位，当组合导航状态不佳，激光slam状态优良的情况下，切换到激光slam进行定位，当组合导航以及激光slam定位状态都不稳定的情况下，获取当前感知车道线信息，根据车道线位置进行短距离车道保持滑行，同时检测组合导航及激光slam定位状态，待状态恢复，及时切回到组合导航及激光slam定位，若短距离滑行时间状态未恢复，车辆即停车。
[0108]
如图2及图3所示，定位的安全备份冗余机制会实时监控判断组合导航定位以及激光slam定位状态，当组合导航定位状态为优时，以组合导航定位为主；当组合导航状态非优且激光slam定位状态为优时，以激光slam定位为主；当组合导航与激光slam导航状态都差时，通过感知车道线信息进行短距离行驶辅助定位，等待激光slam与组合导航状态恢复；当激光slam定位和组合导航定位状态为一般时，需要进一步进行如下判断：
[0109]
对组合导航的定位状态判断基于三个条件：组合导航本身给出的状态置信度值、gps的odom轨迹推算及自身置信度跳变延时判断。当置信度高时，并且置信度高的状态跳变小于第一时间阈值，使用组合导航定位；当置信度居中时，判断组合导航的位置与gps的odom轨迹推算的偏差，偏差小于预设组合导航偏差阈值，且跳变为置信度低的时间小于第二时间阈值，可继续考虑使用组合导航定位，若偏差大于等于预设组合导航偏差阈值或跳变为置信度低时间大于等于第二时间阈值，需要进一步判断激光slam定位状态以及与其的偏差：若激光slam定位状态优，并且置信度高的状态跳变小于第三时间阈值，即使用激光slam定位，若激光slam状态一般，且激光slam的位置与激光slam odom轨迹推算偏差小于预设slam偏差阈值，且跳变为置信度低的时间小于第四时间阈值，使用激光slam定位；若激光slam的位置与激光slam odom轨迹推算偏差大于预设slam偏差阈值或虽然组合导航与激光slam状态均一般但跳变时间大于各自对应的时间阈值，则启用感知车道线信息进行短距离行驶辅助定位，等待激光slam与组合导航状态恢复。
[0110]
需要指出，根据实施的需要，可将本申请中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，以实现本发明的目的。
[0111]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。