CN112200869B - 一种基于点线特征的机器人全局最优视觉定位方法及装置 - Google Patents

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是任意值；将机器人位姿估计问题转化为最大化一致集问题：

其中，t是平移向量；z_i＝0表示o_i是0，z_i＝1表示o_i不是0；n_i＞|e_i|是观测噪声的界；是三维到二维点特征匹配的集合。

进一步地，所述步骤(4)中，根据二维图像点u_i从相机中心得到非归一化向量

将该二维图像点u_i对应的三维地图点p_i转化到相机坐标系下：

其中，由上式获得两个约束，同理由另一组三维到二维的匹配特征再获得两个约束，联立以上四个约束将平移量t消掉，得到由点特征匹配推导的平移无关度量d_p(α)：

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。

进一步地，所述步骤(4)中，二维线段的两端和/>以及对应的三维线段上一点p_k满足如下关系：

从上式得到一个约束，任取该三维线段上另外一点，再得到一个约束，联立以上两个约束消去平移量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(α)的系数；将机器人位姿估计问题转化为如下最大化一致集问题：

其中，z*用于指代z_ij和z_k；z_k＝1表示第k个线特征对应的特征匹配是内点，否则z_k＝0；是线特征的集合，n_k是第k个线特征对应的观测噪声的界。

进一步地，所述步骤(4)中，将d_p(α)和d_l(α)统一为平移无关度量d(α)：

其中，d₁,d₂,d₃为d(α)的系数；偏航角α的平移无关度量的下界d(A)为：

d(A)＝min|a₁sin(α+a₂)+d₃|

其中，A是包含偏航角α的区间子集，α∈A；d(A)对应的最大化一致集问题为：

其中，d _p,ij(A)是在A上的点特征推导的平移无关度量，d _l,k(A)是在A上的线特征推导的平移无关度量；将偏航角α对应的一致集内的内点数量记为能量函数E(α)，E(α)的上界记为

其中，是求解最大化一致集问题得到最优一致集的内点数量；

整个全局最优旋转求解步骤为：把偏航角的范围[-π,π]初始化到多个子集Α中，多个子集Α构成序列q，初始化能量函数最优值和偏航角最优值；在q为空前，循环执行如下操作：取出q的第一个子集Α计算如果/>大于当前能量函数最优值，则根据该子集Α的中心α_c计算E(α_c)，如果E(α_c)仍大于当前能量函数最优值，则更新当前能量函数最优值为E(α_c)，当前偏航角最优值为α_c；否则继续遍历剩余的子集Α，直到q为空，返回最终的偏航角最优值。

进一步地，所述步骤(4)中，已知偏航角，则关于平移量的最大化一致集问题为：

其中，A_i和b_i是已知偏航角对应的关于平移量的线性函数的系数；

根据一组至少有一个点特征的三维到二维匹配特征将关于平移量的最大化一致集问题更新为：

其中，是平移量的估计；将平移量解耦成xyz方向的三个分量，关于x方向的平移量的最大化一致集问题为：

同理可得关于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是任意值。为了处理这些外点，鲁棒位姿估计问题可以被转化为一个最大化一致集问题：

其中，z_i是0或1，用来指代o_i是否是0；是三维到二维点特征匹配的集合。

2)根据给定一个二维图像点u_i，我们可以从相机中心得到一个非归一化向量：

其中，是该二维图像点u_i的非归一化向量表示，/>是该向量/>的横坐标，/>是该向量/>的纵坐标。

之后该二维图像点u_i对应的三维点p_i被转化到相机坐标系下，并且满足：

其中，也就是说R₁,R₂,R₃是旋转矩阵R的三个行向量，t_x,t_y,t_z是平移向量t的三个坐标轴分量。

由以上公式(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。

3)根据给定一个三维线段上一点p_k，该三维线段对应的二维线段的两端为和我们有

任取三维线段上另外一点，同理，根据公式(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₁,d₂,d₃为统一后的平移无关度量d(α)的系数，并且

那么，待求解的偏航角α的度量的下界d(A)可以定义为：

d(A)＝min|a₁sin(α+a₂)+d₃| (13)

其中，A是包含偏航角α的区间。

它对应的最大化一致集问题变为：

其中，d _p,ij(A)是在角度区间A上的点特征构造的平移无关度量，d _l,k(A)是在角度区间A上的线特征构造的平移无关度量。

由此我们可推导得到严格的上界

其中，是公式(14)取得最大值时对应的z值的集合，即为最优一致集。

整个全局最优旋转优化求解步骤为：把偏航角的范围[-π,π]初始化到多个子集Α区间中，初始化能量函数和偏航角的最优估计值，初始化空的序列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的各个维度实现了解耦。

我们将平移t解耦成xyz方向的3个分量，以x方向的平移分量t_x为例，我们有：

据上式(18)可以得到三个维度(对应三个分量)的线性最大化一致集问题。我们根据几何约束，求得对应的平移量和其上下界。遍历所有特征组合，可以得到所有可能的平移估计结果及其对应的上下界/> 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里最大投票数结果对应的投票特征集合的大小大于当前最优能量函数值，更新最优能量函数值和最优内点集合，继续遍历；否则不大于，不更新直接继续遍历。直到碰到结束指令，返回最优内点集合及其对应平移值的均值作为平移最优值。

本发明一种机器人全局最优视觉定位装置，装置包括相机单元和惯性测量单元。其中，相机单元用于获取机器人当前图像；惯性测量单元用于测量重力加速度在当前机器人坐标系下的值，并与先验三维地图中的重力加速度测量值进行对齐，以得到机器人位姿的俯仰角和翻滚角。相机和惯性测量单元之间的相对关系已经事先标定得知。定位装置还包括处理器、存储器以及存储在存储器中的程序，计算机程序被处理器执行时实现机器人视觉惯性点线特征定位方法的步骤。

从相机单元中获取机器人当前图像；从惯性测量单元(IMU，包括陀螺仪和加速度计)中获取加速度和角加速度，步骤中的提取特征点，提取特征线段，进行视觉特征点以及特征线段与地图中的三维特征点和特征线段的匹配，根据匹配的特征点线计算机器人位姿等都属于算法，在机器人携带的处理器或电脑上完成。

本实施例中的方法与前述实施例中的装置是基于同一发明构思下的两个方面，本领域技术人员可根据前述描述清楚地了解本实施中的系统的结构及实施过程。

为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。具体各模块的功能如下：

第一获取模块，用于获取当前场景的先验三维地图，所述先验三维地图是事先构建的；

第二获取模块，用于获取机器人的当前相机图像和惯性传感器数据；

第一计算模块，用于根据所述获得的惯性传感数据与地图的惯性传感数据计算机器人位姿的俯仰角和翻滚角；

匹配模块，用于根据当前图像中检测到的二维点线特征与所述先验三维地图的三维点线特征进行匹配，得到多组二维到三维的点线特征匹配对；

第二计算模块，用于根据所述获得的点线特征匹配对将待求位姿的旋转与平移分离，先求解旋转，再求解平移，从而完成搜索空间的降维，求得精确的估计结果。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本发明可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备执行本发明各个实施例或者实施例的某些部分所述的方法，计算机设备可以是个人计算机，服务器，或者网络设备等。

描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的模块或单元可以是或者也可以不是物理上分开的，作为模块或单元示意的部件可以是或者也可以不是物理模块，既可以位于一个地方，或者也可以分布到多个网络模块上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明可用于众多通用或专用的计算系统环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器系统、基于微处理器的系统、机顶盒、可编程的消费电子设备、网络PC、小型计算机、大型计算机、包括以上任何系统或设备的分布式计算环境等等。

本发明的装置可以包括存储器200、处理器300以及存储在存储器200中的程序，所述程序被配置成由处理器300执行，处理器300执行所述程序时实现上述机器人视觉惯性实时定位方法的步骤。

上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。

Claims (7)

1.一种基于点线特征的机器人全局最优视觉定位方法，其特征在于，包括以下步骤：

(1)获取当前场景的先验三维地图，包含重力加速度的测量数据和三维点线特征；

(2)获取机器人的当前相机图像和测量的重力加速度；

(3)根据机器人当前测量的重力加速度与先验三维地图中重力加速度的测量数据，计算当前机器人位姿的俯仰角和翻滚角；对机器人当前相机图像进行二维点线特征的检测，并将检测到的二维点线特征与先验三维地图中的三维点线特征进行匹配；

(4)根据匹配到的二维到三维点线特征构造平移无关度量，将机器人位姿求解问题解耦为旋转和平移两个子问题，即先求解旋转未知量，再求解平移未知量；其中，旋转未知量为偏航角；偏航角由分支定界算法完成一维搜索枚举进行求解，平移未知量是分别对其三个方向的分量进行一维搜索投票来求解；最终求得全局位姿估计最优解；

所述步骤(4)中，三维地图点p_i和匹配对应的二维图像点u_i满足如下关系：

u_i＝π(Rp_i+t，K)+o_i+e_i

其中，R是旋转矩阵，π是相机内参K的相机投影方程，e_i是观测噪声；错误的匹配为外点，正确的匹配为内点，对于内点o_i为零，对于外点o_i是任意值；将机器人位姿估计问题转化为最大化一致集问题：

其中，t是平移向量；z_i＝0表示o_i是0，z_i＝1表示o_i不是0；n_i>|e_i|是观测噪声的界；是三维到二维点特征匹配的集合；

所述步骤(4)中，根据二维图像点u_i从相机中心得到非归一化向量

将该二维图像点u_i对应的三维地图点p_i转化到相机坐标系下：

其中，由上式获得两个约束，同理由另一组三维到二维的匹配特征再获得两个约束，联立以上四个约束将平移量t消掉，得到由点特征匹配推导的平移无关度量d_p(α)：

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；

所述步骤(4)中，二维线段的两端和/>以及对应的三维线段上一点p_k满足如下关系：

从上式得到一个约束，任取该三维线段上另外一点，再得到一个约束，联立以上两个约束消去平移量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(α)的系数；将机器人位姿估计问题转化为如下最大化一致集问题：

其中，z_*用于指代z_ij和z_k；z_k＝1表示第k个线特征对应的特征匹配是内点，z_ij＝1表示第i和j个点特征对应的特征匹配是内点；是线特征的集合，n_k是第k个线特征对应的观测噪声的界。

2.如权利要求1所述基于点线特征的机器人全局最优视觉定位方法，其特征在于，所述步骤(3)中，将机器人坐标系下测量的重力加速度[x_c y_c z_c]^T和先验三维地图坐标系下测量的重力加速度[x_w y_w z_w]^T进行对齐，得到当前机器人坐标系相对于先验三维地图坐标系的俯仰角β和翻滚角γ，具体为：

3.如权利要求2所述基于点线特征的机器人全局最优视觉定位方法，其特征在于，所述步骤(3)中，计算机器人当前图像的二维点线特征和先验三维地图的三维点线特征相应描述子在某度量空间的距离，将该距离小于预设阈值的特征进行匹配，获取多对匹配特征。

4.如权利要求1所述基于点线特征的机器人全局最优视觉定位方法，其特征在于，所述步骤(4)中，将d_p(α)和d_l(α)统一为平移无关度量d(α)：

其中，d₁，d₂，d₃为d(α)的系数；偏航角α的平移无关度量的下界d(A)为：

d(A)＝min|a₁sin(α+a₂)+d₃|

其中，A是包含偏航角α的区间子集，α∈A；d(A)对应的最大化一致集问题为：

其中，d _p，ij(A)是在A上的点特征推导的平移无关度量，d _l，k(A)是在A上的线特征推导的平移无关度量；将偏航角α对应的一致集内的内点数量记为能量函数E(α)，E(α)的上界记为

其中，是求解最大化一致集问题得到最优一致集的内点数量；

整个全局最优旋转求解步骤为：把偏航角的范围[-π，π]初始化到多个子集A中，多个子集A构成序列q，初始化能量函数最优值和偏航角最优值；在q为空前，循环执行如下操作：取出q的第一个子集A计算如果/>大于当前能量函数最优值，则根据该子集A的中心α_c计算E(α_c)，如果E(α_c)仍大于当前能量函数最优值，则更新当前能量函数最优值为E(α_c)，当前偏航角最优值为α_c；否则继续遍历剩余的子集A，直到q为空，返回最终的偏航角最优值。

5.如权利要求4所述基于点线特征的机器人全局最优视觉定位方法，其特征在于，所述步骤(4)中，已知偏航角，则关于平移量的最大化一致集问题为：

其中，A_i和b_i是已知偏航角对应的关于平移量的线性函数的系数；

根据一组至少有一个点特征的三维到二维匹配特征将关于平移量的最大化一致集问题更新为：

其中，是平移量的估计；将平移量解耦成xyz方向的三个分量，关于x方向的平移量的最大化一致集问题为：

同理可得关于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里最大投票数结果对应的投票特征集合的大小大于当前能量函数最优值，则将较大的值更新为当前能量函数最优值，将对应的特征集合更新为当前最优内点集合，继续遍历；否则不大于则不更新直接继续遍历。

6.如权利要求1所述基于点线特征的机器人全局最优视觉定位方法，其特征在于，所述二维点线特征包括二维点特征和二维线特征；所述二维点特征的检测方法包括ORB、SIFT、LBP和HOG特征点检测算法；所述二维线特征的检测方法包括LBD、Edline和Hough直线检测；所述某度量空间的距离为欧式距离。

7.一种基于权利要求1所述基于点线特征的机器人全局最优视觉定位方法的装置，其特征在于，包括：

第一获取模块，用于获取当前场景的先验三维地图和重力加速度的测量数据；

第二获取模块，用于获取机器人的当前相机图像和测量的重力加速度；

第一计算模块，用于根据机器人测量的重力加速度与先验三维地图中重力加速度的测量数据，计算机器人位姿的俯仰角和翻滚角；

匹配模块，用于根据机器人当前相机图像检测到的二维点线特征与先验三维地图中的三维点线特征进行匹配，得到多对二维到三维的点线特征匹配对；

第二计算模块，用于根据获得的点线特征匹配对将待求机器人位姿的旋转与平移分离，先求解旋转，再求解平移，从而完成搜索空间的降维，求得精确的估计结果。