CN113587933B - 一种基于分支定界算法的室内移动机器人定位方法 - Google Patents
一种基于分支定界算法的室内移动机器人定位方法 Download PDFInfo
- Publication number
- CN113587933B CN113587933B CN202110863303.6A CN202110863303A CN113587933B CN 113587933 B CN113587933 B CN 113587933B CN 202110863303 A CN202110863303 A CN 202110863303A CN 113587933 B CN113587933 B CN 113587933B
- Authority
- CN
- China
- Prior art keywords
- robot
- point cloud
- pose
- cloud data
- algorithm
- 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
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 83
- 238000000034 method Methods 0.000 title claims abstract description 75
- 238000005457 optimization Methods 0.000 claims abstract description 14
- 238000001514 detection method Methods 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 25
- 230000004927 fusion Effects 0.000 claims description 21
- 230000009466 transformation Effects 0.000 claims description 19
- 238000013519 translation Methods 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 4
- 238000009825 accumulation Methods 0.000 claims description 4
- 238000013461 design Methods 0.000 claims description 4
- 238000009499 grossing Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 description 7
- 230000001131 transforming effect Effects 0.000 description 5
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000013138 pruning Methods 0.000 description 1
- 238000011160 research 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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种基于分支定界算法的室内移动机器人定位方法,通过将深度相机和激光雷达采集的数据进行融合,采用分支定界的方法在离线地图中搜索全局初始位姿,再通过迭代最近点的方法对全局初始位姿进行优化,最后将该全局位姿作为激光雷达扫描匹配的全局初始位姿,之后通过相关性扫描匹配的方法不断计算机器人的当前位姿,该方式能够保证机器人在行进过程中的定位精度,减小机器人的累积误差。另外在激光雷达扫描匹配算法的运行过程中,采用分支定界的方法加速回环检测进行全局优化,当回环检测算法未能及时校正累积的误差时,采用重定位判断方法有效检测误差的累积状态,并通过调用全局定位算法校正累积误差,提高定位过程的准确性。
Description
技术领域
本发明属于移动机器人定位技术领域,具体涉及一种基于分支定界算法的室内移动机器人定位方法。
背景技术
现有的定位技术可分为两大类,绝对定位和相对定位;绝对定位需要预先确定好环境模型或通过传感器直接向机器人提供外接位置信息,计算机器人在全局坐标系中的位置;通常采用的方法是概率定位法、地图匹配法、信标定位法和GPS定位。相对定位则假定机器人初始位姿已知,采用相邻时刻传感器信息对机器人位置进行跟踪估计,相对定位分为里程计法和惯性导航法,该过程会随着机器人行进距离的增加而不断累积误差,需要绝对定位的方式进行修正。
分支定界算法是求解整数规划问题的常用方法,是一种搜索与迭代的方法。其主要流程分为分支和定界两个步骤。分支即把整个可行解空间不断分割成越来越小的子集;定界则是对每个子集内的解集计算一个目标下界(对于最小值问题)。在分支定界算法中,最重要的是剪枝的过程,也就是在每次分枝后,凡是界限超出已知可行解集目标值的那些子集不再进一步分枝。在机器人定位过程中,通过该方法能加快扫描匹配的过程,较快地寻找到机器人位姿的最佳匹配。
在机器人定位研究过程中,人们逐渐采用激光雷达扫描匹配的方式来修正机器人的累积误差,这种方式通过当前帧和前一帧或者几帧的扫描结果相匹配,从而确定机器人在环境中的位姿,能有效减小定位过程的累积误差,但该过程与激光雷达的精度有着很大的关系,另外就是环境的特征需要足够丰富,才能准确的进行匹配,但参与匹配的特征越多,则会导致算法时间复杂度提高。
发明内容
基于上述问题,本发明旨在提出一种基于分支定界算法的室内移动机器人定位方法,解决单一传感器定位精度受限的问题,提高室内移动机器人全局定位的能力和相对定位的准确性。
为实现上述目的,本发明提供的一种基于分支定界算法的室内移动机器人定位方法,包括:
步骤1:采集机器人周围环境的点云数据,并将点云数据转换到机器人本体坐标系下;
步骤2:将预先构建好的整个环境的离线地图转换为多层不同分辨率的栅格地图;
步骤3:将融合后的点云数据作为分支定界算法的输入,通过深度优先搜索的方式,在搜索树中查找具有最高得分的叶子节点作为机器人的全局初始位姿;
步骤4:将当前时刻得到的点云数据作为输入点云,离线地图作为目标点云,以机器人的全局初始位姿作为迭代最近点算法的初始位姿,通过迭代最近点算法得到准确的机器人全局位姿;
步骤5:采集机器人当前时刻的位姿信息作为位姿估计值,利用相关性扫描匹配算法得到机器人前进过程中的相对位姿;
步骤6:通过分支定界算法以深度优先搜索的方式加快回环检测的速度,在构建完成的所有子图中寻找当前机器人的相似匹配,采用非线性优化库优化位姿图;
步骤7:每间隔一定的时间判断机器人累积误差的大小,如果超过设定阈值,则返回执行步骤3来校正累积误差,并输出校正之后的机器人位姿。
所述步骤1包括:
步骤1.1:分别通过深度相机、激光雷达采集当前时刻机器人所在位置周围环境的两组点云数据;
步骤1.2:将深度相机采集到的点云数据进行去噪处理;
步骤1.3:在去噪后的深度相机点云数据中截取具有一定高度的数据点,并将该点云数据与激光雷达采集到的点云数据分别投影到机器人本体坐标系下,实现点云数据的融合。
所述步骤2具体表述为:从原始分辨率的栅格地图开始,每层对应一个高度值h,原始分辨率地图对应高度h=0,逐层递增该高度值,在高度为h的栅格地图中每个栅格概率值为原始分辨率地图对应栅格周围2h*2h个栅格概率的最大值。
所述步骤3包括:
步骤3.1:确定节点选择策略,采用深度优先搜索方式确定节点选择策略;
步骤3.2:确定分支规则,对于树中每个节点用一个整数数组c=(x,y,θ,ch)表示,节点所在树中的高度用ch表示,将树中满足ch>1的节点c分成4个高度为ch-1的子节点:
其中,表示设置一定搜索窗口形成的有限集合:
其中,sx、sy、sθ分别代表平移方向和旋转方向的边界搜索范围,Sx、Sy、Sα分别表示x方向线性搜索窗口、y方向线性搜索窗口和角度搜索窗口,r表示栅格地图的分辨率,εθ表示角度步长因子,εθ表示为:
其中,hmax表示一帧激光雷达数据所有击中点中测量得到的距离最大值;
步骤3.3:计算分支定界算法的上界:
式中,表示提前构建的由离线地图转换成的对应高度ch的栅格地图;score(c)为将当前激光雷达数据投影至栅格/>上的概率值之和,/>为节点c对应的位姿,si为激光雷达扫描得到的击中点,K为一帧激光雷达数据中击中点的数量;
步骤3.4:通过深度优先搜素的方式在每层中递归搜索每个节点的上界,将具有最佳得分的叶子节点作为机器人位姿的最佳匹配,从而得到机器人全局初始位姿:
ξglobal=ξ0+(rx,ry,εθθ)
式中,ξ0表示开始搜索时的初始位姿,ξglobal表示搜索结束得到的全局初始位姿。
所述步骤4包括:
步骤4.1:将融合后的点云数据转换到世界坐标系下;
步骤4.2:在世界坐标系下计算两组点云数据的质心:
式中,μp、μq分别表示通过融合后的点云数据P的质心、离线地图的点云数据Q的质心,Np、Nq分别表示点云数据P中的点数、点云数据Q中的点数,pi、qj分别表示点云数据P中第i个点的坐标、点云数据Q中第j个点的坐标;
步骤4.3:设计迭代最近点算法中的误差函数E(R,t);
式中,R为两组点云集变换对应的旋转矩阵,t为平移矩阵,N为参与点云变换的点数;
步骤4.4:利用迭代最近点算法迭代求解R和t,当达到收敛条件时停止迭代运算,输出的最优解作为准确的机器人全局位姿。
所述步骤5包括:
步骤5.1:通过惯性传感器采集机器人的线加速度和角速度,通过轮式里程计采集机器人的速度,将两组数据通过扩展卡尔曼滤波融合之后的数据作为机器人当前位置的位姿估计值;
步骤5.2:根据机器人当前位姿估计值,将当前时刻激光雷达数据转换到当前正在构建的子图中;
步骤5.3:设计相关性扫描匹配算法中的误差函数为:
式中,Hsmooth是双三次插值函数,用于对子图中栅格的概率值进行平滑操作,Tε表示位姿估计对应的变换矩阵,si′表示通过激光雷达采集的第i′帧数据,K为第i′帧激光雷达数据包含的点数;
步骤5.4:利用相关性扫描匹配算法迭代优化误差函数,当满足收敛条件时停止迭代计算,输出值作为当前位姿估计的最优解;
步骤5.5:更新子图中栅格的概率值:
Hnew(x)=clamp(F-1(F(Hold(x))*F(p)))
式中,clamp为区间限定函数,p为栅格被占据的概率,F(p)为栅格占据概率与空闲概率的比值,Hold(x)为更新之前的栅格概率值,Hnew(x)为更新之后的栅格概率值。
所述步骤6包括:
步骤6.1:将当前位姿估计的最优解作为分支定界算法的输入,与构建完成的所有子图进行搜索匹配,通过深度优先搜索的方式得到机器人在所有子图中的相似匹配;
步骤6.2:将当前机器人位姿的相似匹配作为回环约束加入位姿图中优化,全局位姿图优化的误差函数设计为:
式中,为第i个子图对应的位姿,/>为第j帧激光数据对应的位姿,∑ij为第i个子图位姿与第j帧激光雷达位姿之间的协方差,ξij为第i个子图与第j帧激光雷达的相对位姿,δ为鲁棒核函数。
所述步骤7包括:
步骤7.1:根据优化过后的机器人相对位姿,将融合点云数据转换至地图坐标系;
步骤7.2:计算融合后的点云数据在对应原始分辨率的栅格地图上的索引:
Ix=(xtrans-xorigin-0.5*r)/r+1
Iy=(ytrans-yorigin-0.5*r)/r+1
式中,Ix为地图坐标系x轴方向的索引,Iy为地图坐标系y轴方向的索引,xtrans为经过变换的融合点云x方向平移量,ytrans为经过变换的融合点云y方向平移量,xorigin为栅格地图的原点x坐标,yorigin为栅格地图的原点y坐标,r为栅格地图分辨率;
步骤7.3:计算融合后的点云数据与栅格地图的匹配概率Poccupy;
Poccupy=ic/(ic+ie)
式中,ic为所有点云数据中匹配正确的个数,ie为所有点云数据中匹配错误的个数;
步骤7.4:根据设定阈值判断是否进行重定位,当Poccupy小于等于设定阈值时,则认为机器人当前时刻的位姿已经有了一定程度的累积误差,因此需要返回执行步骤3来校正累积误差;
步骤7.5:每间隔一定时间,重复执行步骤7.1~步骤7.5来判断误差累积状态,保证机器人在前进过程中的定位准确性。
本发明的有益效果是:
本发明提出了一种基于分支定界算法的室内移动机器人定位方法,1)通过分支定界方法加快全局定位收敛速度,并通过迭代最近点算法实现机器人全局位姿的准确估计,整体方案通过两阶段的匹配算法不仅实现全局定位的实时性,还能保证算法的定位精度;2)采用多种传感器融合进行定位,解决单一传感器定位失效的问题,利用多种传感器之间的互补特性,减小定位误差的累积,增加算法的定位精度,此外,本发明采用的传感器成本较低,定位算法对硬件平台要求较低;3)在机器人相对定位算法中增加机器人重定位判断方法,能够及时校正机器人累积误差,提高机器人定位精度;4)采用绝对定位与相对定位结合的方式,形成完整的室内移动机器人定位方案,能够实现机器人在环境的初始定位以及位置跟踪,提高算法整体的***性。
附图说明
图1为本发明中基于分支定界算法的室内移动机器人定位方法流程图;
图2为发明中基于分支定界算法的室内移动机器人定位方法原理图;
图3为本发明中采用的分支定界算法流程图;
图4为本发明中相对定位流程图;
图5为本发明中全局定位流程图。
具体实施方式
下面结合附图和具体实施实例对发明做进一步说明。本发明提供了一种基于分支定界算法的室内移动机器人定位方法,机器人安装的传感器包括:深度相机、2D激光雷达、惯性传感器、轮式里程计,其中深度相机安装在机器人的正前方,用来采集机器人正前方的环境图像;2D激光雷达安装在机器人的正上方,用来采集周围环境的点云数据;惯性传感器安装在机器人的正上方,用来采集机器人的线加速度和角速度;轮式里程计安装在机器人的车轮上,用来采集机器人的速度;将深度相机和激光雷达采集的数据进行融合,采用分支定界的方法在离线地图中搜索全局初始位姿,再通过迭代最近点的方法对全局初始位姿进行优化,该方法不仅保证了全局定位的准确性,而且能够有效减小全局定位的定位时间。最后将该全局位姿作为激光雷达扫描匹配的全局初始位姿,之后通过相关性扫描匹配的方式不断计算机器人的当前位姿,该方式能够保证机器人在行进过程中的定位精度,减小机器人的累积误差。另外在激光雷达扫描匹配算法的运行过程中,采用分支定界的方法加速回环检测进行全局优化,当回环检测算法未能及时校正累积误差时,采用误差判断方法有效检测误差的累积状态,及时重定位减小误差,提高定位过程的准确性。
如图1~2所示,一种基于分支定界算法的室内移动机器人定位方法,包括:
获取深度相机点云数据,并截取高度z=0的点云数据投影至机器人本体坐标系,获取激光雷达扫描数据,转换为点云数据投影至机器人本体坐标系,将以上两种机器人本体坐标系上的点云数据进行融合;
选取具有一定高度的点云数据,截取在该高度平面内的点云,根据深度相机和机器人本体坐标系之间的静态坐标变换能够得到相机坐标系到机器人本体坐标系的变换矩阵,将截取的点云通过该变换矩阵,转换到机器人本体坐标系下;
2D激光雷达数据通常以极坐标表示,而点云数据为三维数据,因此需要将激光雷达数据转换成空间直角坐标系下的坐标表示,并把z轴坐标设置成0,将转换之后的点云数据投影至机器人本体坐标系。
步骤1:采集机器人周围环境的点云数据,并将点云数据转换到机器人本体坐标系下;包括:
步骤1.1:分别通过深度相机、激光雷达采集当前时刻机器人所在位置周围环境的两组点云数据;
通过深度相机获得的点云需要进行去噪处理,由于深度相机获取的原始点云数据包含部分噪声、离群点,存在点云密度不规则等问题,需要对原始点云数据进行体素滤波,体素滤波会对输入点云创建一个三维体素网格,每个体素内用体素中所有点的重心来近似表示体素中其他点,这样该体素内所有点都用一个重心点最终表示,该方法能在下采样的过程中保留原始点云的形状。
步骤1.2:将深度相机采集到的点云数据(简称深度相机点云数据)进行去噪处理;具体采用体素滤波方式进行去噪处理;
步骤1.3:在去噪后的深度相机点云数据中截取具有一定高度(z=0)的数据点,并将该点云数据与激光雷达采集到的点云数据分别投影到机器人本体坐标系下,实现点云数据的融合;
根据深度相机坐标系和机器人坐标系之间的静态坐标变换能够得到相机坐标系到机器人坐标系的变换矩阵,将截取的点云通过该变换矩阵,转换到机器人本体坐标系下;转换的公式如下:
Pb=Rbc*Pc+tbc
其中,Pb为变换到机器人坐标系下的点云,Rbc为相机坐标系到机器人坐标系的旋转矩阵,Pc为相机坐标系下的点云,tbc为相机坐标系到机器人坐标系的平移矩阵。
然后,将激光雷达获得的数据(简称激光雷达数据)转换成点云数据,并投影至机器人坐标系下和深度相机获得的点云数据进行融合;
2D激光雷达获得的数据通常以极坐标表示,而点云数据为三维数据,因此需要将2D激光雷达获得的数据转换成空间直角坐标系下的坐标表示,并把z轴坐标设置成0,转换公式如下:
θcur=θmin+i*Δθ
x=R*cosθcur
y=R*sinθcur
z=0
其中,θmin为激光雷达开始扫描的角度,在本实施例中取θmin=0,Δθ表示激光雷达旋转的角分辨率,在本实施例中取Δθ=0.33°,i表示激光雷达从扫描开始到当前位置的第i个点,R表示当前位置激光雷达扫描点返回的距离值,θcur表示当前位置和开始扫描的位置相差的角度,x、y和z分别表示转换成点云数据的三个维度的坐标。
将转换之后的点云数据投影至机器人坐标系,投影的公式如下所示:
Pb=Rbl*Pl+tbl
其中,Pb为变换到机器人坐标系下的点云,Rbl为激光雷达坐标系到机器人坐标系的旋转矩阵,Pl为激光雷达坐标系下的点云,tbl为激光雷达坐标系到机器人坐标系的平移矩阵。
步骤2:将预先构建好的整个环境的离线地图转换为多层不同分辨率的栅格地图;具体表述为:从原始分辨率的栅格地图开始,每层对应一个高度值h,原始分辨率地图对应高度h=0,逐层递增该高度值,在高度为h的栅格地图中每个栅格概率值为原始分辨率地图对应栅格周围2h*2h个栅格概率的最大值。
将融合后的点云数据作为分支定界算法的输入,与离线建立的地图进行搜索匹配,分支定界算法将离线地图转换成六层不同分辨率的栅格地图,通过深度优先搜索的方式得到机器人的全局初始位姿。如图3所示,具体来说,分支定界算法的原理是将问题所有可能的解集表示为树中的节点,其中根节点表示为所有可能的解,每个节点的子节点构成父节点的一个子集,叶子节点为单体结构,表示问题的一个可行解。在本发明中通过深度优先搜索快速评估很多叶子节点,将得分低于最优分数的节点进行剪枝,快速找到最优匹配。
步骤3:将融合后的点云数据作为分支定界算法的输入,通过深度优先搜索的方式,在搜索树中查找每一层具有最高得分的节点,并将具有最高得分的叶子节点作为最佳匹配结果,根据该结果可以得到机器人的全局初始位姿;包括:
步骤3.1:确定节点选择策略,采用深度优先搜索方式确定节点选择策略;该算法能够快速评估很多节点,并对不满足条件的节点快速剪枝,提高分支定界算法的运算效率。
步骤3.2:确定分支规则,对于树中每个节点用一个整数数组c=(x,y,θ,ch)表示,节点所在树中的高度用ch表示,将树中满足ch>1的节点c分成4个高度为ch-1的子节点:
其中,表示设置一定搜索窗口形成的有限集合,取Sx=30Sy=30Sθ=180°:
其中,sx、sy、sθ分别代表平移方向和旋转方向的边界搜索范围,Sx、Sy、Sθ分别表示x方向线性搜索窗口、y方向线性搜索窗口和角度搜索窗口,r表示栅格地图的分辨率,r=0.05,εθ表示角度步长因子,εθ表示为:
其中,hmax表示一帧激光雷达数据所有击中点中测量得到的距离最大值;
步骤3.3:计算分支定界算法的上界:
式中,N(xi,yi)是双三次插值函数,表示提前构建的由离线地图转换成的对应高度ch的栅格地图;score(c)为将当前激光雷达数据投影至栅格/>上的概率值之和,/>为节点c对应的位姿,si为激光雷达扫描得到的击中hit点,K为一帧激光雷达数据中击中点的数量;栅格/>存储的是大小为2h×2h的像素框中的栅格概率最大值,通过预先计算栅格的方式可以快速计算激光扫描匹配的得分。
步骤3.4:通过深度优先搜素的方式在每层中递归搜索每个节点的上界,将具有最佳得分的叶子节点作为机器人位姿的最佳匹配,从而得到机器人全局初始位姿:
ξglobal=ξ0+(rx,ry,εθθ)
式中,ξ0表示开始搜索时的初始位姿,ξglobal表示搜索结束得到的全局初始位姿。
步骤4:将当前时刻得到的点云数据作为输入点云,离线地图作为目标点云,以机器人的全局初始位姿作为迭代最近点算法的初始位姿,通过迭代最近点算法优化该初始位姿,得到准确的机器人全局位姿;包括:
步骤4.1:将融合后的点云数据转换到世界坐标系下,变换公式如下所示:
Pw=Rwb*Pb+twb
其中,Pw为变换到世界坐标系下的点云,Rwb为机器人坐标系到世界坐标系的旋转矩阵,Pb为机器人坐标系下的点云,twb为机器人坐标系到世界坐标系的平移矩阵。
步骤4.2:在世界坐标系下计算两组点云数据的质心:
待转换的点云构成点集:
P={p1,p2,…,pM}
目标点云构成点集:
Q={q1,q2,…,qN}
假设P和Q中点数均为Np,则两组点集的质心分别为:
式中,μp、μq分别表示通过融合后的点云数据P的质心、离线地图的点云数据Q的质心,Np、Nq分别表示点云数据P中的点数、点云数据Q中的点数,pi、qj分别表示点云数据P中第i个点的坐标、点云数据Q中第j个点的坐标;
步骤4.3:设计迭代最近点算法中的误差函数E(R,t);
式中,R为两组点云集变换对应的旋转矩阵,t为平移矩阵,N为参与点云变换的点数;
步骤4.4:利用迭代最近点算法迭代求解R和t,当达到收敛条件时停止迭代运算,输出的最优解作为准确的机器人全局位姿;当达到下列收敛条件之一时停止迭代运算:
1)当迭代次数达到最大迭代次数,则认为算法已经收敛;
2)当前一次变换矩阵与此次变换矩阵之间的差值小于设定阈值,则认为算法已经收敛;
3)当前后两次迭代的点对的欧氏距离均方误差之和小于设定阈值,则认为算法已经收敛。
此时得到的旋转矩阵和平移矩阵能够作为机器人准确的全局初始位姿,当机器人进行持续的位置跟踪时,会将其作为相对定位的初始位姿,整个全局定位算法由步骤3和步骤4组成,如图5所示。
根据得到的机器人全局位姿确定机器人在环境中的绝对位置,之后以轮式里程计和惯性传感器(简称imu)采集到的数据通过扩展卡尔曼滤波之后的数据作为机器人当前位姿估计值,通过相关性扫描匹配算法得到机器人前进过程的相对位姿。
步骤5:采集机器人当前时刻的位姿信息作为位姿估计值,利用相关性扫描匹配算法得到机器人前进过程中的相对位姿;包括:
步骤5.1:通过惯性传感器采集机器人的线加速度和角速度,通过轮式里程计采集机器人的速度,将两组数据通过扩展卡尔曼滤波融合之后的数据作为机器人当前位置的位姿估计值;
步骤5.2:根据机器人当前位置的位姿估计值,将当前时刻激光雷达获取的数据转换到当前正在构建的子图中;
转换公式如下所示:
其中,pm表示转换到子图中的激光雷达数据,ps表示当前时刻激光雷达数据,θ表示位姿估计的旋转角,x和y分别表示位姿估计的x方向偏移量和y方向偏移量。
步骤5.3:设计相关性扫描匹配算法中的误差函数为:
式中,Hsmooth是双三次插值函数,用于对子图中栅格的概率值进行平滑操作,Tε表示位姿估计对应的变换矩阵,si′表示通过激光雷达采集的第i′帧数据,K为第i′帧激光雷达数据包含的点数;
步骤5.4:利用相关性扫描匹配算法迭代优化误差函数,当满足收敛条件时停止迭代计算,输出值作为当前位姿估计的最优解;当达到下列收敛条件之一时停止迭代计算:
1)当达到设定的最大迭代次数时,认为算法已经收敛;
2)当前后两次变换矩阵之间的差值小于设定阈值时,认为算法已经收敛。
步骤5.5:更新子图中栅格的概率值:当一帧激光雷达数据***子图中时,对之前没有观察过的网格点会分配一个概率值,而之前观察过的网格点则会更新其概率值;
Hnew(x)=clamp(F-1(F(Hold(x))*F(p)))
式中,clamp为区间限定函数,p为栅格被占据的概率,F(p)为栅格占据概率与空闲概率的比值,Hold(x)为更新之前的栅格概率值,Hnew(x)为更新之后的栅格概率值。
步骤6:通过分支定界算法以深度优先搜索的方式加快回环检测的速度,在构建完成的所有子图中寻找当前机器人的相似匹配,最后将该相似匹配加入闭环约束中,采用非线性优化库优化位姿图;包括:
步骤6.1:将当前位姿估计的最优解作为分支定界算法的输入,与构建完成的所有子图进行搜索匹配,通过深度优先搜索的方式得到机器人在所有子图中的相似匹配;
步骤6.2:将当前机器人位姿的相似匹配作为回环约束加入位姿图中优化,全局位姿图优化的误差函数设计为:
式中,为第i个子图对应的位姿,/>为第j帧激光数据对应的位姿,∑ij为第i个子图位姿与第j帧激光雷达位姿之间的协方差,ξij为第i个子图与第j帧激光雷达的相对位姿,δ为鲁棒核函数,E(·)表示残差函数,其计算公式如下所示:
步骤7:每间隔一定的时间,判断机器人累积误差的大小,如果超过设定阈值,则返回执行步骤3来校正累积误差,并输出校正之后的机器人位姿;判断机器人累积误差的过程,主要原理是将融合点云数据通过坐标变换至世界坐标系下,并计算变换之后的激光雷达数据中每个点对应于具体栅格的索引,查找离线地图相应位置的概率值,如果离线地图中该位置概率值表示该位置未知或者未占据,则更新不匹配点的个数,否则更新匹配点个数;具体包括:
步骤7.1:根据优化过后的机器人相对位姿,将融合点云数据转换至地图坐标系,转换公式如下所示:
Pw=Rwb*Pb+twb
其中,Pw为变换到世界坐标系下的点云,Rwb为机器人坐标系到世界坐标系的旋转矩阵,Pb为机器人坐标系下的点云,twb为机器人坐标系到世界坐标系的平移矩阵;
步骤7.2:计算融合后的点云数据在对应原始分辨率的栅格地图上的索引,加载离线地图时会将地图每个位置的索引以及对应的概率值保存在一个数组中,以供匹配过程查找,而融合点云在栅格地图索引计算公式如下所示:
Ix=(xtrans-xoriggin-0.5*r)/r+1
Iy=(ytrans-yorigin-0.5*r)/r+1
式中,Ix为地图坐标系x轴方向的索引,Iy为地图坐标系y轴方向的索引,xtrans为经过变换的融合点云x方向平移量,ytrans为经过变换的融合点云y方向平移量,xorigin为栅格地图的原点x坐标,yorigin为栅格地图的原点y坐标,r为栅格地图分辨率,r=0.05;
步骤7.3:计算融合后的点云数据与栅格地图的匹配概率Poccupy;融合后的点云数据落在栅格地图正确位置上时,会递增匹配正确计数值,而当融合后的点云数据与栅格地图原始栅格占据情况不一致时,则会递增错误匹配计数值,最后匹配概率计算公式如下所示:
Poccupy=ic/(ic+ie)
式中,ic为所有点云数据中匹配正确的个数,ie为所有点云数据中匹配错误的个数;
步骤7.4:根据设定阈值判断是否进行重定位,当Poccupy小于等于设定阈值时,则认为机器人当前时刻的位姿已经有了一定程度的累积误差,因此需要返回执行步骤3来校正累积误差;
步骤7.5:每间隔一定时间(ΔT=5s),重复执行步骤7.1~步骤7.5来判断误差累积状态,保证机器人在前进过程中的定位准确性,步骤5~步骤7是机器人行进过程中相对位姿的定位过程,其设计原理图如图4所示。
为了验证本发明的有效性,对该发明的定位效果进行了仿真实验和室内实际环境测试实验,主要进行全局定位和相对定位两部分测试实验,主要测试机器人在已知先验地图的情况下,对其在地图中任意位置的准确估计能力,测试环境分为仿真环境和实际环境,分别进行20次相同环境的测试,测试评估算法的运行效率和准确性,测试结果如下,表中,trans_x、trans_y、trans_z分别表示x方向平移误差,y方向平移误差和z方向平移误差,ang_x、ang_y、ang_z分别表示x方向旋转角度误差,y方向旋转角度误差和z方向旋转角度误差:
表1全局定位精度对比表格
表2全局定位效率对比表格
表3相对定位精度对比表格
根据以上实验数据说明,本发明提出的基于分支定界算法的室内机器人定位方法在全局定位和相对定位两个方面均取得不错的精度;出于实际工程需要,机器人定位算法必须能够进行持续定位,因此实时性是必要条件,因此未对相对定位算法的运行效率进行评估,本实验涉及的算法均能够进行相对局部定位。
Claims (5)
1.一种基于分支定界算法的室内移动机器人定位方法,其特征在于,包括:
步骤1:采集机器人周围环境的点云数据,并将点云数据转换到机器人本体坐标系下;
步骤2:将预先构建好的整个环境的离线地图转换为多层不同分辨率的栅格地图;
步骤3:将融合后的点云数据作为分支定界算法的输入,通过深度优先搜索的方式,在搜索树中查找具有最高得分的叶子节点作为机器人的全局初始位姿;
步骤4:将当前时刻得到的点云数据作为输入点云,离线地图作为目标点云,以机器人的全局初始位姿作为迭代最近点算法的初始位姿,通过迭代最近点算法得到准确的机器人全局位姿;
步骤5:采集机器人当前时刻的位姿信息作为位姿估计值,利用相关性扫描匹配算法得到机器人前进过程中的相对位姿;
步骤6:通过分支定界算法以深度优先搜索的方式加快回环检测的速度,在构建完成的所有子图中寻找当前机器人的相似匹配,采用非线性优化库优化位姿图;
步骤7:每间隔一定的时间判断机器人累积误差的大小,如果超过设定阈值,则返回执行步骤3来校正累积误差,并输出校正之后的机器人位姿;
所述步骤2具体表述为:从原始分辨率的栅格地图开始,每层对应一个高度值h,原始分辨率地图对应高度h=0,逐层递增该高度值,在高度为h的栅格地图中每个栅格概率值为原始分辨率地图对应栅格周围2h*2h个栅格概率的最大值;
所述步骤3包括:
步骤3.1:确定节点选择策略,采用深度优先搜索方式确定节点选择策略;
步骤3.2:确定分支规则,对于树中每个节点用一个整数数组c=(x,y,θ,ch)表示,节点所在树中的高度用ch表示,将树中满足ch>1的节点c分成4个高度为ch-1的子节点:
其中,表示设置一定搜索窗口形成的有限集合:
其中,sx、sy、sθ分别代表平移方向和旋转方向的边界搜索范围,Sx、Sy、Sθ分别表示x方向线性搜索窗口、y方向线性搜索窗口和角度搜索窗口,r表示栅格地图的分辨率,εθ表示角度步长因子,εθ表示为:
其中,hmax表示一帧激光雷达数据所有击中点中测量得到的距离最大值;
步骤3.3:计算分支定界算法的上界:
式中,表示提前构建的由离线地图转换成的对应高度ch的栅格地图;score(c)为将当前激光雷达数据投影至栅格/>上的概率值之和,/>为节点c对应的位姿,si为激光雷达扫描得到的击中点,K为一帧激光雷达数据中击中点的数量;
步骤3.4:通过深度优先搜素的方式在每层中递归搜索每个节点的上界,将具有最佳得分的叶子节点作为机器人位姿的最佳匹配,从而得到机器人全局初始位姿:
ξglobal=ξ0+(rx,ry,εθθ)
式中,ξ0表示开始搜索时的初始位姿,ξglobal表示搜索结束得到的全局初始位姿;
所述步骤6包括:
步骤6.1:将当前位姿估计的最优解作为分支定界算法的输入,与构建完成的所有子图进行搜索匹配,通过深度优先搜索的方式得到机器人在所有子图中的相似匹配;
步骤6.2:将当前机器人位姿的相似匹配作为回环约束加入位姿图中优化,全局位姿图优化的误差函数设计为:
式中,为第i个子图对应的位姿,/>为第j帧激光数据对应的位姿,∑ij为第i个子图位姿与第j帧激光雷达位姿之间的协方差,ξij为第i个子图与第j帧激光雷达的相对位姿,δ为鲁棒核函数,E(·)表示残差函数。
2.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法,其特征在于,所述步骤1包括:
步骤1.1:分别通过深度相机、激光雷达采集当前时刻机器人所在位置周围环境的两组点云数据;
步骤1.2:将深度相机采集到的点云数据进行去噪处理;
步骤1.3:在去噪后的深度相机点云数据中截取具有一定高度的数据点,并将该点云数据与激光雷达采集到的点云数据分别投影到机器人本体坐标系下,实现点云数据的融合。
3.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法,其特征在于,所述步骤4包括:
步骤4.1:将融合后的点云数据转换到世界坐标系下;
步骤4.2:在世界坐标系下计算两组点云数据的质心:
式中,μp、μq分别表示通过融合后的点云数据P的质心、离线地图的点云数据Q的质心,Np、Nq分别表示点云数据P中的点数、点云数据Q中的点数,pi、qj分别表示点云数据P中第i个点的坐标、点云数据Q中第j个点的坐标;
步骤4.3:设计迭代最近点算法中的误差函数E(R,t);
式中,R为两组点云集变换对应的旋转矩阵,t为平移矩阵,N为参与点云变换的点数;
步骤4.4:利用迭代最近点算法迭代求解R和t,当达到收敛条件时停止迭代运算,输出的最优解作为准确的机器人全局位姿。
4.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法,其特征在于,所述步骤5包括:
步骤5.1:通过惯性传感器采集机器人的线加速度和角速度,通过轮式里程计采集机器人的速度,将两组数据通过扩展卡尔曼滤波融合之后的数据作为机器人当前位置的位姿估计值;
步骤5.2:根据机器人当前位置的位姿估计值,将当前时刻激光雷达获取的数据转换到当前正在构建的子图中;
步骤5.3:设计相关性扫描匹配算法中的误差函数为:
式中,Hsmooth是双三次插值函数,用于对子图中栅格的概率值进行平滑操作,Tε表示位姿估计对应的变换矩阵,si,表示通过激光雷达采集的第i′帧数据,K为第i′帧激光雷达数据包含的点数;
步骤5.4:利用相关性扫描匹配算法迭代优化误差函数,当满足收敛条件时停止迭代计算,输出值作为当前位姿估计的最优解;
步骤5.5:更新子图中栅格的概率值;
Hnew(x)=clamp(F-1(F(Hold(x))*F(p)))
式中,clamp为区间限定函数,p为栅格被占据的概率,F(p)为栅格占据概率与空闲概率的比值,Hold(x)为更新之前的栅格概率值,Hnew(x)为更新之后的栅格概率值。
5.根据权利要求1所述的一种基于分支定界算法的室内移动机器人定位方法,其特征在于,所述步骤7包括:
步骤7.1:根据优化过后的机器人相对位姿,将融合点云数据转换至地图坐标系;
步骤7.2:计算融合后的点云数据在对应原始分辨率的栅格地图上的索引:
Ix=(xtrans-xorigin-0.5*r)/r+1
Iy=(ytrans-yorigin-0.5*r)/r+1
式中,Ix为地图坐标系x轴方向的索引,Iy为地图坐标系y轴方向的索引,xtrans为经过变换的融合点云x方向平移量,ytrans为经过变换的融合点云y方向平移量,xorigin为栅格地图的原点x坐标,yorigin为栅格地图的原点y坐标,r为栅格地图分辨率;
步骤7.3:计算融合后的点云数据与栅格地图的匹配概率Poccupy:
Poccupy=ic/(ic+ie)
式中,ic为所有点云数据中匹配正确的个数,ie为所有点云数据中匹配错误的个数;
步骤7.4:根据设定阈值判断是否进行重定位,当Poccupy小于等于设定阈值时,则认为机器人当前时刻的位姿已经有了一定程度的累积误差,因此需要返回执行步骤3来校正累积误差;
步骤7.5:每间隔一定时间,重复执行步骤7.1~步骤7.5来判断误差累积状态,保证机器人在前进过程中的定位准确性。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110863303.6A CN113587933B (zh) | 2021-07-29 | 2021-07-29 | 一种基于分支定界算法的室内移动机器人定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110863303.6A CN113587933B (zh) | 2021-07-29 | 2021-07-29 | 一种基于分支定界算法的室内移动机器人定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113587933A CN113587933A (zh) | 2021-11-02 |
CN113587933B true CN113587933B (zh) | 2024-02-02 |
Family
ID=78251753
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110863303.6A Active CN113587933B (zh) | 2021-07-29 | 2021-07-29 | 一种基于分支定界算法的室内移动机器人定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113587933B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114279434B (zh) * | 2021-12-27 | 2024-06-14 | 驭势科技(北京)有限公司 | 一种建图方法、装置、电子设备和存储介质 |
CN114593737A (zh) * | 2022-03-11 | 2022-06-07 | 美智纵横科技有限责任公司 | 控制方法、装置、机器人及存储介质 |
CN115950427B (zh) * | 2022-12-23 | 2024-02-09 | 合肥中科深谷科技发展有限公司 | 一种基于icp和遗传优化相结合的机器人导航扫描匹配算法 |
CN115880673B (zh) * | 2023-02-22 | 2023-05-26 | 西南石油大学 | 一种基于计算机视觉的避障方法及*** |
CN117031481A (zh) * | 2023-08-14 | 2023-11-10 | 北京数字绿土科技股份有限公司 | 一种基于投影3d激光点云的移动机器人重定位方法及*** |
CN117330083B (zh) * | 2023-12-01 | 2024-04-19 | 深圳市好奇心探索科技有限公司 | 机器人定位方法、机器人和可读存储介质 |
Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
CN110163968A (zh) * | 2019-05-28 | 2019-08-23 | 山东大学 | Rgbd相机大型三维场景构建方法及*** |
KR102029850B1 (ko) * | 2019-03-28 | 2019-10-08 | 세종대학교 산학협력단 | 카메라와 라이다 센서를 이용한 객체 검출 장치 및 그 방법 |
CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
CN111540005A (zh) * | 2020-04-21 | 2020-08-14 | 南京理工大学 | 基于二维栅格地图的回环检测方法 |
CN111693047A (zh) * | 2020-05-08 | 2020-09-22 | 中国航空工业集团公司西安航空计算技术研究所 | 一种高动态场景下的微小型无人机视觉导航方法 |
AU2020101833A4 (en) * | 2019-12-03 | 2020-09-24 | Wuhan University Of Science And Technology | Laser slam method based on subgraph merging and pose optimization |
WO2020237693A1 (zh) * | 2019-05-31 | 2020-12-03 | 华南理工大学 | 一种水面无人装备多源感知方法及*** |
CN112258618A (zh) * | 2020-11-04 | 2021-01-22 | 中国科学院空天信息创新研究院 | 基于先验激光点云与深度图融合的语义建图与定位方法 |
CN112258600A (zh) * | 2020-10-19 | 2021-01-22 | 浙江大学 | 一种基于视觉与激光雷达的同时定位与地图构建方法 |
US10962630B1 (en) * | 2019-10-18 | 2021-03-30 | Toyota Research Institute, Inc. | System and method for calibrating sensors of a sensor system |
CN112785702A (zh) * | 2020-12-31 | 2021-05-11 | 华南理工大学 | 一种基于2d激光雷达和双目相机紧耦合的slam方法 |
CN112985416A (zh) * | 2021-04-19 | 2021-06-18 | 湖南大学 | 激光与视觉信息融合的鲁棒定位和建图方法及*** |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7756631B2 (en) * | 2006-08-31 | 2010-07-13 | Raytheon Company | Method for realtime scaling of the vehicle routing problem |
US8396730B2 (en) * | 2011-02-14 | 2013-03-12 | Raytheon Company | System and method for resource allocation and management |
GB201804395D0 (en) * | 2018-03-19 | 2018-05-02 | Tomtom Navigation Bv | Methods and systems for generating parking routes |
-
2021
- 2021-07-29 CN CN202110863303.6A patent/CN113587933B/zh active Active
Patent Citations (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR102029850B1 (ko) * | 2019-03-28 | 2019-10-08 | 세종대학교 산학협력단 | 카메라와 라이다 센서를 이용한 객체 검출 장치 및 그 방법 |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
CN110163968A (zh) * | 2019-05-28 | 2019-08-23 | 山东大学 | Rgbd相机大型三维场景构建方法及*** |
WO2020237693A1 (zh) * | 2019-05-31 | 2020-12-03 | 华南理工大学 | 一种水面无人装备多源感知方法及*** |
US10962630B1 (en) * | 2019-10-18 | 2021-03-30 | Toyota Research Institute, Inc. | System and method for calibrating sensors of a sensor system |
AU2020101833A4 (en) * | 2019-12-03 | 2020-09-24 | Wuhan University Of Science And Technology | Laser slam method based on subgraph merging and pose optimization |
CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
CN111540005A (zh) * | 2020-04-21 | 2020-08-14 | 南京理工大学 | 基于二维栅格地图的回环检测方法 |
CN111693047A (zh) * | 2020-05-08 | 2020-09-22 | 中国航空工业集团公司西安航空计算技术研究所 | 一种高动态场景下的微小型无人机视觉导航方法 |
CN112258600A (zh) * | 2020-10-19 | 2021-01-22 | 浙江大学 | 一种基于视觉与激光雷达的同时定位与地图构建方法 |
CN112258618A (zh) * | 2020-11-04 | 2021-01-22 | 中国科学院空天信息创新研究院 | 基于先验激光点云与深度图融合的语义建图与定位方法 |
CN112785702A (zh) * | 2020-12-31 | 2021-05-11 | 华南理工大学 | 一种基于2d激光雷达和双目相机紧耦合的slam方法 |
CN112985416A (zh) * | 2021-04-19 | 2021-06-18 | 湖南大学 | 激光与视觉信息融合的鲁棒定位和建图方法及*** |
Non-Patent Citations (8)
Title |
---|
刘鹏鑫 ; 王扬 ; .多传感器集成测量***的数据对齐方法.华南理工大学学报(自然科学版).2009,(第09期),全文. * |
基于多分辨率搜索与多点云密度匹配的快速ICP-SLAM 方法;李鑫;机器人;全文1-12页 * |
基于局部坐标系法线投射的点云精细配准算法;蔡先杰;;现代计算机(专业版)(第26期);全文 * |
基于深度优先搜索分支定界法的Graph-SLAM后端优化算法改进;李敏;自动化技术与应用;全文1-5页 * |
多传感器集成测量***的数据对齐方法;刘鹏鑫;王扬;;华南理工大学学报(自然科学版)(第09期);全文 * |
无人平台越野环境下同步定位与地图创建;刘忠泽;兵工学报;全文 * |
蔡先杰 ; .基于局部坐标系法线投射的点云精细配准算法.现代计算机(专业版).2016,(第26期),全文. * |
融合激光与视觉点云信息的定位与建图方法;张伟伟;计算机应用与软件;全文1-6页 * |
Also Published As
Publication number | Publication date |
---|---|
CN113587933A (zh) | 2021-11-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113587933B (zh) | 一种基于分支定界算法的室内移动机器人定位方法 | |
CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
CN108444482B (zh) | 一种无人机自主寻路避障方法及*** | |
CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、***、装置及存储介质 | |
CN109186606B (zh) | 一种基于slam和图像信息的机器人构图及导航方法 | |
JP2019120927A (ja) | グリッドマップを作成する方法及び装置 | |
CN113168717A (zh) | 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达 | |
US11045953B2 (en) | Relocalization method and robot using the same | |
CN112700479B (zh) | 一种基于cnn点云目标检测的配准方法 | |
WO2022087916A1 (zh) | 定位方法、装置、电子设备和存储介质 | |
CN114004869A (zh) | 一种基于3d点云配准的定位方法 | |
CN113514843A (zh) | 多子图激光雷达定位方法、***以及终端 | |
CN114549738A (zh) | 无人车室内实时稠密点云重建方法、***、设备及介质 | |
CN113706710A (zh) | 基于fpfh特征差异的虚拟点多源点云融合方法及*** | |
CN112166457A (zh) | 点云分割方法、***和可移动平台 | |
CN111928860A (zh) | 一种基于三维曲面定位能力的自主车辆主动定位方法 | |
CN113325389A (zh) | 一种无人车激光雷达定位方法、***及存储介质 | |
CN115131514A (zh) | 一种同时定位建图的方法、装置、***及存储介质 | |
CN113034504B (zh) | Slam建图过程中的平面特征融合方法 | |
CN114187418A (zh) | 回环检测方法、点云地图构建方法、电子设备及存储介质 | |
Pan et al. | Pin-slam: Lidar slam using a point-based implicit neural representation for achieving global map consistency | |
CN114577196A (zh) | 使用光流的激光雷达定位 | |
CN113960614A (zh) | 一种基于帧-地图匹配的高程图构建方法 | |
CN117029870A (zh) | 一种基于路面点云的激光里程计 | |
CN116385999A (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 |