CN112762923A - 一种3d点云地图更新方法和系统 - Google Patents
一种3d点云地图更新方法和系统 Download PDFInfo
- Publication number
- CN112762923A CN112762923A CN202011641143.2A CN202011641143A CN112762923A CN 112762923 A CN112762923 A CN 112762923A CN 202011641143 A CN202011641143 A CN 202011641143A CN 112762923 A CN112762923 A CN 112762923A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- map
- robot
- matrix
- updating
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 37
- 238000005457 optimization Methods 0.000 claims abstract description 23
- 239000011159 matrix material Substances 0.000 claims description 88
- 238000013519 translation Methods 0.000 claims description 42
- 230000009466 transformation Effects 0.000 claims description 24
- 238000009826 distribution Methods 0.000 claims description 13
- 238000004364 calculation method Methods 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 9
- 238000000354 decomposition reaction Methods 0.000 claims description 7
- 238000007476 Maximum Likelihood Methods 0.000 claims description 6
- 239000004576 sand Substances 0.000 claims description 6
- 230000001131 transforming effect Effects 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 3
- 238000007689 inspection Methods 0.000 abstract description 20
- 230000002159 abnormal effect Effects 0.000 abstract description 5
- 238000010276 construction Methods 0.000 abstract description 4
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 15
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 15
- 238000010586 diagram Methods 0.000 description 14
- 238000009616 inductively coupled plasma Methods 0.000 description 10
- 238000004590 computer program Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 7
- 238000012545 processing Methods 0.000 description 4
- 238000012360 testing method Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/50—Depth or shape recovery
- G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明的一种3D点云地图更新方法和系统，构建局部地图，获取机器人粗位姿，然后对机器人位姿进行优化更新；其中更新步骤包括由粗到细的匹配算法、增量式优化算法，最后采用局部地图与全局地图的匹配更新采用点云配准；本发明旨在地图场景变化进而导致导航异常时，对此局部区域地图进行更新，而不影响原地图的其他区域和实施信息，避免了重复工作量。本发明自动化程度高，实施简单，针对特定的巡检环境，可以实现智能自主实施，即使是不专业的施工人员也可以轻松使用机器人进行智能巡检，降低了施工的难度，也减少了现场施工人员的专业技能要求，降低人力成本；在特定情况，如机器人转场时，不需要重复实施。
Description
技术领域
本发明涉及智能机器人和SLAM技术领域，具体涉及一种3D点云地图更新方法和系统。
背景技术
随着现代技术的不断发展，电力系统形成了信息化和智能化的发展趋势，目前越来越多的配电房引入智能巡检机器人来代替人工巡检电力设备是否运行正常，可以减少人工成本，且机器人可以一直循环巡检，提高工作效率，减少出问题的风险；但目前主流的SLAM都是基于先创建的地图进行定位导航，一旦地图场景出现变化，就会导致定位导航异常，需要对地图进行重新创建修复，增加实施工作量。
轮式巡检机器人在巡检前需要先进行地图构建，然后根据创建的地图进行导航巡检；但是现场环境经常会出现变化的情况，尤其是在增加了物体的情况下，这样会导致机器人的定位出现不稳定和不准确的情况，进而导致导航异常，影响正常巡检；
一旦出现这种情况，就只能对地图进行重新构建，但是这样会导致之前已经实施过的点位信息等出现偏差，增加重复工作量；本文提出的方法就是在不改变原有地图的实施信息的前提下，对巡检地图进行局部更新，解决定位导航异常的问题。
地图在环境变化时可用性差，需要重新建图以满足实际巡检需要，或者要尽量避免现场的环境变化，会增加现场的实施工作量。
术语解释：
实施：机器人在智能巡检前，创建巡检地图，确认巡检路径，设置巡检设备点位信息等的操作；
SLAM：simultaneous localization and mapping，即时定位与地图构建；
ICP:Iterative Closest Point,迭代最近点算法；
PointToPlane：点到面匹配模式，ICP匹配算法的一种。
发明内容
本发明提出的一种3D点云地图更新方法和系统，可解决上述技术问题。
为实现上述目的，本发明采用了以下技术方案：
一种3D点云地图更新方法，包括以下步骤：
构建局部地图，获取机器人粗位姿，然后对机器人位姿进行优化更新；
其中，优化更新步骤包括由粗到细的匹配算法，如下：
Step1.对Qk采用K-D树描述，对
中的每个激光点寻找在Qk中的最近点，建立配对；
Step2.建立非线性方程，目标使得所有配对的距离最小；
Step3.利用L-M法求解非线性优化问题，求解Topt＝(R,t)；
Step4.则k+1时刻的机器人位姿为
其中Qk表示k时刻建立的点云地图，
表示k+1时刻当前帧转换到世界坐标系下，
表示k时刻机器人在世界坐标系下的位姿，
表示k+1时刻机器人的位姿增量也即上步粗匹配过程中输出量。
进一步的，优化更新步骤还包括增量式优化算法，如下：
Step21.记t时刻的机器人位姿为xt，传感器观测为zt，控制量为ut，x1:t＝{x1,…,xt}为机器人从起始到t时刻的行驶轨迹，则SLAM问题等价于估计后验概率分布P(x1:t,m|z1:t,u0:t-1)，其中m为环境地图；
由式1可知，地图m依赖于机器人运动轨迹及其对应的传感器观测；
Step22.由上可知SLAM可简化为机器人轨迹估计问题：
Step23.基于位姿图的SLAM方法并不直接求取P(x1:t,mz1:t,u0:t-1)，而仅求取其极大似然：
Step24.假定运动学模型及观测模型噪声若满足高斯分布，则运动学模型及观测模型均可转换为如下形式：
其中，
运算表示坐标变换，Tij和Σij分别为xi与xj之间的相对关系和协方差，可由上述匹配算法获得；在二维情形下，x＝(x,y,θ)T，则T可表示为(xT,yT,θT)T，则
运算的具体形式可以表示为：
于是，式(3)可写成：
Step25.令
其为非线性函数，将其线性化，即有：
式中，
是fj(xi)的雅可比矩阵；
Step26.令
对式(6)进行简化有：
记X＝(δx1 T,δx2 T,…,δxt T)T，并写成一般形式有：
其中，Q(R 0)T是A的QR分解。
进一步的，所述优化更新步骤还包括：局部地图与全局地图的匹配更新采用点云配准；
点云配准的问题描述为
这里Ps和Pt是源点云和目标点云中的对应点，即求解两个点云数据的最优旋转和平移矩阵；R代表旋转矩阵，t代表平移矩阵；
点云匹配的实现步骤如下：
Step31找最近对应点；
利用初始旋转矩阵P0和平移矩阵T0或者上一次迭代的旋转矩阵Ri-1和平移矩阵Ti-1，对初始点云数据进行变换得到一个临时点云，然后用这个点云和目标点云进行比较，找出源点云中每一个点在目标点云中的最近邻点；
Step32求解最优变换矩阵；
根据step31中的变换点云和目标点云，求解此前的最优旋转矩阵和平移矩阵；平移矩阵的计算公式如下：
旋转矩阵计算公式如下：
Step33迭代；
每一次迭代都可得到当前认为的最优旋转和平移矩阵，然后将当前源点云数据按照最优旋转参数进行变换，重复step32的求解最优变换操作，直到满足迭代结束条件，完成匹配迭代。
本发明还公开一种3D点云地图更新方法，包括以下步骤：
Step01实施模块将机器人控制到进行地图更新的区域，并记录下当前机器人的实时位姿，作为后面进行地图匹配的参考；
Step02实施模块下发开始地图更新的指令，更新模块接收到指令后开始进行局部地图的构建；
Step03实施模块控制机器人行走，从地图环境未变化区域，经过变化区域，再到达未变化区域，确保激光雷达将地图环境中变化的部分都扫描入局部地图中；
Step04实施模块下发停止地图更新的指令；
Step05更新模块根据由粗到细的匹配算法和增量式优化算法生成局部点云地图；
Step06更新模块载入原始点云地图数据和局部点云地图数据，并将原始点云数据作为匹配的源点云，局部点云数据作为匹配的目标点云，并将地图更新时的初始位姿作为点云匹配的初始矩阵；
Step07更新模块使用PointToPlane ICP匹配算法，将源点云和目标点云依据初始矩阵进行迭代匹配，得到匹配后的输出点云；
Step08将匹配后的点云数据与原点云进行数据叠加，滤波；
Step09根据最新的点云数据生成全新的巡检地图。
本发明还公开一种3D点云地图更新系统，包括以下单元，
由粗到细的匹配算法单元，所述单元用于执行以下步骤：
Step1.对Qk采用K-D树描述，对
中的每个激光点寻找在Qk中的最近点，建立配对；
Step2.建立非线性方程，目标使得所有配对的距离最小；
Step3.利用L-M法求解非线性优化问题，求解Topt＝(R,t)；
Step4.则k+1时刻的机器人位姿为
其中Qk表示k时刻建立的点云地图，
表示k+1时刻当前帧转换到世界坐标系下，
表示k时刻机器人在世界坐标系下的位姿，
表示k+1时刻机器人的位姿增量也即上步粗匹配过程中输出量。
进一步的，还包括增量式优化算法单元，所述单元用于执行以下步骤：
Step21.记t时刻的机器人位姿为xt，传感器观测为zt，控制量为ut，x1:t＝{x1,…,xt}为机器人从起始到t时刻的行驶轨迹，则SLAM问题等价于估计后验概率分布P(x1:t,m|z1:t,u0:t-1)，其中m为环境地图；
由式1可知，地图m依赖于机器人运动轨迹及其对应的传感器观测；
Step22.由上可知SLAM可简化为机器人轨迹估计问题：
Step23.基于位姿图的SLAM方法并不直接求取P(x1:t,mz1:t,u0:t-1)，而仅求取其极大似然：
Step24.假定运动学模型及观测模型噪声若满足高斯分布，则运动学模型及观测模型均可转换为如下形式：
其中，
运算表示坐标变换，Tij和Σij分别为xi与xj之间的相对关系和协方差，可由上述匹配算法获得；在二维情形下，x＝(x,y,θ)T，则T可表示为(xT,yT,θT)T，则
运算的具体形式可以表示为：
于是，式(3)可写成：
Step25.令
其为非线性函数，将其线性化，即有：
式中，
是fj(xi)的雅可比矩阵；
Step26.令
对式(6)进行简化有：
记X＝(δx1 T,δx2 T,…,δxt T)T，并写成一般形式有：
其中，Q(R 0)T是A的QR分解。
进一步的，还包括点云匹配算法单元，所述单元用于执行以下步骤：
点云配准的问题描述为
这里Ps和Pt是源点云和目标点云中的对应点，即求解两个点云数据的最优旋转和平移矩阵；R代表旋转矩阵，t代表平移矩阵；
点云匹配的实现步骤如下：
Step31找最近对应点；
利用初始旋转矩阵R0和平移矩阵R0或者上一次迭代的旋转矩阵Ri-1和平移矩阵Ri-1，对初始点云数据进行变换得到一个临时点云，然后用这个点云和目标点云进行比较，找出源点云中每一个点在目标点云中的最近邻点；
Step32求解最优变换矩阵；
根据step31中的变换点云和目标点云，求解此前的最优旋转矩阵和平移矩阵；平移矩阵的计算公式如下：
旋转矩阵计算公式如下：
Step33迭代；
每一次迭代都可得到当前认为的最优旋转和平移矩阵，然后将当前源点云数据按照最优旋转参数进行变换，重复step32的求解最优变换操作，直到满足迭代结束条件，完成匹配迭代。
由上述技术方案可知，本发明的3D点云地图更新方法和系统，本发明旨在地图场景变化进而导致导航异常时，对此局部区域地图进行更新，而不影响原地图的其他区域和实施信息，避免了重复工作量。
本发明的3D点云地图更新方法和系统自动化程度高，实施简单，针对特定的巡检环境，可以实现智能自主实施，即使是不专业的施工人员也可以轻松使用机器人进行智能巡检，降低了施工的难度，也减少了现场施工人员的专业技能要求，降低人力成本；在特定情况，如机器人转场时，不需要重复实施。
附图说明
图1是本发明粗到精的匹配算法模型示意图；
图2是本发明的是局部地图与全局地图的匹配更新示意图；
图3是本发明的地图更新流图；
图4是本发明的实验室环境测试效果图；
图5是本发明的走廊环境测试效果图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。
本实施例所述的3D点云地图更新方法，包括以下步骤：
首先是局部地图的构建；
本发明提出一种由粗到精和后端增量式优化的建图方法，针对区域地图，可在不进行回环的情况下，生成较精确的地图；
(1)提出一种由粗到精的匹配算法Rough-to-Fine ScanMatching，
其中粗匹配中利用Generalized-ICP(一种plane-to-plane的ICP变种)，提高帧间匹配的鲁棒性和精确性(以轮式里程计做初值)，精匹配过程考虑如下情况：如图1所示：
其中Qk表示k时刻建立的点云地图，
表示k+1时刻当前帧转换到世界坐标系下，
表示k时刻机器人在世界坐标系下的位姿，
表示k+1时刻机器人的位姿增量(也即上步粗匹配过程中输出量)。如上图所示显然由粗匹配生成的机器人位姿与真实位姿有偏差，表现在
与Qk没有重合。精匹配步骤如下：
Step 11.对Qk采用K-D树描述，对
中的每个激光点寻找在Qk中的最近点(FLANN算法)，建立配对；
Step 12.建立非线性方程，目标使得所有配对的距离最小；
Step 13.利用L-M法求解非线性优化问题，求解Topt＝(R,t)；
Step 14.则k+1时刻的机器人位姿为
经过如上两个步骤，可以生成较为精确的机器人位姿。
(2)提出一种增量式优化算法
Step21.记t时刻的机器人位姿为xt，传感器观测为zt，控制量为ut，x1:t＝{x1,…,xt}为机器人从起始到t时刻的行驶轨迹则SLAM问题等价于估计后验概率分布P(x1:t,m|z1:t,u0:t-1)，其中m为环境地图。
由式1可知，地图m依赖于机器人运动轨迹及其对应的传感器观测。
Step22.由上可知SLAM可以简化为机器人轨迹估计问题：
Step23.基于位姿图的SLAM方法并不直接求取P(x1:t,mz1:t,u0:t-1)，而仅求取其极大似然：
Step24.一般地，假定运动学模型及观测模型噪声若满足高斯分布，则运动学模型及观测模型均可转换为如下形式：
其中，
运算表示坐标变换，Tij和Σij分别为xi与xj之间的相对关系和协方差，可由上述匹配算法获得。在二维情形下，x＝(x,y,θ)T，则T可表示为(xT,yT,θT)T，则
运算的具体形式可以表示为：
于是，式3可写成：
Step25.令
其为非线性函数，为方便处理需要将其线性化，即有：
式中，
是fj(xi)的雅可比矩阵。
Step26.令
对式6进行简化有：
记X＝(δx1 T,δx2 T,…,δxt T)T，并写成一般形式有：
其中，Q(R 0)T是A的QR分解.
本发明采用增量式方法进行QR分解.由于每次迭代过程中A的变化不大，可采用上一时刻QR分解的吉文斯旋转(Givens Rotation)矩阵作为迭代初值.此时，QR分解是增量进行的，而上三角阵求逆的复杂度较小，因此该方法可满足实时运用。
如图2所示，然后是局部地图与全局地图的匹配更新
地图更新采用Generalized—ICP匹配算法进行点云数据的匹配，因为原始地图点云和局部地图点云数据的扫图方向和位置不一样，所以为了让点云匹配能最快收敛，根据地图更新的初始位置，给ICP匹配一个初始矩阵
点云配准的问题可以描述为
这里Ps和Pt是源点云和目标点云中的对应点，即求解两个点云数据的最优旋转和平移矩阵；R代表旋转矩阵，t代表平移矩阵；
点云匹配的实现步骤大致如下：
Step31找最近对应点
利用初始旋转矩阵R0和平移矩阵T0或者上一次迭代的旋转矩阵Ri-1和平移矩阵Ti-1，对初始点云数据进行变换得到一个临时点云，然后用这个点云和目标点云进行比较，找出源点云中每一个点在目标点云中的最近邻点；本发明中初始矩阵我们使用开始地图更新时的位姿矩阵进行匹配。
Step32求解最优变换矩阵
根据step31中的变换点云和目标点云，求解此前的最优旋转矩阵和平移矩阵；平移矩阵的计算公式如下：
旋转矩阵计算公式如下：
Step33迭代
每一次迭代都可以得到当前认为的最优旋转和平移矩阵，然后将当前源点云数据按照最优旋转参数进行变换，重复step2的求解最优变换操作，直到满足迭代结束条件，完成匹配迭代。
实施例2
如图3所示，本系统分为实施模块和更新模块两部分：
Step01实施模块将机器人控制到进行地图更新的区域，并记录下当前机器人的实时位姿，作为后面进行地图匹配的参考；
Step02实施模块下发开始地图更新的指令，更新模块接收到指令后开始进行局部地图的构建；
Step03实施模块控制机器人行走，从地图环境未变化区域，经过变化区域，再到达未变化区域，确保激光雷达将地图环境中变化的部分都扫描入局部地图中；
Step04实施模块下发停止地图更新的指令；
Step05更新模块根据由粗到细的匹配算法和增量式优化算法生成局部点云地图；
Step06更新模块载入原始点云地图数据和局部点云地图数据，并将原始点云数据作为匹配的源点云，局部点云数据作为匹配的目标点云，并将地图更新时的初始位姿作为点云匹配的初始矩阵；
Step07更新模块使用PointToPlane ICP匹配算法，将源点云和目标点云依据初始矩阵进行迭代匹配，得到匹配后的输出点云；
Step08将匹配后的点云数据与原点云进行数据叠加，滤波；
Step09根据最新的点云数据生成全新的巡检地图。
如图4本发明的实验室环境测试效果图；其中周边发白色图案为原始地图，方框中白色区域为变化的部分；
图5是本发明的走廊环境测试效果图；周边白色区域为原始地图，方框中的白色区域为变化部分；
由上可知，本发明的3D点云地图更新方法自动化程度高，实施简单，针对特定的巡检环境，可以实现智能自主实施，即使是不专业的施工人员也可以轻松使用机器人进行智能巡检，降低了施工的难度，也减少了现场施工人员的专业技能要求，降低人力成本；在特定情况，如机器人转场时，不需要重复实施。
同时，本发明实施例还公开一种3D点云地图更新系统，包括以下单元，由粗到细的匹配算法单元，所述单元用于执行以下步骤：
Step1.对Qk采用K-D树描述，对
中的每个激光点寻找在Qk中的最近点，建立配对；
Step2.建立非线性方程，目标使得所有配对的距离最小；
Step3.利用L-M法求解非线性优化问题，求解Topt＝(R,t)；
Step4.则k+1时刻的机器人位姿为
其中Qk表示k时刻建立的点云地图，
表示k+1时刻当前帧转换到世界坐标系下，
表示k时刻机器人在世界坐标系下的位姿，
表示k+1时刻机器人的位姿增量也即上步粗匹配过程中输出量。
进一步的，还包括增量式优化算法单元，所述单元用于执行以下步骤：
Step21.记t时刻的机器人位姿为xt，传感器观测为zt，控制量为ut，x1:t＝{x1,…,xt}为机器人从起始到t时刻的行驶轨迹，则SLAM问题等价于估计后验概率分布P(x1:t,m|z1:t,u0:t-1)，其中m为环境地图；
由式1可知，地图m依赖于机器人运动轨迹及其对应的传感器观测；
Step22.由上可知SLAM可简化为机器人轨迹估计问题：
Step23.基于位姿图的SLAM方法并不直接求取P(x1:t,mz1:t,u0:t-1)，而仅求取其极大似然：
Step24.假定运动学模型及观测模型噪声若满足高斯分布，则运动学模型及观测模型均可转换为如下形式：
其中，
运算表示坐标变换，Tij和Σij分别为xi与xj之间的相对关系和协方差，可由上述匹配算法获得；在二维情形下，x＝(x,y,θ)T，则T可表示为(xT,yT,θT)T，则
运算的具体形式可以表示为：
于是，式(3)可写成：
Step25.令
其为非线性函数，将其线性化，即有：
式中，
是fj(xi)的雅可比矩阵；
Step26.令
对式(6)进行简化有：
记X＝(δx1 T,δx2 T,…,δxt T)T，并写成一般形式有：
其中，Q(R 0)T是A的QR分解。
进一步的，还包括点云匹配算法单元，所述单元用于执行以下步骤：
点云配准的问题描述为
这里Ps和Pt是源点云和目标点云中的对应点，即求解两个点云数据的最优旋转和平移矩阵；R代表旋转矩阵，t代表平移矩阵；
点云匹配的实现步骤如下：
Step31找最近对应点；
利用初始旋转矩阵P0和平移矩阵T0或者上一次迭代的旋转矩阵Ri-1和平移矩阵Ti-1，对初始点云数据进行变换得到一个临时点云，然后用这个点云和目标点云进行比较，找出源点云中每一个点在目标点云中的最近邻点；
Step32求解最优变换矩阵；
根据step31中的变换点云和目标点云，求解此前的最优旋转矩阵和平移矩阵；平移矩阵的计算公式如下：
旋转矩阵计算公式如下：
Step33迭代；
每一次迭代都可得到当前认为的最优旋转和平移矩阵，然后将当前源点云数据按照最优旋转参数进行变换，重复step32的求解最优变换操作，直到满足迭代结束条件，完成匹配迭代。
可理解的是，本发明实施例提供的系统与本发明实施例提供的方法相对应，相关内容的解释、举例和有益效果可以参考上述方法中的相应部分。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (7)
1.一种3D点云地图更新方法，其特征在于：包括以下步骤：
构建局部地图，获取机器人粗位姿，然后对机器人位姿进行优化更新；
其中，优化更新步骤包括由粗到细的匹配算法，如下：
Step1.对Qk采用K-D树描述，对
中的每个激光点寻找在Qk中的最近点，建立配对；
Step2.建立非线性方程，目标使得所有配对的距离最小；
Step3.利用L-M法求解非线性优化问题，求解Topt＝(R,t)；
Step4.则k+1时刻的机器人位姿为
其中Qk表示k时刻建立的点云地图，
表示k+1时刻当前帧转换到世界坐标系下，
表示k时刻机器人在世界坐标系下的位姿，
表示k+1时刻机器人的位姿增量也即上步粗匹配过程中输出量。
2.根据权利要求1所述的3D点云地图更新方法，其特征在于：优化更新步骤还包括增量式优化算法，如下：
Step21.记t时刻的机器人位姿为xt，传感器观测为zt，控制量为ut，x1:t＝{x1,…,xt}为机器人从起始到t时刻的行驶轨迹，则SLAM问题等价于估计后验概率分布P(x1:t,m|z1:t,u0:t-1)，其中m为环境地图；
由式1可知，地图m依赖于机器人运动轨迹及其对应的传感器观测；
Step22.由上可知SLAM可简化为机器人轨迹估计问题：
Step23.基于位姿图的SLAM方法并不直接求取P(x1:t,m|z1:t,u0:t-1)，而仅求取其极大似然：
Step24.假定运动学模型及观测模型噪声若满足高斯分布，则运动学模型及观测模型均可转换为如下形式：
其中，
运算表示坐标变换，Tij和Σij分别为xi与xj之间的相对关系和协方差，可由上述匹配算法获得；在二维情形下，x＝(x,y,θ)T，则T可表示为(xT,yT,θT)T，则
运算的具体形式可以表示为：
于是，式(3)可写成：
Step25.令
其为非线性函数，将其线性化，即有：
式中，
是fj(xi)的雅可比矩阵；
Step26.令
对式(6)进行简化有：
记
并写成一般形式有：
其中，Q(R 0)T是A的QR分解。
3.根据权利要求2所述的3D点云地图更新方法，其特征在于：所述优化更新步骤还包括：
局部地图与全局地图的匹配更新采用点云配准；
点云配准的问题描述为
这里Ps和Pt是源点云和目标点云中的对应点，即求解两个点云数据的最优旋转和平移矩阵；R代表旋转矩阵，t代表平移矩阵；
点云匹配的实现步骤如下：
Step31找最近对应点；
利用初始旋转矩阵R0和平移矩阵T0或者上一次迭代的旋转矩阵Ri-1和平移矩阵Ti-1，对初始点云数据进行变换得到一个临时点云，然后用这个点云和目标点云进行比较，找出源点云中每一个点在目标点云中的最近邻点；
Step32求解最优变换矩阵；
根据step31中的变换点云和目标点云，求解此前的最优旋转矩阵和平移矩阵；平移矩阵的计算公式如下：
旋转矩阵计算公式如下：
Step33迭代；
每一次迭代都可得到当前认为的最优旋转和平移矩阵，然后将当前源点云数据按照最优旋转参数进行变换，重复step32的求解最优变换操作，直到满足迭代结束条件，完成匹配迭代。
4.一种3D点云地图更新方法，其特征在于：包括以下步骤：
Step01实施模块将机器人控制到进行地图更新的区域，并记录下当前机器人的实时位姿，作为后面进行地图匹配的参考；
Step02实施模块下发开始地图更新的指令，更新模块接收到指令后开始进行局部地图的构建；
Step03实施模块控制机器人行走，从地图环境未变化区域，经过变化区域，再到达未变化区域，确保激光雷达将地图环境中变化的部分都扫描入局部地图中；
Step04实施模块下发停止地图更新的指令；
Step05更新模块根据由粗到细的匹配算法和增量式优化算法生成局部点云地图；
Step06更新模块载入原始点云地图数据和局部点云地图数据，并将原始点云数据作为匹配的源点云，局部点云数据作为匹配的目标点云，并将地图更新时的初始位姿作为点云匹配的初始矩阵；
Step07更新模块使用PointToPlane ICP匹配算法，将源点云和目标点云依据初始矩阵进行迭代匹配，得到匹配后的输出点云；
Step08将匹配后的点云数据与原点云进行数据叠加，滤波；
Step09根据最新的点云数据生成全新的巡检地图。
5.一种3D点云地图更新系统，其特征在于：包括以下单元，
由粗到细的匹配算法单元，所述单元用于执行以下步骤：
Step1.对Qk采用K-D树描述，对
中的每个激光点寻找在Qk中的最近点，建立配对；
Step2.建立非线性方程，目标使得所有配对的距离最小；
Step3.利用L-M法求解非线性优化问题，求解Topt＝(R,t)；
Step4.则k+1时刻的机器人位姿为
其中Qk表示k时刻建立的点云地图，
表示k+1时刻当前帧转换到世界坐标系下，
表示k时刻机器人在世界坐标系下的位姿，
表示k+1时刻机器人的位姿增量也即上步粗匹配过程中输出量。
6.根据权利要求5所述的一种3D点云地图更新系统，其特征在于：还包括增量式优化算法单元，所述单元用于执行以下步骤：
Step21.记t时刻的机器人位姿为xt，传感器观测为zt，控制量为ut，x1:t＝{x1,…,xt}为机器人从起始到t时刻的行驶轨迹，则SLAM问题等价于估计后验概率分布P(x1:t,m|z1:t,u0:t-1)，其中m为环境地图；
由式1可知，地图m依赖于机器人运动轨迹及其对应的传感器观测；
Step22.由上可知SLAM可简化为机器人轨迹估计问题：
Step23.基于位姿图的SLAM方法并不直接求取P(x1:t,m|z1:t,u0:t-1)，而仅求取其极大似然：
Step24.假定运动学模型及观测模型噪声若满足高斯分布，则运动学模型及观测模型均可转换为如下形式：
其中，⊕运算表示坐标变换，Tij和Σij分别为xi与xj之间的相对关系和协方差，可由上述匹配算法获得；在二维情形下，x＝(x,y,θ)T，则T可表示为(xT,yT,θT)T，则
运算的具体形式可以表示为：
于是，式(3)可写成：
Step25.令
其为非线性函数，将其线性化，即有：
式中，
是fj(xi)的雅可比矩阵；
Step26.令
对式(6)进行简化有：
记
并写成一般形式有：
其中，Q(R 0)T是A的QR分解。
7.根据权利要求6所述的一种3D点云地图更新系统，其特征在于：还包括点云匹配算法单元，所述单元用于执行以下步骤：
点云配准的问题描述为
这里Ps和Pt是源点云和目标点云中的对应点，即求解两个点云数据的最优旋转和平移矩阵；R代表旋转矩阵，t代表平移矩阵；
点云匹配的实现步骤如下：
Step31找最近对应点；
利用初始旋转矩阵R0和平移矩阵T0或者上一次迭代的旋转矩阵Ri-1和平移矩阵Ti-1，对初始点云数据进行变换得到一个临时点云，然后用这个点云和目标点云进行比较，找出源点云中每一个点在目标点云中的最近邻点；
Step32求解最优变换矩阵；
根据step31中的变换点云和目标点云，求解此前的最优旋转矩阵和平移矩阵；平移矩阵的计算公式如下：
旋转矩阵计算公式如下：
Step33迭代；
每一次迭代都可得到当前认为的最优旋转和平移矩阵，然后将当前源点云数据按照最优旋转参数进行变换，重复step32的求解最优变换操作，直到满足迭代结束条件，完成匹配迭代。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011641143.2A CN112762923A (zh) | 2020-12-31 | 2020-12-31 | 一种3d点云地图更新方法和系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011641143.2A CN112762923A (zh) | 2020-12-31 | 2020-12-31 | 一种3d点云地图更新方法和系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112762923A true CN112762923A (zh) | 2021-05-07 |
Family
ID=75698438
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011641143.2A Pending CN112762923A (zh) | 2020-12-31 | 2020-12-31 | 一种3d点云地图更新方法和系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112762923A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113970754A (zh) * | 2021-10-25 | 2022-01-25 | 北京京东乾石科技有限公司 | 一种自主可行驶设备的定位方法和装置 |
| CN115797425A (zh) * | 2023-01-19 | 2023-03-14 | 中国科学技术大学 | 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105203094A (zh) * | 2015-09-10 | 2015-12-30 | 联想(北京)有限公司 | 构建地图的方法和设备 |
| CN108320329A (zh) * | 2018-02-02 | 2018-07-24 | 维坤智能科技（上海）有限公司 | 一种基于3d激光的3d地图创建方法 |
| CN110532276A (zh) * | 2019-08-21 | 2019-12-03 | 北京汽车股份有限公司 | 地图更新方法和更新装置、车辆 |
| CN110765223A (zh) * | 2019-10-24 | 2020-02-07 | 北京百度网讯科技有限公司 | 地图更新处理方法、装置、设备及存储介质 |
| CN111060113A (zh) * | 2019-12-31 | 2020-04-24 | 歌尔股份有限公司 | 一种地图更新方法及装置 |
| CN111060135A (zh) * | 2019-12-10 | 2020-04-24 | 亿嘉和科技股份有限公司 | 一种基于局部地图的地图修正方法及系统 |
| CN111161412A (zh) * | 2019-12-06 | 2020-05-15 | 苏州艾吉威机器人有限公司 | 三维激光建图方法及系统 |
| CN112014857A (zh) * | 2020-08-31 | 2020-12-01 | 上海宇航系统工程研究所 | 用于智能巡检的三维激光雷达定位导航方法及巡检机器人 |
-
2020
- 2020-12-31 CN CN202011641143.2A patent/CN112762923A/zh active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105203094A (zh) * | 2015-09-10 | 2015-12-30 | 联想(北京)有限公司 | 构建地图的方法和设备 |
| CN108320329A (zh) * | 2018-02-02 | 2018-07-24 | 维坤智能科技（上海）有限公司 | 一种基于3d激光的3d地图创建方法 |
| CN110532276A (zh) * | 2019-08-21 | 2019-12-03 | 北京汽车股份有限公司 | 地图更新方法和更新装置、车辆 |
| CN110765223A (zh) * | 2019-10-24 | 2020-02-07 | 北京百度网讯科技有限公司 | 地图更新处理方法、装置、设备及存储介质 |
| CN111161412A (zh) * | 2019-12-06 | 2020-05-15 | 苏州艾吉威机器人有限公司 | 三维激光建图方法及系统 |
| CN111060135A (zh) * | 2019-12-10 | 2020-04-24 | 亿嘉和科技股份有限公司 | 一种基于局部地图的地图修正方法及系统 |
| CN111060113A (zh) * | 2019-12-31 | 2020-04-24 | 歌尔股份有限公司 | 一种地图更新方法及装置 |
| CN112014857A (zh) * | 2020-08-31 | 2020-12-01 | 上海宇航系统工程研究所 | 用于智能巡检的三维激光雷达定位导航方法及巡检机器人 |
Non-Patent Citations (3)
| Title |
|---|
| 任杰;宋建涛;李功燕;: "开放式环境下变电站机器人SLAM算法研究", 信息技术与网络安全, vol. 37, no. 03, pages 80 - 83 * |
| 张洪华，刘 璇 ，陈付豪，李文彬，张建华: "基于图优化的 SLAM 后端优化研究与发展", 计算机应用研究, vol. 36, no. 1, pages 11 - 17 * |
| 张蒙: "基于改进的ICP算法的点云配准技术", 中国优秀硕士学位论文全文数据库, no. 2015, pages 12 - 32 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113970754A (zh) * | 2021-10-25 | 2022-01-25 | 北京京东乾石科技有限公司 | 一种自主可行驶设备的定位方法和装置 |
| CN115797425A (zh) * | 2023-01-19 | 2023-03-14 | 中国科学技术大学 | 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Sobreira et al. | Map-matching algorithms for robot self-localization: a comparison between perfect match, iterative closest point and normal distributions transform | |
| CN110009718B (zh) | 一种三维高精度地图生成方法及装置 | |
| JP5018458B2 (ja) | 座標補正方法、座標補正プログラム、及び自律移動ロボット | |
| CN112762923A (zh) | 一种3d点云地图更新方法和系统 | |
| WO2019122939A1 (en) | Interactive computer-implemented method, graphical user interface and computer program product for building a high-accuracy environment map | |
| CN111578958A (zh) | 移动机器人导航实时定位方法、系统、介质及电子设备 | |
| CN109557929B (zh) | 移动机器人的运动控制方法及装置 | |
| CN113031621B (zh) | 一种桥式起重机安全避障路径规划方法及系统 | |
| CN113319859B (zh) | 一种机器人示教方法、系统、装置及电子设备 | |
| Geng et al. | A novel welding path planning method based on point cloud for robotic welding of impeller blades | |
| CN112904358A (zh) | 基于几何信息的激光定位方法 | |
| CN114986498B (zh) | 一种移动操作臂协同控制方法 | |
| Lauer et al. | Tool center point control of a large-scale manipulator using absolute position feedback | |
| Kitanov et al. | Mobile robot self-localization in complex indoor environments using monocular vision and 3D model | |
| Wei et al. | Novel robust simultaneous localization and mapping for long-term autonomous robots | |
| CN109542094B (zh) | 无期望图像的移动机器人视觉镇定控制 | |
| CN117621060A (zh) | 一种环境感知的足式机器人落足控制方法及系统 | |
| CN116736866A (zh) | 一种物业室内机器人路径规划与轨迹控制方法及系统 | |
| CN114800523B (zh) | 机械臂轨迹修正方法、系统、计算机及可读存储介质 | |
| CN115655311A (zh) | 一种基于扫描匹配的阿克曼型机器人里程计标定方法 | |
| Gao et al. | Design of mobile robot based on cartographer SLAM algorithm | |
| CN115855026A (zh) | 一种大型场景图元地图切换方法、系统、装置及存储介质 | |
| CN113091743B (zh) | 机器人的室内定位方法及装置 | |
| CN114526740A (zh) | 基于自然物体的单目自动驾驶导航方法、系统及装置 | |
| Pan et al. | Trajectory tracking method of crawler robot based on improved loam |
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 |