CN113379915A - 一种基于点云融合的行车场景构建方法 - Google Patents

一种基于点云融合的行车场景构建方法 Download PDF

Info

Publication number
CN113379915A
CN113379915A CN202110755584.3A CN202110755584A CN113379915A CN 113379915 A CN113379915 A CN 113379915A CN 202110755584 A CN202110755584 A CN 202110755584A CN 113379915 A CN113379915 A CN 113379915A
Authority
CN
China
Prior art keywords
point cloud
cloud data
data
map
scene
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
CN202110755584.3A
Other languages
English (en)
Other versions
CN113379915B (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.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202110755584.3A priority Critical patent/CN113379915B/zh
Publication of CN113379915A publication Critical patent/CN113379915A/zh
Application granted granted Critical
Publication of CN113379915B publication Critical patent/CN113379915B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05Geographic models
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00Geometric image transformations in the plane of the image
    • G06T3/40Scaling of whole images or parts thereof, e.g. expanding or contracting
    • G06T3/4007Scaling of whole images or parts thereof, e.g. expanding or contracting based on interpolation, e.g. bilinear interpolation

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于点云融合的行车场景构建方法,该方法主要采用同步定位与地图构建技术地图构建方案;首先,对点云数据做畸变补偿和传感器时间对齐后再将数据发布,进行前端里程估计;前端里程估计节点接受点云数据后将对其进行点云匹配并生成初步的激光里程计估计值;接着再对估计值进行闭环检测,若检测形成闭环则将信息传递至后端进行后端算法优化;后端算法优化得到预估值节点的位姿信息和路标构成图的顶点集,并将生成的优化后激光里程计数据发布展示。本方法在保证建图实时性的同时极大提高建图的精度,可以快速实现场景中的定位和高精度地图的构建,克服了传统的纯激光里程计建图误差大、实时性不强的缺陷。

Description

一种基于点云融合的行车场景构建方法
技术领域
本发明涉及地图实时构建领域,具体涉及一种基于点云融合的行车场景构建方法。
背景技术
近年来,随着汽车保有量的不断增长,许多城市的道路承载能力已达到满负荷。交通安全、出行效率、节能减排日益突出,推动车辆的智能化和网络化通常将是解决上述交通问题的重要解决方案。在车辆智能化与网络化的发展进程中,行车场景的三维地图重构是关键技术问题之一。
随着时代的进步,同步定位与地图构建技术使用的传感器正不断迭代发展,将激光雷达与惯性测量单元等传感器的融合的方法更受欢迎。同步定位与地图构建技术从一开始基于卡尔曼滤波器或者贝叶斯滤波器的方法向基于图优化的方法转变。该方法相比GPS定位精度具有更高精度,解决具体车道线场景细节难以准确定位的问题,同时支持隧道、山洞等特殊场景的定位与地图构建。
现有技术中的一些非线性优化方法进行地图构建,通过非线性最小二乘法来减少构图过程中的累积误差,但忽略了***的稀疏性,且其使用的离线建图方式在行车场景构建中不能满足实时性的要求。
另外的一些现有技术中,通过在前端估计的数据关联环节引入高斯-牛顿迭代法的方式,同时采用激光雷达扫描数据直接与地图进行匹配的方法完成点云帧之间的配对,但该方法缺少后端优化和闭环检测,对配准初始值比较敏感,导致整体精度不高。
发明内容
本发明的目的是提供一种基于点云融合的行车场景构建方法,用于解决传统地图构建方法算力不足、单设备重复构建的问题。
为了实现上述任务,本发明采用以下技术方案:
一种基于点云融合的行车场景构建方法,包括以下步骤:
步骤1,对车载激光雷达在汽车运动过程中扫描收集的点云数据进行畸变补偿;
步骤2,将畸变补偿后的点云数据进行传感器时间对齐,使点云数据在进行畸变补偿的同时,对同一时间下的里程计数据进行线性插值计算,使里程计数据向点云数据对齐时间,得到实现传感器对齐的机器人的位置姿态作为前端里程估计的初始预测位置姿态;
步骤3,创建一个任务队列用来存放点云数据,并生成一个局部地图;任务队列中每***一帧点云数据时,输入该点云数据对应的机器人的初始预测位置姿态,与局部地图进行点云匹配,对点云数据进行正态分布变换,得到目标点云的所有单位体素,代入高斯概率模型,寻找到局部地图上与初始预测位置姿态最近似的点,得到前端里程估计的位姿预估值;
步骤4,采用基于点云的场景识别方式,将前端里程估计创建的任务队列中的点云数据用scan Context特征图进行描述;定义scan Context特征图之间的距离计算公式;从scan Context特征图中提取描述符环,将其表示为一个多维向量以进行KD树的构建,获得当前场景的环的最相似区域,从中选取和当前场景的环最相似的历史环对应的scancontext作为候选场景的scan context;计算任务队列中的点云数据的scan Context和候选场景的scan context之间的距离,满足阈值的候选场景则被认为是闭环,则将所述位姿预估值进行后端优化;
步骤5,将机器人不同时刻通过前端里程估计得到的位姿预估值抽象定义为节点,机器人在不同位置上的观测所产生的约束被抽象为节点之间的边,节点的位姿信息和路标构成图的顶点集,用边来表示观测数据的约束;
定义误差函数为两个节点之间的虚拟测量值的均值与期望的虚拟测量值的差值;通过高斯牛顿法对误差函数进行迭代求最优解,将求得的最优解叠加到初始预测位置姿态得到新的位姿预估值,将新的位姿预估值作为下一轮迭代的初始预测位置姿态,继续迭代直至位姿预估值误差最小;
步骤1-步骤5在边缘设备中进行,构建出局部地图后再上传到云服务器进行全局地图拼接,最后再将拼接好的地图回传到边缘设备供车辆进行定位操作。
进一步地,所述点云数据进行畸变补偿,包括:
根据读取点云帧的首尾数据计算点云帧夹角,得到点云帧采样的实际时间;
利用读取进来的惯性测量单元信息获取车辆角速度和线速度。用角速度乘以时间得到车辆旋转角度;用线速度乘以时间得到车辆偏移位移,生成旋转偏移矩阵;
将原始点云数据矩阵乘上旋转偏移矩阵,得到畸变补偿后的点云数据。
进一步地,所述实现传感器对齐的机器人的位置姿态的计算公式为:
Figure RE-GDA0003188571960000031
其中,X(tn)代表tn点的机器人的位置姿态,X(to1)、X(to2)分别是to1、to2时刻的里程计数据。
进一步地,当***一帧点云数据计算所得位姿预估值与其上一帧的位姿预估值的距离没有超出预设的阈值时,结束步骤3。
进一步地,点云数据的任何区域可通过环和扇形唯一索引,将点云数据用二维形式表示即得到scan Context特征图;
记两个点云的scan context特征图为Iq和Ic,定义scan Context特征图之间的距离计算公式如下:
Figure RE-GDA0003188571960000032
其中,Ns为扇形(sector)的数量,
Figure RE-GDA0003188571960000033
分别为利用误差函数对Iq和Ic的求解结果;
对于存在同一位置但是不同时间访问的两个scan context特征图的情况,所有可能出现列平移的scan contexts特征图的列向量之和并找到最小值作为最终的距离,列平移的最佳匹配关系n*定义为:
Figure RE-GDA0003188571960000034
最小距离定义为:
Figure RE-GDA0003188571960000035
其中,Ns代表点云中的扇形数量,n代表的就是其中的一个扇形,Nr代表点云数据中的环数量。
进一步地,利用公式(2)计算计算任务队列中的点云数据的scan Context和候选场景的scan context之间的距离,满足阈值的候选场景则被认为是闭环:
Figure RE-GDA0003188571960000036
其中,
Figure RE-GDA0003188571960000041
表示候选场景的scan context,Iq表示任务队列中点云数据的的 scancontext,D是运用公式(4)计算出的Iq
Figure RE-GDA0003188571960000042
间的最小距离,τ是设定的判断两个场景是否相似的距离阈值,c*是最终检测出来的和当前场景一致的历史场景。
进一步地,误差函数的定义eij(xi,xj)为:
Figure RE-GDA0003188571960000043
其中,xi表示节点i的位姿,zij和表示节点i和节点j之间虚拟测量值的均值,
Figure RE-GDA0003188571960000044
代表期望的虚拟测量值,误差eij(xi,xj)取决于期望测量值与实际测量值之差;
将误差函数简化为F(x):
Figure RE-GDA0003188571960000045
x*=argmin F(x) (8)
其中,x*为误差函数定义式(6)的对应特殊解,即使位姿预估值误差最小的值;
Figure RE-GDA0003188571960000046
为误差eij的转置,C为约束,C={<eij,Ωij>},Ωij表示节点i和节点j之间虚拟测量值的信息矩阵。
进一步地,所述通过高斯牛顿法对误差函数进行迭代求最优解,包括:
设初始点为
Figure RE-GDA0003188571960000047
给它一个(Δxi,Δxj)的增量,对误差函数进行一阶 Taylor展开:
Figure RE-GDA0003188571960000048
其中,Δx表示微分,代表函数
Figure RE-GDA0003188571960000049
在初始预测位置姿态
Figure RE-GDA00031885719600000412
处的变化量;Jij为误差函数对
Figure RE-GDA00031885719600000413
计算的雅可比矩阵,对于节点i和节点j之间的边的目标函数项有:
Figure RE-GDA00031885719600000410
所有节点之间的边的目标函数项有:
Figure RE-GDA00031885719600000411
Figure RE-GDA0003188571960000051
其中,c=∑cij,b=∑bij,H=∑Hij,ΔxT、bT分别为Δx、b的转置,∑<i,j>∈C表示对函数
Figure RE-GDA0003188571960000052
在约束条件<i,j>∈C下的所有取值进行求和,
Figure RE-GDA0003188571960000053
中的i,j取值在约束范围C内,C为约束;
目标是找到Δx,令
Figure RE-GDA0003188571960000054
有:
Figure RE-GDA0003188571960000055
求解得到最优解Δx。
一种终端设备,包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序,处理器执行计算机程序时实现基于点云融合的行车场景构建方法的步骤。
一种计算机可读存储介质,计算机可读存储介质存储有计算机程序,计算机程序被处理器执行时实现基于点云融合的行车场景构建方法的步骤。
与现有技术相比,本发明具有以下技术特点:
1.本方法创新性提出并运用一种点云融合的SLAM地图构建方法,该方法通过融合惯性测量单元(惯性传感器)IMU/GPS传感器数据,并结合点云数据畸变补偿、传感器时间对齐、图优化和闭环检测等优化手段,在保证建图实时性的同时极大提高建图的精度,可以快速实现场景中的定位和高精度地图的构建,克服了传统的纯激光里程计建图误差大、实时性不强的缺陷。
2.本方法设计了基于边缘协同的地图构建定位方法,运用云端下发边缘设备,由边缘设备构建子地图再上传至云端进行拼接组合的方法,充分利用了边缘的算力,提高资源利用率,同时减少了单车辆的地图重复构建工作,降低工作冗余度且减轻边缘设备负载,解决了传统地图构建方法因算力不足导致的耗时长、精度低问题。
附图说明
图1为本发明方法的流程框架图;
图2为对点云数据进行畸变补偿的流程示意图;
图3为前端估计里程实现流程图;
图4为闭环检测流程示意图;
图5为传感器时间对齐的示意图。
具体实施方式
本发明提供了一种基于点云融合的行车场景构建方法,该方法主要采用同步定位与地图构建技术地图构建方案。首先,对点云数据做畸变补偿和传感器时间对齐后再将数据发布,进行前端里程估计;前端里程估计节点接受点云数据后将对其进行点云匹配并生成初步的激光里程计估计值;接着再对估计值进行闭环检测,若检测形成闭环则将信息传递至后端进行后端算法优化;后端算法优化得到预估值节点的位姿信息和路标构成图的顶点集,并将生成的优化后激光里程计数据发布展示。
参见附图,本发明的一种基于点云融合的行车场景构建方法,具体实现步骤如下:
步骤1,点云数据畸变补偿
车载的激光雷达在汽车运动的过程扫描收集点云数据,但由于激光点的坐标系不断变化,因此采集到的点云数据存在畸变现象,需要进行畸变补偿。
a.根据读取点云帧的首尾数据计算点云帧夹角,得到点云帧采样的实际时间;
b.利用读取进来的惯性测量单元信息获取车辆角速度和线速度。用角速度乘以时间得到车辆旋转角度;用线速度乘以时间得到车辆偏移位移,生成旋转偏移矩阵;
c.将原始点云数据矩阵乘上旋转偏移矩阵,得到畸变补偿后的点云数据。
步骤2,传感器时间对齐
将畸变补偿后的点云数据进行传感器时间对齐。该方法通过对线性的里程计数据进行线性插值计算消除采样率差异带来的影响,使激光雷达扫描获取点云数据时,机器人的位置姿态实现传感器对齐。具体计算公式如下所示:
Figure RE-GDA0003188571960000061
其中,X(tn)代表tn点的机器人的位置姿态,X(to1)、X(to2)分别是to1、to2时刻的里程计数据。该步骤计算所得的位置姿态将作为步骤3前端里程估计的初始预测位置姿态。
使点云数据在进行畸变补偿的同时,对同一时间下的传感器收集的线性的里程计数据进行线性插值计算,使里程计数据向点云数据对齐时间,就完成了传感器时间对齐,得到的位置姿态X(tn)消除了采样率差异带来的影响。
步骤3,前端里程估计
创建一个任务队列用来存放经步骤1、2处理后的点云数据,并生成一个局部地图。队列中每***一帧点云数据时,输入该点云数据对应的机器人的初始预测位置姿态,与局部地图进行点云匹配;点云匹配即对点云数据进行正态分布变换,得到目标点云的所有单位体素,代入高斯概率模型,寻找到局部地图上与初始预测位置姿态最近似的点,得到前端里程估计的位姿预估值,用于更新一次局部地图。当***一帧点云数据计算所得位姿预估值与其上一帧的位姿预估值的距离没有超出预设的阈值时,结束该步骤。
步骤4,闭环检测
上述过程得到的位姿预估值将通过闭环检测确定机器人是否已返回到先前位置,避免步骤(3)位姿预估值错误导致错误逐帧向下传递产生累积误差。
步骤4.1,采用基于点云的场景识别方式,将前端里程估计创建的任务队列中的点云数据用scan Context特征图(基于非直方图的全局描述符)进行描述。
由于点云的任何区域都可以由ring(环)和sector(扇形)唯一索引,把点云数据用二维形式表示就得到scan Context特征图,也就是得到一个二维特征图,该特征图就是用于表示这个索引关系的。
记两个点云的scan context特征图为Iq和Ic,定义scan Context特征图之间的距离计算公式如下:
Figure RE-GDA0003188571960000071
其中,Ns为扇形(sector)的数量,
Figure RE-GDA0003188571960000072
分别为利用误差函数对Iq和Ic的求解结果,参见式(6)至式(8)。
对于存在同一位置但是不同时间访问的两个scan context特征图的情况,本方法可计算所有可能出现列平移的scan contexts特征图的列向量之和并找到最小值作为最终的距离,列平移的最佳匹配关系n*定义为:
Figure RE-GDA0003188571960000073
最小距离定义为:
Figure RE-GDA0003188571960000081
本方案中,Ns代表点云中的sector数量,n代表的就是其中的一个扇形, Nr代表点云数据中的ring数量。
点云数据可以被分成很多个扇形,取不同的扇形进行距离计算得出的结果会有所不同,上述公式的目的是计算出两个scan context间的最小距离,也就是两个点云的间距可以距离最近的两个对应扇形间的距离来表示。
步骤4.2,从scan context特征图中提取对于旋转不敏感的描述符环ring,将其表示为一个Nr维的向量k用于KD树(k-维树)的构建,获得当前场景的 ring的最相似区域,从中选取和当前场景的ring最相似的若干历史ring对应的 scan context作为候选场景的scan context。
其中,每个scan context代表的都是一帧点云数据,每帧点云数据都是通过激光扫描实际场景得出的,因此每个scan context都对应着一个实际场景,若两个场景对应的scan context距离足够小,可以认为这两个场景基本一致。候选场景即为候选scancontext对应的场景。
由于点云数据一帧一帧输入的,故定义在当前帧之前输入的点云数据对应的scancontext为历史场景。闭环检测是通过比对当前场景和历史场景的相似性判断机器人是否返回到了先前位置,即判断地图是否形成闭环。
步骤4.3,使用公式(2)计算步骤4.1中任务队列的点云数据的scan Context 和步骤4.2中获取的候选场景的scan context之间的距离,满足阈值的候选场景则被认为是闭环:
Figure RE-GDA0003188571960000082
其中,
Figure RE-GDA0003188571960000083
表示候选场景的scan context,Iq表示任务队列中点云数据的的 scancontext,D是运用公式(4)计算出的Iq
Figure RE-GDA0003188571960000084
间的最小距离,τ是设定的判断两个场景是否相似的距离阈值,c*是最终检测出来的和当前场景一致的历史场景。
通过公式5可以进行判断,若相似度足够高,即小于距离阈值时,则认为形成闭环,即步骤3获取的位姿预估值不存在误差,信息可传递给后端进行优化。否则返回步骤3。
步骤5,后端算法优化
后端算法优化主要为图优化,本方法将机器人不同时刻通过前端里程估计得到的位姿预估值抽象定义为节点,机器人在不同位置上的观测所产生的约束被抽象为节点之间的边,节点的位姿信息和路标构成图的顶点集;在图优化中我们用边来表示这些观测数据的约束,也可以将边看成是一种“虚拟测量值”。
误差函数的定义eij(xi,xj)为:
Figure RE-GDA0003188571960000091
其中,xi表示节点i的位姿,zij和表示节点i和节点j之间虚拟测量值的均值,
Figure RE-GDA0003188571960000092
代表期望的虚拟测量值,误差eij(xi,xj)取决于期望测量值与实际测量值之差。
将误差函数简化为F(x):
Figure RE-GDA0003188571960000093
x*=argmin F(x) (8)
其中,x*为误差函数定义式(6)的对应特殊解,即使位姿预估值误差最小的值;
Figure RE-GDA0003188571960000094
为误差eij的转置,C为约束,C={<eij,Ωij>},Ωij表示节点i和节点j之间虚拟测量值的信息矩阵。
通过高斯牛顿法或者Levenberg-Marquardt算法迭代求最优解:
设初始点为
Figure RE-GDA0003188571960000095
给它一个(Δxi,Δxj)的增量,对误差函数进行一阶 Taylor展开:
Figure RE-GDA0003188571960000096
其中,Δx表示微分,代表函数
Figure RE-GDA0003188571960000097
在初始预测位置姿态
Figure RE-GDA0003188571960000098
处(趋近于无穷小)的变化量;Jij为误差函数对
Figure RE-GDA00031885719600000910
计算的雅可比矩阵,对于节点i和节点j之间的边的目标函数项有:
Figure RE-GDA0003188571960000099
所有节点之间的边的目标函数项有:
Figure RE-GDA0003188571960000101
其中,c=∑cij,b=∑bij,H=∑Hij,ΔxT、bT分别为Δx、b的转置,
Figure RE-GDA0003188571960000106
为初始预测位置姿态,∑<i,j>∈C代表的就是对后面的函数
Figure RE-GDA0003188571960000102
在约束条件<i,j>∈C下的所有取值进行求和,
Figure RE-GDA0003188571960000103
中的i,j取值在约束范围 C内,C为约束。
本方法的目标是找到Δx,令
Figure RE-GDA0003188571960000104
有:
Figure RE-GDA0003188571960000105
求解得到最优解Δx叠加到初始预测位置姿态
Figure RE-GDA0003188571960000107
得到新的位姿预估值,将新的位姿预估值作为下一轮迭代的初始预测位置姿态返回到公式5,继续迭代直至获得x*,即位姿预估值误差最小。
由步骤3可知,在前端里程估计中,任务队列中每***一帧点云数据就会获得一次相对应的位姿预估值,每一个位姿预估值都对应着一帧点云数据,每帧点云数据都对应着一个scan context特征图,在公式5中表示为Iq
x*是使位姿预估值误差最小的值,后端优化的目的就是使位姿预估值与实际测量值的误差最小化,在计算出x*的过程中将使位姿预估值不断更新,地图也在不断更新,因此算出x*时的地图实现误差最小化。
步骤3前端里程估计得到的局部地图在步骤4中进行闭环检测,来检查地图路线是否存在错误。在步骤5中,通过图优化方法得到误差最小化的位姿预估值用于更新局部地图,使地图误差最小化,从而得到最终的局部地图。
步骤6,边缘协同
步骤1-步骤5的构建方案将在边缘设备中进行,构建出局部地图后再上传到云服务器进行全局地图拼接,最后再将拼接好的地图回传到边缘设备供车辆进行定位操作。
以上实施例仅用以说明本申请的技术方案,而非对其限制;尽管参照前述实施例对本申请进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围,均应包含在本申请的保护范围之内。

Claims (10)

1.一种基于点云融合的行车场景构建方法,其特征在于,包括以下步骤:
步骤1,对车载激光雷达在汽车运动过程中扫描收集的点云数据进行畸变补偿;
步骤2,将畸变补偿后的点云数据进行传感器时间对齐,使点云数据在进行畸变补偿的同时,对同一时间下的里程计数据进行线性插值计算,使里程计数据向点云数据对齐时间,得到实现传感器对齐的机器人的位置姿态作为前端里程估计的初始预测位置姿态;
步骤3,创建一个任务队列用来存放点云数据,并生成一个局部地图;任务队列中每***一帧点云数据时,输入该点云数据对应的机器人的初始预测位置姿态,与局部地图进行点云匹配,对点云数据进行正态分布变换,得到目标点云的所有单位体素,代入高斯概率模型,寻找到局部地图上与初始预测位置姿态最近似的点,得到前端里程估计的位姿预估值;
步骤4,采用基于点云的场景识别方式,将前端里程估计创建的任务队列中的点云数据用scan Context特征图进行描述;定义scan Context特征图之间的距离计算公式;从scanContext特征图中提取描述符环,将其表示为一个多维向量以进行KD树的构建,获得当前场景的环的最相似区域,从中选取和当前场景的环最相似的历史环对应的scan context作为候选场景的scan context;计算任务队列中的点云数据的scan Context和候选场景的scancontext之间的距离,满足阈值的候选场景则被认为是闭环,则将所述位姿预估值进行后端优化;
步骤5,将机器人不同时刻通过前端里程估计得到的位姿预估值抽象定义为节点,机器人在不同位置上的观测所产生的约束被抽象为节点之间的边,节点的位姿信息和路标构成图的顶点集,用边来表示观测数据的约束;
定义误差函数为两个节点之间的虚拟测量值的均值与期望的虚拟测量值的差值;通过高斯牛顿法对误差函数进行迭代求最优解,将求得的最优解叠加到初始预测位置姿态得到新的位姿预估值,将新的位姿预估值作为下一轮迭代的初始预测位置姿态,继续迭代直至位姿预估值误差最小;
步骤1-步骤5在边缘设备中进行,构建出局部地图后再上传到云服务器进行全局地图拼接,最后再将拼接好的地图回传到边缘设备供车辆进行定位操作。
2.根据权利要求1所述的基于点云融合的行车场景构建方法,其特征在于,所述点云数据进行畸变补偿,包括:
根据读取点云帧的首尾数据计算点云帧夹角,得到点云帧采样的实际时间;
利用读取进来的惯性测量单元信息获取车辆角速度和线速度。用角速度乘以时间得到车辆旋转角度;用线速度乘以时间得到车辆偏移位移,生成旋转偏移矩阵;
将原始点云数据矩阵乘上旋转偏移矩阵,得到畸变补偿后的点云数据。
3.根据权利要求1所述的基于点云融合的行车场景构建方法,其特征在于,所述实现传感器对齐的机器人的位置姿态的计算公式为:
Figure FDA0003147212300000021
其中,X(tn)代表tn点的机器人的位置姿态,X(to1)、X(to2)分别是to1、to2时刻的里程计数据。
4.根据权利要求1所述的基于点云融合的行车场景构建方法,其特征在于,当***一帧点云数据计算所得位姿预估值与其上一帧的位姿预估值的距离没有超出预设的阈值时,结束步骤3。
5.根据权利要求1所述的基于点云融合的行车场景构建方法,其特征在于,点云数据的任何区域可通过环和扇形唯一索引,将点云数据用二维形式表示即得到scan Context特征图;
记两个点云的scan context特征图为Iq和Ic,定义scan Context特征图之间的距离计算公式如下:
Figure FDA0003147212300000022
其中,Ns为扇形(sector)的数量,
Figure FDA0003147212300000023
分别为利用误差函数对Iq和Ic的求解结果;
对于存在同一位置但是不同时间访问的两个scan context特征图的情况,所有可能出现列平移的scan contexts特征图的列向量之和并找到最小值作为最终的距离,列平移的最佳匹配关系n*定义为:
Figure FDA0003147212300000024
最小距离定义为:
Figure FDA0003147212300000031
其中,Ns代表点云中的扇形数量,n代表的就是其中的一个扇形,Nr代表点云数据中的环数量。
6.根据权利要求1所述的基于点云融合的行车场景构建方法,其特征在于,利用公式(2)计算计算任务队列中的点云数据的scan Context和候选场景的scan context之间的距离,满足阈值的候选场景则被认为是闭环:
Figure FDA0003147212300000032
其中,
Figure FDA0003147212300000033
表示候选场景的scan context,Iq表示任务队列中点云数据的的scancontext,D是运用公式(4)计算出的Iq
Figure FDA0003147212300000034
间的最小距离,τ是设定的判断两个场景是否相似的距离阈值,c*是最终检测出来的和当前场景一致的历史场景。
7.根据权利要求1所述的基于点云融合的行车场景构建方法,其特征在于,误差函数的定义eij(xi,xj)为:
Figure FDA0003147212300000035
其中,xi表示节点i的位姿,zij和表示节点i和节点j之间虚拟测量值的均值,
Figure FDA0003147212300000036
代表期望的虚拟测量值,误差eij(xi,xj)取决于期望测量值与实际测量值之差;
将误差函数简化为F(x):
Figure FDA0003147212300000037
x*=argminF(x) (8)
其中,x*为误差函数定义式(6)的对应特殊解,即使位姿预估值误差最小的值;
Figure FDA0003147212300000038
为误差eij的转置,C为约束,C={<eij,Ωij>},Ωij表示节点i和节点j之间虚拟测量值的信息矩阵。
8.根据权利要求1所述的基于点云融合的行车场景构建方法,其特征在于,所述通过高斯牛顿法对误差函数进行迭代求最优解,包括:
设初始点为
Figure FDA0003147212300000039
给它一个(Δxi,Δxj)的增量,对误差函数进行一阶Taylor展开:
Figure FDA00031472123000000310
其中,Δx表示微分,代表函数
Figure FDA0003147212300000041
在初始预测位置姿态
Figure FDA0003147212300000048
处的变化量;Jij为误差函数对
Figure FDA0003147212300000049
计算的雅可比矩阵,对于节点i和节点j之间的边的目标函数项有:
Figure FDA0003147212300000042
所有节点之间的边的目标函数项有:
Figure FDA0003147212300000043
其中,c=∑cij,b=∑bij,H=∑Hij,ΔxT、bT分别为Δx、b的转置,∑<i,j>∈C表示对函数
Figure FDA0003147212300000044
在约束条件<i,j>∈C下的所有取值进行求和,
Figure FDA0003147212300000045
中的i,j取值在约束范围C内,C为约束;
目标是找到Δx,令
Figure FDA0003147212300000046
有:
Figure FDA0003147212300000047
求解得到最优解Δx。
9.一种终端设备,包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序,其特征在于,处理器执行计算机程序时实现根据权利要求1-8中任一权利要求所述的基于点云融合的行车场景构建方法的步骤。
10.一种计算机可读存储介质,计算机可读存储介质存储有计算机程序,其特征在于,计算机程序被处理器执行时实现根据权利要求1-8中任一权利要求所述的基于点云融合的行车场景构建方法的步骤。
CN202110755584.3A 2021-07-05 2021-07-05 一种基于点云融合的行车场景构建方法 Active CN113379915B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110755584.3A CN113379915B (zh) 2021-07-05 2021-07-05 一种基于点云融合的行车场景构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110755584.3A CN113379915B (zh) 2021-07-05 2021-07-05 一种基于点云融合的行车场景构建方法

Publications (2)

Publication Number Publication Date
CN113379915A true CN113379915A (zh) 2021-09-10
CN113379915B CN113379915B (zh) 2022-12-23

Family

ID=77580986

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110755584.3A Active CN113379915B (zh) 2021-07-05 2021-07-05 一种基于点云融合的行车场景构建方法

Country Status (1)

Country Link
CN (1) CN113379915B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116229119A (zh) * 2022-08-30 2023-06-06 智瞰深鉴(北京)科技有限公司 一种变电站巡检机器人及其重定位方法

Citations (12)

* 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
CN109974707A (zh) * 2019-03-19 2019-07-05 重庆邮电大学 一种基于改进点云匹配算法的室内移动机器人视觉导航方法
CN110648389A (zh) * 2019-08-22 2020-01-03 广东工业大学 基于无人机和边缘车辆协同的城市街景3d重建方法和***
CN110689622A (zh) * 2019-07-05 2020-01-14 电子科技大学 一种基于点云分割匹配闭环校正的同步定位与构图算法
AU2018278849A1 (en) * 2018-07-02 2020-01-16 Beijing Didi Infinity Technology And Development Co., Ltd. Vehicle navigation system using pose estimation based on point cloud
CN111337947A (zh) * 2020-05-18 2020-06-26 深圳市智绘科技有限公司 即时建图与定位方法、装置、***及存储介质
WO2020154966A1 (en) * 2019-01-30 2020-08-06 Baidu.Com Times Technology (Beijing) Co., Ltd. A rgb point clouds based map generation system for autonomous vehicles
CN111882977A (zh) * 2020-05-06 2020-11-03 北京嘀嘀无限科技发展有限公司 一种高精度地图构建方法及***
CN111929699A (zh) * 2020-07-21 2020-11-13 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及***
CN112734852A (zh) * 2021-03-31 2021-04-30 浙江欣奕华智能科技有限公司 一种机器人建图方法、装置及计算设备
CN112966542A (zh) * 2020-12-10 2021-06-15 武汉工程大学 一种基于激光雷达的slam***和方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及***

Patent Citations (12)

* 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
AU2018278849A1 (en) * 2018-07-02 2020-01-16 Beijing Didi Infinity Technology And Development Co., Ltd. Vehicle navigation system using pose estimation based on point cloud
WO2020154966A1 (en) * 2019-01-30 2020-08-06 Baidu.Com Times Technology (Beijing) Co., Ltd. A rgb point clouds based map generation system for autonomous vehicles
CN109974707A (zh) * 2019-03-19 2019-07-05 重庆邮电大学 一种基于改进点云匹配算法的室内移动机器人视觉导航方法
CN110689622A (zh) * 2019-07-05 2020-01-14 电子科技大学 一种基于点云分割匹配闭环校正的同步定位与构图算法
CN110648389A (zh) * 2019-08-22 2020-01-03 广东工业大学 基于无人机和边缘车辆协同的城市街景3d重建方法和***
CN111882977A (zh) * 2020-05-06 2020-11-03 北京嘀嘀无限科技发展有限公司 一种高精度地图构建方法及***
CN111337947A (zh) * 2020-05-18 2020-06-26 深圳市智绘科技有限公司 即时建图与定位方法、装置、***及存储介质
CN111929699A (zh) * 2020-07-21 2020-11-13 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及***
CN112966542A (zh) * 2020-12-10 2021-06-15 武汉工程大学 一种基于激光雷达的slam***和方法
CN112734852A (zh) * 2021-03-31 2021-04-30 浙江欣奕华智能科技有限公司 一种机器人建图方法、装置及计算设备
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及***

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
DAI Y ET AL.: "Artificial intelligence empowered edge computing and caching for internet of vehicles", 《IEEE WIRELESS COMMUNICATIONS》 *
SHIN K ET AL.: "Roarnet: A robust 3d object detection based on region", 《IEEE INTELLIGENT VEHICLES SYMPOSIUM》 *
李丰哲: "基于多传感器融合的电力巡检机器人定位技术研究", 《全国优秀硕士学位论文全文数据库》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116229119A (zh) * 2022-08-30 2023-06-06 智瞰深鉴(北京)科技有限公司 一种变电站巡检机器人及其重定位方法

Also Published As

Publication number Publication date
CN113379915B (zh) 2022-12-23

Similar Documents

Publication Publication Date Title
CN109709801B (zh) 一种基于激光雷达的室内无人机定位***及方法
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
Lenac et al. Fast planar surface 3D SLAM using LIDAR
CN110930495A (zh) 基于多无人机协作的icp点云地图融合方法、***、装置及存储介质
US11158065B2 (en) Localization of a mobile unit by means of a multi hypothesis kalman filter method
CN112965063B (zh) 一种机器人建图定位方法
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN113916243A (zh) 目标场景区域的车辆定位方法、装置、设备和存储介质
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
CN115690338A (zh) 地图构建方法、装置、设备及存储介质
CN115077519A (zh) 基于模板匹配与激光惯导松耦合的定位建图方法和装置
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
Zhou et al. Lane information extraction for high definition maps using crowdsourced data
CN115371662A (zh) 一种基于概率栅格移除动态对象的静态地图构建方法
CN113379915B (zh) 一种基于点云融合的行车场景构建方法
CN114777775A (zh) 一种多传感器融合的定位方法及***
Zhang et al. Continuous indoor visual localization using a spatial model and constraint
Qiao et al. Online monocular lane mapping using catmull-rom spline
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及***
CN115239899B (zh) 位姿图生成方法、高精地图生成方法和装置
CN110849349A (zh) 一种基于磁传感器与轮式里程计融合定位方法
CN116202487A (zh) 一种基于三维建模的实时目标姿态测量方法
CN114088103B (zh) 车辆定位信息的确定方法和装置
Li et al. An SLAM algorithm based on laser radar and vision fusion with loop detection optimization

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