一种普适机器人系统同时定位、标定与建图方法

ao-Blackwellized粒子滤波的同时传感器网络标定与机器人定位方法，但对于联合机器人定位、传感器网络标定和环境建图的研究，目前尚未展开。

　　考虑到普适机器人系统同时机器人定位、传感器网络标定与环境建图问题的耦合关系，为充分融合定位、标定和建图过程中涉及的多类信息来源，并避免繁琐的传感器网络离线标定环节，本文提出了普适机器人系统同时定位、标定与建图的概念。在进行三者的联合求解时，从概率的角度进行理论分析，将联合条件概率分解为若干可解项分别求解。基于Rao-Blackwellized粒子滤波思想，联合机器人控制信息、传感器网络对机器人的观测，以及机器人对已建环境地图的观测估计机器人位姿粒子及其权值分布，进而根据传感器网络对机器人和已建环境地图的观测来标定传感器网络的参数，最后联合机器人和传感器网络对环境的观测构建家庭环境的特征地图。

　　1系统描述

　　1.1系统构成

　　本文讨论的对象为普适机器人系统，该系统主要由三个部分构成：具备普适感知和处理能力的传感器网络及处理主机、与普适处理主机交互的移动式服务机器人，以及环境中的各种目标（包括服务对象、操作物品及环境路标等，这里统称为目标）。图1给出了一个典型的家庭普适机器人系统的示意图，其实现方案如下：由RGB-D摄像头作为节点构建传感器网络，该摄像头能够同时获取视域范围内目标的颜色和距离信息，各RGB-D摄像头通过数据线连接到一台处理主机的图像采集卡上，该主机负责分析处理各摄像头所捕获的图像，并通过无线网络实现与服务机器人的通讯;家庭服务机器人平台选用配备手眼系统的Pioneer3DX型移动机器人，并为该机器人设计标识色块以便传感器节点观测定位;选取家庭环境和目标的尺度不变特征变换（Scaleinvariantfeaturetransform，SIFT）［16］进行特征检测、匹配和识别。此外，为家庭常见目标粘贴标识其名称、功能及用法等信息的QRcode标签，通过阅读标签机器人能实现对物品的深层次认知。

　　传感器网络的节点部署需要综合考虑节点观测范围、能耗、障碍分布等因素，该问题已有相关文献论述［17−19］，在此不做过多讨论，假定传感器节点已被较为合理地部署。

　　2普适机器人系统同时定位、标定与建图

　　2.1基本思想

　　从概率的观点看，普适机器人系统同时定位、标定与建图问题可以用概率密度

.3粒子权值计算及重采样
 

鉴于采样粒子数量有限及样本退化问题,由粒子集模拟的机器人轨迹与机器人实际轨迹之间往往存在偏差,故需要引入重采样环节,以去除权值较小粒子对机器人轨迹估计的影响.上一节中,在近似估计式(10)时,并未考虑不同粒子之间归一化系数η(k)的差异,为了弥补这种偏差,将机器人运动轨迹的后验概率分布与假定后验概率分布之比作为重采样时粒子的权值,即

　　同时定位、标定与建图问题的本质是多传感器

　　信息融合意义下的状态估计问题。本文采用序贯方式融合两类观测信息进行状态的观测更新，针对每一类观测，当同时存在多个该类观测时，仍然采用序贯方式加以融合，如对于步骤2中2）的观测更新，当存在多个传感器节点对机器人的观测时，采用序贯方式融合多传感器节点的观测信息，该策略同样应用于步骤2中3）、步骤3中2）和步骤3中3）中存在多个同类观测的情形。假定普适机器人系统中包含一个移动机器人和一个由M个节点构成的传感器网络，且环境中包含N个特征点，粒子个数选为K.在极端情形下，即各传感器节点始终可以观测到机器人和所有环境特征点，且机器人在任意时刻也都可以观测到所有环境特征点时，本文算法所要融合的数据量最大，通过算法分析可知此情况下算法循环次数为K（M（N+1）+N（M+1）），或者说，本算法在最坏情况下的时间复杂度为O（KMN）

　3实验与分析

　　由于实际的普适机器人系统尚未搭建完成，仅进行了仿真实验，验证本文方法的可行性和有效性.

　　由于充分考虑机器人观测信息，定位精度较高，但由于传感器随机器人运动，导致这两种传统SLAM方法都存在定位误差随运动时间明显增大的缺陷。而本文U-SLAM中，由于传感器网络各节点独立于机器人，解除了观测与机器人运动之间的数据耦合，在明显提高机器人位姿估计精度的同时，估计的稳定性也有大幅改善.

第二组仿真实验进行传感器网络标定精度对比，分别对仅依据运动方程的机器人位姿、依据FastSLAM2.0的机器人位姿，以及本文的普适机器人系统