CN114608568B - 一种基于多传感器信息即时融合定位方法 - Google Patents
一种基于多传感器信息即时融合定位方法 Download PDFInfo
- Publication number
- CN114608568B CN114608568B CN202210160719.6A CN202210160719A CN114608568B CN 114608568 B CN114608568 B CN 114608568B CN 202210160719 A CN202210160719 A CN 202210160719A CN 114608568 B CN114608568 B CN 114608568B
- Authority
- CN
- China
- Prior art keywords
- positioning
- point
- positioning information
- information
- gps
- 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
- 238000000034 method Methods 0.000 title claims abstract description 57
- 230000004927 fusion Effects 0.000 title claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims description 16
- 238000009826 distribution Methods 0.000 claims description 11
- 238000013519 translation Methods 0.000 claims description 10
- 230000009466 transformation Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000005457 optimization Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 5
- 238000011478 gradient descent method Methods 0.000 claims description 2
- 230000010354 integration Effects 0.000 claims description 2
- 230000001737 promoting effect Effects 0.000 claims description 2
- 238000010187 selection method Methods 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 6
- 238000001914 filtration Methods 0.000 description 5
- 230000007547 defect Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明针对室内外多场景定位算法选取与切换问题，提出了一种基于多传感器信息即时融合定位方法，解决了单一传感器鲁棒性差、精确度低、实时性差的问题。包括：步骤一、通过雷达利用NDT方法确定无人车平台的当前位置与姿态，并采用欧式距离残差的方法求解定位置信度；步骤二、将GPS和组合惯导发布的原始定位信息与步骤一得到的当前位置与姿态进行匹配，采用基于滑动窗口和LO‑RANSAC方法进行GPS定位信息的置信度估计，得到经过时间戳对齐的激光雷达和GPS定位信息配对点集；步骤三、将步骤一得到的当前位置与姿态和定位置信度以及步骤二得到的激光雷达和GPS定位信息配对点集结合IMU位姿信息采用UKF方法进行融合，得到无人车平台实时高精度定位结果。
Description
技术领域
本发明涉及一种基于UKF(UnscentedKalman filter，无迹卡尔曼滤波)的GPS、惯性测量单元和3D激光雷达多传感器信息即时融合定位方法，属于机器人感知与导航领域。
背景技术
多传感器信息融合定位技术是当前机器人和无人驾驶领域具有研究前景的技术领域。多传感器信息融合定位技术克服了单一传感器存在的信息量少、及时性和鲁棒性差等问题，从而有效的提高了移动机器人在复杂环境下定位感知的准确性和可行性。
多传感器信息融合定位技术目前主要融合对象为视觉或激光SLAM(SimultaneousLocalization andMapping，即时定位与建图)发布的定位信息、IMU(InertialMeasurementUnit，惯性测量单元)与GPS(GlobalPositioning System，全球定位系统)，融合手段主要为基于滤波的UKF，以及基于优化的位姿图优化等算法。多传感器信息融合定位技术有效避免了单一传感器定位技术的缺陷，面对复杂多变的环境具有较强的适应能力，并持续能够提供精确度高、实时性好的定位信息。但是多传感器信息融合定位技术仍然存在一系列问题，如何更有效的融合传感器的定位信息使其适应各种复杂环境如室内外多场景定位、未知环境下定位方案的选取和切换仍然是专家学者们研究的热点。
针对有效融合多传感器信息融合定位的问题，已有的解决方案有如下几种：
方案1：文献(张少将.基于多传感器信息融合的智能车定位导航系统研究[D].哈尔滨工业大学,2020.)提出了一种在户外应用自适应卡尔滤波的方法融合GPS、IMU与激光SLAM的定位信息，在室内采用非线性紧耦合优化的方法融合了视觉SLAM与激光SLAM的算法框架，从而实现了室内外多场景的即时定位。但是算法没有实现室内外的灵活切换同时也没有给出传感器信息来源的置信度，从而无法判断传感器信息是否精准可靠。
方案2：文献(DemirM,FujimuraK.Robust Localization with Low-MountedMultiple LiDARs in Urban Environments[C]//2019IEEE Intelligent TransportationSystems Conference-ITSC.IEEE,2019.)提出了一种采用扩展卡尔曼滤波算法融合GPS、IMU、视觉和激光SLAM的室外定位框架，采用基于NDT(Normal Distributions Transform，正态分布变换)的激光定位方法，结合动态障碍物识别算法提出一种计算激光定位置信度的方案，从而有效的弥补了激光定位信息置信度不确定的缺陷，进而提高了整体框架的定位精度。但美中不足的是，文中没有计算GPS的定位置信度，以致无法判断GPS提供的定位信息是否准确。
本发明基于上述两种方案的启发，根据所要解决问题的本身特点，加以实验验证，提出一种基于UKF的GPS、IMU和3D激光雷达多传感器信息即时融合定位方法，在点云地图已知的情况下，依据差分GPS所给出的定位信息初始化基于NDT的激光雷达定位算法，并同时通过UKF算法融合GPS、激光定位算法与IMU提供的位姿信息并实时计算激光定位算法的定位置信度以及应用LO-RANSAC(Locally Optimized Random Sample Consensus，随机抽样一致算法)算法实时计算与GPS定位信息的匹配程度，从而克服了差分GPS易被遮挡，激光雷达定位算法及时性差，IMU位姿信息精度低等缺陷，进而能够在室内外等复杂环境中即时去除置信度较低的传感器信息，保留置信度较高的传感器信息，从而保持融合后定位的准确性与实时性。
发明内容
本发明针对室内外多场景定位算法选取与切换问题，提出了一种基于多传感器信息即时融合定位方法，解决了单一传感器鲁棒性差、精确度低、实时性差的问题。
本发明通过以下技术方案实现。
一种基于多传感器信息的即时融合定位方法，包括：
步骤一、通过雷达利用NDT方法确定无人车平台的当前位置与姿态，并采用欧式距离残差的方法求解定位置信度；
步骤二、将GPS和组合惯导发布的原始定位信息与步骤一得到的当前位置与姿态进行匹配，采用基于滑动窗口和LO-RANSAC方法进行GPS定位信息的置信度估计，得到经过时间戳对齐的激光雷达和GPS定位信息配对点集；
步骤三、将步骤一得到的当前位置与姿态和定位置信度以及步骤二得到的激光雷达和GPS定位信息配对点集结合IMU位姿信息采用UKF方法进行融合，得到无人车平台实时高精度定位结果。
本发明的有益效果：
本发明通过激光雷达确定无人车平台的当前位置与姿态并计算其定位置信度并将激光雷达与GPS原始定位信息进行匹配计算得到GPS定位信息置信度估计。依据上述所得的激光雷达与GPS定位信息与置信度，赋予激光雷达和GPS定位信息合适的协方差矩阵，并加入IMU获取的姿态信息进行UKF融合，最终得到由激光雷达、GPS定位信息与IMU姿态信息的无人车平台实时高精度定位结果；在室内外多环境、未知环境等复杂条件下依然持续发布高精度的定位信息，同时采用的LO-RANSAC方法较传统RANSAC方法精确度更高、计算时间更少，并在实际测试中具有较好的鲁棒性、实时性和精确性。
附图说明
图1为本发明的基于多传感器信息即时融合定位方法流程图。
具体实施方式
下面结合附图对本发明做详细说明。
如图1所示，本发明具体实施方式的一种基于多传感器信息即时融合定位方法，具体包括以下步骤：
步骤一、通过雷达利用NDT(Normal Distributions Transform,正态分布变换)方法确定无人车平台的当前位置与姿态，并采用欧式距离残差的方法求解定位置信度；具体实施时，本步骤在点云地图和初始坐标与姿态已知的条件下进行，所述雷达采用激光雷达；具体如下：
1.1通过下采样方式将当前雷达扫描得到的点云转换为稀疏的局部点云，并利用NDT方法将所述局部点云和对应的原始点云地图转换为二维栅格地图的概率分布qi和pi；
1.2将上一时刻计算得到的姿态增量以及对IMU位姿数据进行预积分得到当前位姿的预测值，将所述概率分布根据所述预测值粗匹配至世界坐标系，得到粗略的平移旋转矩阵Tini；
1.3通过构建优化目标匹配当前局部点云与输入的原始点云地图，利用梯度下降法最小化的优化方程，以所述粗略的旋转平移矩阵Tini作为初始值，得到当前无人车位姿与上一时刻位姿间的旋转平移矩阵从而得到当前无人车平台的位姿信息，实现对无人车平台当前时刻位姿精准的定位；具体为：
其中，pi为原始点云地图中点经过NDT变换得到的二维栅格地图的概率分布，qi为与之对应的雷达扫描点的NDT变换后的二维栅格地图的概率分布，为最终得到的旋转平移矩阵；
1.4根据所述旋转平移矩阵计算得到当前雷达扫描点与原始点云地图对应点之间的欧式距离残差和。具体计算公式如下所示：
其中，Pi为原始点云地图中点的二维坐标，Qi为与之对应的雷达局部扫描点的二维坐标，score为欧式距离残差和，即激光雷达定位置信度估计。
这一步骤的实现原理是，通过对所述欧式距离残差和进行评估，进而对激光雷达定位置信度进行估计，从而对激光雷达定位算法发布的定位里程计信息赋予合适的协方差矩阵。
步骤二、将GPS和组合惯导发布的原始定位信息与步骤一得到的当前位置与姿态进行匹配，采用基于滑动窗口和LO-RANSAC方法进行GPS定位信息的置信度估计，得到经过时间戳对齐的激光雷达和GPS定位信息配对点集；
本实施例中，所述采用基于滑动窗口和LO-RANSAC方法进行GPS定位信息的置信度估计，具体步骤如下：
2.1通过初始化滑动窗口得到适当长度的滑动窗口和窗口内一组时间戳较为匹配初始的激光雷达和GPS定位信息点集，
2.2利用基于时间戳的方法匹配输入的激光雷达定位信息和GPS定位信息，同时选择前一次LO-RANSAC计算时间作为滑动窗口的前进速度(本实施例中约1.0s)；
这两步骤通过激光雷达和GPS信息的读取与预处理提高了LO-RANSAC匹配检测结果的实时性和精确度，利用了基于多线程和滑动窗口的数据读取与预处理机制，解决了激光雷达和GPS定位信息在发布、传输和读取过程中存在不同程度的时延导致匹配结果实时性差精确度低的问题。
2.3采用LO-RANSAC算法匹配所述激光雷达定位信息和GPS定位信息，估计GPS定位置信度；具体步骤如下：
2.3.1根据总点集中选择最小样本点数，利用随机选择方法得到一个最小样本点数的最小点集Sm；
2.3.2利用ICP(Iterative Closest Point，迭代最近点)方法估计符合所述最小点集Sm的模型，所述ICP方法的匹配公式如下：
其中，qi为第i个激光雷达定位信息，pi为第i个GPS定位信息，R为旋转矩阵，t为平移向量；
2.3.3将所述模型推广至总点集，计算在预设阈值(本实施例中)内符合该模型的点集数量，即内点数量；
2.3.4比较设定的比例常数和所述内点数量实际占总点数比例，从而判断所述模型好坏；
本实施例中，采用以下方式：若内点所占比例大于预设比例，则通过局部优化算法得到精确的内点数和模型，即将当前内点集作为输入点集，重复执行步骤2.3.1-2.3.3指定次数，同时更新运行次数上限k的方法进行优化；若不满足上述条件，则重复步骤2.3.1-2.3.3。所述运行次数上限k的更新规则如下所示：
其中，p为默认置信度，w为内点所占比例，m为输入的指定运行次数。
2.3.5比较运行次数和所述运行次数上限，当达到所述运行次数上限k，则将存储的运行结果取平均值并输出内点占输入点集的元素的比例，即内点率；
2.3.6将所述内点率赋予GPS定位信息的协方差矩阵，判断GPS定位置信度。
这一步骤解决了在室内外切换环境下GPS定位置信度波动以及组合惯导未输出协方差矩阵的情况下对GPS定位置信度的估计的问题。
步骤三、将步骤一得到的当前位置与姿态和定位置信度以及步骤二得到的激光雷达和GPS定位信息配对点集结合IMU位姿信息采用UKF进行融合，得到无人车平台实时高精度定位结果。具体步骤如下：
3.1将步骤一得到的当前位置与姿态和定位置信度结合IMU位姿信息作为观测变量对无人车平台进行建模，得到运动方程和观测方程：
Xt＝g(Xt-1)
Zt＝h(Xt)
其中，为t时刻无人车的状态变量，其中，x，y，z为三维坐标，roll，pitch，raw为欧拉角，/>为线速度，/>为角速度，/>为线加速度；Zt为观测变量，其中激光雷达和GPS定位信息中位置和姿态的融合变量为二维坐标x，y以及欧拉角roll，pitch，raw，IMU位姿信息的融合变量为角速度/>g和h为状态方程和观测方程的变换函数，本实施例中运动方程为短时间的均速直线运动模型，观测方程为单位阵乘以对应的观测变量。
3.2根据所述运动方程和观测方程计算状态变量的加权均值和方差的，再利用t-1时刻的所述均值μt-1和方差∑t-1对t-1时刻状态变量进行采样得到σ点，利用非线性变换得到t时刻状态变量并选择权重，结合测量误差计算得到预测的观测变量均值和方差；具体公式如下所示：
其中，μt-1为t-1时刻的均值，∑t-1为t-1时刻的方差，γ为比例因子，为无噪声的预测的t时刻无人车的状态变量，/>为预测的t时刻无人车的状态变量的加权均值，/>为预测的t时刻无人车的状态变量的方差，/>为选取的权值，Rt为测量误差。
3.3根据所述预测的观测变量均值和方差采样得到σ点，并对得到的σ点进行非线性变换得到预测的观测变量，结合过程噪声计算预测观测变量均值和方差，如下所示：
其中，为加入了预测阶段不确定性的新σ点，/>为预测的观测变量，/>为预测的观测变量的加权均值，/>为所选取的权值，St为预测的观测变量的方差，Qt为过程噪声。
3.4利用所述预测的状态变量和观测变量以及所述加权均值和方差计算得到卡尔曼增益Kt，计算过程如下所示：
通过上一步骤得到的卡尔曼增益Kt得到当前时刻t的无人车平台的定位信息，利用卡尔曼增益Kt计算当前时刻t无人车平台的状态变量Xt、均值μt和方差∑t，进而得到当前时刻t无人车平台的定位信息，具体计算过程如下所示：
这一步骤通过对前两步骤得到的GPS、3D激光雷达的定位里程计信息和输入的IMU发布的位姿信息进行融合得到了鲁棒性高、实时性好且精确度高的融合定位里程计信息，利用了基于UKF的滤波融合方式进行多传感器融合定位，解决了在室内外切换环境和未知环境中，差分GPS易被遮挡，激光雷达定位算法实时性差，IMU位姿信息精度低等缺陷的问题。
Claims (2)
1.一种基于多传感器信息即时融合定位方法，其特征在于，包括：
步骤一、通过雷达利用NDT方法确定无人车平台的当前位置与姿态，并采用欧式距离残差的方法求解定位置信度；
步骤二、将GPS和组合惯导发布的原始定位信息与步骤一得到的当前位置与姿态进行匹配，采用基于滑动窗口和LO-RANSAC方法进行GPS定位信息的置信度估计，得到经过时间戳对齐的激光雷达和GPS定位信息配对点集；
步骤三、将步骤一得到的当前位置与姿态和定位置信度以及步骤二得到的激光雷达和GPS定位信息配对点集结合IMU位姿信息采用UKF方法进行融合，得到无人车平台实时高精度定位结果；
所述求解定位置信度具体步骤如下：
1.1通过下采样方式将当前雷达扫描得到的点云转换为稀疏的局部点云，并利用NDT方法将所述局部点云和对应的原始点云地图转换为二维栅格地图的概率分布；
1.2将上一时刻计算得到的姿态增量以及对IMU位姿数据进行预积分得到当前位姿的预测值，将所述概率分布根据所述预测值粗匹配至世界坐标系，得到粗略的平移旋转矩阵；
1.3构建优化目标匹配当前局部点云与输入的原始点云地图，利用梯度下降法最小化的优化方程，以所述粗略的旋转平移矩阵作为初始值，得到当前无人车位姿与上一时刻位姿间的旋转平移矩阵；
1.4根据所述旋转平移矩阵计算得到当前雷达扫描点与原始点云地图对应点之间的欧式距离残差和；
所述采用基于滑动窗口和LO-RANSAC方法进行GPS定位信息的置信度估计，具体步骤如下：
2.1通过初始化滑动窗口得到适当长度的滑动窗口和窗口内一组时间戳较为匹配初始的激光雷达和GPS定位信息点集，
2.2利用基于时间戳的方法匹配输入的激光雷达定位信息和GPS定位信息，同时选择前一次LO-RANSAC计算时间作为滑动窗口的前进速度；
2.3采用LO-RANSAC算法匹配所述激光雷达定位信息和GPS定位信息，估计GPS定位置信度；
所述采用LO-RANSAC算法匹配所述激光雷达定位信息和GPS定位信息，具体步骤如下：
2.3.1根据总点集中选择最小样本点数，利用随机选择方法得到一个最小样本点数的最小点集；
2.3.2利用ICP方法估计符合所述最小点集的模型；
2.3.3将所述模型推广至总点集，计算在预设阈值内符合该模型的点集数量，即内点数量；
2.3.4比较设定的比例常数和所述内点数量实际占总点数比例，从而判断所述模型好坏；
2.3.5比较运行次数和运行次数上限，当达到运行次数上限k，则将存储的运行结果取平均值并输出内点占输入点集的元素的比例，即内点率；
2.3.6将所述内点率赋予GPS定位信息的协方差矩阵，判断GPS定位置信度；
所述判断所述模型好坏，采用以下方式：
若内点所占比例大于预设比例，则通过局部优化算法得到精确的内点数和模型，即将当前内点集作为输入点集，重复执行步骤2.3.1-2.3.3指定次数，同时更新运行次数上限的方法进行优化；若不满足，则重复步骤2.3.1-2.3.3；
所述运行次数上限k的更新规则如下所示：
其中，p为默认置信度，w为内点所占比例，m为输入的指定运行次数；
采用UKF进行融合得到无人车平台实时高精度定位结果，具体步骤如下：
3.1将步骤一得到的当前位置与姿态和定位置信度结合IMU位姿信息作为观测变量对无人车平台进行建模，得到运动方程和观测方程；
3.2根据所述运动方程和观测方程计算状态变量的加权均值和方差的，再利用t-1时刻的所述均值和方差对t-1时刻状态变量进行采样得到σ点，利用非线性变换得到t时刻状态变量并选择权重，结合测量误差计算得到预测的观测变量均值和方差；
3.3根据所述预测的观测变量均值和方差采样得到σ点，并对得到的σ点进行非线性变换得到预测的观测变量，结合过程噪声计算预测观测变量均值和方差；
3.4利用所述预测的状态变量和观测变量以及所述加权均值和方差计算得到卡尔曼增益。
2.如权利要求1所述的一种基于多传感器信息即时融合定位方法，其特征在于，所述预设阈值选择大于占总点集数的80％。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210160719.6A CN114608568B (zh) | 2022-02-22 | 2022-02-22 | 一种基于多传感器信息即时融合定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210160719.6A CN114608568B (zh) | 2022-02-22 | 2022-02-22 | 一种基于多传感器信息即时融合定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114608568A CN114608568A (zh) | 2022-06-10 |
| CN114608568B true CN114608568B (zh) | 2024-05-03 |
Family
ID=81858510
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210160719.6A Active CN114608568B (zh) | 2022-02-22 | 2022-02-22 | 一种基于多传感器信息即时融合定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114608568B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114755693B (zh) * | 2022-06-15 | 2022-09-16 | 天津大学四川创新研究院 | 基于多旋翼无人机的基建设施测量系统和方法 |
| CN116165958A (zh) * | 2023-04-25 | 2023-05-26 | 舜泰汽车有限公司 | 一种两栖特种无人平台的自动驾驶系统 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及系统 |
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN112082545A (zh) * | 2020-07-29 | 2020-12-15 | 武汉威图传视科技有限公司 | 一种基于imu和激光雷达的地图生成方法、装置及系统 |
| CN112904317A (zh) * | 2021-01-21 | 2021-06-04 | 湖南阿波罗智行科技有限公司 | 一种多激光雷达与gnss_ins系统标定方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11243081B2 (en) * | 2019-03-29 | 2022-02-08 | Trimble Inc. | Slam assisted INS |
| CN113945206B (zh) * | 2020-07-16 | 2024-04-19 | 北京图森未来科技有限公司 | 一种基于多传感器融合的定位方法及装置 |
-
2022
- 2022-02-22 CN CN202210160719.6A patent/CN114608568B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及系统 |
| CN112082545A (zh) * | 2020-07-29 | 2020-12-15 | 武汉威图传视科技有限公司 | 一种基于imu和激光雷达的地图生成方法、装置及系统 |
| CN112904317A (zh) * | 2021-01-21 | 2021-06-04 | 湖南阿波罗智行科技有限公司 | 一种多激光雷达与gnss_ins系统标定方法 |
Non-Patent Citations (1)
| Title |
|---|
| 一种基于多传感融合的室内建图和定位算法;纪嘉文 等;成都信息工程大学学报;第33卷(第4期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114608568A (zh) | 2022-06-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
| Adams et al. | SLAM gets a PHD: New concepts in map estimation | |
| CN114608568B (zh) | 一种基于多传感器信息即时融合定位方法 | |
| CN109597864B (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| Qu et al. | Landmark based localization in urban environment | |
| CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
| CN113252033B (zh) | 基于多传感器融合的定位方法、定位系统及机器人 | |
| CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
| CN111060099B (zh) | 一种无人驾驶汽车实时定位方法 | |
| CN112965063B (zh) | 一种机器人建图定位方法 | |
| CN110763239A (zh) | 滤波组合激光slam建图方法及装置 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN113776519B (zh) | 一种无光动态开放环境下agv车辆建图与自主导航避障方法 | |
| CN113252038A (zh) | 基于粒子群算法的航迹规划地形辅助导航方法 | |
| JP2023525927A (ja) | 車両位置推定システム及び方法 | |
| CN115980765A (zh) | 一种基于环境感知辅助粒子滤波的车辆定位方法 | |
| CN114442133A (zh) | 无人机定位方法、装置、设备及存储介质 | |
| CN112965076B (zh) | 一种用于机器人的多雷达定位系统及方法 | |
| Schüle et al. | Augmenting night vision video images with longer distance road course information | |
| CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及系统 | |
| CN113359167A (zh) | 一种通过惯性测量参数将gps与激光雷达融合定位的方法 | |
| CN114459474B (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
| CN113379915B (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 |