基于三焦张量和关键帧策略的移动机器人位姿估计方法
【技术领域】
[0001] 本发明属于机器人与计算机视觉的交叉领域，涉及基于视觉的移动机器人位姿估 计问题，尤其涉及基于三焦张量和关键帧策略的移动机器人位姿估计方法。
【背景技术】
[0002] 随着机器人技术的迅速发展，机器人在实际中扮演着重要的角色，其承担的任务 也更加复杂多样。特别的，对于移动机器人，传统的方法往往基于GPS、里程计等位置测量设 备对其进行控制，其控制精度受到传感设备精度的限制，而且任务的灵活度较差，需要给定 机器人在物理空间中的参考轨迹，增加了实现难度和成本。视觉伺服控制利用视觉信息作 为反馈，对环境进行非接触式的测量，利用更大的信息量，提高了机器人系统的灵活性和 精确性，在机器人控制中具有不可替代的作用。
[0003] 经典的视觉伺服控制方法可以分为基于位置、图像和几何约束的方法，主要体现 为误差系统构造方式的不同。根据以往的综述（贾丙西，刘山，张凯祥，陈剑.机器人 视觉伺服研究进展：视觉系统与控制策略，自动化学报，2015, 41 (5):861-873)，基于图 像和几何约束的方法对图像噪声和相机标定误差的鲁棒性更好，并且需要较少的先验知 识，应用范围更广。但是，对于大部分视觉伺服系统而言，一般都要求在控制过程中目标 特征点至少部分在视野内，从而构造有效的视觉反馈。然而，由于相机的视野范围有限， 而且现有图像特征提取的方法在发生大的旋转/平移时重复性和精确度并不好，从而影 响了视觉伺服系统的精度，限制了其工作空间。另外，对于移动机器人而言，大部分的移 动机器人都具有非完整约束，即具有三个空间自由度，但是只有两个控制自由度。以往 的研究中有学者（Salaris P, Fontanelli D, Pallottino L，et al. Shortest paths for a robot with nonholonomic and field-of-view constraints, IEEE Transactions on Robotics，2010, 26 (2) : 269-281)针对非完整约束移动机器人提出了保证目标可见的轨迹 规划策略，但是需要一定的先验知识，而且规划得到的路径往往是曲折的，这在实际中降低 了机器人的工作效率。
[0004] 视觉伺服中常用的多视图几何约束主要包括单应性、对极几何、三焦张量等，基于 多视几何的视觉伺服一般都涉及对机器人的姿态进行估计。其中，基于单应性的视觉伺服 通常结合三维空间信息与图像信息进行误差系统的构建，但在对单应性矩阵进行分解时存 在多解问题（四组解），往往需要目标平面的先验知识，为了求解方便往往要求特征点共 面；基于对极几何的方法利用两个视图之间的极点信息估计机器人的相对姿态，但是当两 个视图邻近时会出现奇异，并且当两个极点都调整到零时存在位姿的歧义，只能保证二者 方向相同且共线，需要切换到其他控制策略从而调整到目标位姿。基于三焦张量的方法利 用三个视图的对应关系对机器人的姿态进行估计，与场景的结构无关，而仅与视图之间的 相对姿态有关，具有更好的普适性。然而，相比于单应性和对极几何这种两视图几何的方 法，三焦张量要求三个视图中具有匹配的特征点，对视野约束的要求更强。另外，特征点提 取和匹配的方法在发生旋转和平移变换时的重复性和精度并不好，影响了视觉伺服系统的 精度，也限制了机器人视觉伺服的工作空间。
【发明内容】
[0005] 为了克服以往技术的不足，本发明针对视觉伺服跟踪控制问题提出了基于三焦张 量和关键帧策略的移动机器人位姿估计方法，以及根据该方法的视觉伺服跟踪系统和移动 机器人。
[0006] -种基于三焦张量和关键帧策略的移动机器人位姿估计方法，用于移动机器人的 视觉轨迹跟踪任务，利用移动机器人在示教的过程拍摄的环境图像作为期望轨迹，从期望 轨迹中选取关键帧，然后基于三焦张量对三个视图进行几何模型构建，从而提取位姿信息； 所述的三个视图为当前视图C和其它任意两个视图C。，Cf。
[0007] 所述的几何模型构建过程为，对于当前视图C，通过计算与另外两个视图C。，Cf的 三焦张量，从而得到当前视图相对于(T的位姿信息，包括角度信息和含未知比例因子的位 置信息。
[0008] 从所述期望轨迹中选取的包含初始视图和最终视图的图像序列作为关键帧，且相 邻的关键帧之间具有预设阈值的匹配度；在位姿估计的过程中，首先计算当前视图与两个 最匹配的关键帧之间的三焦张量，得到相对于最匹配关键帧的位姿信息，再迭代变换到最 终视图下，得到全局坐标下连续的位姿测量。
[0009] -种采用根据任一项方法的视觉伺服跟踪系统。
[0010] -种采用根据任一项方法的移动机器人。
[0011] 本发明的有益效果：
[0012] 针对大范围的视觉伺服任务，不需要当前视图与初始视图和最终视图之间存在匹 配的特征点，从而大大扩展了机器人视觉伺服的工作空间；由于相邻关键帧之间具有接近 预设阈值的匹配率，因此基于关键帧的位姿估计算法中三焦张量的计算更加精确，提高了 系统的精度。
【附图说明】
[0013] 图1是视觉伺服跟踪任务示意图；
[0014] 图2是三焦张量原理图；
[0015] 图3是基于关键帧的位姿估计不意图；
[0016] 图4是关键帧之间位姿变换示意图。
【具体实施方式】
[0017] 以下结合【具体实施方式】并对照附图对本发明加以详细说明。
[0018] 本发明针对一般化的场景，利用三焦张量描述初始视图、当前视图和最终视图之 间的几何关系，并基于此得到机器人含未知比例因子的位姿信息。针对大范围的视觉伺服 跟踪问题，充分利用三焦张量与环境结构无关这一特点，提出了基于关键帧的姿态估计方 法，先计算当前视图相对于两个最相似关键帧的位姿信息，再迭代变换到最终视图下，得到 全局坐标下连续的位姿测量。基于三焦张量的移动机器人位姿估计方法，主要用于基于视 觉的轨迹跟踪问题。这一类问题一般分为示教和跟踪两个环节。如图1，在示教环节，机器 人在运动的过程中拍摄环境的图像作为期望轨迹；在跟踪环节，机器人基于视觉反馈跟踪 期望图像从而完成期望轨迹的跟踪，该跟踪过程主要分为位姿估计和控制两个部分。本发 明提出包含未知比例因子的位姿估计方法，再利用现有的方法易于设计出相应的自适应控 制器，完成轨迹跟踪任务，如（Chen J，Dixon W E，Dawson D M，et al.Homography-based visual servo tracking control of a wheeled mobile robot[J]. Robotics, IEEE Transactions on, 2006, 22(2) :406-415.)。针对移动机器人的位姿估计，本发明主要分为 几何模型构建和基于关键帧的位姿估计两部分。1、几何模型
[0019] 如图2,三焦张量IT1I1 = U3描述了三个视图之间的相互关系，且与环境结构无关。 对于三幅图像Ic，I，Γ中相应的特征点X <：，X，X%具有如下的关系：
[0020]
[0021] 其中，x'x彳,#分别表不X。，X，乂飞勺第i, j, k个元素 ，T iqr表示T i中第q行第r列 的元素，e.]qs，￡^为排列符号，定义如下：
[0022]
[0023] 特别的，对于平面移动机器人，考