第1期 伍明，等：基于协方差交集的多机器人协作目标跟踪算法 ·67 耦合问题.Wag等41首先对该问题进行了研究，并 环境特征Lm,箭头虚线代表此时R:的3种观测值 提出一种基于扫描点匹配的解决方法，由于该方法 采用的是ICP(iterative closest point)匹配算法，因此 无法描述机器人和目标的不确定性和相关性，并且 存在累积误差5].伍明等[6]提出了一种基于卡尔 2 曼滤波的算法，该算法将机器人状态、目标状态以及 环境特征状态作为整体来处理，增加了不同对象状 态之间耦合关系的估计，进而提高了算法的准确性. 以上方法均是针对单个机器人，并未发挥机器人团 队的协作优势.介于此，本文设计了一种基于协方差 交集数据融合的未知环境下多机器人协作目标跟踪 分布式算法，单机器人利用文献[16]提出的卡尔曼 图1系统观测对象示意 滤波方法进行SLAMOT(simultaneous localization, Fig.1 The diagram of object observations 设k时刻R:的系统状态向量为X（R)= mapping,and object tracking),多机器人利用协方差 交集方法进行不同对象的状态融合.协方差交集方 [(x)T()T(L)r]T,其中， 法的采用，使算法并不需要知道相关估计对象之间 L=[(X)T(X)T…(x)T]T 的协方差阵，因此算法具有分布式特点 为R:目前发现的环境特征状态估计分量.X:= [(X)TX]T为R对自身状态的估计分量，其中 1多机器人协作同时定位，地图构建 X=[xy]严代表位置状态估计分量，X”=代 与目标跟踪问题描述 表角度估计分量.此处将位置状态和角度状态分开表 假设存在m个固定环境特征{lm,L2,…,lmm}, 示的原因在于：由于观测值只能提供位置信息，因此 其中第i个标志柱的位置状态记为Xm.n台机器人 融合过程只针对状态位置分量进行.X,?为R对目 组成的机器人团队为{R1,R2,…,Rn},其中第i个机 标的状态估计分量.X,(R)对应的协方差阵为P 器人在k时刻的位姿状态为X.环境中存在需要追 另外，设R对R的估计为 踪的目标T,其在k时刻的状态记为X, X周=[(X)TX]T, 其中X罗为R对R的位置状态估计，X为R对 假设R:在时刻获得n个环境观测值，将该 观测值集合记为：，则包含4种可能的观测对 R的角度状态估计.Xs对应的协方差阵为P. 象，即袋=URUUZ,分别为： 未知环境下多机器人协作目标跟踪过程为：首 先单个机器人R,在未知环境中进行同时定位、地图 1)对于环境特征的观测值集合：={z, 构建与目标跟踪，若R:和R建立通讯并且R得到 z2,…,z{.其中m为k时刻机器人R:观测到 对R的观测值时，R:将通过计算得到其对R状态 的环境特征个数，：=[d:y]T为环境特征相 的估计X,R并将该估计值连同对目标的估计X,T 对于机器人的距离和角度观测值. 一同传输给R,R在得到R:发送来的信息后运用 2)对于目标的观测值：zr=[dy]T. 数据融合方法对本地信息进行更新，以提高本地信 3)对于同伴机器人R的观测值： 息估计的准确性.若R:和R建立通讯但R:没有观 z鸟=[d号y8]T 测到R,R,只把对目标的估计X,T传输给R,那么 4)伪规测值集合： R将只对本地目标状态估计进行融合： z={z听，z5,…,z. 不同于文献[2]方法，本文设计的算法是分布式 式中：≥0为k时刻机器人R,获得的伪观测值数 的，因此能满足鲁棒性和时效性要求.需要说明的是以 量，并且有n=m++2. 下只考虑2个机器人的情况，对于更多机器人的情况 假设z如能够区别于，z,z,并且 可以结合利用文献[18]的数据通讯方法来解决另外， z不存在伪观测值（在实际应用中可以采用特定 算法只对目标和机器人状态进行融合并没有将环境特 的环境特征和性能较好的环境特征识别算法来 征进行融合，其原因有二，首先，环境特征融合会带来 达到该要求)，系统观测对象如图1所示.该图是以 通讯和计算负担从而影响系统实时性，其次，从文献 R:为主体的系统观测过程示意图，其中空花形机器 [16]研究可知，SLAMOT估计准确性的关键在于对机 人代表同伴R,实花形机器人代表目标T,圆点代表 器人状态的正确估计，因此一旦提高机器人状态估计