CN113758491B - 基于多传感器融合无人车的相对定位方法、***及车辆 - Google Patents

基于多传感器融合无人车的相对定位方法、***及车辆 Download PDF

Info

Publication number
CN113758491B
CN113758491B CN202110897052.3A CN202110897052A CN113758491B CN 113758491 B CN113758491 B CN 113758491B CN 202110897052 A CN202110897052 A CN 202110897052A CN 113758491 B CN113758491 B CN 113758491B
Authority
CN
China
Prior art keywords
imu
frame
map
data
point cloud
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
CN202110897052.3A
Other languages
English (en)
Other versions
CN113758491A (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.)
Chongqing Changan Automobile Co Ltd
Original Assignee
Chongqing Changan Automobile 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 Chongqing Changan Automobile Co Ltd filed Critical Chongqing Changan Automobile Co Ltd
Priority to CN202110897052.3A priority Critical patent/CN113758491B/zh
Publication of CN113758491A publication Critical patent/CN113758491A/zh
Application granted granted Critical
Publication of CN113758491B publication Critical patent/CN113758491B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W60/00—Drive control systems specially adapted for autonomous road vehicles
    • 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/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
    • G01C21/30—Map- or contour-matching
    • 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
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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
    • 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
    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W2400/00—Indexing codes relating to detected, measured or calculated conditions or factors

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)
  • Human Computer Interaction (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种基于多传感器融合无人车的相对定位方法、***及车辆，包括以下步骤：（1）数据接入；（2）定位初始化；（3）IMU积分推算；（4）地图数据库匹配；（5）融合定位建模；（6）融合定位求解。本发明降低了累计误差对定位结果的影响，以及有效地减少了匹配过程中出现的误匹配结果。

Description

基于多传感器融合无人车的相对定位方法、***及车辆
技术领域
本发明属于定位技术领域，具体涉及一种基于多传感器融合无人车的相对定位方法、***及车辆。
背景技术
对于自主移动无人车来说，它需具备的一个核心能力是运用传感器来感知周围的环境。无人车从事自主导航工作时，在环境中的实时定位一直是一个热门的研究问题。传统的定位方法是通过安装在无人车轮子上的编码器所检测的数据来实时推算出无人车的当前位姿。这种方法简单易用但是存在其固有的累积误差。机器人在地面移动时不可避免地会出现打滑情况，以及***设计制造过程中存在的误差，这些误差随着机器人的移动会不断累积，运行一段时间后位姿的计算结果将会出现无法校正和应用的情况，因而这种方法有相应的局限性。
在机器人定位的研究过程中，人们逐渐采用激光雷达扫描匹配的技术来修正里程计在位姿计算过程中所产生的偏差。扫描匹配是将当前时刻的激光雷达扫描结果同前一时刻的扫描结果相匹配，通过匹配可得到两组激光点云的位姿变化关系，进而确定机器人本体在环境中的位姿。这种方法较好地弥补了里程计定位所存在的累积误差结果，通常在雷达具有本身精度较高的情况下，定位结果精度也较高。然而激光扫描匹配的方法也有一定的局限性，对于扫描的环境必须包含较多的路标特征点，同时路标特征不能高度相似，否则会出现误匹配的情况。
因此，有必要开发一种新的基于多传感器融合无人车的相对定位方法、***及车辆。
发明内容
本发明的目的是提供一种基于多传感器融合无人车的相对定位方法、***及车辆，能降低累计误差对定位结果的影响，以及能有效减少匹配过程中出现的误匹配结果。
第一方面，本发明所述的一种基于多传感器融合无人车的相对定位方法，包括以下步骤：
步骤1.数据接入；
实时接收GPS、IMU、轮速计和激光雷达所输出的数据，对每一帧的数据附上当前时刻的时间戳信息，并分别存入对应的缓存区中；
步骤2.定位初始化；
从GPS缓存区弹出两帧GPS数据，基于两帧GPS数据来判断车辆是否发生移动，若是，则计算出当前车辆的位置P、速度V和航向角；
步骤3.IMU积分推算；
根据IMU动力学公式，通过使用IMU的线速度和角速度积分得到每个IMU时刻的IMU状态(R、p、V、ba、bg)，其中R表示姿态旋转量，p表示平移量，ba表示陀螺仪的偏差，bg表示加速度计的偏差；
根据历史定位姿态和IMU积分得出的位姿(R、p)对实时的激光雷达点云做畸变矫正；
步骤4.地图数据库匹配；
根据IMU积分推算的平移量p，使用KNN查找地图数据库中与其最近的地图帧，使用NDT匹配算法计算出实时雷达帧和查找到的地图帧之间对应的位姿(R,p)，并将其作为一元因子添加到因子图模型计算图中；
步骤5.融合定位建模；
根据接收到的GPS信号，并将其作为一元Factor添加到因子图模型计算图中；
根据接收到的轮速信号，并将其作为一元Factor添加到因子图模型计算图中；
将IMU状态(R，p，v，ba，bg)作为二元Factor添加到因子图模型计算图中；
步骤6.融合定位求解；
将所有数据的因子发送到iSAM2模块中,得到最后的本车在地图中的横纵向坐标及航向角。
可选地，使用两帧大于0.1米的GPS数据，通过前一帧减去后一帧的位移再除以时间，得到速度，通过X方向和Y方向上的速度向量求出航向角；将计算出的速度用作IMU的初始化速度，并把GPS坐标发送到NDT数据库，用于搜索距离最近一帧NDT数据的bin文件；搜索到以后，使用当前帧和地图做配准，得出当前车辆的位置P、速度V和航向角，同时将计算出的位置P及航向角作为IMU的初始化位姿,同时速度V一起作为主线的IMU初始化参数。
可选地，求每个时刻的IMU状态向量，具体为：
首先查找两个时刻之间的IMU数据，创建一个6*1的矩阵，包括加速度和角速度，并将两帧数据的时间差送入因子图模型中的imu_preintegrated函数来做积分,根据每进来一帧的IMU数据来预测IMU的状态(R、p、V、ba、bg)。
可选地，所述步骤4，具体为：
建图过程：输入一条轨迹，即通过imufactor计算得出的定位结果{Ri，pi}i＝1....N；其中，Ri表示旋转向量，pi表示平移向量；以及对应时刻的雷达点云帧：{Li}i＝1....N，其中，Li表示局部坐标系下的激光雷达每一帧的位姿信息，将每一帧的点云位姿转换到世界坐标系下，即其中，/>表示局部坐标系转换到世界坐标系下的雷达每一帧的位姿信息，pi表示坐标转换时的平移量，最终得到激光雷达的点云地图；
地图匹配：使用NDT来匹配，即以当前点为中心，将参考点云所占的空间划分成指定大小的网格或者体素；将激光雷达的扫描点落在小格子当中的统计下来，当每个格子收集到的激光点不少于4个时，计算每个网格的多维正态分布参数，求出对应格子的均值和协方差；然后通过IMU得到的里程计数据赋值给变换参数，对于要配准的点云，通过变换矩阵T将其转换到参考点云的网格中，即其中，/>表示配准后坐标转换到地图下的点云坐标，/>表示未转换坐标的激光雷达点云坐标，Lnew表示局部坐标系下的激光雷达点云坐标；计算位于格子内的点云的正态分布PDF参数：p(k)＝exp[-0.5(k-q)t∑-1(k-q)]，其中，k表示一个网格内所有的扫描点，q表示正态分布的均值；对每个网格计算出的概率密度相加得到NDT配准的得分，再根据牛顿优化算法对目标函数score进行优化，即寻找变换参数使得得分值最优，得到最后的定位NDT定位结果；接到GPS第一帧的坐标信息以后，开始寻找距离这帧坐标最近的地图帧，找到后将地图帧保存到容器中，然后用去除点云畸变坐标转换后的当前激光雷达数据同地图帧做配准，得到配准后的相对位姿，并将得到后的位姿作为一元因子传到因子图模型计算图中。
第二方面，本发明所述的一种基于多传感器融合无人车的相对定位***，包括存储器和控制器，所述控制器与存储器连接，所述存储器内存储有计算机可读程序，所述控制器调用该计算机可读程序时，能执行如本发明所述的基于多传感器融合无人车的相对定位方法的步骤。
第三方面，本发明所述的一种车辆，采用如本发明所述的基于多传感器融合无人车的相对定位***。
本发明具有以下优点：本发明能够对以往里程计***定位所存在的固有约束进行提高，降低了累计误差对定位结果的影响；本发明所运用的激光点云扫描匹配与IMU、GPS、里程计定位相结合，针对大范围环境或者环境路标特征相似度较高的情况，能够有效地减少匹配过程中出现的误匹配结果。
附图说明
图1为本实施例中各传感器相互关系以及处理流程；
图2为本实施例中算法模块中各传感器数据在因子图模型中的处理方式；
图3为本实施例中基于GPS轨迹使用NDT算法建成的点云地图；
图4为本实施例中定位轨迹与高精度GPS轨迹对比图(已经全部重合)；
图5为现有单激光雷达相对定位结果与高精度GPS轨迹对比图(误差较大)。
具体实施方式
下面结合附图对本发明作进一步说明。
如图1所示，本实施例中，一种基于多传感器融合无人车的相对定位方法，包括以下步骤：
(1)数据接入；
分别接入GPS、IMU、轮速计和激光雷达所输出的数据，对每一帧的数据附上当前时刻的时间戳信息，并分别储存到对应的缓存区中。
(2)定位初始化；
IMU初始化，车辆首先静止50秒以上。GPS初始化，初始化使用两帧大于0.1米的GPS数据，前一帧减去后一帧的位移除以时间，可以算出来速度，通过X和Y方向上的速度向量，即可求出航向角yaw。如果没有轮速速度传入的话，会使用GPS速度来替换轮速加入。同时计算出的速度用作IMU的初始化速度，并把GPS坐标发送到NDT(正态分布变换建图)数据库，用来搜索距离最近一帧NDT数据的bin文件。搜索到以后，使用当前帧和地图做配准，从而得出当前车辆的位置P(即横纵向定位结果x,y)、速度V和航向角，同时将计算出的位置P及航向角作为IMU的初始化位姿,同时将前面GPS计算出的速度V一起作为主线的IMU初始化参数。
(3)IMU积分推算；
IMU得到初始化参数以后，就要开始求每个时刻的IMU状态向量。由于IMU在瞬时状态精准度比较高，但是积分误差非常大，IMU推算位姿方法，首先查找两个时刻之间的IMU数据，创建一个6*1的矩阵，包括加速度和角速度，并将两帧数据的时间差送入因子图模型中的imu_preintegrated函数来做积分，根据每进来一帧的IMU数据来预测IMU的状态(R、p、V、ba、bg)，同时需要其他传感器作为观测量来不断地校准，使用因子图模型中的imufactor函数来计算IMU累加值即得到里程计信息。根据历史定位姿态和IMU积分得出的位姿(R、p)对实时的激光雷达点云做畸变矫正。
(4)地图数据库匹配；
建图过程：输入一条轨迹，也即通过imufactor计算得出的定位结果{Ri，pi}i＝1....N；其中，Ri表示旋转向量，pi表示平移向量；以及对应时刻的雷达点云帧：{Li}i＝1....N，其中，Li表示局部坐标系下的激光雷达每一帧的位姿信息，将每一帧的点云位姿转换到轨迹的坐标系下，即其中，/>表示局部坐标系转换到世界坐标系下的雷达每一帧的位姿信息，pi表示坐标转换时的平移量，最终得到激光雷达的点云地图。
地图匹配：使用NDT即正态分布变换算法来匹配；NDT的大致思想是以当前点为中心，将参考点云所占的空间划分成指定大小的网格或者体素；例如(10cm)×(10cm)×(10cm)的小格子，激光雷达的扫描点落在小格子当中的被统计下来，只要满足每个格子收集到的激光点不少于4个，计算每个网格的多维正态分布参数，求出对应格子的均值和协方差。然后使用上面通过IMU得到的里程计数据赋值给变换参数，对于要配准的点云，通过变换矩阵T将其转换到参考点云的网格中，即其中，/>表示配准后坐标转换到地图下的点云坐标，/>表示未转换坐标的激光雷达点云坐标，Lnew表示局部坐标系下的激光雷达点云坐标。计算位于格子内的点云的正态分布PDF参数：p(k)＝exp[-0.5(k-q)t∑-1(k-q)]，其中，k表示一个网格内所有的扫描点，q表示正态分布的均值。每个PDF可以认为是局部平面的近似，描述了平面位置、朝向、形状和平滑性等特征。在对每个网格计算出的概率密度相加得到NDT配准的得分(score)，再根据牛顿优化算法对目标函数score进行优化，即寻找变换参数使得score值最最优，得到最后的定位NDT定位结果。接到GPS第一帧的坐标信息以后，开始寻找距离这帧坐标最近的地图帧，找到后将地图帧保存到容器中，然后用去除点云畸变坐标转换后的当前激光雷达数据同地图帧做配准，过程就是求解当前帧和NDT地图中每个格子中的高斯分布关系。得到配准后得到的相对位姿。将得到后的位姿作为一元因子传到因子图模型中，作为调整IMU数据的一元因子。
(5)融合定位建模；
GPS接收到的数据也作为的IMU的位置观测量添加到因子图模型计算图中，同时自建一个一元因子来用于轮速计的数据，轮速计信息主要用做观测量，来推断本车的位置信息，同GPS信息做融合，作为一个参数不断的纠正IMU的位置信息。
(6)融合定位求解；
将所有数据的因子发送到iSAM2模块(长期滑动窗),得到最后的本车在地图中的横纵向坐标及航向角。
如图3所示，为本实施例中基于GPS轨迹使用NDT算法建成的点云地图。
如图4所示，为本实施例中定位轨迹与高精度GPS轨迹对比图，从图4中可看出，定位轨迹和高精度GPS轨迹已经全部重合，说明采用本方法的定位精度高，且稳定。
如图5所示，为现有单激光雷达相对定位结果与高精度GPS轨迹对比图，从图中可看出，单激光雷达相对定位结果的误差较大。
本实施例中，一种基于多传感器融合无人车的相对定位***，包括存储器和控制器，所述控制器与存储器连接，所述存储器内存储有计算机可读程序，所述控制器调用该计算机可读程序时，能执行如本实施例中所述的基于多传感器融合无人车的相对定位方法的步骤。
本实施例中，一种车辆，采用如本实施例中所述的基于多传感器融合无人车的相对定位***。

Claims (6)

1.一种基于多传感器融合无人车的相对定位方法，其特征在于，包括以下步骤：
步骤1.数据接入；
实时接收GPS、IMU、轮速计和激光雷达所输出的数据，对每一帧的数据附上当前时刻的时间戳信息，并分别存入对应的缓存区中；
步骤2.定位初始化；
从GPS缓存区弹出两帧GPS数据，基于两帧GPS数据来判断车辆是否发生移动，若是，则计算出当前车辆的位置P、速度V和航向角；
步骤3.IMU积分推算；
根据IMU动力学公式，通过使用IMU的线速度和角速度积分得到每个IMU时刻的IMU状态(R、p、V、ba、bg)，其中R表示姿态旋转量，p表示平移量，ba表示陀螺仪的偏差，bg表示加速度计的偏差；
根据历史定位姿态和IMU积分得出的位姿(R、p)对实时的激光雷达点云做畸变矫正；
步骤4.地图数据库匹配；
根据IMU积分推算的平移量p，使用KNN查找地图数据库中与其最近的地图帧，使用NDT匹配算法计算出实时雷达帧和查找到的地图帧之间对应的位姿(R，p)，并将其作为一元因子添加到因子图模型计算图中；
步骤5.融合定位建模；
根据接收到的GPS信号，并将其作为一元Factor添加到因子图模型计算图中；
根据接收到的轮速信号，并将其作为一元Factor添加到因子图模型计算图中；
将IMU状态(R，p，v，ba，bg)作为二元Factor添加到因子图模型计算图中；
步骤6.融合定位求解；
将所有数据的因子发送到iSAM2模块中，得到最后的本车在地图中的横纵向坐标及航向角。
2.根据权利要求1所述的基于多传感器融合无人车的相对定位方法，其特征在于：使用两帧大于0.1米的GPS数据，通过前一帧减去后一帧的位移再除以时间，得到速度，通过X方向和Y方向上的速度向量求出航向角；将计算出的速度用作IMU的初始化速度，并把GPS坐标发送到NDT数据库，用于搜索距离最近一帧NDT数据的bin文件；搜索到以后，使用当前帧和地图做配准，得出当前车辆的位置P、速度V和航向角，同时将计算出的位置P及航向角作为IMU的初始化位姿，同时速度V一起作为主线的IMU初始化参数。
3.根据权利要求2所述的基于多传感器融合无人车的相对定位方法，其特征在于：求每个时刻的IMU状态向量，具体为：
首先查找两个时刻之间的IMU数据，创建一个6*1的矩阵，包括加速度和角速度，并将两帧数据的时间差送入因子图模型中的imu_preintegrated函数来做积分，根据每进来一帧的IMU数据来预测IMU的状态(R、p、V、ba、bg)。
4.根据权利要求2所述的基于多传感器融合无人车的相对定位方法，其特征在于：所述步骤4，具体为：
建图过程：输入一条轨迹，即通过imufactor计算得出的定位结果{Ri，pi}i＝1....N；其中，Ri表示旋转向量，pi表示平移向量；以及对应时刻的雷达点云帧：{Li}i＝1....N，其中，Li表示局部坐标系下的激光雷达每一帧的位姿信息，将每一帧的点云位姿转换到世界坐标系下，即其中，/>表示局部坐标系转换到世界坐标系下的雷达每一帧的位姿信息，pi表示坐标转换时的平移量，最终得到激光雷达的点云地图；
地图匹配：使用NDT来匹配，即以当前点为中心，将参考点云所占的空间划分成指定大小的网格或者体素；将激光雷达的扫描点落在小格子当中的统计下来，当每个格子收集到的激光点不少于4个时，计算每个网格的多维正态分布参数，求出对应格子的均值和协方差；然后通过IMU得到的里程计数据赋值给变换参数，对于要配准的点云，通过变换矩阵T将其转换到参考点云的网格中，即其中，/>表示配准后坐标转换到地图下的点云坐标，/>表示未转换坐标的激光雷达点云坐标，Lnew表示局部坐标系下的激光雷达点云坐标；计算位于格子内的点云的正态分布PDF参数：p(k)＝exp[-0.5(k-q)t∑-1(k-q)]，其中，k表示一个网格内所有的扫描点，q表示正态分布的均值；对每个网格计算出的概率密度相加得到NDT配准的得分，再根据牛顿优化算法对目标函数score进行优化，即寻找变换参数使得得分值最优，得到最后的定位NDT定位结果；接到GPS第一帧的坐标信息以后，开始寻找距离这帧坐标最近的地图帧，找到后将地图帧保存到容器中，然后用去除点云畸变坐标转换后的当前激光雷达数据同地图帧做配准，得到配准后的相对位姿，并将得到后的位姿作为一元因子传到因子图模型计算图中。
5.一种基于多传感器融合无人车的相对定位***，包括存储器和控制器，所述控制器与存储器连接，其特征在于：所述存储器内存储有计算机可读程序，所述控制器调用该计算机可读程序时，能执行如权利要求1至4任一所述的基于多传感器融合无人车的相对定位方法的步骤。
6.一种车辆，其特征在于：采用如权利要求5所述的基于多传感器融合无人车的相对定位***。
CN202110897052.3A 2021-08-05 2021-08-05 基于多传感器融合无人车的相对定位方法、***及车辆 Active CN113758491B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110897052.3A CN113758491B (zh) 2021-08-05 2021-08-05 基于多传感器融合无人车的相对定位方法、***及车辆

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110897052.3A CN113758491B (zh) 2021-08-05 2021-08-05 基于多传感器融合无人车的相对定位方法、***及车辆

Publications (2)

Publication Number Publication Date
CN113758491A CN113758491A (zh) 2021-12-07
CN113758491B true CN113758491B (zh) 2024-02-23

Family

ID=78788807

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110897052.3A Active CN113758491B (zh) 2021-08-05 2021-08-05 基于多传感器融合无人车的相对定位方法、***及车辆

Country Status (1)

Country Link
CN (1) CN113758491B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及***
WO2019188704A1 (ja) * 2018-03-29 2019-10-03 パイオニア株式会社 自己位置推定装置、自己位置推定方法、プログラム及び記憶媒体
CN111413721A (zh) * 2020-01-14 2020-07-14 华为技术有限公司 车辆定位的方法、装置、控制器、智能车和***
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
CN111949943A (zh) * 2020-07-24 2020-11-17 北京航空航天大学 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
CN112083725A (zh) * 2020-09-04 2020-12-15 湖南大学 一种面向自动驾驶车辆的结构共用多传感器融合定位***
CN112254729A (zh) * 2020-10-09 2021-01-22 北京理工大学 一种基于多传感器融合的移动机器人定位方法
CN112923931A (zh) * 2019-12-06 2021-06-08 北理慧动(常熟)科技有限公司 一种基于固定路线下的特征地图匹配与gps定位信息融合方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP3078935A1 (en) * 2015-04-10 2016-10-12 The European Atomic Energy Community (EURATOM), represented by the European Commission Method and device for real-time mapping and localization

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019188704A1 (ja) * 2018-03-29 2019-10-03 パイオニア株式会社 自己位置推定装置、自己位置推定方法、プログラム及び記憶媒体
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及***
CN112923931A (zh) * 2019-12-06 2021-06-08 北理慧动(常熟)科技有限公司 一种基于固定路线下的特征地图匹配与gps定位信息融合方法
CN111413721A (zh) * 2020-01-14 2020-07-14 华为技术有限公司 车辆定位的方法、装置、控制器、智能车和***
CN111522043A (zh) * 2020-04-30 2020-08-11 北京联合大学 一种无人车激光雷达快速重新匹配定位方法
CN111949943A (zh) * 2020-07-24 2020-11-17 北京航空航天大学 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法
CN112083725A (zh) * 2020-09-04 2020-12-15 湖南大学 一种面向自动驾驶车辆的结构共用多传感器融合定位***
CN112254729A (zh) * 2020-10-09 2021-01-22 北京理工大学 一种基于多传感器融合的移动机器人定位方法

Also Published As

Publication number Publication date
CN113758491A (zh) 2021-12-07

Similar Documents

Publication Publication Date Title
US11002859B1 (en) Intelligent vehicle positioning method based on feature point calibration
CN109709801B (zh) 一种基于激光雷达的室内无人机定位***及方法
Zou et al. A comparative analysis of LiDAR SLAM-based indoor navigation for autonomous vehicles
JP7086111B2 (ja) 自動運転車のｌｉｄａｒ測位に用いられるディープラーニングに基づく特徴抽出方法
CN107239076B (zh) 基于虚拟扫描与测距匹配的agv激光slam方法
KR102628778B1 (ko) 위치결정을 위한 방법, 컴퓨팅 기기, 컴퓨터 판독가능 저장 매체 및 컴퓨터 프로그램
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位***
WO2022007776A1 (zh) 目标场景区域的车辆定位方法、装置、设备和存储介质
CN112965063B (zh) 一种机器人建图定位方法
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN114004869A (zh) 一种基于3d点云配准的定位方法
CN114777775B (zh) 一种多传感器融合的定位方法及***
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN113218408B (zh) 适用于多地形的多传感器融合的2Dslam方法及其***
CN113192142A (zh) 复杂环境下的高精度地图构建方法、装置和计算机设备
CN114689035A (zh) 基于多传感器融合的长航程农田地图构建方法及***
CN116337045A (zh) 一种基于karto和teb的高速建图导航方法
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN115311349A (zh) 一种车辆自动驾驶辅助定位融合方法及其域控制***
CN113758491B (zh) 基于多传感器融合无人车的相对定位方法、***及车辆
CN116878542A (zh) 一种抑制里程计高度漂移的激光slam方法
CN117029870A (zh) 一种基于路面点云的激光里程计
CN113379915B (zh) 一种基于点云融合的行车场景构建方法
CN116067358A (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