（通信与信息系统专业论文）自主式水下机器人的同时定位与地图构建算法及实现.pdf

；， 4 ； 自主式水下机器人的同时定位与地图构建 算法及实现 学位论文完成日期：盈鲤二竺兰! 指导教师签字：红! 喀 答辩委员会成员签字： lt)，*。；： 、“9 独创声明 本人声明所呈交的学位论文是本人在导师指导下进行的研究工作及取得的 研究成果。据我所知，除了文中特别加以标注和致谢的地方外，论文中不包含 其他人已经发表或撰写过的研究成果，也不包含未获得 一【注! 垫邀查墓丝盏要挂型童明丝! 奎拦互窒) 或其他教育机构的学位或证书 使用过的材料。与我一同工作的同志对本研究所做的任何贡献均已在论文中作 了明确的说明并表示谢意。 学位论文作者签名：任寿云签字日期：别。年j 月刁日 学位论文版权使用授权书 本学位论文作者完全了解学校有关保留、使用学位论文的规定，有权保留 并向国家有关部门或机构( 如中国科学技术信息研究所等) 送交论文的复印件和 磁盘，允许论文被查阅和借阅。本人授权学校可以将学位论文的全部或部分内 容编入有关数据库进行检索，可以采用影印、缩印或扫描等复制手段保存、汇 编学位论文。( 保密的学位论文在解密后适用本授权书) 学位论文作者签名： 侄春云 签字日飙幻阳年r 月1 日 学位论文作者毕业后去向： 工作单位： 通讯地址： 导师签字： 沥皮 签字日期：加，口年芗月可日 电话： 邮编： 自主式水下机器人的同时定位与地图构建算法及实现 摘要 自主式水下机器人( a u v ) 在海洋探测、资源开发中得到了广泛的应用， 是当今机器人研究领域的热点之一。在自主移动机器人相关研究中，导航技术是 研究的核心，高精度的导航和精确的定位信息对机器人安全航行和高效完成任务 具有决定性作用。同时定位与地图构建算法( s 圳) 不需要先验的地图信息， 依靠机器人自身携带的传感器感知环境信息，实现自身定位，同时构建环境地图。 s l a m 在水下导航定位算法中极具发展空间和潜力，也已成为近二十年来自主移 动机器人领域中的研究热点。 本文主要有两个目的：一是利用c r a n g e r a u v 提供的传感器数据，通过 扩展卡尔曼滤波s l a m 算法( e k f s l a m ) 实现机器人的自主定位和环境构图； 二是针对s l a m 算法中声纳更新比较耗时，难以满足a u v 导航系统的实时性要 求，故在s l a m 算法引入了多线程技术使其满足实时性要求。 本文研究的内容主要包含以下几个部分：首先介绍了国内外a u v 的发展现 状，s l a m 算法的研究现状和多线程的概念。然后介绍了c r a n g e r a u v 的系 统模型及e k f s l a m 算法在c r a n g e r 平台上的执行过程，并用m a t l a b 平 台对此算法进行了仿真，结果表明此算法的定位精度高于推位，并且一致收敛， 可以满足a u v 导航的要求。接下来介绍了c r a n g e r 平台的软件体系结构， 包括平台所包含的模块、模块的作用以及模块间的接口等。第四章是本文的重点， 首先对s l a m 算法中的耗时原因进行了分析，然后提出了将声纳数据更新与速 度角度等传感器数据更新并行执行的方案，使其满足系统的实时性要求，并应用 多线程技术进行了实现。最后对全文进行了总结并提出了需改善的地方，作为以 后工作的参考。 关键词：自主式水下机器人；扩展卡尔曼滤波；同时定位与地图构建；多线程； 实时性 i i a n a l g o r i t h ma n di m p l e m e n t a t i o no f s i m u l t a n e o u s l o c a l i z a t i o na n dm a p p i n gf o ra u v a b s t r a c t a u t o n o m o u su n d e r w a t e rv e h i c l e ( a u v ) ，w h i c hi s w i d e l yu s e di n m a r i n e e x p l o r a t i o na n dr e s o u r c ed e v e l o p m e n t ，i saf o c u si nt h er o b o t i cr e s e a r c hf i e l dt o d a y a n dn a v i g a t i o ni st h ec o r eo ft h ea u t o n o m o u sm o b i l er o b o t r e l a t e dt e c h n o l o g y n a v i g a t i o na n dl o c a l i z a t i o ni n f o r m a t i o n 埘m1 1 i g hp r e c i s i o np l a y sad e c i s i v er o l ei n c o m p l e t i n gt h em i s s i o ns a f e l ya n de f f e c t i v e l y s i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g ( s l a m ) a l g o r i t h mr e l i e so ns e l f - c o n t a i n e ds e n s o r s t oe x t r a c tt h eu s e f u li n f o r m a t i o nt o l o c a t ei t s e l fw i t h o u tt h eh e l po fp r i o rm a pi n f o r m a t i o n , a n da tt h es a m et i m e ，c o n s t r u c t t h ef e a t u r em a po fu n d e r w a t e re n v i r o n m e n t s l a ma l g o r i t h mh a sg r e a tp o t e n t i a li n u n d e r w a t e rn a v i g a t i o n ，s oi th a sb e e naf o c u si nt h ef i e l do fa u t o n o m o u sm o b i l er o b o t r e s e a r c hf o rt h ep a s tt w od e c a d e s t h i sp a p e rh a st w om a i np u r p o s e s f i r s t l y ，i tu s e se k f s l a mt oi m p l e m e n t a u v sl o c a l i z a t i o na n db u i l de n v i r o n m e n tm a p sa c c o r d i n gt ot h es e n s o r sd a t af r o m t h ec - r a n g e ra u v s e c o n d l y ，i tu s e sm u l t i t h r e a d i n gt e c h n o l o g yt om e e tt h e r e a l - t i m e r e q u i r e m e n t f o rt h es l a m a l g o r i t h m w h o s e i m p l e m e n t a t i o n i s t i m e - c o n s u m i n g t h i sp a p e rm a i n l yc o n t a i n st h ef o l l o w i n gs e c t i o n s t h ef i r s ts e c t i o ni n t r o d u c e st h e d e v e l o p m e n ta n d r e s e a r c hc o n d i t i o no fa u va n ds l a ma l g o r i t h m ，a n d m u l t i t h r e a d i n gi sp r o p o s e da t t h es a m et i m e t h e n , t h i sp a p e re x p l a i n st h es y s t e m m o d e la n dt h es l a me x e c u t i o np r o c e s so nt h ec - r a n g e rp l a t f o r m t h er e s u l to f s i m u l a t i o ne x p e r i m e n t sd o n eb ym a t l a bd e m o n s t r a t e st h a t t h i sm e t h o di s c o n v e r g e n ta n dt h el o c a l i z a t i o na c c u r a c yi sh i g h e rt h a nd e a dr e c k o n i n g o v e r a l l ，i tc a n m e e tt h er e q u i r e m e n t so fa u vn a v i g a t i o n t h et l l i r ds e c t i o ni n t r o d u c e st h es o , w a r e a r c h i t e c t u r eo ft h ec - r a n g e rp l a t f o r m t h ef o u r t hs e c t i o ni st h ek e yp o i n to ft h i s p a p e r , w h i c hd e s c r i b e sh o w t ou s em u l t i t h r e a d i n gt os a t i s f yt h er e a l t i m er e q u i r e m e n t i i i o fs l a ms y s t e m f i r s t l yt h i ss e c t i o na n a l y z e sr e a s o n sw h ys l a ma l g o r i t h mi ss o t i m e - c o n s u m i n g ，a n dt h eu p d a t i n gp a r t so fs o n a rs e n s o ra n di n e r t i a ls e n s o r sa r ep u t i n o n et h r e a d r e s p e c t i v e l y s ot 1 1 a tt h e yc a nl u l li np a r a l l e lt om e e tt h er e a l t i m e r e q u i r e m e n t t h el a s ts e c t i o ns u m m a r i z e st h ep a p e ra n dp r o p o s e st h ei m p r o v e m e n ta s ar e f e r e n c ef o rf u t u r ew o r k k e y w o r d s ：a u v ；e x t e n d e dk a l m a nf i l t e r ；s l a m ；m u l t i - t h r e a d i n g ；r e a l - t i m e i v 目录 摘要i a b s t i 认c t i i i 1 绪论1 1 1 国内外a u v 研究现状2 1 1 1 国外研究概况。2 1 1 2 国内研究概况3 1 2 同时定位与地图构建算法( s l a m ) 综述：3 1 2 1 基于卡尔曼滤波的s l a m 算法4 1 2 2 基于粒子滤波的s l a m 算法5 1 2 3 基于概率的s l a m 算法6 1 3 多线程与实时性。7 1 3 1 多线程介绍7 1 3 2 多线程实现实时性8 1 4 本文主要工作及论文的组织结构9 2e k f - s l a b t 算法及其仿真1 1 2 1 系统模型l l 2 1 1c r a n g e ra u v 数学模型。1 1 2 1 2 坐标系统模型1 2 2 1 3a u v 运动模型：1 2 2 1 4 传感器观测模型1 3 2 1 5 环境特征模型1 3 2 1 6 噪声模型1 4 2 2c r a n g e r 平台上的e k f s l a m 算法1 4 2 2 1 坐标系1 5 2 2 2 系统状态向量l5 2 2 3 预测l7 2 2 4 速度角度等传感器数据更新1 8 2 2 5 路标更新1 9 2 2 6 状态扩充21 2 3 数据关联2 2 2 蔓l 门限过滤2 3 2 3 2 最邻近数据关联2 4 2 4 仿真结果2 4 2 5 本章小结2 7 3a i j v 软件系统结构2 9 v 3 1a u v 系统整体介绍2 9 3 2 子系统间的通信接口3 2 3 2 1s o c k e t 接口3 2 3 2 2 命名管道接口3 3 3 3 3 消息队列接口3 4 3 3 本章小结3 5 4 基于v c + + 的a i ns l a m 多线程实现3 6 4 1w i n o o w s 多线程介绍3 6 4 2s l a m 算法结构3 8 4 3s l a m 算法耗时分析及解决方案4 0 4 3 1 耗时分析4 l 4 3 2 解决方案4 2 4 4 多线程实现4 3 4 4 1s i ，a m 预测更新线程4 4 4 4 2 声纳数据关联更新扩充线程4 5 4 4 3 数据预处理线程和声纳点特征处理线程。4 6 4 4 4 线程同步问题4 7 4 4 5 实验结果及分析4 9 4 5 小结5 0 5 总结与展望5 l 参考文献5 3 致谢5 6 个人简历5 7 攻读硕士学位期间发表的论文5 8 v i 自主式水下机器人的同时定位与地图构建算法及实现 1 绪论 二十一世纪将是海洋的世纪，海洋覆盖了地球表面百分之七十以上的面积， 蕴藏着丰富的生物和矿物资源，是最现实、最有发展潜力的空间，开发海洋成为 人类在二十一世纪面临的重大课题。但是海洋探索面临着巨大的困难，海底能见 度极低，环境非常恶劣，人体和普通设备都很难在这种条件下完成工作。此时， 水下机器人特别是智能水下机器人显示出了它们的优势和作用。水下机器人的发 展经历了载人潜水器、有缆遥控水下机器人( r e m o t e l yo p e r a t e dv e h i c l e s ，简称 r o v ) 和自主式水下运载体( 又称为无缆潜水器a u t o n o m o u su n d e r w a t e rv e h i c l e s ， 简称a u v ) 三个阶段【l j 。其中a u v 具有水下活动范围大、机动性能好、安全、 结构简单等优点【2 】，同时a u v 的研究代表了水下机器人的发展方向，已经广泛 引起各国海洋开发界的重视，成为海洋领域研究的热点和前沿。然而受相关技术 发展水平的限制，目前a u v 还不像r o v 那样成熟。其中，制约a u v 发展的最 关键的一项技术为自主导航。 在自主移动机器人相关技术的研究中，导航技术是其研究核心。导航是指移动 机器人通过传感器感知环境和自身状态，实现在有障碍物的环境中面向目标的自 主运动【3 】。目前而言，可供水下航行器使用的导航方式主要有非自主导航( 如 g p s ) 和自主导航( 如惯性导航) 两大类。非自主导航方式如g p s 仅在接收机 能收到导航信号时才能使用，但由于电磁波在水中衰减迅速的原因，这种方式在 a u v 上的使用受到限制。在自主导航方式中，惯性导航方式是最主要的导航方 式之一，具有其它导航方法无可比拟的隐蔽性、自主性和全天候的优点，但是惯 性导航方式的主要问题是随着航行时间的增长，误差会不断累积。 在导航定位技术中，a u v 的定位和地图的创建是关键，但是a u v 不能利用 全局系统定位，并且环境地图往往未知，这时s l a m 方法就显示了它的独特优 势。同时定位与构图算法( s l a m ) 4 1 允许载体在未知环境中运动，利用传感器 提取环境中的特征信息实现自身定位，同时增量的构建环境地图。该算法依靠自 身携带的传感器获得环境信息以及自身的速度、加速度、姿态等，且不需先验地 图的辅助【5 】f 6 】，在水下导航定位算法中极具发展空间和潜力，也已成为近二十年 来自主移动机器人领域中的研究热点。本文主要分析c r a n g e ra u v 上的 中国海洋大学硕士学位论文 e k f s l a m 算法，以及如何用多线程实现e k f s l a m 来满足系统的实时性要求。 1 1 国内外a u v 研究现状 目前，各海洋大国的许多大学和研究机构都投入了大量的人力和资金，用于 研究和开发自主式水下机器人。其中，美国、加拿大、英国、法国、德国、意大 利、俄罗斯、日本等国处于领先地位【7 8 1 。我国虽然起步较晚，但也取得了很大 的成就。上世纪9 0 年代开始，国内已有许多研究机构和团体部门从事s l a m 的 研究，已经取得了较大的成果。 1 1 1 国外研究概况 早在二十世纪9 0 年代末，真实的传感器数据就已经开始用来执行s l a m 构 架。1 9 9 7 年9 月，美国海军水下作战中心利用一艘改装的海军货船装载完备的一， 传感器系统包括常规声纳、高精度可视声纳、侧扫声纳、多普勒测速仪( d v l ) 以及差分g p s 等来开展实验，该实验获取的数据集主要用来测试两种不同的 s ia m 方法。 澳大利亚悉尼大学菲尔德机器人研究中心很长时间以来一直在从事s l a m 问题的研究【l 】，自行开发的名为“o b e r o n 的a u v 采用了两个低频地形辅助扫 描声纳、彩色照相机、深度传感器以及光纤陀螺等仪器。通过提取声纳图像获得 点特征，然后利用这些点特征构建环境地图，与此同时估计a u v 的位置。 英国的h e d o t - w a t t 大学提出了在执行海底勘测任务时使用由d v l 、罗盘和 测扫声纳组成的系统来感知环境，并执行s l a m 算法来进行自身的定位和地图 的构建，结果表明s l a m 算法可以作为g p s 的替代方法。 麻省理工学院( m i t ) 在2 0 0 0 年也进行了利用真实海洋声纳数据的s l a m 算法的测试。它们利用在n a r r a g a n s e t t 海湾由美国高分辨阵列( h r a ) 前视成像 声纳获得的真实数据，从声纳图像中提取点状特征，然后作为s l a m 算法的输 入，以探测海床上的物体及其位置，同时对潜器进行定位。实验以差分g p s ( d g p s ) 的定位结果为标准，比较了由s l a m 离线处理和n s 多普勒计程仪 ( d v l ) 获得的结果，证明了s l a m 算法误差增长率比i n s d v l 要慢得多，具 有在水下航行器上成功应用的潜力，可以在浅水域探测和跟踪明显的特征。 除此之外，日本、韩国、英国等也从事这方面的研究，取得了定的成就。 2 自主式水下机器人的同时定位与地图构建算法及实现 如韩国国立大学的“s n u u v i ”、日本的“t w i n b u r g e r l 2 、“p t e r o a 2 5 0 ”和 w o o d sh o l e 的“a b e ”和“r e m u s ”【3 】等都比较著名。 1 1 2 国内研究概况 国内对水下机器人的研究起步比较晚，但也取得了很大的成就。如哈尔滨工 程大学研制的“智水系列，中科院沈阳自动化研究所等单位于1 9 9 5 年共同研 制的“探索者 号c r - 0 16 0 0 0 米的深海无缆自主式潜器，2 0 0 8 年3 月研制的6 0 0 0 米自治水下机器人c r - 0 2 ( 图1 1 ) 等，这些机器人的研制成功标志着我国研制 的水下机器人在智能控制技术上已经接近了世界先进水平。但总的来说，目前我 国智能水下机器人( a u v ) 尚处于研究、试用阶段，还有一些关键技术问题需要解 决，和世界先进水平相比还存在着较大的差距。但是随着人们对水下机器人技术 研究越来越重视，以及我国对水下机器人特别是智能水下机器人的需求，国内的 中科院沈阳自动化研究所、哈尔滨工程大学、上海交通大学等多家高校和研究机 构均已加大了人力和资源的投入，加强了在水下机器人技术特别是核心导航算法 的研究力度。 图1 - 1c r - 0 2 水下机器人 1 2 同时定位与地图构建算法( s l a m ) 综述 同时定位与地图构建( s l a m ) 问题是移动机器人导航领域研究的焦点， s l a m 问题的解决将成为实现真正全自主移动机器人的关键9 , 1 0 1 。它可以描述为： s l a m 允许机器人在完全未知环境中，利用自身携带的外部传感器感知环境，提 中国海洋大学硕士学位论文 取环境中的有用信息，利用这些信息进行自身定位，与此同时，增量的构建环境 地图的过程。s l a m 算法于上个世纪9 0 年代提出，与机器人位姿确定的地图构 建问题和地图已知的定位问题相比，未知环境地图构建中的定位和地图构建是一 个鸡与蛋的问题。其复杂性主要来自于测量数据中存在误差，这些误差相互关联、 相互影响并且随着时间累积。s l a m 算法主要解决导航问题中面临的三个主要问 题1 8 】【1 9 1 ：“在哪里? ( w h e r e 锄i ) 、“去哪里? ( w h e r ek i nig o i n g ) 、 “怎么去? ( h o ws h o u l dig o i n gt h e r e ) 。 s l a m 主要分为两大问题，即定位和地图构建的问题。定位是确定机器人 在其作业环境中所处位置的过程。更具体的说是利用先验环境地图信息、机器人 位姿的当前估计以及传感器的观测值等输入信息，经过一定处理和变换，产生更 加准确的对机器人当前位姿的估计。 地图构建是指构建机器人所在环境的地图的过程。一个自主移动的机器人必 须能够用它的传感器进行测量，然后利用这些测量信号确定它与环境的关系【1 3 】。 利用对环境的感知信息来对现实世界进行建模，自动地构建环境地图。 现有的s l a m 方法主要有基于卡尔曼滤波、基于粒子滤波和基于概率三种， 其他的一些s l a m 算法主要为这三种的扩展。 1 2 1 基于卡尔曼滤波的s l a m 算法 s m i t h 和c h e e s m a n 最早提出将e k f 用来解决s l a m 问题【2 5 】，目前大部分的 s l a m 算法，都是在e k f s l 。蝴【2 嘶捌的基础上发展的。e k f 适合解决非线性 系统的估计问题，主要包括预测和更新两个部分，首先将载体姿态和地图特征存 储在一个独立的状态向量中，然后通过先预测再观测的迭代递推过程来估计系统 状态，实现对载体的定位和特征地图的创建。预测阶段实际上执行的是通常意义 下的推位，造成载体状态估计误差较大。观测阶段获得特征值，如果第一次观测 到该特征，则将其添加到状态向量中。如果通过数据关联判定该特征己存储在状 态向量中，则更新a u v 姿态和地图中已有特征的值。 由于e k f 在地图中包含了机器人和环境特征状态之间的协方差矩阵，每次 估计和校正都要处理该矩阵，计算复杂度达到o ( n 3 ) ( 以是环境特征的个数) ，严 重制约了该方法应用于大尺度未知环境。近年来许多人致力于压缩e k f s l a m 4 自主式水下机器人的同时定位与地图构建算法及实现 的计算规模，如子地图法，把全局地图分解成许多子地图，在子地图中应用 e k f s l 川，降低了协方差更新过程中的计算复杂度，最后再把子地图组合起来 组成全局的特征地图。此外还有稀疏扩展信息滤波器法( s e l f ) 、细关联树法 ( t j t f ) 、协方差交集等方法【9 】。 口在应用数据关联时，如果来自目标环境特征观测量的概率太低，可能就 会被认为是一个新的环境特征，而且如果这个数据关联是错误的，那么这个错误 的观测量的影响可能永远不会消除。现在已经发展了一些健壮的数据关联方法来 处理有噪声环境中的不确定性，如局部地图序列法( l o c a lm a ps e q u e n c i n g ) ， 它最早由t a r d o s 等人提出，当机器人在短距离移动时，此算法采集大量的声纳 数据。这些数据经过霍夫变换处理，提取特征用于构建机器人所处的局部环境的 地图，最后子地图拼接在一起构建全局地图。这个算法使得数据关联变得更加有 效，因为一个正确的路标信息是由机器人在不同位置采集的多个声纳数据决定 的。其他较好的数据关联方法还有j c b b 、多假设跟踪等【9 】。 够。 1 2 2 基于粒子滤波的s l a m 算法 卡尔曼滤波器和扩展卡尔曼滤波器都是使用参数化高斯模型来描述概率分 布，而粒子滤波器用有限的状态集合即粒子集来描述分布。高概率的区域粒子密。： 度大，低概率的区域粒子数量很少或者基本没有粒子。在样本数目无限制的情况 下，真正的分布可以确切的重现。粒子滤波器需要离散加权采样数据来模拟后验 概率，并通过状态预测、权值更新和重采样等步骤来实现滤波。粒子滤波不拘泥 于系统的线性假设和传感器高斯噪声假设，可以有效处理非线性、非高斯的系统， 它已被成功的应用到各种实际估计问题【9 】中。 在机器人领域最常见的粒子定位方法是蒙特卡罗定位，即m c l l 9 , 2 1 1 。在m c l 中，粒子集合用来描述机器人可能的位姿分布，用采样的方法近似难以处理的积 分，因此尤其适用于对高维积分的处理。例如在f a s t s l a m 中可以将地图和机 器人的后验概率分解成比较简单项的乘积，并且假定在机器人运动路径信息已知 的情况下，一个环境特征的观测量不会提供其他环境特征观测量的信息，那么后 验概率就可以表示为 p ( x k , 1 9 l g k , u k , n k ) = p ( x k “，刀) 兀p ( 9l x k z ku kg l k ) ( 1 一1 ) 5 中国海洋大学硕士学位论文 从上式可以看出，通过分解，计算的复杂度大大降低了。 虽然粒子滤波器有很多优点，但是粒子滤波器都是建立在假设分布和后验概 率分布可以基本匹配的条件下。如果机器人运动时有大量噪声并且传感器非常精 确，那么它们就很难匹配，这时粒子滤波器就会退化。为了克服以上问题，很多 研究者对常规m c l 做了修改，提出了许多方法，最具代表性的是f a s t s “蝴 2 0 、m i x t u r e - m c l 方法和自适应采样的粒子滤波方法 9 , 2 3 1 。 1 2 3 基于概率的s l a m 算法 尽管不如e k f 那样流行，但由于用概率表达机器人定位问题的不确定性非 常自然合理，基于概率的s l a m t 2 8 1 也吸引了很多人的目光。同样是基于概率， 也有多种地图表示及构图方法，以下简要介绍最大似然估计( m a x i m u m l i k e l i h o o de s t i m a t i o n ，m l e ) 方法。 b a u m w r e l c h ( 或口一) 算法【2 7 】是最为有效的最大似然估计算法。基于这种 方法的构图与定位问题可看作是机器人与环境特征位置的最大似然估计问题。该 算法包括两步：e s t e p ( e x p e c t a t i o n ) 和m s t e p ( m a x i m i z a t i o n ) 。 机器人运动模型和感知环境模型为： 尸( 孝) = f p ( 孝i “，善妒( 孝) d 孝 ( 1 5 ) p ( o i 孝。，所) ( 1 6 ) 机器人运动的不确定性通过条件概率p ( 引甜，d 表示，其中甜表示控制命令 ( 与e k f 相同) ，善表示机器人当前状态。观测的不确定性通过条件概率p ( ol 孝，m ) 表示，其中r a 表示环境地图。p ( o ，m ) 决定了机器人在孝。状态下所观测到的环 境特征与地图的相似度。地图采用概率格来表示，即将固定范围的观测区域划分 为蜂窝状的格子，每个格子的概率值表示环境特征的占据情况，例如：o 表示无 环境特征，o 5 表示无观测值，l 表示有环境特征。定位过程首先利用运动模型 在假定当前地图确定情况下，计算出在控制甜作用下机器人可能到达的新位置孝。 ( e s t e p ) ，然后计算机器人在此位置形成的环境地图中每个特征同以往地图相比 出现的频率( 作为最大相似性估计器) 。经过计算，准确存在的环境特征在地图中 6 自主式水下机器人的同时定位与地图构建算法及实现 的概率值增加了，而不存在的环境特征在地图中的概率值降低t ( m s t e p ) 。 基于概率方法的s l a m 由于采用0 - - - 1 的概率值表示环境特征出现的频率， 并充分利用过去若干时间步的数据，因而对观测值具有鲁棒性：观测的环境特征 不必非常清晰，甚至可以是错误的。与e k f 相比，该算法克服了对数据关联的 准确性的依赖，提高了算法的收敛性。但该方法处理的数据量过大，对运算速度 和存储需求都非常高，制约了其在大规模环境下的应用。 近几年，基于概率的s l a m 研究着力于改善算法的实现方法，降低时间和 空间复杂度，使之可应用于实际环境中。文献【4 3 】应用m l e 解决机器人团队中 的定位问题，尽管文中表明定位精度不高，但没有深入分析是由于算法原因还是 选择机器人( 动态) 本身作为路标或其它原因引起的。文献【2 8 】介绍一个实际的博 物馆导游机器人系统m i n e r v a 。作者详细介绍系统结构以及所面对的问题，并再 次强调定位是机器人生存的基本要求。系统采用m l e 进行同时定位与建立地图 ( s l a m ) ，在博物馆真实环境中运行1 4 天，总计移动超过4 4 公里。 1 3 多线程与实时性 1 3 1 多线程介绍 线程【1 2 , 1 3 , 1 4 1 是一组指令的集合，或者是程序的特殊段，是操作系统分配c p u 时间的基本实体，是程序运行的最小单位。线程由一个堆栈、c p u 寄存器的状 态和系统调用列表中的一个入口组成。每个进程可以包含一个以上的线程，这些 线程可以同时独立地执行进程地址空间中的代码，共享进程中的所有资源。每一 个进程至少有一个主线程，它无需由用户去主动创建，是由系统自动创建的。用 户根据需要在应用程序中创建其它线程，多个线程并发地运行于同一个进程中。 一个进程中的所有线程都在该进程的虚拟地址空间中，共同使用这些虚拟地址空 间、全局变量和系统资源，所以线程间的通讯非常方便，多线程技术的应用也较 为广泛。 使用线程的好处有以下几点： 1 ) 多线程和进程相比，它是一种非常”节俭”的多任务操作方式。启动一个 新的进程必须分配给它独立的地址空间，建立众多的数据表来维护它的代码段、 7 中国海洋大学硕士学位论文 堆栈段和数据段，这是一种”昂贵”的多任务工作方式。而运行于一个进程中的多 个线程，它们彼此之间使用相同的地址空间，共享大部分数据，启动一个线程所 花费的空间远远小于启动一个进程所花费的空间，而且，线程间彼此切换所需的 时间也远远小于进程间切换所需要的时间。 2 ) 线程问有着方便的通信机制。对不同进程来说，它们具有独立的数据空 间，要进行数据的传递只能通过通信的方式进行，这种方式不仅费时，而且很不 方便。线程则不然，由于同一进程下的线程之间共享数据空间，所以一个线程的 数据可以直接为其它线程所用，这不仅快捷，而且方便。当然，数据的共享也带 来其他一些问题，有的变量不能同时被两个线程所修改，有的子程序中声明为 s t a t i c 的数据更有可能给多线程程序带来灾难性的打击，这些正是编写多线程程 序时最需要注意的地方。 3 ) 提高应用程序响应。这对图形界面的程序尤其有意义，当一个操作耗时 很长时，整个系统都会等待这个操作，此时程序不会响应键盘、鼠标、菜单的操 作，而使用多线程技术，将耗时长的操作( t i m ec o n s u m i n g ) 如用户输入、文件 读写和网络收发数据等置于一个新的线程中，避免了某项操作一直占用 c p u 。 4 ) 使多c p u 系统更加有效。操作系统会保证当线程数目不大于c p u 数目 时，不同的线程运行于不同的c p u 上。 5 ) 改善程序结构。一个既长又复杂的进程可以考虑分为多个线程，成为几 个独立或半独立的运行部分，这样的程序会利于理解和修改。 但我们应注意多线程是为了同步完成多项任务，为了提高资源使用效 率来提高系统的效率和实时性要求，除非在多c p u 或多核机器上，多线程 不能缩短整个系统的响应时间。 1 3 2 多线程实现实时性： 实时性可以简单的理解为在规定的时间之内完成一定得工作，可以分为在通 用计算机系统上的实时性和嵌入式实时系统上的实时性。一般的嵌入式实时系统 都是在操作系统和应用软件两部分来设计实时性。而通用操作系统如w i n d o w s 、 l i n u x 等都是弱实时操作系统，提高某个程序的响应时间必须对程序的结构加以 8 自主式水下机器入的同时定位与地图构建算法及实现 优化，如采用操作系统多任务多线程机制改善程序的结构。我们使用多个线程同 时处理多个任务，满足系统的并行处理要求。同时还可以设定每个线程的优先级， 当高优先级的线程执行时，即实时性要求高的任务需要执行时，可以自动地终止 其他线程的工作，转而执行这一线程或者是为高优先级的线程分配较多的时间片 等【1 4 1 。通过这一方法，可以在应用层上来满足一般的实时性要求。如果多线程程 序运行在多c p u 或者是多核机器上可以实现真正的并行处理，那么运行的速度 会相应的提高。 本文主要针对的是在通用操作系统上提高程序的实时性，用多线程运行在多 核机器上来满足s l a m 算法的实时性要求，避免由于某一部分消耗时间过多， 而导致其他部分无法执行。 1 4 本文主要工作及论文的组织结构 本文基于c r a n g e r a u v 提供的传感器数据，构建了合适的系统模型，在 e k f s l a m 基础上加入了传感器数据更新的环节，每次获得传感器的测量数据 后，利用e k f 算法进行更新，获得速度和姿态角的最优估计，然后再利用声纳 数据继续更新和扩充地图，并用系统状态向量的最优估计预测a u v 下一时刻的 位置。 同时本文介绍了整个a u v 系统的软件体系结构和s l a m 算法在整个系统中 的作用，并利用多线程实现了上述s l a m 算法将声纳数据的更新与构图部 分放在一个单独的线程处理，将速度、角度等传感器数据更新与声纳数据更新扩 充看成是两个相对独立的部分，缓解了s l a m 算法中由于声纳更新和扩充花费 时间过多，而导致的实时性达不到要求的问题。 本论文各章的内容安排如下： 第一章绪论，主要对水下机器人的研究现状进行介绍，论述了目前主要的 s l a m 算法，并指出了它们的优缺点。同时介绍了多线程的概念及如何用多线程 来提高实时性。 第二章首先介绍了a u v 的系统模型，然后阐述如何将速度和角度等传感器 的数据更新加入到基本的e k f s l a m 算法中，同时对数据关联做了介绍，最后 用m a t l a b 对算法进行了仿真。 9 中国海洋大学硕士学位论文 第三章介绍了a u v 系统的软件体系结构、a u v 各模块的作用及它们的通信 接口等。 第四章详细阐述了如何用多线程来实现e k f s l a m 算法，来缓解s l a m 中 声纳数据处理消耗时间过多的问题，以提高实时性。 第五章对全文进行总结和展望。 1 0 自主式水下机器人的同时定位与地图构建算法及实现 2e k f s l a m 算法及其仿真 s l a m 问题【4 】可以描述为：在一个完全未知的环境中，机器人从一个己知位 置出发，机器人在运动过程中通过自身携带的传感器( 数字罗盘、多普勒测速仪、 声纳、陀螺仪等) 在每一个机器人经过的位置感知环境信息，经过预测、观测、 更新等步骤实现自身的定位，同时增量的构建环境地图的过程。在所有s l a m 算法中，e k f s l a m 及其扩展算法使用最为广泛，而且适合于解决非线性问题。 由于大多数的实际导航系统都是非线性的，针对此种情况，本章讨论c r a n g e r 平台上e k f s l a m 算法的执行过程，同时为了验证此算法在c r a n g e r 平台上 的准确性，我们用m a t l a b 进行了仿真。 2 1 系统模型 移动机器人的运动模型和观测模型是实现定位与构图方法的基础。为此我们 对文中所用到的各种模型进行了介绍，包括c r a n g e r a u v 数学模型、坐标系 统模型、运动模型、观测模型等。在本节中，我们假设这些模型均是线性的。虽 然实际的系统往往并非如此，但是由于非线性系统可以通过泰勒展开，并略去二 阶以上项来进行线性化，所以这种线性假设并不影响算法的分析结果。 2 1 1c - r a n g e ra u v 数学模型 由于本文研究的是s l a m 在c ra n g e r a l j v 上的应用，我们首先介绍一下 c - r a n g e r a u v ( 图2 1 ) ，c 。r a n g e r a u v 采用开架式设计，是一种小型试 验用a u v ，它有两个电子舱和五个水下推进器，配置了主动声纳、垂直陀螺仪、 多普勒测速仪、数字罗盘等设备。主动声纳可以扫描二维平面内的地图信息，垂 直陀螺仪可以测量加速度和角速度，数字罗盘可以测航向角信息，多普勒测速仪 可以给出速度值。我们的研究内容是在一定深度的a u v 的运动，所以仅考虑二 维平面上的定位。 中国海洋大学硕士学位论文 2 1 2 坐标系统模型 图2 1c - r a n g e ra u v 目前，机器人领域主要用到三类坐标系统：笛卡尔坐标系、极坐标系和 d i n 7 0 0 0 0 坐标系。机器人全局系统使用的是笛卡尔坐标系，大多数距离方向传 感器，如声纳、激光测距仪等使用极坐标系，d i n 7 0 0 0 0 主要应用在欧洲研制的 机器人中。笛卡尔坐标系和d i n 7 0 0 0 0 坐标系是直角坐标系统的两种表示形式。 本文选用笛卡尔坐标系统作为直角坐标的表示形式，机器人位姿、环境特征位置 均采用笛卡尔坐标表示。 在移动机器人的自主导航研究中，主要用到全局坐标系、机器人坐标系和传 感器坐标系，一般情况下可以认为机器人坐标系和传感器坐标系