移动机器人同时定位与地图创建算法研究_图文

南京理工大学 硕士学位论文 移动机器人同时定位与地图创建算法研究 姓名:邓翔 申请学位级别:硕士 专业:模式识别与智能系统 指导教师:赵春霞 20080620

硕士论文

移动机器人同时定位与地图创建算法研究





机器人通常需要基于一定的环境地图进行定位,从而实现自主导航。然而当机器人 处在未知环境中时,没有足够先验地图信息,机器人需要通过对环境特征的感知与估计 来定位,同时对所获得的环境信息构建环境地图,即进行同时定位与地图创建(SLAM)。 本文的研究工作就是对同时定位与地图创建的相关方法进行研究与探讨。 本文通过研究机器人系统相关模型,结合SLAM问题的一般模型构建了一个简化的 仿真平台;在该仿真平台中对移动机器人同时定位与地图创建问题相关算法展开研究。 基于几种不同仿真环境,对EKF SLAM方法与UKF

SLAM方法的各项性能如定位精确

度、运行时间、环境特征估计等进行了详细的分析和对比。 FastSLAM近年来做为SLAM方法的研究热点备受关注,本文通过用UKF代替EKF

作为建议分布,结合FastSLAM的方法框架,得到了UKF FastSLAM方法。同样,本文

在所构建的仿真环境中详细分析与对比了FastSLAM与UKF FastSLAM方法,实验结果表
明,UKF FastSLAM是一种更加有效的机器人同时定位与地图创建方法。

关键词:同时定位与地图创建、扩展卡尔曼滤波、FastSLAM、UKF

FastSLAM

Abstract

硕士论文

Abstract

In order to autonomous navigation,robot usually needs the map of the environment to locate its position.However,when the robot is in
a11

Rnknown circumstance,there is
sense

not any

map information.So,in

order to locate its position,robot needs to

and estimate the
on

features of the environment and the robot needs to create the

map

of

environment based

the sensed and estimated

environment

knowledge

at

the same time.This process is called
content of

simultaneous localization and mapping problem.The research
on

this paper is focused

the algorithm of simultaneous localization and mapping. This paper built


simulation

platform

after studying the model of robot’S system

and

integrating SLAM problem model。After this process,we study the algorithm of simultaneous localization and mapping
and UKF
on our

built platform.This paper also analyzes EKF

SLAM method

SLAM method in detail,and compares the performance of EKF SLAM method
SLAM such
as

、砘th UKF
On.

positioning accuracy,run-time,the estimated environmental features

and

SO

FastSLAM

considered as




hot spots method of SLAM is payed much attention in

recent

years.This paper gets

method called UKF

FastSLAM
also

method according to using UKF

method as proposal distribution instead of using EKF method and integrating the framework
of

FastSLAM.At the same time,this paper

analyzes

EKF

SLAM method

and UKF

SLAM method in detail and compares the FastSLAM
effective

performance

of FastSLAM method with UKF


method.The

experiment rcsuRs show that UKF FastSLAM method is

more

simultaneous

localization and mapping method for robots.

Keywords:Mobile robot,SLAM,Simultaneous localization and mapping,Extended

kalman

filter,Unscented transformation,Particle filter,FastSLAM,UKF-FastSLAM



声.明
本学位论文是我在导师的指导下取得的研究成果,尽我所知,在 本学位论文中,除了加以标注和致谢的部分外,不包含其他人已经发 表或公布过的研究成果,也不包含我为获得任何教育机构的学位或学 历而使用过的材料。与我一同工作的同事对本学位论文做出的贡献均 己在论文中作了明确的说明。

研究生签名:

学位论文使用授权声明

南京理工大学有权保存本学位论文的电子和纸质文档,可以借阅 或上网公布本学位论文的部分或全部内容,可以向有关部门或机构送 交并授权其保存、借阅或上网公布本学位论文的部分或全部内容。对 于保密论文,按保密的有关规定和程序处理。

研究生签名:』孕肇k

御年/彤日

硕士论文

移动机器人同时定位与地图创建算法研究

1绪论
机器人从诞生之日起便备受关注。在经历了一个长期缓慢的发展过程后,随着计算 机技术、人工智能技术、微电子技术等的快速发展;已经从最初功能单一、简单的工业 机器人向功能多样化、应用广泛化扩展。作为机器人领域的自主移动机器人更是受到世 界范围的研究与应用。自主移动机器人已经称为机器人学与人工智能领域研究的重要对 象。自主移动机器人的研究,开发与应用是一个涉及到人工智能、传感器技术、计算机、 电子、控制工程、机械等多个领域的系统工程。它在工业、军事、航空、航天、医疗等 方面都有着广阔的前景。
”Where

anl



Where

aria

I going?How should I get

there?”这是Leonard,J等提出的关

于移动机器人导航的三个基本问题【l。21。第一个问题就是移动机器人的定位问题。显然 问题一是整个工程的关键所在。定位问题就是要明确任意时刻机器人在环境中的位姿, 要实现定位就必须依托于环境地图。 本文针对移动机器人的同时定位与地图创建问题进行研究。

1.1移动机器人发展概述
具有高度智能的自主移动机器人,从机器人产生的那天起就一直是人们追求与研究 的最高目标。近年来随着计算机技术、传感器技术及人工智能等技术的迅猛发展,自主 移动机器人的研究已经有了长足的进步,并逐步向工程应用和产业化方向发展,有的已 经进入商业化开发阶段。 1.1.1国外现状 国外对移动机器人的研究起步较早。上世纪60年代末,斯坦福研究院(SRI)的Nisl
Nilssen和Charles Rosen等人【3】,在1969年至1972年中研制出了名为Shakey的自主移动机

器人,开了移动机器人领域研究的先河。80年代中期,随着一批著名的世界性公司与院 校开始进行移动机器人平台的研制与开发,移动机器人出现了多研究方向的趋势,在许 多方面都取得了重要进展;一股自主移动机器人研发的浪潮汹涌而来。 时至今日,在众多方面都有移动机器人的应用。上至空中、太空、行星表面下至地 面、水下等环境都有着关于移动机器人的研究。美国航空航天局(NASA)研制的 Sojourner、Rocky7、Spirit等火星探测机器人;卡耐基.梅隆大学(CMU)研制的

CMU.Rover;麻省理工学院(MIT)的半自主型机器人Wheelesley;悉尼大学的自主导
航车;美国马里兰系统工程实验室(MSEL)研制的EAVE III型自主式水下移动机器人 等都代表着移动机器人领域的较高水平。这些研究工作说明移动机器人的研究工作有着

第一章绪论

硕士论文

蓬勃的发展和重要意义,给人类生活带来极大的进步。德国Bonn大学研制开发的RHINO 及卡耐基.梅隆大学的MINERVA代表着目前室内自主移动机器人的典型应用;它们采用 模块化,分布式的架构;将定位、地图构建,避障、路径规划等模块集于一体,在实时 构建与更新地图的基础上完成各种任务。 1.1.2国内现状 国内对移动机器人的研究较国外虽然起步晚,但是越来越受到重视,国家863智能 机器人组就把智能移动机器人的研究作为今后发展的重点。从八五计划开始,我院就已 经展开了自主移动机器人方面的研究,对基于地图的全局路径规划、基于传感器的局部 路径规划、信息融合技术等一系列方面都进行了研究与探索。同时,国内很多科研机构 与院校也在展开着对自主移动机器人的深入研究与开发。以中科院自动化研究所、清华 大学、国防科技大学、东北大学、上海交通大学、哈尔滨工业大学为代表的我国机器人 研究基地,已经取得了令人瞩目的成果。有以下一些主要的研究工作:清华大学研制的 THMR系列移动机器人系统、中科院自动化的智能移动机器人CASIA.I,香港城市大学 智能设计、自动化以及制造研究中心的自动导航车和服务机器人,哈尔滨工业大学机器 人研究所研制的能在未知、复杂、多变、恶劣的环境下运行的全方位轮式移动机器人平 台等。此外,我院在十五期间研制完成的智能机器人在避障、自主移动、危险环境作业 等方面也有其自身的优点。这些研究工作充分地说明我国在移动机器人领域已经有了长 足的进步。但总体来讲,与发达国家还存在着差距。

1.2移动机器人的地图创建与定位
通常进行路径规划和定位总是结合一定的环境地图,这样对地图创建问题就成了一 个需要研究的重要内容。通常机器人利用各种传感器获得对环境的感知信息。基于对环 境的感知构建相应的环境地图。通常使用栅格地图,几何特征地图,和拓扑地图等方法
来构建地图。

栅格地图是最初用来表示地图的方法,它是一种简单的地图表示方法。对于2D的栅 格地图,整个环境被分割成一定大小的栅格,每个栅格赋以一定值,表示该单元被占用 的概率。该方法I扫Moravec和Elfes提出【l孓161,在很多方面有着广泛应用。通常情况下, 一个单元格表示一个方形区域,用[0,1】区间中的值来表示被占概率,表示对应空间处是 否存在物体,对于被占的栅格赋值为l,自由空间为0。从栅格地图中可以清楚地看出某 一区域有物体存在还是属于自由空间。栅格地图易于生成和扩展,但用于定位时要求大 的存储空间和计算量;随着环境增长及精度要求上升,计算量及存储空间急剧增长,不 能满足实时性需求。几何特征地图以环境路标特征在全局坐标系中的位置参数表示来表 述环境地图【9,1‘71。任意一个环境特征都在环境地图中精确描述,要求环境观测目标具有


硕士论文

移动机器人同时定位与地图创建算法研究

一些基本的几何模型,这样可以很方便地结合模型通过一些参数来描述这些特征。由于 室内环境具有显著的结构化特征,所以可以很方便地用点和线段来描述。因此几何特征 地图在室内环境中构建地图是常用的。拓扑地图运用节点和连接路径来表示环境地图, 用一张图来描述环境信息。拓扑地图定义为一个图数据结构,一个图节点表示环境中的 一个特殊位置,一个图中弧表示两个节点间的路径信息。这样,两个不相邻节点间导航 就表示为一串中间节点。计算机器人相对于拓扑地图的相对位置,是基于拓扑地图定位 要做的主要工作。基于拓扑地图的移动机器人路径规划等后续工作是相对容易展开的。 几何特征地图的构建就是一个从数据源中提取点、线等几何模型,并利用线段和特 征点的位置参数来表示环境特征对象的过程。环境地图由坐标系和所有几何特征在该坐 标系中的位置参数构成。在室内环境中,墙角、边缘等都是常见特征。要利用这些特征
通常要进行角点和边缘提取等预处理工作。

对于移动机器人来说实现其自主性、精确的位置估计是必须的。只有获得了其自身 的精确位置才能更好地达到完成规定任务的目的。有各种各样的方法可以获得机器人的 位置信息。一部分定位方法是通过自身的运动信息来实现。而另外一些定位方法多借助 予对外部环境信息的感知。 从最初利用掩埋的导航带进行定位导航开始,依次经历了基于信号灯的定位,以自 然路标为基础的结合环境地图的定位。发展至今,已有许多成熟的滤波方法应用于现代 化的融合多种外部与内部传感器的定位系统。对于移动机器人的定位主要需要解决位置 跟踪和全局定位两个方面的问题。 位置跟踪是指从给定的初始位置开始,对机器人的位置和路径进行跟踪的问题。航 位推测是其最基本的方法,它仅仅使用运动估计来对机器人位置进行计算。对于航位推 测来说,最大的问题就是误差的累计,随着时间的变化,误差不断增大。这种误差是无 法消除的即使提高传感器的精度也没什么效果。在短时间范围内航位推测是有效的,可 以结合由传感器获得的环境信息,基于一定的环境地图,从而实现机器人的精确定位。 通常将机器人的位姿处理为一种系统状态,结合滤波方法来对机器人的位置进行滤波处 理,得到更精确的位姿信息。这样机器人的位置跟踪问题就成为一个最优位置估计问题。 通常机器人的动态系统方程与其对环境的观测方程都是高度线性化的,需要进行近似处 理来得到其次优位置估计。通常使用对概率密度的近似状态来处理,有基于蒙特卡洛方 法的粒子滤波器;或者使用对非线性系统进行线性化处理的方法,扩展卡尔曼滤波就是 这种方法的代表;还有使用对贝叶斯公式进行数字化积分的方法。 全局定位指的是在不确定情况下得到在全局中的位置信息。要解决全局定位的问题 通常需要有已知的环境地图。通过比较机器人所获得的局部信息与全局信息进行匹配与 比较来确定机器人的位置,它能很好地解决机器人丢失或机器人绑架问题。

第一章绪论

硕士论文

1.3移动机器人的同时定位与地图创建
自主性是对移动机器人的最朴素与基本的需求。为了使机器人在所处环境中自主执 行任务。首先要让机器人了解所处的环境及其自身在环境中的位置。机器人感知自身在 环境中位置便是机器人的定位问题。要了解所处环境最直接的方式就是对环境进行建模
即创建环境地图。

通常情况下,总是单独处理移动机器人的定位问题与地图创建问题。针对定位问题 已经有很多方法应运而生,一些定位方法借助外部的环境感知信息,一些定位方法完全 依靠机器人自身的运动信息。对于移动机器人的定位主要有位置跟踪与全局定位两种方 式。对于地图创建也有栅格地图、几何特征地图、拓扑地图等一列方法来解决这方面的 问题。 在有一定先验信息的环境中,进行移动机器人的定位与地图创建已经取得了有效的 进展,这类问题已能被很好解决。但是当面临一个未知和陌生的环境时,很多实际应用 (如敌对环境侦测),就面临这样一个局面,预先不存在地图,为了定位,机器人需要有 确切的环境地图;为了创建环境地图,机器人又需要知道各时刻自身的位置。为了有效 感知未知环境,机器人需要通过自身传感器来获取环境数据,利用这些数据来估计环境 地图,同时利用预估地图进行定位,也就是说需要进行同时定位与地图创建。
移动机器人的同时定位与地图创建(SLAM)I;-]题,也称为CML(Concurrent Mapping
and

Localization)可以描述为:机器人在未知环境中从一个未知的位置开始移动,在移动

过程中根据位置估计和传感器观测进行自身定位,同时建立环境地图【6刁】.Simth
Cheeseman

Sel印

P卜副最早提出SL』蝴问题,将定位与地图创建问题整合在一起。SLAM已经

被看成是移动机器人自主的关键。近年已经成为移动机器人领域的研究热点。有关移动 机器人SLAM的研究已经在理论和实践方面取得长足进展,很多实用性强的方法已经在 室内、户外及水下等环境中得到应用。 针对移动机器人的同时定位与地图创建问题主要有基于扩展卡尔曼滤波(EKF)的 SLAM方法【4,9-10】、基于粒子滤波的SLAM方法is]等。由于EKF算法在处理不确定信息时 的独特优势,因此在机器人导航领域有着广泛应用。自提出后针对不同传感器产生多项

研究,EKF成为应用最广泛的SLAM方法,并相继在室内环境【111、结构化道路、水下u2】、
小范围室外环境【13】等取得成功。同时很多基于EKF的改进算法也不断提出,如Sebastian Thmn等人提出的基于空间扩展信息滤波器的SLAM方法Il钔。粒子滤波是一种应用样本 集表示概率密度分布的蒙特卡洛方法,可以用于任何状态空间模型,而且当样本数量 Ⅳ寸∞时可以逼近任何形式的概率密度分布。因此,粒子滤波能够比较精确地表达基 于观测量和控制量的后验概率分布,从而用于解决SLAM问题。

要有效地解决SLAM,下面几个问题是需要解决的:


硕士论文

移动机器人同时定位与地图创建算法研究

(1)SLAM问题的一个难点来自对测量噪声的建模问题。当来自不同传感器的测量噪 声是不相关时,地图创建问题将很容易解决,机器人可以简单地通过采集更多数据来消 除噪声的影响。不幸的是来自不同传感器的测量数据误差具有一定的相关性,这是因为 这些误差随着时间累计并影响传感器后续数据获取,可以通过引入一种滤波方法来解决 这一问题。同时引出测量模型和机器人自身的非线性,最后将面临系统的不可完全观测。 (2)SLAM问题的又一个复杂方面来自在地图创建时,被表示的实体具有高维度性, 当使用拓扑地图表示时,像走廊、交叉口、房间、门等用少量数据就可以表示,当用详 细的二维几何特征地图来表示时通常有数以千记的特征需要表示。尤其需要建立3D视觉 地图时数据将是海量的。从统计学的观点来看,每一个数据的都是一个潜在的估计问题。 这样使得地图创建成为了一个超多维问题。 (3)SLAM问题中最难解决的一个问题就是数据匹配的问题,即数据的相关性问题。 匹配问题指的是确定机器人的传感器在不同时空位置上观测到的数据是否来自场景中 的同一物体。当机器人在一个大循环环境中想要闭环时,它必须找出那些是和已经建立 的地图相关的数据。当世界循环结束时,机器人的累计位姿误差会无限大使得这一问题 相当复杂。可能存在的假设数目随着时间成指数级增长,也使得数据匹配相当困难。 (4)所面临的很多环境是都是动态的,环境随时间而变化。机器人所处环境的动态 性给SLAM造成了很大的挑战。假设机器人面临这样一个场景,一扇关着的门打开了, 门在闭着时已经处理过。这样一个状态有两种假设可以用来解释,门的状态发生了变化, 或者机器人已经不在它认为的场景中。迄今为止,也没有有效的地图创建算法可以解决 动态环境这一问题。取而代之的是对环境进行静态性假设,只有机器人自身是随时间而 动,其他物体的运动等均当作噪声处理。 相当多的研究者相信没有一个传感器可以单独获取关于环境足够多的相关特性。为 了解决这一问题,有必要进行传感器融合,融合来自不同传感器的数据。 SLAM还面临这样的情况,那就是机器人在创建地图时同时得选择自己的路径。通 常将在机器人构建地图的同时决定机器人运动轨迹称为自主探索,作为机器人路径规划 与控制的一部分。为了构建地图机器人需要探索并对未知区域构建地图。通常假设机器 人在开始探索时没有任何关于环境的先验信息,然后为了在最短时间内最大化地探索地 图而指定一定运动策略,称这种运动策略为探索策略,它对所使用的传感器有着很强的
依赖性。

一言以蔽之,SLAM需要解决好对不确定信息的处理、算法复杂度、数据关联、动 态性、多传感器融合等关键问题。 对于SLAM问题的研究有着以下几个方面的趋势:扩展应用环境、寻找更有效的

SL姒方法,以及更高效的地图表示方法等。

第一章绪论

硕士论文

1.4论文的研究内容与组织结构
定位问题是机器人自主导航的核心问题,本文研究工作围绕机器人自主定位进行, 定位需要依靠环境地图的构建,本文同时研究环境地图构建。环境地图的构建以机器人 位置为基础,而机器人定位又需要环境地图。要解决这两个相互依托互为前提的问题就 必须把它们综合成一个问题,即同时定位与地图创建来加以研究。 在当前有关移动机器人同时定位与地图创建问题的研究中大多采用声纳和激光雷 达等距离传感器,但是这些距离传感器所获得的信息量较少,且分辨率低。所以在复杂 环境中,由于传感器测量的不确定性所带来的准确度和可靠性严重下降。采用高分辨率 的视觉传感器或者基于多传感器的数据融合为解决这一问题提供了一个方向。本文在仿 真激光雷达传感器进行移动机器人同时定位与地图创建算法研究的同时,尝试采用视觉 的方法,通过仿真视觉传感器的相关性能,来研究移动机器人同时定位与地图创建这一 问题。 本文主要研究机器人定位、环境地图创建及同时定位与地图创建的相关理论与算 法。文章结构如下: 第一章为绪论。介绍了移动机器人的研究现状,同时对移动机器人的同时定位与地 图创建问题进行了阐述。 第二章为SLAM问题的相关模型。本章中定义了移动机器人系统需要的各种模型, 包括坐标系统模型、环境地图模型、机器人位置模型、里程计模型、机器人运动模型、 传感器模型、环境特征动态模型、噪声模型。依据这些模型建立仿真环境,为后续工作 做好理论铺垫。 第三章为基于卡尔曼滤波(KF)的SLAM研究。首先探讨了将扩展卡尔曼滤波(EKF) 应用到SLAM问题中的具体方法与流程。接着分析了将基于UT变化的UKF应用到SL』~M 中方法与过程。最后进行了仿真实验,对EFK SLAM与UKF SLAM进行比较与分析。 第四章为基于粒子滤波的SLAM研究。首先给出了粒子滤波的一般原理。然后研究 了RBPF的粒子滤波方法与基于RBPF架构的FastSLAM方法。接着研究了改进的 FastSLAM方法UKF.FastSLAM方法。最后进行了仿真对比实验,分析与对比了FastSLAM
与UKF.FastSLAM方法。

第五章为全文总结与展望。总结本文的具体工作,并分析其中的不足及对进一步研 究提出一些展望。



硕士论文

移动机器人同时定位与地图创建算法研究



SLAM问题相关模型
要构建一个好的仿真系统,并基于这样一个仿真系统来研究相关的SLAM方法,

就需要精确定义各种相关模型。任何SLAM方法的实现都是基于一定的移动机器人运动 控制系统及其环境的数学模型进行的。只有精确地描述这些模型才能更好地构建这些模

型的相关环境来解决SLAM问题。在本章中详细定义了在SLAM中用到的各种模型:
有坐标系统模型、环境地图模型、机器人位置模型、里程计或控制命令模型、机器人运 动模型、传感器观测模型、环境特征动态模型、噪声模型等,本文基于这些模型构建简 化的仿真环境,给后续的SLAM方法研究夯实基础。

2.1机器人相关模型
2.1.1坐标系统模型 在移动机器人的定位导航中,主要使用三类坐标系统:一是笛卡尔坐标系统;二是 极坐标系统,大多数距离/方向传感器,如激光雷达、声纳等,都是采用这种坐标系统; 三是D1N70000坐标系统,在欧洲的移动机器人领域广泛使用。笛卡尔坐标系和 DIN70000坐标系是直角坐标系统的两种表示形式。本文选用笛卡尔坐标系作为直角坐

标的表示形式,机器人位姿群=[《Y:①:】、环境特征位置厶=(薯,Yt)和传感器位置
xs=(妊挑)均采用笛卡尔坐标表示。 在移动机器人的定位导航中,主要用到三个坐标系:全局坐标系XwOwYw,机器

人坐标系XRO“R及传感器坐标系XsOsYs。其相互关系如图2.1所示。
一般情况下,机器人的系统模型都是基于这三个坐标系来定义的。有时也会把传感 器坐标系统--N机器人坐标系中,只采用全局坐标系和机器人坐标系。

图2.1移动机器人的坐标系统

2.1.2环境地图模型



第二章SLAM问题相关模型

硕士论文

地图创建是一个对移动机器人感知到的环境信息进行表示的过程。通常有几何特征 地图、栅格地图及拓扑地图等典型的表示环境地图的方法。本文在进行移动机器人同时 定位与地图创建算法研究时采用几何特征地图来表征环境,使用点特征来抽象环境特征 模型最后构成环境地图,任意特征点均使用其在全局坐标系中的位置坐标厶--(x,,M)来
表示,环境地图表示如下

厶 X:=M= 厶



(2.1.1)

厶 由于SLAM问题本质上就是解决未知环境中的定位与导航问题,所以本文研究 SLAM相关算法时将地图创建过程归结为对一系列环境特征点的位置估计问题;同时地 图需要动态更新,因此环境特征点的位置将和机器人位姿一起进行估计与更新。 2.1.3机器人位置模型 对移动机器人进行定位是一个对机器人的位姿进行估计的过程,也就是确定机器入 在全局坐标系中的位置及车体方向的过程。通常将机器人的位置使用全局坐标中的一个 坐标点(《,Y:)表示,而车体方向则用机器人车体偏离全局坐标系Xw轴的夹角西;表示, ①:的方向定义为:以Xw轴为零度,顺时针方向为负,逆时针方向为正,夹角的范围为
一180~180度。

由于本文所研究的情况中将机器人限定在二维平面中运动,所以用一个三维状态向

量《=[《Y:睇】来表示机器人的位姿,其中(《,Y:)表示机器人k时刻的位置,彤表
示该时刻机器人车体的方向。 2.1.4里程计或控制命令模型 在进行机器人的位置跟踪时,机器人当前位置的推算是基于先前的位置估计和内部 传感器信息。里程计作为相对定位的有效传感器已经在移动机器人定位中得到了充分的 应用,它记录了在一时间间隔内,机器人所走过的距离和方向偏转。通常假设机器人是 沿一个圆弧运动,如果时间间隔比较小,这种假设是合理的。 假设机器人左右轮的移动距离分别为△d:.和△以,两轮间距为a,机器人从位姿

五=(xk,虬,皖)移动到.硌+。=(‰。,Yk+p嚷+。)如图所示。则机器人移动的距离为
ZkD=(△吮+zk/R)/2,机器人转过的角度为AO=(△吒一△畋)/a。以‰+l=(ADk+l'△幺+I) 作为里程模型的输入。里程计模型主要考虑机器人在运动中航向角的变化,以圆弧近似 机器人移动的实际轨迹。模型方程为:



硕士论文

移动机器人同时定位与地图创建算法研究

五+I=厂(五,Uk+。)

最+%(sin(皖+△‰)痂皖 n一甓(cos(吼+△钆)…s皖
皖-'1-△幺+1

,I△皖I>o(2.1.2)

另一种获取运动信息的方法是基于控制命令的。如果某一时刻的控制命令为机器人 运动的线速度及角度偏转(圪,%),时间间隔为AT,则机器人走过的距离为圪?△丁。

图2.2轮式机器人里程计圆弧模型

2.1.5机器人运动模型 通常从概率上来考虑移动机器人的同时定位与地图创建问题。从概率意义上考虑, 将SLAM问题中的运动模型当成一个马尔可夫(Markov)过程来处理,即系统k时刻的状 态完全由k一1时刻的信息完全决定,这是用概率方法处理SLAM问题的关键。

运动模型描述的是机器人状态Z在控制输入蚝和噪声干扰唯作用下随时间发生变
化的具体过程,通常用一个离散时间差分方程表示

群=工(墨l,U七)+唯

(2.1.3)

其中,工(?)为系统的状态转移函数,通常其具有高度非线性。群为k时刻的机器
人位姿状态,‰为输入的控制指令,%用来表示机器人运动过程中,传感器的误差漂移、 轮子打滑及系统建模的误差等各方面误差。 仿真不同的传感获取的数据,就可以得到不同的动态模型来建模机器人的运动。理


第二章SLAM问题相关模型

硕士论文

想状态中,机器人的运动应该被所使用的机器人运动模型准确地描述,得到机器人状态 的运动变化过程。现实中,只使用有限的参数所进行的系统建模不可能完全表达系统的 动态变化过程,并且机器人所使用的传感器及机器人运动本身都就有一定程度的噪声, 使得机器人的运动模型带具有某种程度上的不确定性。这样就必须采用一个高度复杂的 非线性函数来完整描述机器人的运动,这为导航算法的实现带来了难度。在实际运用过 程中,通常采用一个简化的运动模型来近似,本文采用如下方式在仿真中进行处理。

图2.3轮式机器人模型

当基于控制输入模型时,对于图2.3所示的轮式机器人,将传感器坐标系统一到机 器人坐标系中,有控制输入心=[圪吼】r施加于机器人,机器人的运动模型可表示为

砝l+AT?攻?cos(O;一1+吼) E=

2I扇+AT?圪?sin(嘁一I+%)

啦。+掣
1 0 O 0 1

+圈

(2.1.4)

其中,圪为机器人前向速度,吼为机器人前轮偏角,△r为航迹推算传感器的采样 时间,三为两轴的轴距,匕,b,~为噪声项,用于描述轮子打滑等未知特征的影响。 此时,可得到运动模型相对于X’及"的Jacobian矩阵: 0-AT?圪?siIl(①:-l+%) 1-AT?圪?cos(C,;一l+%)
(2.1.5)

既=熹k㈧=
既=鼍k㈧=
CW、“H”t


AT?cos(①:-l+%)?AT?圪?sin(S;一l+%) -AT?sin(e1);一l+哝)AT?圪?cos(①:;-l+吼)
AT?sin g 工
10

(2.1.6)

AT?圪?cos岱k


硕士论文

移动机器人同时定位与地图创建算法研究

2.1.6传感器观测模型 机器人自定位的方法中航位推算是最常见的一种形式,但随着时间变化它的误差无 限制累积,致使机器人位姿的不确定单调增加,必须融合其它距离传感器获得的对环境 特征的观测信息,以减小定位误差。 距离传感器的观测量Z通常为某个环境特征相对于机器人的距离和方向,在极坐标 系中表示为 Z=【,护r
(2.1.7)

传感器观测模型描述了传感器观测量与机器人位置之间的相互关系,对于不同的观 测量表示形式,会有不同的观测方程

磊=办(五)+%

(2.1.8)

其中,乏为k时刻观测量,办(?)为测量函数,通常为非线性,%为观测噪声,用于
描述测量中的噪声和模型本身的误差。

SLAM中,使用视觉传感器或激光雷达等传感器测量得到特征离机器人的距离厂及 方向夹角乡,观测模型可写为

z:=办c五,=[;]=
矩阵

+‰

(2.1.9)

其中,(t,M)为探测到的第f个特征的位置坐标。此时,可得到观测模型的Jacobian

爿七2面I(以j4)_

zr

锄l

垒一Ay △ △



o….垒


.A...y......0 △

一等冬一l 0….百Ax △2 △2 △2


(2.1.10)

等…o

其中:缸=(《一五);Ay=(联一乃);△=√(缸)2+(如)2。
2.1.7环境特征动态模型 在SLAM算法中,使用环境特征的动态模型描述环境特征状态随时间的变化。通 常为了简便,假设环境特征为静止状态,使用点特征对其建模,并用其在全局坐标中的 位置厶=(墨,Yi)表示,其中i=1,2,…n为环境特征的数量。由于特征是静止的,所以环 境特征的动态模型可以表示为

嘲=x,(k-01]lj
l咒(尼)j—
对地图的动态模型描述如下

叫,)
uJJu

第二章SLAM问题相关模型

硕士论文

霹=磙l
2.1.8噪声模型

(2.1.12)

在对移动机器人的同时定位与地图创建问题研究中,使用各种不同传感器对机器人 运动本身或者所处环境中的环境特征进行测量。由于受到各种噪声的干扰,这些测量是 具有某种程度的不确定性,通常用一个噪声模型来表示这些不确定。高斯噪声模型是最 常用的噪声模型。 为了得到完全精确的系统模型,在进行系统建模时需要引入许多具有高度非线性的 参数。为了表示方便,通常只是建立一个近似模型,在近似处理过程中同样也引入了误 差,通常称为模型噪声,也用高斯噪声模型来表示。
2.2

SL?w问题的一般模型
为了获得所处环境的相关信息,机器人通过安装各种不同的感知外部信息的传感器

来达到这一目的。激光雷达、声纳、摄像头等是最常用的传感器。同时机器人通常使用 惯导、里程计、电子罗盘等内部传感器来预测自身位置即进行自定位。由于测量误差存

在于所有的传感器中,这个便成了SLAM的模型中需要处理的一个问题,在SLAM模
型中必须要考虑它们并加以处理。

采用概率方法来处理上述问题是目前SLAM问题中常见的手段。因为通过概率方法
传感器误差能有效地成为到SLAM模型中一部分。同时,由于传感器误差,使得机器人 位置的不确定性随时间累积,并且对于外部环境特种的观测也是相对机器人位置得到与 描述的,最终机器人位置的不确定性将在所创建的环境地图中体现出来。使用概率方法 可以有效地表达出机器人自身状态与其所创建的环境地图的这种相关性。 综上所述,通常SLAM问题所需要解决的就是得到一个有关机器人位姿及环境地图 的联合后验概率密度分布:

尸(噩I Zl:七,%:I)

(2.2.1)

其中,五为k时刻的系统联合状态向量;互为k时刻的观测值,记 Zl尘=【zl,Z2,…,乙r,表示到k时刻为止的观测序列;魄为k时刻的控制命令,记
U1:七=[Ul,甜:,…,‰】r,表示到k时刻为止控制命令序列。

系统联合状态向量五为
厂yv]

五=I=l


(2.2.22)

群= “

12

(2.2.3)

硕士论文

移动机器人同时定位与地图创建算法研究

厶 x:=M= 厶



(2.2.4)



其中,X:表示机器人的位置及姿态角;。硭表示环境地图,使用2.1.2节中的所描 述的点特征表示环境地图,厶=k咒r表示第f个特征,假设环境特征为静止状态。
2.2.1系统联合状态向量的动态模型 综合在2.1.5节和2.1.7节中,分别对机器人的运动模型及环境特征的动态模型的描 述可以得到最终的系统联合状态向量的动态模型。通常SLAM问题中的系统联合状态向

量鼍包含机器人的位姿及环境特征位置,综合式(2.1.3)、(2.1.11),得到对于系统联合状
态向量的动态模型:

五=阱胞一[纵掣帆]
其中,f(-)为联合状态的转移函数。

亿2渤

基于2.1节与2.2节中所探讨的各种相关模型,本文构建了一个简化的SLAM问题 仿真平台。在该仿真程序中,可以建立各种不同的仿真环境与机器人模型,用以探讨、 实现、研究与对比分析本文后续的相关算法的各种相关性能,如定位的精确度、运行时 间、对环境特征估计准确度等。这些模型的研究与仿真程序的实现为后续工作打好一个
坚实的基础。

2.3本章小结
本章描述了本文中所用到的与SLAM问题相关的各种模型,包括坐标系统模型、环 境地图模型、机器人位置模型、里程计或控制命令模型、机器人运动模型、传感器观测 模型、环境特征动态模型、噪声模型以及联合状态的动态模型等,并基于这些模型建立 了简化的仿真平台为后续章节中的相关SLAM算法的实现提供了必要的基础。

13

第三章基于卡尔曼滤波的SLAM研究

硕士论文

3基于卡尔曼滤波的SLAM研究
R.Smith,M.Self,and P.Cheesemant271等人最早提出有关SLAM算法,他们的方法

现今已被许多研究者采用,成为一种广泛的SLAM方法。他们是在对Ayache and Faugeras,
Crowley,Chatila and

Laumond的工作分析的基础之上,基于一个离散的状态空间框架,

引入概率地图的概念,通过线性化及高斯分布假设,‘采用扩展卡尔曼滤波(EKF)方法对 系统状态空间中的机器人位姿和环境特征位置(路标位置)进行同时估计。 卡尔曼滤波(KF)方法是一种基于概率学的方法,它基于传感器观测数据和一系列限 制性假设,如高斯噪声分布和高斯初始不确定分布,利用高斯分布来描述每一个时刻机 器人位姿的后验概率密度,是一种简单有效的线性递推估计算法。EKF方法运用一阶泰 勒级数展开实现对非线性系统状态方程和非线性观测方程的线性化,同时能利用KF方 法去递归地对系统状态空间进行估计。UT变换(Unscented Transform)是计算进行非线性 传递的随机向量概率的一种方法,它是基于如下的想法:对任意一种概率分布进行近似 处理比对一种任意的非线性方程或者非线性变换进行近似处理要容易的多。 本章首先介绍了KF方法与EKF方法,然后将EKF方法与2.2节中所描述的SLAM 问题模型综合起来得到EFK SLAM方法实现的具体操作流程。引入UT变换的概念并将 UT变换应用到EKF SLAM方法中,得出UKF SLAM的方法与实现过程。最后在不同 仿真环境下针对这两种方法进行了实验比较与分析。

3.1扩展卡尔曼滤波SLAM
3.1.1卡尔曼滤波 卡尔曼滤波是一种最优线性递归估计算法,KF利用线性的系统状态方程和观测方程 得到一个全局最优的状态估计,而不用考虑数据的准确度。 一个线性的离散时间系统可以用下列的动态方程和测量方程表示:
.1) %2,七-l+ x1.3(Lj??lJ %=五xk-l+ co, k

乙=巩xk+岐

(3.1.2)

其中,砟是k时刻的状态向量,气是系统的观测序列,最是系统状态转移矩阵,吼
是观测矩阵。畋是过程噪声向量,哝是观测噪声向量。同时假设q和‰均为O均值高
斯白噪声序列,即:

q~N(0,g)

(3.1.3)

14

硕士论文

移动机器人同时定位与地图创建算法研究

‰~N(0,R)

(3.1.4)

其中,Q为系统过程噪声%的协方差矩阵,R为系统观测噪声%的协方差矩
阵。

卡尔曼滤波是一种典型的贝叶斯滤波算法,在给定初始状态值、协方差矩阵和k时 刻观测值后就能根据式3.1.1与3.1.2递推计算k时刻的状态向量,有以下步骤: 1、运用前一时刻的状态估计袁一。和协方差矩阵和协方差矩阵最州来预测当前时刻的

状态估计或和协方差矩阵戽,即:
%2吒%一l 露=E毫一。xk
L (3.1.5) -)J

最=最最一。硭

(3.1.6)

2、根据预测的协方差矩阵最和观测噪声协方差矩阵Rk计算卡尔曼阈值ⅨahnaIl
gain)Kk:

疋=最硪(玩最磁+也)。1
差矩阵:

(3.1.7)

3、根据预测的状态估计磊和实际观测值名修正系统的状态估计并计算相应的协方

≈=五+≮(靠一巩xk)
最=(x-Kk也)最
3.1.2扩展卡尔曼滤波SLAM

(3.1.8) (3.1.9)

扩展卡尔曼滤波(EKE)对非线性的系统方程进行线性化,使KF方法可以对非线性 系统的系统状态向量进行滤波估计。扩展卡尔曼滤波(EKF)是一种最小均方差估计

似MsE)方法。对于状态向量五,若给定观测序列Zl√=【Zl,z2,…,乙】r,则其MMsE估
计为

Xt全研墨I五,】

(3.1.10)

在EKF方法中,假设所处理的变量均服从高斯分布,使用均值及方差对其进行表

示,即五一Ⅳ(舅七,卑),舅t,最分别为忌时刻的估计值及估计协方差 Xk全E【五l zl=J}】
(3.1.11) (3.1.12)

最=研(五一殳☆)(.瓦一殳t)r】

简便起见,过程噪声%及观测噪声M均假定为零均值,方差分别为级及R的高斯 白噪声,有:
1S

第三章基于卡尔曼滤波的SLAM研究

硕士论文

E[喙】=E[%】=O,Vk

(3.1.13)

Etv,q]-8,f『o 研M蟛】_岛R
并且,过程噪声与观测噪声相互独立,即 Ehw;】=o,Vi,,
EKF

(3.1。14)

(3.1.15)

SLAM算法具体的迭代计算过程如下:

(1)预测过程

假设在七一1时刻有系统状态向量估计舅eq及估计协方差丑-l, 将最一。写成分块矩阵
的形式有:

叫乏r乏]

@¨6,

舭m[f(Xk辆)】=l纵等¨l

(3.1.17)

羞札¨摇鼍] 耽=瓢岫,_l最J
16

B¨9,

硕士论文

移动机器人同时定位与地图创建算法研究

盼爹h,
既=鼍k‰,
进行一个线性化的处理过程。 (2)更新过程

(3.地。)

睨、睨分别表示Z(?)相对于x”及甜的Jacobian矩阵,在这里EKF

SLAM需要

有了最新的观测值后,依据得到的观测值对预测进行更新,系统状态向量的估计均

值免及估计协方差最分别为:
X七=X七^一l+%(乙-h(xt,蹦))
(3.1.21)
‘ 。

最=(,一K巩)最¨
其中

妻二主筌R
瓯=玩最扣,甄1+R

n地2,


巩=嚣k_)
中EKF SLAM同样需要进行线性化处理。 (3)向量增广

(3.1.23)

G忱。一417

≮为卡尔曼增益;瓯为新息方差;巩为A(?)相对于X的Jacobian矩阵,这一过程

利用机器人传感器获得的观测信息中,通常不但有己知环境特征的观测而且也有新 的环境特征的观测。对于已知环境特征的观测信息,使用预测值对其进行更新,而新的 环境特征则要加入到系统状态向量中同时对其进行初始化。 假定当前状态有
=f X—k:匕IjI /Xk

(3.1.24)

最=Ecc五一舅七,c五一殳七,r,=[乏r二兰]
最=E[(五一x_i})(五一x七)2】=l:’r n””I
对环境地图中新的环境特征%删初始化有

(3..) c3.1.25,

mn删=g(《,乏)
其中,函数g(?)使用观测模型推导得到。 将新的环境特征加入到系统状态向量中

(3.1.26)

17

第三章基于卡尔曼滤波的SLAM研究

硕士论文

X:

五=蹦=
厶 只=

X1::
肌n刚

(3.1.27)


己m

£,?%r
(V豉?P删)r
(3.1.98)

岛r

1i79x?Pw飞gx?PⅧ勺gx?PW闪g?+勺g:?R闪g?。
其中,

魄=面Og I(轨)

(3.1。29)

%=老k磊,

d厶LJII,血J

%、%分别为g(?)相对于X”及z I约Jacobian矩阵。
3.2 UKF

SLAM

UKF是一种用采样策略逼近非线性分布的方法,它以UT(Unscented Tranformation)t2伽变换为基础,采用卡尔曼线性滤波框架,具体采样形式为确定性采样, UKF的计算复杂度与EFK算法相同但性能优于EKF,不需要通过求导计算雅可比矩阵; 对任意的非线性模型能够逼近泰勒展开式而达到二阶精度,EKF的精度为一阶,对于特 殊的高斯分布模型,能够达到三阶精度。
3.2.1

UT变换 UT变换COmcented Transform)是计算进行非线性传递的随机向量概率的一种方法,

它是基于如下的想法:对任意一种概率分布进行近似处理比对一种任意的非线性方程或 者非线性变换进行近似处理要容易的多。 设x是像维的随机向量,g:吼b专吼%是一非线性函数z=g(X),考虑将x通过非线 性函数g传递,假定x的均值和协方差分别为x和£。为了计算关于z的统计量,我们 首先选择29/x+1个带有权值的样本点(也称sigma点)墨={形,石},使其能够完全获取 随机变量x的真实的均值和协方差。sigma点的选择以及权值的确定是根据以下方程:
Zo=i

(3.2.1)
f=1,…,心 f=以+1,...,2n。

石=孑+(√(刀,+见)只), 石=蟊一(√(他+旯)只)h。
"1

(3.2.2) (3.2.3)

硕士论文

移动机器人同时定位与地图创建算法研究

哪 ll

(3.2.4)

啦杀南一
五一一 一彳

(3.2.5)

W}m)-叩2瓦丽1 2(以,+名)


吼…,2%




(3.2.6)

其中兄=口2(nx+tc)-n,是一个尺度调节因子,a决定了我们选择的sigma点在其均值引村 近的分布情况,通常将a设置为一个很小的正值(如0.001)。K是次级尺度调节因子, 通常设置为0,∥是用来结合关于x的分布的先验知识(对于高斯分布,∥的最佳取值为

2)。(√(心+五)只),是矩阵(心+旯)只平方根的第f行,形表示第f个SIGMA点的权值,且

满足∑睁1。形‘m’是用来计算均值(mean)的权值,彬‘。’是用来计算协方差(covariance)
的权值,二者除在初始情况下不同外,在f=1,...,2刀,时都是一样的。 将每个sigma点通过非线性函数向前传递,
Zi


g(石)

扛O,..?,2,z,

(3.2.7)

通过计算可以得到z的均值和协方差的估计
2n。

三=∑彬‘”’互 』J‘


(3.2.8)


J*0

2n,

£=∑形‘。){互一->{互一习r
3.2.2

(3.2.9)

UK卫’SLAM
UKF是Julier和Uhlman[191提出的利用卡尔曼框架来进行非线性估计的方法。它通

过UT变换取代EKF中的非线性方程的近似过程,UKF通过选用一些采样点来代替随 机变量本身,然后对采样点进行非线性变换。基于变换后的采样点,计算随机变量的均 值和方差。首先对协方差矩阵进行Cholesky分解,得到2N+1个采样点,对每一个采样 点进行非线性动态变换,基于变换后的采样点进行观测预测并计算新息、信息方差和互 协方差。最后计算滤波器的增益从而得到系统状态的后验估计。
UKF

SLAM方法与EKF SLAM方法是相似的。其具体流程为:

(1)计算sigma点:从NxN维的协方差矩阵P得到2N+1个sigma点。这些sigma 点的均值和方差等于源随机变量的均值与方差。

磁々=‰j}
19

第三章基于卡尔曼滤波的SLAM研究

硕士论文

W。:土
珂+尼

Z悻=坼陆+(舾再瓦), 硒i+n=‰一(厮再厍),
W,:W,¨:

(3.2.10)


2(n+七)

其中(抓五i厍),可以通过对A进行Cholesky分解得到,nP=ArA,W‘为第i个
点的权重。n为增广系统状态空间的维数 (2)预测过程:运用非线性动态方程,对每一个采样点进行非线性变换,并计算变 换后采样点的均值与方差

幺m=厂(硝。,‰,以)


砟嘶I-∑W’幺+lI七
i=O

(3.2.11)

最+ll。:圭形r(幺+耻一%+耻)(幺+耻一%+耻)r
(3)观测更新:运用非线性观测方程,计算预测观测值:

乏嘶=办(幺俳)+以
J口

气嘶=∑W’乏m
i=O p

踹忙=∑矽‘(乏+lII—z七+lI七)(乏+耻-zk+l|七)r
t=O

(3.2.12)

碥I。:壹形,(幺嘶一坼嘶)(三‰七一乙m)r
(4)状态增广:使用卡尔曼增益对系统均值方差进行更新替换,得到后验的状态估 计,即对系统状态向量进行增广。

K=踹t(P荔)q
黾+llI+l=%+耻+j眨t+呐

(3.2.13)

最州“l=最+耻一KP荔Kr

硕士论文

移动机器人同时定位与地图创建算法研究

l,‘,w‘为sigma点集的噪声。也可通过从X,P,O和R构建一个增广点集来计算。 在UKF算法中要求一个增广点集来进行更新估计,可以通过对P进行Cholesky因 式分解得到,它的计算复杂度为O(n3)。

3.3实验与结果分析
本节将EKE SLAM方法和UKF SLAM方法应用于不同仿真环境进行仿真实验,对 这两个算法进行比较,验证与分析算法的性能。 3.3.1机器人运动模型与观测模型 采用2.1.5节中定义的基于控制命令的运动模型。机器人模型如图3.3.1所示,有控

制输入‰=哌吼】丁施加于机器人,预测机器人运动的状态转移方程为:
xl—l+AT?圪?cos(O;一l+%)

群=

2I珐l+AT?圪?siIl(①:一l+%)

吆+—ATo_Vk.广sin(一of_k)

+[ii]

(3.2.14)

其中,K为机器人前向速度,瓯为机器人前轮偏角,△丁为航迹推算传感器的采样 时间,L为两轴的轴距,K,W,%为噪声项,用于描述轮子打滑等未知特征的影响。

图3.3.1机器人模型

在本节进行的仿真试验中分别使用激光雷达和视觉传感器(摄像机)作为距离传感 器。 机器人利用自身配备的激光雷达传感器探测路标,得到路标的距离和方位角。其观 测模型为:

21

第三章基于卡尔曼滤波的SLAM研究

硕士论文

乙=办cj气,=[;]= ar咖爱叫
xi—xk

正j了瓦鬲万
+wk

(3.2.15)

路标到传感器距离为,.,方向夹角为0,(t,Yi)为探测到的第f个特征的位置坐标。 对于使用摄像机做观测用的距离传感器时,通过建立完整和精确的摄像机模型来处 理不同类型的摄像机取代使用简单针孔模型的摄像机模型。对摄像机的观测模型如下: U=Z?(x盔+口劫2)+cl
V=/2?杨2+C2 【n?2?16)

(u,v)表示图像坐标,石、五分别为焦距产度,主点为q和巳,口为畸变系数,劫。和 劫:使用下述公式计算:

轳(1蜥,2+和4)害

%:=(1+向?,.2+如?,.4)?萼
毛,如为摄像机径向畸变系数。r和(鼍,艺,Zc)的关系定义如下:

(3.2.17)

r2:墨堡
ol


其中:
,i

xn
xf3

(3.2.18)

‰嘞‰O

‘‘‘●

下。. 。, j



l=c#cqp,,i2=sOsOc缈一c矽J缈,r13:c臼J矽c伊+J臼J伊,眨l=c矽s缈,吃2=J9J矽s伊+印J伊 ,23=c口s≯s缈一spc缈,吩l=一s≯,吩2=sOc#,吩3=cpc缈

其中C-"COS,S=sin,秒,矽,9,为相对于X,Y,z轴的旋转角。(ol,■2,o,)为环境路

标位置,【‘l,Xr2,‘3】为机器人位置。当机器人在平面上运动是令‘:=o,0=0,妒=o。
该模型描述了一个高度非线性化的系统。当使用简单针孔模型时,令口,墨,也为零。 3.3.2实验结果分析 基于这样一种思路进行实验与分析:通过比较EKF SLAM和UKF SLAM方法在不 同仿真环境下,结合不同的传感器处理移动机器人同时定位与地图创建的效率,来分析

硕十论文

移动机器人同时定位与地图创建算法研究

EKF

SLAM方法与UKF SLAM方法的适用性、算法效率、以及处理定位问题的精确度、

对由点特征表示的环境特征估计等几个方面的性能。通过多组独立重复实验获取大量相

关数据,从统计学的角度出发来分析与研究EKF SLAM方法与UKF SLAM方法。主要 依据对仿真实验所得数据利用MSE(均方误差)方法来分析数据。通过设计三组不同 仿真环境下的实验,将EKF SLAM和UKF

SLAM方法同时应用到这些仿真环境中。首

先通过直观的给出小样本情况下的EKF SLAM方法性能和UKF SLAM方法得到初步 分析。接着各进行一百次独立重复实验,从中提出相关数据做统计分析,以期通过最大 程度上的仿真来反映将这两种方法引用到实际中会出现的状况。对在实验中出现的有关
重复试验误差数据的图片做一说明。这些图片反映相应的第n次实验时的相关误差信息,

图片的横轴为相应实验次数,纵轴为对应误差数据其单位为米。而有关单次实验误差数 据的图例中横轴坐标单位为秒,而纵坐标单位为米。 对于每个实验均采用如下步骤:先描述仿真环境的相关情况,以及所采用传感器的 类型与相关参数。接着给出其中一次的实验结果与相应误差分析数据图片给出直观的分 析。随后进行多次实验以统计更真实的情况。本文所进行的仿真实验都在基于第二章给 出的机器人相关模型上精确建模的,并在程序中实现了这些相关模型。 3.3.2.1仿真实验一 在本实验中仿真一个走廊环境,使用航位推测仪作为内部传感器,视觉传感器作为 外部传感器观测环境特征。走廊的长度为20米,宽度为2.5米,有22个视觉特征分布 在该环境中。航位推测仪的采样时间为0.025s,速度误差O"v=0.3m/s。角度误差为仃。=3。。 视觉传感器的误差为0.5像素。机器人以lm/s的速度绕该走廊环境运动,最大转动角度
30。。

使用EKF SLAM方法得到的实验结果如下:
● ’ ‘















口毋口





由一田









口 日





















'O



¨



'●

-n■_

图3.3.1

EKF

SLAM实验结果

图3.3.1中黑线所示部分为机器人的实际路径,蓝色线条为机器人的估计路径,黑 色方框为环境路标特征,红色为机器人对相应环境路标的估计。

第二三章皋于卡尔曼滤波的SLAM研究

硕十论文

图3.3.2机器人X方向误差

图3.3.3机器人Y方向误差

图3.3.2和图3.3.3为机器人某一次在环境中运动时,机器人的x和Y坐标的实际位
置与估计值之间的误差统计。

使用UKF SLAM方法得到的实验结果如下:

图3.3.4

UKF

SLAM实验结果

图3.3.4中黑线所示部分为机器人的实际路径,蓝色线条为机器人的估计路径,黑 色方框为环境路标特征,红色为机器人对相应环境路标的估计。

3.3.5机器人X方向误差

图3.3.6机器人Y方向误差

图3.3.5和图3.3.6为机器人某一次在环境中运动时,机器人X和Y坐标的实际位置 与估计值之间的误差统计。
在该环境中分别使用EKF SLAM方法与UKF SLAM方法进行1 00次独立重复试验,

对得到的机器人位置的X和Y坐标,实际位置和估计值问的均值和方差统计见表3-3.1
24

硕士论文

移动机器人同时定位与地图创建算法研究

与图3.3.7.3.3.14。图3.3.7.3.3.10为对EKF SLAM方法产生的统计结果,其余为UKF

SLAM方法所产生的统计数据。

图3.3.7 100次独立试验X方向误差均值

图3.3.8 100次独立试验Y方向误差均值

图3.3.9 100次独立试验想X方向误差方差

图3.3.10 100次独立试验y方向误差方差

图3.3.11 100次独立试验想X方向误差均值

图3.3.12 100次独立试验Y方向误差均值

图3.3.13 100次独立试验想X方向误差方差

图3.3.14 100次独立试验Y方向误差方差
25

第三章基于卡尔曼滤波的SLAM研究

硕i:论文

可以看出在走廊这种小环境中,使用视觉传感器进行观测并结合里程计信息时,
EKF

SLAM和UKF SLAM两种算法的效率相当,由于环境较小,对机器人的位置估计

都较为准确。同时可以看出虽然在该环境中视觉特征较少,但是使用视觉传感器仍然是 相当有效的。我们可以得出这样的结论在简单的结构化环境中,系统为弱非线性,EKF SLAM方法与UKF SLAM都是相当有效的,均有较强鲁棒性。但是这两种方法都需要 事先给出精确的系统噪声协方差和观测噪声协方差。
表3.3.1
EKF EKF UKF

SLAM性能对比表
UKF

均值(m)
X坐标误差 Y坐标误差 运行时间
.0.0007 0.0074 114s

方差(m)
0.00009 0.0033

均值(m)
.O.0103 0.009l 120s

方差(m)
0.0001 0.0052

3.3.2.2仿真实验二

本次实验仿真一个较大的,相对复杂的环境,环境长220米、宽160米,环境中分 布94个可以被感知的环境特征。使用航位推测仪作为内部传感器,激光雷达作为外部 传感器观测环境特征。航位推测仪的采样时间为0.025s,速度误差o-v=0.3m/s。角度误 差为%=3。。激光雷达采样时间0.27s,感知范围29m,观测噪声0.1m。机器人以2.5m/s 速度运行。
使用EKF SLAM方法得到的实验结果如下:

图3.3.11

EKF

SLAM实验结果

图3.3.11中黑线所示部分为机器人的实际路径,蓝色线条为机器人的估计路径,黑 色十字为环境路标特征,绿色十字为机器人对相应环境路标的估计。

硕十论文

移动机器人同时定位与地图创建算法研究

图3.3.12机器人x方向误差

图3.3.13机器人Y方向误差

图3.3.12和图3.3.13为机器人某一次在环境中运动时,机器人的X和Y坐标的实际 位置与估计值之间的误差统计。
使用UKF SLAM方法得到的实验结果如下:

图3.3.14

UKF

SLAM实验结果

图3.3.14中黑线所示部分为机器人的估计路径,绿色线条为机器人的实际路径,黑 色十字为环境路标特征,红色十字为机器人对相应环境路标的估计。 图3.3.15和图3.3.16为机器人某一次在环境中运动时,机器人的X和Y坐标的实际 位置与估计值之间的误差统计。

图3.3.11机器人X方向误差

图3.3.12机器人Y方向误差

27

第三章基于卡尔曼滤波的SLAM研究

硕士论文

图3.3.13

x方向误差均值图

图3.3.14 Y方向误差均值图

在该环境中分别使用EKF SLAM方法与UKF SLAM方法进行100次独立重复试验,

对机器人位置的x和Y坐标,实际位置和估计值间的均值和方差统计如下。

图3.3.15

x方向误差方差图

图3.3.16

Y方向误差方差图

图3.3.13.3.3.16为使用EKF SLAM方法在该仿真环境中进行100次独立重复试验

得到的关于机器人位置误差的均值和方差统计图。图3.3.17.3.3。20为使用UKF SLAM 方法在仿真环境中进行100次独立重复试验得到的关于机器人位置误差的均值和方差统
计图。

图3.3.17

x方向误差均值图

图3.3.18 Y方向误差均值图图

硕士论文

移动机器人同时定位与地图创建算法研究

图3.3.19

x方向误差方差图

图3.3.20

Y方向误差方差图

上述结果的统计见表3.3.2
表3.3.2
EKF EKF UKF

SLAM性能对比表
UKF

均值(m) X坐标误差 Y坐标误差 运行时间
.0.1613 -0.0035 205s

方差(m)
0.1966 0.1250

均值(m)
.0.0639 0.0397 350s

方差(m)
0.1844 0.1288

该仿真实验是对大规模环境进行模拟的,因此采用激光雷达进行环境特征的感知。
从上面对EKF SLAM与UKF

SLAM进行多次试验后的统计结果我们可以清晰地看出这

两种方法在大规模环境中也有着较好的效能,对机器人的位置估计的准确度也在可以接 受的范围内,从仿真中可以看出定位的误差均在正负一米左右,对于大范围环境来说这

SLAM对环境特征的估计和对机器人位置的估计略为精 确。但是其运行与处理速度较EKF SLAM的效果要差些,更为在仿真试验中,UKF SLAM
是可以接受的。总的来讲UKF 会随着被观测到特征的增加,由于需要进行Cholesky分解,对采样点进行处理,由于观 测到特征的维数激增,UKF SLAM花费了大量时间在这一部分的计算上。这也是UKF

SLAM方法无法避免的一大缺点。
3.3.2.3仿真实验三 本实验仿真一个较大的环境,环境长120米、宽100米,该环境中能被机器人感知 的特征比较稀疏。设计该实验用以考察EFK

SLAM方法和UKF SLAM方法在处理稀疏

环境特征路标中的效果。使用航位推测仪作为内部传感器,激光雷达作为外部传感器观 测环境特征。航位推测仪的采样时间为0.025s,速度误差吼=0.3m/s。角度误差为吒=3。。 激光雷达采样时间0.27s,感知范围29m,观测噪声0.1m。机器人以2.5m/s速度运行。
使用EKF

SLAM方法得到的实验结果如下:

第三章基于卡尔曼滤波的SLAM研究

硕十论文

图3.3.21

EKF

SLAM实验结果

图3.3.21中黑线所示部分为机器人的实际路径,蓝色线条为机器人的估计路径,黑 色方框为环境路标特征,红色为机器人对相应环境路标的估计。

图3.3.22机器人x方向误差

图3.3.23机器人Y方向误差

选取多次试验中一次的机器人的X和Y坐标的实际位置与估计值之间的误差统计展
示结果如图图3.3.22和图3.3.23。 使用UKF SLAM方法得到的实验结果如下:

图3.3.24

UKF

SLAM实验结果

●l'lllt.

图3.3.25机器人X方向误差

图3.3.26机器人Y方向误差

硕士论文

移动机器人同时定位与地图创建算法研究

图3.3.24中黑线所示部分为机器人的实际路径,绿色线条为机器人的估计路径,黑 色方框为环境路标特征,红色椭圆为机器人对相应环境路标的估计。 选取多次试验中一次的机器人的X和Y坐标的实际位置与估计值之间的误差统计展
示结果如图3.3.25和图3.3.26。

同样在该仿真场景下分别对EKF SLAM和UKF SLAM进行100次独立重复试验精 确统计这两种方法的处理精度与处理效率。 从实验数据统计中我们可以直观地看出,在环境特征稀疏的情况下UKF SLAM的 误差较大,而EKF SLAM对环境特征的依赖性方面的鲁棒性及在环境特征稀疏的情况 下处理能力也是相当出色的。 对于这个实验我们可以很清晰地看出EKF SLA方法在性能方面优于UKF SLAM方 法,EKF

SLAM方法在特征较稀疏时具有较好的鲁棒性。而UKF SLAM由于在进行UT

变换生成Sigma点时没有足够的样本使得其性能得不到保障。 图3.3.27.图3.3.30为对UKF SLAM进行独立重复试验后的误差数据统计。

图3.3.27机器人x方向误差均值

图3.3.28机器人Y方向误差均值

图3.3.29机器人x方向误差方差

图3.3.30机器人Y方向误差方差

第三章基于卡尔曼滤波的SLAM研究

硕士论文

图3.3.31机器人x方向误差均值

图3.3.32机器人Y方向误差均值

图3.3.33机器人x方向误差方差

图3.3.34机器人Y方向误差方差

图3.3.31.图3.3.34为对EKF SLAM进行独立重复试验后的误差数据统计。
表3.3.3
EKF EKF UKF

SLAM性能对比表
UKF

均值(m) X坐标误差 Y坐标误差
.0.0087 .0.0272 91S

方差(m)
0.0980 0.1417

均值(m)
0.1339 .0.1737 127S

方差(m)
1.1907 1.2274

运行时间

从上述三个仿真实验我们可以得出这样的结论,UKF SLAM方法在环境特征较多的 情况下有着更高的处理精确度,但是随着特征处理数量的增加UKF SLAM方法所消耗
的时间也更多。并且UKF SLAM方法在环境特征稀疏的场景中定位误差较大。而EKF

SLAM方法在对机器人位姿处理,环境特征估计以及算法处理速度与鲁棒性等方面比较 均衡。 从上面的实验及对其结果的数据特征统计与分析我们可以看出将这两个方法应用 于现实中类似的环境会有好的效果,如类似的室内环境,结构化道路,小范围室外环境 等。从相关理论可知这两个方法的算法复杂度都达到了O(n3),并且这两种方法都同时 包含了机器人与环境特征间的关联信息。同时从分析中可以看出EKF SLAM方法和
32

硕士论文

移动机器人同时定位与地图创建算法研究

UKF

SLAM所进行的滤波处理是一致与收敛的。同时这两个方法都对先验的系统噪声协

方差和观测噪声协方差有着密切关系。若没有确定的相关噪声信息会降低这两种方法的 精度,并会导致发散[28,29】.这对于真实中的移动机器人及其所处环境等非线性系统会严重
影响性能。

3.4本章小结
本章在研究了卡尔曼滤波、扩展卡尔曼滤波理论的基础之上,分析研究了将扩展卡 尔曼滤波应用到移动机器人同时定位与地图创建的具体过程与细节。并探讨了将对扩展 卡尔曼滤波进行UT变换后得到UKF方法应用到移动机器人同时定位与地图创建的具体 操作流程。最后将EKF

SLAM、UKF SLAM应用到不同的仿真环境中对这两种方法从统

计学角度对实验结果进行了分析与对比。EKF算法的优化程度与系统噪声协方差Q和观 测噪声协方差R所提供的先验信息有密切关系。不充分的噪声先验信息会降低状态估计 的精度。UKF是一种用采样策略逼近非线性分布的方法,UKF的计算复杂度与EFK算法 相同,但性能优于EKF不需要通过求导计算雅可比矩阵;对任意的非线性模型能够逼近 泰勒展开式而达N-阶精度,EKF的精度为一阶,对于特殊的高斯分布模型,能够达到 三阶精度。

第四章基于粒子滤波的SLAM研究

硕士论文

4基于粒子滤波的SLAM研究
对于未知环境中的同时定位与地图创建来说,不确定性无处不再。移动机器人对所 处环境一无所知,环境又存在着很大的动态性,机器人本身的内外部传感器等同样受噪
声影响。这样要精确地做到同时定位与地图创建是十分困难的。通常使用基于概率理论

的方法以期解决好移动机器人的同时定位与地图创建问题。基于概率理论的同时定位与 地图创建解决方案也面临着许多挑战。 流行的基于高斯分布假设的卡尔曼滤波系列方法在很多情况下都是一种高效的解 决方法。然而其不能用于解决多峰值概率分布的环境。对于多峰值概率分布的系统环境 通常需要引入基于能表示使用所有可能情况的概率的方法。马尔可夫方法是一种利用关 于所有可能位置的概率分布来表示机器人状态的后验概率密度,基于这种前提它能表示 复杂的多峰值概率分布。Dellaert[27,30]等基于马尔可夫方法提出利用蒙特卡洛采样法的粒 子滤波方法。它通过对状态空间进行采样,利用这些加权样本来近似系统状态的后验概 率密度来避免求解复杂的概率分布函数。但是普通的粒子滤波方法存在这需要大量样本 及样本退化问题。为了有效地解决这些问题Doucetl22,24,35]等人通过引入 Rao.Blackwellized方法提出Rao.Blackwellized粒子滤波算法。FastSLAMt31,32]是RBPF滤波 器的一个实例,它成功地将Rao.Blackwellized方法用于解决移动机器人在未知环境中的 同时定位与地图创建问题。FastSLAM将SLAM分解为对机器人的定位问题以及对机器人 所在环境地图的创建问题,通过一个粒子滤波器来实现机器人的定位问题,对于环境地 图的创建问题通过N个独立的EKF实现,一个路标对应于一个EKF,是一种混合粒子滤 波与EKF的混合方法。对于EKF可以使用基于卡尔曼滤波系列的其他方法进行替代而产

生不同的处理结果。本文尝试将UKF运用到FaStSL√蝴来代替EKF,形成UKF.FastSLAM。
4.1粒子滤波原理
粒子滤波算法,通常又将其称为顺序蒙特卡洛方法,它采用一组从概率密度函数上 随机抽取的进行了加权处理的样本来近似处理后验概率密度。粒子滤波算法摆脱了非线 性、非高斯问题的约束,可以用于任何通过状态空间模型可以描述的系统,它可以达到 逼近最优估计的精度。基于贝叶斯的滤波理论需要通过积分处理才能得到系统状态的后 验概率:这是一个非常复杂与困难的过程。粒子滤波是通过引入顺序重要性采样算法来 有效地逼近对系统状态的后验概率。事实表明这是一种非常有效的手段。 4.1.1粒子滤波理论 贝叶斯滤波通过初始状态的概率分布p(xo),观测数据序列互叫结合系统的状态方 程和观测方程进行递推计算来求解系统状态≮的后验概率密度p(也I互.。),从而得出系

硕士论文

移动机器人同时定位与地图创建算法研究

统状态的最优估计解。 对于初始状态的概率分布为p(Xo)的系统,结合一阶马尔可夫过程,由 Chapman-Kolmogorov方程可以计算出时刻系统状态磁的先验概率密度:
P(Xk I zl¨)=IP(Xk l砟一lp(≮-l l ZI:H)出t—I
(4.1.1)

当获得观测值z。后,可以根据贝叶斯公式更新先验概率密度p(xk l zl士q),从而得到 系统状态讫的后验概率密度

p(‰k):丝粤掣掣
PLzI l
zl孟一l J,

(4.1.2)

对于p(xk l

Z1:“)=fp<x。I xk一。p(毪一。I z1州)呶一。这一归一化常数是由似然函数

P(Zk I‰)与系统观测噪声的统计特向确定。

最优贝叶斯估计就是由式4.1.2所构成的递推关系得到。但是要通过式4.1.1进行积分 才能求解后验概率密度p(吒l五:¨)。这是一个非常困难的问题,尤其是对离散状态的非
线性、非高斯系统。

针对需要进行积分才能求解后验概率密度P(Xk I互州)的问题。粒子滤波算法通过引 进顺序重要性采样算法来解决这一问题,从而得到系统后验概率密度的有效近似。 顺序重要性采样算法是一种蒙特卡洛方法,这一算法利用蒙特卡洛模拟方法实现递 推的贝叶斯滤波器。它基于抽取系列随机样本的加权和来计算所需后验概率密度,计算 系统状态的估计值。当样本数足够大时能很好地表示后验概率密度。由概率理论可知, 当样本量趋于无穷大时,后验概率密度函数与蒙特卡洛特性是一致的,也就说基于顺序 重要性采样的粒子滤波其可以趋近最优贝叶斯估计。

用{《:七,以)墨。表示系统后验概率密度函数p(xo盘I钆)的粒子集合:,其中{矗:七,

i=l,...’N)是支持样本集,相应的权值为{以,f=l,...,N),且满足∑以-1,而
Xo虫=扛,,,=o,...,k>表示到时刻k系统所有状态的集合,所以时刻k的后验銎度可以近似
表示为:


P(Xo:七I

z,:七)≈∑以万(‰:七一《:七)
i=l

(4.1.3)

于是就有了一种表示真实后验密度p(xo:七I毛:七)的离散式带权值近似表示方法,而那 些关于数学期望的复杂计算(通常带有复杂的积分运算)就可以简化为和运算了,如:

E(g(xo:I))=I g(Xo:1)p(xo:I I气:七勋%:||}
可以近似为:

(4.1.4)

E(g(Xo:。))=∑以g(《:七)

(4.1-51)

35

第四章基于粒子滤波的SLAM研究

硕士论文

粒子权值就是根据重要采样技术来选择的。如果根据重要密度q(Xo:七l互:。)选择粒子, 那么粒子的权值可以定义为:

以oc畿等
递归的表示方法,可以将选择的重要密度函数因式分解为: q(Xo:★I毛:七)=鸟(五I而:“,气t)g(黾¨I zl:M)
(4.1.7)

Ⅲ,

在时刻k.1,如果已经得到k—I时刻后验密度p(《:纠l Z1州)的近似表示的粒子集合, 下一步就是用一个新的粒子集合来近似表示k时刻的后验密度p(《:。l毛:七)。为了得到一种

然后,通过将获得的新状态《~q(Xo:.k I‰4,钆)加入到已知的粒子集合

《:々q—q(Xo:。。l刁:川)中,得到新的粒子集合如一g(h七I毛:七)。根据贝叶斯规则,可以得
到权值更新方程如下:

p(‰I

z1.k):丝出唔单磐逍

:翌(垒l墨;±:三!;&=12旦!羔!叠生:!:刍生=12翌!叠;&=!!刍;盘=12

2 p Xo:k-I

结合式4.1.8得到权值更新方程如下:

以芘掣糍舞涨 啦?唰糌
以芘咄铡辫

h扣-)血P捌(Zk
P(Zk l Z1.t—1)

(4.m)
I毛:七一lJ

(4.I.9)

为了得到一种更为简单的形式,设其符合一阶马尔可夫过程则有
q(xk I而:¨,Z1:I)=g(诈I磁中乙),即重要性采样函数只与晚一。和气有关,这样就仅仅需要

记录当前时刻的样本集《一.即可,而可以不用记录过去的样本集《:纠与过去的观测值
zI虫-.,这样便大大降低了空间复杂度。根据一阶马尔可夫假设对权重修正如下:

@¨。,

4.1.2样本退化处理方法 粒子滤波算法的一个主要问题是退化问题,它指的是由于权重的方差随时间会有随
36

硕士论文

移动机器人同时定位与地图创建算法研究

机递增现象发生,即少数样本会占去总体样本的绝大部分权重,很有可能在经过多次迭 代后,出现单样本非零权重问题。这样使得样本集不能实现所要表达的后验概率密度, 同时大量计算均浪费在更新对求解p(xk l乙:七)毫无用处的样本上。

通常使用有效样本数%来度量样本的退化问题:
Ⅳ万2

1广—一
∑(畦)2



(4.1.11)

从上式可知有效样本数越小退化现象加剧。这样就需要采用大样本来缓解这一问 题。然而样本量加大,算法复杂度提高,算法效率会降低。通常通过重采样和选取重要 性采样函数来解决这一问题。 重采样通过过滤掉样本集中权重较低的样本,保留高权重样本,使权重集中在权重 大的样本上,来达到解决退化问题。一般根据权重对后验概率密度进行离散近似处理:
p(xk

k)≈∑以万(‰-4)

(4.1.12)

经过N次重采样,得到新的样本集{‰i歹瑚N,同时使p(《-4)=以,重新设置权重

以=专。
经过重采样处理,样本退化问题得到一定程度的解决,但是由于高权重样本被多次 选取,使多个样本被重复采样,使得样本不具有多样性,造成样本贫化。通常有最小方 差采样、多项式采样,残差采样等重采样方法等。本文使用残差采样算法进行重采样。 也可以根据重要性采样函数进行重采样来达到解决样本退化影响的目的。通常采用

先验概率密度p(磁I《一。)作为重要性采样函数,即 g(%l《-l'z七)=p(讫I《一1)
这样权重就变成如下形式:
(4.1.13)

以OC以一。p(z。I《)

(4.1.14)

粒子滤波其有着适用范围广,通用性高的特点,它能适应任意感知模型,动态模型 和噪声分布。它也是一种容易实现与应用的方法。但是当样本数量较少,并且没有包括 正确位置的样本时,粒子滤波的效果较差。同时还有因重采样造成的样本贫化和丢失样 本的多样性问题。
4.2 Fast

SLAM

Murphy[36】研究发现,如果知道机器人的确切路径,则路标位置的确定可以分解成N
37

第四章基于粒子滤波的SLAM研究

硕士论文

个独立的估计问题,每一个问题对应于一个路标,并由此提出一种有效的算法用于栅格
地图的学习。基于这一理论Montemerlo提出FastSLAM解决方案。
4.2.1

Rao.Blackwellised粒子滤波方法

Doucet[24,27,281等提出使用Rao—Blackwellised方法的粒子滤波算法。对于应用普通的
PF来解决SLAM问题时由于SLAM中系统状态是一个高维向量,普通PF算法效率太低, 只用在采用大样本的情况下算法性能才有保证。同时大样本却会造成算法计算复杂度的 急剧增大。Rao.Blackwellised粒子滤波方法能有效地解决这个问题。 基于RBPF的SLAM方法其基本原理是:根据观测信息zf={乙I f=1,2,3…,f)和里程计 运动信息U‘={U,l t=1,2,3,..j)估计机器人可能路径S‘={S1 S2,岛,...,Sk>的后验概率密度 p(s‘阿,Ut n‘),然后利用该概率密度计算地图与机器人路径联合的后验概率密度
p(st,m l p(st,m l
Zt Ut

n‘)。设其符合一阶马尔可夫模型,根据贝叶斯规则有:
Zt Ut

Zt

Ut,n‘)=p(m l St

n‘)p(,I Zt甜‘,∥)
(4.2.1)

=p(,阿,甜‘,∥)却(他∥,zf,矿,∥)
上工
g-I

由上面推导可知SLAM问题可以通过联合单独的机器人定位问题和基于位姿估计的 路标位置估计问题从而得到解决。对于机器人路径的后验概率密度p(s‘I
Zt

Ut∥)估计问

题RBPF采用粒子滤波器来处理。每个粒子样本与一个单独的环境地图关联。对于地图 的后验概率密度p(m I St Zt“‘,行‘)通过结合机器人路径,和传感器观测信息7通过计算
得到。

根据机器人的运动模型可以得到机器人的路径,这样就可以选用系统运动模型作为 粒子滤波器的重要性采样函数。 RBPF算法的具体步骤如下: 采样:根据前一时刻的样本集{《一,),根据重要性采样分布g(墨l Zt
Ut

n‘)采集样本

{吐,),用于描述当前时刻机器人的位姿。

重要性权值计算:根据4.2.2式计算每个样本的权重嵋,即:

以=嬲

㈣)

重新采样:根据权重矿对样本集进行重采样。通过重采样减少低权重样本数量,保 留权重高样本,以期解决样本退化问题。对于实际后验分布与重要性采样分布存在差异 时,通过重新采样可以使其应用与这种环境。 估计地图:通过p(舛Ⅳ”,Zt Ut∥)结合表征机器人位姿的每一个样本《得到相应的
地图估计m:

硕士论文

移动机器人同时定位与地图创建算法研究

算法流程图如图4.2.1所示:

图4.2.1
4.2.2 Fast

RBPF算法流程图

SLAM

FastSLAM是当今SLAM算法中最为成功的例子,已经在10万个路标的环境中测试成

功,FastSLAM将SLAM分解为对机器人的定位问题以及对机器人所在环境地图的创建问
题,通过一个粒子滤波器来实现机器人的定位问题,对于环境地图的创建问题通过N个 独立的EKF实现,一个环境路标特征对应于一个EKF,是一种混合粒子滤波与EKF的混 合方法。

FastSLAM是RBPF滤波器的一个实例,它是基于式4.2.3中的假设来近似处理后验概
率p(st,m I Zt∥,∥),其中,={sl。82,s3。。-5)的机器人轨迹。 p(St,m I z‘,“‘,∥)=p(,I

z’,∥,∥)却(%F,7,“t∥)
上上
t=l

^厂

(4…2

3)

这个假设在SLAM问题中是精确和普遍适用的。也就是说假设我们知道了机器人的
运动路径,具体环境中的每个路标可以相互独立地估计出来。但是在实际中我们对机器 人的运动路径是不知道的。虽然如此,基于这种关于环境路标可以相互独立地进行估计

的方法,可以将SLAM问题在式中的近似后验概率分解为两个独立的后验概率的乘积形
39

第四章基于粒子滤波的SLAM研究

硕士论文

式,一个是关于机器人路径的后验概率,一个是在给定路径的情况下N个路标的后验概
率。

FastSLAM使用粒子滤波器对路径进行采样。每个粒子的形式为 墨‘=∥”,∥il’f,∑‘u,...,∥‘Ⅳp∑‘肌>,其中i是粒子的序号,J“是该粒子所经过的路径估计, ∥‘Ⅳ,f,∑‘Ⅳ’r为该粒子在第N个路标时刻的均值和方差。FastSLAMfl习以下几个步骤组成: (1)采样新位姿。环境特征路标估计依赖机器人路径,而机器人整个路径是由机器 人从1至t时刻的所有位姿,构成,因此对粒子的采样步骤是十分重要的。新位姿<从后 验概率p(只I
St-l'i

gt,“‘,∥)采样,J卜1'‘是第i个粒子知道t-l时刻的路径,即采样步骤考虑

机器人的最近观测信息,这样就可以是对采样就有重要性的粒子避免被移除,即不会发 生粒子损耗问题。可以使用EFK产生该后验概率的高斯逼近这一有效手段来实现,即:
P(St I
St-l'l zt

9“t∥)=Ⅳ(墨,瓦,∑。)

(4.2.4)

N(st,夏,∑耳)表示墨符合均值为瓦,协方差为∑而的高斯分布。 对于第i个粒子的具体处理过程过程如下: 1.预测该粒子的机器人位姿状态 长。=厂(吐p%) 2.假设传感器观测到环境特征路标所。对传感器观测进行预测:
(4.2.5)

乏一。=g(豇。,吼) q=Vmg(st,,啊)I^。钆,如以。
Gm=Vsg(s,,%)I毛;%,缸山 3.结合来自传感器的真实观测值五更新状态
硪l 2‘一l+C二lq K-1(乙一Z州t) 疑l=“Ll+只!lG善KSt 1(乙一。) (4?2?6)

∑:=[掣KqG+(耵1]。1
K=qC!lqr+足 4.在新的位姿处采样并对移动机器人的路径进行扩展
(4.2.7)

《~p(墨I St-l'i甜t zt拧‘)=Ⅳ(墨;%,∑:) J‘。(St-l,l《)
上述公式中厂(?)为机器人的运动模型,符合N(st;f(st以,珥),B)分布,g(?)为 系统观测模型,符合N(zt;g(st,%),足)分布。T表示矩阵转置。
(4.2.8)

(2)对被观测的环境路标特征进行估计。对于环境路标特征m的估计由以,,∑:,I,表

硕士论文

移动机器人同时定位与地图创建算法研究

示,对环境特征路标的更新由传感器对其观测状态决定。路标未被传感器观测到时,即 m≠珥,则该环境特征路标的后验概率保持原来形式;当环境特征路标被观测到时,进
行如下更新:

p(, t。l?r ’f,:r ,z,f, z‘)= ?i!:!:! -1 ! z-:-! : !j; ; i; !i ;fj! i;:! ;I!:u! {!:-n掣(|4.2.8)
口l z.I S”.Z。。.




由于当前数据关联惕、墨、%对坼毫无任何作用,同时观测值只取决于珥、st、鸭。 根据这些对上式简化得: p(%I
St'i Zt

Ut,n‘)=rlp(z,l rn.,《,n‘)p(,珥I St-lJ,Zt-1
-N(磊;g(mn,彳,R)

Ut-1

n卜1)(4.2.9)

一Ⅳ(’珥;卢《,∑霉)

对概率分布p(竹I St-l:,gt-I'Ut-I以“)用均值和方差分别为∥,£《的高斯分布来表示。

p(乙1%,g,n‘)可以用高斯分布表示。r/为常量。
(3)重采样。重采样是FastSLAM中必不可少的环节,这是因为采样取得的粒子会和 实际的后验概率密度不匹配,这种不匹配是由于对不同的粒子归一化常量,7不同产生 的。归一化值通过对第i个粒子的概率测量值求逆得到的。rl‘=(p(刁1%,Sl't,Zt-I 将这种不匹配考虑在内,根据下式中重要性因数进行:
Ut

n‘))一。

叫OC p(五I St-l'i Zt-I"“‘,∥)





掣 掣堕~ 箸 尘 噤等



(4.2.10)

上式使用g对其线性化再一次逼近高斯分布。高斯分布的均值为乏,它的协方差为:

q只q。+嘭∑名。q’+足

(4.2.11)

上述过程为FastSLAM的处理过程。与EKF SLAM方法相比较而言,FastSLAM有着 以下的优势:较低的算法复杂度,通常为O(Nlog K),N为样本数量。在后验概率为非 高斯,多模型分布的情况下EKF SLAM方法通常会发生数据关联失败,而FastSLAM能 很好地处理这种状况,同时其处理数据关联时有着更强的鲁棒性。在FastSLAM方法总 是假定特征间的数据关联已知然而实际环境中特征存在着很大的不确定性,Montemerlo 提出基于最大相似度方法估计每个粒子的关联变量矽=argman. 的错误关联问题。
4.3

1妒,伟),从而不同

的粒子将对应不同的值,并且可能对应不同的路标,从而有效解,P决(Z了t传统EKF方法对应

UKF.FastSL埘
以UKF滤波器取代EKF滤波器对FastSLAM进行改进。改进的FastSLAM同样使用粒

子滤波器估计机器人路径的后验概率p(s‘p,Ut 9n‘),但是使用UKF滤波器来对N个环境
4l

第四章基于粒子滤波的SLAM研究

硕士论文

特征路标的概率分布p(m,I

St

Zt

Ut,n‘),f_1,2,3,...N。每个粒子的形式为

彰=爷t,i,“,,∑:∥…,以p∑0,>,其中i是粒子的序号,s^‘是该粒子的路径估计,是该粒
子第N个环境特征路标时刻的均值和方差。 改进后的UKF.FastSLAM在对新位姿进行采样估计和重采样过程据与FastSLAM使 用相同的处理方法。只是在对被观测到的环境特征路标时使用UT变换逼近观测模型 g(耽,《),其具体过程如下: 1.采用对称采样策略的UT变换生成Sigma采样点


(4.3.2) (4-3?3)

2.使用观测模型计算观测预测值的均值和方差 瓮+肚=办(Z+雌)+嵋

气+壮=∑形‘乏+l忙
t=0

%I。=∑肜‘(乏椰-zk一(‰七--Zk—r
3.对路标的后验概率更新:

IK=%lt(P2)q {≮椰+l=Xk+llI+瓯+ll七 l最+qI+l=丑+耻一艘露Kr

通过上述过程使得UKF应用到FastSL蝴中,对UKF进行了一定的改进。UKF从理
路上来讲其估计精度要高于EKF,这样对环境特征路标与实际环境特征路标的误差会 FastSLAM更小。这点将在后续试验中加以验证。

4.4实验与结果分析
本节将FastSLAM和UKF.FastSLAM方法应用于不同的环境进行,对其进行仿真实 验,对这两个算法进行比较,验证算法的性能。 4.4.1机器人运动模型与观测模型 机器人的运动模型p(s‘I 人运动的状态转移方程为
Zt

U‘)用于描述随时序变化的机器人位姿变化。使用2.1.5

节定义的基于控制命令的运动模型。有控制输入‰_[圪%r施加于机器人,预测机器

42

硕士论文

移动机器人同时定位与地图创建算法研究

x:=

《l+AT?圪?cos(①:一l+吒) =l珐l+AT?圪?sill(蛾一1+%)

屹+—AT.丁Vk

osin(ak)

+圈
+wk

其中,K为机器人前向速度,哝为机器人前轮偏角,△r为航迹推算传感器的采样 时间,L为两轴的轴距,屹,0,%为噪声项,用于描述轮子打滑等未知特征的影响。 机器人利用自身配备的距离传感器探测路标,得到路标的距离和方位角。其观测模
型为:

Zk=h(耻阡 arc伽舞曲:
薯一≮

瓜i万瓦历了

路标到传感器距离为,,方向夹角为0,(而,咒)为探测到的第if特征的位置坐标。
结合路标的运动模型p(%I St Zt“‘)来描述路标在全局坐标系中的变化。通常忽略环境 路标特征的动态性,其运动模型如下:

I五(r)=xj(t-1)
Ixj(t)=薯O一1) 式中(薯(f),咒(f))为t时刻路标i位于全局坐标系的位置。
4.4.2实验结果分析 4.4.2.1仿真实验一

本实验仿真一个中等环境场景,环境长100米、宽120米。使用航位推测仪作为内 部传感器,激光雷达作为外部传感器观测环境特征。航位推测仪的采样时间为0.025s, 速度误差吼=0.3m/s。角度误差为吒=3。。激光雷达采样时间0.27s,感知范围20m,感 知角度180。。观测噪声0.2m、8。。分别使用5个,10个及20个粒子进行实验比较FastSLAM 与UKF.FastSLAM的性能。针对上述情景分别进行10次独立重复试验。 针对每一种情况各给出其中一个场景来直观的反映两种算法在这些情况下的效果, 随后给出对这10次实验的一个平均误差情况。以表格的形式给出其统计信息。在这些
数据基础上对这两种方法深入分析

43

::::::二{韶享睾≤#:::二}::::

i、
÷……?÷…‘

}/广k{
。; iⅣ?}


i..7—7’’ 咚i:、1j
!L/ r

!//?—》≮j:、==S
;∥t—i
t∥



{-

…:f…争7专…一-

:::蔓毒兰主二二参二二
}∥一{

}。I r I

;‘;

i’ -’.
?



4.

……;¨’,’攀k 形 =斧式■
‘i、彳?i
i…

…≥…毒害拳雾
图4.4.2 lO个粒子

.i

Z}



l/;+;、7?;
图4.4.3 20个粒子

图4.4.1 5个粒子
'6

|-……Common
1 4



Enor

co㈣Y E,mI
-●●●●●●

.≯ 1.2 .-£~

^…~




蛳7

.扩1



,:


….勘 k

Oa








鬈”
…04 02 0 m2

飞 珏‰ 削1 f……
图4.4.4

≯ ,一善Z.

: …j


●●-●●

∥.



J. ̄‰ w一

铲 。^




矗●

5个粒子的x方向误差

图4.4.5

5个粒子的y方向误差

|__…一Common

' I
’赢

Y i!nmr

.玉

…:!i|
fI

孵 …嚣”…
,:

-I燧强 、 7‘滞1



礴一


毪。l

甄。; ….q』…….
【{ l
锇{。,i

jl….
;{

F鬻


Error

爵j二:≥疆:F享乏二翳:-j
?'卜…~÷……j……÷…~÷…?飞一!i=÷…:~j~,R;年舛…一÷…一

?r

三隧珏霹
图4.4.6 10个粒子的x方向误差

图4.4.7 lO个粒子的y方向误差

I……-c*I_m Y



i、i

l川



,签

‘f气



澎~


≤效‘:
o圮: l

冀f.


、 、

一嗨。






舢rⅢ寸…r—r一;丫r'…r一矿‘r.1


.1.£主I:兰主:蔓煎鹾I二-jII
E■q一
?

N.奠 户 9一’

.12卜…~;……÷……÷…一÷……;……÷.:?…事…一÷……÷…一-1

图4.4.8 20个粒子的x方向误差

图4.4.9 20个粒子的y方向误差

_-—●_●__-_____-___●_--__-________________-_--_——_——_———●—_————●_———’—__●●●-—_—_-—_———————————————————————————————————一一 一.

硕十论文

整垫翌!矍△旦壁壅垡皇丝塑型堡簦鲨婴壅

图4.4.1.4.4.9分别为使用5个粒子、lO个粒子、20个粒子的UKF—FastSLAM在仿 真环境中的结果,以及X,Y方向的误差。对这三种情况分别做10次独立实验后的统计
数据如下表:
表4.4.1 UKF.SLAM针对不同粒子数效果统计

粒子数

Mean(x)

Mean(y)

var(x)

Var(y)

图4.4.10

5个粒子

图4.4.11

10个粒子

图4.4.12

20个粒子

25



l……?Common×Error


I.……Ccrrnm.,nY
'.5

Eriot

1 5 ,




渺鼍


t√’












哮 I
Vt




1 一 {











馨n5


05





k。型‰. .j姆
时旧/秒

龟冉
-O.5 。




、i



-05



W邕愚一






对蔷k



’∞

1∞

图4.4.13

5个粒子的X方向误差

图4.4.14

5个粒子的Y方向误差

45

第四章基于粒子滤波的SLAM研究

硕士论文

图4.4.15 10个粒子的X方向误差

图4.4.16 10个粒子的Y方向误差

图4?4?17 20个粒子的x方向误差

图4.4.18 20个粒子的y方向误差

图4.4.10.4.4.18分别为使用5个粒子、10个粒子、20个粒子的FastSLAM在仿真环 境中的结果,以及X,Y方向的误差。对这三种情况分别做10次独立实验后的统计数据 如下表:
表4.4.2FastSLAM针对不同粒子数效果统计

粒子数

Mean(x)

Mean(y)

var(x)

var(y)

5 lO 20

0.8969 0.2339 1.0214

0.1006 0.1201

0.5435 O.1808 0.5099

0.7540 0.3979 O.1124

m.1028

图4.1.1_4.1.3为分别使用5个、10个、以及20个粒子利用UKF.FastSLAM的效果 图,图中蓝色的点为环境中特征路标,红色点为相应的粒子对环境路标的估计。红色线 条为机器人的实际路径,黑线为粒子对机器人的平均估计路径。图4.1.10-4.1.12为分别 使用5个、10个、以及20个粒子利用FastSLAM的效果图。黑线为机器人的实际路径, 红线为粒子对机器人的平均估计路径,图中绿色点为环境特征即路标。红色点为对环境
46

硕士论文

移动机器人同时定位与地图创建算法研究

能够特征的估计。虽然UKF.FastSLAM使用的是和FastSLAM一致的采样原理,但是对 系统环境特征进行了UT变换,并使用UKF对特征进行更新。从图中可以看出由于UKF 对状态估计进行了精确的转换,比FastSLAM只是简单的对非线性进行线性近似效果要 好。从统计数据来看误差的均值和方差都有所下降。但是所产生的效果还是有限的。同 时由于要进行UT变换,UKF.SLAM所需的计算量也有所增加。从上面对相关误差的统 计数来看,这两种方法的效率是相当的。都能很好地对环境路标特征和相应机器人的位 姿做出精确的估计。在大规模场景中还是比较精确的手段与方法。

4.5本章小结
本章首先研究了粒子滤波的相关理论与问题。紧接着研究了基于Rao.Blackwellised 粒子的FastSLAM方法。使用UKF方法替代FastSLAM中的EKF方法得到 UKF.FastSLAM方法。最后对FastSLAM方法与UKF.FastSLAM方法进行了仿真实验对 比,分析了这两种SLAM算法的相关性能与优缺点。

47

第五章总结与展望

硕士论文

5总结与展望
为了做到真正意义上的移动机器人自主导航,必须将对移动机器人的定位问题与环 境地图创建问题合二为一;进行移动机器人的同时定位与地图创建。近年来已经有多种 方法成功地应用到对移动机器人的同时定位与地图创建。本文对移动机器人同时定位与 地图创建的相关方法进行了详细的研究。
本文主要做了以下工作:

本文首先对机器人相关模型进行了深入的研究,并结合SLAM问题的一般模型构建 仿真平台,本文的仿真实验就是在这一平台上进行的。本文对EKF SLAM方法和UKF SLAM方法进行了深入的研究,并对EKF SLAM移动机器人同时定位与地图创建方法与
UKF

SLAM移动机器人同时定位与地图创建方法的进行了仿真实验。EKF算法的优化程

度与系统噪声协方差Q和观测噪声协方差R所提供的先验信息有密切关系。不充分的噪 声先验信息会降低状态估计的精度。UKF是一种用采样策略逼近非线性分布的方法, UKF的计算复杂度与EFK算法相同,但性能优于EKF;不需要通过求导计算雅可比矩阵; 对任意的非线性模型能够逼近泰勒展开式而达N--阶精度,EKF的精度为一阶,对于特 殊的高斯分布模型,能够达到三阶精度。从对比的实验结果看,UKF要好于EKF。所以 本文通过用UKF代替EKF作为建议分布,结合FastSLAM的方法框架,得到UKF FastSLAM方法。同样,本文在所构建的仿真环境中详细分析与对比了FastSLAM与UKF FastSLAM方法,实验结果表明,UKF FastSLAM是一种更加有效的机器人同时定位与地 图创建方法。
本文的UKF FastSLAM方法还是有缺陷的,时间复杂度还是比较高。本文的移动机

器人的同时定位于地图创建方法进行研究时都是假设基于静态环境的,在大规模,动态, 高度非线性环境中的移动机器人同时定位与地图创建问题,尤其是3D的移动机器人同时 定位于地图创建有待进一步研究。 移动机器人同时定位与地图创建的相关方法将仍是一个研究热点,需要不断的深入 研究。新的方法和新的思路将会得到更好的效果。

硕士论文

移动机器人同时定位与地图创建算法研究





似水流年,匆匆而过。值此研究生学习阶段行将结束之际,回首两年的学习时光, 感慨万千,感念种种,谨借此机会对这段时间中指导与帮助过我的所有人致以最真诚的 致意与感谢。 首先向我的导师赵春霞教授致以最诚挚的敬意与感谢。在两年研究生学习阶段,赵 老师为我们创造了良好的学习环境与学术氛围;赵老师总是在百忙之中抽出时间对我们 悉心指导。本文真是在赵老师的悉心指导与帮助之下得以完成。期间,赵老师始终给予 我严格的要求,充分的信任,热情的鼓励和诸多全面锻炼的机会,并在治学态度、工程 项目和专业知识方面给予我悉心的指导和谆谆教诲;这一切将使我受益终身。 其次,感谢这两年学习时光中给我授课的老师和给予过我帮助的老师。在他们的悉 心教导与严格要求下,这两年的时光,我在专业方面有了很大提高,研究生生活过的充 实、有收获。在此感谢叶有培、任明武、娄震、张二华、孙怀江、肖亮、严悍、朱耀琴、 朱保平等老师的墩墩教导;感谢姚德主任、张美荣老师在生活上的关心和帮助。 再次,感谢在论文撰写期间给予我无私帮助的南京理工大学603教研室的所有同学 与朋友。感谢正在攻读博士学位的郭剑辉博士、王超博士、殷俊博士以及已经毕业的张 浩峰博士在我的论文工作中给予我的大力支持与帮助;感谢教研室的马伟、赵笑可、孙 奥、陆子辉等硕士研究生。 然后,深深地感谢我的父母与家人。漫漫求学之旅,正是他们的支持与鼓励我才能 坚强地一路走来。谨以此文献给我敬爱的父母。 最后,再次向所有给过我关心和帮助的师长、同学和朋友致以最诚挚的敬意与感谢。

49

参考文献

硕士论文

参考文献
[1】Leonard J.J,Durrant—Whyte
IEEE Transaction
on

H.E

Mobile robot

localization by tracking geometric beacons.

Robotics and

Automation.1991.7(3):376-382.
on

[2】Durrant-Whyte
Robot

H.E讹ere

am I?A tutorial

mobile vehicle localization.Industrial

1994.21(2):11~16.
mobile automation:An application of artificial intelligence techniques.
on

[3】Nilsson N.J.A

Proceedings of the 1 st Int.Joint Conference 509.520.

Artificial

Intelligence,Washin舒on

D.C.1 969

[4】Smith&Self M,Cheesman

P.A stochastic map for uncertain spatial relationships in

robotics.Autonomous Mobile Robots:Perception,Mapping and Navigation,1 987 1:323-330.

[5】Smith RSelf M,Cheesman P.Estimating Autonomous
Robot Vehicles.1 990 l:1 67.1 93.

uncertain spatial

relationships in robotics.

[6】Dissanayake G5lDurrant—Whyte

H.F,Bailey T.A computationaUy efficient solution to the
on

simultaneous localization and map building(SLAM)problem.In ICRA'2000 workshop

Mobile

Robot Navigation

and Mapping.2000.
J.The

【7】Castellanos J.A,Motiel J.M.M,Neira

Spmap:A probabilistic framework for
on

simultaneous localization and map
1 999.1 5(5):948?953.

building.IEEE Transaction

Robotics and Automation.

[8】Arulampalam
2002.50(2):174—188.

S,Maskell

S,Gordon

N.A tutotial

on

patticle
on

filters Signal

for

on.1ine

non-linear/non-Gaussian Bayesian tracking.IEEE

Transactions

Processing.

[9】Leonard J.J,Durrant-Whyte H.F.Dynamic map

building for

an

autonomous mobile

robot.International Journal of robotics Research.1 992.1 1(4):286—298.

[1 0】Hu
IEEE

S,Hu D.D,Gu

O,Landmark-based
on

navigation of mobile robots

in

manufacturing.
Automation.

International

Conference

Emerging

Technologies&Factory

Spain:1999:18-21.

[1 1】Myung-Jin

Jung,Hyun light 2D

Myung,Sun?Gi

Hong,Dong—Ryeol Park,Hyoung?Ki

Lee,SeokWon
building
on

Bang,Structured

range

finder

for simultaneous lacalization

and map

(SLAM)in home environment.Proceedings of the 2004 Intemational Symposium
Oct.-3 Nov.2004:371.376.

31

[12】Guivant
sensors

J,Nebot E,Balker S.Autonomous navigation and map building

using laser

range

in outdoor applications.Journal ofRobotic Systems.2000.17(10):565-583.
P,Dissanayake

[1 3】Williams S.B,Newman
50

C毛Durrant-Whyte H.F.Autonomous

underwater

硕士论文

移动机器人同时定位与地图创建算法研究

simultaneous localization and map building.Robotics and Automation,2000 Proceedings. ICRA'00.IEEE International Conference
on

Volume 2.24-28 April 2000:1793—1798.
lacalization and

【1 4】Sebastian thrun,Yufeng Liu,Daphne Koller,Andrew Y.Ng,,Simultaneous

mapping with spare extended information filters.The international Journal of Robotics

research.2004.23(7—8):693-716. [1 5】Moravec
H,Elfes A.High resoulution maps

from谢de angle

sonar.In Proceeding of the

IEEE Intetnational

Conference

on

Robotics

and Automation(ICRA'85).1 985:1 1 6—2 1 1.
and navigation

[16】Elfes A.Occupancy
PhD thesis.Carnegie

grids:A probabilistic framework for robot perception 989。

Mellon University.1

【1 7】Chong K,Kleeman L.Mobile
accurate

Robot

map

building from

advanced

sonar

array

and

odometry.Iternational

Journal ofRobotics

Research.1999.18(1):20.36.
unscented particle

【1 8】Merwe

R,Doucet A.,Freitas Nando

de,Wan Eric.The

filter.Cambridge
of the

University,engineering

department:Technical

report,CUED/F-INFENG/TR 3 80,2000.

[1 9】Julier 【20】Wan

S J,UhhTlann J K.Unscented filtering

and nonlinear estimation.Proceedings

IEEE,2004,92(3):401--422.
Eric

A,Merwe

R.The

unscented
on

kalman

filter for nonlinear

estimation.In:
Processing,

Proceedings

of International

Symposium

Adaptive

Systems for

Signal

Communications

and Control,Alberta,Canada,2000,153-158. G.Bayesian
filtering for location

【2 1】Fox

D,Hightower J.,Liao L.,Schulz D.,Bordello

estimation.IEEE Pervasive Computing,2003,2(3):24-33.

[22】Gustafsson
tracking.IEEE

E,Gunnarsson F.,et a1.Particle filters for positioning,navigation
on

and

Transactions


Signal Processing,2002,50(2):425.--437.
localization.Montreal,Quebec,
Report:

【23】Rekleitis

M.A

particle filter tutorial for mobile robot

Canada:Centre

for

Intelligent

Machines,McGill

University,Technical

TM-CIM-04-02,2004. 【24】Doucet
A.On sequential

simulation-based

methods for

bayesian

filtering.University of

Cambridge:Technical
on-line

Report,CUED/F—INFENG/TR.3 1 0,1 998.
T..A tutorial
on

【25】Arulampalam M.S.,Maskell S.,Gordon N.,Clapp

particle filters for Signal Processing,
?

nonlinear/non-gaussian

bayesian tracking.IEEE Transactions

on

2002,50(2):174-1 88.
【26】Gordon

N…J

Salmond

D…J

Smith A.F.M..Novel approach Proceedings
on

to

nonlinear/non-gaussian
Processing,1 993.

bayesian

state

estimation.IEEE

Radar

and Signal

140(2):107-113. 【27】Smith R,Self M,Cheeseman P.Estimating uncertainspatial
relationships in robotics.In
51

参考文献

硕士论文

Autonomous Robot Vehnicles,Springer,1 990.

[28】Mehra

R K.On the identification of variances and adaptive Kalman filtering。IEEE

Transactions onAutomatic

Contr01.1970.ac一15(2):175—184.
on

【29】Fizgerald

R.J.Divergence of the Kalrnan filter.IEEE Transcation

Automatic Contr01.

1971,Ac-16(6):165-176. 【30]Dellaert F,Burgard W,Fox
D,Thrun

S.Monte

carlo localization for mobile robots.

Artificial Intelligence.2001,128(1-2):99—141

[3 1】Montermerlo M,Thrun S.Simultaneous
association using

localization and

mapping晰tll

unknown data
on

FastSLAM.Proceeding

of the IEEE International Confeence

Robotics

and Automatin,Taipei,China,2003:1 985—1 99 1.

[32】Montermerlo M,Thnm

s,Koller D,et

a1.FastSLAM 2.0:an improved

particle filtering

algorithm for simultaneous localization and mapping that provably converges.Proceedings of
the 1 8m

International

Joint

Conference

on.Artificial

Intelligence,Acapulco,Mexico,

2003:1 151.1156.

【33】Fox D.Markov

localization:A probabilistic framework for mobile robot localization ofBonn,1998. Fusing Noisy Visual

and

navigation.Germany:University

【34】Ayache and Faugeras.Building,Registating,and
Robotics research,1 988,7:45-65.

Maps.Journal

of

[3 5】Roumeliotis



I.Bekey



A.Bayesian estimation

and kalman filtering:A unified

framework for mobile robot localization.InProceedings of IEEE Intemational Conference On
Robotics

and Automation.San Francisco.Califomia.2000:2985.2992.
S.Rao-blackwellized
particle filtering for

【36】Murphy K,Russell
networks.In Sequential

dynamic

byesian

Montecarlo Methods in and

Practice,Springer,200 1.

【37】Newman

P.On the structure

solution for the

simultaneous localization and

map

building problem.PhD thesis,U11iV.of Sydney,2000.

【3 8】Dissanayake

GNewmanm,Clark

S,Durrant-Whyte

H F,and Csoba

M.An experimental

and theretical investigation into

simultaneous

localization

and map building(SLAM).Lecture

Notes in Control and Information Sciences:Experimental Robotics VI,Springer 2000.

【39】Dissanayake G5-Newman

P,Clark

S,Durrant-Whyte

H F

and

Csorba

M.A

solution to the

simultaneous localization and map
and Automation,200 1.

building(SLAM)problem.IEEE Transactions of Robotics

52

移动机器人同时定位与地图创建算法研究
作者: 学位授予单位: 邓翔 南京理工大学

本文链接:http://d.g.wanfangdata.com.cn/Thesis_Y1367066.aspx


相关文档

移动机器人的同时定位与地图创建的算法研究
移动机器人同时定位与地图创建自适应算法研究
移动机器人同时定位与地图创建研究进展
移动机器人的同时定位和地图创建方法
基于SIFT算法的移动机器人同时定位与地图创建
移动机器人的同步自定位与地图创建研究进展
移动机器人即时定位与地图创建问题研究
基于 SIFT 算法的移动机器人同时定位与地图创建
仪的移动机器人同时定位和地图创建
移动机器人定位、地图创建及路径规划研究
电脑版