第2期 伍明，等：基于粒子滤波的未知环境下机器人同时定位、地图构建与目标跟踪 ·169 追踪的过程中，机器人需要始终保持与目标的追随关 在此基础上给出基于Bayesian估计的机器人同 系，也就是说，机器人在跟踪过程中保持运动状态，因 时定位、地图构建和目标跟踪理论描述.为了递归式 此存在机器人状态估计问题，而由于机器人事先并不 地得到机器人状态S,目标状态t,以及地图状态L 了解其运动环境，所以还需对环境特征状态进行估 的后验概率，应用条件概率的定义将5k、…、的 计，实际上这就是传统的SLAM问题；2)机器人状态 联合概率以S、、LM的条件概率进行扩展可得 和目标状态相关性.目标的状态估计是在机器人状态 p(s,Ln,zu)= 估计的基础上完成的，如果将两者分开进行那么就不 p(,p(u). (2) 能有效表示两者的相关性，进而影响最终目标估计的 准确性.由此可见，本文研究的问题是SLAM问题和 目标跟踪问题的耦合，如何设计一种一体化的估计算 环境特征 法正是本文的目的.Wang等o川首先对该问题进行 环境特征 了研究并提出一种基于扫描点匹配的解决方法，由于 观测 该方法采用的是ICP(iterative closest point)匹配算 机器人状 法，因此无法描述机器人和目标的不确定性和相关 态序列 机器人控 性，并且存在累积误差2] 制序列 1同时定位、地图构建与目标跟踪问 且标观测 题描述 目标状态 序列 假设机器人R以一定轨迹s=[s1S2…5] 图1 SLAMTO的Bayesain网络关系 运动，其在k时刻的状态为5k=[y是]，并 Fig.1 A dynamic Bayesian network of SLAMTO 且运动环境中包含若干个固定标志点Lw= 类似地，以z。的条件概率进行扩展可得： [m1n,…1nn],标志点i对应的坐标为1= p(S)= [xy:],i∈{1,2，…，m.同时目标轨迹为 p(a4ls,4,Lw2,tp(s4,y|t,).(3) t=[tt2…],其在k时刻的状态为= 变换式(2)并将式(3)代入得： xy].设z=[z12…],为从观测开 始一直到k时刻机器人对于环境的观测值，在很多 p(s,4Lwlt,)=p4Lw之1，) p(zu) 应用场合该观侧值可以是角度和深度信息.那么 p(a4ls,4,Lw2,t)p(s,4,Lwl2-,） SLAMTO(simultaneous localization and mapping with p(zu) tracking of objects)问题等同于求式(1)： p(zst,Lv)p(s,Luu) p(S:,L,tI z,u). (1) p(zu) (4) 也就是说，要通过观测值z=[zz2…2.]和机 式(4)为SLAMT问题的Bayesian估计描述，其 器人的控制量4=[4142…k]来确定机器 中p(z.lsk,t,L)表示观测值的相似度(likehood), 人的状态S,标志点的位置LM以及目标的状态t. 用于对预测概率分布的矫正；p(zz-1,4)为归一 该过程实际上可以描述成为动态Bayesain网络，如 化系数，p(s,t,Luz-1,)是对机器人状态、目标 图1示.图1中节点表示动态系统中各部分在时间 状态和地图的预测概率分布， 序列中的状态，k代表时刻，箭头表示各状态的依赖 下面介绍式（4)分子部分第2项p(5k,t, 关系.从该图可知目标k时刻的观测值：依赖于机器 Lwz-1,)的推导.利用全概率公式用sk-1和tk- 人k时刻的状态S以及目标k时刻的状态k: 对该式变换可得： 因此利用该观测值可以估计机器人和目标的状 p(St,t,Lul,u)= 态；对于标志柱斥时刻的观测值依赖于标志柱的 p(ss14e,4a1,Lwlt-l,d)ds1d业1…(5) 位置1n(i=1,2,…,m)以及机器人k时刻的状态，因 此同样能够利用该值估计机器人状态和标志的位置。 利用条件概率定义，用sk-1,-1和Lw对式(5)进行