CN114518108B - 一种定位地图构建方法及装置 - Google Patents
一种定位地图构建方法及装置 Download PDFInfo
- Publication number
- CN114518108B CN114518108B CN202011298342.8A CN202011298342A CN114518108B CN 114518108 B CN114518108 B CN 114518108B CN 202011298342 A CN202011298342 A CN 202011298342A CN 114518108 B CN114518108 B CN 114518108B
- Authority
- CN
- China
- Prior art keywords
- module
- map
- laser
- point cloud
- positioning
- 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
Links
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/20—Instruments for performing navigational calculations
-
- 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
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)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
本发明属于自动驾驶技术领域,具体涉及一种定位地图构建方法及装置。该方法首先获取当前时刻下多源感知测量设备的感知数据;然后不同的位姿推算模块计算处理输出多个当前时刻的位置姿态;接着根据影响位姿推算模块精度的影响因子,自适应分配各个位姿推算模块位姿结果的权重占比;最后根据权重占比,对各个位姿推算模块的结果进行归一化加权取均值,输出当前时刻下自动驾驶车辆的位置姿态结果;将该时刻下的激光帧数据按照最终的位置姿态结果投影至地图坐标系下,进行地图构建。本发明融合多种位姿推算模块的位姿推算结果,使得各种驾驶场景下得到自动驾驶车辆的运动轨迹是稳定且准确的,进而构建出高精度、强鲁棒的激光定位点云地图。
Description
技术领域
本发明属于自动驾驶技术领域,具体涉及一种定位地图构建方法及装置。
背景技术
定位地图是实现自动驾驶的基础,可以帮助自动驾驶车辆进行感知与定位,识别出交通场景中的各种要素(例如道路信息),还可进行障碍物检测,以规划出合适的行驶路线、行驶速度并避开障碍物。传统的电子地图主要依靠卫星影像产生,再由GPS定位,可以达到米级精度。而无人驾驶需要精细化定义的精度达到厘米级的高精度地图,传统的电子地图便不再适用。
为了构建出高精度的定位地图,常常采用多种传感器相融合的方法,利用多种传感器的数据来构建高精度地图。例如,申请公布号为CN109887057A的中国发明专利公开了一种生成高精度地图的方法,该方法将全球导航卫星系统GNSS/惯性测量单元IMU测量得到的姿态、相机姿态和雷达姿态直接进行融合处理,得到更为准确的姿态估计。但是,每种定位系统的定位结果并非时时刻刻均较优,也就无法保证各种环境下得到的姿态估计均是准确的。例如,在一些隧道、地下车库等信号微弱甚至没有信号的环境下,全球导航卫星系统GNSS的定位结果较差,此时若仍旧较为相信GNSS的定位结果将无法保证姿态估计结果,进而无法保证地图的精度。
发明内容
本发明提供了一种定位地图构建方法及装置,用以解决现有技术中将多种传感器检测的结果直接进行融合处理造成的定位效果不佳进而使得构建的地图精度不高的问题。
为解决上述技术问题,本发明的技术方案包括:
本发明提供了一种定位地图构建方法,包括如下步骤:
1)获取某一时刻移动设备的周围环境信息;
2)获取移动设备上设置的各个位姿推算模块同步后的数据,并进行相应处理得到该时刻各个位姿推算模块对移动设备的位姿推算结果;所述各个位姿推算模块包括GNSS组合导航定位模块、轮式里程计推算模块、激光里程计推算模块和视觉里程计推算模块中的至少两种,位姿推算结果包括位置和姿态;
3)根据该时刻影响各个位姿推算模块位姿推算精度的影响因素,确定各个位姿推算模块的权重占比,并进行归一化处理;其中,所述GNSS组合导航定位模块的影响因素包括卫星数量、信道比、精度因子和协方差比率,所述轮式里程计递推模块的影响因素包括轮式里程计的递推时长和编码器精度,所述激光里程计递推模块的影响因素包括激光里程计的递推时长和特征点数,所述视觉里程计递推模块的影响因素包括视觉里程计的递推时长和帧间匹配度;
4)根据各个位姿推算模块归一化处理后的权重占比,对该时刻各个位姿推算模块的位姿推算结果进行加权求和,得到的结果作为该时刻该移动设备的位姿推算结果;
5)根据各个时刻该移动设备的位姿推算结果以及周围环境信息,进行地图构建。
上述技术方案的有益效果为:本发明利用影响各个位姿推算模块的位姿推算精度的影响因素来确定各个位姿推算模块的权重占比,根据权重占比对各个位姿推算模块的位姿推算结果进行筛选和判断,使输出信号稳定和推算结果精度较高的位姿推算模块的位姿推算结果占比较高,相反的使其他的位姿推算模块的位姿推算结果占比较低,根据该占比情况融合多个位姿推算模块的位姿推算结果得到该移动设备的位姿推算结果,进而使得各种驾驶场景下得到自动驾驶车辆的运动轨迹是稳定且准确的,包括封闭园区等开阔路段,以及长隧道、地下停车场等半封闭或者封闭场景,进而结合自动驾驶车辆周围的环境信息便可构建得到高精度、强鲁棒的激光定位点云地图。
进一步的,所述GNSS组合导航定位模块的权重占比为:WeightGNSS=(δ卫星数量+δ信道比+δDop+δRatio)/4;其中,δ卫星数量为与卫星数量相关的系数,δ信道比为与信道比相关的系数,δDop为与精度因子相关的系数,δRatio为与协方差比率相关的系数;
所述轮式里程计推算模块的权重占比为Weight轮式里程计=(δ轮式里程计的递推时长+δ编码器影响因子)/2;其中,δ轮式里程计的递推时长为与轮式里程计推算模块的递推时长相关的系数,δ编码器精度为与编码器精度相关的系数;
所述激光里程计推算模块的权重占比为:Weight激光里程计=(δ激光里程计的递推时长+δ特征点数)/2;其中,δ激光里程计的递推时长为与激光里程计推算模块的递推时长相关的系数,δ特征点数为与特征点数相关的系数;
所述视觉里程计推算模块的权重占比为:Weight视觉里程计=(δ视觉里程计的递推时长+δ帧间匹配度)/2;其中,δ视觉里程计的递推时长为与视觉里程计推算模块的递推时长相关的系数,δ帧间匹配度为与帧间匹配度相关的系数。
进一步的,所述GNSS组合导航定位模块的权重占比为:
其中,Ratio为GNSS组合导航定位定姿模块输出结果的协方差比率,根据GNSS组合导航定位定姿模块输出结果的协方差计算得到,且Ratio=方差次优解/方差最优解;
所述与轮式里程计推算模块的递推时长相关的系数δ轮式里程计的递推时长为:
其中,C1为轮式里程计递推时长常值系数,Δt1为轮式里程计的递推时间间隔;
所述与编码器精度相关的系数δ编码器精度为:
δ编码器精度=C2*D前后轴距+C3*r轮子半径
其中,C2为编码精度常值系数,D前后轴距为前后轮轴距离,C3为编码器精度常值系数,r轮子半径为四轮的轮子半径。
进一步的,所述与激光里程计推算模块的递推时长相关的系数δ激光里程计的递推时长为:
其中,C4为激光里程计递推时长常值系数,Δt2为激光里程计的递推时间间隔;
所述与特征点数相关的系数δ特征点数为:
其中,C5为特征点数常值系数,n激光点云总数为用于激光里程计计算的激光帧点云总数,n匹配的特征点数为相邻帧间匹配的激光点云数量;
所述与视觉里程计推算模块的递推时长相关的系数δ视觉里程计的递推时长为:
其中,C6为视觉里程计递推时长常值系数,Δt3为视觉里程计的递推时间间隔;
所述与帧间匹配度相关的系数δ帧间匹配度为:
其中,C7为视觉里程计的帧间匹配度常值系数,xi为第i个特征点的位置,i=1,2,……,n,|xi-xi+1|为匹配的视觉特征点欧式距离差值。
进一步的,为了获取准确的周围环境信息,步骤1)中,所述周围环境信息包括周围环境的激光点云数据、或者包括周围环境的激光点云数据和视觉图像数据。
进一步的,所述各个位姿推算模块包括GNSS组合导航定位模块,将初始时刻GNSS组合导航定位模块的位姿推算结果作为初始时刻该移动设备的位姿推算结果。
进一步的,为了将地面点点云进行剔除,步骤5)中,构建好地图后,还包括根据地图中激光点云的高程将地图中的地面点点云进行过滤的步骤:
判断激光点云的高程是否大于高程阈值:若大于等于高程阈值,则该激光点云为非地面点点云,保留该激光点云数据;若小于高程阈值,则该激光点云为地面点点云,滤除该激光点云数据。
进一步的,为了移除动态目标物以提高地图的精度,步骤5)中,构建好地图后,还包括将地图中的动态目标物进行过滤的步骤:
获取同一场景不同时刻的局部地图,对各个局部地图进行网格划分,对比相同网格内不同时刻的激光点云数据:不同时刻该网格内均含有一目标物,则确定该目标物为非动态目标物,保留该目标物;至少一个时刻该网格内不含有该目标物且至少一个时刻该网格内含有该目标物,则确定该目标物为动态目标物,过滤该目标物。
进一步的,步骤5)中,为了减少激光匹配定位过程的搜索范围,构建好地图后,还包括将地图进行栅格处理的步骤:
将构建好的地图划分为n*m个格子,将对应的激光点云数据填充到对应的格子中,每个格子构成一个子地图并进行存储。
本发明还提供了一种定位地图构建装置,包括存储器和处理器,所述处理器用于执行存储在存储器中的指令以实现上述介绍的定位地图构建方法,并达到与该方法相同的效果。
附图说明
图1是本发明的定位地图构建方法的整体流程图;
图2是本发明的数据同步采集的流程图;
图3是本发明的构建多源融合地图的流程图;
图4是本发明的滤除地面点点云的流程图;
图5是本发明的滤除动态目标物的流程图;
图6是本发明的地图分块处理的流程图;
图7是本发明的定位地图构建装置的结构图。
具体实施方式
本发明在自动驾驶车辆移动过程中,利用自动驾驶车辆上安装的多种传感器,一方面获取该自动驾驶车辆的位姿推算结果(包括位置和姿态),另一方面可获取该自动驾驶车辆的周围环境信息,利用这两种信息构建激光点云地图。得到该激光点云地图后,便可在自动驾驶车辆处于自动驾驶模式时,利用构建的激光点云地图进行路径规划。
下面结合附图和实施例,对本发明的一种定位地图构建方法以及一种定位地图构架装置进行详细说明。
方法实施例:
本发明在自动驾驶车辆上加装有多种传感器设备,包括GNSS组合导航设备、激光雷达、双目视觉设备、编码器等。其中,利用多个激光雷达实现对自动驾驶车辆周围环境信息的感知量测;利用GNSS组合导航设备可构成GNSS组合导航位姿推算模块,利用激光雷达和GNSS组合导航中的IMU设备可构成激光里程计位姿推算模块,利用编码器可构成轮式里程计位姿推算模块,利用双目视觉设备和GNSS组合导航中的IMU设备可构成视觉里程计位姿推算模块,这些均可实现对自动驾驶车辆的位姿推算。其中,需说明的是,GNSS组合导航设备由GPS接收机和惯性单元IMU组成,将两者结合可输出车辆的位置和姿态信息。而且,激光里程计和视觉里程计在推算过程中需要结合GNSS组合导航中的IMU信息进行修正融合,以便输出稳定的位置和姿态信息。
利用上述自动驾驶车辆,可实现本发明的一种定位地图构建方法,下面结合图1,具体介绍整个地图构建的过程。需说明的是,在整个过程中,该自动驾驶车辆处于非自动驾驶模式下。
步骤一,获取自动驾驶车辆的车身信息数据,并在自动驾驶车辆移动过程中获取多种传感器设备采集的数据,视觉相机包括像素灰度信息、RGB颜色信息、环境纹理信息等,激光雷达包括空间位置信息线、面特征点环境结构信息等,轮式里程计包括三轴加速度、三轴四度等信息,进行数据时钟同步和文件加密后的离线存储。具体流程如图2所示,包括:
1、使自动驾驶车辆从GNSS组合导航设备信号稳定的区域出发,并记录下初始时刻GNSS组合导航定位模块的位姿推算结果,并将该位姿推算结果作为初始时刻该自动驾驶车辆的位姿推算结果,位姿推算结果包括自动驾驶车辆的位置和姿态,完成初始化操作。
2、通过GNSS组合导航设备的时钟同步系统,同步采集不同传感器的数据。为了提高存储效率和时钟同步精度,以GNSS组合导航设备的时钟作为触发信号完成各个传感器设备不同数据的时钟同步,同时开启多个非阻塞线程进行数据存储,最大化数据的时间软同步,最后加密存储私有数据格式。
步骤二,加载同步后的各个传感器的数据,根据激光雷达设备、GNSS天线设备、双目视觉设备等设备间的内、外标定参数,将视觉纹理信息映射至激光点云数据,以得到自动驾驶车辆的周围环境信息;然后按照相应的权重分配策略,得到稳定的自动驾驶车辆的轨迹位置和姿态数据;将这些数据进行一定的旋转、缩放、平移,转换成到统一的大地坐标系下,完成全部激光帧和视觉数据的快速拼接和融合,最后输出保存为加密私有地图文件。具体流程如图3所示,包括:
1、根据时间戳,同步读取t时刻的激光帧数据,按照各激光雷达设备的内、外标定参数对激光帧数据进行旋转、平移和缩放处理,完成单帧间的多个激光雷达数据的拼接。
LidarFramOutn=LidarFramInn*Rn+Tn (1)
LidarFramOut=LidarFramOut1+…LidarFramOutk+…+LidarFramOutn (2)
式中,k=1,2,…,n表示激光雷达传感器编号;Rn表示旋转矩阵且Tn表示平移矩阵且/>LidarFramInn表示编号为n的激光雷达转换前的激光点云数据集合;LidarFramOutn表示编号为n的激光雷达转换后的激光点云数据集合;LidarFramOut表示多个激光雷达转换拼接后的激光点云数据集合。
2、根据t时刻影响各个位姿推算模块位姿推算精度的影响因素,确定各个位姿推算模块的权重占比。
①GNSS组合导航定位模块定位定姿,影响其位姿推算精度的影响因素包括卫星数量、信道比、精度因子和协方差比率,每个影响因素都对应有其系数,分别为δ卫星数量、δ信道比、δDop、δRatio;其中,δ卫星数量为与卫星数量相关的影响系数,δ信道比为与信道比相关的影响系数,δDop为与精度因子相关的影响系数,δRatio为与协方差比率相关的影响系数。
根据GNSS组合导航定位定姿模块输出结果的协方差,得到协方差比率Ratio=方差次优解/方差最优解,主要包括固定解(Ratio≥3)、浮点解(1≤Ratio<3)、单点解(0<Ratio<1)3种定位状态。固定解的定位精度在3厘米以内,定位精度最高,置信度最高,权重最大;浮点解的定位精度小于50厘米,定位精度次高,置信度次之,权重次之;单点解的定位精度一般在5-10米之间,有时还会更大,定位精度基本不可用,置信度最差,权重最小。
根据三种状态,对输出的定位定姿结果进行权重分配:
②轮式里程计推算模块定位定姿,通过车身信息的车速、加速度、航向实现车辆位置的定位,其位姿推算结果与轮式里程计的递推时长、编码器精度等成线性增长关系,对应的权重占比为:
Weight轮式里程计=(δ轮式里程计的递推时长+δ编码器影响因子)/2 (4)
其中,δ轮式里程计的递推时长为与轮式里程计推算模块的递推时长相关的系数,C1为轮式里程计递推时长常值系数,本实施例中取为0.0429,Δt1为轮式里程计的递推时间间隔;δ编码器精度为与编码器精度相关的系数,δ编码器精度=C2*D前后轴距+C3*r轮子半径,C2为编码精度常值系数,本实施例中取为0.00137,D前后轴距为前后轮轴距离,C3为编码器精度常值系数,本实施例中取为0.0264,r轮子半径为四轮的轮子半径。
③激光里程计位姿推算定位定姿,通过实时连续激光帧间进行匹配,计算出相对位置姿态,实现相对定位的过程,其递推位姿推算结果与激光里程计的递推时长、环境激光特征点数线性相关,对应的权重占比为:
Weight激光里程计=(δ激光里程计的递推时长+δ特征点数)/2 (5)
其中,δ激光里程计的递推时长为与激光里程计推算模块的递推时长相关的系数,C4为激光里程计递推时长常值系数,本实施例中驱常值0.8285,Δt2为激光里程计的递推时间间隔;δ特征点数为与特征点数相关的系数,C5为特征点数常值系数,本实施例中取常值0.0726,n激光点云总数为用于激光里程计计算的激光帧点云总数;n匹配的特征点数为相邻帧间匹配的激光点云数量。
④视觉里程计推算模块定位定姿,通过视觉传感器采集的影像信息完成立体像对匹配及车辆位置姿态的计算,其递推位姿推算结果与视觉里程计的递推时长、帧间匹配度线性相关,对应的权重占比为:
Weight视觉里程计=(δ视觉里程计的递推时长+δ帧间匹配度)/2 (6)
其中,δ视觉里程计的递推时长为与视觉里程计推算模块的递推时长相关的系数,
C6为视觉里程计递推时长常值系数,本实施例中取常值0.0942,Δt3为视觉里程计的递推时间间隔;δ帧间匹配度为与帧间匹配度相关的系数,C7为视觉里程计的帧间匹配度常值系数,本实施例中取常值0.1738,xi为第i个特征点的位置,i=1,2,……,n,|xi-xi+1|为匹配的视觉特征点欧式距离差值。
3、将所有权重占比进行归一化处理,得到归一化后的权重占比。具体公式为:
WeightTotal=WeightGNSS+Weight轮式里程计+Weight激光里程计+Weight视觉里程计 (7)
WeightGNSS_1=WeightGNSS/WeightTotal (8)
Weight轮式里程计_1=Weight轮式里程计/WeightTotal (9)
Weight激光里程计_1=Weight激光里程计/WeightTotal (10)
Weight视觉里程计_1=Weight视觉里程计/WeightTotal (11)
4、根据各个位姿推算模块归一化处理后的权重占比,对各个位姿推算模块t时刻的位姿推算结果进行加权求和,得到的结果作为t时刻该移动设备的位姿推算结果,即t时刻激光帧的位置姿态:
例如:在室外开放道路场景下,GNSS组合导航数据为固定解,即GNSS组合导航信号无遮挡,定位误差低于±0.05cm时,根据上述计算结果,将选取GNSS组合导航位置(x,y,z)和姿态信息(Roll,Pitch,Yaw)作为当前激光帧的位置姿态;在高架桥道路场景下,GNSS组合导航数据为非固定解,但激光里程计数据和视觉里程计权重增大,则自动融合采用激光里程计数据和视觉里程计融合后的位置和姿态数据作为当前激光帧的位置姿态。需说明的是,这两种场景下均忽略了几种位姿推算模块的位姿推算结果,这是因为被忽略的位姿推算模块的权重占比约为0(实际不为0),第一种场景下GNSS组合导航定位模块的权重占比较大,第二种场景下激光里程计推算模块和视觉里程计推算模块的权重占比较大。
5、结合各个激光帧以及其位置姿态,进行一定的旋转、缩放、平移,转换成到统一的大地坐标系下,完成全部激光帧和视觉数据的快速拼接和融合,得到激光点云地图数据,并称该地图为多源融合地图,最后输出保存为加密私有地图文件。
步骤三,得到激光点云地图后,需对激光点云地图(多源融合地图)进行优化及加工处理。首先将地图数据中的地面点云和动态目标物进行快速过滤,然后按照固定格网大小进行地图分块操作,最终输出用于车辆定位的高精度定位子地图文件及地图索引文件。具体流程如图4所示,包括:
1、为了提高后续自动驾驶过程中激光地图匹配定位的精度,需对得到的激光点云地图进行加工处理。激光点云地图中的地面数据量大,由于其相似性特征的缘故会降低激光地图匹配定位的精度,因此需要进行地面点云的快速剔除。其流程如图5所示。
首先,由于激光设备安装在车辆固定位置,切扫描角度与地面平行,通过高程阈值H0直接将低于H0的点云进行快速过滤。其次,通过地面拟合算法,将真实地面点云快速分割出来,再从点云地图中进行剔除,即完成了地面点云的快速过滤。
2、在数据采集构建地图过程中,往往存在许多不必要的车辆、行人等动态目标物的遮挡等现象出现,致使地图精度大大下降,不利于后期地图定位的实现,需要对这些动态目标物进行过滤。其流程如图6所示。
对同一场景进行重复两次采集,分别得到语义点云地图A和语义点云地图B,假设两次采集场景中,极少概率出现同一位置有同一动态目标物出现在相同位置场景。此时,对地图数据进行网格Grid划分,网格大小默认取1cm效果最佳,对比相同网格内两个时刻的激光点云数据,若其中一次激光点云数据中该网格内含有该目标物而另一次激光点云数据中该网格内不含有该目标物,则认为该目标物为动态目标物,完成数据采集过程中的行人、车辆等动态目标物的剔除过滤;若两次激光点云数据中该网格内均含此目标物,则认为该目标物为非动态目标物,进行保留。
保留后,首先进行场景区域栅格划分,将栅格与语义点云地图进行关系映射。遍历子网格内语义点云地图A和语义点云地图B的点云数量:若这两地图子网格内点云为空,则子网格取空即可;若这两地图子网格内点云为非空,则在该子网格内两地图所含点云数量一致性高于90%时任取其中一个激光语义点云地图,新建栅格地图进行点云存储,或者在该子网格内两地图所含点云数量一致性低于或者等于90%时取点云数量较少的激光语义地图,新建栅格地图进行点云存储。
3、构建激光点云地图是为了自动驾驶车辆实现高精度激光定位,若将整个场景下的地图数据读入内存,激光匹配定位缓慢,会造成位姿推算模块卡顿或者崩溃,影响位姿推算效果和精度。因此需要进行地图分块,以减少激光匹配定位过程的搜索范围,提高定位实时性,从而提高定位精度。
首先,对激光点云数据按照网格大小Grid_Size,将激光点云地图数据进行栅格处理,划分生成n行m列,共Num=n*m个格子;然后,将对应的点云数据填充到对应的小格子中;最后,将每个格网进行文件私有加密存储,生成定位子图地图数据,同时生成包含关联的高精度子图的id信息、Grid分块范围外接矩形的左下点大地坐标和右上点大地坐标和地图数据左上角的点云坐标作为地图基准数据。
至此,便可完成整个激光点云地图的构建。本发明首先获取当前时刻下自动驾驶车辆多源感知测量设备的感知数据;然后经时间同步模块和坐标变换模块,输出同步后的初始融合数据,并将初始融合数据输入至不同的位姿推算模块计算处理输出多个当前设备的位置姿态;接着根据影响位姿推算模块精度的影响因子,自适应分配各个位姿推算模块位姿结果的权重占比;最后根据权重占比,对各个位姿推算模块的结果进行归一化加权取均值,输出当前时刻下自动驾驶车辆的位置姿态结果;将该时刻下的激光帧数据按照最终的位置姿态结果,经坐标旋转变换和平移,投影至地图坐标系下,进行地图构建。本发明通过融合多种位姿推算模块结果,输出适应于各种自动驾驶场景的高精度位置姿态数据,进而构建出高精度、强鲁棒的激光定位点云地图。
装置实施例:
本发明的一种定位地图构建装置实施例,如图7所示,包括存储器、处理器和内部总线,处理器、存储器之间通过内部总线完成相互间的通信和数据交互。存储器包括至少一个存储于存储器中的软件功能模块,处理器通过运行存储在存储器中的软件程序以及模块,执行各种功能应用以及数据处理,实现本发明的方法实施例中介绍的一种定位地图构建方法,即上述方法实施例中的方案步骤通过程序来指令相关硬件完成,该程序在执行时,执行包括上述方法实施例的步骤。
其中,处理器可以为微处理器MCU、可编程逻辑器件FPGA等处理装置。存储器用于存储程序,处理器在接收到执行指令后,执行程序。
存储器可为利用电能方式存储信息的各式存储器,RAM、ROM等;利用磁能方式存储信息的各式存储器,例如硬盘、软盘、磁带、磁芯存储器、磁泡存储器、U盘等;利用光学方式存储信息的各式存储器,例如CD、DVD等。当然,还有其他方式的存储器,例如量子存储器、石墨烯存储器等。
Claims (8)
1.一种定位地图构建方法,其特征在于,包括如下步骤:
1)获取某一时刻移动设备的周围环境信息;
2)获取移动设备上设置的各个位姿推算模块同步后的数据,并进行相应处理得到该时刻各个位姿推算模块对移动设备的位姿推算结果;所述各个位姿推算模块包括GNSS组合导航定位模块、轮式里程计推算模块、激光里程计推算模块和视觉里程计推算模块中的至少两种,位姿推算结果包括位置和姿态;
3)根据该时刻影响各个位姿推算模块位姿推算精度的影响因素,计算与各个影响因素相关的系数,对于某一位姿推算模块,求取其各个系数的均值得到该位姿推算模块的权重占比,进而确定各个位姿推算模块的权重占比,并进行归一化处理;
其中,所述GNSS组合导航定位模块的影响因素包括卫星数量、信道比、精度因子和协方差比率,且GNSS组合导航定位模块输出的协方差比率为固定解、浮点解、单点解三种定位状态对应的GNSS组合导航定位模块的权重占比依次减小;
所述轮式里程计推算模块的影响因素包括轮式里程计的递推时长和编码器精度,与轮式里程计推算模块的递推时长相关的系数δ轮式里程计的递推时长和与编码器精度相关的系数δ编码器精度分别为:δ编码器精度=C2*D前后轴距+C3*r轮子半径,C1为轮式里程计递推时长常值系数,Δt1为轮式里程计的递推时间间隔,C2为编码精度常值系数,D前后轴距为前后轮轴距离,C3为编码器精度常值系数,r轮子半径为四轮的轮子半径;
所述激光里程计推算模块的影响因素包括激光里程计的递推时长和特征点数,与激光里程计推算模块的递推时长相关的系数δ激光里程计的递推时长和与特征点数相关的系数δ特征点数分别为:C5为特征点数常值系数,n激光点云总数为用于激光里程计计算的激光帧点云总数,n匹配的特征点数为相邻帧间匹配的激光点云数量;
所述视觉里程计推算模块的影响因素包括视觉里程计的递推时长和帧间匹配度,与视觉里程计推算模块的递推时长相关的系数δ视觉里程计的递推时长和与帧间匹配度相关的系数δ帧间匹配度分别为:C6为视觉里程计递推时长常值系数,Δt3为视觉里程计的递推时间间隔,C7为视觉里程计的帧间匹配度常值系数,xi为第i个特征点的位置,i=1,2,……,n,|xi-xi+1|为匹配的视觉特征点欧式距离差值;
4)根据各个位姿推算模块归一化处理后的权重占比,对该时刻各个位姿推算模块的位姿推算结果进行加权求和,得到的结果作为该时刻该移动设备的位姿推算结果;
5)根据各个时刻该移动设备的位姿推算结果以及周围环境信息,进行地图构建。
2.根据权利要求1所述的定位地图构建方法,其特征在于,所述GNSS组合导航定位模块的权重占比为:
其中,Ratio为GNSS组合导航定位定姿模块输出结果的协方差比率,根据GNSS组合导航定位定姿模块输出结果的协方差计算得到,且Ratio=方差次优解/方差最优解。
3.根据权利要求1或2所述的定位地图构建方法,其特征在于,步骤1)中,所述周围环境信息包括周围环境的激光点云数据、或者包括周围环境的激光点云数据和视觉图像数据。
4.根据权利要求1所述的定位地图构建方法,其特征在于,所述各个位姿推算模块包括GNSS组合导航定位模块,将初始时刻GNSS组合导航定位模块的位姿推算结果作为初始时刻该移动设备的位姿推算结果。
5.根据权利要求3所述的定位地图构建方法,其特征在于,步骤5)中,构建好地图后,还包括根据地图中激光点云的高程将地图中的地面点点云进行过滤的步骤:
判断激光点云的高程是否大于高程阈值:若大于等于高程阈值,则该激光点云为非地面点点云,保留该激光点云数据;若小于高程阈值,则该激光点云为地面点点云,滤除该激光点云数据。
6.根据权利要求3所述的定位地图构建方法,其特征在于,步骤5)中,构建好地图后,还包括将地图中的动态目标物进行过滤的步骤:
获取同一场景不同时刻的局部地图,对各个局部地图进行网格划分,对比相同网格内不同时刻的激光点云数据:不同时刻该网格内均含有一目标物,则确定该目标物为非动态目标物,保留该目标物;至少一个时刻该网格内不含有该目标物且至少一个时刻该网格内含有该目标物,则确定该目标物为动态目标物,过滤该目标物。
7.根据权利要求3所述的定位地图构建方法,其特征在于,步骤5)中,构建好地图后,还包括将地图进行栅格处理的步骤:
将构建好的地图划分为n*m个格子,将对应的激光点云数据填充到对应的格子中,每个格子构成一个子地图并进行存储,n≥1,m≥1。
8.一种定位地图构建装置,其特征在于,包括存储器和处理器,所述处理器用于执行存储在存储器中的指令以实现如权利要求1~7任一项所述的定位地图构建方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011298342.8A CN114518108B (zh) | 2020-11-18 | 2020-11-18 | 一种定位地图构建方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011298342.8A CN114518108B (zh) | 2020-11-18 | 2020-11-18 | 一种定位地图构建方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114518108A CN114518108A (zh) | 2022-05-20 |
CN114518108B true CN114518108B (zh) | 2023-09-08 |
Family
ID=81594449
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011298342.8A Active CN114518108B (zh) | 2020-11-18 | 2020-11-18 | 一种定位地图构建方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114518108B (zh) |
Citations (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104854428A (zh) * | 2013-12-10 | 2015-08-19 | 深圳市大疆创新科技有限公司 | 传感器融合 |
CN105045263A (zh) * | 2015-07-06 | 2015-11-11 | 杭州南江机器人股份有限公司 | 一种基于Kinect的机器人自定位方法 |
CN107918386A (zh) * | 2017-10-25 | 2018-04-17 | 北京汽车集团有限公司 | 用于车辆的多传感器数据融合方法、装置及车辆 |
CN108181636A (zh) * | 2018-01-12 | 2018-06-19 | 中国矿业大学 | 石化工厂巡检机器人环境建模与地图构建装置和方法 |
CN108254758A (zh) * | 2017-12-25 | 2018-07-06 | 清华大学苏州汽车研究院(吴江) | 基于多线激光雷达和gps的三维道路构建方法 |
CN108345008A (zh) * | 2017-01-23 | 2018-07-31 | 郑州宇通客车股份有限公司 | 一种目标物检测方法、点云数据提取方法及装置 |
CN108664841A (zh) * | 2017-03-27 | 2018-10-16 | 郑州宇通客车股份有限公司 | 一种基于激光点云的动静态目标物识别方法及装置 |
CN109697710A (zh) * | 2017-10-21 | 2019-04-30 | 江苏华扬信息科技有限公司 | 一种基于分层格网的滤波处理算法 |
CN110196044A (zh) * | 2019-05-28 | 2019-09-03 | 广东亿嘉和科技有限公司 | 一种基于gps闭环检测的变电站巡检机器人建图方法 |
CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及系统 |
CN110430534A (zh) * | 2019-09-12 | 2019-11-08 | 北京云迹科技有限公司 | 一种定位选择方法、装置、电子设备及存储介质 |
CN111595333A (zh) * | 2020-04-26 | 2020-08-28 | 武汉理工大学 | 视觉惯性激光数据融合的模块化无人车定位方法及系统 |
CN111739063A (zh) * | 2020-06-23 | 2020-10-02 | 郑州大学 | 一种基于多传感器融合的电力巡检机器人定位方法 |
CN111812698A (zh) * | 2020-07-03 | 2020-10-23 | 北京图森未来科技有限公司 | 一种定位方法、装置、介质和设备 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10579068B2 (en) * | 2016-10-03 | 2020-03-03 | Agjunction Llc | Using optical sensors to resolve vehicle heading issues |
-
2020
- 2020-11-18 CN CN202011298342.8A patent/CN114518108B/zh active Active
Patent Citations (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104854428A (zh) * | 2013-12-10 | 2015-08-19 | 深圳市大疆创新科技有限公司 | 传感器融合 |
CN105045263A (zh) * | 2015-07-06 | 2015-11-11 | 杭州南江机器人股份有限公司 | 一种基于Kinect的机器人自定位方法 |
CN108345008A (zh) * | 2017-01-23 | 2018-07-31 | 郑州宇通客车股份有限公司 | 一种目标物检测方法、点云数据提取方法及装置 |
CN108664841A (zh) * | 2017-03-27 | 2018-10-16 | 郑州宇通客车股份有限公司 | 一种基于激光点云的动静态目标物识别方法及装置 |
CN109697710A (zh) * | 2017-10-21 | 2019-04-30 | 江苏华扬信息科技有限公司 | 一种基于分层格网的滤波处理算法 |
CN107918386A (zh) * | 2017-10-25 | 2018-04-17 | 北京汽车集团有限公司 | 用于车辆的多传感器数据融合方法、装置及车辆 |
CN108254758A (zh) * | 2017-12-25 | 2018-07-06 | 清华大学苏州汽车研究院(吴江) | 基于多线激光雷达和gps的三维道路构建方法 |
CN108181636A (zh) * | 2018-01-12 | 2018-06-19 | 中国矿业大学 | 石化工厂巡检机器人环境建模与地图构建装置和方法 |
CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及系统 |
CN110196044A (zh) * | 2019-05-28 | 2019-09-03 | 广东亿嘉和科技有限公司 | 一种基于gps闭环检测的变电站巡检机器人建图方法 |
CN110430534A (zh) * | 2019-09-12 | 2019-11-08 | 北京云迹科技有限公司 | 一种定位选择方法、装置、电子设备及存储介质 |
CN111595333A (zh) * | 2020-04-26 | 2020-08-28 | 武汉理工大学 | 视觉惯性激光数据融合的模块化无人车定位方法及系统 |
CN111739063A (zh) * | 2020-06-23 | 2020-10-02 | 郑州大学 | 一种基于多传感器融合的电力巡检机器人定位方法 |
CN111812698A (zh) * | 2020-07-03 | 2020-10-23 | 北京图森未来科技有限公司 | 一种定位方法、装置、介质和设备 |
Non-Patent Citations (1)
Title |
---|
基于卡尔曼滤波模型的多传感器数据融合导航定位建模与仿真;宋之卉 等;《数字通信世界》;62-63 * |
Also Published As
Publication number | Publication date |
---|---|
CN114518108A (zh) | 2022-05-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109341706B (zh) | 一种面向无人驾驶汽车的多特征融合地图的制作方法 | |
CN108802785B (zh) | 基于高精度矢量地图和单目视觉传感器的车辆自定位方法 | |
CN111272165B (zh) | 一种基于特征点标定的智能车定位方法 | |
CN109631887B (zh) | 基于双目、加速度与陀螺仪的惯性导航高精度定位方法 | |
WO2022007776A1 (zh) | 目标场景区域的车辆定位方法、装置、设备和存储介质 | |
CN102042835B (zh) | 自主式水下机器人组合导航系统 | |
CN113870343B (zh) | 相对位姿标定方法、装置、计算机设备和存储介质 | |
CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法 | |
AU2022201558A1 (en) | Pose estimation method and device, related equipment and storage medium | |
CN112162297B (zh) | 一种剔除激光点云地图中动态障碍伪迹的方法 | |
EP4124829B1 (en) | Map construction method, apparatus, device and storage medium | |
Oniga et al. | Curb detection for driving assistance systems: A cubic spline-based approach | |
CN112734765A (zh) | 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质 | |
CN113920198B (zh) | 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法 | |
CN111860072A (zh) | 泊车控制方法、装置、计算机设备及计算机可读存储介质 | |
CN113240813B (zh) | 三维点云信息确定方法及装置 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN116997771A (zh) | 车辆及其定位方法、装置、设备、计算机可读存储介质 | |
CN114485698A (zh) | 一种交叉路口引导线生成方法及系统 | |
CN115046542A (zh) | 地图生成方法、装置、终端设备及存储介质 | |
CN111829514A (zh) | 一种适用于车辆底盘集成控制的路面工况预瞄方法 | |
CN113515128B (zh) | 一种无人车实时路径规划方法及存储介质 | |
CN114518108B (zh) | 一种定位地图构建方法及装置 | |
CN114459474B (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
CN113403942B (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 | ||
CB02 | Change of applicant information |
Address after: No. 6, Yutong Road, Guancheng Hui District, Zhengzhou, Henan 450061 Applicant after: Yutong Bus Co.,Ltd. Address before: No.1, Shibali Heyu Road, Guancheng Hui District, Zhengzhou City, Henan Province Applicant before: ZHENGZHOU YUTONG BUS Co.,Ltd. |
|
CB02 | Change of applicant information | ||
GR01 | Patent grant | ||
GR01 | Patent grant |