CN109752003B - 一种机器人视觉惯性点线特征定位方法及装置 - Google Patents

Abstract

本发明公开了一种机器人视觉惯性点线特征定位方法及装置包括获取当前场景的先验三维地图和惯性传感器的测量数据，所述先验三维地图是事先构建的，且包含三维点线特征；获取机器人的当前图像和惯性传感器的测量数据等步骤，本发明利用多传感器的融合，在算法中充分发挥各个传感器的优势信息，最终达到提升定位精度、鲁棒性的效果。

Description

一种机器人视觉惯性点线特征定位方法及装置

技术领域

本发明涉及机器人导航技术，具体地说，涉及一种机器人视觉惯性点线特征定位方法及装置。

背景技术

目前，越来越多不同类型的机器人出现在生产生活的方方面面。对于仓储物流、巡检监控等领域而言，其工作需要机器人能够在一个相对固定的环境内实现长期稳定的运行，且能实现精确的自定位。视觉传感器生产成本低，获取信息量大，相关定位方法受到广泛的研究与应用。传统基于特征点匹配进行相机基础矩阵估计的视觉定位方法容易受到视角变化、动态遮挡、环境光照以及气候变化等问题的影响，造成特征点难以重复检测以及检测到的特征点错误匹配率高等问题。而线特征相比于点特征是一种更高级的语义特征，具有更稳定的检测效果，即使在部分遮挡的情况下也能检测出部分线段，可以很大程度上弥补点特征的不足。另外惯性传感器与视觉的融合也能够为相机位姿的估计问题降低求解维度，从而减少估计相机基础矩阵所需的匹配特征数量，因而有效提高容错率，增强算法鲁棒性。

发明内容

为了克服现有技术的不足，本发明的目的在于提供一种机器人视觉惯性点线特征定位方法及装置，定位时同时利用环境中的点特征和线特征，最大化利用了视觉提供的信息，应用场景更加广泛；线特征相比于点特征是一种更高级的语义信息，尤其在工厂等视觉点特征不那么丰富的人造环境中，线特征的优势更为明显。同时与惯性传感器的结合降低了相机位姿求解问题的维度，减少了模型估计所需的匹配数量，从而即使在视觉提供的信息存在大量误匹配的情况下也能得到准确的定位结果，有效提高容错率，大大提升鲁棒性。本发明是通过以下技术方案来实现的：

本发明公开了一种机器人视觉惯性点线特征定位方法，包括以下步骤：

获取当前场景的先验三维地图和惯性传感器的测量数据，所述先验三维地图是事先构建的，且包含三维点线特征；

获取机器人的当前图像和惯性传感器的测量数据；

根据当前机器人的惯性传感器的测量数据与所述先验三维地图中的惯性传感器的测量数据计算当前机器人位姿的俯仰角和翻滚角；对机器人当前图像进行二维点线特征的检测，并将当前图像中测量到的二维点线特征与所述先验三维地图中测量到的三维点线特征进行匹配；

根据所述匹配到的两对二维到三维点特征或者一对二维到三维匹配的点特征加一对二维到三维匹配的线特征计算当前机器人位姿的其余未知量。

作为进一步的改进，本发明所述的先验三维地图有如下要求：

要包含重力加速度在所述地图坐标系下的测量数据，由惯性传感器测量所得；

所述先验三维地图要包含三维点线特征。

作为进一步的改进，本发明所述的机器人在当前图像中惯性传感器的测量数据为重力加速度在当前图像机器人坐标系下的测量值；所述先验三维地图中的惯性传感器的测量数据为重力加速度在先验三维地图坐标系下的测量值；将重力加速度在两个坐标系下的测量值进行对齐，从而得到地图坐标系相对于当前机器人坐标系的俯仰角和翻滚角，具体根据以下算式进行计算：

其中[x_w y_w z_w]^T为重力加速度在先验三维地图坐标系下的测量值，[x_c y_c z_c]^T为重力坐标系在当前图像机器人坐标系下的测量值，β,γ分别为所求的先验三维地图坐标系相对于当前图像机器人坐标系的俯仰角和翻滚角。

作为进一步的改进，本发明所述当前图像测量到的二维点线特征和先验三维地图中的三维点线特征可以进行二维到三维的点线特征的匹配，具体包括以下子步骤：

根据所述机器人获取的当前图像检测多个二维特征点，所述二维特征点的检测不限方法，具体可以为ORB，SIFT，LBP或HOG特征点检测算法；

根据所述机器人获取的当前图像检测多条二维线段，所述二维线段的检测不限方法，具体可以为LBD，Edline或Hough直线检测；

根据所述当前图像的二维点线特征和先验三维地图中的三维点线特征进行匹配，具体表现为计算相应描述子在某度量空间的距离，一般可以取为欧式距离；该距离小于预设阈值范围的特征则认为匹配成功，由此迭代获取多对二维与三维的匹配点线特征。

作为进一步的改进，本发明根据匹配到的两对二维到三维匹配的点特征可以计算先验三维地图相对于当前图像机器人坐标系的位姿的其余未知量，具体包括以下子步骤：

根据地图坐标系{W₀}中匹配到的两个三维点P₁ ⁰,

引入一个中间坐标系{W₁}，其中两个三维点在所述中间坐标系下表示为P₁＝(0 0 0)^T,P₂＝(X₂ Y₂ Z₂)^T，而{W₀}到{W₁}的变换则表示为P₁ ⁰→P₁的简单平移；

根据所述机器人当前图像中匹配到的二维点和相机标定的内参矩阵K可得二维点在机器人相机坐标系{C}下的归一化平面上的三维坐标：

其中C为相机的光心；

根据投影几何可得三点(C,D₁,RP₁+t)共线，且三点(C,D₂,RP₂+t)共线，其中R为{W₁}相对于{C}的旋转变换矩阵，t为{W₁}相对于{C}的平移变换向量；根据权利要求3所述{W₀}相对于{C}的俯仰角和翻滚角，可将R简化为：

其中α为未知量，表示为地图坐标系相对于机器人当前相机坐标系的偏航角，设t＝[t₁ t₂ t₃]^T，则全部未知量一共有四个，分别为α,t₁,t₂,t₃；

根据三点(C,D₁,RP₁+t)共线，可得两个方程如下：

a₁t₂-b₁t₁＝0

a₁t₃-t₁＝0

根据三点(C,D₂,RP₂+t)共线，可得两个方程如下：

a₂(R₂₁X₂+R₂₂Y₂+R₂₃Z₂+t₂)-b₁(R₁₁X₂+R₁₂Y₂+R₁₃Z₂+t₁)＝0

a₂(R₃₁X₂+R₃₂Y₂+R₃₃Z₂+t₃)-(R₁₁X₂+R₁₂Y₂+R₁₃Z₂+t₁)＝0

联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿；

作为进一步的改进，本发明根据匹配到的一对二维到三维匹配的点特征加一对二维到三维匹配的线特征可以计算地图相对于当前机器人坐标系的位姿的其余未知量，具体包括以下子步骤：

根据地图坐标系{W₀}中匹配到的一个三维点P₁ ⁰和一条三维线段

引入一个中间坐标系{W₁}，其中三维点在所述中间坐标系下表示为P₁＝(000)^T，{W₀}到{W₁}的变换则表示为P₁ ⁰→P₁的简单平移，易得三维线段的端点在所述中间坐标系下表示为L₂＝(X₂ Y₂Z₂)^T,L₃＝(X₃ Y₃ Z₃)^T；

根据所述机器人当前图像中匹配到的一个二维点和一条二维线段，以及相机标定的内参矩阵K可得二维点和二维线段的端点在机器人相机坐标系{C₀}下的归一化平面上的三维坐标

和

引入一个中间坐标系{C₁}，使得相机光心以及匹配到的点和线段端点的表示为：

其中C为相机的光心；所述中间坐标系{C₁}到{C₀}的变换可由

的变换求得；

根据投影几何可得三点(C,D₁,RP₁+t)共线，四点(C,D₂,D₃,RL₂+t)以及(C,D₂,D₃,RL₃+t)共面，其中R为{W₁}相对于{C₁}的旋转变换矩阵，t为{W₁}相对于{C₁}的平移变换向量；

根据三点(C,D₁,RP₁+t)共线，可得两个方程如下：

a₁t₂-b₁t₁＝0

b₁t₃-t₂＝0

根据四点(C,D₂,D₃,RL₂+t)共面，可得方程：

R₂₁X₂+R₂₂Y₂+R₂₃Z₂+t₂＝0

根据四点(C,D₂,D₃,RL₃+t)共面，可得方程：

R₂₁X₃+R₂₂Y₃+R₂₃Z₃+t₂＝0

联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿。

本发明还公开了一种机器人视觉惯性点线特征定位装置，包括：

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

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

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

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

第二计算模块，用于根据所述获得的点线特征匹配对计算机器人位姿的剩余未知量。

相比现有技术，本发明的有益效果在于：

本发明公开了一种视觉惯性点线特征定位方法及装置，获取当前场景的先验三维地图，所述地图是事先构建的；获取机器人的当前图像和惯性传感器的测量数据；根据当前惯性传感器数据与所述先验三维地图中的惯性传感器数据计算当前机器人位姿的俯仰角和翻滚角；根据当前图像中检测到的二维点线特征与所述先验三维地图中的三维点线特征进行匹配；根据所述匹配到的两对二维到三维点特征或者一对点特征加一对线特征计算当前机器人位姿的其余未知量。

目前移动机器人的主要使用领域在仓储物流、巡检监控等，他们对于机器人的要求是能够在一个相对固定的环境内实现长期稳定的运行，且能实现精确的自定位。视觉传感器生产成本低，获取信息量大，相关定位方法受到广泛的研究与应用。

传统基于特征点匹配进行相机基础矩阵估计的视觉定位方法容易受到视角变化、动态遮挡、环境光照以及气候变化等问题的影响，造成特征点难以重复检测以及检测到的特征点错误匹配率高等问题。本发明在定位时同时利用环境中的点特征和线特征，最大化利用了视觉提供的信息，应用场景更加广泛。线特征相比于点特征是一种更高级的语义信息，具有更稳定的检测效果，即使在部分遮挡的情况下也能检测出部分线段，对光照以及气候变化等不敏感，可以很大程度上弥补点特征的不足。尤其在工厂等视觉点特征不那么丰富的人造环境中，线特征往往更易于检测且数量更多，其优势更为明显。在野外自然环境中，线特征不那么明显的情况下，本发明还支持两对特征点定位方法，仅需在机器人当前图像中匹配到两组对应的地图上的三维特征点就能实现对机器人位姿的准确估计，其适用场景非常广泛，可以有效提高机器人对环境的自适应能力。

另外，IMU(惯性测量单元)的引入增强了***的鲁棒性，使其在快速运动时定位不易丢失，从而达到较好的防抖动效果。IMU的测量数据与外界环境无关，只与自身的运动有关，因此融合进IMU的数据，可以大大减弱恶劣观测环境对定位精度的影响，从而保证长期定位的稳定性。同时本发明利用IMU的数据降低了相机位姿求解问题的维度，具体表现为利用机器人当前IMU测量的重力加速度与地图坐标系下的重力加速度的测量值进行对齐，求解得出机器人相对于地图坐标系位姿的俯仰角和翻滚角，从而将六自由度机器人位姿估计问题降低为四自由度。降维后，相机位姿估计由最初的三对匹配特征进行计算减少为两对匹配特征，从而在视觉数据存在大量误匹配的情况下，提高定位成功率，大大提升***鲁棒性。由于俯仰角和翻滚角在视觉惯性***中是全局可观测的，利用惯性数据提前获得这两个量，在一定程度上提高了定位精度。

本发明利用多传感器的融合，在算法中充分发挥各个传感器的优势信息，最终达到提升定位精度、鲁棒性的效果。

附图说明

图1为本发明机器人视觉惯性点线特征定位方法的流程示意图；

图2为图1中所用投影几何约束示意图；

图3为图1中坐标系变换示意图；

具体实施方式

下面，结合附图以及具体实施方式，对本发明的技术方案做进一步地描述。

图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直线检测；

根据所述当前图像的二维点线特征和地图中的三维点线特征进行匹配，具体表现为计算相应描述子在某度量空间的距离，作为优选的实施方式，可以取为欧式距离；该距离小于预设阈值范围的特征则认为匹配成功，由此迭代获取多组二维与三维的匹配点线特征。

5.根据所述匹配到的两对二维到三维点特征或者一对点特征加一对线特征计算当前机器人位姿的其余未知量，具体分为以下两种实施情况：

1)根据匹配到的两组二维到三维匹配的点特征可以计算地图相对于当前机器人坐标系的位姿的其余未知量，具体包括以下子步骤：

根据地图坐标系{W₀}中匹配到的两个三维点

引入一个中间坐标系{W₁}，其中两个三维点在所述中间坐标系下表示为P₁＝(0 0 0)^T,P₂＝(X₂ Y₂ Z₂)^T，而{W₀}到{W₁}的变换则表示为P₁ ⁰→P₁的简单平移，具体为：

根据所述机器人当前图像中匹配到的二维点和相机标定的内参矩阵K可得二维点在机器人相机坐标系{C}下的归一化平面上的三维坐标：

其中C为相机的光心；

根据图2所示的投影几何约束可得三点(C,D₁,RP₁+t)共线，且三点(C,D₂,RP₂+t)共线，其中R为{W₁}相对于{C}的旋转变换矩阵，t为{W₁}相对于{C}的平移变换向量；根据权利要求3所述{W₀}相对于{C}的俯仰角和翻滚角，可将R简化为：

其中α为未知量，表示为地图坐标系相对于机器人当前相机坐标系的偏航角，设t＝[t₁ t₂ t₃]^T，则全部未知量一共有四个，分别为α,t₁,t₂,t₃；

根据三点(C,D₁,RP₁+t)共线，可得两个方程如下：

a₁t₂-b₁t₁＝0

a₁t₃-t₁＝0

根据三点(C,D₂,RP₂+t)共线，可得两个方程如下：

a₂(R₂₁X₂+R₂₂Y₂+R₂₃Z₂+t₂)-b₁(R₁₁X₂+R₁₂Y₂+R₁₃Z₂+t₁)＝0

a₂(R₃₁X₂+R₃₂Y₂+R₃₃Z₂+t₃)-(R₁₁X₂+R₁₂Y₂+R₁₃Z₂+t₁)＝0

联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿；

2)根据匹配到的一组二维到三维匹配的点特征加一组二维到三维匹配的线特征可以计算地图相对于当前机器人坐标系的位姿的其余未知量，具体包括以下子步骤：

根据地图坐标系{W₀}中匹配到的一个三维点P₁ ⁰和一条三维线段

引入一个中间坐标系{W₁}，其中三维点在所述中间坐标系下表示为P₁＝(0 0 0)^T，{W₀}到{W₁}的变换则表示为P₁ ⁰→P₁的简单平移，易得三维线段的端点在所述中间坐标系下表示为L₂＝(X₂ Y₂Z₂)^T,L₃＝(X₃ Y₃ Z₃)^T；

根据所述机器人当前图像中匹配到的一个二维点和一条二维线段，以及相机标定的内参矩阵K可得二维点和二维线段的端点在机器人相机坐标系{C₀}下的归一化平面上的三维坐标

和

具体见以下公式；引入一个中间坐标系{C₁}，使得相机光心C以及匹配到的点D₁和线段端点

的表示如下式：

C⁰＝(0 0 0)^T

其中C为相机的光心；所述中间坐标系{C₁}到{C₀}的变换

可由

的变换求得，坐标系变换示意图见图3；接下来求解中间坐标系中的D₁的具体坐标，将投影射线

经过

变换到中间坐标系下，并延长取其与xy平面的交点即得D₁；

D₁＝(a₁ b₁ 0)^T

根据图2所示的投影几何约束可得三点(C,D₁,RP₁+t)共线，四点(C,D₂,D₃,RL₂+t)以及(C,D₂,D₃,RL₃+t)共面，其中R为{W₁}相对于{C₁}的旋转变换矩阵，t为{W₁}相对于{C₁}的平移变换向量；

根据三点(C,D₁,RP₁+t)共线，可得两个方程如下：

a₁t₂-b₁t₁＝0

b₁t₃-t₂＝0

根据四点(C,D₂,D₃,RL₂+t)共面，可得方程：

R₂₁X₂+R₂₂Y₂+R₂₃Z₂+t₂＝0

根据四点(C,D₂,D₃,RL₃+t)共面，可得方程：

R₂₁X₃+R₂₂Y₃+R₂₃Z₃+t₂＝0

联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿。

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

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

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

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

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

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

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

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

第二计算模块，用于根据所述获得的点线特征匹配对计算机器人位姿的剩余未知量。

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

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

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

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

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

Claims (5)

1.一种机器人视觉惯性点线特征定位方法，其特征在于，包括以下步骤：

获取当前场景的先验三维地图和惯性传感器的测量数据，所述先验三维地图是事先构建的，且包含三维点线特征；

获取机器人的当前图像和惯性传感器的测量数据；

根据当前机器人的惯性传感器的测量数据与所述先验三维地图中的惯性传感器的测量数据计算当前机器人位姿的俯仰角和翻滚角；对机器人当前图像进行二维点线特征的检测，并将当前图像中测量到的二维点线特征与所述先验三维地图中测量到的三维点线特征进行匹配；

根据匹配到的两对二维到三维点特征或者一对二维到三维匹配的点特征加一对二维到三维匹配的线特征计算当前机器人位姿的其余未知量；

所述的根据匹配到的两对二维到三维点特征计算先验三维地图相对于当前图像机器人坐标系的位姿的其余未知量，具体包括以下子步骤：

根据地图坐标系{W₀}中匹配到的两个三维点P₁ ⁰,P₂ ⁰，引入一个中间坐标系{W₁}，其中两个三维点在所述中间坐标系下表示为P₁＝(0 0 0)^T,P₂＝(X₂ Y₂ Z₂)^T，而{W₀}到{W₁}的变换则表示为P₁ ⁰→P₁的简单平移；根据所述机器人当前图像中匹配到的二维点和相机标定的内参矩阵K可得二维点在机器人相机坐标系{C}下的归一化平面上的三维坐标：

其中C为相机的光心；

根据投影几何可得三点(C,D₁,RP₁+t)共线，且三点(C,D₂,RP₂+t)共线，其中R为{W₁}相对于{C}的旋转变换矩阵，t为{W₁}相对于{C}的平移变换向量；根据{W₀}相对于{C}的俯仰角和翻滚角，可将R简化为：

其中α为未知量，表示为地图坐标系相对于机器人当前相机坐标系的偏航角，设t＝[t₁t₂ t₃]^T，则全部未知量一共有四个，分别为α,t₁,t₂,t₃；

根据三点(C,D₁,RP₁+t)共线，可得两个方程如下：

a₁t₂-b₁t₁＝0

a₁t₃-t₁＝0

根据三点(C,D₂,RP₂+t)共线，可得两个方程如下：

a₂(R₂₁X₂+R₂₂Y₂+R₂₃Z₂+t₂)-b₁(R₁₁X₂+R₁₂Y₂+R₁₃Z₂+t₁)＝0

a₂(R₃₁X₂+R₃₂Y₂+R₃₃Z₂+t₃)-(R₁₁X₂+R₁₂Y₂+R₁₃Z₂+t₁)＝0

联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿。

2.如权利要求1所述的机器人视觉惯性点线特征定位方法，其特征在于，所述的先验三维地图有如下要求：

要包含重力加速度在地图坐标系下的测量数据，由惯性传感器测量所得；

所述先验三维地图要包含三维点线特征。

3.如权利要求1所述的机器人视觉惯性点线特征定位方法，其特征在于，所述机器人在当前图像中惯性传感器的测量数据为重力加速度在当前图像机器人坐标系下的测量值；所述先验三维地图中的惯性传感器的测量数据为重力加速度在先验三维地图坐标系下的测量值；将重力加速度在两个坐标系下的测量值进行对齐，从而得到地图坐标系相对于当前图像机器人坐标系的俯仰角和翻滚角，具体根据以下算式进行计算：

其中[x_w y_w z_w]^T为重力加速度在先验三维地图坐标系下的测量值，[x_c y_c z_c]^T为重力加速度在当前图像机器人坐标系下的测量值，β,γ分别为所求的先验三维地图坐标系相对于当前图像机器人坐标系的俯仰角和翻滚角。

4.如权利要求1所述的机器人视觉惯性点线特征定位方法，其特征在于，根据所述当前图像测量到的二维点线特征和先验三维地图中的三维点线特征可以进行二维到三维的点线特征的匹配，具体包括以下子步骤：

根据所述机器人获取的当前图像检测多个二维特征点，所述二维特征点的检测为ORB，SIFT，LBP或HOG特征点检测算法；

根据所述机器人获取的当前图像检测多条二维线段，所述二维线段的检测为LBD，Edline或Hough直线检测；

根据所述当前图像的二维点线特征和先验三维地图中的三维点线特征进行匹配，表现为计算相应描述子在某度量空间的距离，取为欧式距离；该距离小于预设阈值范围的特征则认为匹配成功，由此迭代获取多对二维与三维的匹配点线特征。

5.如权利要求1所述的机器人视觉惯性点线特征定位方法，其特征在于，根据匹配到的一对二维到三维匹配的点特征加一对二维到三维匹配的线特征可以计算地图相对于当前图像机器人坐标系的位姿的其余未知量，具体包括以下子步骤：

根据地图坐标系{W₀}中匹配到的一个三维点P₁ ⁰和一条三维线段

引入一个中间坐标系{W₁}，其中三维点在所述中间坐标系下表示为P₁＝(0 0 0)^T，{W₀}到{W₁}的变换则表示为P₁ ⁰→P₁的简单平移，易得三维线段的端点在所述中间坐标系下表示为L₂＝(X₂ Y₂ Z₂)^T,L₃＝(X₃ Y₃ Z₃)^T；

根据所述机器人当前图像中匹配到的一个二维点和一条二维线段，以及相机标定的内参矩阵K可得二维点和二维线段的端点在机器人相机坐标系{C₀}下的归一化平面上的三维坐标

和

引入一个中间坐标系{C₁}，使得相机光心以及匹配到的点和线段端点的表示为：

其中C为相机的光心；所述中间坐标系{C₁}到{C₀}的变换可由

的变换求得；根据投影几何可得三点(C,D₁,RP₁+t)共线，四点(C,D₂,D₃,RL₂+t)以及(C,D₂,D₃,RL₃+t)共面，其中R为{W₁}相对于{C}的旋转变换矩阵，t为{W₁}相对于{C}的平移变换向量；

根据三点(C,D₁,RP₁+t)共线，可得两个方程如下：

a₁t₂-b₁t₁＝0

b₁t₃-t₂＝0

根据四点(C,D₂,D₃,RL₂+t)共面，可得方程：

R₂₁X₂+R₂₂Y₂+R₂₃Z₂+t₂＝0

根据四点(C,D₂,D₃,RL₃+t)共面，可得方程：

R₂₁X₃+R₂₂Y₃+R₂₃Z₃+t₂＝0

联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿。

Images