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

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

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

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

　　信息融合意义下的状态估计问题。本文采用序贯方式融合两类观测信息进行状态的观测更新，针对每一类观测，当同时存在多个该类观测时，仍然采用序贯方式加以融合，如对于步骤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的机器人位姿，以及本文的普适机器人系统U-SLAM中联合机器人位姿与环境特征这三类传感器网络标定方法进行对比，三类方法各运行50次，得到的传感器网络标定误差的数学期望和方差如图6所示。可以看出，仅依据运动方程的方法，机器人位姿估计误差随运动距离迅速增加，得到的标定结果误差很大，难以满足要求;依据FastSLAM2.0算法能够有效减少机器人位姿估计误差，从而提高了节点标定的精度，但由于仅考虑了节点对机器人的观测，产生的标定误差仍然较高。而本文U-SLAM中，传感器网络各节点标定、机器人位姿估计相对独立，解除了观测与机器人运动之间的数据耦合，在明显提高机器人位姿估计精度的同时，传感器节点标定的精度和稳定性也得到了大幅改善。
 

 

　　中融合了机器人的观测信息，但机器人感知范围有限而未察觉目标变动，仍以先前获取Lm的位置和当前观测来推算当前位姿，从而导致定位错误。对于FastSLAM1.0算法，机器人定位只根据自身控制信息完成，未融合观测信息从而避免受动态环境的影响。而本文U-SLAM中，通过传感器网络监测动态环境特征的变化，并将其告知移动机器人，避免了动态路标对机器人定位的影响，并获得了比FastSLAM1.0更高的定位精度。

　　

　　上述实验均在WindowsXP系统下进行，计算主机的CPU采用Pentium42.4GHz，内存为1G.在该配置下运行EKF-SLAM的平均耗时为12.3s，UKF-SLAM的平均耗时为15.4s，FastSLAM1.0平均耗时为80.7s，FastSLAM2.0的平均耗时为135.4s.本文U-SLAM方法运行的平均耗时为280.8s，虽然实时性较差，但仍能满足实际系统的需要，并且重要的是，能够在定位和建图过程中在线完成传感器网络标定。实验中没有进行本文方法与传统先离线标定后在线定位建图的对比，主要原因是定位和建图的精度很大程度上取决于标定的精度，而在传统方法中离线标定的精度本身随标定方法和策略差异很大。

　　

　　4结论

　　本文针对普适机器人系统提供服务的基础工作，即机器人定位、传感器网络标定和环境建图三个互相耦合的关键问题展开研究，提出了普适机器人系统同时定位、标定与建图的概念，给出了同时机器人定位、传感器网络标定与环境建图问题的联合条件概率表示，基于贝叶斯公式和马尔科夫特性将其分解为若干可解项，并由Rao-Blackwellized粒子滤波迭代求解，设计了位姿粒子的采样提议分布和权值更新公式，并给出了机器人定位、传感器网络标定和环境建图的递推公式。

　　作为人们日常生活和工作的重要场所，机器人的服务环境往往较为复杂，其中目标往往具有不同程度的动态性。在这种动态服务环境下普适机器人系统具有独特的优势，将会发挥更显著的作用。在完成传感器网络标定和环境地图的初步构建之后，如何通过机器人和传感器网络的交互共同维护随环境动态更新的特征地图是下一步的研究内容.