CN112444246B - 高精度的数字孪生场景中的激光融合定位方法 - Google Patents
高精度的数字孪生场景中的激光融合定位方法 Download PDFInfo
- Publication number
- CN112444246B CN112444246B CN202011231670.6A CN202011231670A CN112444246B CN 112444246 B CN112444246 B CN 112444246B CN 202011231670 A CN202011231670 A CN 202011231670A CN 112444246 B CN112444246 B CN 112444246B
- Authority
- CN
- China
- Prior art keywords
- covariance
- pose
- relative
- current moment
- absolute
- 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 45
- 230000004927 fusion Effects 0.000 title claims abstract description 19
- 238000004088 simulation Methods 0.000 claims abstract description 21
- 238000004364 calculation method Methods 0.000 claims abstract description 18
- 238000004422 calculation algorithm Methods 0.000 claims description 27
- 238000005457 optimization Methods 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000010354 integration Effects 0.000 claims description 7
- 238000013519 translation Methods 0.000 claims description 7
- 238000012850 discrimination method Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 description 7
- 230000009466 transformation Effects 0.000 description 7
- 238000005315 distribution function Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 239000013598 vector Substances 0.000 description 3
- 238000005259 measurement Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000003153 chemical reaction reagent Substances 0.000 description 1
- 238000007796 conventional method Methods 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
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000001131 transforming effect Effects 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
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- 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)
- Theoretical Computer Science (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Data Mining & Analysis (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Biology (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开了一种高精度的数字孪生场景中的激光融合定位方法,其包括:利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算,得到当前时刻的观测绝对位姿及其协方差;获取上一时刻的实际绝对位姿及其协方差;根据用于相对定位的传感器数据计算得到当前时刻相对于上一时刻的相对位姿及其协方差;基于上一时刻的实际绝对位姿及其协方差,结合当前时刻相对于上一时刻的相对位姿及其协方差,计算得到当前时刻的预测绝对位姿及其协方差;将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差融合,得到当前时刻的实际绝对位姿及其协方差。本发明实现了定位方法的高精度,高鲁棒性,高可扩展性以及高时效性等特点。
Description
技术领域
本发明涉及移动机器人定位技术领域。更具体地说,本发明涉及一种高精度的数字孪生场景中的激光融合定位方法。
背景技术
随着计算机技术和人工智能的发展,智能自主移动机器人成为机器人领域的一个重要研究方向和研究热点。移动机器人的定位是自主移动机器人的眼睛,是该领域的热点研究问题,也是决定机器人是否自主移动的关键问题之一。自主移动机器人导航过程需要回答3个问题:“我在哪里?”,“我要去哪里?”和“我怎样到达那里?”。移动机器人定位技术就是要解决第1个问题。准确来说,移动机器人定位的目的就是确定机器人在其运动环境中的世界地图的坐标,即机器人通过感知获取环境信息,经过相关的信息处理而确定自身位置的过程。而现有的机器人定位过程,可分为相对定位和绝对定位:相对定位也叫作位姿跟踪,假定机器人初始位姿,采用相邻时刻传感器信息对机器人位置进行跟踪估计,然而相对定位的计算过程中会有误差累积导致定位精度差;绝对定位又称为全局定位,完成机器人的全局定位需要预先确定好环境模型,通过外部传感器直接向机器人提供外界位置信息,计算机器人在全局坐标系中的位置,而绝对定位过程中需要传感器采集外界信息,这个过程中容易受到外界光线干扰,也容易导致定位不准确。
发明内容
本发明的一个目的是解决至少上述问题,并提供至少后面将说明的优点。
本发明还有一个目的是提供一种高精度的数字孪生场景中的激光融合定位方法,通过融合相对定位与绝对定位技术,采用松耦合及非线性优化方法,实现定位方法的高精度,高鲁棒性,高可扩展性以及高时效性等特点。
为了实现根据本发明的这些目的和其它优点,提供了一种高精度的数字孪生场景中的激光融合定位方法,包括:
获取用于绝对定位的传感器数据,利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算,得到当前时刻的观测绝对位姿及其协方差;
获取上一时刻的实际绝对位姿及其协方差;
获取用于相对定位的传感器数据,根据用于相对定位的传感器数据通过预积分计算得到当前时刻相对于上一时刻的相对位姿及其协方差;
基于上一时刻的实际绝对位姿及其协方差,结合当前时刻相对于上一时刻的相对位姿及其协方差,通过预积分计算得到当前时刻的预测绝对位姿及其协方差;
将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合,得到当前时刻的实际绝对位姿及其协方差;
对比当前时刻的实际绝对位姿的协方差和第一预设阈值,根据第一预设条件判断是否取信当前时刻的实际绝对位姿。
优选的是,用于相对定位的传感器至少有两种,每种用于相对定位的传感器数据单独计算当前时刻相对于上一时刻的预测相对位姿及其协方差,再采用图优化的方法将所有用于相对定位的传感器数据得到的当前时刻相对于上一时刻的预测相对位姿进行融合、当前时刻相对于上一时刻的预测相对位姿的协方差进行融合。
优选的是,将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合前,先对比当前时刻的预测绝对位姿的协方差和第二预设阈值,根据第二预设条件判断是否取信当前时刻的预测绝对位姿,若当前时刻的预测绝对位姿不可信,则将当前时刻的观测绝对位姿及其协方差直接作为当前时刻的实际绝对位姿及其协方差。
优选的是,用于相对定位的传感器包括里程计和轮速计。
优选的是,根据用于相对定位的传感器数据计算当前时刻相对于上一时刻的相对位姿及其协方差前,先通过马氏距离判别法排除不可信的用于相对定位的传感器数据。
优选的是,根据用于绝对定位的传感器数据计算当前时刻的观测绝对位姿及其协方差前,先通过马氏距离判别法排除不可信的用于绝对定位的传感器数据。
优选的是,用于绝对定位的传感器包括激光雷达和GNSS传感器。
优选的是,利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算,得到当前时刻的观测绝对位姿及其协方差的方法包括:
通过GNSS传感器获取当前时刻的位置信息,通过遍历姿态信息获取每个姿态信息下数字孪生场景仿真地图的图像信息;
通过激光雷达获取真实场景的图像信息;
计算每个姿态信息下数字孪生场景仿真地图的图像信息与真实场景的图像信息的匹配得分;
当某一姿态信息下数字孪生场景仿真地图的图像信息与真实场景的图像信息的匹配得分符合第三预设条件时,将该姿态信息和当前时刻的位置信息作为当前时刻的观测绝对位姿,根据该姿态信息下的匹配得分计算当前时刻的观测绝对位姿的协方差。
优选的是,当数字孪生场景仿真地图为点云地图时,计算每个姿态信息下点云地图的图像信息与真实场景的图像信息的匹配得分的算法为ICP算法或者NDT算法;
当数字孪生场景仿真地图为栅格地图时,计算每个姿态信息下栅格地图的图像信息与真实场景的图像信息的匹配得分的算法为高斯牛顿算法或者LM算法。
本发明至少包括以下有益效果:可以根据用户实际情况设置多种不同的传感器进行融合,可扩展性强,并且不同传感器可信度不同,因此,可以配置相应的测量协方差来达到不同的测量需求,同时,各个阶段都会对传感器异常值进行检查和排除,这保证了定位的高鲁棒性,而相对定位融合绝对定位可以消除相对定位带来的累积误差,通过融合不同传感器,得到高精度的定位数据。
本发明的其它优点、目标和特征将部分通过下面的说明体现,部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。
附图说明
图1为本发明所述激光融合定位方法的流程图。
具体实施方式
下面结合附图对本发明做进一步的详细说明,以令本领域技术人员参照说明书文字能够据以实施。
需要说明的是,下述实施方案中所述实验方法,如无特殊说明,均为常规方法,所述试剂和材料,如无特殊说明,均可从商业途径获得;在本发明的描述中,术语“横向”、“纵向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,并不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
如图1所示,本发明提供一种高精度的数字孪生场景中的激光融合定位方法,包括:
S101、获取用于绝对定位的传感器数据,利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算,得到当前时刻的观测绝对位姿及其协方差;
这里用于绝对定位的传感器可以采用激光雷达和GNSS传感器。
通过GNSS传感器获取当前时刻的位置信息,通过遍历姿态信息获取每个姿态信息下数字孪生场景仿真地图的图像信息,这里位置信息即移动机器人的位置坐标,姿态信息即移动机器人的姿态角,由于姿态角在360°范围内取值,可以等差数列的形式试算一系列的姿态角,如30°、60°、90°……360°逐一尝试;
这里获取当前时刻的位置信息还可以采取其他方式,如采用人工输入位置坐标的方式或者直接载入移动机器人最后一次保存的位置坐标的方式。
通过激光雷达获取真实场景的图像信息;
通过马氏距离判别法排除不可信的GNSS传感器数据和真实场景的图像信息;
计算每个姿态信息下数字孪生场景仿真地图的图像信息与真实场景的图像信息的匹配得分;
这里当数字孪生场景仿真地图为点云地图时,计算每个姿态信息下点云地图的图像信息与真实场景的图像信息的匹配得分的算法为ICP算法或者NDT算法;
ICP算法能够使不同的坐标下的点云数据合并到同一个坐标***中,首先是找到一个可用的变换,配准操作实际是要找到从坐标系1到坐标系2的一个刚性变换。ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对,计算最优刚体变换,直到满足正确配准的收敛精度要求。ICP算法的目的是要找到待配准点云数据与参考云数据之间的旋转参数和平移参数,使得两点数据之间满足某种度量准则下的最优匹配。假设给两个三维点集X1和X2,ICP方法的配准步骤如下:第一步,计算X2中的每一个点在X1点集中的对应近点;第二步,求得使上述对应点对平均距离最小的刚体变换,求得平移参数和旋转参数;第三步,对X2使用上一步求得的平移和旋转参数,得到新的变换点集;第四步,如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值,则停止迭代计算,否则新的变换点集作为新的X2继续迭代,直到达到目标函数的要求。
NDT算法是一种很有用途的点云配准方法,是一个一次性初始化工作,不需要消耗大量的代价计算最近邻搜索匹配点,并且概率密度函数在两幅图像采集之间的额时间可以离线计算出来,但仍在存在的问题很多,包括收敛域差、NDT代价函数的不连续性以及稀疏室外环境下不可靠的姿态估计等。NDT算法的步骤(1)该算法的第一步是将扫描占用的空间细分为单元格网格(2D图像中的正方形或3D中的立方体),基于单元内的点分布计算每个单元的概率分布函数。每个单元格中的概率分布函数可以解释为单元格内表面点的生成过程。将点云投票到各个格子中,计算每个格子的概率分布函数,概率分布函数可以当做表面的近似表达,协方差矩阵的特征向量和特征值可以表达表面信息(朝向、平整度)格子内少于3个点,经常会协方差矩阵不存在逆矩阵,所以只计算点数大于5的单元格,涉及到下采样方法。正态分布给出了点云的分段平滑表示,具有连续导数。每个概率分布函数都可以看作是局部表面的近似值,描述了表面的位置以及它的方向和平滑度。在多维的情况下,平均值和方差由平均向量μ和协方差来描述矩阵Σ。协方差矩阵的对角元素表示方差每个变量,非对角线元素表示的是协方差变量。(2)将第二幅扫描点云的每个点按转移矩阵的变换。(3)第二幅扫描点落于参考帧点云的哪个格子,计算响应的概率分布函数。(4)当使用NDT进行扫描点配准时,目标是找到当前扫描点的位姿,以最大化当前扫描的点位于参考扫描表面上的可能性,该位姿是要优化的参数,也就是说,当前扫描的点云估计的旋转和平移向量。最后求所有点的最优值。
当数字孪生场景仿真地图为栅格地图时,计算每个姿态信息下栅格地图的图像信息与真实场景的图像信息的匹配得分的算法为高斯牛顿算法或者LM算法。
高斯牛顿迭代法是非线性回归模型中求回归参数进行最小二乘的一种迭代方法,该法使用泰勒级数展开式去近似地代替非线性回归模型,然后通过多次迭代,多次修正回归系数,使回归系数不断逼近非线性回归模型的最佳回归系数,最后使原模型的残差平方和达到最小。
LM算法是介于牛顿法与梯度下降法之间的一种非线性优化方法,对于过参数化问题不敏感,能有效处理冗余参数问题,使代价函数陷入局部极小值的机会大大减小,这些特性使得LM算法在计算机视觉等领域得到广泛应用。
当某一姿态信息下数字孪生场景仿真地图的图像信息与真实场景的图像信息的匹配得分符合第三预设条件时,将该姿态信息和当前时刻的位置信息作为当前时刻的观测绝对位姿,根据该姿态信息下的匹配得分计算当前时刻的观测绝对位姿的协方差。
这里第三预设条件由用户自定义设置,上述每种配准算法的匹配得分及其协方差的计算方式在现有技术中已经公开,故不再赘述。
S102、获取上一时刻的实际绝对位姿及其协方差;
这里移动机器人每更新一次实际绝对位姿及其协方差,均将实际绝对位姿及其协方差数据保存于存储设备中,故可直接获取上一时刻的实际绝对位姿及其协方差。
S103、获取用于相对定位的传感器数据,根据用于相对定位的传感器数据通过预积分计算得到当前时刻相对于上一时刻的相对位姿及其协方差;
这里用于相对定位的传感器可以采用里程计和轮速计。
通过马氏距离判别法排除不可信的里程计传感器数据和轮速计传感器数据。
根据里程计传感器数据,计算当前时刻相对于上一时刻的相对位姿,公式如下:
k-1Pk=(oPk-1)-1·oPk
其中k-1Pk为当前时刻相对于上一时刻的相对位姿,oPk-1为里程计上一时刻相对于初始时刻的相对位姿,oPk为里程计当前时刻相对于初始时刻的相对位姿,协方差的计算则由如下公式计算得到:
首先初始化得到(R,p)=(I,0),然后迭代计算以下公式:
(dR,dp)=(oPk-1,m-1)-1oPk-1,m
Cm=F·Cm-1·FT+G·Q·GT
(R,p)=(R·dR,p+R·dp)
其中R为相对位姿,p为相对平移,C为相对位姿的协方差,Q为里程计传感器数据的协方差。
根据轮速计传感器数据,预积分计算得到相对位姿及其协方差,迭代公式如下:
ω=0.5·(ωm-1+ωm)
dR=exp(ω·dt)
v=0.5·(vm-1+dR·vm)
dp=v·dt
Rn=R·dR
pn=p+R·dp
Cm=F·Cm-1·FT+G·Q·GT+H·Q·HT
(R,p)=(Rn,pn)
其中R为相对位姿,p为相对平移,C为相对位姿的协方差,ω为角速度,v为线速度,dt为相对时间,Q为轮速计传感器数据的协方差。
采用图优化的方法将通过里程计传感器数据得到的当前时刻相对于上一时刻的预测相对位姿和通过轮速计传感器数据得到的当前时刻相对于上一时刻的预测相对位姿进行融合,将通过里程计传感器数据得到的当前时刻相对于上一时刻的预测相对位姿协方差和通过轮速计传感器数据得到的当前时刻相对于上一时刻的预测相对位姿协方差进行融合。
S104、基于上一时刻的实际绝对位姿及其协方差,结合当前时刻相对于上一时刻的相对位姿及其协方差,通过预积分计算得到当前时刻的预测绝对位姿及其协方差,计算公式如下:
wRk=wRk-1·k-1Rk
Ck=F·Ck-1·FT+G·Crel·GT
Mk=Ck -1
其中wRk为当前时刻的预测绝对位姿,wRk-1为上一时刻的实际绝对位姿,k-1Rk为当前时刻相对于上一时刻的相对位姿,Ck为当前时刻的预测绝对位姿的协方差,Ck-1为当前时刻相对于上一时刻的相对位姿的协方差,Crel为上一时刻的实际绝对位姿的协方差,Mk为当前时刻的信息矩阵。
S105、将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合,得到当前时刻的实际绝对位姿及其协方差;
这里需要注意的是由于GNSS传感器无法获取姿态信息,因此其对应的信息权重应为0。
S106、对比当前时刻的实际绝对位姿的协方差和第一预设阈值,根据第一预设条件判断是否取信当前时刻的实际绝对位姿。
这里第一预设阈值和第一预设条件可用户自定义设置,如设置为若当前时刻的实际绝对位姿的协方差小于第一预设阈值,则取信当前时刻的实际绝对位姿,若当前时刻的实际绝对位姿的协方差大于第一预设阈值,则不取信当前时刻的实际绝对位姿。
这里当出现不取信当前时刻的实际绝对位姿,则通过相对定位的传感器和绝对定位的传感器重新采集数据,重复上述激光融合定位方法,重新计算实际绝对位姿。
在另一实施例中,将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合前,先对比当前时刻的预测绝对位姿的协方差和第二预设阈值,根据第二预设条件判断是否取信当前时刻的预测绝对位姿,若当前时刻的预测绝对位姿不可信,则将当前时刻的观测绝对位姿及其协方差直接作为当前时刻的实际绝对位姿及其协方差。
这里第二预设阈值和第二预设条件可用户自定义设置,如设置为若当前时刻的预测绝对位姿的协方差小于第二预设阈值,则取信当前时刻的预测绝对位姿,若当前时刻的预测绝对位姿的协方差大于第二预设阈值,则不取信当前时刻的预测绝对位姿。
这里当出现取信当前时刻的预测绝对位姿,则继续将当前时刻的观测绝对位姿和预测绝对位姿进行融合,否则当前时刻的观测绝对位姿及其协方差直接作为当前时刻的实际绝对位姿及其协方差。
尽管本发明的实施方案已公开如上,但其并不仅仅限于说明书和实施方式中所列运用,它完全可以被适用于各种适合本发明的领域,对于熟悉本领域的人员而言,可容易地实现另外的修改,因此在不背离权利要求及等同范围所限定的一般概念下,本发明并不限于特定的细节和这里示出与描述的图例。
Claims (6)
1.高精度的数字孪生场景中的激光融合定位方法,其特征在于,包括:
获取用于绝对定位的传感器数据,利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算,得到当前时刻的观测绝对位姿及其协方差;
获取上一时刻的实际绝对位姿及其协方差;
获取用于相对定位的传感器数据,根据用于相对定位的传感器数据通过预积分计算得到当前时刻相对于上一时刻的相对位姿及其协方差;
基于上一时刻的实际绝对位姿及其协方差,结合当前时刻相对于上一时刻的相对位姿及其协方差,通过预积分计算得到当前时刻的预测绝对位姿及其协方差;
将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合,得到当前时刻的实际绝对位姿及其协方差;
对比当前时刻的实际绝对位姿的协方差和第一预设阈值,根据第一预设条件判断是否取信当前时刻的实际绝对位姿;
其中,用于相对定位的传感器包括里程计和轮速计,每种用于相对定位的传感器数据单独计算当前时刻相对于上一时刻的预测相对位姿及其协方差,再采用图优化的方法将所有用于相对定位的传感器数据得到的当前时刻相对于上一时刻的预测相对位姿进行融合、当前时刻相对于上一时刻的预测相对位姿的协方差进行融合;
根据里程计传感器数据,计算当前时刻相对于上一时刻的相对位姿,公式如下:
k-1Pk=(oPk-1)-1·oPk
其中k-1Pk为当前时刻相对于上一时刻的相对位姿,oPk-1为里程计上一时刻相对于初始时刻的相对位姿,oPk为里程计当前时刻相对于初始时刻的相对位姿,协方差的计算则由如下公式计算得到:
首先初始化得到(R,p)=(I,0),然后迭代计算以下公式:
(dR,dp)=(oPk-1,m-1)-1oPk-1,m
Cm=F·Cm-1·FT+G·Q·GT
(R,p)=(R·dR,p+R·dp)
其中R为相对位姿,p为相对平移,C为相对位姿的协方差,Q为里程计传感器数据的协方差;
根据轮速计传感器数据,预积分计算得到相对位姿及其协方差,迭代公式如下:
ω=0.5·(ωm-1+ωm)
dR=exp(ω·dt)
v=0.5·(vm-1+dR·vm)
dp=v·dt
Rn=R·dR
pn=p+R·dp
Cm=F·Cm-1·FT+G·Q·GT+H·Q·HT
(R,p)=(Rn,pn)
其中R为相对位姿,p为相对平移,C为相对位姿的协方差,ω为角速度,v为线速度,dt为相对时间,Q为轮速计传感器数据的协方差;
基于上一时刻的实际绝对位姿及其协方差,结合当前时刻相对于上一时刻的相对位姿及其协方差,通过预积分计算得到当前时刻的预测绝对位姿及其协方差,计算公式如下:
wRk=wRk-1·k-1Rk
Ck=F·Ck-1·FT+G·Crel·GT
Mk=Ck -1
其中wRk为当前时刻的预测绝对位姿,wRk-1为上一时刻的实际绝对位姿,k-1Rk为当前时刻相对于上一时刻的相对位姿,Ck为当前时刻的预测绝对位姿的协方差,Ck-1为当前时刻相对于上一时刻的相对位姿的协方差,Crel为上一时刻的实际绝对位姿的协方差,Mk为当前时刻的信息矩阵;
将当前时刻的观测绝对位姿及其协方差和预测绝对位姿及其协方差通过非线性优化的方式融合前,先对比当前时刻的预测绝对位姿的协方差和第二预设阈值,根据第二预设条件判断是否取信当前时刻的预测绝对位姿,若当前时刻的预测绝对位姿不可信,则将当前时刻的观测绝对位姿及其协方差直接作为当前时刻的实际绝对位姿及其协方差。
2.如权利要求1所述的高精度的数字孪生场景中的激光融合定位方法,其特征在于,根据用于相对定位的传感器数据计算当前时刻相对于上一时刻的相对位姿及其协方差前,先通过马氏距离判别法排除不可信的用于相对定位的传感器数据。
3.如权利要求1所述的高精度的数字孪生场景中的激光融合定位方法,其特征在于,根据用于绝对定位的传感器数据计算当前时刻的观测绝对位姿及其协方差前,先通过马氏距离判别法排除不可信的用于绝对定位的传感器数据。
4.如权利要求1所述的高精度的数字孪生场景中的激光融合定位方法,其特征在于,用于绝对定位的传感器包括激光雷达和GNSS传感器。
5.如权利要求4所述的高精度的数字孪生场景中的激光融合定位方法,其特征在于,利用绝对定位的传感器数据与预存的数字孪生场景仿真地图进行匹配计算,得到当前时刻的观测绝对位姿及其协方差的方法包括:
通过GNSS传感器获取当前时刻的位置信息,通过遍历姿态信息获取每个姿态信息下数字孪生场景仿真地图的图像信息;
通过激光雷达获取真实场景的图像信息;
计算每个姿态信息下数字孪生场景仿真地图的图像信息与真实场景的图像信息的匹配得分;
当某一姿态信息下数字孪生场景仿真地图的图像信息与真实场景的图像信息的匹配得分符合第三预设条件时,将该姿态信息和当前时刻的位置信息作为当前时刻的观测绝对位姿,根据该姿态信息下的匹配得分计算当前时刻的观测绝对位姿的协方差。
6.如权利要求5所述的高精度的数字孪生场景中的激光融合定位方法,其特征在于,当数字孪生场景仿真地图为点云地图时,计算每个姿态信息下点云地图的图像信息与真实场景的图像信息的匹配得分的算法为ICP算法或者NDT算法;
当数字孪生场景仿真地图为栅格地图时,计算每个姿态信息下栅格地图的图像信息与真实场景的图像信息的匹配得分的算法为高斯牛顿算法或者LM算法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011231670.6A CN112444246B (zh) | 2020-11-06 | 2020-11-06 | 高精度的数字孪生场景中的激光融合定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011231670.6A CN112444246B (zh) | 2020-11-06 | 2020-11-06 | 高精度的数字孪生场景中的激光融合定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112444246A CN112444246A (zh) | 2021-03-05 |
CN112444246B true CN112444246B (zh) | 2024-01-26 |
Family
ID=74736522
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011231670.6A Active CN112444246B (zh) | 2020-11-06 | 2020-11-06 | 高精度的数字孪生场景中的激光融合定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112444246B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113252023A (zh) * | 2021-04-26 | 2021-08-13 | 深圳市优必选科技股份有限公司 | 基于里程计的定位方法、装置及设备 |
CN114373111A (zh) * | 2021-12-29 | 2022-04-19 | 北京博能科技股份有限公司 | 一种基于孪生数据驱动的融合方法及引擎*** |
CN114396944B (zh) * | 2022-01-18 | 2024-03-22 | 西安塔力科技有限公司 | 一种基于数字孪生的自主定位误差矫正方法 |
CN115855082B (zh) * | 2022-12-07 | 2023-10-20 | 北京理工大学 | 一种基于点云先验地图的双模式快速重定位方法 |
CN117346768B (zh) * | 2023-11-03 | 2024-04-19 | 昆明理工大学 | 同时适用于室内外的多传感器融合感知定位方法 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107704821A (zh) * | 2017-09-29 | 2018-02-16 | 河北工业大学 | 一种弯道的车辆位姿计算方法 |
CN107909614A (zh) * | 2017-11-13 | 2018-04-13 | 中国矿业大学 | 一种gps失效环境下巡检机器人定位方法 |
CN109186601A (zh) * | 2018-07-05 | 2019-01-11 | 南京理工大学 | 一种基于自适应无迹卡尔曼滤波的激光slam算法 |
WO2019057173A1 (zh) * | 2017-09-22 | 2019-03-28 | 华为技术有限公司 | 传感器数据处理的方法及装置 |
CN110490900A (zh) * | 2019-07-12 | 2019-11-22 | 中国科学技术大学 | 动态环境下的双目视觉定位方法及*** |
CN110646825A (zh) * | 2019-10-22 | 2020-01-03 | 北京新能源汽车技术创新中心有限公司 | 定位方法、定位***及汽车 |
CN110702107A (zh) * | 2019-10-22 | 2020-01-17 | 北京维盛泰科科技有限公司 | 一种单目视觉惯性组合的定位导航方法 |
CN110954112A (zh) * | 2019-03-29 | 2020-04-03 | 北京初速度科技有限公司 | 一种导航地图与感知图像匹配关系的更新方法和装置 |
CN111136660A (zh) * | 2020-02-19 | 2020-05-12 | 清华大学深圳国际研究生院 | 机器人位姿定位方法及*** |
CN111489393A (zh) * | 2019-01-28 | 2020-08-04 | 速感科技(北京)有限公司 | Vslam方法、控制器和可移动设备 |
CN111882612A (zh) * | 2020-07-21 | 2020-11-03 | 武汉理工大学 | 一种基于三维激光检测车道线的车辆多尺度定位方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10317214B2 (en) * | 2016-10-25 | 2019-06-11 | Massachusetts Institute Of Technology | Inertial odometry with retroactive sensor calibration |
CN108732603B (zh) * | 2017-04-17 | 2020-07-10 | 百度在线网络技术(北京)有限公司 | 用于定位车辆的方法和装置 |
-
2020
- 2020-11-06 CN CN202011231670.6A patent/CN112444246B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019057173A1 (zh) * | 2017-09-22 | 2019-03-28 | 华为技术有限公司 | 传感器数据处理的方法及装置 |
CN107704821A (zh) * | 2017-09-29 | 2018-02-16 | 河北工业大学 | 一种弯道的车辆位姿计算方法 |
CN107909614A (zh) * | 2017-11-13 | 2018-04-13 | 中国矿业大学 | 一种gps失效环境下巡检机器人定位方法 |
CN109186601A (zh) * | 2018-07-05 | 2019-01-11 | 南京理工大学 | 一种基于自适应无迹卡尔曼滤波的激光slam算法 |
CN111489393A (zh) * | 2019-01-28 | 2020-08-04 | 速感科技(北京)有限公司 | Vslam方法、控制器和可移动设备 |
CN110954112A (zh) * | 2019-03-29 | 2020-04-03 | 北京初速度科技有限公司 | 一种导航地图与感知图像匹配关系的更新方法和装置 |
CN110490900A (zh) * | 2019-07-12 | 2019-11-22 | 中国科学技术大学 | 动态环境下的双目视觉定位方法及*** |
CN110646825A (zh) * | 2019-10-22 | 2020-01-03 | 北京新能源汽车技术创新中心有限公司 | 定位方法、定位***及汽车 |
CN110702107A (zh) * | 2019-10-22 | 2020-01-17 | 北京维盛泰科科技有限公司 | 一种单目视觉惯性组合的定位导航方法 |
CN111136660A (zh) * | 2020-02-19 | 2020-05-12 | 清华大学深圳国际研究生院 | 机器人位姿定位方法及*** |
CN111882612A (zh) * | 2020-07-21 | 2020-11-03 | 武汉理工大学 | 一种基于三维激光检测车道线的车辆多尺度定位方法 |
Non-Patent Citations (2)
Title |
---|
Visual-Inertial Monocular SLAM With Map Reuse;Raúl Mur-Artal等;《 IEEE Robotics and Automation Letters 》;全文 * |
视觉/惯性组合导航技术发展综述;张礼廉;屈豪;毛军;胡小平;;导航定位与授时(第04期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN112444246A (zh) | 2021-03-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112444246B (zh) | 高精度的数字孪生场景中的激光融合定位方法 | |
CN111429574B (zh) | 基于三维点云和视觉融合的移动机器人定位方法和*** | |
CN109211251B (zh) | 一种基于激光和二维码融合的即时定位与地图构建方法 | |
Borrmann et al. | Globally consistent 3D mapping with scan matching | |
Olson | Probabilistic self-localization for mobile robots | |
Nieto et al. | Recursive scan-matching SLAM | |
US9071829B2 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
CN108615246B (zh) | 提高视觉里程计***鲁棒性和降低算法计算消耗的方法 | |
CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、***、装置及存储介质 | |
JP7131994B2 (ja) | 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、学習装置、学習方法及び学習プログラム | |
CN110717927A (zh) | 基于深度学习和视惯融合的室内机器人运动估计方法 | |
Jessup et al. | Robust and efficient multirobot 3-d mapping merging with octree-based occupancy grids | |
CN108332752B (zh) | 机器人室内定位的方法及装置 | |
CN114494629A (zh) | 一种三维地图的构建方法、装置、设备及存储介质 | |
CN111474932B (zh) | 一种集成情景经验的移动机器人建图与导航方法 | |
Chiu et al. | Precise vision-aided aerial navigation | |
CN114047766B (zh) | 面向室内外场景长期应用的移动机器人数据采集***及方法 | |
Garcia et al. | Portable multi-hypothesis Monte Carlo localization for mobile robots | |
D’Adamo et al. | Registration of three‐dimensional scanning LiDAR sensors: An evaluation of model‐based and model‐free methods | |
CN117451032A (zh) | 一种低算力与松耦合的激光雷达和imu的slam方法及*** | |
CN117308982A (zh) | 一种核电站水下清洁机器人的定位方法及装置 | |
Padial et al. | Tumbling target reconstruction and pose estimation through fusion of monocular vision and sparse-pattern range data | |
CN113379915B (zh) | 一种基于点云融合的行车场景构建方法 | |
Kelly et al. | Simultaneous mapping and stereo extrinsic parameter calibration using GPS measurements | |
Arturo et al. | Cooperative simultaneous localisation and mapping using independent rao–blackwellised filters |
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 |