第３４卷第ｌ期２叭３年１月仪器仪表学报

ＣｈｉｎｅｓｅＪｏｕｍａｌｏｆＳｃｉｅｎｔｉｆｉｃＩｎｓｔｌｌｌｍｅｎｔＶ０１．３４Ｎｏ．１Ｊａｎ．２０１３

基于惯性传感器和视觉里程计的机器人定位

夏凌楠１，张波２，王营冠１，魏建明２

（１．中国科学院上海微系统与信息技术研究所无线传感网与通信重点实验室上海２０００５０；

２．中国科学院上海高等研究院上海２０１２１０）

摘要：针对机器人快速运动下，由运动模糊而导致视觉里程计定位估计精度下降的问题，结合惯性传感器和视觉里程计提出一

种定位算法。该方法以扩展卡尔曼滤波（ｅｘｔｅｎｄｅｄＫａｌｍａｎ６ｌｔｅｒ，ＥＫＦ）为框架，利用惯性传感器的航位推算构建ＥＫＦ的过程模型，

视觉里程计作为相对线速度和相对角速度传感器用来建模观测方程，同时考虑到机器人运动在平面上，在垂直方向和侧向方向不

会产生跳动和滑动，利用这两个方向上瞬时速度为零的约束构建另外一个观测方程。提出的定位方法能够克服视觉定位和惯性

定位的缺点，提高了定位精度。基于机器人实测数据进行实验，结果表明提出的算法优于单独采用惯性传感器和视觉里程计。

关键词：机器人定位；惯性传感器；视觉里程计；扩展卡尔曼滤波

中图分类号：ＴＰ３０１．６文献标识码：Ａ国家标准学科分类代码：４６０．４０

ＲｏｂｏｔｌＯｃａｌｉｚａｔｉｏｎａｌｇｏｒｉｔｈｍｂａｓｅｄｏｎｉｎｅｒｔｉａｌｓｅｎｓｏｒａｎｄ订ｄｅｏｏｄ锄ｅｔｒｙ

ＸｉａｎｎｇＩｌａｎｌ，ＺｈａｎｇＢ０２，ＷａｎｇＹｉｎｇｇｕａｎｌ，ＷｅｉＪｉａｎｍｉｎ９２（Ｊ．研儿６０删Ｄ¨矿耽ｒｅｋｓ＆嬲ｏｒ＾协埘。以＆ｃｏ肼Ｈｕ凡缸谢如凡，肌ｏ，ｌ咖ｉ觚胁眦ｅ矿舭瑚”￡ｅｍ

Ⅱ以倒台丌Ｍ￡ｉｏｎ‰＾加２０９ｙ，吼ｉ耻ｓｅＡｃ础，可矿Ｓｃ如Ｍ∞，‰，劬以２０∞５０，吼ｉ№；２．Ｓｋ啦ｏｉＡ幽口愀ｄ

鼢眦ｎ如Ｊｗ疵眦ｅ，吼ｉｎ部ｅ』４ｃ础，Ⅳｏ厂＆如，艄，虢口，ｌｇ讹ｉ２ＤＪ２ＪＤ，ｃ＾ｉ船）

Ａｂｓｔｒａｃｔ：Ａｉｍｉｎｇａｔｔｈｅｐｒｏｂｌｅｍｔｈａｔｔｈｅ１０ｃａｌｉｚａｔｉｏｎｅｓｔｉｍａｔｉｏｎａｃｃｕｒａｃｙｏｆＶｉｓｕａｌｏｄｏｍｅｔｒｙｄｒｏｐｓｄｕｅｔｏｍｏｔｉｏｎｂｌｕｒ

ｕｎｄｅｒｆａｓｔｒｏｂｏｔｍｏｔｉｏｎ，ａｌｏｃａｌｉｚａｔｉｏｎａｌｇｏｒｉｔｈｍｉｓｐｒｏｐｏｓｅｄｂａｓｅｄｏｎｉｎｅｎｉａｌｓｅｎｓｏｒａｎｄＶｉｓｕａｌｏｄｏｍｅｔｒｙ．Ｔａｋｉｎｇｅｘ—ｔｅｎｄｅｄＫａｌｍｅｍ６ｌｔｅｒ（ＥＫＦ）ａｓｔｈｅｆｒａｍｅｗｏｒｋ，ｔｈｅｐｒｏｐｏｓｅｄａｌｇｏｒｉｔｈｍｃｏｎｓｔｍｃｔｓｔｈｅＥＫＦｐｍｃｅｓｓｍｏｄｅｌｕｓｉｎｇｔｈｅ

ｄｅａｄｒｅｃｋｏｎｉｎｇｏｆｉｎｅｒｔｉａｌｓｅｎｓｏｒ；ＶｉｓｕａｌｏｄｏｍｅｔｒｙｉｓｕｓｅｄａｓｒｅｌａｔｉｖｅｌｉｎｅａｒＶｅｌｏｃｉｔｙａｎｄａｎｇｕｌａｒＶｅｌｏｃｉｔｙｓｅｎｓｏｒｓｔｏｍｏｄｅｌｔｈｅｏｂｓｅｎｒａｔｉｏｎｅｑｕａ上ｉｏｎ．ＴａｋｉｎｇｉｎｔｏａｃｃｏｕｎｔｔｈｅｆａｃｔｏｒｔｈａｔｔｈｅＩＤｂｏｔｄｏｅｓｎｏｔｓｌｉｄｅａｎｄｊｕｍｐｉｎｖｅｒｔｉｃａｌａｎｄ

ｌａｔｅｒａｌｄｉｒｅｃｔｉｏｎｓｗｈｉｌｅｍｏＶｉｎｇｏｎａｐｌａｎｅ，ｔｈｅｃｏｎｓｔｒａｉｎｔｔｈａｔｔｈｅｉｎｓｔａｎｔａｎｅｏｕｓｖｅｌｏｃｉｔｉｅｓｉｎｔｈｅｓｅｔｗｏｄｉｒｅｃｔｉｏｎｓａｒｅ

ｚｅｒｏｉｓｕｓｅｄｔｏｃｏｎｓｔｍｃｔａｎｏｔｈｅｒｏｂｓｅｎ，ａｔｉｏｎｅｑｕａｔｉｏｎ．Ｔｈｅｐｒｏｐｏｓｅｄｌｏｃａｌｉｚａｔｉｏｎａｌｇｏｄｔｈｍｏｖｅｒｃｏｍｅｓｔｈｅｓｈｏｒｔｃｏｍｉｎｇｓ

ｏｆｉｎｅｎｉａｌｌｏｃａｌｉｚａｔｉｏｎａｎｄＶｉｓｕａｌｌｏｃａｌｉｚａｔｉｏｎａｎｄｉｍｐｒ０Ｖｅｓｌｏｃａｌｉｚａｔｉｏｎａｃｃｕｒａｃｙａｓｗｅｌｌ．Ｅｘｐｅｒｉｍｅｎｔｓｏｎｒｅａｌｒｏｂｏｔ

ｔｅｓｔｄａｔａｗｅｒｅｃｏｎｄｕｃｔｅｄ．ＥｘｐｅＩｉｍｅｍｒｅｓｕｌｔｓｓｈｏｗｔｈａｔｔｈｅｐｒｏｐｏｓｅｄａｌｇｏｒｉｔｈｍｉｓｓｕｐｅｒｉｏｒｔ０ｔｈｅａｏ甜ｒｉｔｈｍｓｕｓｉｎｇｉｎｅｒ—

ｔｉａｌｓｅｎｓｏｒｏｒｖｉｓｕａｌｏｄｏｍｅｔｒｙａｌｏｎｅ．

Ｋｅｙｗｏｒｄｓ：ｒｏｂｏｔｌｏｃａｌｉｚａｔｉｏｎ；ｉｎｅｒｔｉａｌｓｅｎｓｏｒ；ｖｉｓｕａｌｏｄｏｍｅｔｒｙ（ＶＯ）；ｅｘｔｅｎｄｅｄＫａｌｍｅｍ６ｌｔｅｒ（ＥＫＦ）

１引言

定位是移动机器人应用的关键技术之一，是机器人实现

自主化的—个首要问题，也是机器人自主导航和路径规划的

的基础。定位技术分为相对定位和绝对定位２类。绝对定

位是指利用导航信标、主动或被动标识或卫星导航技术（ｄｏｂａｌｐｏｓｉｔｉｏ血ｎｇｓｙｓｔｅｍ，ＧＰＳ）进行定位¨。，一般用在星空

探索，而且应用场合一般在室外有信标或者ＧＰＳ信号覆盖的

地方。相对定位则是采用里程计、惯性传感器（ＩＭｕ）、声呐

及视觉传感器等确定机器人在操作环境中相对于初始时刻

的位置和姿态。传统的相对定位算法主要分为里程计法和

惯性导航法‘２Ｊ。里程计（ｏｄ０咖田）是一种被广泛应用的方

收稿日期：２０１２—０５ＲｅｃｅｉｖｅｄＤａｔｅ：２０１２—０５

；基金项目：国家重大专项（２０ｌＯｚＸ０３００６瑚４）、中国科学院知识创新工程重要方向项目（Ｙ０２２０８１１３１）资助

万方数据第１期夏凌楠等：基于惯性传感器和视觉里程计的机器人定位１６７

式，通过安装在车轮上的光电编码器记录机器人的运动过程

从而估计其位置。惯性导航法则利用加速度计和陀螺仪实

现定位。由于轮子的侧滑或者积分累积误差的存在，里程计

和惯性导航的定位精度往往很差。基于视觉里程计（ｖｉｕｓａｌ

０ｄｏ腿Ⅱｙ，ＶＯ）的定位技术是目前较为新颖和研究较多的一

种定位技术∞剖。ＶＯ通过刚性挂载在机器人上的摄像机采

集图像数据，根据图像特征和运动约束进行位姿估计，不会

产生积累误差。基于ＶＯ的定位技术是复杂导航算法的基础，如Ａ．Ｊ．Ｄ撕ｓｏｎ∞ｊ及Ｐ．ＮｅⅦｍ＿置等人利用单目摄像机

进ＳｕＬＭ（同时定位与测绘）。但当视觉传感器数据采样率

和机器人运动速度相比较低时，会造成兴趣点数目减少而引

起估计精度降低。与视觉不同，惯性传感器对于短期运动预

测精度较好，不会受到突然运动变化的影响，考虑到视觉和

惯性传感器的这种互补性，本文将着重研究如何通过两者结

合提高定位精度。现有的ＩＭｕ和视觉测程算法大都没有充

分考虑机器人的运动约束。针对此问题，本文将研究使用双

目视觉传感器结合珊ｕ进行定位，考虑到ＩＭｕ和Ｖ０各自

的特点建立系统模型，主要创新点如下：

１）根据ＩＭＵ瞬时反应快的特性，将ＩＭＵ的测量值用

来驱动过程模型以适应机器人的机动性；

２）根据立体视觉不会产生积累误差的特性，将立体

视觉的定位估计结果作为观测模型的观测值以矫正ＩＭｕ

的误差；

３）根据机器人在运动时可能产生的瞬时滑动和跳跃

的约束条件，将ｙ轴和ｚ轴上的观测速度建模为零均值

的噪声。

２算法设计

本文设计的坐标系如图１所示，分别为导航坐

标系（凡），代表机器人相对于初始位置的坐标；ＩＭｕ坐

标系（６），表示惯性测量单元自身的坐标系；摄像机坐标

系（ｃ），代表摄像机自身坐标系。由于ＩＭｕ和视觉里程

计的直接测量值是针对自身坐标系的，因此在系统建模

时需要通过坐标变换转换为本地导航坐标系。

㈥ｌ饥器人’Ｆ台毖ｇ！际系

Ｆｉｇ．１ＲｏｂｏｔｐｌａⅡｏ硼ａｎｄｒｅｌａｔｅｄｃｏｏｒｄｉｎａｔｅｓｙｓｔｅｍ机器人定位问题可归结为非线性系统状态估计问

题。对于一般的非线性动态系统，过程方程和观测方程

可由式（１）和式（２）表示，其中工。和ｙ。分别表示状态向量

和观测向量，Ｈ。为控制向量，，和＾，表示过程函数和量测

函数。扩展卡尔曼滤波通过雅可比矩阵来实现系统的近

似线性化，雅可比矩阵由过程函数和测量函数的偏导得

到，如式（３）～（５）所示，其中Ｒ和Ｆ。表示过程函数对

状态向量和控制向量的偏导，峨表示测量方程对状态变

量的偏导。在过程模型及观测模型已知时，就可以通过扩

展卡尔曼滤波的方法对机器人进行状态估计，具体实现

过程可见文献［９］。

ｊ。＝，（工．Ｉ，Ｈ。）＋ｌ，，（１）

ｙ，＝九。（工。，’．，。）（２）

扯鼽。。（３）

。誓ｋ（４）

峨＝尝＿。．（５）

下面将分别介绍ＥＫＦ的两个重要组成部分，过程模

型和观测模型。

２．１过程模型

过程模型用来描述状态变量在前后两个时刻的传递

关系，针对机器人定位，需要估计的状态向量如式所示：

ｘ＝［ｐ：ｙ：贼鹾８∞：．。］（６）

式中：ｘ表示状态变量，变量中各元素分别表示机器人３个方

向坐标、３个方向线速度、３个方向角速度、加速度误差向量及

角速度误差向量，七表示时间。上标ｎ表示局部导航坐标系，

所有估计都相对于该坐标系。６表示ＩＭＵ参考坐标系。

ＩＭｕ的测量值作为过程模型的输入变量，过程模型

表示为：

ｐ：＝ｐ：一。＋△丁·ｐ：一。（７）

ｌ，：＝ｌ，：＋△ｒ·［ｃ：厂５一９４］（８）

蛾＝蛾一。＋△ｒ·Ｅ：∞：（９）

鲈：＝８，：一。（１０）

８∞：．＾＝８∞：＊ｌ（１１）

式中：ｃ：为ＩＭｕ参考系到本地导航系的离散余弦变换矩

阵（ｄｉｒｅｃｔｉｏｎｃｏｓｉｎｅｍａｔｒｉｎ，ＤＣＭ），联表示从ＩＭＵ参考系

到本地导航系的的旋转率变换矩阵。△ｒ为采样时间间

隔。考虑到ＩＭｕ的测量值包含噪声和偏置，则有：

，６＝，：一８，Ｌ．．，。（１２）

∞：＝ｗ：．。一８’．，：一’．，。（１３）

式中，：和甜：．。为ＩＭｕ的非重力加速度及角速度的测量

值。＇．，。和＇．，。分别表示加速度计和陀螺仪的噪声，６，：和

８∞：．。表示测量值相应的偏置量。将式（１２）、（１３）代人

万方数据１６８仪器仪表学报第３４卷

式（７）～（１１），可以得到由ＩＭｕ原始测量值导出的系统

状态转移模型，如式（１４）～（１８）所示：

ｐ：＝ｐ：一。＋△ｒ·ｌ，：（１４）

ｙ：＝ｌ，：一。＋△丁（ｃ：厂：一ｃ：８，６一ｃ：’．，。一ｇ“）（１５）

蛾＝蛾一。＋△ｒ·（层：’．，：．。一Ｅ：６’．，：一Ｅ：’．’。）

鲈：＝６，：一。

８∞：．Ｉ＝８∞：．＾

２．２观测模型（１６）

（１７）

（１８）

观测模型表达观测量和状态变量之问的关系，一般

来说，该模型可由传感器的测量模型近似得到。在机器

人运动过程中，立体像机连续观测环境的特征点，利用多

视角几何和图像特征提取技术实现定位。利用视觉进行

定位的技术称为视觉运动估计或视觉里程计，视觉里程

计定位包含如下４个步骤：

１）特征检测。首先从左右相机拍摄的图片中进行兴趣

点检测，从摄像机的图像中检测特征点，如Ｈａｒｒｉｓｃｏｍｅｒ¨…、

ＫＬ变换（ｋａ出ｕｎｅｎ１０ｅｖｅｔＩａｎ舶邢，ＫＬＴ）。１“、加速稳健特征

（ｓｐｅｅｄｅｄｕｐ１１ＤｂｕｓｔｆＢａｔｕｒｅｓ，ＳＵＲＦ）¨２。和尺度不变特征转换（ｓｃａｌｅ．ｉｎｖａｒｉａｎｔ胁ｕｒｅｔｒａＩｌｓｆｏ硼，ＳＩＦｒ）¨３。等算法。本文图像

特征提取采用ＳＩ兀１算子，ｓＩＦｒ算子的优势在于其特征点对

图像缩放、旋转甚至仿射能够保持不变性；

２）立体匹配。立体匹配的目的是针对物理空间的同

一点，将其在不同图像中（双目摄像机的左右摄像头图

像）的成像点对应起来，形成视差信息以求得该点的相对

三维坐标。本文采用对极几何法获得。

３）时域特征点跟踪。利用最小化零均值归一化互相

关的方法实现同一摄像机（如左摄像机）在不同时刻的

特征点匹配，而后通过三角定位获取特征点相对当前摄

像机的三维坐标。

４）运动估计。在立体匹配和时域特征点跟踪的基础上，

就可以通过机器人在相邻采样间隔内相对于同一物理空间

点的相对位姿的变化，对其进行运动估计。本文采用

ＲＡＮＳＡＣ（砌ｄｏｍｓ趼ｌｐｌｅｃｏｎｓｅＩｌｓｕｓ）方法实现运动估计¨“。

视觉里程计得到是前后两个连续采样时刻的相对位姿，

那么除以前后两个时刻的时间差，就能够得到机器人的相对

线速度和角速度，以此作为量测值，可建立如下的观测模型：

ｌ，：．＾＝ｃ：ｌ，：＋Ｉ，¨（１９）

Ⅱ垆一佃严∞：：Ｉ，Ｉ＝Ｅ：（型—≯）＋％，。（２０）‘ｊ』式中：下标ｍ表示测量值，ｌ，：．。和∞：．。为Ｖｏ观测到的机

器人的线速度和角速度向量。ｐ。为均值为０、协方差矩阵

为霞。的高斯白噪声。

机器人在平面上运动，理想情况下并不会在平面上

产生跳动或侧滑，假设在机器人沿着其参考系的ｚ轴方向行进，则自由约束下其在ｙ轴和三轴方向上的速度为

零。将这两个约束作为另一个量测值，构建另一组量测方

程。如式（２１）、（２２）所示：

秽ｈ＝０（２１）

秽ｋ＝０（２２）

ＩＭｕ参考系和导航坐标系的关系由式（２３）计算：

盱哦篆并兰茹篙抄

『钌＂（ｃｏｓ咖ｃｏｓ砂＋ｓｉｎ咖ｓｉｎｐｓｉｎ砂）１．ｏ秽。，（ｃｏｓ咖ｓｉｎ口ｃｏｓ砂一ｓｉｎ币ｃｏｓ砂）１

［嚣宅。¨：：：：】㈤，

考虑到实际应用中，往往会产生侧滑，因此上述理想

情况并不成立，本文中，将这种模型的不确定性建模为高

斯噪声，如Ｉ，。，和ｌ，ｋ，咖、９和沙表示３个方向上的欧拉角。

２．３算法框架本文采用的算法流程如图２所示。首先，基于视觉

传感器进行视觉运动估计，将其估计结果作为ＥＫＦ的观

测，然后结合由ＩＭＵ驱动的过程模型，最终得到系统状

态的估计结果。

系统状态估计

图２本文提出的算法流程图

Ｆｉｇ．２ｎｏｗｃｈａｒｔｏｆｔｈｅｐＩ＿ｏｐｏｓｅｄａｌｇｏｒｉｔｈｍ

在ＥＫＦ的迭代滤波过程中，由于观测值的偏差可能

会导致滤波发散，因此本文在更新过程中，利用观测值和

预测观测值之间的差别来表征滤波的性能，这个差别可

称为残差。这里采用马氏距离来计算残差，以检测含噪

观测值，如式（２４）所示：

ｙ＝（ｚ｝一ｚ＾）１Ｓ叫（ｚ＆一ｚＩ）（２４）

式中：ｚ；和三。分别为观测值及预测观测值，两者的协方差

万方数据