SLAM算法论文（共5篇）
SLAM算法论文 篇1
同时定位与地图创建SLAM (simultaneous localization and mapping) 是实现真正自主移动机器人的关键, 也是机器人研究领域中的难点问题。利用机器人自身携带的传感器感知周围的环境以确定机器人在环境中的位置以及对环境进行建模, 是机器人进行自主导航以及其他复杂任务的前提和保证。多机器人SLAM在单个机器人SLAM的快速发展中得到了促进, 使得多机器人SLAM算法开始被越来越多的人们研究。计算智能有着传统的人工智能无法比拟的优越性, 利用它解决复杂的多机器人SLAM问题具有广阔的前景。本文根据使用方法的不同, 对基于计算智能算法的多机器人SLAM进行了介绍。
多机器人协作系统凭借其更显著的优势在在未知环境中多机器人的协作显示出了巨大的优势, 能够在一些复杂、危险、人类难以到达的环境发挥作用, 比如抢险救治、勘探资源、空间探测、工业生产。其中有四个显而易见的优点: (1) 效率高——使用空间分布的传感器, 相比单一系统可以更快地减少地图的不确定性; (2) 可靠性和鲁棒性——机器人功能更加全面, 任何特定的机器人失去作用并不一定会导致总体任务的失败; (3) 专业化——拥有更多的感官视角和运动技巧, 多机器人拥有更多可以扩展的功能, 这就克服了单个机器人的局限性, 增加协作系统的总体效用; (4) 容错性——对一个机器人犯错误的容忍能力比较高。
计算智能, 就是借鉴生物学的思想, 从仿生的角度来抽象成数学计算的方法。其中主要分类有人工神经网络、模糊算法、进化算法、人工免疫系统、蚁群算法、粒子群算法、多代理 (Agent) 系统等等。利用计算智能算法解决多机器人SLAM问题, 面对复杂的不确定的问题, 显示出了很强的优越性。本文依据使用方法的不同, 对多机器人的SLAM进行介绍。
SLAM方法研究进展
神经网络算法
Saeedi等人为了解决多机器人图像融合的有效性问题, 提出了一种基于神经网络的多机器人SLAM方法, 解决了地图融合的问题。单个机器人搭载激光传感器利用卡尔曼滤波 (EKF) 算法生成局部栅格地图, 然后融合成全局地图。每一个栅格地图神经元进行分裂, 利用SOM算法进行神经元集聚, 有效降低了栅格地图神经元成为障碍的不确定性, 集聚后的神经元抽象成全局地图。优点在于地图学习过程中, 细胞集群代表空间形式的地图使得处理更容易, 速度也更快, 使得机器人在未知环境中更快感知环境。算法流程如图1所示。
多代理Agent方法
南京理工大学提出了一种利用进化的多Agent再励学习方法, 实现未知环境下多机器人自组织地进行地图构建任务。文章使用了分散型同质结构Multi-Agent系统。该学习方法的重要特征是基于全局的适应度标准来进化群体机器人的运动策略, 从而优化了其行为选择与地图更新的效率。仿真时应用基于进化算法的多Agent强化学习方法, 利用一群机器人自组织地构建未知环境的地图, 取得了预期的实验效果。
进化算法
刘利枚等人为了解决多机器人在未知环境下的地图融合问题, 提出了一种改进的差异进化算法。改进之处在于算法中的搜索策略得到优化, 速度更快, 有效的实现了栅格地图的重叠性搜索。仿真实验中, 进行了地图融合的实验, 如图2所示。
通过和其他算法对比, 证明了改进算法在减小误差方面的优越性。
遗传算法
山东大学提出了基于遗传算法的多机器人系统地图融合算法, 适应性遗传算法的改进主要在于, 通过适应性的调整遗传过程中的交叉概率和变异概率, 提高了算法的可靠性。最后的对比仿真实验表明, 基于遗传算法的多机器人地图融合能够在全局范围内进行并行搜索, 使得地图融合的效果得到了很大的提升。
姜健等人针对如何提高多机器人协作探测未知环境的效率, 减少机器人间碰撞几率, 提高系统可靠性的问题, 提出了基于改进的并列选择遗传算法和信息共享的策略。仿真结果表明, 在真实的环境中, 该算法可以有效降低机器人碰撞的概率。
华南理工大学提出了一种遗传变异梯度下降的改进的扫描匹配算法。很好修正了里程计的误差。
粒子群算法
中南大学针对室内环境地图建模, 传感器建模和机器人运动建模, 将粒子群优化思想引入Fasts LAM中, 提出了改进的SLAM算法。改进之处在于优化了机器人下一时刻位姿采样的效率。通过和其他SLAM方法进行对比测试, 对同一SLAM算法使用不同的粒子数进行了比较试验, 通过仿真实验, 将该算法运用到机器人在线验证平台上, 验证了该算法的可行性。
广东工业大学根据粒子滤波器算法, 本文引入蚁群算法对其进行改进, 旨在粒子状态空间中找到最优的粒子, 然后进行精度更高的SLAM研究。
潘薇等人提出一种基于改进粒子滤波器的移动机器人SLAM方法。该方法亮点是结合了粒子滤波器与粒子群优化算法, 引入实时的观测信息以调整粒子的分布, 从而在保证算法精度的同时, 减少SLAM所需的粒子数。仿真结果表明, 该文方法为移动机器人的SLAM问题提供一种更好的方案。
多机器人SLAM研究存在的问题
多机器人的SLAM研究, 不光要处理单个机器人各自的SLAM, 还要考虑到机器人之间如何通信, 如何协作等问题, 以及后续的如何对各自的信息进行融合, 进行地图融合等等。
不确定性的处理
机器人SLAM研究避免不了的是对不确定性进行研究。存在着3种不同形式的不确定性:在数据关联上、在导航不确定上、在传感器受干扰上。此外动态环境本身也存在着某种形式的不确定性。解决真实环境下的不确定性, 往往需要同时考虑多种条件, 需要结合多种方法进行具体问题具体分析。不确定信息如图3所示。
地图构建的效率
机器人身处动态的, 不确定的未知环境, 能够获取的信息只是来自所搭载的传感器, 为了实现导航, 路径规划等等更高级的任务, 对SLAM算法的精度提出了更高要求, 也要求更大的实时性和可靠性。
多机器人SLAM的发展趋势
创建三维环境地图
现行的SLAM研究都是集中在二维平面, 往往只能用来实验仿真, 难以运用到真实的环境中。现实的环境都是立体的、动态的三维环境, 所以需要我们将研究内容转移到三维地图的创建, 尽管面临的问题会更多, 难度会更大, 但依旧是必须解决的问题。
动态环境的学习能力
机器人执行任务时我们更希望它具有更多的自主性, 可以自行去解决问题, 使得机器人显得更“智能”, 这就需要增强机器人面对动态环境具有自主学习能力和适应能力。动态环境往往是时间和空间不断变化, 共同影响着机器人的定位与建图。我们可以增加如强化学习, 进化学习等机器学习的策略, 使移动机器人更好地学习动态环境。
摘要：本文针对动态的复杂的多机器人SLAM问题, 提出利用计算智能算法进行分析, 凭借仿生学的特性, 具有很强的优势, 在多机器人SLAM领域会得到更广阔的应用前景, 使得机器更加“智能”。
SLAM算法论文 篇2
关键词：重定位,几何约束分枝定界算法,同时定位与地图创建,联合相容
0 引言
当环境未知时,为使移动机器人实现自主行走,必须同时解决定位和地图创建问题,也称SLAM问题(Si⁃multaneous Localization and Mapping)[1]。而其中的重定位则是研究在缺乏初始位姿等先验知识的前提下,使机器人根据其所携带的传感器测量值,确定自身在给定地图中的位置及姿态,此后可重新开始进行同时定位与地图创建。当无法获得GPS信号时,重定位是惟一有效的方法,适用于以下三种场合:在定位误差较大时自行恢复;在机器人“迷失”时重新启动SLAM过程;在循环较大时使其能够安全地结束。
要处理重定位问题,目前主要有两种不同的思路[2]:以最大团[3]、假设检验[4]与联合相容分枝定界(JCBB)[5,6]等方法为代表,其实质是在地图中寻找与传感器探测信息相一致的特征,再参照机器人与这些特征之间的相对距离与方位,最终确定机器人在地图中的位置;以蒙特卡洛定位[7]和马尔可夫定位[8]等方法为代表,其实质是在机器人可能所处的各位置,分别对传感器探测信息与地图间的一致性进行检验。第一种思路的关键是数据关联,数据关联[9,10]可在传感器观测与地图特征之间建立对应的关系,并直接影响到SLAM中的状态估计,而实际中各种不确定性与干扰因素的作用使得数据关联更为复杂与困难。
为解决数据关联问题,研究者们提出多种不同的算法[11,12,13,14]。其中,最邻近(NN)算法是最早提出的,其算法简单、易于实现,但对传感器测量精度要求较高,且不适用于干扰程度较严重的环境。文献[15]将分枝定界和几何约束相结合,提出了一种性能较好的几何约束分枝定界(GCBB)算法,然而当观测数目增大时计算量也呈指数关系增加,使得实时性变差,限制了其实际应用。鉴于此,本文将从两个方面对GCBB算法进行改进:在观测数目较多时,分组进行关联,以降低计算量;利用机器人传感器的测量范围将数据关联的范围限制在局部可能区域内,以减少数据关联时的搜索空间,从而进一步降低计算量。在此基础上,设计了一种快速几何约束分枝定界(Fast Geometric Constrains Branch and Bound,FGCBB)算法,并利用悉尼大学提供的“Victoria Park”数据集[16]对FGCBB算法进行验证,以期为重定位问题提供一种新的解决思路。
1 SLAM问题的一般数学模型
SLAM问题中,利用状态向量及其协方差矩阵可将地图信息表示为Map={Xk,Pk}。系统的状态向量可表示为:
式中:Xvk=[xvkyvkΦvk]T,(xvk,yvk)是机器人的位置坐标,Φvk是机器人的姿态角;M=[x1y1x2y2⋯xnyn]T表示各个地图特征的位置坐标,对于静止特征,位置坐标是不随时间变化的。
协方差矩阵的表达式为:
式中:Pvv是机器人位姿估计协方差;Pmm是特征位置估计协方差;Pvm是交叉协方差。
设k时刻,机器人所带传感器获得环境特征Ei的观测为zk,i(i=1,2,⋯,m),以Fj={xj,yj}(j=1,2,⋯,n)表示地图中的已有特征,为建立zk,i和Fj之间的关系,需要通过数据关联将地图特征Fji与每一观测zk,i进行对应,进而得到关联假设:
当Hm中存在某个ji=0,则表明观测zk,i可能是虚假观测或新特征,故无法与已有特征相匹配。数据关联本质上是一个搜索解空间的过程。设观测数为m,可能与第i个观测相匹配的特征数为(n+1),那么可知需搜索的空间大小为(n+1)m,与观测数m成指数关系。
2 GCBB算法
2.1 几何约束
几何约束包括一元(unary)几何约束与二元(bina⁃ry)几何约束,当缺少位置估计时,可以利用几何约束实现数据关联。
(1)一元几何约束
设pij=(Ei,Fj)为所给定的“观测⁃特征”配对,以分别代表Ei与Fj的某几何特性参数,如长度、半径和角度等。pij=(Ei,Fj)满足一元几何约束的条件是:
式中:d=dim(ui);1-α是给定的置信水平,通常取95%。χ2d,1-α的值可在卡方分布表中依据自由度d与置信水平1-α查表得到。若观测数目为m,则对应有m个一元几何约束。
(2)二元几何约束
设pij=(Ei,Fj)与pkl=(Ek,Fl)为所给定“观测⁃特征”配对,则二元几何约束是指Ei,Ek间的几何关系与Fj,Fl间的几何关系必须一致。以fb(·)表示两者间的几何关系函数,则Fj,Fl之间的几何关系估计值与方差的表达式分别为:
式中:均为雅可比矩阵。
同理,可得到观测Ei,Ek之间的几何关系估计值与方差Pik。若有:
则认为pij=(Ei,Fj)与pkl=(Ek,Fl)满足二元几何约束。若观测数目为m,则对应有m⋅(m-1)/2个二元几何约束。
2.2 联合相容检验
利用2.1节中的几何约束将观测和特征配对后,还需通过联合相容(Joint Compatibility,JC)检验所得配对的一致性,并获得一致性最佳的配对。在进行检验时,联合相容所采用的依据是特征间的相对误差,随着配对数目的增加,单个错误配对与其余配对联合相容的概率将降低。目前,大部分的重定位算法都是基于联合相容来检验关联的一致性,其效果差别较小,但计算效率却存在较大差异。
机器人的观测方程可写为:
式中:h(·)为非线性函数;nk是方差为Rk的高斯白噪声。
设关联假设Hm={j1,j2,⋯,jm},则可列出联合观测方程:
联合新息vHm与协方差SHm的表达式为:
式中:zk=[zk,1⋯zk,m]′表示k时刻的所有观测值;分别表示k时刻机器人的预测位姿及其预测方差;为雅可比矩阵。
而观测zk与Hm中相应的地图特征满足联合相容的条件为联合马氏距离:
式中d=dim(zk)。
将几何约束和分枝定界法两者进行结合,便形成了几何约束分枝定界(GCBB)重定位算法,具体的推导过程可参见文献[15]。
3 改进的GCBB重定位算法
由于GCBB算法的计算复杂度与观测数目m成指数关系,当m较大时,算法的执行速度将大幅度降低,直接影响到算法的实际应用。下面将从两个方面对GCBB算法进行改进。
3.1 数据关联的分组处理
当观测数目较大时,先将所有观测进行分组,再对各组进行数据关联。设每组观测数目的上限为N,此时搜索空间的大小由(n+1)m变为m N⋅(n+1)N,相应地,计算复杂度与观测数目m之间也从指数关系变成线性关系,计算效率得到了较大提高。按分组方式进行数据关联时,配对出错的概率随着N的增加而降低,为有效地防止错误关联,一般要求N6,考虑到随着N的增大,计算量也会迅速增加,故N通常在6~10之间选取。
3.2 关联特征的局部化选取
在进行数据关联时,所采用的地图特征数目越多,计算量越大。考虑到任一时刻机器人所携带的传感器的探测范围是有限的,为减少计算量,可根据传感器的探测范围选取相应的局部区域内的地图特征,实施数据关联。
首先,假设地图中有n个特征,定义n×n维矩阵C,则:
式中:(xi,yi),(xj,yj)分别表示地图中第i,j个特征的位置;R由传感器探测范围确定。只有满足Cij=1,才能执行下一层的联合相容配对。
3.3 快速几何约束分枝定界算法(FGCBB)
综合第3.1,3.2节所提出的改进,便可得到FGCBB算法,其处理流程的伪代码如下所示。需要注意的是,与常规GCBB伪代码相比,在分枝判定中增加了Cij==1,以考虑特征的区域性。
FGCBB处理流程伪代码:
GCBB处理流程伪代码:
4 算例仿真与分析
由于“Victoria Park”数据集提供了丰富的GPS数据、激光雷达数据与DR数据,为了对FGCBB算法进行验证,本节利用该数据集进行仿真。设置传感器探测范围为R=16 m,N=6。计算平台CPU为IntelⓇCoreTMi5⁃3210M,主频为2.5 GHz,算法通过Matlab R2013a实现。
将EKF⁃SLAM算法[2]执行1 000步以获得环境地图,其中含有99个树特征。设一元、二元几何约束分别取为树干半径与树间距离。继续执行EKF⁃SLAM算法,在第1 001~2 500步对重定位算法进行验证,而每一步计算所得的机器人位姿则为重定位结果提供参考。
1 001步时激光雷达测量中提取的树特征分布如图1所示,此时观测数目为m=7。图2给出了此时快速几何约束分枝定界算法重定位的结果。从图2知,所有的观测与地图特征进行了正确关联,重定位位姿与参考位姿相差很小。
1 001~2 500步中快速几何约束分枝定界算法(FGCBB)的重定位结果见图3。在此过程中,有737步机器人是位于地图中的,其中的74步观测数小于6,而地图之外的其余各点均没有获得定位。
图4给出了1 001~2 500步中不同观测数目条件下FGCBB与GCBB的平均执行时间对比曲线。可以看出,当观测数目大于6时,FGCBB比GCBB的执行时间要少,且观测数目越大,两者的差距越大。
5 结语
SLAM算法论文 篇3
Tennis diversity has become a prominent issue across the whole world.It is not uncommon that people living outside the same geographical boundaries can understand tennis in ways that differ from the other cultures.
Australian Open
Australian Open is the youngest championship among Grand Slam Tennis,which started in Melbourne Park in1905,just as the establishment of the young nation itself.Tennis is as much a religion to many men in Australia as it is anywhere else.They disappear in a weekly pilgrimage to their local courts each weekend to their reverence,reflect,pray and perhaps hit the ball outside the court or abuse the ball.Owing to its particular geographical location,most of Grand Slam winners and other high-ranking players believe that Australian Open is the most difficult one to play largely because its high and dry temperatures in January.But the authorities insist that they do not close the roof of the stadium until the temperature reaches 40Celsius Degrees.As is the same case mentioned above,the early settlers and its aboriginal people could survive in one of the driest continent in the world,so do the professional players.It is explicitly stated that a strong message in Christianity is courage.His strength and courage are traits that all Christianity is reminded of repeatedly.Australian's development represents a triumph over remoteness and a harsh environment.So do the winners of the Australian Open.Despite the unbearable weather,supporters ar e still filled with full enthusiasm.It is likely that most Aussies are immersed into a relaxed casual tennis holiday mood being felt throughout the entire tournament.Besides,the horrid and sunny climate leaves few of them empty.
French Open
French Open is the only big tournament among Grand Slam Tennis that is held on clay in Roland Carros in mid-May in Paris.Comparing to the other three big championships,the speed of tennis on such a court is much slower.Romance can be found everywhere in and out the court.A couple of sweetie lovers holding,or hugging others,permeated with sincere,harmonious and warm atmosphere.A big surprise for every supporter,he or she can receive a rose when entering the court.Obviously,in such a high context culture,when people communicate with others,large amount of information is either concealed in social context or internalized in communicator's heart;comparatively speaking,explicit coding conveys less amount of information.In other words,people are sensitive to the change of delicate environment.In addition,French supporters are far more impressed with passion and emotion.Their eyes move along with the route of ball and tend to be so intense that even some people may feel intimidated.People who speak Latin can do extraordinarily best in such a court because they prefer slower court.There is even a proverb that states,“To a hurried demand,a leisurely reply.”
U.S Open
The U.S.Open occurs at the USTA National Tennis Center each summer,one of the four venues for Grand Slam Tennis.The tennis event carries the richest purse in professional sports.As an official sponsor of the 2001 U.S.Open,Lincoln has transformed an 8,000-square-foot indoor tennis facility.And the inferior is designed to be indulgent and comfortable———all characteristics that define American Luxury.Americans grow up believing the motto“He who hesitates is lost.”Therefore,the speed of tennis court in U.S.Open is at lighting speed,and it is incredibly suitable for American players.Today,Puritans are no longer in existence.But their legacies are still felt in American society.The American value,such as hard work,owes very much to the Puritan beliefs.A major reward of hard work,and an important American value,is leisure.It is relief from the regularity of work.In terms of that,it is not uneasy for people to understand why tennis matches are held during the night period of time.
In conclusion,it is clear that the same tennis embodies different cultural phenomenon.Tennis is very powerful both in the pursuit of excellence and helping to tackle social and health problem.What we aim to do is improve access for all by permitting and encouraging the highest quality of tennis and be displayed.
参考文献
[1]http://www.Australian Open.com.
[2]http://www.French Open.com.
[3]http://www.Winbledon.com.uk
SLAM算法论文 篇4
SLAM (Simultaneous Localization And Mapping) 是移动机器人构建环境地图同时进行自身定位的过程。 SLAM中, 在没有任何先验信息的情况下, 移动机器人结合里程仪等外部测量设备检测外界的环境特性, 在线估测平面内的运行轨迹和所有环境特征的定位。
2 扩展卡尔曼滤波器
卡尔曼滤波器估计用线性随机差分方程描述的离散时间过程的状态变量, 广泛应用于定位、创建地图和导航等算法上。但如果被估计的过程和 ( 或) 观测变量与过程的关系是非线性的, 那么将期望和方差线性化的卡尔曼滤波器称作扩展卡尔曼滤波器 (Extended Kalman Filter) , 简称EKF。
3 EKF-SLAM环境特征
在EKF-SLAM中, 地图是一个巨大的传感器向量集和路标状态。 EKF通过预测和校正过程确定地图。为了获取真实的测量值, EKF过程中有一个路标初始化的步骤, 新观测的路标会被添加到地图中。路标初始化由逆观测函数及其雅可比进行计算。
在EKF-SLAM中, 地图由一个使用平均值和状态向量协方差矩阵的高斯变量P表示:
4 EKF-SLAM过程
4.1 地图初始化
初始时刻地图中没有路标, 因此n=0。通常将当前的机器人位姿作为初始位置:,
4.2 机器人运动状态的预测
在EKF中, 设x为状态向量, u为控制向量, n为扰动向量, 那么可以将时间更新函数表示为:。
在SLAM问题中移动机器人的状态是随时间改变的, 因此状态向量的每部分都有不同的表示:
将机器人运动过程中的EKF预测式简化如下:
4.3 地图中路标的观测值
在EKF中, 设y是包括噪声的测量值, h () 是观测函数, v是测量噪声, 路标观测值由EKF一步步处理。观测值仅仅依赖于机器人位置及状态R, 传感器状态S和路标状态。假设已经观测到路标, 定义观测函数:
EKF-SLAM中的雅可比函数:
简化后, 校正公式可以表示为:
在这些等式中, 式7 和8 为观测更新均值和协方差矩阵, 式9 和10 组成了滤波器的更新在测量路标时, 这些等式会迭代应用。
5 测试结果与分析
运用MATLAB进行仿真, 图1 中深色的椭圆形表示校正的误差范围, 蓝色实心点集表示每一个时刻k对路标的预测值, 绿色实心点表示机器人预定轨迹, 绿色圆圈表示对路标的后验观测结果, 红色星号表示预置路标。清晰的显示了移动机器人同时定位与地图创建过程。
SLAM算法论文 篇5
关键词：增广扩展卡尔曼滤波,同步定位与地图构建,测程法误差,均方根误差,归一化误差
0 引言
自主移动机器人在未知环境中，行进时通过自身位姿估计和传感器观测信息增进式地构建环境地图，同时利用该地图实现自身定位和导航。这就是移动机器人同步定位与地图构建(SLAM)[1]。该问题一直是智能移动机器人研究领域的热点和难点，被认为是能否真正实现机器人自主导航的关键问题。
机器人在行进过程中所积累的误差大致可分为两类:非系统误差和系统误差。非系统误差通常由复杂的环境地图特征和机器人不规则随机运动所引起，如不平的地面、机器人过度加速或快速转弯等。一般来说，很难预测非系统误差。而系统误差则是由机器人设计缺陷、机械启动、测程法难校准等问题引起的。
对于测程法产生的系统误差，Borenste和Feng等[2]提出UMB校准方案进行校准;Doh等[3]提出PC校验法对系统误差进行补偿;Antonelli等[4]建立了包涵3个参数的测程法模型，基于最小二乘法同样提出了校验方案。但这些校准方案都是离线的，即机器人在经过具体路径之后才执行测程法校准，不能对测程法引起的系统误差进行实时更新。
Larsen等[5]率先提出增广扩展卡尔曼滤波(AEKF)模型来在线校正系统误差;Martinelli等[6]对AEKF进行了扩展，能估计系统误差和非系统误差。Yun等[7]基于AEKF和家庭服务机器人的固有家庭定位也提出了测程法误差校正方案。但是，这些基于AEKF的方法非常依赖于精确的特征地图。
本研究基于特征地图、采用AEKF算法框架，并根据实际机器人模型选取速度校正参数来解决SLAM问题。所提出的SLAM方法同时用于估计初始SLAM状态空间和系统速度校正参数，笔者在预测和更新步骤中对这些参数进行实时修正，降低航向角误差。由于EKF-SLAM的状态向量与系统速度校正参数是相互独立不相关的，该方法可以实现自身的精准定位并构建更加精确的特征地图。
1 基于AEKF的SLAM数学模型
1.1 SLAM状态空间增广
对于差速驱动系统的移动机器人来说，驱动整个机器人向前行进的车轮半径、行驶速度和航向角变化对测程法的影响至关重要。如不等的左、右车轮半径会使左、右轮产生较大的速度差，尤其是机器人在转向时左、右轮速度未加修正，导致测程法测距轨迹向左或向右倾斜，机器人航向角的偏离误差不断累积，这种情况将使传统的SLAM(如EKF/Fast SLAM)算法发散，性能严重受损，致使机器人无法正确构建地图、定位与自主导航。
测程法的误差模型可以映射为前驱左、右车轮的速度误差模型[8]。设前驱左、右轮的速度分别为vl,vr，引入左、右轮速度系统校正参数δl，δr，则机器人的速度为:
在EKF-SLAM算法结构下，系统参数δl，δr将会被添加到SLAM的估计状态向量，形成增广EKF状态空间。基于EKF算法，在测量更新步骤中，笔者用卡尔曼增益和新息矩阵来更新机器人位姿、系统速度校正参数和地图特征位置。在预测步骤中，已更新的系统速度校正参数用于降低机器人的航向角误差和测程法误差。
1.2 AEKF-SLAM算法总体结构
设k时刻机器人的位姿状态向量空间为xv(k)=[xv(k),yv(k)，θv(k)]T，系统速度校正参数向量为xs(k)=[δl，δr]T，地图特征位置向量为xm(k)=[x1,y1，…，xn,yn]T，其中n为特征数。则AEKF-SLAM的空间状态向量为:
其协方差矩阵P(k)为:
SLAM的估计状态向量和协方差分别为)x(k),)P(k)，滤波器根据预测值)x-(k),)P-(k)和当前的测量值z(k)来递归更新)x+(k)和)P+(k)。
1.3 AEKF-SLAM算法步骤
与EKF-SLAM相似，AEKF-SLAM包含同样3个步骤:预测、更新和状态增广[9]。
(1)预测k-1时刻，机器人位姿向量为)xv+(k-1),k-1到k时刻机器人的控制量为u(k)=[δl，δr，γ]T，δl，δr为速度校正参数，γ为航向角。在这里，采用运用范围较广的Ackerman机器人运动过程模型[10]，考虑到机器人系统校正参数，对机器人模型进行修改，则机器人的动力学方程可描述为:
式中:f(·)—机器人的运力学模型方程，dt—控制时间步长，L—前、后轴的轮基距。
那么，k时刻SLAM的状态空间和协方差的预测值分别为:
式中:—机器人运动模型关于机器人位姿向量和控制向量u(k)的雅克比(Jacobian)行列式;Qk—过程噪声的误差协方差，假定过程噪声是均值为零的高斯噪声。
(2)更新。假定环境中的路标是静止的，是机器人观测到的第i个路标位置，机器人装载的距离方位传感器返回传感器与该路标之间的距离和角度，那么观测模型可表示为:
依据以下方式来递归更新估计的状态空间和协方差:
上述S(k)为新息矩阵，现对其进行如下处理:
以上式(9)使新息矩阵对称化，式(10)表示对新生成的对称化新息矩阵进行Cholesky分解，这种处理结果在递归运算过程中在数值上要比简单运用新息矩阵更加稳定。
Kalman增益Kk则变化为:
递归更新方程为:
式(8～13)中:—观测模型关于状态空间的雅克比行列式，Rk—观测噪声的误差协方差，假定观测噪声是均值为零的高斯噪声。
(3)状态增广。当机器人观测到一个新的路标，经过校验后，需要把这个新观测到的路标的位置状态添加到系统状态向量中。运用增广模型对新的系统状态向量和协方差矩阵进行更新。新观测到的路标位置可表示为:
那么包含新路标的系统状态空间为:
对应地的协方差矩阵为:
其中:
式中:关于机器人位姿向量和观测向量的雅克比行列式。
2 实验分析
2.1 仿真环境及结果
仿真实验采用Matlab仿真平台。假设机器人的探索仿真环境如图1所示。环境中分布着35个特征点和17个路径点，分别用“*”和“·”表示，细实线表示路径点确定的路线。
在实验中，机器人的位姿初始化为xv=[0,0,0]T，沿路径点确定的轨迹逆时针绕行一周。前、后车轴距L=4 m，前驱左、右轮初始化速度vl=vr=3 m/s，校正参数初始化为δl=δr=1，其附加噪声是零均值，方差为0.1的高斯噪声，航向角的范围是(-π/4，π/4)，控制时间步长tc=0.025 s，观测时间步长to=0.2s，传感器最大探测距离为30 m，探测夹角为(-π/2，π/2)，系统协方差初始化为0，控制噪声协方差为Qk=diag{0.22,0.22,(2.0·π/180)2}，观测噪声的协方差为Rk=diag{0.22,(2.0·π/180)2}。
标准EKF-SLAM和AEKF-SLAM的仿真结果分别如图2(a)、2(b)所示。图中实线为机器人真实路径，虚线为SLAM算法的估计路径，“*”为SLAM估计的路标位置，“椭圆”表示所估计路标位置的不确定度。
2.2 结果分析
本研究使用标准EKF和AEKF进行状态估计产生的机器人位姿的定位误差如图3所示。其中，上方的图表示的是机器人偏离原定路线x轴和y轴的位置误差(单位为m)，下方的图表示的是机器人偏离原定路线的航向角误差(单位为rad)。
从图3(a)可见，在时间步长N(1步=0.025 s)约在2 700之前，系统噪声和观测噪声对系统产生的影响较小，机器人的定位精度保持在较高水平。由于EKF假设速度是不变的，随着噪声的不断累积，机器人不断转向，航向角误差逐步积累，观测值的可信度下降，观测值误差加大，滤波器变得不稳定，导致机器人定位误差越来越大。注意到，当N大约为2 700时，机器人的航向角的误差超过0.03 rad，即1.7°，滤波器开始变得不一致(见下文一致性分析)。随着噪声进一步增强，机器人的航向角误差呈持续上升状态，滤波器逐渐发散，机器人定位不准确，估计的路标位置同样不准确。与图3(a)相比，图3(b)机器人的位姿误差始终保持着较低水平，航向角的误差始终低于0.03 rad，机器人定位和路标位置精度大大提高，滤波器的稳定性得到有效提升，保持了滤波器的一致性。这正是由于AEKF使用了机器人速度校正参数，根据机器人的运动状态，校正参数不断被估计和更新来产生相应的速度，使航向角实时适应机器人的变化运动状态。
为比较所提出方法的性能，笔者引入均方根误差(Root Mean Squared Error,RMSE)[11]，其定义如下:
均方根误差越小说明算法的估计性能越好。使用标准EKF和AEKF两种算法进行状态估计得到的机器人位姿的均方根误差如表1所示，实验数据表明AEKF算法很大程度上提升了估计精度。
现考虑这两种算法的一致性[12]，采用归一化估计方差(Normalized Estimation Error Squared,NEES)，其定义如下:
为估计EKF/AEKF的一致性，需执行M次Monte Carlo试验，来计算平均NEES:
在实验中，执行50次Monte Carlo试验，平均NEES的95%置信区间为[2.36,3.72][13]。如果)εk上升超过上界，则滤波算法表现出不一致性，低于上界就是一致的，如果)εk低于下界，则滤波算法是保守的[14]。使用标准EKF和AEKF进行状态估计产生的机器人位姿NEES如图4所示，实线表示平均NEES，虚线表示置信区间。
从图4(a)中可以看出，当N约为2 700时，平均NEES开始超过上界，滤波算法表现出不一致，该时刻的航向角误差约为0.03 rad。结果表明，滤波算法的一致性与航向角的误差息息相关，导致滤波算法不一致的航向角误差临界值约为0.03 rad。从图4(b)可见，由于航向角误差始终小于0.03 rad，平均NEES绝大多数时刻保持较低水平，滤波算法表现出一致性和稳定性。
3 结束语