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

　　机器人定位、传感器网络标定与环境建图是普适机器人系统中三个相互耦合的基本问题，其有效解决是普适机器人系统提供高效智能服务的前提。本文提出了普适机器人系统同时机器人定位、传感器网络标定与环境建图的概念，通过分析三者之间的耦合关系，给出同时定位、标定与建图问题的联合条件概率表示，基于贝叶斯公式和马尔科夫特性将其分解为若干可解项，并借鉴Rao-Blackwellized粒子滤波的思想分别求解。首先，联合传感器网络对机器人的观测、机器人对已定位环境特征的观测以及机器人自身控制量，设计了位姿粒子的采样提议分布和权值更新公式;其次，联合传感器网络对机器人运动轨迹及已定位环境特征的观测，设计了传感器网络标定的递推公式;然后，联合传感器网络和机器人对（已定位或新发现）环境特征的观测，设计了环境建图的递推公式。给出了完整的同时定位、标定与建图算法，并通过仿真实验验证了该算法的有效性。

　　在机器人研究领域中，普适机器人系统是传感器网络技术与服务机器人技术的交叉领域［1−3］，通过将这两种技术有效结合实现了两者的优势互补：一方面，传感器网络可视为机器人感知能力的延伸，遍布整个环境的传感器网络能够为机器人提供全局感知能力，弥补机器人对全局环境感知能力的缺陷;另一方面，机器人可视为传感器网络的执行机构，从而使传感器网络具备了主动服务能力。因此，普适机器人系统在面积较大、动态性较强的服务环境，如家庭、医院、展览馆等具有重要应用前景。

　　众所周知，自身定位和环境地图构建是移动机器人进行环境认知及路径规划，并最终提供高效智能服务的基础［4］。对普适机器人系统而言，传感器网络标定同样是保证其高效工作的重要一环，已标定传感器网络能够实时定位机器人和环境动态目标，并据此动态更新环境地图。事实上，不知道传感器节点位置而采集的数据在实际应用中并没有太大意义［2］。在将普适机器人系统引入某一服务环境伊始，机器人定位、传感器网络标定和环境建图就成为其面临的三个基础问题［3］。其中定位贯穿于机器人工作的始终，而传感器网络标定和环境地图构建则在普适机器人系统正常工作之初完成，并在工作过程中随网络节点或环境变化而实时更新。此外，分析可知，普适机器人系统中机器人定位、传感器网络标定和环境建图三个问题相对独立而又互相耦合：一方面，传感器网络提供全局观测，能辅助机器人完成动态环境下的定位;另一方面，传感器网络的精确标定是其辅助机器人定位的前提。更进一步地，通过对已建地图的观测，将有助于提高传感器网络标定精度和机器人定位精度。若仅探讨机器人定位、环境建图二者之间耦合关系，则退化为目前国内外学者广泛研究的同时机器人定位与环境建图（SimultaneouslocalizaTIonandmapping，SLAM）问题。

　　在同时机器人定位与环境建图方面，利用概率方法解决SLAM问题是目前的研究方向和热点［4］，其中，基于扩展卡尔曼滤波（ExtendedKalman?l-ter，EKF）的SLAM方法不适于解决存在非高斯噪声情况的估计问题［5］;粒子滤波方法适用于非线性和非高斯情形，但这种方法在问题维数较高时计算量过大，难以满足系统实时性要求;Rao-Blackwellized粒子滤波用扩展卡尔曼滤波处理非线性部分，用粒子滤波处理非高斯部分，因此同时具备扩展卡尔曼滤波和粒子滤波的优势，并已被成功应用于SLAM中［6−8］。然而，由于传感器机载，机器人观测误差与运动误差相耦合，定位和建图误差会随机器人运动距离发生不可避免的扩散［9］。文献［10］提出了基于粒子滤波的无线传感器网络辅助SLAM方法，用于解决求解问题空间维数高和多数据关联问题;文献［11］提出了基于传感器网络分布式感知的移动机器人SLAM方法，来创建大规模环境的精确地图;文献［12］提出了无线传感器网络环境下基于粒子滤波的移动机器人SLAM算法。上述方法虽然都体现了利用传感器网络的全局观测来辅助SLAM的思想，但都尚未考虑传感器网络节点的标定问题，而该问题的有效解决是传感器网络辅助机器人定位和环境特征建图的基础。

　　在同时传感器网络标定与机器人定位方面，文献［13］提出了一种基于机器人的摄像机网络在线自标定方法，以此为基础，文献［14］提出了一种分布式感知协作的移动机器人MonteCarlo定位方法，利用已完成标定传感器网络的观测来辅助机器人定位。事实上，类似于SLAM问题，传感器网络标定与机器人定位可以同时进行（即同时传感器网络标定与机器人定位），针对该问题，文献［15］提出了一种基于Rao-Blackwellized粒子滤波的同时传感器网络标定与机器人定位方法，但对于联合机器人定位、传感器网络标定和环境建图的研究，目前尚未展开。

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

　　1系统描述

　　1.1系统构成

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

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

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

　　2.1基本思想

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