[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN114608568A - 一种基于多传感器信息即时融合定位方法 - Google Patents

一种基于多传感器信息即时融合定位方法 Download PDF

Info

Publication number
CN114608568A
CN114608568A CN202210160719.6A CN202210160719A CN114608568A CN 114608568 A CN114608568 A CN 114608568A CN 202210160719 A CN202210160719 A CN 202210160719A CN 114608568 A CN114608568 A CN 114608568A
Authority
CN
China
Prior art keywords
positioning
information
gps
positioning information
point
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.)
Granted
Application number
CN202210160719.6A
Other languages
English (en)
Other versions
CN114608568B (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202210160719.6A priority Critical patent/CN114608568B/zh
Publication of CN114608568A publication Critical patent/CN114608568A/zh
Application granted granted Critical
Publication of CN114608568B publication Critical patent/CN114608568B/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/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作为初始值，得到当前无人车位姿与上一时刻位姿间的旋转平移矩阵
Figure BDA0003514538900000041
从而得到当前无人车平台的位姿信息，实现对无人车平台当前时刻位姿精准的定位；具体为：
Figure BDA0003514538900000042
其中，pi为原始点云地图中点经过NDT变换得到的二维栅格地图的概率分布，qi为与之对应的雷达扫描点的NDT变换后的二维栅格地图的概率分布，
Figure BDA0003514538900000043
为最终得到的旋转平移矩阵；
1.4根据所述旋转平移矩阵
Figure BDA0003514538900000051
计算得到当前雷达扫描点与原始点云地图对应点之间的欧式距离残差和。具体计算公式如下所示：
Figure BDA0003514538900000052
其中，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方法的匹配公式如下：
Figure BDA0003514538900000061
其中，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的更新规则如下所示：
Figure BDA0003514538900000062
其中，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)
其中，
Figure BDA0003514538900000071
为t时刻无人车的状态变量，其中，x，y，z为三维坐标，roll，pitch，raw为欧拉角，
Figure BDA0003514538900000072
为线速度，
Figure BDA0003514538900000073
为角速度，
Figure BDA0003514538900000074
为线加速度；Zt为观测变量，其中激光雷达和GPS定位信息中位置和姿态的融合变量为二维坐标x，y以及欧拉角roll，pitch，raw，IMU位姿信息的融合变量为角速度
Figure BDA0003514538900000075
g和h为状态方程和观测方程的变换函数，本实施例中运动方程为短时间的均速直线运动模型，观测方程为单位阵乘以对应的观测变量。
3.2根据所述运动方程和观测方程计算状态变量的加权均值和方差的，再利用t-1时刻的所述均值μt-1和方差∑t-1对t-1时刻状态变量进行采样得到σ点，利用非线性变换得到t时刻状态变量并选择权重，结合测量误差计算得到预测的观测变量均值和方差；具体公式如下所示：
Figure BDA0003514538900000076
Figure BDA0003514538900000077
Figure BDA0003514538900000078
Figure BDA0003514538900000081
其中，μt-1为t-1时刻的均值，∑t-1为t-1时刻的方差，γ为比例因子，
Figure BDA0003514538900000082
为无噪声的预测的t时刻无人车的状态变量，
Figure BDA0003514538900000083
为预测的t时刻无人车的状态变量的加权均值，
Figure BDA0003514538900000084
为预测的t时刻无人车的状态变量的方差，
Figure BDA0003514538900000085
为选取的权值，Rt为测量误差。
3.3根据所述预测的观测变量均值和方差
Figure BDA0003514538900000086
采样得到σ点，并对得到的σ点进行非线性变换得到预测的观测变量，结合过程噪声计算预测观测变量均值和方差，如下所示：
Figure BDA0003514538900000087
Figure BDA0003514538900000088
Figure BDA0003514538900000089
Figure BDA00035145389000000810
其中，
Figure BDA00035145389000000811
为加入了预测阶段不确定性的新σ点，
Figure BDA00035145389000000812
为预测的观测变量，
Figure BDA00035145389000000813
为预测的观测变量的加权均值，
Figure BDA00035145389000000814
为所选取的权值，St为预测的观测变量的方差，Qt为过程噪声。
3.4利用所述预测的状态变量和观测变量以及所述加权均值和方差计算得到卡尔曼增益Kt，计算过程如下所示：
Figure BDA00035145389000000815
Figure BDA00035145389000000816
通过上一步骤得到的卡尔曼增益Kt得到当前时刻t的无人车平台的定位信息，利用卡尔曼增益Kt计算当前时刻t无人车平台的状态变量Xt、均值μt和方差∑t，进而得到当前时刻t无人车平台的定位信息，具体计算过程如下所示：
Figure BDA0003514538900000091
Figure BDA0003514538900000092
Figure BDA0003514538900000093
这一步骤通过对前两步骤得到的GPS、3D激光雷达的定位里程计信息和输入的IMU发布的位姿信息进行融合得到了鲁棒性高、实时性好且精确度高的融合定位里程计信息，利用了基于UKF的滤波融合方式进行多传感器融合定位，解决了在室内外切换环境和未知环境中，差分GPS易被遮挡，激光雷达定位算法实时性差，IMU位姿信息精度低等缺陷的问题。

Claims (8)

1.一种基于多传感器信息即时融合定位方法，其特征在于，包括：
步骤一、通过雷达利用NDT方法确定无人车平台的当前位置与姿态，并采用欧式距离残差的方法求解定位置信度；
步骤二、将GPS和组合惯导发布的原始定位信息与步骤一得到的当前位置与姿态进行匹配，采用基于滑动窗口和LO-RANSAC方法进行GPS定位信息的置信度估计，得到经过时间戳对齐的激光雷达和GPS定位信息配对点集；
步骤三、将步骤一得到的当前位置与姿态和定位置信度以及步骤二得到的激光雷达和GPS定位信息配对点集结合IMU位姿信息采用UKF方法进行融合，得到无人车平台实时高精度定位结果。
2.如权利要求1所述的一种基于多传感器信息即时融合定位方法，其特征在于，所述求解定位置信度具体步骤如下：
1.1通过下采样方式将当前雷达扫描得到的点云转换为稀疏的局部点云，并利用NDT方法将所述局部点云和对应的原始点云地图转换为二维栅格地图的概率分布；
1.2将上一时刻计算得到的姿态增量以及对IMU位姿数据进行预积分得到当前位姿的预测值，将所述概率分布根据所述预测值粗匹配至世界坐标系，得到粗略的平移旋转矩阵；
1.3构建优化目标匹配当前局部点云与输入的原始点云地图，利用梯度下降法最小化的优化方程，以所述粗略的旋转平移矩阵作为初始值，得到当前无人车位姿与上一时刻位姿间的旋转平移矩阵；
1.4根据所述旋转平移矩阵计算得到当前雷达扫描点与原始点云地图对应点之间的欧式距离残差和。
3.如权利要求1或2所述的一种基于多传感器信息即时融合定位方法，其特征在于，所述采用基于滑动窗口和LO-RANSAC方法进行GPS定位信息的置信度估计，具体步骤如下：
2.1通过初始化滑动窗口得到适当长度的滑动窗口和窗口内一组时间戳较为匹配初始的激光雷达和GPS定位信息点集，
2.2利用基于时间戳的方法匹配输入的激光雷达定位信息和GPS定位信息，同时选择前一次LO-RANSAC计算时间作为滑动窗口的前进速度；
2.3采用LO-RANSAC算法匹配所述激光雷达定位信息和GPS定位信息，估计GPS定位置信度。
4.如权利要求3所述的一种基于多传感器信息即时融合定位方法，其特征在于，所述采用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定位置信度。
5.如权利要求4所述的一种基于多传感器信息即时融合定位方法，其特征在于，所述判断所述模型好坏，采用以下方式：
若内点所占比例大于预设比例，则通过局部优化算法得到精确的内点数和模型，即将当前内点集作为输入点集，重复执行步骤2.3.1-2.3.3指定次数，同时更新运行次数上限的方法进行优化；若不满足上述条件，则重复步骤2.3.1-2.3.3。
6.如权利要求5所述的一种基于多传感器信息即时融合定位方法，其特征在于，所述运行次数上限k的更新规则如下所示：
Figure FDA0003514538890000031
其中，p为默认置信度，w为内点所占比例，m为输入的指定运行次数。
7.如权利要求1或2所述的一种基于多传感器信息即时融合定位方法，其特征在于，采用UKF进行融合得到无人车平台实时高精度定位结果，具体步骤如下：
3.1将步骤一得到的当前位置与姿态和定位置信度结合IMU位姿信息作为观测变量对无人车平台进行建模，得到运动方程和观测方程；
3.2根据所述运动方程和观测方程计算状态变量的加权均值和方差的，再利用t-1时刻的所述均值和方差对t-1时刻状态变量进行采样得到σ点，利用非线性变换得到t时刻状态变量并选择权重，结合测量误差计算得到预测的观测变量均值和方差。
3.3根据所述预测的观测变量均值和方差采样得到σ点，并对得到的σ点进行非线性变换得到预测的观测变量，结合过程噪声计算预测观测变量均值和方差；
3.4利用所述预测的状态变量和观测变量以及所述加权均值和方差计算得到卡尔曼增益。
8.如权利要求4所述的一种基于多传感器信息即时融合定位方法，其特征在于，所述预设阈值选择大于占总点集数的80％。
CN202210160719.6A 2022-02-22 2022-02-22 一种基于多传感器信息即时融合定位方法 Active CN114608568B (zh)

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 true CN114608568A (zh) 2022-06-10
CN114608568B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114755693A (zh) * 2022-06-15 2022-07-15 天津大学四川创新研究院 基于多旋翼无人机的基建设施测量系统和方法
CN116165958A (zh) * 2023-04-25 2023-05-26 舜泰汽车有限公司 一种两栖特种无人平台的自动驾驶系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及系统
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
US20200309529A1 (en) * 2019-03-29 2020-10-01 Trimble Inc. Slam assisted ins
CN112082545A (zh) * 2020-07-29 2020-12-15 武汉威图传视科技有限公司 一种基于imu和激光雷达的地图生成方法、装置及系统
CN112904317A (zh) * 2021-01-21 2021-06-04 湖南阿波罗智行科技有限公司 一种多激光雷达与gnss_ins系统标定方法
US20220018962A1 (en) * 2020-07-16 2022-01-20 Beijing Tusen Weilai Technology Co., Ltd. Positioning method and device based on multi-sensor fusion

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
US20200309529A1 (en) * 2019-03-29 2020-10-01 Trimble Inc. Slam assisted ins
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及系统
US20220018962A1 (en) * 2020-07-16 2022-01-20 Beijing Tusen Weilai Technology Co., Ltd. Positioning method and device based on multi-sensor fusion
CN112082545A (zh) * 2020-07-29 2020-12-15 武汉威图传视科技有限公司 一种基于imu和激光雷达的地图生成方法、装置及系统
CN112904317A (zh) * 2021-01-21 2021-06-04 湖南阿波罗智行科技有限公司 一种多激光雷达与gnss_ins系统标定方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
纪嘉文 等: "一种基于多传感融合的室内建图和定位算法", 成都信息工程大学学报, vol. 33, no. 4 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114755693A (zh) * 2022-06-15 2022-07-15 天津大学四川创新研究院 基于多旋翼无人机的基建设施测量系统和方法
CN114755693B (zh) * 2022-06-15 2022-09-16 天津大学四川创新研究院 基于多旋翼无人机的基建设施测量系统和方法
CN116165958A (zh) * 2023-04-25 2023-05-26 舜泰汽车有限公司 一种两栖特种无人平台的自动驾驶系统

Also Published As

Publication number Publication date
CN114608568B (zh) 2024-05-03

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
Adams et al. SLAM gets a PHD: New concepts in map estimation
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN105760811B (zh) 全局地图闭环匹配方法及装置
CN111288989B (zh) 一种小型无人机视觉定位方法
CN113252033B (zh) 基于多传感器融合的定位方法、定位系统及机器人
CN111060099B (zh) 一种无人驾驶汽车实时定位方法
Cai et al. Mobile robot localization using gps, imu and visual odometry
CN108592914A (zh) 无gps情况下的复杂区域巡视机器人定位、导航及授时方法
CN113776519B (zh) 一种无光动态开放环境下agv车辆建图与自主导航避障方法
CN103776453A (zh) 一种多模型水下航行器组合导航滤波方法
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN113739795B (zh) 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法
CN114035187B (zh) 一种自动驾驶系统的感知融合方法
CN110763239A (zh) 滤波组合激光slam建图方法及装置
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
Ye et al. Enhanced Kalman filter using noisy input Gaussian process regression for bridging GPS outages in a POS
CN114367990A (zh) 一种基于机理数据混合模型的机械臂触觉外力估计方法
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
Fernandes et al. Gnss/mems-ins integration for drone navigation using ekf on lie groups
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集系统及方法
CN115900708A (zh) 基于gps引导式粒子滤波的机器人多传感器融合定位方法
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法

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