CN115046547A - 一种复杂城市环境下的自动驾驶汽车定位***及方法 - Google Patents
一种复杂城市环境下的自动驾驶汽车定位***及方法 Download PDFInfo
- Publication number
- CN115046547A CN115046547A CN202210506316.2A CN202210506316A CN115046547A CN 115046547 A CN115046547 A CN 115046547A CN 202210506316 A CN202210506316 A CN 202210506316A CN 115046547 A CN115046547 A CN 115046547A
- Authority
- CN
- China
- Prior art keywords
- time
- vehicle
- information
- unit
- representing
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 18
- 239000011159 matrix material Substances 0.000 claims abstract description 100
- 238000001514 detection method Methods 0.000 claims abstract description 49
- 230000001133 acceleration Effects 0.000 claims description 28
- 238000005259 measurement Methods 0.000 claims description 12
- 230000004927 fusion Effects 0.000 claims description 10
- 238000001914 filtration Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 9
- 230000000694 effects Effects 0.000 claims description 8
- 238000013507 mapping Methods 0.000 claims description 6
- 150000001875 compounds Chemical class 0.000 claims description 3
- 230000007812 deficiency Effects 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 claims description 3
- LIWAQLJGPBVORC-UHFFFAOYSA-N ethylmethylamine Chemical compound CCNC LIWAQLJGPBVORC-UHFFFAOYSA-N 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 238000005457 optimization Methods 0.000 description 4
- 238000012795 verification Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000012418 validation experiment Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000012217 deletion Methods 0.000 description 1
- 230000037430 deletion Effects 0.000 description 1
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/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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- 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/20—Instruments for performing navigational calculations
-
- 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
- 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
- G01S19/49—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 whereby the further system is an inertial position system, e.g. loosely-coupled
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)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种复杂城市环境下的自动驾驶汽车定位***及方法,***包括:GPS单元、IMU单元、轮式里程计单元、激光雷达目标检测单元、温度传感器单元、扩展卡尔曼滤波器单元及***噪声方差矩阵更新单元;本发明能够使自动驾驶汽车在复杂城市环境中可以针对不同行驶环境调整滤波器的***噪声方差矩阵,进而实现稳定且准确的自车定位,为其后续***中对运动的决策、规划与控制提供充足的前置数据。
Description
技术领域
本发明属于车辆定位技术领域,具体涉及一种适应于复杂城市环境下的自动驾驶汽车***及方法。
背景技术
自动驾驶汽车(Autonomous vehicles;Self-driving automobile)又称无人驾驶汽车、电脑驾驶汽车、或轮式移动机器人,是一种通过电脑***实现无人驾驶的智能汽车。
与传统汽车相比,自动驾驶汽车需要实时获取自身的位置与姿态,来为下一时刻的运动的规划与决策提供必需的定位信息。目前广泛应用于自动驾驶汽车的定位方法为全球导航卫星***(GNSS)与惯性导航***(INS)的融合定位,采用的***件为GPS、IMU与轮式里程计,通常用到的算法是卡尔曼滤波算法、扩展卡尔曼滤波及其变种,这种定位方式因其所需器件简单,成本低廉,故应用广泛。但是IMU和轮式里程计的噪声不是固定不变的,当汽车行驶在复杂城市环境中时,由于车辆频繁的加速减速,再加上传感器温度变化的影响,便会导致IMU和轮式里程计的噪声频繁变化,且复杂城市环境容易导致GPS信号缺失,没有GPS信号的修正,便会导致滤波发散,最终导致定位失效。
因此,如何提升自动驾驶汽车在城市复杂交通***中的定位稳定性与准确性,保证自动驾驶汽车可以在GPS信号丢失情况下稳定获取自身的位置,是目前自动驾驶领域急需解决的问题。
发明内容
针对于上述现有技术的不足,本发明的目的在于提供一种复杂城市环境下的自动驾驶汽车定位***及方法,以解决现有技术中由于车载GPS信号缺失,无法对因复杂城市环境导致的变化IMU噪声进行修正,进而导致定位失效的问题。本发明能够使自动驾驶汽车在复杂城市环境中可以针对不同行驶环境调整滤波器的***噪声方差矩阵,进而实现稳定且准确的自车定位,为其后续***中对运动的决策、规划与控制提供充足的前置数据。
为达到上述目的,本发明采用的技术方案如下:
本发明的一种复杂城市环境下的自动驾驶汽车定位***,包括:GPS单元、IMU单元、轮式里程计单元、激光雷达目标检测单元、温度传感器单元、扩展卡尔曼滤波器单元及***噪声方差矩阵更新单元;
所述GPS单元,安装在车辆外部,用于获取车辆的实时经纬度定位信息和航向角信息,并将获得的信息实时发送给扩展卡尔曼滤波器单元;
所述IMU单元,安装在车辆内部,用于获取车辆的瞬时加速度信息和瞬时角速度信息,并将获得的信息实时发送给扩展卡尔曼滤波器单元;
所述轮式里程计单元,安装在车辆车轮上,用于获取车辆的瞬时速度信息,并将获得的瞬时速度信息实时发送给扩展卡尔曼滤波器单元;
所述激光雷达目标检测单元,安装在车辆顶部,用于实时获取车辆周围的点云数据,通过PV-CRNN算法计算出车辆周围运动目标的数量和距离车辆的距离信息,并将计算得到的信息传输给***噪声方差矩阵更新单元;
所述温度传感器单元,用于实时获取IMU和轮式里程计的温度信息,并将温度信息实时传递给***噪声方差矩阵更新单元;
所述扩展卡尔曼滤波器单元,安装在车辆内部,当GPS单元信号可用时,实时获取GPS单元发送的经纬度定位信息和航向角信息、IMU单元发送的瞬时角速度信息和瞬时加速度信息以及轮式里程计单元发送的瞬时速度信息,再利用该扩展卡尔曼滤波器单元中扩展卡尔曼滤波器将上述信息进行融合,最终获得车辆的实时定位信息;当GPS单元信号不可用时,接收***噪声方差矩阵更新单元发送的***噪声方差矩阵,通过其对扩展卡尔曼滤波器单元中的扩展卡尔曼滤波器的***噪声方差矩阵进行更新,并实时获取IMU单元发送的瞬时角速度信息和瞬时加速度信息以及轮式里程计单元发送的瞬时速度信息,再使用更新后的扩展卡尔曼滤波器将从IMU单元和轮式里程计单元获取的信息进行融合,最终获得车辆的实时定位信息;
所述***噪声方差矩阵更新单元,安装在车辆内部,接收激光雷达目标检测单元发送的车辆周围运动目标的数量和距离车辆距离信息,及温度传感器单元发送的轮式里程计和IMU的温度信息,并根据上述接收到的信息,使用噪声增益算法计算得到使扩展卡尔曼滤波器滤波效果最好的***噪声方差矩阵,并将其发送给扩展卡尔曼滤波器单元,来替换扩展卡尔曼滤波器单元中扩展卡尔曼滤波器当前的***噪声方差矩阵。
进一步地,所述PV-RCNN算法是一种3D目标检测框架,采用的特征提取方式是利用3Dvoxel卷积和基于点的pointnet卷积方式,从点云数据中检测3D目标,并得到3D目标的置信度、类型以及距离车辆的距离信息。
本发明还提供一种复杂城市环境下的自动驾驶汽车定位方法,基于上述***,步骤如下:
1)实时获取车辆自身的经纬度定位信息和航向角信息,车辆自身的瞬时加速度信息和瞬时角速度信息,车辆的瞬时速度信息,车辆周围3D目标的置信度、类型、距离车辆的距离信息,及IMU和轮式里程计的温度信息;
2)判定GPS单元接收到的信息是否可用:对接收到的GPS原始数据进行解码,得到GPS的状态信息,若解码出的状态信息为非负数,则GPS单元接收到的信息可用,进入步骤3);若解码出的状态信息为负数,则GPS单元接收到的信息不可用,进入步骤4);
3)将步骤1)中获取到的车辆经纬度定位信息和航向角信息、车辆瞬时加速度信息和瞬时角速度信息、车辆瞬时速度信息发送给扩展卡尔曼滤波器单元,通过该单元中的扩展卡尔曼滤波器将上述信息进行融合,获得车辆的实时定位信息;
4)将车辆周围目标的数目和距离信息,及IMU和轮式里程计的温度信息发送给***噪声方差矩阵更新单元,通过噪声增益算法得出此状态下使扩展卡尔曼滤波器滤波效果最佳的***噪声方差矩阵,并将其发送给扩展卡尔曼滤波器单元,替换扩展卡尔曼滤波器单元中扩展卡尔曼滤波器当前的***噪声方差矩阵;利用更新后的扩展卡尔曼滤波器对IMU单元发送的瞬时加速度信息和瞬时角速度信息以及轮式里程计单元发送的瞬时速度信息进行融合,得到在GPS缺失情况下的定位结果。
进一步地,所述步骤2)中使用的GPS解码方式为:使用LINUX***下的功能包nmea_navsat_driver对GPS原始数据进行解码。
进一步地,所述步骤3)中使用的扩展卡尔曼滤波器的具体算法如下:
Xn=f(Xn-1,un)
Zn=h(Xn)
式中,Xn-1表示车辆在n-1时刻的状态量,xn-1表示n-1时刻的纬度值,yn-1表示n-1时刻的经度值,表示n-1时刻的航向角,Xn表示车辆在n时刻的状态量,xn表示n时刻的纬度值,yn表示n时刻的经度值,表示n时刻的航向角,un表示n时刻***的输入量,Zn表示车辆在n时刻的观测量,f(),h()分别表示状态转移方程与观测方程;
式中,Fn表示***n时刻的状态转移矩阵,Gn表示n时刻***输入到***状态的映射矩阵,Hn表示n时刻***的观测矩阵;根据上述三个矩阵公式,描述标准EKF算法的流程如下:给定汽车状态量初始值与***概率协方差矩阵初始值然后按照以下公式不断迭代,对无人驾驶汽车进行状态估计:
式中,表示车辆在n时刻的先验状态估计量,表示车辆在n时刻的后验状态估计量,表示车辆在n-1时刻的***状态后验概率协方差矩阵,表示***n时刻的状态转移矩阵的转置,表示n时刻***输入到***状态的映射矩阵的转置,表示n时刻***的观测矩阵的转置,Kn表示n时刻的卡尔曼增益,表示车辆在n时刻的***状态先验概率协方差矩阵,表示车辆在n时刻的***状态后验概率协方差矩阵,Rn表示观测值的不确定性对应的协方差矩阵,Rn由所使用的传感器来决定,Qn表示n时刻***噪声方差矩阵,ev,n表示轮式里程计在n时刻的速度测量噪声,ea,n表示IMU在n时刻的加速度测量噪声,eω,n表示IMU在n时刻的角速度测量噪声,ev,n,ea,n,eω,n的初始值由标定试验获得;
进一步地,所述步骤4)中的噪声增益算法具体如下:
式中,Δk-1,k表示从k-1时刻到k时刻的路况增益,αk表示k时刻的车辆影响权重系数,αk-1表示k-1时刻的车辆影响权重系数,βk表示k时刻的行人影响权重系数,βk-1表示k-1时刻的行人影响权重系数,n1表示k时刻激光雷达目标检测单元获取到的周围车辆数目,n2表示k时刻激光雷达目标检测单元获取到的周围行人数目,n3表示k-1时刻激光雷达目标检测单元获取到的周围车辆数目,n4表示k-1时刻激光雷达目标检测单元获取到的周围行人数目,表示k时刻激光雷达目标检测单元获取到的第i个车辆目标的置信度,表示k时刻激光雷达目标检测单元获取到的第j个行人目标的置信度,表示k-1时刻激光雷达目标检测单元获取到的第i个车辆目标的置信度,表示k-1时刻激光雷达目标检测单元获取到的第j个行人目标的置信度,表示k时刻激光雷达目标检测单元获取到的第i个车辆目标距离自车的距离,表示k时刻激光雷达目标检测单元获取到的第j个行人目标距离自车的距离,表示k-1时刻激光雷达目标检测单元获取到的第i个车辆目标距离自车的距离,表示k-1时刻激光雷达目标检测单元获取到的第j个行人目标距离自车的距离,表示从k-1时刻到k时刻的IMU温度增益,表示温度传感器单元获取到IMU在k时刻的温度,单位为华氏摄氏度,表示温度传感器单元获取到IMU在k-1时刻的温度,单位为华氏摄氏度,表示从k-1时刻到k时刻的轮式里程计温度增益,表示温度传感器单元获取到轮式里程计在k时刻的温度,单位为华氏摄氏度,表示温度传感器单元获取到轮式里程计在k-1时刻的温度,单位为华氏摄氏度,表示从k-1时刻到k时刻的IMU增益系数,表示从k-1时刻到k时刻的轮式里程计增益系数,θE表示环境影响因素权重,取值为0.7,θI表示IMU温度影响因素,取值为0.3,θW表示轮式里程计温度影响因素权重,取值为0.3;
设k-1时刻的***噪声方差矩阵为Qk-1:
式中,ev,k-1为轮式里程计在k-1时刻的速度噪声,ea,k-1为IMU在k-1时刻的加速度噪声,eω为IMU在k-1时刻的角速度噪声;
则k时刻的***噪声方差矩阵为Qk:
用所述***噪声方差矩阵Qk替换扩展卡尔曼滤波器的原有矩阵,再使用更新后的扩展卡尔曼滤波器进行融合定位。
本发明的有益效果:
1、本发明保证了自动驾驶汽车在GPS信号缺失时定位信号输出的稳定性,提高了自动驾驶车辆在城市道路中行驶的安全性;
2、本发明可以针对不同的城市环境自动调整滤波器的***噪声方差矩阵,保证滤波器的稳定性,从而稳定输出较为精确的定位信息。
附图说明
图1为本发明定位方法原理图。
图2为本发明验证实验具体选取的5个GPS缺失区域。
图3为本发明验证实验中的一条具体路径定位效果对比图。
图4为本发明验证实验中五次GPS缺失区域最大误差值对比图。
图5为本发明验证实验中五次GPS缺失区域均方根误差值对比图。
具体实施方式
为了便于本领域技术人员的理解,下面结合实施例与附图对本发明作进一步的说明,实施方式提及的内容并非对本发明的限定。
本发明的一种复杂城市环境下的自动驾驶汽车定位***,包括:GPS单元、IMU单元、轮式里程计单元、激光雷达目标检测单元、温度传感器单元、扩展卡尔曼滤波器单元及***噪声方差矩阵更新单元;
所述GPS单元,安装在车辆外部,用于获取车辆的实时经纬度定位信息和航向角信息,并将获得的信息实时发送给扩展卡尔曼滤波器单元;
所述IMU单元,安装在车辆内部,用于获取车辆的瞬时加速度信息和瞬时角速度信息,并将获得的信息实时发送给扩展卡尔曼滤波器单元;
所述轮式里程计单元,安装在车辆车轮上,用于获取车辆的瞬时速度信息,并将获得的瞬时速度信息实时发送给扩展卡尔曼滤波器单元;
所述激光雷达目标检测单元,安装在车辆顶部,用于实时获取车辆周围的点云数据,通过PV-CRNN算法计算出车辆周围运动目标的数量和距离车辆的距离信息,并将计算得到的信息传输给***噪声方差矩阵更新单元;
其中,所述PV-RCNN算法是一种3D目标检测框架,采用的特征提取方式是利用3Dvoxel卷积和基于点的pointnet卷积方式,从点云数据中检测3D目标,并得到3D目标的置信度、类型以及距离车辆的距离信息。
所述温度传感器单元,包含两个温度传感器,分别安装在IMU和轮式里程计上,用于实时获取IMU和轮式里程计的温度信息,并将温度信息实时传递给***噪声方差矩阵更新单元;
所述扩展卡尔曼滤波器单元,安装在车辆内部,当GPS单元信号可用时,实时获取GPS单元发送的经纬度定位信息和航向角信息、IMU单元发送的瞬时角速度信息和瞬时加速度信息以及轮式里程计单元发送的瞬时速度信息,再利用该扩展卡尔曼滤波器单元中扩展卡尔曼滤波器将上述信息进行融合,最终获得车辆的实时定位信息;当GPS单元信号不可用时,接收***噪声方差矩阵更新单元发送的***噪声方差矩阵,通过其对扩展卡尔曼滤波器单元中的扩展卡尔曼滤波器的***噪声方差矩阵进行更新,并实时获取IMU单元发送的瞬时角速度信息和瞬时加速度信息以及轮式里程计单元发送的瞬时速度信息,再使用更新后的扩展卡尔曼滤波器将从IMU单元和轮式里程计单元获取的信息进行融合,最终获得车辆的实时定位信息;
所述***噪声方差矩阵更新单元,安装在车辆内部,接收激光雷达目标检测单元发送的车辆周围运动目标的数量和距离车辆距离信息,及温度传感器单元发送的轮式里程计和IMU的温度信息,并根据上述接收到的信息,使用噪声增益算法计算得到使扩展卡尔曼滤波器滤波效果最好的***噪声方差矩阵,并将其发送给扩展卡尔曼滤波器单元,来替换扩展卡尔曼滤波器单元中扩展卡尔曼滤波器当前的***噪声方差矩阵。
参照图1所示,本发明还提供一种复杂城市环境下的自动驾驶汽车定位方法,基于上述***,步骤如下:
1)实时获取车辆自身的经纬度定位信息和航向角信息,车辆自身的瞬时加速度信息和瞬时角速度信息,车辆的瞬时速度信息,车辆周围3D目标的置信度、类型、距离车辆的距离信息,及IMU和轮式里程计的温度信息;
2)判定GPS单元接收到的信息是否可用:对接收到的GPS原始数据进行解码,得到GPS的状态信息,若解码出的状态信息为非负数,则GPS单元接收到的信息可用,进入步骤3);若解码出的状态信息为负数,则GPS单元接收到的信息不可用,进入步骤4);
具体地,所述步骤2)中使用的GPS解码方式为:使用LINUX***下的功能包nmea_navsat_driver对GPS原始数据进行解码。
3)将步骤1)中获取到的车辆经纬度定位信息和航向角信息、车辆瞬时加速度信息和瞬时角速度信息、车辆瞬时速度信息发送给扩展卡尔曼滤波器单元,通过该单元中的扩展卡尔曼滤波器将上述信息进行融合,获得车辆的实时定位信息;
使用的扩展卡尔曼滤波器的具体算法如下:
Xn=f(Xn-1,un)
Zn=h(Xn)
式中,Xn-1表示车辆在n-1时刻的状态量,xn-1表示n-1时刻的纬度值,yn-1表示n-1时刻的经度值,表示n-1时刻的航向角,Xn表示车辆在n时刻的状态量,xn表示n时刻的纬度值,yn表示n时刻的经度值,表示n时刻的航向角,un表示n时刻***的输入量,Zn表示车辆在n时刻的观测量,f(),h()分别表示状态转移方程与观测方程;
式中,Fn表示***n时刻的状态转移矩阵,Gn表示n时刻***输入到***状态的映射矩阵,Hn表示n时刻***的观测矩阵;根据上述三个矩阵公式,描述标准EKF算法的流程如下:给定汽车状态量初始值与***概率协方差矩阵初始值然后按照以下公式不断迭代,对无人驾驶汽车进行状态估计:
式中,表示车辆在n时刻的先验状态估计量,表示车辆在n时刻的后验状态估计量,表示车辆在n-1时刻的***状态后验概率协方差矩阵,表示***n时刻的状态转移矩阵的转置,表示n时刻***输入到***状态的映射矩阵的转置,表示n时刻***的观测矩阵的转置,Kn表示n时刻的卡尔曼增益,表示车辆在n时刻的***状态先验概率协方差矩阵,表示车辆在n时刻的***状态后验概率协方差矩阵,Rn表示观测值的不确定性对应的协方差矩阵,Rn由所使用的传感器来决定,Qn表示n时刻***噪声方差矩阵,ev,n表示轮式里程计在n时刻的速度测量噪声,ea,n表示IMU在n时刻的加速度测量噪声,eω,n表示IMU在n时刻的角速度测量噪声,ev,n,ea,n,eω,n的初始值由标定试验获得;
4)将车辆周围目标的数目和距离信息,及IMU和轮式里程计的温度信息发送给***噪声方差矩阵更新单元,通过噪声增益算法得出此状态下使扩展卡尔曼滤波器滤波效果最佳的***噪声方差矩阵,并将其发送给扩展卡尔曼滤波器单元,替换扩展卡尔曼滤波器单元中扩展卡尔曼滤波器当前的***噪声方差矩阵;更新后的扩展卡尔曼滤波器接收来IMU单元发送的瞬时加速度信息和瞬时角速度信息以及轮式里程计单元发送的瞬时速度信息,将以上信息进行融合,得到在GPS缺失情况下的定位结果;
所述噪声增益算法具体如下:
式中,Δk-1,k表示从k-1时刻到k时刻的路况增益,αk表示k时刻的车辆影响权重系数,αk-1表示k-1时刻的车辆影响权重系数,βk表示k时刻的行人影响权重系数,βk-1表示k-1时刻的行人影响权重系数,n1表示k时刻激光雷达目标检测单元获取到的周围车辆数目,n2表示k时刻激光雷达目标检测单元获取到的周围行人数目,n3表示k-1时刻激光雷达目标检测单元获取到的周围车辆数目,n4表示k-1时刻激光雷达目标检测单元获取到的周围行人数目,表示k时刻激光雷达目标检测单元获取到的第i个车辆目标的置信度,表示k时刻激光雷达目标检测单元获取到的第j个行人目标的置信度,表示k-1时刻激光雷达目标检测单元获取到的第i个车辆目标的置信度,表示k-1时刻激光雷达目标检测单元获取到的第j个行人目标的置信度,表示k时刻激光雷达目标检测单元获取到的第i个车辆目标距离自车的距离,表示k时刻激光雷达目标检测单元获取到的第j个行人目标距离自车的距离,表示k-1时刻激光雷达目标检测单元获取到的第i个车辆目标距离自车的距离,表示k-1时刻激光雷达目标检测单元获取到的第j个行人目标距离自车的距离,表示从k-1时刻到k时刻的IMU温度增益,表示温度传感器单元获取到IMU在k时刻的温度,单位为华氏摄氏度,表示温度传感器单元获取到IMU在k-1时刻的温度,单位为华氏摄氏度,表示从k-1时刻到k时刻的轮式里程计温度增益,表示温度传感器单元获取到轮式里程计在k时刻的温度,单位为华氏摄氏度,表示温度传感器单元获取到轮式里程计在k-1时刻的温度,单位为华氏摄氏度,表示从k-1时刻到k时刻的IMU增益系数,表示从k-1时刻到k时刻的轮式里程计增益系数,θE表示环境影响因素权重,取值为0.7,θI表示IMU温度影响因素,取值为0.3,θW表示轮式里程计温度影响因素权重,取值为0.3;
设k-1时刻的***噪声方差矩阵为Qk-1:
式中,ev,k-1为轮式里程计在k-1时刻的速度噪声,ea,k-1为IMU在k-1时刻的加速度噪声,eω为IMU在k-1时刻的角速度噪声;
则k时刻的***噪声方差矩阵为Qk:
用所述***噪声方差矩阵Qk替换扩展卡尔曼滤波器的原有矩阵,再使用更新后的扩展卡尔曼滤波器进行融合定位。
进一步验证所设计定位方法的定位准确性,选择一条道路进行实验验证,具体路径如图2所示,圈出的部分为缺失GPS信号部分。选取其中一条GPS信号缺失时的路径如图3所示,可以看到优化后的定位效果相较于优化前更贴近于参考路径。对五次GPS缺失时的定位数据进行分析:最大误差值如图4所示,可以看到优化后的定位最大误差值均小于优化前的定位最大误差值,最大误差值平均降低了45.32%;均方根误差值如图5所示,可以看到优化后的定位数据的均方根误差值均小于优化前的定位数据的均方根误差值,均方根误差值平均降低了32.58%。
本发明具体应用途径很多,以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以作出若干改进,这些改进也应视为本发明的保护范围。
Claims (6)
1.一种复杂城市环境下的自动驾驶汽车定位***,其特征在于,包括:GPS单元、IMU单元、轮式里程计单元、激光雷达目标检测单元、温度传感器单元、扩展卡尔曼滤波器单元及***噪声方差矩阵更新单元;
所述GPS单元,安装在车辆外部,用于获取车辆的实时经纬度定位信息和航向角信息,并将获得的信息实时发送给扩展卡尔曼滤波器单元;
所述IMU单元,安装在车辆内部,用于获取车辆的瞬时加速度信息和瞬时角速度信息,并将获得的信息实时发送给扩展卡尔曼滤波器单元;
所述轮式里程计单元,安装在车辆车轮上,用于获取车辆的瞬时速度信息,并将获得的瞬时速度信息实时发送给扩展卡尔曼滤波器单元;
所述激光雷达目标检测单元,安装在车辆顶部,用于实时获取车辆周围的点云数据,通过PV-CRNN算法计算出车辆周围运动目标的数量和距离车辆的距离信息,并将计算得到的信息传输给***噪声方差矩阵更新单元;
所述温度传感器单元,用于实时获取IMU和轮式里程计的温度信息,并将温度信息实时传递给***噪声方差矩阵更新单元;
所述扩展卡尔曼滤波器单元,安装在车辆内部,当GPS单元信号可用时,实时获取GPS单元发送的经纬度定位信息和航向角信息、IMU单元发送的瞬时角速度信息和瞬时加速度信息以及轮式里程计单元发送的瞬时速度信息,再利用该扩展卡尔曼滤波器单元中扩展卡尔曼滤波器将上述信息进行融合,最终获得车辆的实时定位信息;当GPS单元信号不可用时,接收***噪声方差矩阵更新单元发送的***噪声方差矩阵,通过其对扩展卡尔曼滤波器单元中的扩展卡尔曼滤波器的***噪声方差矩阵进行更新,并实时获取IMU单元发送的瞬时角速度信息和瞬时加速度信息以及轮式里程计单元发送的瞬时速度信息,再使用更新后的扩展卡尔曼滤波器将从IMU单元和轮式里程计单元获取的信息进行融合,最终获得车辆的实时定位信息;
所述***噪声方差矩阵更新单元,安装在车辆内部,接收激光雷达目标检测单元发送的车辆周围运动目标的数量和距离车辆距离信息,及温度传感器单元发送的轮式里程计和IMU的温度信息,并根据上述接收到的信息,使用噪声增益算法计算得到使扩展卡尔曼滤波器滤波效果最好的***噪声方差矩阵,并将其发送给扩展卡尔曼滤波器单元,来替换扩展卡尔曼滤波器单元中扩展卡尔曼滤波器当前的***噪声方差矩阵。
2.根据权利要求1所述的复杂城市环境下的自动驾驶汽车定位***,其特征在于,所述PV-RCNN算法是一种3D目标检测框架,采用的特征提取方式是利用3D voxel卷积和基于点的pointnet卷积方式,从点云数据中检测3D目标,并得到3D目标的置信度、类型以及距离车辆的距离信息。
3.一种复杂城市环境下的自动驾驶汽车定位方法,基于权利要求1-2中任意一项所述***,其特征在于,步骤如下:
1)实时获取车辆自身的经纬度定位信息和航向角信息,车辆自身的瞬时加速度信息和瞬时角速度信息,车辆的瞬时速度信息,车辆周围3D目标的置信度、类型、距离车辆的距离信息,及IMU和轮式里程计的温度信息;
2)判定GPS单元接收到的信息是否可用:对接收到的GPS原始数据进行解码,得到GPS的状态信息,若解码出的状态信息为非负数,则GPS单元接收到的信息可用,进入步骤3);若解码出的状态信息为负数,则GPS单元接收到的信息不可用,进入步骤4);
3)将步骤1)中获取到的车辆经纬度定位信息和航向角信息、车辆瞬时加速度信息和瞬时角速度信息、车辆瞬时速度信息发送给扩展卡尔曼滤波器单元,通过该单元中的扩展卡尔曼滤波器将上述信息进行融合,获得车辆的实时定位信息;
4)将车辆周围目标的数目和距离信息,及IMU和轮式里程计的温度信息发送给***噪声方差矩阵更新单元,通过噪声增益算法得出此状态下使扩展卡尔曼滤波器滤波效果最佳的***噪声方差矩阵,并将其发送给扩展卡尔曼滤波器单元,替换扩展卡尔曼滤波器单元中扩展卡尔曼滤波器当前的***噪声方差矩阵;利用更新后的扩展卡尔曼滤波器对IMU单元发送的瞬时加速度信息和瞬时角速度信息以及轮式里程计单元发送的瞬时速度信息进行融合,得到在GPS缺失情况下的定位结果。
4.根据权利要求3所述的复杂城市环境下的自动驾驶汽车定位方法,其特征在于,所述步骤2)中使用的GPS解码方式为:使用LINUX***下的功能包nmea_navsat_driver对GPS原始数据进行解码。
5.根据权利要求3所述的复杂城市环境下的自动驾驶汽车定位方法,其特征在于,所述步骤3)中使用的扩展卡尔曼滤波器的具体算法如下:
Xn=f(Xn-1,un)
Zn=h(Xn)
式中,Xn-1表示车辆在n-1时刻的状态量,xn-1表示n-1时刻的纬度值,yn-1表示n-1时刻的经度值,表示n-1时刻的航向角;Xn表示车辆在n时刻的状态量,xn表示n时刻的纬度值,yn表示n时刻的经度值,表示n时刻的航向角;un表示n时刻***的输入量;Zn表示车辆在n时刻的观测量;f(),h()分别表示状态转移方程与观测方程;
式中,Fn表示***n时刻的状态转移矩阵,Gn表示n时刻***输入到***状态的映射矩阵,Hn表示n时刻***的观测矩阵;根据上述三个矩阵公式,描述标准EKF算法的流程如下:给定汽车状态量初始值与***概率协方差矩阵初始值然后按照以下公式不断迭代,对无人驾驶汽车进行状态估计:
式中,表示车辆在n时刻的先验状态估计量,表示车辆在n时刻的后验状态估计量,表示车辆在n-1时刻的***状态后验概率协方差矩阵,表示***n时刻的状态转移矩阵的转置,表示n时刻***输入到***状态的映射矩阵的转置,表示n时刻***的观测矩阵的转置,Kn表示n时刻的卡尔曼增益,表示车辆在n时刻的***状态先验概率协方差矩阵,表示车辆在n时刻的***状态后验概率协方差矩阵,Rn表示观测值的不确定性对应的协方差矩阵,Rn由所使用的传感器来决定,Qn表示n时刻***噪声方差矩阵,ev,n表示轮式里程计在n时刻的速度测量噪声,ea,n表示IMU在n时刻的加速度测量噪声,eω,n表示IMU在n时刻的角速度测量噪声,ev,n,ea,n,eω,n的初始值由标定试验获得;
6.根据权利要求5所述的复杂城市环境下的自动驾驶汽车定位方法,其特征在于,所述步骤4)中的噪声增益算法具体如下:
式中,Δk-1,k表示从k-1时刻到k时刻的路况增益,αk表示k时刻的车辆影响权重系数,αk-1表示k-1时刻的车辆影响权重系数,βk表示k时刻的行人影响权重系数,βk-1表示k-1时刻的行人影响权重系数,n1表示k时刻激光雷达目标检测单元获取到的周围车辆数目,n2表示k时刻激光雷达目标检测单元获取到的周围行人数目,n3表示k-1时刻激光雷达目标检测单元获取到的周围车辆数目,n4表示k-1时刻激光雷达目标检测单元获取到的周围行人数目,表示k时刻激光雷达目标检测单元获取到的第i个车辆目标的置信度,表示k时刻激光雷达目标检测单元获取到的第j个行人目标的置信度,表示k-1时刻激光雷达目标检测单元获取到的第i个车辆目标的置信度,表示k-1时刻激光雷达目标检测单元获取到的第j个行人目标的置信度,表示k时刻激光雷达目标检测单元获取到的第i个车辆目标距离自车的距离,表示k时刻激光雷达目标检测单元获取到的第j个行人目标距离自车的距离,表示k-1时刻激光雷达目标检测单元获取到的第i个车辆目标距离自车的距离,表示k-1时刻激光雷达目标检测单元获取到的第j个行人目标距离自车的距离,表示从k-1时刻到k时刻的IMU温度增益,表示温度传感器单元获取到IMU在k时刻的温度,单位为华氏摄氏度,表示温度传感器单元获取到IMU在k-1时刻的温度,单位为华氏摄氏度,表示从k-1时刻到k时刻的轮式里程计温度增益,表示温度传感器单元获取到轮式里程计在k时刻的温度,单位为华氏摄氏度,表示温度传感器单元获取到轮式里程计在k-1时刻的温度,单位为华氏摄氏度,表示从k-1时刻到k时刻的IMU增益系数,表示从k-1时刻到k时刻的轮式里程计增益系数,θE表示环境影响因素权重,取值为0.7,θI表示IMU温度影响因素,取值为0.3,θW表示轮式里程计温度影响因素权重,取值为0.3;
设k-1时刻的***噪声方差矩阵为Qk-1:
式中,ev,k-1为轮式里程计在k-1时刻的速度噪声,ea,k-1为IMU在k-1时刻的加速度噪声,eω为IMU在k-1时刻的角速度噪声;
则k时刻的***噪声方差矩阵为Qk:
用所述***噪声方差矩阵Qk替换扩展卡尔曼滤波器的原有矩阵,再使用更新后的扩展卡尔曼滤波器进行融合定位。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210506316.2A CN115046547A (zh) | 2022-05-10 | 2022-05-10 | 一种复杂城市环境下的自动驾驶汽车定位***及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210506316.2A CN115046547A (zh) | 2022-05-10 | 2022-05-10 | 一种复杂城市环境下的自动驾驶汽车定位***及方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115046547A true CN115046547A (zh) | 2022-09-13 |
Family
ID=83157817
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210506316.2A Pending CN115046547A (zh) | 2022-05-10 | 2022-05-10 | 一种复杂城市环境下的自动驾驶汽车定位***及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115046547A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116068538A (zh) * | 2023-04-06 | 2023-05-05 | 中汽研(天津)汽车工程研究院有限公司 | 一种用于批量式车辆激光雷达的可调节标定***和方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109099901A (zh) * | 2018-06-26 | 2018-12-28 | 苏州路特工智能科技有限公司 | 基于多源数据融合的全自动压路机定位方法 |
CN110032180A (zh) * | 2018-01-12 | 2019-07-19 | 福特全球技术公司 | 激光雷达定位 |
US20190258251A1 (en) * | 2017-11-10 | 2019-08-22 | Nvidia Corporation | Systems and methods for safe and reliable autonomous vehicles |
CN110887494A (zh) * | 2019-11-29 | 2020-03-17 | 腾讯科技(深圳)有限公司 | 车辆定位方法、装置 |
CN112083726A (zh) * | 2020-09-04 | 2020-12-15 | 湖南大学 | 一种面向园区自动驾驶的双滤波器融合定位*** |
CN114341760A (zh) * | 2019-08-28 | 2022-04-12 | 三星电子株式会社 | 用于定位和路径规划的传感器融合 |
-
2022
- 2022-05-10 CN CN202210506316.2A patent/CN115046547A/zh active Pending
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190258251A1 (en) * | 2017-11-10 | 2019-08-22 | Nvidia Corporation | Systems and methods for safe and reliable autonomous vehicles |
CN110032180A (zh) * | 2018-01-12 | 2019-07-19 | 福特全球技术公司 | 激光雷达定位 |
CN109099901A (zh) * | 2018-06-26 | 2018-12-28 | 苏州路特工智能科技有限公司 | 基于多源数据融合的全自动压路机定位方法 |
CN114341760A (zh) * | 2019-08-28 | 2022-04-12 | 三星电子株式会社 | 用于定位和路径规划的传感器融合 |
CN110887494A (zh) * | 2019-11-29 | 2020-03-17 | 腾讯科技(深圳)有限公司 | 车辆定位方法、装置 |
CN112083726A (zh) * | 2020-09-04 | 2020-12-15 | 湖南大学 | 一种面向园区自动驾驶的双滤波器融合定位*** |
Non-Patent Citations (2)
Title |
---|
刘岩 等: "基于自适应EKF的弱GNSS信号跟踪算法", 《计算机测量与控制》, 31 December 2014 (2014-12-31), pages 1842 - 1845 * |
李理敏 等: "基于自适应扩展卡尔曼滤波的载波跟踪算法", 《航空学报》, 25 July 2012 (2012-07-25), pages 1319 - 1328 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116068538A (zh) * | 2023-04-06 | 2023-05-05 | 中汽研(天津)汽车工程研究院有限公司 | 一种用于批量式车辆激光雷达的可调节标定***和方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111307162B (zh) | 用于自动驾驶场景的多传感器融合定位方法 | |
CN106840179B (zh) | 一种基于多传感器信息融合的智能车定位方法 | |
CN106289275B (zh) | 用于改进定位精度的单元和方法 | |
JP6202151B2 (ja) | モバイルコンピュータ大気圧システム | |
US8775063B2 (en) | System and method of lane path estimation using sensor fusion | |
JP4964047B2 (ja) | 位置検出装置及び位置検出方法 | |
Jo et al. | GPS-bias correction for precise localization of autonomous vehicles | |
CN102529975B (zh) | 用于精确的分车道车辆定位的***和方法 | |
US20180328744A1 (en) | Travelling road information generation system of vehicle and on-board apparatus based on correction amount | |
TWI522258B (zh) | Based on electronic map, global navigation satellite system and vehicle motion detection technology Lane identification method | |
CN112147651B (zh) | 一种异步多车协同目标状态鲁棒估计方法 | |
CN111536972B (zh) | 一种基于里程计刻度系数修正的车载dr导航方法 | |
CN114264301B (zh) | 车载多传感器融合定位方法、装置、芯片及终端 | |
WO2024041156A1 (zh) | 车辆定位校准方法、装置、计算机设备及存储介质 | |
CN109343095A (zh) | 一种车载导航车辆组合定位装置及其组合定位方法 | |
KR20190040818A (ko) | 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템 | |
JP2021113047A (ja) | 移動体制御装置、移動体制御方法、および、移動体制御装置用プログラム | |
CN112229422A (zh) | 基于fpga时间同步的里程计快速标定方法及*** | |
CN113405555B (zh) | 一种自动驾驶的定位传感方法、***及装置 | |
CN115046547A (zh) | 一种复杂城市环境下的自动驾驶汽车定位***及方法 | |
CN113063441B (zh) | 里程计累计推算误差的数据源纠正方法及装置 | |
JP2019174191A (ja) | データ構造、情報送信装置、制御方法、プログラム及び記憶媒体 | |
CN115046546A (zh) | 一种基于车道线识别的自动驾驶汽车定位***及方法 | |
WO2019188820A1 (ja) | 情報送信装置、データ構造、制御方法、プログラム及び記憶媒体 | |
Moussa et al. | Ultrasonic wheel based aiding for land vehicle navigation in GNSS denied environment |
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 |