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

　　机器人定位、传感器网络标定与环境建图是普适机器人系统中三个相互耦合的基本问题，其有效解决是普适机器人系统提供高效智能服务的前提。本文提出了普适机器人系统同时机器人定位、传感器网络标定与环境建图的概念，通过分析三者之间的耦合关系，给出同时定位、标定与建图问题的联合条件概率表示，基于贝叶斯公式和马尔科夫特性将其分解为若干可解项，并借鉴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］提出了一种基于R