CN111307162B - 用于自动驾驶场景的多传感器融合定位方法 - Google Patents

用于自动驾驶场景的多传感器融合定位方法 Download PDF

Info

Publication number
CN111307162B
CN111307162B CN202010168559.0A CN202010168559A CN111307162B CN 111307162 B CN111307162 B CN 111307162B CN 202010168559 A CN202010168559 A CN 202010168559A CN 111307162 B CN111307162 B CN 111307162B
Authority
CN
China
Prior art keywords
vehicle
information
particle
lane
yaw
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
CN202010168559.0A
Other languages
English (en)
Other versions
CN111307162A (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.)
AutoCore Intelligence Technology Nanjing Co Ltd
Original Assignee
AutoCore Intelligence Technology Nanjing 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 AutoCore Intelligence Technology Nanjing Co Ltd filed Critical AutoCore Intelligence Technology Nanjing Co Ltd
Priority to CN202010168559.0A priority Critical patent/CN111307162B/zh
Publication of CN111307162A publication Critical patent/CN111307162A/zh
Application granted granted Critical
Publication of CN111307162B publication Critical patent/CN111307162B/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/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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种用于自动驾驶场景的多传感器融合定位方法，采用低成本传感器结合矢量地图通过改进的粒子滤波算法实现车道级定位。本发明不仅具有明显价格优势，有利于自动驾驶技术普及，同时能保证定位精度，而且使用更加方便，还能输出频率可调的高频定位信息，其能为环境感知和车身控制提供基准数据。

Description

用于自动驾驶场景的多传感器融合定位方法
技术领域
本发明是2019111650580的分案申请，属于自动驾驶领域，特别涉及用于自动驾驶场景的多传感器融合定位方法。
背景技术
高精度定位是自动驾驶领域的重要研究课题，目前常用高精度组合导航、多线激光雷达、摄像等传感器结合高精度地图实现车道级定位，主要采用卡尔曼滤波、粒子滤波、SLAM(即时定位与地图构建)等融合定位算法。其中常规的卡尔曼滤波算法需要价格昂贵的高精度RTK(实时动态测量技术)和IMU，在高架桥、隧道等GPS信号不稳定地方，其定位精度较低；现有的粒子滤波算法需要借助深度学习模块检测道路标志，将其与先验地图中的道路特征匹配，更新模型参数；SLAM算法在车辆快速运行的高速公路上鲁棒性较低，此外其需要高性能的计算平台。所以，在现有技术中没有一种适用于车辆自动驾驶的既经济又能够精准定位的方法。
发明内容
发明目的：本发明针对现有技术存在的问题，提出了一种不仅经济又能够对车辆进行准确定位的用于自动驾驶场景的多传感器融合定位方法。
技术方案：为实现上述目的，本发明提供了一种用于自动驾驶场景的多传感器融合定位方法，包括以下步骤：
步骤1：车载传感器实施采集车辆的行驶信息；其中，车辆的行驶信息包括，车辆所在的经纬度、车辆的速度信息、航向信息、车辆所在的车道和车辆与所在车道的中心线之间的距离；
步骤2：在矢量地图上以步骤1采集到的车辆所在的经纬度为圆心，GPS定位偏差为半径做圆；并在圆内根据高斯分布设置粒子群；其中，矢量地图中含有车道线、车道宽度、车道航向角的信息；
步骤3：将传感器采集的航向信息、速度信息和步骤2中设置的粒子群中每个粒子的的位置信息加入高斯噪声后输入到第一恒定转率和速度运行模型中，第一恒定转率和速度模型输出每个粒子的状态信息，其中每个粒子的状态值包括每个粒子在UTM坐标系下的坐标值和每个粒子的航向信息；
步骤4：将不在车辆所在车道内的粒子的权重值设置为0；再分别计算剩下粒子点的权重值；
步骤5：将步骤3获得每个粒子的状态信息和步骤4获得每个粒子的权重值通过加权平均法求出车辆的位置信息。
进一步，所述步骤1中采用的传感器有GPS、IMU、Camera和里程计。这里采用的传感器更加的经济实惠。
进一步，所述步骤2中GPS定位偏差通过公式
Figure BDA0002408064070000021
计算获得，其中，λ为GPS定位偏差，η表示GPS定位精度，θ为收到星数，h为水平精度因子，β的取值范围在0.55～0.65，σ为稳定系数，μ为水平精度系数。有效的保证了整个方法的鲁棒性。
进一步，所述步骤4中粒子点的权重值获取方法，包括以下步骤：
步骤401：根据公式：
Figure BDA0002408064070000022
Figure BDA0002408064070000023
分别计算粒子群中每个粒子在车道中位置差值和航向差值；其中，Δdi表示第i个粒子的位置差值，dc表示camer输出当前车辆距离车道中心线的距离偏差，
Figure BDA0002408064070000024
表示第i个粒子的距离车道中心线的距离偏差，Δyawi表示第i个粒子的航向角差值，yawc表示camer输出的当前车辆与车道的航向角的偏差，
Figure BDA0002408064070000025
表示第i个粒子所在车道的道路的航向角，
Figure BDA0002408064070000026
表示第i个粒子的航向角。
步骤402：将步骤401获得的每个粒子在车道中的位置差值和航向差值代入概率密度函数，经过归一化后得到每个粒子点的权重值wi；
Figure BDA0002408064070000027
其中，wi为第i个粒子的权重，σd表示camer检测车辆距离车道中心线的距离偏差的方差，ud表示camer检测车辆距离车道中心线的距离偏差的均值，σyaw表示camer检测车辆与车道的航向角的偏差的方差，uyaw表示camer检测车辆与车道的航向角的偏差的均值。
进一步，还包括高频模块，将步骤5中获得的车辆的位置信息、实时的车辆速度信息和车辆的航向信息输入到高频模块中，高频模块输出车辆位置信息；其中高频模块通过恒定转率和速度模型对车辆的位置信息进行计算。能够输出频率可调的高频定位信息。
进一步，所述高频模块工作包括以下步骤：
步骤601：将步骤5中获得的车辆的位置信息和当前采集的车辆速度信息以及车辆的航向信息输入到第二恒定转率和速度模型中计算下一时刻车辆的位置信息{xt，yt，yawt}并输出，其中，第二恒定转率和速度模型为：
Figure BDA0002408064070000031
xt＝xt-1+v′t×cos(yawt)×Δt
yt＝yt-1+v′t×sin(yawt)×Δt
式中，yawt表示t时刻车辆的航向角，yawt-1表示t-1时刻车辆的航向角，
Figure BDA0002408064070000032
表示在t-1时刻IMU输出的车辆的航向角的角速度，xt表示t时刻车辆在UTM坐标系下的横坐标，xt-1表示t-1时刻车辆在UTM坐标系下的横坐标，v′t在t时刻车辆ODOM(里程计)输出的车辆的速度，yt表示t时刻车辆在UTM坐标系下的纵坐标，yt-1表示t-1时刻车辆在UTM坐标系下的纵坐标；
步骤602：检测是否采集到新的车辆速度信息和车辆的航向信息；如果采集到新的车辆速度信息和车辆的航向信息，则执行步骤603，如果没有采集到新的车辆速度信息和车辆的航向信息则执行步骤602；
步骤603：检测步骤5是否有输出新的车辆的位置信息，如果没有，则用步骤601获得车辆的位置信息{xt，yt，yawt}结合此时采集到的新的车辆速度信息以及车辆的航向信息作为输入数据输入到第二恒定转率和速度模型中计算下一时刻车辆的位置信息{xt，yt，yawt}并输出，然后重复步骤602；如果步骤5有输出新的车辆的位置信息，则重复步骤601～602。
工作原理：本文提出一种针对自动驾驶场景的定位方案，采用低成本GPS、IMU、Camera等传感器结合矢量地图通过改进的粒子滤波算法实现车道级定位。该方案使用的硬件设备和矢量地图数据均是自动驾驶技术的常用资源，其无需高昂传感设备和计算平台即可实现精准的车道级定位。
有益效果：与现有技术相比，本发明具有以下优点：1.本发明采用的GPS和IMU均为低成本传感设备，具有明显价格优势，有利于自动驾驶技术普及。
2.本发明将自动驾驶车辆必备传感器GPS的绝对位置信息和Camera的道路感知信息进行融合定位，有效的保证了定位精度。
3.本发明通过粒子群构建GPS绝对偏差和车道横向定位的约束关系，实现车道级定位，其无需深度学习等目标检测平台可实现同级定位精度，使用更加方便。同时，把高斯噪声加入到整个粒子群中，用于模拟真实情况，让算法的输入数据包含系统的不确定性，经过观测模块更新粒子权重后，粒子滤波算法的输出会更加精确。
4.本发明将文中粒子滤波输出的定位信息和车辆预测模型结合，能输出频率可调的高频定位信息，其能为环境感知和车身控制提供基准数据。
附图说明
图1为本发明的系统框图；
图2为矢量地图上粒子群设置范围示意图；
图3为矢量地图上粒子群设置示意图；
图4为根据视觉车道检测信息筛选粒子群后的示意图；
图5为定位结果示意图。
具体实施方式
下面将结合本发明实例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1：
本实施例公开的一种用于自动驾驶场景的多传感器融合定位方法，具体包括如下步骤：
步骤1：设置在汽车上的GPS、IMU、Camera和车辆ODOM分别采集汽车实时信息，其中，GPS设置在车顶并且位于车辆旋转中心位置，IMU装在车辆旋转中心位置，Camera安装在位于车辆中轴线的前挡风玻璃上。GPS主要采集车辆所在位置的经纬度信息，本实施例中采用RAC-P1型号的GPS；IMU采集车辆的航向信息，航向信息包括车辆的航向角和航向角的角速度，本实施例中采用MTI-30型号的IMU；Camera采集车道检测信息，其中车道的检测信息包括车辆所在的具体车道和车辆在该车道中距离车道中心线的距离和车辆与车道的航向角的偏差，本实施例中采用MV-UBS131GC型号的Camera；车辆ODOM采集车辆的速度信息。
本实例中GPS输出信息：经度：118.974608，维度：31.708664，星数：12，水平精度因子：0.66。UTM坐标系：东向(x轴)：687116.344，北向(y轴)：3509839.137。定位偏差：1.8m。IMU的输出的航向角：90°，IMU的角速度0.1°/s。车辆的速度5m/s。摄像头检测到的信息：偏离中线的距离0.3m，车辆与道路的夹角2°。
步骤2：根据公式
Figure BDA0002408064070000051
计算出定位偏差，其中，λ为GPS定位偏差，η表示GPS定位精度，θ为收到星数，h为水平精度因子，β为该GPS置信度较高时的水平因子，β的取值范围在0.55～0.65，σ为稳定系数，μ为水平精度系数。
步骤3：在矢量地图上以步骤1采集到的车辆所在的经纬度为圆心，步骤2中获得的GPS定位偏差λ为半径做圆；并在圆内根据高斯分布设置粒子群。其中，矢量地图是在UTM坐标系下的，其中含有车道线、车道宽度、车道航向角等道路信息。
本实施例中为了方便理解只设置了5个粒子点，在本发明提供的方法中一般设置的粒子群中的粒子数量在1000个以上，设置的粒子数据越多获得的定位会更加准确。本实施例中每个粒子的初始位置如表一所示：
表一
粒子id 1 2 3 4 5
X坐标 687116.832 687118.211 687114.517 687117.523 687115.422
Y坐标 3509839.201 3509840.972 3509837.347 3509839.983 3509838.769
步骤4：将IMU输出的航向信息、车辆ODOM(里程计)输出的速度信息和步骤3中设置的粒子群中每个粒子的位置信息加入高斯噪声后输入到第一CTRV(恒定转率和速度模型)运行模型中，第一CTRV运行模型输出每个粒子的状态信息，其中，每个粒子的状态信息包括每个粒子在UTM坐标系下的坐标值和每个粒子的航向信息，第一CTRV运行模型为：
Figure BDA0002408064070000061
Figure BDA0002408064070000062
Figure BDA0002408064070000063
式中，
Figure BDA0002408064070000064
表示第i个粒子在t时刻的航向角，
Figure BDA0002408064070000065
表示第i个粒子在t-1时刻的航向角，
Figure BDA00024080640700000614
表示在t-1时刻IMU输出的车辆的航向角的角速度加入高斯噪声的值，Δt表示时间差，
Figure BDA0002408064070000066
表示第i个粒子在t时刻在UTM坐标系下的横坐标，
Figure BDA0002408064070000067
表示第i个粒子在t-1时刻在UTM坐标系下的横坐标，vt表示在t时刻车辆ODOM(里程计)输出的车辆的速度加入高斯噪声后的值，
Figure BDA00024080640700000612
表示第i个粒子在t时刻在UTM坐标系下的纵坐标，
Figure BDA00024080640700000613
表示第i个粒子在t-1时刻在UTM坐标系下的纵坐标。CTRV运行模型初始状态下使用粒子的位置信息时，需要在每个粒子的横、纵坐标值上加入高斯噪声。
本实施例中，第一CTRV运行模型输出每个粒子的状态信息如表二所示：
表二
粒子id 1 2 3 4 5
X坐标 687116.832 687118.211 687114.517 687117.523 687115.422
Y坐标 3509839.201 3509840.972 3509837.347 3509839.983 3509838.769
航向角 89° 87° 91° 92° 90°
步骤5：根据车辆上的Camera采集车道检测信息，将不在车辆所在车道内的粒子点的权重值设置为0；再分别计算剩下粒子点的权重值。其中粒子点的权重值的计算方法包括以下步骤：
步骤501：根据公式：
Figure BDA0002408064070000068
Figure BDA0002408064070000069
分别计算粒子群中每个粒子在车道中位置差值和航向差值；其中，Δdi表示第i个粒子的位置差值，dc表示camer输出当前车辆距离车道中心线的距离偏差，
Figure BDA00024080640700000610
表示第i个粒子的距离车道中心线的距离偏差，Δyawi表示第i个粒子的航向角差值，yawc表示camer输出的当前车辆与车道的航向角的偏差，
Figure BDA00024080640700000611
表示第i个粒子所在车道的道路的航向角，
Figure BDA0002408064070000071
表示第i个粒子的航向角。
步骤502：将步骤501获得的每个粒子在车道中的位置差值和航向差值代入概率密度函数，经过归一化后得到每个粒子点的权重值wi；
Figure BDA0002408064070000072
其中，wi为第i个粒子的权重，σd表示camer检测车辆距离车道中心线的距离偏差的方差，ud表示camer检测车辆距离车道中心线的距离偏差的均值，σyaw表示camer检测车辆与车道的航向角的偏差的方差，uyaw表示camer检测车辆与车道的航向角的偏差的均值。
本实施例中每个粒子位置偏差、航向偏差，以及权重值如表三所示：
表三
粒子id 1 2 3 4 5
位置偏差 0.13 0.33 0.37 0.24 0.17
航向偏差 0.21 0.45 0.39 0.33 0.29
权重 0.37 0.08 0.09 0.22 0.23
步骤6：将步骤4获得每个粒子的每个状态信息分别和步骤5获得每个粒子的权重值通过加权平均法求出车辆的位置信息。其中，求出的车辆的位置信息包括车辆在UTM坐标系下的横坐标、纵坐标和航向角。最后获得的当前车辆的位置信息如表四所示：
表四
X坐标 687116.559
Y坐标 3509839.249
航向角 89.919°
速度 4.947m/s
实施例2：
如图1所示，本实施例公开的一种用于自动驾驶场景的多传感器融合定位方法，具体包括如下步骤：
步骤1：设置在汽车上的GPS、IMU、Camera和车辆ODOM分别采集汽车实时信息，其中，GPS设置在车顶并且位于车辆旋转中心位置，IMU装在车辆旋转中心位置，Camera安装在位于车辆中轴线的前挡风玻璃上。GPS主要采集车辆所在位置的经纬度信息，本实施例中采用RAC-P1型号的GPS；IMU采集车辆的航向信息，航向信息包括车辆的航向角和航向角的角速度，本实施例中采用MTI-30型号的IMU；Camera采集车道检测信息，其中车道的检测信息包括车辆所在的具体车道和车辆在该车道中距离车道中心线的距离和车辆与车道的航向角的偏差，本实施例中采用MV-UBS131GC型号的Camera；车辆ODOM采集车辆的速度信息。
步骤2：根据公式
Figure BDA0002408064070000081
计算出定位偏差，其中，λ为GPS定位偏差，η表示GPS定位精度，θ为收到星数，h为水平精度因子，β为该GPS置信度较高时的水平因子，β的取值范围在0.55～0.65，σ为稳定系数，μ为水平精度系数。
步骤3：在矢量地图上以步骤1采集到的车辆所在的位置为圆心，步骤2中获得的GPS定位偏差λ为半径做圆；如图2所示，三条平行的虚线和与其平行的两条实线为矢量地图的车道线；白色矩形为自动驾驶车辆；扇形虚线为视觉车道检测结果，据此可获取车辆所在车道；灰色圆形区域为GPS的定位信息，其圆形半径为GPS的定位偏差。并在圆内根据高斯分布设置粒子群；如图3所示，实心黑点为设置的粒子点。其中，矢量地图中含有车道线、车道宽度、车道航向角等道路信息。
步骤4：将IMU输出的航向信息、车辆ODOM(里程计)输出的速度信息和步骤3中设置的粒子群中每个粒子的位置信息加入高斯噪声后输入到第一CTRV(恒定转率和速度模型)运行模型中，第一CTRV运行模型输出每个粒子的状态信息，其中，每个粒子的状态信息包括每个粒子在UTM坐标系下的坐标值和每个粒子的航向信息，第一CTRV运行模型为：
Figure BDA0002408064070000082
Figure BDA0002408064070000083
Figure BDA0002408064070000084
式中，
Figure BDA0002408064070000085
表示第i个粒子在t时刻的航向角，
Figure BDA0002408064070000086
表示第i个粒子在t-1时刻的航向角，
Figure BDA0002408064070000089
表示在t-1时刻IMU输出的车辆的航向角的角速度加入高斯噪声的值，Δt表示时间差，
Figure BDA0002408064070000087
表示第i个粒子在t时刻在UTM坐标系下的横坐标，
Figure BDA0002408064070000088
表示第i个粒子在t-1时刻在UTM坐标系下的横坐标，vt表示在t时刻车辆ODOM(里程计)输出的车辆的速度加入高斯噪声后的值，
Figure BDA0002408064070000091
表示第i个粒子在t时刻在UTM坐标系下的纵坐标，
Figure BDA0002408064070000092
表示第i个粒子在t-1时刻在UTM坐标系下的纵坐标。CTRV运行模型初始状态下使用粒子的位置信息时，需要在每个粒子的横、纵坐标值上加入高斯噪声。
步骤5：根据车辆上的Camera采集车道检测信息，将不在车辆所在车道内的粒子点的权重值设置为0，如图4所示，剔除位于该车辆所在车道外的粒子；再分别计算剩下粒子点的权重值。其中粒子点的权重值的计算方法包括以下步骤：
步骤501：分别计算粒子群中每个粒子在车道中位置差值和航向差值；
Figure BDA0002408064070000093
Figure BDA0002408064070000094
其中，Δdi表示第i个粒子的位置差值，dc表示camer输出当前车辆距离车道中心线的距离偏差，
Figure BDA0002408064070000095
表示第i个粒子的距离车道中心线的距离偏差，Δyawi表示第i个粒子的航向角差值，yawc表示camer输出的当前车辆与车道的航向角的偏差，
Figure BDA0002408064070000096
表示第i个粒子所在车道的道路的航向角，
Figure BDA0002408064070000097
表示第i个粒子的航向角。
步骤502：将步骤501获得的每个粒子在车道中的位置差值和航向差值代入概率密度函数，经过归一化后得到每个粒子点的权重值wi；
Figure BDA0002408064070000098
其中，wi为第i个粒子的权重，σd表示camer检测车辆距离车道中心线的距离偏差的方差，ud表示camer检测车辆距离车道中心线的距离偏差的均值，σyaw表示camer检测车辆与车道的航向角的偏差的方差，uydw表示camer检测车辆与车道的航向角的偏差的均值。
步骤6：将步骤4获得每个粒子的状态信息和步骤5获得每个粒子的权重值通过加权平均法求出车辆的位置信息，其中，求出的车辆的位置信息包括车辆在UTM坐标系下的横坐标、纵坐标和航向角。如图5所示。
步骤7：将步骤6中获得的车辆的位置信息、车辆ODOM采集的速度信息和IMU采集的车辆的航向信息输入到高频模块中，高频模块输出车辆位置信息；其中高频模块通过CTRV模型对车辆的位置信息进行计算，具体包括：
步骤701：将步骤6中获得的车辆的位置信息和当前采集的车辆速度信息以及车辆的航向信息输入到第二CTRV运行模型中计算下一时刻车辆的位置信息{xt，yt，yawt}并输出，其中，第二CTRV运行模型为：
Figure BDA0002408064070000101
xt＝xt-1+v′t×cos(yawt)×Δt
yt＝yt-1+v′t×sin(yawt)×Δt
式中，yawt表示t时刻车辆的航向角，其为第二CTRV运行模型的输出数据；yawt-1表示t-1时刻车辆的航向角，其为第二CTRV运行模型的输入数据；
Figure BDA0002408064070000102
表示在t-1时刻IMU输出的车辆的航向角的角速度，其为第二CTRV运行模型的输入数据；xt表示t时刻车辆在UTM坐标系下的横坐标，其为第二CTRV运行模型的输出数据；xt-1表示t-1时刻车辆在UTM坐标系下的横坐标，在步骤6获得新的车辆位置信息时，xt-1直接使用步骤6中的获得的车辆位置的横坐标，如果没有获得新的车辆位置信息，则直接使用上一时刻得到的第二CTRV运行模型输出的车辆的横坐标进行迭代，其为第二CTRV运行模型的输入数据；v′t在t时刻车辆ODOM(里程计)输出的车辆的速度，其为第二CTRV运行模型的输入数据；yt表示t时刻车辆在UTM坐标系下的纵坐标，其为第二CTRV运行模型的输出数据；yt-1表示t-1时刻车辆在UTM坐标系下的纵坐标，在步骤6获得新的车辆位置信息时，yt-1直接使用步骤6中的获得的车辆位置的纵坐标，如果没有获得新的车辆位置信息，则直接使用上一时刻得到的第二CTRV运行模型输出的车辆的纵坐标进行迭代，其为第二CTRV运行模型的输入数据。
步骤702：检测是否采集到新的车辆速度信息和车辆的航向信息；如果采集到新的车辆速度信息和车辆的航向信息，则执行步骤703，如果没有采集到新的车辆速度信息和车辆的航向信息则执行步骤702；
步骤703：检测步骤6是否有输出新的车辆的位置信息，如果没有，则用步骤701获得车辆的位置信息{xt，yt，yawt}结合此时采集到的新的车辆速度信息以及车辆的航向信息作为输入数据输入到第二CTRV运行模型中计算下一时刻车辆的位置信息{xt，yt，yawt}并输出，然后重复步骤702；如果步骤6有输出新的车辆的位置信息，则重复步骤701～702。
因为Camera采集车道检测信息的频率是10HZ，而IMU和ODOM的采集频率是50HZ，增加高频模块就可以有效的提高整个系统对车辆位置信息的输出频率。

Claims (2)

1.一种用于自动驾驶场景的多传感器融合定位方法，其特征在于：包括以下步骤：
步骤1：车载传感器实时采集车辆的行驶信息；其中，车辆的行驶信息包括，车辆所在的经纬度、车辆的速度信息、航向信息、车辆所在的车道和车辆与所在车道的中心线之间的距离；
步骤2：在矢量地图上以步骤1采集到的车辆所在的经纬度为圆心，GPS定位偏差为半径做圆；并在圆内根据高斯分布设置粒子群；其中，矢量地图中含有车道线、车道宽度、车道航向角的信息；
步骤3：将传感器采集的航向信息、速度信息和步骤2中设置的粒子群中每个粒子的位置信息加入高斯噪声后输入到第一恒定转率和速度运行模型中，第一恒定转率和速度模型输出每个粒子的状态信息，其中每个粒子的状态值包括每个粒子在UTM坐标系下的坐标值和每个粒子的航向信息；
步骤4：将不在车辆所在车道内的粒子的权重值设置为0；再分别计算剩下粒子点的权重值；
步骤5：由步骤4获得的粒子权重值作为步骤3中相应粒子状态信息的权重值，对步骤3中的粒子信息进行加权平均；求出车辆的位置信息、实时的车辆速度信息和车辆的航向信息；
所述步骤2中GPS定位偏差通过公式
Figure FDA0002616672100000011
计算获得，其中，λ为GPS定位偏差，η表示GPS定位精度，θ为收到星数，h为水平精度因子，β的取值范围在0.55～0.65，σ为稳定系数，μ为水平精度系数；
所述步骤4中粒子点的权重值获取方法，包括以下步骤：
步骤401：根据公式：
Figure FDA0002616672100000012
Figure FDA0002616672100000013
分别计算粒子群中每个粒子在车道中位置差值和航向差值；其中，Δdi表示第i个粒子的位置差值，dc表示camera输出当前车辆距离车道中心线的距离偏差，
Figure FDA0002616672100000014
表示第i个粒子距离车道中心线的距离偏差，Δyawi表示第i个粒子的航向角差值，yawc表示camera输出的当前车辆与车道的航向角的偏差，
Figure FDA0002616672100000021
表示第i个粒子所在车道的道路的航向角，
Figure FDA0002616672100000022
表示第i个粒子的航向角；
步骤402：将步骤401获得的每个粒子在车道中的位置差值和航向差值代入概率密度函数，经过归一化后得到每个粒子点的权重值wi；
Figure FDA0002616672100000023
其中，wi为第i个粒子的权重，σd表示camera检测车辆距离车道中心线的距离偏差的方差，ud表示camera检测车辆距离车道中心线的距离偏差的均值，σyaw表示camera检测车辆与车道的航向角的偏差的方差，uyaw表示camera检测车辆与车道的航向角的偏差的均值；
步骤6：将步骤5中获得的车辆的位置信息、实时的车辆速度信息和车辆的航向信息输入到高频模块中，高频模块输出车辆位置信息；其中高频模块通过第二恒定转率和速度模型对车辆的位置信息进行计算，具体包括以下步骤：
步骤601：将步骤5中获得的车辆的位置信息和当前采集的车辆速度信息以及车辆的航向信息输入到第二恒定转率和速度模型中计算下一时刻车辆的位置信息{xt，yt，yawt}并输出，其中，第二恒定转率和速度模型为：
Figure FDA0002616672100000024
xt＝xt-1+v′t×cos(yawt)×Δt
yt＝yt-1+v′t×sin(yawt)×Δt
式中，yawt表示t时刻车辆的航向角，yawt-1表示t-1时刻车辆的航向角，
Figure FDA0002616672100000025
表示在t-1时刻IMU输出的车辆的航向角的角速度，xt表示t时刻车辆在UTM坐标系下的横坐标，xt-1表示t-1时刻车辆在UTM坐标系下的横坐标，v′t在t时刻车辆里程计ODOM输出的车辆的速度，yt表示t时刻车辆在UTM坐标系下的纵坐标，yt-1表示t-1时刻车辆在UTM坐标系下的纵坐标；Δt表示时间差；
步骤602：检测是否采集到新的车辆速度信息和车辆的航向信息；如果采集到新的车辆速度信息和车辆的航向信息，则执行步骤603，如果没有采集到新的车辆速度信息和车辆的航向信息则执行步骤602；
步骤603：检测步骤5是否有输出新的车辆的位置信息，如果没有，则用步骤601获得车辆的位置信息{xt，yt，yawt}结合此时采集到的新的车辆速度信息以及车辆的航向信息作为输入数据输入到第二恒定转率和速度模型中计算下一时刻车辆的位置信息{xt，yt，yawt}并输出，然后重复步骤602；如果步骤5有输出新的车辆的位置信息，则重复步骤601～602。
2.根据权利要求1所述的用于自动驾驶场景的多传感器融合定位方法，其特征在于：所述步骤1中采用的传感器有GPS、IMU、Camera和里程计。
CN202010168559.0A 2019-11-25 2019-11-25 用于自动驾驶场景的多传感器融合定位方法 Active CN111307162B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010168559.0A CN111307162B (zh) 2019-11-25 2019-11-25 用于自动驾驶场景的多传感器融合定位方法

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201911165058.0A CN110631593B (zh) 2019-11-25 2019-11-25 一种用于自动驾驶场景的多传感器融合定位方法
CN202010168559.0A CN111307162B (zh) 2019-11-25 2019-11-25 用于自动驾驶场景的多传感器融合定位方法

Related Parent Applications (1)

Application Number Title Priority Date Filing Date
CN201911165058.0A Division CN110631593B (zh) 2019-11-25 2019-11-25 一种用于自动驾驶场景的多传感器融合定位方法

Publications (2)

Publication Number Publication Date
CN111307162A CN111307162A (zh) 2020-06-19
CN111307162B true CN111307162B (zh) 2020-09-25

Family

ID=68979526

Family Applications (2)

Application Number Title Priority Date Filing Date
CN201911165058.0A Active CN110631593B (zh) 2019-11-25 2019-11-25 一种用于自动驾驶场景的多传感器融合定位方法
CN202010168559.0A Active CN111307162B (zh) 2019-11-25 2019-11-25 用于自动驾驶场景的多传感器融合定位方法

Family Applications Before (1)

Application Number Title Priority Date Filing Date
CN201911165058.0A Active CN110631593B (zh) 2019-11-25 2019-11-25 一种用于自动驾驶场景的多传感器融合定位方法

Country Status (1)

Country Link
CN (2) CN110631593B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11790664B2 (en) 2019-02-19 2023-10-17 Tesla, Inc. Estimating object properties using visual image data
US11797304B2 (en) 2018-02-01 2023-10-24 Tesla, Inc. Instruction set architecture for a vector computational unit
US11816585B2 (en) 2018-12-03 2023-11-14 Tesla, Inc. Machine learning models operating at different frequencies for autonomous vehicles
US11841434B2 (en) 2018-07-20 2023-12-12 Tesla, Inc. Annotation cross-labeling for autonomous control systems
US11893774B2 (en) 2018-10-11 2024-02-06 Tesla, Inc. Systems and methods for training machine models with augmented data
US11893393B2 (en) 2017-07-24 2024-02-06 Tesla, Inc. Computational array microprocessor system with hardware arbiter managing memory requests
US12014553B2 (en) 2019-02-01 2024-06-18 Tesla, Inc. Predicting three-dimensional features for autonomous driving
US12020476B2 (en) 2022-10-28 2024-06-25 Tesla, Inc. Data synthesis for autonomous control systems

Families Citing this family (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018176000A1 (en) 2017-03-23 2018-09-27 DeepScale, Inc. Data synthesis for autonomous control systems
US11409692B2 (en) 2017-07-24 2022-08-09 Tesla, Inc. Vector computational unit
US10671349B2 (en) 2017-07-24 2020-06-02 Tesla, Inc. Accelerated mathematical engine
US11157441B2 (en) 2017-07-24 2021-10-26 Tesla, Inc. Computational array microprocessor system using non-consecutive data formatting
US11215999B2 (en) 2018-06-20 2022-01-04 Tesla, Inc. Data pipeline and deep learning system for autonomous driving
US11636333B2 (en) 2018-07-26 2023-04-25 Tesla, Inc. Optimizing neural network structures for embedded systems
US11562231B2 (en) 2018-09-03 2023-01-24 Tesla, Inc. Neural networks for embedded devices
US11196678B2 (en) 2018-10-25 2021-12-07 Tesla, Inc. QOS manager for system on a chip communications
US11537811B2 (en) 2018-12-04 2022-12-27 Tesla, Inc. Enhanced object detection for autonomous vehicles based on field view
US11610117B2 (en) 2018-12-27 2023-03-21 Tesla, Inc. System and method for adapting a neural network model on a hardware platform
US10997461B2 (en) 2019-02-01 2021-05-04 Tesla, Inc. Generating ground truth for machine learning from time series elements
US11567514B2 (en) 2019-02-11 2023-01-31 Tesla, Inc. Autonomous and user controlled vehicle summon to a target
CN111399503A (zh) * 2020-03-09 2020-07-10 西南科技大学 一种小型无人船艇自动停泊方法
EP3882649B1 (en) * 2020-03-20 2023-10-25 ABB Schweiz AG Position estimation for vehicles based on virtual sensor response
CN111586632B (zh) * 2020-05-06 2021-09-07 浙江大学 基于通信传感异步数据融合的协作邻居车辆定位方法
CN111813127A (zh) * 2020-07-28 2020-10-23 丹阳市安悦信息技术有限公司 一种自动驾驶式的汽车搬运机器人系统
CN112505718B (zh) * 2020-11-10 2022-03-01 奥特酷智能科技(南京)有限公司 用于自动驾驶车辆的定位方法、系统及计算机可读介质
CN113188539A (zh) * 2021-04-27 2021-07-30 深圳亿嘉和科技研发有限公司 一种巡检机器人的组合定位方法
CN112985427B (zh) * 2021-04-29 2021-07-30 腾讯科技（深圳）有限公司 车辆的车道跟踪方法、装置、计算机设备及存储介质
CN114323033B (zh) * 2021-12-29 2023-08-29 北京百度网讯科技有限公司 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN116222588B (zh) * 2023-05-08 2023-08-04 睿羿科技(山东)有限公司 一种gps与车载里程计融合的定位方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108225341A (zh) * 2016-12-14 2018-06-29 乐视汽车（北京）有限公司 车辆定位方法
CN110440801A (zh) * 2019-07-08 2019-11-12 浙江吉利控股集团有限公司 一种定位感知信息获取方法、装置及系统

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103472459A (zh) * 2013-08-29 2013-12-25 镇江青思网络科技有限公司 一种基于gps伪距差分的车辆协作定位方法
US9283967B2 (en) * 2014-07-16 2016-03-15 GM Global Technology Operations LLC Accurate curvature estimation algorithm for path planning of autonomous driving vehicle
CN104076382B (zh) * 2014-07-22 2016-11-23 中国石油大学(华东) 一种基于多源信息融合的车辆无缝定位方法
CN105628033B (zh) * 2016-02-26 2019-04-02 广西鑫朗通信技术有限公司 一种基于道路连通关系的地图匹配方法
CN106767853B (zh) * 2016-12-30 2020-01-21 中国科学院合肥物质科学研究院 一种基于多信息融合的无人驾驶车辆高精度定位方法
CN107161141B (zh) * 2017-03-08 2023-05-23 深圳市速腾聚创科技有限公司 无人驾驶汽车系统及汽车
CN107782321B (zh) * 2017-10-10 2020-10-30 武汉迈普时空导航科技有限公司 一种基于视觉和高精度地图车道线约束的组合导航方法
CN108459618A (zh) * 2018-03-15 2018-08-28 河南大学 一种无人机自动对移动平台投放的飞行控制系统及方法
CN109556615B (zh) * 2018-10-10 2022-10-04 吉林大学 基于自动驾驶的多传感器融合认知的驾驶地图生成方法
US10373323B1 (en) * 2019-01-29 2019-08-06 StradVision, Inc. Method and device for merging object detection information detected by each of object detectors corresponding to each camera nearby for the purpose of collaborative driving by using V2X-enabled applications, sensor fusion via multiple vehicles

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108225341A (zh) * 2016-12-14 2018-06-29 乐视汽车（北京）有限公司 车辆定位方法
CN110440801A (zh) * 2019-07-08 2019-11-12 浙江吉利控股集团有限公司 一种定位感知信息获取方法、装置及系统

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11893393B2 (en) 2017-07-24 2024-02-06 Tesla, Inc. Computational array microprocessor system with hardware arbiter managing memory requests
US11797304B2 (en) 2018-02-01 2023-10-24 Tesla, Inc. Instruction set architecture for a vector computational unit
US11841434B2 (en) 2018-07-20 2023-12-12 Tesla, Inc. Annotation cross-labeling for autonomous control systems
US11893774B2 (en) 2018-10-11 2024-02-06 Tesla, Inc. Systems and methods for training machine models with augmented data
US11816585B2 (en) 2018-12-03 2023-11-14 Tesla, Inc. Machine learning models operating at different frequencies for autonomous vehicles
US12014553B2 (en) 2019-02-01 2024-06-18 Tesla, Inc. Predicting three-dimensional features for autonomous driving
US11790664B2 (en) 2019-02-19 2023-10-17 Tesla, Inc. Estimating object properties using visual image data
US12020476B2 (en) 2022-10-28 2024-06-25 Tesla, Inc. Data synthesis for autonomous control systems

Also Published As

Publication number Publication date
CN110631593A (zh) 2019-12-31
CN110631593B (zh) 2020-02-21
CN111307162A (zh) 2020-06-19

Similar Documents

Publication Publication Date Title
CN111307162B (zh) 用于自动驾驶场景的多传感器融合定位方法
CN111272165B (zh) 一种基于特征点标定的智能车定位方法
CN109946732B (zh) 一种基于多传感器数据融合的无人车定位方法
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN105509738B (zh) 基于惯导/多普勒雷达组合的车载定位定向方法
CN110208842A (zh) 一种车联网环境下车辆高精度定位方法
JP5162849B2 (ja) 不動点位置記録装置
Tao et al. Lane marking aided vehicle localization
US11525682B2 (en) Host vehicle position estimation device
CN104061899B (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN110307836B (zh) 一种用于无人清扫车辆贴边清扫的精确定位方法
WO2018072350A1 (zh) 车辆轨迹预测方法及装置
CN107274721B (zh) 一种智能交通系统中多车辆协作定位方法
CN104464375B (zh) 一种识别车辆高速转弯的方法
CN109696177B (zh) 补偿陀螺感测值的装置、具有该装置的系统及其方法
CN113147738A (zh) 一种自动泊车定位方法和装置
CN110057356B (zh) 一种隧道内车辆定位方法及装置
CN112147651B (zh) 一种异步多车协同目标状态鲁棒估计方法
CN110940344B (zh) 一种用于自动驾驶的低成本传感器组合定位方法
CN108974054B (zh) 无缝列车定位方法及其系统
CN113220013A (zh) 一种多旋翼无人机隧洞悬停方法及系统
CN112346104A (zh) 一种无人机信息融合定位方法
CN111025366A (zh) 基于ins及gnss的网格slam的导航系统及方法
CN110018503B (zh) 车辆的定位方法及定位系统
CN114323050A (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
CP02 Change in the address of a patent holder

Address after: 210012 room 401-404, building 5, chuqiaocheng, No. 57, Andemen street, Yuhuatai District, Nanjing, Jiangsu Province

Patentee after: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd.

Address before: 211800 building 12-289, 29 buyue Road, Qiaolin street, Jiangbei new district, Pukou District, Nanjing City, Jiangsu Province

Patentee before: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd.

CP02 Change in the address of a patent holder