2016 年 12 月 第 34 卷第 6 期 西北工业大学学报 Journal of Northwestern Polytechnical University Dec． 2016 Vol．34 No．6 一种视觉机器人演示学习的轨迹生成方法 1， 2 1 1， 3 4 李建良 ，杜承烈 ，赵晔 ，禹科 ( 1．西北工业大学 计算机学院，陕西 西安 3．陕西省人民医院，陕西 西安 710129； 2．西北农林科技大学 信息工程学院，陕西 杨陵 712100 710068； 4．中航工业自控所 飞行器控制一体化技术重点实验室，陕西 西安 710065 ) 要： 目前， 机器人演示学习已成为机器人学中最为活跃的研究课题之一，而作为演示学习三要点 之一的轨迹生成便成为研究热点。轨迹生成是决定演示学习是否成功的重要因素，传统上使用 SIFT 摘 算法生成轨迹， 但是这种方法存在很多局限，例如特征点较多、选择轨迹困难、轨迹存在一定噪声等。 PCA 和 UKF 等算法相结合的新的轨迹生成方法， 为此， 提出了一种将 SIFT、 通过实验仿真和机器人实 体运行， 结果表明了算法的有效性。 词： 演示学习； SIFT 算法； 主成分分析； 无迹卡尔曼滤波 中图分类号： TP391 文献标志码： A 文章编号： 1000-2758（ 2016） 06-1082-06 关 键 工业机器人技术是近年来研究的热点之一，其 发展状况是衡量一个国家制造业水平是否先进的主 ［1］ 要标志 。大力发展工业机器人是国家发展制造 业的必由之路，也是新时期顺利实现工业转型升级 ［2］ 的必然要求 。随着“再工业化 ”的不断兴起，高端 的移动轨迹，并加入相应的限制条件生成新的轨迹 点，最后通过 B 样条插值算法生成机器人代码的过 程。本文将 SIFT、PCA 和 UKF 算法相结合，构建新 的轨迹生成方法，并通过实验仿真和机器人实体运 行，验证算法的有效性。 ［3］ ，而高端 智能的发展对机器人演示学习提出了新的挑战 。 智能制造日益成为世界各国的竞争方向 近年来，机 器 人 演 示 学 习 （ learning by demonstration，LBD） 已成为机器人学中最为活跃的研究课 ［4-5］ 题之一 。 它封装了机器人的感知能力、学习模 仿能力、新技能生成能力和适应能力； 可以根据环境 的变化，对于给定的任务，通过自主地结合外部参 ［6-7］ 产生运动轨迹，如学习焊接。 与机器人编程 的传统方法相比，LBD 提供了机器人友好的教学框 数 1 整体设计 LBD 轨迹生成的主要难点，是根据演示视频得 到精确 的 运 动 轨 迹。 文 献［8，12-13］都 提 到 使 用 SIFT 算法来实现演示学习轨迹的生成方法，但 SIFT 特征点较多，会生成大量的轨迹。因此，找出最合理 的轨迹就成为研究的重点。本文提出了一种新的演 架 ，催生了新的研究方向，使机器人可以在社会 ［9］ 环境中与人互动 。 已经有许多研究者试图构建 示学习轨迹生成方法： 首先，使用 SIFT 算法对演示 视频进行特征提取，根据所得到的特征对相邻 2 帧 视频进行匹配，将匹配到的特征点连线生成匹配图 ， ［10］ 如下的模型来建立工业机器人 LBD 学习环境 ： 一个自然的学习环境，它可能是一个复杂的场景，有 并根据其坐标连线生成演示轨迹； 其次，为解决生成 轨迹较多、选择轨迹困难的问题，使用主成分分析法 多人和多个移动目标，在不同的时间和地点发生动 作。在 这 样 的 环 境 下，最 为 关 键 的 是 3 个 基 本 问 PCA（ principal component analysis，PCA） 对生成轨迹 进行主成分提取，同时对轨迹进行降维，这不仅可去 ［11， 13］ 。 轨迹 题———轨迹生成、动作分解和动作重组 生成是指机器人在 LBD 过程中，通过观察人或者移 除噪声，而且可以发现其中的数据模式； 最后，引入 无痕卡尔曼算法，对所提取到的二维轨迹进行滤波 动目标，利用数字图像处理或外部传感器得到目标 和平滑处理，进一步消除噪声、匹配误差。其总体框 收稿日期： 2016-04-21 基金项目： 航空科学基金（ 20150753011） 资助 ［8］ 作者简介： 李建良（ 1971—） ，西北工业大学博士研究生，西北农林科技大学副教授，主要从事 CPS 系统软件工程研究。 李建良，等： 一种视觉机器人演示学习的轨迹生成方法 第6期 · 1083· 像二维平面空间和高斯差分 （ difference of Gaussian， DoG） 尺度空间中，同时检测局部极值，作为关键特 架如图 1 所示。 征点，以使特征点具备良好的稳定性。 高斯差分图 像定义为 2 个不同尺度的高斯核差分，它具有计算 简单的特点，是归一化算子的近似，其计算公式为 D（ x，y，σ） = （ G（ x，y，kσ） － G（ x，y，σ） ） * I（ x，y） = L（ x，y，kσ） － L（ x，y，σ） （ 3） 式中，k 为比例系数。 2） 确定稳定特征点。 通过尺度空间极值检测 得到备用关键特征点后，还需要将低对比度点和边 缘特征点过滤掉，以提高关键点的定位精度和抗噪 声干扰能力，且增加匹配时的稳定性。 3） 确定关键点的方向。 在关键点所在的尺度 上，利用该点邻域像素的梯度方向分布特性 ，为每个 关键特征点指定方向参数，使算子具备旋转不变性。 4） 建立 SIFT 特 征 向 量 描 述 子。 通 过 以 上 步 骤，可以得到每个关键点的位置、尺度和方向信息。 接下来为每个关键点建立一个特征描述子 ，使其对 尺度、旋转、线性光照、仿射变形等宽基线变化具有 图1 一定的不变性。此外，特征描述子还能描述每个关 键点的独特性，使不同特征点的特征向量之间有显 整体处理流程图 著的差异。 2 基于 SIFT 算法的轨迹生成 经上述处理后，SIFT 特征向量己经去除了尺度 变化、旋转等因素的影响。 再继续将特征向量的长 SIFT 是从不变量技术发展而来的一种特征检 度归一化，则可以进一步去除线性光照变化的影响 。 2．3 SIFT 描述子的特性 测方法。 2．1 尺度空间理论 尺度理论主要用来对图像数据的多尺度特征进 行模拟。 二维高斯函数通常可写为 1 －（ x2 +y2） /2σ2 e G（ x，y，σ） = 2πσ （ 1） 式中，σ 是正态分布方差。 SIFT 特征向量具有如下特性： 1） 多量性，即使 少量的图像信息，也可以产生大量的 SIFT 特征； 2） 独特性好，具有丰富的信息，可以用于海量数据的挖 掘； 3） SIFT 特征是图像的局部特征，具有很好的稳 定性。 3 生成轨迹优化 图像与高斯核卷积可以得到不同尺度下的尺度 空间模型 3．1 （ 2） L（ x，y，σ） = G（ x，y，σ） * I（ x，y） 式中，图像的像素位置为 （ x，y） ，σ 作为尺度空间因 子，L 称之为图像的尺度空间。 主成分分析是一种基于 K-L 分解的多元统计 方法，旨在通过研究少量主成分来显示变量之间的 2．2 SIFT 特征提取方法 1 幅图像的 SIFT 特征向量生成算法包括如下 4 步操作： 1） 尺度空间极值检测，初步确定关键点位置和 所在尺度。通过构造尺度空间的高斯金字塔，在图 轨迹主成分提取 内部关系，因此通常将其用来提取多个变量之间的 主要关系。由于基于 SIFT 特征生成的轨迹量较多 且难以确定最优轨迹，因此本文引用 PCA 算法对轨 迹进行提取、优化。 PCA 算法对轨迹处理的步骤如下： 对于一个给定的轨迹训练集，假设训练样本数 西 · 1084· 北 工 业 大 2， 为 N，先 将 每 条 路 径 构 成 一 个 列 向 量 k i ，i = 1， …，N 1） 计算所有轨迹训练集的总平均路径 ： 1 N u = ∑ ki N i=1 （ 4） 2） 将每条路径减去平均路径，得到中心化轨迹 （ 5） 2，…，N） k珋 i = k i － u（ i = 1， 3） 将中心化后的轨迹依次作为列向量 ，构成 矩阵 k珋 …，k珋 A = ［k 1 － u，k 2 － u，…，k N － u］ = ［k珋 1， 2， N］ （ 6） 4） 计算协方差矩阵 S = AA T 的特征值 λ 和特征 向量 v，即 利用奇异值分解定理和 K-L 变换原理，通过求 T 解 A A 的特征值和特征向量，来求解 AA 的特征值 T 和特征向量。 5） AA T 的正交归一化特征向量为 v珋 i = 1 槡λ i Av i （ 7） （ 8） λi ∑ i=1 η 一般取 成立的 k 个特征值对应的特征向量 v珋 i， 大于 90% 的值。 于是可得到 PCA 的投影子空间 W pca W pca = （ v珋 v珋 …，v珋 （ 9） 1， 2， k） 7） 最 后，便 可 以 通 过 （ 10） 式 将 轨 迹 投 影 到 PCA 子空间 Y = W Tpca（ X － u） 程直接求解。该算法的实现不需要对雅可比矩阵进 行显式计算，也不需要对状态方程和观测方程进行 线性化。Unscented 变换实际上是选择一批可表达 系统状态均值和方差的采样点 （ 称为 Sigma 点 ） ，经 过非线性变换使这些采样点的分布以二阶精度逼近 真实均值和方差。 在强非线性情况下，UKF 更能显 示优势。 用 PCA 方法提取的轨迹，由于在进行 SFIT 特 征点匹配时会产生误差，选择 SFIT 特征点时也会产 生误差，故可将这些误差视为噪声，利用 UKF 滤波 方法便能很好地解决此问题。 将离散非线性系统描述为 x t = f（ x t － 1 ，v t － 1 ） y t = h（ x t ，n t ） （ 11） （ 12） 式中： x t 为系统状态向量，y t 为观测向量； v t 、n t 分别 T k l 第 34 卷 报 T T 于 λ i 的 A A 的特征向量。 6） 将计算出来的特征值按降序排列： λ 1 ≥ λ 2 … ≥ ≥ λ l ，然后选择使得 =η 学 为零均值的状态噪声和观测噪声，且存在 E［v i v j ］ = T 式中，λ i 为矩阵 AA 的第 i 个非零特征值，v i 为对应 λi ∑ i=1 学 （ 10） PCA 方法对噪声具有一定的鲁棒性，且具有完 善的理论基础和较好的可操作性，按该方法选择出 来的特征具有最优的表现力，因此利用该算法能够 很好地解决轨迹选择问题。 3．2 轨迹滤波去噪 无迹卡尔曼滤波（ unscented Kalman filter，UKF） 是一种递归贝叶斯估计方法，是基于 Unscented 变 换的一种新的卡尔曼滤波算法。 其基本思想是，首 先将均值和方差参数化，然后利用变换后的状态方 T δ ij Q，E［n i n j ］= δ ij Ｒ，E［v i n j ］= 0。Δ 为脉冲函数，Q 为系统噪声序列的方差阵，为非负定阵，Ｒ 为量测噪 声序列的方差阵。 对于 i，j，将 UT 变换用于扩展 卡尔曼滤波算法统计特性的递推过程，便得到 UKF 滤波算法，其执行过程如下： 1） 初始化 K = 1，将系统状态、状态噪声和观测 a 噪声组成增广状态向量 x ，设其维数为 L，方差为 Pa ： T （ 13） xa = [ xT vT nT ] T x珔0 = E［x 0 ］，P 0 = E［（ x 0 － x珔0 ） （ x 0 － x珔0 ） ］ （ 14） 0] x珔 = [ x珔 0 T 0 a 0 T P0 ，P = 0 0 a 0 0 Q 0 0 0 （ 15） Ｒ 2） 计算采样点的状态向量 a x珋 （ L + λ ） P at－ 1 ] t－1 ± 槡 a χ at－ 1 = [ x珋 t－1 共 有 2L + 1 个 采 样 点。 = [ （ χx） T （ χv） T （ χn） T] （ 16） a 其 中 χ T 3） 时间更新 χ xt| t － 1 = f（ χ xt－ 1 ，χ vt－ 1 ） （ 17） 2L x珋 t| t － 1 = W （i m） χ xi，t| t － 1 ∑ i=0 （ 18） 2L P t| t － 1 = W （i m） ［χ xi，t| t － 1 ∑ i=0 T － x珋 ［χ xi，t| t － 1 － x珋 t| t － 1］ t| t － 1］ （ 19） γ t| t － 1 = h（ χ xt－ 1 ，χ nt－ 1 ） （ 20） 李建良，等： 一种视觉机器人演示学习的轨迹生成方法 第6期 2L y珋 t| t － 1 = W （i m） γ i，t| t － 1 ∑ i=0 （ 21） 4） 观测更新 P y ty t = W ∑ i=0 （ c） i 珋 （ γ i，t| t － 1 － y珋 t| t － 1 ） （ γ i，t| t － 1 － y t| t － 1 ） 2L W （i c） （ χ xi，t| t － 1 ∑ i=0 － x珋 珋 t| t － 1 ） （ γ i，t| t － 1 － y t| t － 1 ） T （ 23） Kt = P x ty t （ 24） P x ty t （ 25） x珋 珋 珋 t = x t| t － 1 + K t （ y t － y t| t － 1 ） P t = P t| t － 1 － K t P y ty t K Tt （ 26） UKF 适用于任何能用 EKF 解决的非线性问题。 UKF 不需要计算雅可比矩阵，与 EKF 计算量相当； UKF 在计算的过程中，没有像 EKF 一样忽略高阶 图2 实验分析 T （ 22） P x ty t = 量，因此具有比 EKF 更高的精度。 4 2L · 1085· 实验分 2 个阶段： 轨迹生成阶段和机器人实体 验证阶段。轨迹生成实验使用的电脑 CPU 为 Intel Core（ TM） i5-4200U（ 4 CPUs） ，CPU 的处理频率为 1．60 GHz； 实验软件为 Matlab 2015a； 实验机器人为 新时达 SD500 型机器人； 实验数据为在实验室条件 下使用摄像机（ 本文仿真分析使用的是双目摄像头 中右侧摄像头所拍摄视频 ） 所拍摄的一段长度为约 为 5 s、帧频为 29 帧 / s 的视频（ 为突出 SIFT 特征点， 在拍摄时演示者手带白色手套，并在白色手套上涂 上黑色标记） ，视频帧数为 154，视频分辨率为 640* 480（ 部分视频图像如图 2 所示，图 2 中的 a） ～ d） 分 别和图 3、图 4 中的图像一一对应） 。 演示视频部分图像 图3 演示视频 SIFT 特征提取结果 图4 演示视频 SIFT 特征匹配结果 按照第一节所描述流程，首先进行 SIFT 特征提 取， 提取特征如图 3 所示，SIFT 特征匹配结果如图 4 西 · 1086· 北 工 业 大 学 学 报 第 34 卷 所示。从 SIFT 特征提取和 SIFT 特征匹配结果可以 看出，存 在 一 些 错 误 的 轨 迹 点 （ 如 瓶 身 上 的 反 光 从图 5 可以看出，PCA 提取到 了 轨 迹 的 主 成 分，且消除了一部分误差，但提取到的主成分“锯 点） 。为获得合理的轨迹并消除这些点对轨迹造成 的误差，引入 PCA 算法对生成的二维轨迹进行主成 齿”比较严重，这些误差不利于机器人的运行，因此 引入 UKF 对二维轨迹进行平滑处理，平滑处理结果 分提取，提取结果如图 5 所示。 如图 5 所示。 完成轨迹优化之后，利用双目标定原理和方法 完成 2D 图像轨迹到 3D 空间轨迹的转换。 在实验 中，为了消除算法对轨迹生成的精度的影响，将 3D 轨迹最低点与实验对象重心重合，对 3D 进行处理。 处理完成的轨迹在工业机器人实体上进行运行 ，运 行结果表明： 机器人能够按照预设轨迹完成对物体 图5 的抓取、移动和松绑，可以满足演示学习研究的需 要，实体运行结果如图 6 所示。 生成轨迹以及优化结果 图6 5 结 机器人实体验证 论 轨迹生成是决定演示学习是否成功的重要因 素，传统上使用 SIFT 跟踪直接生成轨迹。 但是，这 种方法存在很多缺点，例如特征点较多、选择轨迹困 难、轨迹存在一定噪声等等。因此，本文提出了一种 将 SIFT、PCA 和 UKF 等算法相结合的轨迹生成方 法。首先，通过 SIFT 特征点，对视频图像进行匹配， 生成初始轨迹； 然后利用 PCA 算法和 UKF 算法，对 轨迹进行优化处理，生成最优轨迹。 本文通过将由 此所得的最优轨迹在机器人上进行实体运行 ，结果 表明本文所提出的轨迹生成方法相当有效 。 参考文献： 2014（ 9） ： 1-13 ［1］ 王田苗，陶永． 我国工业机器人技术现状与产业化发展战略［J］． 机械工程学报， Wang Tianmiao，Tao Yong． Ｒesearch Status and Industrialization Development Strategy of Chinese Industrial Ｒobot［J］． Journal of Mechanical Engineering，2014（ 9） ： 1-13 （ in Chinese） ［2］ 李睿，曲兴华． 工业机器人运动学参数标定误差不确定度研究［J］． 仪器仪表学报， 2014（ 10） ： 2192-2199 Li Ｒui，Qu Xinghua． Study on Calibration Uncertaintyof Insudtrial Ｒobot Kinematic Parameters［J］． Chinese Journal of Scientific Instrument，2014（ 10） ： 2192-2199 （ in Chinese） ［3］ 管贻生，邓休，李怀珠，等． 工业机器人的结构分析与优化［J］． 华南理工大学学报（ 自然科学版） ， 2013（ 9） ： 126-131 Guan Yisheng，Deng Xiu，Li Huaizhu，et al． Structural Analysis and Optimization of Industrial Ｒobot［J］． Journal of South China University of Technology（ Natural Science Edition） ，2013（ 9） ： 126-131 （ in Chinese） 李建良，等： 一种视觉机器人演示学习的轨迹生成方法 第6期 · 1087· ［4］ Billard A，Calinon S，Dillmann Ｒ，et al． Ｒobot Programming by Demonstration，Springer Handbook of Ｒobotics［M］． Springer Berlin Heidelberg，2008： 1371-1394 ［5］ Ｒozo Leonel，Jimenez Pablo，Torras Carme． A Ｒobot Learning from Demonstration Framework to Perform Force－Based Manipulation Tasks［J］． Intelligent Service Ｒobotics，2013， 6（ 1） ： 33-51 ［6］ Demiris Y，Aziz-Zadeh L，Bonaiuto J． Information Processing in the Mirrorneuron System in Primates and Machines［J］． Neuroinformatics，2014（ 12） ： 63-91 ［7］ Suleman，Khawaja M U，Awais Mian M． Learning from Demonstration in Ｒobots Using the Shared Circuits Model［J］． IEEE Trans on Autonomous Mental Development，2014， 6（ 4） ： 244-258 ［8］ Pagliuca Paolo，Nolfi Stefano． Integrating Learning by Experience and Demonstration in Autonomous Ｒobots［J］． Adaptive Behavior，2015， 23（ 5） ： 300-314 ［9］ Peters J，Vijayakumar S，Schaal S． Ｒeinforcement Learning for Humanoid Ｒobotics［C］∥Proceedings of the 3rd IEEE-ＲAS International Conference on Humanoid Ｒobots（ Humanoids） ，Karlsruhe，Germany，2003： 1-20 ［10］Yoshikawa Y，Shinozawa K，Ishiguro H，et al． Miyamoto． Ｒesponsive Ｒobot Gaze to Interaction Partner［C］∥Proceedings of the 2006 Ｒobotics Science and Systems Conference，2006： 287-293 ［11］Wu Yan，Su Yanyu，Demiris Yiannis． A Morphable Template Framework for Ｒobot Learning by Demonstration： Integrating OneShot and Incremental Learning Approaches［J］． Ｒobotics and Autonomous Systems，2014， 62（ 10） ： 1517-1530 ［12］ Mitic Marko； Miljkovic Zoran． Neural Network Learning from Demonstration and Epipolar Geometry for Visual Control of A Nonholonomic Mobile Ｒobot［J］． Soft Computing，2014， 18（ 5） ： 1011-1025 ［13］ Vukovic Najdan，Mitic Marko，Miljkovic Zoran． Trajectory Learning and Ｒeproduction for Differential Drive Mobile Ｒobots Based on Gmm / Hmm and Dynamic Time Warping Using Learning from Demonstration Framework［J］． Engineering Applications of Artificial Intelligence，2015， 45： 388-404 One Path Generation Method of Visual Ｒobot Based on BLD Li Jianliang 1，2 ，Du Chenglie 1 ，Zhao Ye 1，3 1．School of computer science，Northwestern Polytechnical University，Xi'an 710129，China 2．College of Information Engineering，Northwest A＆F University，Xi'an 712100，China 3．Shaanxi Provincial People's Hospital，Xi'an 710069，China 4．Science and Technology on Aircraft Control Laboratory，FACＲI，Xi'an 710065，China Abstract： Ｒobot learning by demonstration has become one of the most active research topics in robotics，and trajectory generation as one of the three points of demonstration study has become a hot research topic． Trajectory generation is decided to demonstrate learning an important factor in the success，the traditional method using sift tracking direct trajectory generation，but this method exists many shortcomings，such as more feature points selected tracks difficulties，trajectory there are certain noise，and so on． In this paper，a combination of SIFT，PCA and UKF algorithm algorithm based on trajectory generation method，the simulations and experiments with a real robot operation proves the validity of the algorithm． Keywords： learning by demonstration； scale invariant feature transform； principal component analysis； unscented kalman filter