CN111696155A - 一种基于单目视觉的多传感融合机器人定位方法 - Google Patents

Abstract

本发明公开了一种基于单目视觉的多传感器融合机器人定位方法，所述的多传感器包括安装于移动机器人底轮上的光电编码器，安装于车体上的IMU以及安装于单自由度云台上的绝对值编码器和单目相机。单自由度云台及其上的绝对值编码器可辅助单目相机在通过视觉实现定位的同时完成其它作业。本发明的主要目的在于使用一个作业摄像头同时完成作业和定位两个目标，减少硬件成本和处理器计算量。本发明的多传感融合特点在于，利用扩展卡尔曼滤波算法融合IMU与光电编码器数据来对单目视觉得到的相机位姿进行矫正；同时利用IMU预积分辅助图像进行特征点匹配，对图像中提取的特征点进行聚类分析得到动态场景下图像背景的特征点。

Description

一种基于单目视觉的多传感融合机器人定位方法

技术领域

本发明属于机器人定位技术领域，具体涉及一种基于单目视觉的多传感融合机器人定位方法。

背景技术

随着近年来计算机处理能力、人工智能技术等的快速发展，移动机器人已经成为学术界和产业界的重要研究对象。现如今，室内移动机器人在生活中和工业上都有了越来越多的应用。一个室内自主移动机器人首先要解决的问题是通过安装在机器人身上的多种传感器来获取机器人周围的环境信息，通过估计机器人在环境中的位姿实现机器人自身定位。单目视觉里程计只选用一个相机作为视觉传感器来得到运动物体的实时定位结果和周围环境的地图信息，具有成本价格低廉，功耗小，易于携带同时具有较强的抗电磁干扰能力的优点，但是具有缺少尺度因子的问题。

在机器人通过视觉传感器实现自身定位之后，往往还需要去执行其它视觉任务，比如目标跟踪、地图构建和环境搜索等作业。此时如果再增加一个视觉传感器来实现这些作业，将会增加机器人整体的重量、硬件成本和处理器资源。

为了解决单目视觉存在的不足，同时尽可能的降低机器人的成本，本发明提出了一种基于单目视觉的多传感融合机器人定位方法。本发明利用机器人上的摄像头在进行目标跟踪或监控等作业时采集的图像数据计算机器人的定位信息，仅利用了图像中除动态目标之外的场景信息，就可多计算一个定位信息。

发明内容

本发明的目的在于提出一种基于单目视觉的多传感融合机器人定位方法，以

解决背景中提到的单目视觉定位存在的诸多不足，并使用一个作业摄像头同时完成作业和定位两个目标，减少硬件成本和处理器计算量。本发明利用机器人上的摄像头在进行目标跟踪或监控等作业时采集的图像数据计算机器人的定位信息，仅利用了图像中除动态目标之外的场景信息，就可多计算一个定位信息。

为实现上述目的，本发明首先对搭载在机器人上的单目相机进行标定处理，得到单目相机的内参数矩阵，该内参数矩阵将用于相机位姿的计算。在机器人移动的过程中，可通过单目相机采集到每一时刻的场景图像，对该场景图像进行处理不仅可以得到机器人的定位，还可以通过单自由度云台带动单目相机一起旋转，在运动场景范围内实现其它作业。

本发明的一种基于单目视觉的多传感融合机器人定位方法，包括如下步骤：

第一步分别从搭载于云台上的单目相机采集图像、从安装于底轮上的光电编码器获取角度信息，从搭载于车体的IMU获取角速度及加速度信息，从单自由度云台上的绝对值编码器获取云台的角度信息；

第二歩将采集到的图像序列通过灰度变换，然后对每一帧灰度图像通过前述光电编码器和IMU信息进行特征提取，得到特征点信息，并针对前、后帧进行特征点匹配形成特征点对，再利用特征点对估计出该周期内机器人上云台的不带实际尺度信息的移动轨迹。

第三步利用扩展卡尔曼滤波算法将编码器数据和IMU数据进行融合，将每个图像采样周期的融合结果作为该周期的实际平移向量，用于解决第二步中单目视觉无尺度信息的问题。

第四步利用第三步中得到的实际平移向量对第二步中的不带实际尺度信息的机器人上单目相机的移动轨迹进行加权轨迹校正。

第五步根据绝对值编码器得到的角度信息，对单目相机与机器人运动中心的相对位姿进行变换运算，得到机器人的实际运动轨迹。

与现有技术相比，本发明的有益效果是：

(1)使用一个作业摄像头同时完成作业和定位两个目标，减少硬件成本和处理器计算量。本发明的开放性允许诸如目标跟踪、目标围捕、地图构建、环境搜索等作业的同时运作。

(2)通过将单目视觉、编码器数据、IMU数据进行融合，充分发挥多传感器的优势，提升了单目视觉定位的效果，解决了单目视觉里程计的尺度不确定性问题，使单目视觉能更好地在机器人定位中应用。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例子一起用于解释本发明，并不构成对本发明的限制。

图1机器人上的传感器安装示意图；

图2机器人运动轨迹计算的系统框图；

图3相机运动轨迹计算的流程图；

图4多传感数据融合流程图；

图5整车速度观测值选取原理示意图；

图6机器人中心坐标系与机器人上单目相机坐标系的建立示意图；

图7两台移动机器人中心位置与观测目标位置的几何关系图；

图8两台机器人轨迹推算观测目标轨迹的流程图。

图中：1-单目相机；2-绝对值编码器；3-IMU；4-光电编码器。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例子。基于本发明中的实施例子，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例子，都属于本发明保护的范围。

参阅图1，本发明提供了一种技术方案：一种基于单目视觉的多传感融合机器人定位方法，所述的多传感器包括安装于移动机器人底轮上的光电编码器，安装于车体上的IMU以及安装于单自由度云台上的绝对值编码器和单目相机；

参阅图2，本发明提供了一种技术方案：一种基于单目视觉的多传感融合机器人定位方法，当机器人移动时，单目相机实时采集场景图像；底轮上的光电编码器获取角度信息；搭载于车体的IMU获取角速度及加速度信息；单自由度云台上的绝对值编码器获取云台的角度信息。首先通过图像计算模块把单目相机得到的场景序列图像转换为不带实际尺度信息的机器人上单目相机的移动轨迹；同时，通过数据融合模块将编码器数据和IMU数据进行融合，并将每个图像采样周期的融合结果作为该周期的实际尺度缩放因子；在实际轨迹尺度恢复模块中利用数据融合模块中得到的缩放因子，将图像计算模块中得到的不带实际尺度信息的移动轨迹转换为单目相机的实际移动轨迹。在位姿变换模块中，根据绝对值编码器得到的角度信息，对单目相机与机器人运动中心的相对位姿进行变换运算，得到机器人的实际运动轨迹。

参阅图3，所述的图像计算模块中，获取机器人运动时的图像序列，采用灰度变换算法，将采集到的RGB图像转换为灰度图像；然后采用FAST特征点提取算法对上述所得的图像进行特征提取，并在此基础上利用光电编码器和IMU信息对特征点做聚类分析，获取并保存特征点信息；接着利用前、后帧图像的特征点信息进行匹配，得到特征点对信息；利用对极几何约束，估计出该前后帧周期内机器人上单目相机的旋转矩阵R和平移矩阵t；以第一帧图像所在相机坐标系为世界坐标系，则第一帧图像的相机位姿相对于世界坐标系的旋转矩阵和平移矩阵为

t₁＝[0 0 0]^T

合并为刚体变换表示为

此时相机位姿

C₁＝T₁ (2)

最终可以得到在世界坐标系下每帧图像对应的相机位姿为

从而可以得到不带实际尺度因子的相机运动轨迹。

所述的数据融合模块中，将采集到的光电编码器数据和IMU数据，利用扩展卡尔曼滤波估计出整台机器人速度，计算步骤如下：

(1)运动模型预测阶段：

v′_k＝v_k-1+a*Δt (4)

P′_k＝P_k-1+Q_k (5)

(2)测量模型更新阶段：

K_k＝P′_k*(P′_k+R_k)^-1 (6)

v_k＝v′_k+K_k*(z_k-v′_k) (7)

P_k＝P′_k*(1-K_k) (8)

其中，v′_k为时刻k处，速度的先验估计，a为IMU测量得到的X轴加速度，P_k为v_k的协方差，Q_k为输入噪声的协方差，R_k为输出噪声的协方差，K_k为卡尔曼增益，z_k为整台机器人速度观测值，其选取原理如图4所示，v_k为在时刻k处，给定测量值z_k时，整台机器人速度的后验估计。

在图像采样周期内的整台机器人速度v进行积分，得到该周期内的整台机器人位移s，即为该周期内的尺寸缩放因子，也即

其融合流程示意图如图5所示。

所述的实际轨迹尺度恢复模块中，将上述扩展卡尔曼滤波所得的位移信息作为尺度因子s，对上述所得的不带实际尺度因子的相机运动轨迹进行实际尺度化，得到带有实际尺度的运动轨迹。平移矩阵t可表示为

其中x、y、z表示在当前相机坐标系下三个方向上的平移，将t的长度归一化为1，可得

对于长度为1的平移矩阵t'，我们使用扩展卡尔曼滤波算法融合机器人上安装的电机编码器和IMU的数据来确定t'的真实尺度s，对无尺度信息的相机运动轨迹进行加权轨迹校正，即

S＝s₁t₁+s₂t₂+....+s_nt_n (12)

所述的位姿变换模块中，由于机器人上单目相机的安装位置与机器人运动中心存在位姿偏差，故将上述所得的带有实际尺度的单目相机运动轨迹，经过单目相机与机器人运动中心的相对位姿变换，即可得到机器人运动中心的实际运动估计。图6描述了机器人中心坐标系与机器人上单目相机坐标系的建立。令机器人中心距离地面的高度为h₁，相机中心距离地面的高度为h₂，机器人中心与相机中心的水平距离为d。安装参数h₁、h₂和d都是已知的。假设相机轨迹某一时刻的位姿为R_c,t_c，对应的机器人中心的轨迹为R_r,t_r。当机器人的云台相机相对于机器人中心没有产生角度的旋转时，以机器人中心为原点的坐标系O_r-X_rY_rZ_r的X_rY_rZ_r轴与相机坐标系O_c-X_cY_cZ_c的X_cY_cZ_c轴是平行的。

则有

R_r＝R_c

当机器人的云台相机相对于机器人中心产生了角度大小为θ的旋转时，该角度是沿Y轴的一个旋转，用旋转矩阵可表示为

此时有

为使本发明的目的、技术方案和优点更加清楚明白，下面具体介绍本发明的一个应用例子。

当机器人移动时，位于机器人上的单目相机不断地采集环境中的图像信息，首先通过图像计算模块、数据融合模块、实际轨迹尺度恢复模块和位姿变换模块可以对机器人进行定位。对于多台同时运动的机器人，使用本发明的一种基于单目视觉的多传感机器人定位方法可得到多台机器人的运动轨迹，通过初始化信息可以将它们的轨迹拼接到同一坐标系下；根据该坐标系下的不同机器人的轨迹可以计算出机器人之间的距离和夹角。同时多台机器人通过单自由度云台上的单目相机可以对前方的目标进行检测和跟踪，具体方法是单自由度云台可以根据目标在图像中的像素坐标，通过旋转一定的角度来使得目标在图像中的像素坐标为图像中心的像素坐标。从而可以达到仅使用一个单目相机就能同时实现机器人定位和对目标进行跟踪的效果。并且当目标在图像中的像素坐标为图像中心的像素坐标时，还可以根据三角几何关系得到目标定位。图7描述的就是通过两台机器人的定位得到目标的定位的几何关系图。具体流程可参阅图8。这种通过一个单目相机来同时实现机器人定位和其它多任务作业的方法，可以避免一般机器人需要携带两个及以上相机来实现多个视觉任务的情况，在减少硬件成本的同时还能节约处理器的计算资源。

需要说明的是，在本文中，诸如术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (1)

1.一种基于单目视觉的多传感融合机器人定位方法，其特征在于：所述的多传感器包括安装于移动机器人底轮上的光电编码器，安装于车体上的IMU以及安装于单自由度云台上的绝对值编码器和单目相机；当所述机器人移动时，所述的单目相机实时采集场景图像；所述的底轮上的光电编码器获取角度信息；所述的搭载于车体的IMU获取角速度及加速度信息；所述的单自由度云台上的绝对值编码器获取云台的角度信息；本发明主要包括图像特征提取模块、图像位姿计算模块、数据融合模块、相机位姿矫正模块和位姿变换模块；所述图像特征提取模块把绝对值编码器和IMU的信息用于图像特征点聚类分析，提取和机器人运动有关的背景特征点；所述图像位姿计算模块用于得到单自由度云台上单目相机的不带实际尺度信息的运动估计；所述数据融合模块将光电编码器数据和IMU数据进行融合，得到图像位姿计算模块中的无尺度信息的相机位姿的平移向量；所述相机位姿矫正模块利用数据融合模块中得到的平移向量，将图像位姿计算模块中得到的不带实际尺度信息的移动轨迹转换为单目相机的实际移动轨迹；所述位姿变换模块对单目相机与机器人运动中心的相对位姿进行变换运算，得到机器人的实际运动轨迹，完成机器人的定位。

Images