CN110689576B - 一种基于Autoware的动态3D点云正态分布AGV定位方法 - Google Patents

一种基于Autoware的动态3D点云正态分布AGV定位方法 Download PDF

Info

Publication number
CN110689576B
CN110689576B CN201910935403.8A CN201910935403A CN110689576B CN 110689576 B CN110689576 B CN 110689576B CN 201910935403 A CN201910935403 A CN 201910935403A CN 110689576 B CN110689576 B CN 110689576B
Authority
CN
China
Prior art keywords
point cloud
map
normal distribution
probability
agv
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
CN201910935403.8A
Other languages
English (en)
Other versions
CN110689576A (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.)
Guilin University of Electronic Technology
Original Assignee
Guilin University of Electronic 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 Guilin University of Electronic Technology filed Critical Guilin University of Electronic Technology
Priority to CN201910935403.8A priority Critical patent/CN110689576B/zh
Publication of CN110689576A publication Critical patent/CN110689576A/zh
Application granted granted Critical
Publication of CN110689576B publication Critical patent/CN110689576B/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
    • G06T7/00Image analysis
    • G06T7/70Determining position or orientation of objects or cameras
    • G06T7/73Determining position or orientation of objects or cameras using feature-based methods
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88Lidar systems specially adapted for specific applications
    • G01S17/89Lidar systems specially adapted for specific applications for mapping or imaging
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00Indexing scheme for image analysis or image enhancement
    • G06T2207/10Image acquisition modality
    • G06T2207/10028Range image; Depth image; 3D point clouds
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00Indexing scheme for image analysis or image enhancement
    • G06T2207/20Special algorithmic details
    • G06T2207/20076Probabilistic image processing
    • YGENERAL 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
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02TCLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00Road transport of goods or passengers
    • Y02T10/10Internal combustion engine [ICE] based vehicles
    • Y02T10/40Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

本发明公开了一种基于Autoware的动态3D点云正态分布AGV定位方法,该方法步骤为:1)对激光点云数据建立三维点云地图并重建为稠密点云;2)蒙特卡洛定位;3)在三维点云地图中导入大小均匀的单位网格,并将单位网格内的粒子转换成正态分布概率估计,构造出连续分段的正态分布模型,在建立正态分布模型的基础上,使用最大似然估计算法对概率分布参数和进行优化,得到适合两点云匹配的坐标转换参数值;4)对已进行划分的单位网格内的正态分布模型进行参数实时的更新,在正态分布的基础上,迭代更新其短期局部地图中的均值和协方差,最终达到动态更新全局地图。该方法解决了传统网格定位效率较低的问题,又解决了AGV在动态环境中运动的定位问题。

Description

一种基于Autoware的动态3D点云正态分布AGV定位方法
技术领域
本发明涉及AGV自动导引车的定位技术领域,具体涉及一种动态环境中基于Autoware的3D点云正态分布定位方法。
背景技术
自动导引车AGV目前广泛应用于现代物流行业,但是由于在真实环境中,由于其他运动的物体使得其工作场景不可能完全静态,如何实现新的观测值与过去的观测值正确的关联,并更新全局地图,从而使AGV在动态场景下定位,逐渐成为实现AGV自主导航中的热点问题。
在AGV定位的定位方法中,传统的定位方法通常采用基于icp点云匹配的算法,但是由于激光点云数据不是瞬时间获得的,因此若在匹配时加入速度量,则会产生严重的运动畸变问题,匹配精度较低。精确的定位在现代物流应用场景中至关重要,基于贝叶斯滤波的蒙特卡洛定位法,利用概率模型定义AGV的定位问题。
本文在传统的蒙特卡洛定位基础上,采用基于3D点云的正态分布算法,该算法在单位网格下,计算点云数据的正态概率密度函数,以得到连续可导的定位路径。同时为了提高动态环境下AGV的定位精度,本文对正态分布算法进行改进,在此基础上迭代更新正态分布样本中的均值和协方差,从而实现实时更新位姿矩阵,实现自动导引车AGV在动态环境中的实时定位,初步解决了AGV在路径跟踪时的定位误差较大的问题。
发明内容
本发明的目的在于克服现有技术的不足,而提供一种基于Autoware的动态3D点云正态分布AGV定位方法,该方法将正态分布算法(NDT)与蒙特卡洛定位算法(MCL)相结合,并实时的更新正态分布中的样本协方差矩阵,既解决了传统网格定位效率较低的问题,又解决了AGV在动态环境中运动的定位问题。
实现本发明目的的技术方案是:
一种基于Autoware的动态3D点云正态分布AGV定位方法,包括如下步骤:
1)为了解决在指定环境下的激光雷达定位问题,需对激光点云数据建立三维点云地图并重建为稠密点云:采用robosence3D激光雷达采集点云数据,在Autoware开源框架中导入点云数据,通过调用pcl点云库中的NDT匹配算法,得到较为密集的环境地图并导出pcd文件,在此基础上加载点云pcd地图,并同时接收激光雷达topic/points_raw,实现点云与地图的匹配;
2)在建立激光点云的三维点云地图的基础上,为了使AGV在已知传感器信息下实现定位,使用基于概率估计的蒙特卡洛算法,构造随机变量的概率分布模型,估计AGV位姿的后验概率分布,计算简单轻巧,可以快速估计出所需要的物理模型,并使用粒子滤波算法对待采样的观测值赋权重,加速求解AGV的后验概率模型;
3)为避免粒子滤波的算法定位中出现的不连续的问题,在三维点云地图中导入大小均匀的单位网格,并将单位网格内的粒子转换成正态分布概率估计,构造出连续分段的正态分布模型,为解决蒙特卡洛定位算法较为离散的缺陷,提供良好的优化,在建立正态分布模型的基础上,使用最大似然估计算法对概率分布参数和进行优化,得到适合两点云匹配的坐标转换参数值,缩小点云间的定位误差;
4)为适应动态环境下,新测量的量随时间增加导致地图改变后定位不准确的问题,对已进行划分的单位网格内的正态分布模型进行参数实时的更新,在正态分布的基础上,只迭代更新其短期局部地图中的均值和协方差,最终达到动态更新全局地图。
所述的AGV的后验概率模型,转换为正态分布的形式为:
Figure GDA0004069246150000021
p(xt|ut,zt,m)为后验概率模型,其中xt为AGV的状态量,ut为控制量,zt为所需的观测值,m为此时刻的静态地图,u为所求的均值,Σ为所求得方差;
扫描匹配算法通过比较参考位置处获得的扫描与AGV实际位置处获得的扫描点匹配,获得AGV实际位置和参考位置之间的相对距离和角度,一次更新AGV的位置,AGV的实际位置由航位推算获得,具体步骤如下:
3-1)建立参考点云xi的正态分布变换
首先在局部坐标系中,将给定的环境划分为形状规则的单位网格,并选定单位网格内N个点云数据为xi={x1,x2,…,xN},确定传感器下的观测值为
Figure GDA0004069246150000022
其中pi为单位网格内单个粒子的位姿,将粒子累积到网格中并进行正态分布转换,得到正态分布转换后的观测值模型可表示为
Figure GDA0004069246150000023
最后得到分段连续的点云概率模型;
经过正态分布变换的单位网格内点云数据的概率密度为:
Figure GDA0004069246150000031
其中概率密度参数均值和方差分别为:
Figure GDA0004069246150000032
Figure GDA0004069246150000033
3-2)点云配准,计算坐标转换后的概率密度之和
坐标变换的参数值为s(p),若概率密度和的值较高,则说明坐标转换的参数较好,点云配准较为准确,其计算公式如下:
Figure GDA0004069246150000034
为实现当前的扫描点在参考扫描面上的最大化,对当前的点进行坐标变换,其中变换参数为j,空间转换函数为T(j,xi),在已知当前时刻的局部坐标系xt和环境地图m下,单元格属性为c,得到观测值的似然函数;
其最大似然估计的形式为
Figure GDA0004069246150000035
转化为负对数似然函数为:
Figure GDA0004069246150000036
对原概率和求解最大化,则可转化为对负对数似然函数求解最小值,通过对坐标转换的概率密度和s(p)进行评估,将求解坐标转换的参数问题转换成对s(p)的优化问题,最终通过优化,不断逼近真实值达到精确定位。
步骤4)中,所述的短期局部地图,动态环境下的定位,分为两部分,初始测量时,在静态环境下采用已知地图进行定位;在动态环境下采用已知位姿为基准的定位方式,在已知环境静态地图的基础上,其短期局部地图可以表示为:
Figure GDA0004069246150000037
其中,uj和∑j是当前的平均值和协方差估计值,pj是占用概率,表示对于每一帧时间t,Nm个点云数据在此正态分布的参数下的置信度;
对短期局部地图的更新分为两个阶段,具体步骤如下:
4-1)估计随时间变化的平均值和方差
首先定义短期局部地图中每个单位网格的属性c,ci={uii,Ni,p(mi|zi=1:t)},其中ui和Σi是估计高斯分量的参数,Ni是目前用于估计正态分布参数的点数,p(mi|zi=1:t)是网格被占用的概率,对当前时间的平均值和协方差进行更新,采用正态分布参数迭代更新算法;
首先给定两组测量值
Figure GDA0004069246150000041
Figure GDA0004069246150000042
执行以下计算:
Figure GDA0004069246150000043
Figure GDA0004069246150000044
Figure GDA0004069246150000045
Figure GDA0004069246150000046
Figure GDA0004069246150000047
Figure GDA0004069246150000048
其中ux,y为原始点云下的平均值和协方差,T为空间转换函数,若短期局部地图发生改变,则在两个时间刻度下,对平均值和协方差分别进行更新,得到u1,m和um+1,m+n
对于组合,有:
Figure GDA0004069246150000049
Figure GDA00040692461500000410
对更新后的高斯分布参数进行迭代,通过公式可知,若对单位网格内的参数样本进行更新,只需要知道每个单元存储的向量t、矩阵s和点数即可;
Figure GDA0004069246150000051
Figure GDA0004069246150000052
Figure GDA0004069246150000053
Figure GDA0004069246150000054
若经过m次更新对测量值的影响很小,则可将变化后的参数近似于一个固定值;
4-2)更新过程中每个粒子的权重计算
为了计算每个单位网格内粒子观测值的概率密度,需要根据相应粒子的预测值进行投影测量,并将观测值相对于世界坐标系进行坐标转换,将观察值记作czi,同时从静态图中搜索最接近的正态分布cmi,最终使用最大似然估计法评估其概率分布L2
Figure GDA0004069246150000055
其uzi和pzi是czi的平均值和协方差,umi和pmi的协方差是cmi的平均值和协方差,若获得的可能性高于人工给定的阈值,选择属于静态时间刻度的组件,否则用短期局部地图重复该过程,
其中,在静态地图ms和动态地图mt以及观测值zt下,粒子的权重w可以表示为:
Figure GDA0004069246150000056
将当前位置生成的短期局部地图和以前生成的全局地图相匹配,最终导出匹配的表达式。
本发明提供的一种基于Autoware的动态3D点云正态分布AGV定位方法,该定位方法在蒙特卡洛定位算法的基础上采用连续的NDT正态分布算法,用以解决激光点云在与地图坐标匹配的过程中的离散化问题,提高了定位精度。同时在建立正态分布模型上,对模型中均值和方差随时间变化实时更新,从而解决了在动态环境中的实时定位问题,以适应工业环境中的动态场景。
附图说明
图1为在Autoware下通过调用激光雷达的点云数据并通过ndt-matching算法对数据进行拟合,建立的三维点云地图;
图2为在三维点云地图的基础上,使用蒙特卡罗算法,对激光点云进行的定位图;
图3为在蒙特卡洛定位的基础上,为避免点云数据过于离散化,采用基于正态分布的NDT算法正态分布定位图;
图4将指定的位置信息作为目标测试集,并与基于蒙特卡洛定位算法优化后的激光雷达点云数据分析,生成的matlab误差对比图;
图5为动态环境下NDT算法优化的激光点云数据,与指定位置信息数据集,在matlab下进行误差对比图。
具体实施方式
下面结合附图和实施例对本发明做进一步阐述,但不是对本发明的限定。
实施例:
本实施例采用里程计模型作为AGV的运动仿真模型,并采用三轮全向底盘模拟AGV在真实工业环境中的运动,通过航迹推算确定出AGV的运动模型。
一种基于Autoware的动态3D点云正态分布AGV定位方法,包括如下步骤:
1)建立三维点云地图并重建
为了解决在指定环境下的激光雷达定位问题,需对激光点云数据建立三维点云地图并重建为稠密点云:采用robosence3D激光雷达采集点云数据,在Autoware开源框架中导入点云数据,通过调用pcl点云库中的NDT匹配算法,得到较为密集的环境地图并导出pcd文件,在此基础上加载点云pcd地图,并同时接收激光雷达topic/points_raw,实现点云与地图的匹配;
如图1所示,首先在终端中启动激光雷达的驱动文件roslaunch rs_lidar_16.launch,同时驱动AGV沿指定轨迹移动,并调用rosbag record<name.bag>topic对点云信息进行录制,之后启动Autoware仿真框架,在simulation中调入录制完成的bag包,选择勾选ndt_mapping,此时打开rviz可视化界面,可得到三维重建过程。
2)蒙特卡洛定位:在建立激光点云的三维地图的基础上,为了使AGV在已知传感器信息下实现定位,需与激光雷达发出的激光点云标定,以实现AGV位姿在地图中的实时定位。如图2所示使用蒙特卡洛算法,首先估计AGV位姿的后验概率分布,之后使用粒子滤波算法对待采样的观测值赋权重,达到加速求解AGV的后验概率模型的目的。
蒙特卡洛定位算法(MCL)在AGV运动模型的概率分布中加入时间序列,是一种基于贝叶斯估计的概率分布算法,通过已知状态量x在t-1时刻的大小,同时给定t时刻的观测值zt,从而估计状态量在t时刻的概率分布,为了处理MCL在定位过程中的非线性情况,并适应全局定位问题,需使用粒子滤波算法更新状态量,利用从目标概率分布中采集到的样本,去估计概率分布中函数的期望值,在对观测值进行采样的基础上更新权重信息,从而实现从目标分布中采样。
MCL定位主要过程为:
2-1)预测:已知当前的状态量xt-1和粒子的观测值概率为zt,通过里程计模型变换得到当前时刻的状态量xt的概率,如下公式所示:
Figure GDA0004069246150000071
其中p(zt|xt,m)为给定静态地图m下传感器的观测模型,p(ut,xt-1)为里程计的运动模型,表明了已知控制量ut和上一时刻的状态量xt-1
2-2)更新:已知在观测值为zt时刻下对状态量xt进行修正,同时将每个粒子赋予权重,方便于对N个观测值进行采样,其公式可表示为:
Figure GDA0004069246150000072
2-3)粒子滤波算法:N个粒子在t时刻的集合可以表示为
Figure GDA0004069246150000073
xt为假设AGV位姿的状态量,wt为粒子附加的权重值,表示对观测数据和地图匹配程度的评估,具体步骤如下:
2-3-1)已知t-1时刻的概率分布,确定粒子传播模型为:
Figure GDA0004069246150000074
根据数据ut,通过导入AGV运动模型,确定t时刻的粒子分布为:
Figure GDA0004069246150000075
2-3-2)通过观测值评估每一个粒子与地图的匹配程度,对粒子的权重进行迭代更新,更新后的权重为:
Figure GDA0004069246150000076
2-4)重采样:为了避免在迭代过程中出现权值退化的问题,需要以粒子的权重作为离散分布,将每个粒子对应的权重归一化,进行重采样,并采用有效粒子数来衡量粒子的退化程度,有效粒子数越小,权重方差较大,粒子间的权重相差较大,表明权值退化越严重,经过重采样后,粒子的后验概率可以表示成如下形式:
Figure GDA0004069246150000081
其中,ni是指产生新的粒子集时xk (i)被复制的次数,在粒子归一化后,所有的粒子权重都变为同一值1/N,只有个别粒子的次数会增多,变为ni次。
在matlab仿真软件下,调用基于蒙特卡洛定位的粒子滤波算法,将测试点云数据集与原始样本点云数据进行拟合对比,如图4所示,两条线段分别为原始样本集,和测试数据集,可看出定位产生了明显的偏移。
3)NDT正态分布模型。为避免粒子滤波的算法定位中出现的不连续的问题,将单位网格内的粒子转换成正态分布概率估计,构造出连续分段的正态分布模型,为蒙特卡洛定位算法提供良好的优化,在建立参数优化模型的基础上,使用最大似然估计算法对概率分布参数和进行优化,得到适合两点云匹配的坐标转换参数值,缩小点云间的定位误差;
在传统的蒙特卡洛定位的基础上,易存在网格映射模型中点云数据过于离散化,而造成的定位不准确的问题。为了提高AGV的定位精度,本实施例采用基于地图的定位方法,在建立的贝叶斯后验概率模型的基础上,使用正态分布算法作为连接地图和里程计数据的模型,ndt算法可以建立连续分段的正态分布模型,从而得到光滑连续可导的概率密度模型,增强定位的准确性,如图3在slam中应用正态分布转换法(ndt)可以有效的提高定位精度,将AGV的后验概率模型转换成正态分布的基本形式为:
Figure GDA0004069246150000082
扫描匹配算法主要通过比较参考位置处获得的扫描与AGV实际位置处获得的扫描点匹配,获得AGV实际位置和参考位置(激光扫描点)之间的相对距离和角度,一次更新AGV的位置,AGV的实际位置由航位推算获得,具体步骤如下:
3-1)建立参考点云xi的正态分布变换
首先在局部坐标系中,将给定的环境划分为形状规则的单位网格,并选定单位网格内N个点云数据为xi={x1,x2,…,xN},确定传感器下的观测值为
Figure GDA0004069246150000083
其中pi为单位网格内单个粒子的位姿,将粒子累积到网格中并进行正态分布转换,得到正态分布转换后的观测值模型可表示为
Figure GDA0004069246150000084
最后得到分段连续的点云概率模型。
经过正态分布变换的单位网格内点云数据的概率密度为:
Figure GDA0004069246150000091
其中概率密度参数均值和方差分别为:
Figure GDA0004069246150000092
Figure GDA0004069246150000093
3-2)点云配准,计算坐标转换后的概率密度之和
坐标变换的参数值为s(p),若概率密度和的值较高,则说明坐标转换的参数较好,点云配准较为准确,其计算公式如下:
Figure GDA0004069246150000094
为实现当前的扫描点在参考扫描面上的最大化,对当前的点进行坐标变换,其中变换参数为j,空间转换函数为T(j,xi),在已知当前时刻的局部坐标系xt和环境地图m下,可以得到观测值的似然函数。其最大似然估计的形式为
Figure GDA0004069246150000095
转化为负对数似然函数为:
Figure GDA0004069246150000096
对原概率和求解最大化,则可转化为对负对数似然函数求解最小值,通过对坐标转换的概率密度和s(p)进行评估,将求解坐标转换的参数问题转换成对s(p)的优化问题,最终通过优化,不断逼近真实值达到精确定位。
4)为适应动态环境下,新测量的量随时间同步增加,地图改变后,定位不准确的问题,在已进行划分的单位网格内,对正态分布模型的参数实时的更新,但是若更新单位网格内的每一个点,则会造成巨大的内存消耗,同时不能保证动态环境下定位的实时性,因此,本实施例在正态分布的基础上,只迭代更新其短期局部地图中的均值和协方差,最终达到动态更新全局地图。
传统的ndt定位算法是在静态环境下进行运算的,当环境变化时,里程计的测量值和激光雷达的观测值无法得到正确的匹配,因此为了解决在真实的工业环境中由于人或车的变换,使得动态环境下的定位不够准确的问题,本实施例在正态分布的ndt定位的基础上对变换的参数进行实时的迭代更新,将已知的静态地图和随环境变换的短期局部地图的位姿相匹配,由于不需要对整个地图进行迭代的更新,从而降低了内存消耗,同时可以保持定位的实时性。
动态环境下的定位通常分为两部分,初始测量时,在静态环境下采用已知地图进行定位,在动态环境下采用已知位姿为基准的定位方式,在已知环境静态地图的基础上,对于每一帧时间t,其短期局部地图可以表示为:
Figure GDA0004069246150000101
其中,uj和∑j是当前的平均值和协方差估计值,pj是占用概率,表示在此正态分布的参数下的置信度;
对短期局部地图的更新分为两个阶段,具体步骤如下:
4-1)估计随时间变化的平均值和方差。
首先定义短期局部地图中每个单位网格的属性c,ci={uii,Ni,p(mi|zi=1:t)}。其中,ui和Σi是估计高斯分量的参数,N是目前用于估计正态分布参数的点数,p(mi|zi=1:t)是网格被占用的概率,对当前时间的平均值和协方差进行更新,采用正态分布参数迭代更新算法;
首先给定两组测量值
Figure GDA0004069246150000102
Figure GDA0004069246150000103
执行以下计算:
Figure GDA0004069246150000104
Figure GDA0004069246150000105
Figure GDA0004069246150000106
Figure GDA0004069246150000107
Figure GDA0004069246150000108
Figure GDA0004069246150000109
其中ux,y为原始点云下的平均值和协方差,若短期局部地图发生改变,则在两个时间刻度下,对平均值和协方差分别进行更新,得到u1,m和um+1,m+n,
对于组合,有:
Figure GDA0004069246150000111
Figure GDA0004069246150000112
对更新后的高斯分布参数进行迭代,通过公式可知,若对单位网格内的参数样本进行更新,只需要知道每个单元存储的向量t、矩阵s和点数即可;
Figure GDA0004069246150000113
Figure GDA0004069246150000114
Figure GDA0004069246150000115
Figure GDA0004069246150000116
若经过m次更新对测量值的影响很小,则可将变化后的参数近似于一个固定值;
4-2)更新过程中每个粒子的权重计算
为了计算每个单位网格内粒子观测值的概率密度,需要根据相应粒子的预测值进行投影测量。并将观测值相对于世界坐标系进行坐标转换,将观察值记作czi,同时从静态图中搜索最接近的正态分布cmi,最终使用最大似然估计法评估其概率分布L2
Figure GDA0004069246150000117
其uzi和pzi是czi的平均值和协方差,umi和pmi的协方差是cmi的平均值和协方差,若获得的可能性高于人工给定的阈值,选择属于静态时间刻度的组件,否则用短期局部地图重复该过程,
其中,在静态地图ms和动态地图mt以及观测值zt下,粒子的权重可以表示为:
Figure GDA0004069246150000118
将当前位置生成的短期局部地图和以前生成的全局地图相匹配,最终导出匹配的表达式。
如图5所示,在动态环境下,采用基于NDT正态分布模型,对采集到的激光点云数据与地图位置信息进行匹配,得到两条线段分别为原始数据集和测试样本集,可得出优化后的地图匹配度更好,定位精度更高。

Claims (2)

1.一种基于Autoware的动态3D点云正态分布AGV定位方法,其特征在于,包括如下步骤:
1)对激光点云数据建立三维点云地图并重建为稠密点云:采用robosence3D激光雷达采集点云数据,在Autoware开源框架中导入点云数据,通过调用pcl点云库中的NDT匹配算法,得到较为密集的环境地图并导出pcd文件,在此基础上加载点云pcd地图,并同时接收激光雷达topic/points_raw,实现点云与地图的匹配;
2)在建立激光点云的三维点云地图的基础上,使用基于概率估计的蒙特卡洛算法,构造随机变量的概率分布模型,估计AGV位姿的后验概率分布,估计出所需要的物理模型,并使用粒子滤波算法对待采样的观测值赋权重,加速求解AGV的后验概率模型;
3)在三维点云地图中导入大小均匀的单位网格,并将单位网格内的粒子转换成正态分布概率估计,构造出连续分段的正态分布模型,在建立正态分布模型的基础上,使用最大似然估计算法对概率分布参数和进行优化,得到适合两点云匹配的坐标转换参数值;
4)对已进行划分的单位网格内的正态分布模型进行参数实时的更新,在正态分布的基础上,只迭代更新短期局部地图中正态分布模型的均值和协方差,最终达到动态更新全局地图;
步骤4)中,所述的短期局部地图,动态环境下的定位,分为两部分,初始测量时,在静态环境下采用已知地图进行定位;在动态环境下采用已知位姿为基准的定位方式,在已知环境静态地图的基础上,其短期局部地图表示为:
Figure FDA0004069246140000011
其中,uj和∑j是当前的平均值和协方差估计值,pj是占用概率,表示对于每一帧时间t,Nm个点云数据在此正态分布的参数下的置信度;
对短期局部地图的更新分为两个阶段,具体步骤如下:
4-1)估计随时间变化的平均值和方差
首先定义短期局部地图中每个单位网格的属性c,ci={ui,Σi,Ni,p(mi|zi=1:t)},其中ui和Σi是估计高斯分量的参数,Ni是目前用于估计正态分布参数的点数,p(mi|zi=1:t)是网格被占用的概率,对当前时间的平均值和协方差进行更新,采用正态分布参数迭代更新算法;
首先给定两组测量值
Figure FDA0004069246140000012
Figure FDA0004069246140000013
执行以下计算:
Figure FDA0004069246140000021
Figure FDA0004069246140000022
Figure FDA0004069246140000023
Figure FDA0004069246140000024
Figure FDA0004069246140000025
Figure FDA0004069246140000026
其中ux,y为原始点云下的平均值和协方差,T为空间转换函数,若短期局部地图发生改变,则在两个时间刻度下,对平均值和协方差分别进行更新,得到u1,m和um+1,m+n;对于组合,有:
Figure FDA0004069246140000027
Figure FDA0004069246140000028
对更新后的高斯分布参数进行迭代,通过公式可知,若对单位网格内的参数样本进行更新,只需要知道每个单元存储的向量t、矩阵s和点数即可;
Figure FDA0004069246140000029
Figure FDA00040692461400000210
Figure FDA00040692461400000211
Figure FDA00040692461400000212
若经过m次更新对测量值的影响很小,则可将变化后的参数近似于一个固定值;
4-2)更新过程中每个粒子的权重计算
为了计算每个单位网格内粒子观测值的概率密度,需要根据相应粒子的预测值进行投影测量,并将观测值相对于世界坐标系进行坐标转换,将观察值记作czi,同时从静态图中搜索最接近的正态分布cmi,最终使用最大似然估计法评估其概率分布L2
Figure FDA0004069246140000031
其uzi和pzi是czi的平均值和协方差,umi和pmi的协方差是cmi的平均值和协方差,若获得的可能性高于人工给定的阈值,选择属于静态时间刻度的组件,否则用短期局部地图重复该过程,
其中,在静态地图ms和动态地图mt以及观测值zt下,粒子的权重w可以表示为:
Figure FDA0004069246140000032
将当前位置生成的短期局部地图和以前生成的全局地图相匹配,最终导出匹配的表达式。
2.根据权利要求1所述的一种基于Autoware的动态3D点云正态分布AGV定位方法,其特征在于,所述的AGV的后验概率模型,转换为正态分布的形式为:
Figure FDA0004069246140000033
p(xt|ut,zt,m)为后验概率模型,其中xt为AGV的状态量,ut为控制量,zt为所需的观测值,m为此时刻的静态地图,u为所求的均值,Σ为所求得方差;
扫描匹配算法通过比较参考位置处获得的扫描与AGV实际位置处获得的扫描点匹配,获得AGV实际位置和参考位置之间的相对距离和角度,一次更新AGV的位置,AGV的实际位置由航位推算获得,具体步骤如下:
3-1)建立参考点云xi的正态分布变换
首先在局部坐标系中,将给定的环境划分为形状规则的单位网格,并选定单位网格内N个点云数据为xi={x1,x2,…,xN},确定传感器下的观测值为
Figure FDA0004069246140000034
其中Pi为单位网格内单个粒子的位姿,将粒子累积到网格中并进行正态分布转换,得到正态分布转换后的观测值模型可表示为
Figure FDA0004069246140000035
最后得到分段连续的点云概率模型;
经过正态分布变换的单位网格内点云数据的概率密度为:
Figure FDA0004069246140000036
其中概率密度参数均值和方差分别为:
Figure FDA0004069246140000041
Figure FDA0004069246140000042
3-2)点云配准,计算坐标转换后的概率密度之和
坐标变换的参数值为s(p),若概率密度和的值较高,则说明坐标转换的参数较好,点云配准较为准确,其计算公式如下:
Figure FDA0004069246140000043
为实现当前的扫描点在参考扫描面上的最大化,对当前的点进行坐标变换,其中变换参数为j,空间转换函数为T(j,xi),在已知当前时刻的局部坐标系xt和环境地图m下,单元格属性为c,得到观测值的似然函数;
其最大似然估计的形式为
Figure FDA0004069246140000044
转化为负对数似然函数为:
Figure FDA0004069246140000045
对原概率和求解最大化,则可转化为对负对数似然函数求解最小值,通过对坐标转换的概率密度和s(p)进行评估,将求解坐标转换的参数问题转换成对s(p)的优化问题,最终通过优化,不断逼近真实值达到精确定位。
CN201910935403.8A 2019-09-29 2019-09-29 一种基于Autoware的动态3D点云正态分布AGV定位方法 Active CN110689576B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910935403.8A CN110689576B (zh) 2019-09-29 2019-09-29 一种基于Autoware的动态3D点云正态分布AGV定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910935403.8A CN110689576B (zh) 2019-09-29 2019-09-29 一种基于Autoware的动态3D点云正态分布AGV定位方法

Publications (2)

Publication Number Publication Date
CN110689576A CN110689576A (zh) 2020-01-14
CN110689576B true CN110689576B (zh) 2023-04-07

Family

ID=69111026

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910935403.8A Active CN110689576B (zh) 2019-09-29 2019-09-29 一种基于Autoware的动态3D点云正态分布AGV定位方法

Country Status (1)

Country Link
CN (1) CN110689576B (zh)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11852751B2 (en) 2020-03-02 2023-12-26 Beijing Baidu Netcom Science And Technology Co., Ltd. Method, apparatus, computing device and computer-readable storage medium for positioning
CN111581909B (zh) * 2020-04-15 2022-11-15 东南大学 基于改进的自适应重要性采样算法的sram良率评估方法
CN111553937B (zh) * 2020-04-23 2023-11-21 东软睿驰汽车技术(上海)有限公司 激光点云地图构建方法、装置、设备及系统
CN111768493B (zh) * 2020-06-22 2022-08-05 浙江大学 一种基于分布参数编码的点云处理方法
CN112797981B (zh) * 2020-12-24 2024-04-05 长三角哈特机器人产业技术研究院 一种基于正态分布的粒子滤波定位方法
CN113034577A (zh) * 2021-02-20 2021-06-25 意欧斯物流科技(上海)有限公司 一种基于ndt算法的里程计方法
CN113139569B (zh) * 2021-03-04 2022-04-22 山东科技大学 目标分类检测方法、装置与系统
CN113252023A (zh) * 2021-04-26 2021-08-13 深圳市优必选科技股份有限公司 基于里程计的定位方法、装置及设备
CN113516777A (zh) * 2021-05-13 2021-10-19 天讯方舟(北京)信息科技有限公司 一种城市建筑物三维自动建模与可视化方法
CN113536232B (zh) * 2021-06-28 2023-03-21 上海科技大学 用于无人驾驶中激光点云定位的正态分布变换方法
CN113779926A (zh) * 2021-08-03 2021-12-10 深圳天狼芯半导体有限公司 一种电路的检测方法、装置、电子设备及可读存储介质
CN114066773B (zh) * 2021-11-26 2023-10-27 哈尔滨理工大学 一种基于点云特征与蒙特卡洛扩展法的动态物体去除
CN114565616B (zh) * 2022-03-03 2022-12-09 湖南大学无锡智能控制研究院 一种非结构化道路状态参数估计方法及系统
CN115792890B (zh) * 2023-02-06 2023-08-15 成都九洲迪飞科技有限责任公司 基于凝聚量测自适应互联的雷达多目标跟踪方法及系统
CN117523021B (zh) * 2023-11-20 2024-05-28 友上智能科技(苏州)有限公司 基于agv控制系统的快速扫描地图方法
CN117289294B (zh) * 2023-11-27 2024-03-15 睿羿科技(山东)有限公司 一种基于多分辨率贝叶斯网格的融合定位方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104599272A (zh) * 2015-01-22 2015-05-06 中国测绘科学研究院 面向可移动靶标球的机载LiDAR点云与影像联合配准方法
CN106780751A (zh) * 2017-01-19 2017-05-31 桂林电子科技大学 基于改进的屏蔽泊松算法的三维点云重建方法
US9870624B1 (en) * 2017-01-13 2018-01-16 Otsaw Digital Pte. Ltd. Three-dimensional mapping of an environment
CN108564605A (zh) * 2018-04-09 2018-09-21 大连理工大学 一种三维测量点云优化配准方法
WO2019098318A1 (ja) * 2017-11-20 2019-05-23 パナソニック インテレクチュアル プロパティ コーポレーション オブ アメリカ 三次元点群データ生成方法、位置推定方法、三次元点群データ生成装置、および、位置推定装置
CN109949349A (zh) * 2019-01-24 2019-06-28 北京大学第三医院(北京大学第三临床医学院) 一种多模态三维图像的配准及融合显示方法
CN110109134A (zh) * 2019-05-05 2019-08-09 桂林电子科技大学 一种基于2d激光雷达测距的折线提取极大似然估计的方法
WO2019161558A1 (en) * 2018-02-26 2019-08-29 Intel Corporation Method and system of point cloud registration for image processing
CN110285806A (zh) * 2019-07-05 2019-09-27 电子科技大学 基于多次位姿校正的移动机器人快速精确定位算法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2013106319A (ru) * 2013-02-13 2014-08-20 ЭлЭсАй Корпорейшн Основанная на характерных точках надежная регистрация трехмерного твердого тела

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104599272A (zh) * 2015-01-22 2015-05-06 中国测绘科学研究院 面向可移动靶标球的机载LiDAR点云与影像联合配准方法
US9870624B1 (en) * 2017-01-13 2018-01-16 Otsaw Digital Pte. Ltd. Three-dimensional mapping of an environment
CN106780751A (zh) * 2017-01-19 2017-05-31 桂林电子科技大学 基于改进的屏蔽泊松算法的三维点云重建方法
WO2019098318A1 (ja) * 2017-11-20 2019-05-23 パナソニック インテレクチュアル プロパティ コーポレーション オブ アメリカ 三次元点群データ生成方法、位置推定方法、三次元点群データ生成装置、および、位置推定装置
WO2019161558A1 (en) * 2018-02-26 2019-08-29 Intel Corporation Method and system of point cloud registration for image processing
CN108564605A (zh) * 2018-04-09 2018-09-21 大连理工大学 一种三维测量点云优化配准方法
CN109949349A (zh) * 2019-01-24 2019-06-28 北京大学第三医院(北京大学第三临床医学院) 一种多模态三维图像的配准及融合显示方法
CN110109134A (zh) * 2019-05-05 2019-08-09 桂林电子科技大学 一种基于2d激光雷达测距的折线提取极大似然估计的方法
CN110285806A (zh) * 2019-07-05 2019-09-27 电子科技大学 基于多次位姿校正的移动机器人快速精确定位算法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
"针孔"模拟成像下的单航空影像与LiDAR点云配准;吴军等;《遥感学报》;20160125(第01期);全文 *
基于重力约束正态分布变换的室内3维地图重建方法;戚明旭等;《上海交通大学学报》;20180128(第01期);全文 *
融合高斯混合模型和点到面距离的点云配准;林桂潮等;《计算机辅助设计与图形学学报》;20180415(第04期);全文 *

Also Published As

Publication number Publication date
CN110689576A (zh) 2020-01-14

Similar Documents

Publication Publication Date Title
CN110689576B (zh) 一种基于Autoware的动态3D点云正态分布AGV定位方法
CN108320329B (zh) 一种基于3d激光的3d地图创建方法
CN109798896B (zh) 一种室内机器人定位与建图方法及装置
CN110865343B (zh) 基于lmb的粒子滤波检测前跟踪方法及系统
CN112882056B (zh) 基于激光雷达的移动机器人同步定位与地图构建方法
CN107133966B (zh) 一种基于采样一致性算法的三维声纳图像背景分割方法
CN107703496B (zh) 一种交互式多模伯努利滤波的机动弱目标检测前跟踪方法
CN114332348B (zh) 一种融合激光雷达与图像数据的轨道三维重建方法
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
CN113777600A (zh) 一种多毫米波雷达协同定位跟踪方法
CN108871365B (zh) 一种航向约束下的状态估计方法及系统
Leung et al. An improved weighting strategy for rao-blackwellized probability hypothesis density simultaneous localization and mapping
CN116189147A (zh) 一种基于yolo的三维点云低功耗快速目标检测方法
CN113063412B (zh) 一种基于可靠性分析的多机器人协同定位与建图方法
CN111765883B (zh) 机器人蒙特卡罗定位方法、设备及存储介质
CN108520550B (zh) 基于噪声分类与mls的点云数据泊松曲面重建方法
CN113124882A (zh) 一种背景磁场未知情形下的多偶极子磁源反演定位方法
CN109816726B (zh) 一种基于深度滤波器的视觉里程计地图更新方法和系统
CN113191427A (zh) 一种多目标车辆跟踪方法及相关装置
CN117392215A (zh) 一种基于改进amcl和pl-icp点云匹配的移动机器人位姿校正方法
CN116295414A (zh) 基于鲸鱼算法优化的AUV-UFastSLAM算法
CN112612788B (zh) 一种无导航卫星信号下的自主定位方法
Li et al. An SLAM Algorithm Based on Laser Radar and Vision Fusion with Loop Detection Optimization
CN115169136A (zh) 三维空间下快速uk-gmphd多目标跟踪方法
CN115143958A (zh) 一种基于gpu加速的多传感器融合的slam方法

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