WO2022073172A1 - 一种基于点线特征的机器人全局最优视觉定位方法及装置 - Google Patents
一种基于点线特征的机器人全局最优视觉定位方法及装置 Download PDFInfo
- Publication number
- WO2022073172A1 WO2022073172A1 PCT/CN2020/119891 CN2020119891W WO2022073172A1 WO 2022073172 A1 WO2022073172 A1 WO 2022073172A1 CN 2020119891 W CN2020119891 W CN 2020119891W WO 2022073172 A1 WO2022073172 A1 WO 2022073172A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- point
- translation
- dimensional
- feature
- robot
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 230000004807 localization Effects 0.000 title abstract description 6
- 238000013519 translation Methods 0.000 claims abstract description 109
- 238000005259 measurement Methods 0.000 claims abstract description 20
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 13
- 230000009467 reduction Effects 0.000 claims abstract description 6
- 230000014616 translation Effects 0.000 claims description 107
- 230000001133 acceleration Effects 0.000 claims description 30
- 230000000007 visual effect Effects 0.000 claims description 30
- 238000001514 detection method Methods 0.000 claims description 13
- 239000013598 vector Substances 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000012886 linear function Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- 230000000694 effects Effects 0.000 abstract description 4
- 230000004927 fusion Effects 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 28
- 238000005516 engineering process Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000012384 transportation and delivery Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
- B25J9/1697—Vision controlled systems
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
-
- G05D1/246—
-
- G05D1/249—
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Electromagnetism (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Image Processing (AREA)
Abstract
一种基于点线特征的机器人全局最优视觉定位方法及装置,该方法包括:获取事先构建的当前场景的先验三维地图;获取机器人的当前图像和惯性传感器的测量数据;根据当前惯性传感器数据与先验三维地图中的惯性传感器数据计算当前机器人位姿的俯仰角和翻滚角;根据当前图像中检测到的二维点线特征与先验三维地图中的三维点线特征进行匹配;根据匹配到的特征对将待求位姿的旋转与平移分离,先求解旋转再求解平移,从而完成搜索空间的降维;将六维相机位姿空间的搜索降低为4个一维空间的搜索,在保证取得最优解的同时保证了计算速度;利用多传感器的融合,在算法中充分发挥各个传感器的优势信息,最终达到提升定位精度、鲁棒性的效果。
Description
本发明属于机器人定位技术领域,尤其涉及一种基于点线特征的机器人全局最优视觉定位方法及装置。
随着移动机器人技术的发展,移动机器人执行任务的种类越来越多,与人类生活的联系也越来越密切。目前,移动机器人的应用范围包括仓储物流搬运、快递配送、清洁、巡检监控以及无人驾驶等等,极大地推动了这些行业的变革和无人化进程。对于机器人的要求是能够在一个相对固定的环境内,实现高精度且长期稳定的自定位是自主移动机器人完成任务的前提。
视觉定位技术相对于距离传感器虽然成熟度较低,但相机为人机交互提供直观环境信息,是不可缺少的传感器单元;相机的成本很低,多个相机的成本也要低于单个激光,且轻量易集成;相机视野虽窄,但分辨率很高,能够充分描述环境的表观信息,可提供较好的特征提取和描述。因此,视觉传感器生产成本低,获取信息量大,相关定位方法受到广泛的研究与应用。
尽管视觉定位存在以上优势,基于视觉定位的移动机器人要实现真正长期可靠的运行,满足动态多变环境变化要求仍然有一些问题亟待解决:视觉视野窄,对动态物体遮挡、环境无纹理等问题鲁棒性差;环境光照、天气和季节变化引起的环境外观的改变会直接影响视觉信息;故传统基于特征点匹配进行相机基础矩阵估计的视觉定位方法容易受到视角变化、动态遮挡、环境光照以及气候变化等问题的影响。以上情况均造成了视觉提取特征数量减少、难以重复检测且特征匹配的错误率高等问题,对视觉定位算法的鲁棒性提出了极大挑战。
当环境变化比较大,匹配得到的视觉特征的错误率达到80%及以上的情况下,基于RAN SAC的方法难以保证位姿估计的成功率。现有技术中,基于BnB的保证最优算法可以保证取得在已有特征匹配情况下的最优解;但多是在6DoF空间进行枚举搜索,速度比较慢。
发明内容
本发明的目的在于针对现有技术的不足,提供一种基于点线特征的机器人全局最优视觉定位方法及装置。
本发明是通过以下技术方案来实现的:一种基于点线特征的机器人全局最优视觉定位方法,包括以下步骤:
(1)获取当前场景的先验三维地图,包含重力加速度的测量数据和三维点线特征;
(2)获取机器人的当前相机图像和测量的重力加速度;
(3)根据机器人当前测量的重力加速度与先验三维地图中重力加速度的测量数据,计算当前机器人位姿的俯仰角和翻滚角;对机器人当前相机图像进行二维点线特征的检测,并将检测到的二维点线特征与先验三维地图中的三维点线特征进行匹配;
(4)根据匹配到的二维到三维点线特征构造平移无关度量,将机器人位姿求解问题解耦为旋转和平移两个子问题,即先求解旋转未知量,再求解平移未知量;其中,旋转未知量为偏航角;偏航角由分支定界算法完成一维搜索枚举进行求解,平移未知量是分别对其三个方向的分量进行一维搜索投票来求解;最终求得全局位姿估计最优解。
进一步地,所述步骤(3)中,将机器人坐标系下测量的重力加速度[x
c y
c z
c]
T和先验三维地图坐标系下测量的重力加速度[x
w y
w z
w]
T进行对齐,得到当前机器人坐标系相对于先验三维地图坐标系的俯仰角β和翻滚角γ,具体为:
进一步地,所述步骤(3)中,计算机器人当前图像的二维点线特征和先验三维地图的三维点线特征相应描述子在某度量空间的距离,将该距离小于预设阈值的特征进行匹配,获取多对匹配特征。
进一步地,所述步骤(4)中,三维地图点p
i和匹配对应的二维图像点u
i满足如下关系:
u
i=π(Rp
i+t,K)+o
i+e
i
其中,R是旋转矩阵,π是相机内参K的相机投影方程,e
i是观测噪声;错误的匹配为外点,正确的匹配为内点,对于内点o
i为零,对于外点o
i是任意值;将机器人位姿估计问题转化为最大化一致集问题:
将该二维图像点u
i对应的三维地图点p
i转化到相机坐标系下:
d
p(α)=d
p,1sinα+d
p,2cosα+d
p,3
其中,d
p,1、d
p,2、d
p,3是d
p(α)的系数;α是偏航角;将机器人位姿估计问题转化为如下最大化一致集问题:
其中,n
ij=min(n
i,n
j);z
ij=1表示第i组和第j组点特征匹配都是内点,否则z
ij=0。
从上式得到一个约束,任取该三维线段上另外一点,再得到一个约束,联立以上两个约束消去平移量t,得到由线特征匹配推导的平移无关度量d
l(α):
d
l(α)=d
l,1sinα+d
l,2cosα+d
l,3
其中,d
l,1、d
l,2、d
l,3是d
l(α)的系数;将机器人位姿估计问题转化为如下最大化一致集问题:
进一步地,所述步骤(4)中,将d
p(α)和d
l(α)统一为平移无关度量d(α):
其中,d
1,d
2,d
3为d(α)的系数;偏航角α的平移无关度量的下界
d(A)为:
d(A)=min|a
1sin(α+a
2)+d
3|
其中,A是包含偏航角α的区间子集,α∈A;
d(A)对应的最大化一致集问题为:
整个全局最优旋转求解步骤为:把偏航角的范围[-π,π]初始化到多个子集Α中,多个子集Α构成序列q,初始化能量函数最优值和偏航角最优值;在q为空前,循环执行如下操作:取出q的第一个子集Α计算
如果
大于当前能量函数最优值,则根据该子集Α的中心α
c计算E(α
c),如果E(α
c)仍大于当前能量函数最优值,则更新当前能量函数最优值为E(α
c),当前偏航角最优值为α
c;否则继续遍历剩余的子集Α,直到q为空,返回最终的偏航角最优值。
进一步地,所述步骤(4)中,已知偏航角,则关于平移量的最大化一致集问题为:
其中,A
i和b
i是已知偏航角对应的关于平移量的线性函数的系数;
根据一组至少有一个点特征的三维到二维匹配特征将关于平移量的最大化一致集问题更 新为:
同理可得关于yz方向的平移量的最大化一致集问题;根据几何约束,遍历所有特征组合,得到所有可能的
及其对应的上界
和下界
t
ij;通过对所有
进行投票,得到一致估计结果的特征匹配支持集,进而得到全局最优的平移估计;
整个全局最优平移求解步骤为:根据所有平移估计结果和其对应上下界,初始化能量函数最优值为0,最优内点集合为空;对平移的x分量进行投票并得到全部投票结果和对应的投票特征集合S
x,并按特征集合的大小进行降序排列;遍历S
x中的每个投票结果和特征集合S
x([i]):若S
x([i])的特征集合大小小于当前能量函数最优值则结束,不小于则继续下一步;对于S
x([i])的特征集合内的所有元素在平移y分量进行投票,得到集合S
y;遍历S
y中的每个投票集合和特征集合S
y([j]):若S
y([j])的特征集合大小小于当前能量函数最优值则结束,不小于则继续下一步;对于S
y([j])的特征集合内的所有元素在平移z分量进行投票,得到集合S
z;若S
z里最大投票数结果对应的投票特征集合的大小大于当前能量函数最优值,则将较大的值更新为当前能量函数最优值,将对应的特征集合更新为当前最优内点集合,继续遍历;否则不大于则不更新直接继续遍历。
进一步地,所述二维点线特征包括二维点特征和二维线特征;所述二维点特征的检测方法包括ORB、SIFT、LBP和HOG特征点检测算法;所述二维线特征的检测方法包括LBD、Edline和Hough直线检测;所述某度量空间的距离为欧式距离。
一种基于上述方法的机器人全局最优视觉定位装置,包括:
第一获取模块,用于获取当前场景的先验三维地图和重力加速度的测量数据;
第二获取模块,用于获取机器人的当前相机图像和测量的重力加速度;
第一计算模块,用于根据机器人测量的重力加速度与先验三维地图中重力加速度的测量数据,计算机器人位姿的俯仰角和翻滚角;
匹配模块,用于根据机器人当前相机图像检测到的二维点线特征与先验三维地图中的三 维点线特征进行匹配,得到多对二维到三维的点线特征匹配对;
第二计算模块,用于根据获得的点线特征匹配对将待求机器人位姿的旋转与平移分离,先求解旋转,再求解平移,从而完成搜索空间的降维,求得精确的估计结果。
相比现有技术,本发明的有益效果在于:
本发明在定位时同时利用环境中的点特征和线特征,最大化利用了视觉提供的信息,应用场景更加广泛。线特征相比于点特征是一种更高级的语义信息,具有更稳定的检测效果,即使在部分遮挡的情况下也能检测出部分线段,对光照以及气候变化等不敏感,可以很大程度上弥补点特征的不足。尤其在工厂等视觉点特征不那么丰富的人造环境中,线特征往往更易于检测且数量更多,其优势更为明显。在野外自然环境中,线特征不那么明显的情况下,本发明还支持两对特征点定位方法,仅需在机器人当前图像中匹配到两组对应的地图上的三维特征点就能实现对机器人位姿的准确估计,其适用场景非常广泛,可以有效提高机器人对环境的自适应能力。
本发明研究了基于BnB的保证最优算法,提出将待求位姿的旋转与平移分离,先求解旋转,再求解平移,从而完成搜索空间的降维,将六维相机位姿空间的搜索降低为4个一维空间的搜索,在保证取得最优解的同时,提高运算速度。本发明利用多传感器的融合,在算法中充分发挥各个传感器的优势信息,最终达到提升定位精度、鲁棒性的效果。
图1为本发明基于点线特征的机器人全局最优视觉定位方法的流程示意图。
下面结合附图以及具体实施方式,对本发明的技术方案做进一步地描述。
本发明根据两组点特征匹配关系,可得一个只与旋转未知量相关的方程;对应的,在一组线段匹配情况下也可得出一个只与旋转有关的方程;于是,本发明中旋转和平移的求解就可以被分离为先求解旋转,再求解平移;另外利用惯性测量数据,我们可以将旋转未知量降低为只有一个偏航角。其中,旋转未知量(偏航角)的求解采用的是分支定界算法(Branch and Bound,BnB),完成的是一维搜索枚举,而平移的三个分量分别进行一维搜索投票,最终求得全局位姿估计最优解。关键是给定待求状态空间范围,求得正确特征匹配数量的上下限。那么,根据点线特征推导的只与偏航角有关的方程,任意给定某偏航角的具体数值,并给定正确特征匹配的误差,就可以遍历所有点线特征,统计得到正确特征匹配的数量。同样,给出偏航角的某个取值范围,我们也可以方便的求出对应的方程的界限,从而求出在该范围内的最大正确特征匹配数量。所以,我们初始化待求偏航角的范围为(-π,π],通过不断二分缩小偏航角范围,寻找到使得正确特征匹配数量最大化的最优偏航角。
由于观测都伴随着一定程度的噪声,我们假定观测的图像特征的坐标的噪声是有界噪声,那么通过构造点共线和线共面的约束,在已经求得旋转的前提下,可以构造出平移未知量与特征匹配的约束。根据构造的约束,可以将观测上的有界噪声推导到待求平移上,从而得到待求平移的界。也就是说,给定某一点一线或者两点,根据几何约束,我们可以求得对应的平移量和其界限。那么遍历所有特征组合,可以得到所有可能的平移估计结果及其对应的界限。
图1为本发明基于点线特征的机器人全局最优视觉定位方法的流程示意图,包括以下步骤:
1.获取当前场景的先验三维地图,所述地图是由任意数量的相机和惯性传感器事先构建的,该地图要求包含重力加速度在所述地图坐标系下的测量数据,一般由惯性传感器测量所得,具体为在确定地图坐标系的同时保存惯性传感器测量的加速度的数据;所述三维地图还要包含三维点线特征,以便于后续进行定位时与机器人当前图像中的二维点线特征进行匹配,具体三维点线特征的生成不限方法,作为优选的实施方式,可以按如下步骤获取:三维点特征可以在建图时由图像检测到的二维点特征三角化得到,三维线特征可以直接在建好的三维地图里进行Line3Dpp算法处理得到;
2.获取机器人的当前图像和惯性传感器的测量数据;
3.根据当前机器人的惯性传感器数据与所述先验三维地图中的惯性传感器数据计算当前机器人位姿的俯仰角和翻滚角,具体计算方法如下:
所述机器人在当前图像中惯性传感器的测量数据为重力加速度在当前图像机器人坐标系下的测量值;所述先验三维地图中的惯性传感器的测量数据为重力加速度在先验三维地图坐标系下的测量值;将重力加速度在两个坐标系下的测量值进行对齐,从而得到当前机器人坐标系相对于地图坐标系的俯仰角和翻滚角,具体根据以下算式进行计算:
其中,[x
w y
w z
w]
T为重力加速度在先验三维地图坐标系下的测量值,[x
c y
c z
c]
T为重力坐标系在当前图像机器人坐标系下的测量值,β,γ分别为所求的当前图像机器人坐标系相对于先验三维地图坐标系的俯仰角和翻滚角。
4.根据当前相机图像中检测到的二维点线特征与所述先验三维地图中的三维点线特征进行匹配,具体实施步骤如下:
根据所述机器人获取的当前图像检测多个二维特征点,所述二维特征点的检测不限方法,作为优选的实施方式,具体可以为ORB,SIFT,LBP或HOG特征点检测算法;
根据所述机器人获取的当前图像检测多条二维线段,所述二维线段的检测不限方法,作为优选的实施方式,具体可以为LBD,Edline或Hough直线检测;
根据所述当前图像的二维点线特征和地图中的三维点线特征进行匹配,具体表现为计算相应描述子在某度量空间的距离,作为优选的实施方式,可以取为欧式距离;该距离小于预设阈值范围(本实施例为8)的特征则认为匹配成功,由此迭代获取多组二维与三维的匹配点线特征。
5.根据所述匹配到的二维到三维点特征和线特征计算当前机器人位姿的其余未知量,具体分为以下实施情况:
1)根据视觉惯性定位是一类从包含若干错误匹配(即为外点)的三维到二维特征匹配中进行位姿估计的问题,给定一组三维地图点p
i和一组对应二维视觉点u
i,它们满足:
u
i=π(Rp
i+t,K)+o
i+e
i (2)
其中,R是旋转矩阵,t是平移向量,π是已知相机内参K的相机投影方程;|e
i|<n
i是假设的观测噪声,n
i是观测噪声的界;对于正确的特征匹配(即为内点)o
i为零,对于外点o
i是任意值。为了处理这些外点,鲁棒位姿估计问题可以被转化为一个最大化一致集问题:
2)根据给定一个二维图像点u
i,我们可以从相机中心得到一个非归一化向量:
之后该二维图像点u
i对应的三维点p
i被转化到相机坐标系下,并且满足:
由以上公式(5)三维到二维的匹配点特征可以获得两个约束,给定另一组三维到二维的匹配特征u
j和p
j,我们可以获得新的两个约束:
联立以上公式(5)和(6)的约束,我们可以将平移量消掉。于是给定两组三维到二维的匹配点特征可以推导出一个平移无关度量:
d
p(α)=d
p,1sinα+d
p,2cosα+d
p,3 (7)
其中,d
p(α)是由点特征推导的平移无关度量的表示,d
p,1(α),d
p,2(α),d
p,3(α)是该平移无关度量d
p(α)的系数;α是未知的偏航角;由于全局的俯仰角和翻滚角在惯性测量下是已知的,该式中的未知量只有偏航角。这样我们就可以将前述问题转化为新的最大化一致集问题:
其中,i,j对应两对点特征匹配;d
p,ij(α)是由第i和第j个点特征匹配构造的平移无关度量d
p(α),R(α)代表由未知量偏航角α决定的旋转矩阵,n
ij=min(n
i,n
j);z
ij=1对应第i和第j个特征匹配都是内点,否则z
ij=0。
任取三维线段上另外一点,同理,根据公式(9)我们可以取得另一个方程,联立以上公式(9)对应的两个方程,可以消去平移量。于是一条三维到二维的匹配线段就可以给出一个平移无关度量:
d
l(α)=d
l,1sinα+d
l,2cosα+d
l,3 (10)
其中,d
l(α)是由线段特征推导的平移无关度量的表示,d
l,1,d
l,2,d
l,3是该平移无关度量d
l(α) 的系数。
联立以上公式(8)和(10)的信息,我们就可以将问题转化为新的最大化一致集问题:
其中,z
*是任一点线特征对应的z值的表示,z
k是第k个线特征对应的z值表示,z
ij是第i和第j个点特征对应的z值表示,即z
k和z
ij组成z
*;d
l,k(α)是由第k个线特征求得的平移无关度量d
l(α),d
p,ij(α)是由第i和第j个点特征求得的平移无关度量d
p(α),
是线特征的集合,n
k是第k个线特征对应的观测噪声的界,n
ij是第i和j个点特征对应的观测噪声的界。
4)根据式(11)推导的最大化一致集问题的目标函数,其中未知量包括偏航角α和所有点线特征对应的{z
*}集合,也就是一致集{z
i}。另外我们不难看出,α确定后,对应的一致集{z
i}也可以唯一确定。我们将此对应关系表示为能量函数E(α),表示的是当偏航角取α时对应的一致集内的特征数量。接下来,我们在偏航角的子集A(α∈A)下推导能量函数E(α)的上界,定义为
根据式(8)和(10),我们可以看到点特征和线特征推导得到的平移无关度量的形式是相同的,因此我们统称平移无关度量为d(α),并且有
那么,待求解的偏航角α的度量的下界
d(A)可以定义为:
d(A)=min|a
1sin(α+a
2)+d
3| (13)
其中,A是包含偏航角α的区间。
它对应的最大化一致集问题变为:
其中,
d
p,ij(A)是在角度区间A上的点特征构造的平移无关度量,
d
l,k(A)是在角度区间A上的线 特征构造的平移无关度量。
整个全局最优旋转优化求解步骤为:把偏航角的范围[-π,π]初始化到多个子集Α区间中,初始化能量函数和偏航角的最优估计值,初始化空的序列q,将所有子集Α区间插入到序列q中。在q为空之前,一直:取出q的第一个子集Α区间,按公式(15)计算能量函数的上界
如果上界大于能量函数的当前最优值,将子集Α区间的中心设为α
c,按公式(11)计算能量函数E(α
c)。如果能量函数E(α
c)大于能量函数的当前最优值,更新能量函数的最优值为E(α
c),偏航角的最优值为α
c;否则继续循环遍历所有的子集Α区间,直到q为空结束循环,返回偏航角最优值。
5)当旋转已经被估计,我们更新待求解的关于平移的最大化一致集问题为:
其中,A
i和b
i是由公式(5)和(9)代入最优旋转量α
c后得到的关于平移量的线性函数的系数。
对于一个三维到二维点特征的匹配,我们有两个关于平移的独立约束,具体由公式(5)导出,对于一个三维到二维线特征的匹配,我们有一个关于平移的约束,具体由公式(9)导出。所以给定一组至少有一个点特征的三维到二维匹配特征,包括第i个二维到三维点特征和第j个点特征或者线特征,足够求解平移变量
此时问题更新为:
我们将平移t解耦成xyz方向的3个分量,以x方向的平移分量t
x为例,我们有:
由于特征匹配中存在一定数量的正确特征匹配,正确特征匹配估计的平移是相一致的,而错误匹配估计的平移量则是杂乱无序的。通过对所有平移估计结果进行投票,我们便可以得到一致估计结果的特征匹配支持集进而得到全局最优的平移估计。
具体地,整个全局最优平移求解步骤为:计算所有平移估计结果和其对应上下界,初始化能量函数最优值为0,最优内点集合为空。对平移的x分量进行投票并得到全部投票结果和对应的投票特征集合S
x,并按特征集合的大小进行降序排列。遍历S
x中的每个投票结果和特征集合S
x([i]):若S
x([i])的特征集合大小小于当前最优能量函数值,结束;否则不小于,继续下一步。对于S
x([i])的特征集合内的所有元素在平移y分量进行投票,得到集合S
y;遍历S
y中的每个投票集合和特征集合S
y([j]):若S
y([j])的特征集合大小小于当前最优能量函数值,结束;否则不小于,继续下一步。对于S
y([j])的特征集合内的所有元素在平移z分量进行投票,得到集合S
z;若S
z里最大投票数结果对应的投票特征集合的大小大于当前最优能量函数值,更新最优能量函数值和最优内点集合,继续遍历;否则不大于,不更新直接继续遍历。直到碰到结束指令,返回最优内点集合及其对应平移值的均值作为平移最优值。
本发明一种机器人全局最优视觉定位装置,装置包括相机单元和惯性测量单元。其中,相机单元用于获取机器人当前图像;惯性测量单元用于测量重力加速度在当前机器人坐标系下的值,并与先验三维地图中的重力加速度测量值进行对齐,以得到机器人位姿的俯仰角和翻滚角。相机和惯性测量单元之间的相对关系已经事先标定得知。定位装置还包括处理器、存储器以及存储在存储器中的程序,计算机程序被处理器执行时实现机器人视觉惯性点线特征定位方法的步骤。
从相机单元中获取机器人当前图像;从惯性测量单元(IMU,包括陀螺仪和加速度计)中获取加速度和角加速度,步骤中的提取特征点,提取特征线段,进行视觉特征点以及特征线段与地图中的三维特征点和特征线段的匹配,根据匹配的特征点线计算机器人位姿等都属于算法,在机器人携带的处理器或电脑上完成。
本实施例中的方法与前述实施例中的装置是基于同一发明构思下的两个方面,本领域技术人员可根据前述描述清楚地了解本实施中的系统的结构及实施过程。
为了描述的方便,描述以上装置时以功能分为各种模块分别描述。当然,在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。具体各模块的功能如下:
第一获取模块,用于获取当前场景的先验三维地图,所述先验三维地图是事先构建的;
第二获取模块,用于获取机器人的当前相机图像和惯性传感器数据;
第一计算模块,用于根据所述获得的惯性传感数据与地图的惯性传感数据计算机器人位姿的俯仰角和翻滚角;
匹配模块,用于根据当前图像中检测到的二维点线特征与所述先验三维地图的三维点线特征进行匹配,得到多组二维到三维的点线特征匹配对;
第二计算模块,用于根据所述获得的点线特征匹配对将待求位姿的旋转与平移分离,先求解旋转,再求解平移,从而完成搜索空间的降维,求得精确的估计结果。
通过以上的实施方式的描述可知,本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品可以存储在存储介质中,如ROM/RAM、磁碟、光盘等,包括若干指令用以使得一台计算机设备执行本发明各个实施例或者实施例的某些部分所述的方法,计算机设备可以是个人计算机,服务器,或者网络设备等。
描述的装置实施例仅仅是示意性的,其中所述作为分离部件说明的模块或单元可以是或者也可以不是物理上分开的,作为模块或单元示意的部件可以是或者也可以不是物理模块,既可以位于一个地方,或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下,即可以理解并实施。
本发明可用于众多通用或专用的计算系统环境或配置中。例如:个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等。
本发明的装置可以包括存储器200、处理器300以及存储在存储器200中的程序,所述程序被配置成由处理器300执行,处理器300执行所述程序时实现上述机器人视觉惯性实时定位方法的步骤。
上述实施方式仅为本发明的优选实施方式,不能以此来限定本发明保护的范围,本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。
Claims (10)
- 一种基于点线特征的机器人全局最优视觉定位方法,其特征在于,包括以下步骤:(1)获取当前场景的先验三维地图,包含重力加速度的测量数据和三维点线特征;(2)获取机器人的当前相机图像和测量的重力加速度;(3)根据机器人当前测量的重力加速度与先验三维地图中重力加速度的测量数据,计算当前机器人位姿的俯仰角和翻滚角;对机器人当前相机图像进行二维点线特征的检测,并将检测到的二维点线特征与先验三维地图中的三维点线特征进行匹配;(4)根据匹配到的二维到三维点线特征构造平移无关度量,将机器人位姿求解问题解耦为旋转和平移两个子问题,即先求解旋转未知量,再求解平移未知量;其中,旋转未知量为偏航角;偏航角由分支定界算法完成一维搜索枚举进行求解,平移未知量是分别对其三个方向的分量进行一维搜索投票来求解;最终求得全局位姿估计最优解。
- 如权利要求2所述基于点线特征的机器人全局最优视觉定位方法,其特征在于,所述步骤(3)中,计算机器人当前图像的二维点线特征和先验三维地图的三维点线特征相应描述子在某度量空间的距离,将该距离小于预设阈值的特征进行匹配,获取多对匹配特征。
- 将该二维图像点u i对应的三维地图点p i转化到相机坐标系下:d p(α)=d p,1sinα+d p,2cosα+d p,3其中,d p,1、d p,2、d p,3是d p(α)的系数;α是偏航角;将机器人位姿估计问题转化为如下最大化一致集问题:其中,n ij=min(n i,n j);z ij=1表示第i组和第j组点特征匹配都是内点,否则z ij=0。
- 从上式得到一个约束,任取该三维线段上另外一点,再得到一个约束,联立以上两个约束消去平移量t,得到由线特征匹配推导的平移无关度量d l(α):d l(α)=d l,1sinα+d l,2cosα+d l,3其中,d l,1、d l,2、d l,3是d l(α)的系数;将机器人位姿估计问题转化为如下最大化一致集问题:
- 如权利要求6所述基于点线特征的机器人全局最优视觉定位方法,其特征在于,所述步骤(4)中,将d p(α)和d l(α)统一为平移无关度量d(α):其中,d 1,d 2,d 3为d(α)的系数;偏航角α的平移无关度量的下界 d(A)为:d(A)=min|a 1sin(α+a 2)+d 3|其中,A是包含偏航角α的区间子集,α∈A; d(A)对应的最大化一致集问题为:
- 如权利要求7所述基于点线特征的机器人全局最优视觉定位方法,其特征在于,所述步骤(4)中,已知偏航角,则关于平移量的最大化一致集问题为:其中,A i和b i是已知偏航角对应的关于平移量的线性函数的系数;根据一组至少有一个点特征的三维到二维匹配特征将关于平移量的最大化一致集问题更新为:同理可得关于yz方向的平移量的最大化一致集问题;根据几何约束,遍历所有特征组合,得到所有可能的 及其对应的上界 和下界 t ij;通过对所有 进行投票,得到一致估计结果的特征匹配支持集,进而得到全局最优的平移估计;整个全局最优平移求解步骤为:根据所有平移估计结果和其对应上下界,初始化能量函数最优值为0,最优内点集合为空;对平移的x分量进行投票并得到全部投票结果和对应的投票特征集合S x,并按特征集合的大小进行降序排列;遍历S x中的每个投票结果和特征集合S x([i]):若S x([i])的特征集合大小小于当前能量函数最优值则结束,不小于则继续下一步;对于S x([i])的特征集合内的所有元素在平移y分量进行投票,得到集合S y;遍历S y中的每个投票集合和特征集合S y([j]):若S y([j])的特征集合大小小于当前能量函数最优值则结束,不小于则继续下一步;对于S y([j])的特征集合内的所有元素在平移z分量进行投票,得到集合S z;若S z里最大投票数结果对应的投票特征集合的大小大于当前能量函数最优值,则将较大的值更新为当前能量函数最优值,将对应的特征集合更新为当前最优内点集合,继续遍历;否则不大于则不更新直接继续遍历。
- 如权利要求1所述基于点线特征的机器人全局最优视觉定位方法,其特征在于,所述二维点线特征包括二维点特征和二维线特征;所述二维点特征的检测方法包括ORB、SIFT、LBP和HOG特征点检测算法;所述二维线特征的检测方法包括LBD、Edline和Hough直线检测;所述某度量空间的距离为欧式距离。
- 一种基于权利要求1所述基于点线特征的机器人全局最优视觉定位方法的装置,其特征在于,包括:第一获取模块,用于获取当前场景的先验三维地图和重力加速度的测量数据;第二获取模块,用于获取机器人的当前相机图像和测量的重力加速度;第一计算模块,用于根据机器人测量的重力加速度与先验三维地图中重力加速度的测量数据,计算机器人位姿的俯仰角和翻滚角;匹配模块,用于根据机器人当前相机图像检测到的二维点线特征与先验三维地图中的三维点线特征进行匹配,得到多对二维到三维的点线特征匹配对;第二计算模块,用于根据获得的点线特征匹配对将待求机器人位姿的旋转与平移分离,先求解旋转,再求解平移,从而完成搜索空间的降维,求得精确的估计结果。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
PCT/CN2020/119891 WO2022073172A1 (zh) | 2020-10-09 | 2020-10-09 | 一种基于点线特征的机器人全局最优视觉定位方法及装置 |
US17/489,738 US11964401B2 (en) | 2020-10-09 | 2021-09-29 | Robot globally optimal visual positioning method and device based on point-line features |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
PCT/CN2020/119891 WO2022073172A1 (zh) | 2020-10-09 | 2020-10-09 | 一种基于点线特征的机器人全局最优视觉定位方法及装置 |
Related Child Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
US17/489,738 Continuation US11964401B2 (en) | 2020-10-09 | 2021-09-29 | Robot globally optimal visual positioning method and device based on point-line features |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2022073172A1 true WO2022073172A1 (zh) | 2022-04-14 |
Family
ID=81078580
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/CN2020/119891 WO2022073172A1 (zh) | 2020-10-09 | 2020-10-09 | 一种基于点线特征的机器人全局最优视觉定位方法及装置 |
Country Status (2)
Country | Link |
---|---|
US (1) | US11964401B2 (zh) |
WO (1) | WO2022073172A1 (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115187667B (zh) * | 2022-09-08 | 2022-12-20 | 中国科学院合肥物质科学研究院 | 一种基于认知理解的大场景精准定位方法及系统 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107871327A (zh) * | 2017-10-23 | 2018-04-03 | 武汉大学 | 基于特征点线的单目相机位姿估计和优化方法及系统 |
CN109558879A (zh) * | 2017-09-22 | 2019-04-02 | 华为技术有限公司 | 一种基于点线特征的视觉slam方法和装置 |
CN109752003A (zh) * | 2018-12-26 | 2019-05-14 | 浙江大学 | 一种机器人视觉惯性点线特征定位方法及装置 |
KR20190104304A (ko) * | 2019-09-03 | 2019-09-09 | 주식회사 모빌테크 | 점군을 포함하는 라이다 지도 생성 방법 |
CN110570473A (zh) * | 2019-09-12 | 2019-12-13 | 河北工业大学 | 一种基于点线融合的权重自适应位姿估计方法 |
CN111283730A (zh) * | 2020-03-03 | 2020-06-16 | 广州赛特智能科技有限公司 | 基于点线特征机器人初始位姿获取方法及开机自定位方法 |
CN111429574A (zh) * | 2020-03-06 | 2020-07-17 | 上海交通大学 | 基于三维点云和视觉融合的移动机器人定位方法和系统 |
CN111583316A (zh) * | 2020-04-28 | 2020-08-25 | 清华大学 | 一种视觉自主定位系统实现方法 |
CN111595334A (zh) * | 2020-04-30 | 2020-08-28 | 东南大学 | 基于视觉点线特征与imu紧耦合的室内自主定位方法 |
CN111679291A (zh) * | 2020-06-17 | 2020-09-18 | 四川智动木牛智能科技有限公司 | 基于三维激光雷达的巡检机器人目标定位配置方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3759562B1 (en) * | 2018-03-02 | 2022-12-28 | DeepMap Inc. | Camera based localization for autonomous vehicles |
US11748449B2 (en) * | 2020-11-25 | 2023-09-05 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Data processing method, data processing apparatus, electronic device and storage medium |
US20220164595A1 (en) * | 2020-11-25 | 2022-05-26 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Method, electronic device and storage medium for vehicle localization |
-
2020
- 2020-10-09 WO PCT/CN2020/119891 patent/WO2022073172A1/zh active Application Filing
-
2021
- 2021-09-29 US US17/489,738 patent/US11964401B2/en active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109558879A (zh) * | 2017-09-22 | 2019-04-02 | 华为技术有限公司 | 一种基于点线特征的视觉slam方法和装置 |
CN107871327A (zh) * | 2017-10-23 | 2018-04-03 | 武汉大学 | 基于特征点线的单目相机位姿估计和优化方法及系统 |
CN109752003A (zh) * | 2018-12-26 | 2019-05-14 | 浙江大学 | 一种机器人视觉惯性点线特征定位方法及装置 |
KR20190104304A (ko) * | 2019-09-03 | 2019-09-09 | 주식회사 모빌테크 | 점군을 포함하는 라이다 지도 생성 방법 |
CN110570473A (zh) * | 2019-09-12 | 2019-12-13 | 河北工业大学 | 一种基于点线融合的权重自适应位姿估计方法 |
CN111283730A (zh) * | 2020-03-03 | 2020-06-16 | 广州赛特智能科技有限公司 | 基于点线特征机器人初始位姿获取方法及开机自定位方法 |
CN111429574A (zh) * | 2020-03-06 | 2020-07-17 | 上海交通大学 | 基于三维点云和视觉融合的移动机器人定位方法和系统 |
CN111583316A (zh) * | 2020-04-28 | 2020-08-25 | 清华大学 | 一种视觉自主定位系统实现方法 |
CN111595334A (zh) * | 2020-04-30 | 2020-08-28 | 东南大学 | 基于视觉点线特征与imu紧耦合的室内自主定位方法 |
CN111679291A (zh) * | 2020-06-17 | 2020-09-18 | 四川智动木牛智能科技有限公司 | 基于三维激光雷达的巡检机器人目标定位配置方法 |
Also Published As
Publication number | Publication date |
---|---|
US11964401B2 (en) | 2024-04-23 |
US20220111531A1 (en) | 2022-04-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11668571B2 (en) | Simultaneous localization and mapping (SLAM) using dual event cameras | |
WO2021232470A1 (zh) | 基于多传感器融合的slam制图方法、系统 | |
CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
CN111258313B (zh) | 多传感器融合slam系统及机器人 | |
CN109752003B (zh) | 一种机器人视觉惯性点线特征定位方法及装置 | |
Svärm et al. | City-scale localization for cameras with known vertical direction | |
CN112634451B (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
CN111275763B (zh) | 闭环检测系统、多传感器融合slam系统及机器人 | |
CN112200869B (zh) | 一种基于点线特征的机器人全局最优视觉定位方法及装置 | |
CN111791235B (zh) | 一种机器人多相机视觉惯性点线特征定位方法及装置 | |
CN112219087A (zh) | 位姿预测方法、地图构建方法、可移动平台及存储介质 | |
CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质 | |
CN107818598B (zh) | 一种基于视觉矫正的三维点云地图融合方法 | |
WO2020063878A1 (zh) | 一种处理数据的方法和装置 | |
CN111882602B (zh) | 基于orb特征点和gms匹配过滤器的视觉里程计实现方法 | |
WO2022073172A1 (zh) | 一种基于点线特征的机器人全局最优视觉定位方法及装置 | |
CN114092564B (zh) | 无重叠视域多相机系统的外参数标定方法、系统、终端及介质 | |
CN114429487A (zh) | 基于平面运动的机器人视觉特征匹配和定位方法及装置 | |
CN113223163A (zh) | 点云地图构建方法及装置、设备、存储介质 | |
Hu et al. | Efficient Visual-Inertial navigation with point-plane map | |
Piekenbrock et al. | Automated aerial refueling: Parallelized 3D iterative closest point: Subject area: Guidance and control | |
CN114119885A (zh) | 图像特征点匹配方法、装置及系统、地图构建方法及系统 | |
CN116524011B (zh) | 一种居家环境下机器人对目标位姿的精化方法 | |
Zhuang et al. | 3D-laser-based visual odometry for autonomous mobile robot in outdoor environments | |
He et al. | An Indoor Positioning Approach based on Vision and INS |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 20956487 Country of ref document: EP Kind code of ref document: A1 |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 20956487 Country of ref document: EP Kind code of ref document: A1 |