移动机器人视觉SLAM过程中图像匹配及相机位姿求解的研究

第46卷第11期

Machine Design and Manufacturing Engineering

机械设计与制造工程

Nov. 2017

Vol.46 No. 11

DOI ： 10. 3969/j. issn. 2095 - 509X. 2017.11.003

移动机器人视觉SLAM过程中 图像匹配及相机位姿求解的研究

林志诚\\郑松

(1.福州大学电气工程与自动化学院，福建福州350116)

(2.福建省工业控制信息安全技术企业重点实验室，福建福州350008)

摘要：针对移动机器人视觉SLAM过程中的关键环节图像匹配及相机位姿求解问题，通过ORB特 征匹配算法匹配移动机器人采集的两帧图像，得到特征点集。分析了最小二乘法理论特征，根据 两帧图像中特征点不变性，采用基于最小二乘法的ICP算法建立相机运动模型，使用SVD分解法 求解运动模型，估计相机位姿状态，并通过非线性优化算法验证SVD分解法。利用ROB - D相机

Kinect采集图像，对上述算法进行实验，得到准确的相机姿态数据，从而有效解决了三维空间下移

2

动机器人局部移动过程中的定位问题。

关键词！视觉同步定位与地图构建;ORB特征；匹配算法；迭代最近点算法中图分类号:TP242.6

文献标识码:A

文章编号:2095 -509X(2017)11 -0013 -06

自主导航能力，提高移动机器人的智能化程度以及

未知环境下信息处理能力。

移动机器人是集环境感知、动态决策与规划、 行为控制与执行等功能于一体的综合系统，是集传 感器技术、信息处理、电子工程、计算机工程、自动 化控制工程以及人工智能等多种学科的综合研究 成果。移动机器人研究方向十分广泛，其中未知环 境中自主导航移动机器人是最具挑战性的研究方 向之一。移动机器人自主导航由3个部分组成:定 位、建图以及导航[1&。SLAM (simultaneous localiza­tion and mapping， 同步定位与地图构建） 是指利用 激光仪、里程计等传感器，于运动过程中建立周围 环境地图模型，同时估计自己的位姿[2]。移动机 器人视觉SLAM采用相机为传感器，相机种类主要 分为单目相机(monocular)%3]、双目相机（stereo )[4] 和深度相机(ROB - D)[5]，其中ROB - D相机不仅 能够采集彩色图像，而且还能获得图像深度信息， 因此采用ROB - D相机作为解决SLAM问题的外 界信息的传感器正逐渐成为一种趋势。本文利用 ROB - D相机，得到三维空间点在相机成像平面上 的投影值，即由数字排列而成的复杂矩阵。利用相 机在机器人运动过程中采集两帧图像，检测并匹配 图像信息中特征点集，根据图像中特征点集保持不 变的特性，采用算法求解相机姿态变化，从而构建 局部地图并解决机器人定位问题，提升移动机器人

收稿日期=2017 -

1 ORB特征检测匹配算法

为了简化图像处理算法，选取相机运动时图像 中保持不变的点，只有在确定这些点的基础上，才

能估计相机位姿。在视觉SLAM过程中，这些点称 为图像特征点（feature - point)。

特征点由关键点（key - point)和描述子（de- scriptor)组成。关键点是指特征点在图像中的位 置;描述子是按照特定的方式，比如向量，描述该关 键点周围像素信息。在当前的视觉SLAM领域中， 常用的特征提取算法有SIFT[6]、SURF[7]、ORB[8] 等。SIFT算法对旋转、尺度缩放、亮度变化保持不 变，对视角变化、仿射变换、噪声也保持一定程度的 稳定性。SURF算法是SIFT算法的改进，其最大特 点是采用Hessi是

o

矩阵提高计算效率，且计算速度 倍左右。SIFT和

SURF算法都具

SIFT算法的3

有计算量过大的缺点，故在处理图像过程中存在时 滞效果，且对硬件有高性能的需求。目前为止，普 通计算机的CPU运算速度均无法满足实时计算的 要求。ORB算法采用了 FAST特征检测算子，并采 用二进制格式的BRIEF特征描述算子，极大地提

08 -18

作者简介:林志诚（1994一），男，福建莆田人，福州大学硕士研究生,主要研究方向为移动机器人SLAM(视觉处理(多移动机器人协同控制。

• 13 •

2017年第46卷机械设计与制造工程

升了 。本 在经典的BRIEF特征描述进，并通过实验改进的算

子的基础上进

$为了抗噪声扰动，先对图像进：斯滤

(方差为2， 窗口 9x9个像素点$。

1

，有[了 [率。

!1 FAST特征检测算子

FAST特征检测算子是在

2$

点为中心的领

FAST特征点为中心，取31 x31 像

的邻域大窗口，然随机选取两 窗口，对窗

口内的像 的和进行比较，并通过式（3 $进

进制赋值。

3 $在大窗口中随机选取128对子窗口，重 骤2$的进制赋值，形成

对特征点的

，

，测并比较该点与 像素点的 值

大小，如果 显不同，则该点为特征点。如果该 的像素点 为（，则称为FAST-(。 体测过程如下！

进制编码，这编征

子。

1

$在像中选取像素点L，假设灰度值为M。

2$设置阈值）3 $ L点为中心，选取半径为2的圆上的（个

像素点。

4$如果以点L为中心的

连续（个点

的灰度值都大于(M + )或者小于(M ')，那么 认为 征点。5$循环 4 骤，对每个像素点依次操作。

在特征点位置处，计算特征点的Harris响应 ，然选取其中/ 应值大的特征点作为最的特征点集合。ORB通 对FAST征点添加方向的

，具体操作步骤如下！

1$选取

征点\"为几何中心的图像块，定

义该图像块的矩为：

1? = %aPbqI(a，b$

?ly-〇，1.

(1$

式中/(0，1$为征点（0，1$处的 。

2$通矩可以找到图像块的

# 1io 1〇1)

3$连 像块的几何中心\"和

#得到向量，这向量与水平的偏转

用于描征点的方向，于该方向为：)=arctan(i01/i10 $ (2$

，FAST特征点便有了旋转方向的描述。! 2

BRIEF特征描述算子

得到FAST特征点后，需要

种方式

这

征点的属性。这属 为该特征点的子。ORB算法采用二进制格式的BRIEF特征 子，BRIEF特征 子 许 0和1组成

的向量，〇和

1

的编 像素点；与征点\"的灰

大小关系决定：

#(;，\"$ & f1 6⑴ <6(\"$

(3$

0 6(;$'6(\"$式中：6(；$，6(\"$是像点；与征点\"处的灰

。 征点属

^维0，1组成的向量

表示。BRIEF特征 子的具体求 骤如下：

+ 14 +

! 3 特征提取与匹配实验

征匹配是视觉SLAM过程中极为关键的一

， 到视觉SLAM过程中 关联的

，

通 取 像的 FAST 征 测 子

BRIEF

征

子，对

子进行精确匹配，为续的

的姿态估计、优化操作提供重要的基础。采

用最简单的暴力匹配（brute - force matcher)，更能 凸显出ORB特征检测算子的性能。图1为移动 人通过ROB - D相机采集到的 像，图2为

移动 人平移 5cm后RGB-D 采集到

的

像# 3 为 采 用 FAST 征 测 子 和

BRIEF特征 子对图2进 征提取的图像，图4为匹配测试结果。

1l^

图1

原始图像

图2

平移变换后图像

图3

特征提取图像

2017年第11期

林志诚!移动机器人视觉slam过程中图像匹配及相机位姿求解的研究

图4特征匹配图像

problem)的模型。它的

2

非线性优化

于 ！最大

2.1

状态估计问题

在 人视觉slam过程中，对 人进行状估计，即已知输入 \"惯 $和测 $ 人 像特征$的条件下，计算状态！的条件概率分布:：(！h，\"。

为了简化 ，通常 惯 ，而测 看作图像中一系 点集，从而转换为如 许 像中 间 。贝 ， 人 量的后验 为：

在先验 ：(！$已知的条件下，求 大后验 ：(！ I h$ 于求解！最大似然：（$ h$。

人观测方程有：Zk，〇=h ( )，！ 6$ +%6，； (0)式中:E()，！6$为 模型;）为图像中特征点;！k为 姿 量;%k，为噪声项，且足 分

布％k〜/(0，'k$。所以观测数据的条件概率为：

:(Z，k\\Xk，yJ $ =N( h ()，Xk$，Qk，j $ (6$式(6) ，为了求该式最大似然估计，就必须求使它最大化的！k，），对于多维高斯 分布x〜N(M，+ )，通常采用以下形式来求解高斯

的最大似然估计：

-lnP( X $ ^$1. %( 2#$ Ndet( + $] +

( X -

*$2+$ (7$

对 求最大化 于对负对数求 化。在式(7$中 右边第一*项与X无关， 去。只要求 右边第二项二次型的 化，就得到了对 的最大似然估计。代入视觉slam的观测模型，相当于求解：

X = —(Zk. - h(Xk，\" $$ U (Zk.- h(xk，\" $ $] (8)

定 与估 间存在以下误差：

ey，j，k ~Zk，j ~h( X k，yj $ (9$

根据式(（$求该误差的平方和：

6(X)

= % %e，k，Qk-. e，k， ( 10 $

，得到

(least square

似然估计。 动程和观测方程估计整体误

，求这个误差的 典型化的 程。

2.2 非线性最小二乘法

在 人系统中，环境变量不满足 分， 误 程为 程， 在

人

模型 中起到至关重要的作用。

误差的平方和 为准则

来估 模型 的一种 估 。对于 间中的一点B为了得到目标点E，对B点进行旋转位移变换得到&，根据式(（$构建&与E 的误差公式，从而建立 模型，其中&: 矩阵，用来 体旋转。利用 ；向量即李代数来表 体旋转变化，李代数与旋转矩阵通过矩阵 函 立映射关系。

对于利用 模型求误 ， 统为 标函 为零，然后求 。李代 d(h形式如下：

_ & $ _ (( h - exp(\")s) _

式中：6为 比矩阵;\"为 旋转 向量组

成的对称矩阵。该 程过于 ，且只适用于

程。针对 采用迭代的方

式求解，具体骤如下：

1$给定 R〇。2$开进行迭代，对于迭代过程， 下降矢量#!，使得||/(!k +#!) || /达到极 。

3)若足 ， 止迭代。否则返回重复步骤2 $。

为了 骤2$的极 ，必须寻找一个梯度，D(x$进 阶泰勒展开得到：

/(x+#x$ &/(x) +/(!k)#x (12$式中：/'(!k) 矩阵的 ，也 ：一

比矩阵。为了 下降矢量#!， 得

||/(X+#X) *2达到 ， 于构建

程：

• 15 •

2017年第46卷

机械设计与制造工程

2)计算初始点集对应点？'和最终点集对应点L

!+ sargmin\"1 ||/(x) +/’（！6)#X || 2

Ax 2(D)

分解式（13)得到：

|||/(x) +/(xt)Ax ||2 =士（ ||/(x) *2 +2f(x) T/(x) Ax +AxT/(x) T/(x) Ax) (14)求关于Ax的导数，并令其为0#经过简化可以得 到：

/(x)T/(x)Ax =-/(x)T/(x) (15)由于求解的变量为Ax，因此式（15)为线性方

’。

3 )计算旋转变化矩阵&6，平移变换向量，使

4)计算公式？+1' =&?' +:，其中V?

得公式% II L/ - (&?/ +:6) II 2达到最小值。

#P，给定

II2)

最小误差值，直到最小均方差S(丄％ I L’ 3 6=1I

程。上述方法称为高斯牛顿法，具体步骤如下：

小于或者到达预期迭代次数#为止，否则重复步骤2) 。

3.2 SVD方法

1) 给定初始值x。。2) 对于6次迭代，求出当前雅可比矩阵f和矩阵函数/(x)。

3) 求解公式（15)。

4) 若Ax足够小，则停止迭代。否则，令xxt +Ax，返回步骤2)。

该算法能令/ (x) y (x)近似为二阶Hessian

矩阵，从而忽略计算/ (x) y (x )。高斯牛顿法通

过求解非线性方程的误差平方和达到最小从而求 解机器人状态方程，虽然收敛速度较慢，但是在视

觉SLAM过程中仍有大量的应用。

3

相机位姿求解

基于上述数学问题，在视觉SLAM过程中采用 RGB - D相机采集图像，同时获得图像深度距离，

通过特征点进行相机姿态的估计。

假设通过ORB算法采集到一组匹配好的特征 点集^ 和 = -?，…，？.，9 = - L，…，L.。

点集P通过旋转、平移变化，得到点集9!

=&? +: (16)

式中:&为旋转矩阵为平移向量。

关于 姿的求 ， 通 像 征点匹配计算出图像移动过程，从而映射出相机移动过程。 该方法需要建立两个点集之间的对应关系，利用迭 代最近点（iterative closest point，ICP)寻找对应关 系，求解移动机器人3D空间的位姿。对于中等大 小的点集来说，该方法具备计算速度快、鲁棒性强、 误差小等特性，因此被广泛应用于机器人运动过程 中相机的姿态计算。3.1 ICP算法

ICP 3D 点集匹配 ， 于乘法的最优匹配算法，即通过重复计算旋转矩阵以 及平移向量使得初始点集与最终点集误差满足最 小度量。设初始点集为特征点集P，最终点集为特 征点集9,通过寻找初始点集到最终点集的最小距 离，使得初始集每个点到最终点集都会有映射，其 具体步骤如下：

1

)寻找初始点集P与最终点集9。• 16 •

#P算法根据给定的初始点集与最终点集误 差建立最小二乘法模型，姿求解，)

从而进行相机旋转平移位 初始点集与最终点集中第C个点的误差 项：

8 =Pc-(RL+t) (17)式中:？为点集P的去质心坐标点;L为点集9的 去质心坐标点。

求点集中所有点误差和达到最小的旋转变化

矩阵&和平移变换向量：。利用ICP算法构建的

模型为m：6=士％ II ? - (&l+:) II2 (16)忽略平移变换向量：，将式18)展开得：

min

II P

-&L II 2 =+%(??【+

-2P&L ) (1()

式中:等号右边括号里第一项与&无关，等号右边 括号里第二项由于&&=/，亦与&无关，因此目标 函 为:%P&L =-r( &%LP ) (20)

采用奇异值分解1方 法（SVD)将旋转矩阵分解 为3个特殊矩阵的乘积，先定义矩阵：< = %PL (21)对 < 进彳¥ SVD分解得：

< = =$>

(22)

式中：$为奇异值组成的对角矩阵;=和 >为正交 矩阵。当 <满秩时，有：

& = =>T (23)解出&，可求得平移变换向量：：

：=?’ -Rq’ (24)通过以上的旋转矩阵以及平移向量，对观察模 型进行持续性的变换，使得求解出来的变换后的点 集趋近于真实点集。在使用SVD方法求解过程 中，并没有引入相机的模型，因此显著地减少了计 算量，为求解机器人移动过程旋转矩阵以及平移向 量带来了极大的方便。3.3 非线性优化方法求解

在视觉 SLAM 程中 通 化 求

xt +1 =

2017年第11期

林志诚!移动机器人视觉slam过程中图像匹配及相机位姿求解的研究

误 精确对 化 ，从而估 。该方

（ ， 在 的缺点，

函数难以通用化，故通常利用非线验并优化SV! 求 果。假设

其中(R，/，*)为初始点集P中点？坐标(R， 到 标系下的坐标;/r，/；，/*为 ，

。通 进行迭代，解出李代数

%，从而解析出旋转矩阵与平移矩阵。

P，*)

3D特征点集9中点L，其投 像的像素点坐

标为T，exp(,)?表示特征点集p中点？旋转平 移 的像素点像素位置，与间点 如下：

T =exp(%)?

利用

立模型：

(

3.4

实验验证

的关系

( 25 )

8% =m%i士％ || t -exp(%)L || 2

26)

点集 估 人

移动姿态， 采用常用的RGB - D Kinect 1代进 像采集，Kinect 1代大的特点是可以发送 ，遇到体时 射，Kinect 1代 射光， 发射到返 间 的时间，确定物体与 间的距离， 通 Ki-nect 1代可以获得整个图像的像素深度。Kinect 1 代除了普通的摄像头 ， 有1个发射器和1

， 如 0 ，通 1中匹配好的图

像来验证SVD 求 模型的过程，其中Kinect 1代采集到的图1及图2的深度图像 如图6所示。

红外,发送器

j RGB摄像头

j

丨红外线接收器

ICP算法主要利用

李代数％的表达式如下：

%

«

8

( 27 )

式中:!&为旋转矩阵;\"为平移矩阵。将

化为：

8%+#%&8% +6#%

便

用

代数增量;

!)函数

(28)

进行求解。式中!#%为李

为

比矩阵，

6 Y

；6

~T

0 _LR_ 0 _ ~~_-

>V

_~ ； ; LrJx + ~

图5

Kinect 1代结构图

图6 深度图

最终得出矩阵数组为!

997. 29 7 982. 1012 454. 90

967.10 1 498.90 -4 141.208 490. 50 -4 094.90 38 316.600.277 312 0. 943 324 -0. 182 312 -0. 092 535 0. 215 095 0. 972 199 0. 956 313 -0.252 733 0. 146 939 0. 267 770 0. 945 497 -0. 185 298 -0.093 294 0.0.971 734 216 862

-0.242 9140. 146 2780. 958 955

0. 999 947 651

-0.001 553 141 -0. 010 113 548 3

-0. 001 241 249 380. 999 948 086 37 .

& = 0. 001 540 665 0. 999 998 043

0.010 115 456 0.001 225 602 7

通 误差实验

迭代次

012345678

表1

化方法求解式（25 )，得到总体见表1。

总体误差实验数据体误

2 307 924 136.843 7562 307 911 777.515 9202 307 905 803.967 1052 307 902 851.811 6792 307 901 383.941 9582 307 900 652.705 2692 307 900 288.157 0702 307 900 106.333 0462 307 900 115.609 100

时/

s

0.000 100 8150.000 168 6140. 000 234 9500.000 300 7150. 000 367 8890.000 430 7860.000 493 7060.000 556 5170.000 611 769

：

y

% - 0. 561 249 643 6 - 0.011 017 809 8

-1.088 716 244 5]

• 17 •

2017年第46卷

机械设计与制造工程

由表可以发现，在第6次迭代后总体误差基本 稳定不变，说明算法已经收敛，且非线性优化方法 求解式(25 $模型的变换矩阵为：

% =0.999 950 - 0.001 670 - 0.009 830

器人运动过程中旋转矩阵和平移矩阵。该方法不

仅可以应用于ROB - D相机，同样适用于激光传 感器的位姿估计，对于移动机器人SLAM以及导航 控制的研究具有一定的参考价值。

-0.568 204 -0.010 851 -1.087 120

1

0.001 665 0.999 998 0.009 833 0.⑴1 261

0

0

- 0.001 2770.999 951

0

参考文献：

根据式(26)，％-矩阵左上角3 x3子矩阵为旋转矩阵&，右上角3 x1子矩阵为平移向量：

SVD

方法求解的旋转矩阵&与平移向量f相比几乎无

异样，说明SVD方法已经给出解析解，且给出的结 果是相机位姿的最优值。

通过MATLAB软件的Robotic Toolbox逆解旋转矩阵&，得到旋转角度为0.01 °，基本满足实际情 况，平移矩阵f第一维为-0.56，鉴于Kinect 1代为 深度相机，即图中特征点3D位置已知，因此具备 视觉尺度确定性。K

ic

1代相机内部参数矩阵

已标定，故所获取的图像以8cm为单位测量实际 环境，因此-0.56表亦相机向左平移4. 48cm。由 图1得到图2,与实际情况相机左移5cm大致相 同，从而验证了上述算法的正确性。

4

结束语

在移动机器人视觉SLAM过程中，本文采用

ORB算法对图像特征点进行提取和匹配，利用已

匹配的深度图像，通过ICP算法建立相机运动位姿 模型，求解过程采用SVD方法，并通过实验得出机

% 1 & STACHNISS C. Robotic Mapping and Exploration% M&. Berlin：

Springer Berlin Heidelberg，2009.

%2& LIU H，ZHANG G，BAO H. A survey of monocular simultane­

ous localization and mapping % J &. Journal of Computer - Aided Design & Computer Graphics，2016,28(6) ：530 -535.

%3& DAVISON A J. Real - time simultaneous localisation and map­

ping witli a single camera% C&// IEEE International Conference on Computer Vision， 2003. Proceedings. New York: IEEE， 2003： 1403 -1410.

%4& KOLTER J Z，KIM Y，NG A Y. Stereo vision and terrain model­

ing for quadruped robots % C& // IEEE International Conference on Robotics and Automation. New York: IEEE，2009:1557 - 1564.

%5& HENRY Peter，KRAININ M，HeRBST E，et al. RGB - D Map­

ping ：Using Depth Cameras for Dense 3D Modeling of Indoor En­vironments% M&. Heidelberg: Springer Berlin Heidelberg，2014 : 647 -663.%6 & LOWE D G. Distinctive image fearures from scale - invariant

keypoints % J &. International Journal of Computer Vision， 2004 60(2) ：91 -110.

%7& BAY H，TUYTELAARS T，GOOL L V. SURF： speeded up ro­

bust features % J &. Heidelberg: Springer Berlin Heidelberg， 2006:404 -417.%8& RUBLEE E，RABAUDV，KONOLIGE K，et al. ORB: an effi­

cient alternative to SIFT or SURF % J&. Proceedings， 2011， 58 (11)：2564 -2571.

Research on image matching and camera pose resolution in mobile robot vision SLAM process

LINZhiheng1，ZHENG Song2

(1. College of Electrical Engineering and Automation，Fuzhou University，Fujian Fuzhou，350116,China)

(2. Fujian Key Laboratory of Industrial Control and

Information Security Technology，Fujian Fuzhou，350008，China)solve

the

key

problem of algorithm

image and

matching matches the

and

Abstract ： In order to

camera pos

process，it proposes ORB feature matching obtains a set of feature points. It analyses

two frames

cap

the least squares theory characteristics，uses ICP method to

the camera motion model according to the feature points of two frame image invariance，applies SVD camera pose estimation to build the motion model. It verifies the SVD decomposition met!iod based on the nonlinear optimiza­tion algorit!im，uses the RGB - D Kinect camera to capture images，tests the algorithm. The obtained camerapose data solve effectively process.

the

problem of

three

- dimensional positioning of

Key words : visual SLAM) ORBfeature) matching algorithm) ICP algorithm

+ 18 +