基于CAD模型的立体视觉目标运动估计

第 32 卷第 3 期 2002 年 5 月 　 东南大学学报 ( 自 然 科 学 版 ) JOURNAL OF SOUTHEAST UNIVERSITY (Natural Science Edition) 　 Vol132 No13 May 2002 　　　　　　 基于 CAD 模型的立体视觉目标运动估计 罗　翔1 　倪受东2 　颜景平1 (1 东南大学机械工程系 ,南京 210096) (2 南京工业大学机械工程学院 ,南京 210037) 摘要 : 提出了一种基于 CAD 模型的目标运动估计和目标轨迹实时生成方法. 分为两步 :首先 采用立体视觉系统由图像特征点求出目标特征点在每个采样周期的的空间位置 ;然后由其匹 配模型的特征点集 ,通过扩展卡尔曼滤波方法估计目标运动参数. 有了上述参数 ,就可以设计 与伺服系统带宽匹配、消除图像处理延时影响的目标轨迹生成器. 文中对目标的运动估计算法 进行了仿真研究 ,结果表明该算法是可行的. 文章创新之处在于推导了双目立体视觉系统的运 动估计算法. 关键词 : 机器人视觉 ;运动参数估计 ;扩展卡尔曼滤波 中图分类号 : TP24 　　文献标识码 : B 　　文章编号 : 1001 - 0505 (2002) 0320397205 Kinetic parameters estimation of objects based on the CAD models by a stereo vision system Luo Xiang1 　Ni Shoudong2 　Yan Jingping1 (1Department of Mechanical Engineering , Southeast University , Nanjing 210096 , China) (2College of Mechanical , Nanjing University of Industry , Nanjing 210037 , China) Abstract :　A scheme of kinetic parameters estimation and real2time track occurrence of objects based on the CAD models is investigated. The scheme has two steps. Firstly , the 3D positions of feature points of objects are obtained in every sample interval by a two2camera stereo vision system. Secondly , an extended Kalman filter is employed to estimate the kinetic parameters. With these parameters , a real2time track oc2 curring rule is presented to match the bandwidth of the robot servo system and eliminate the influence of the delay involved in vision system. A simulation of kinetic parameters estimation of an object demon2 strates the feasibility. The algorithm of kinetic parameters estimation by stereo vision systems is original . Key words :　robot vision ; kinetic parameters estimation ; extended Kalman filters 　收稿日期 : 2002201210. 　基金项目 : 211 工程资助项目 (2102002001) . 　作者简介 : 罗　翔 (1968 —) ,男 ,讲师 ,博士 ,luox @jlonline. com ; 颜景平 (联系人) ,男 ,教授 ,博士生导师. 　　在视觉反馈机器人系统[1 ,2]中 ,对目标的稳定视 觉跟踪是系统正常运行的前提. 对一个完全没有先 验知识的操作目标 ,其模型的辨识和三维重构相当 复杂.在实际的应用系统中 ,如机械装配、零件搬运 等 ,大多数对象的模型是精确已知的. 因此 ,采用基 于 CAD 模型的视觉跟踪方法是一个合理的选择. 文 献[3]详细介绍了基于 CAD 模型的机器视觉原理. 在基于 CAD 的目标跟踪研究中 ,主要问题有 : 如何选择适当的图像和模型特征来正确描述目标 ; 如何有效地预估目标位置和消除噪声以达到跟踪 的目的. Janabi2Sharifi[4 ]系统地研究了图像特征的 选择问题以及基于 CAD 模型的特征自动选择策 略. Wilson[5 ]研究了手 眼系统的目标跟踪 ,采用扩 展卡尔曼方法对刚体三维运动进行滤波和预测 ,由 于使用的是单目主动视觉系统 ,必须在预测递推式 中嵌入基于模型的特征间的几何约束 ,因此预测算 法相当复杂. 本文研究了采用双目平行配置立体视觉系 统[6 ]的目标运动参数估计问题. 该方法分为两部 : 首先采用立体视觉系统由图像特征点求出目标特 征点集的空间位置时间序列 ;然后由其匹配模型的 特征点集 ,通过扩展卡尔曼滤波方法估计目标运动 参数. 在上述参数基础上 ,设计了与伺服系统带宽 匹配 ,消除图像处理延时影响的目标轨迹生成器. 1 　立体视觉系统描述 采用双摄像机光轴平行配置组成立体视觉系统 如图 1 所示. 小孔模型摄像机 1 的图像中心在 oc1 ; 摄像机2的图像中心在 oc2 ; oc1 与 oc2 的距离为 d ;焦 距均为 fo . 并设参考坐标系与摄像机 1 坐标系重合. 图 1 　双目立体视觉系统 设空间点 E( x , y , z) 在2个图像平面的投影分 别为 E′( x1 , y1) 和 E″( x2 , y2) . 其投影关系为 ωx1 ωy1 ωx2 ωy2 ω = f 0 0 0 0 f 0 0 f 0 0 - df 0 f 0 0 0 0 - 1 f x y z 1 (1) 式中 ,ω为非零系数. 由点 E 在两图像平面坐标也 可确定点的空间位置如下 : x = x1 1 + x2 - x1 + d x2 - x1 y = y1 1 + x2 - x1 + d x2 - x1 z = f ( x2 - x1 + d) ( x2 - x1) (2) 2 　CAD 模型在机器视觉中的应用 2. 1 　基于 CAD 模型的目标辨识思想 　　CAD 模型可以为机器人视觉系统提供十分有 用的信息 ,如操作对象的表面、边界及连接关系的 定性和定量描述. 而立体视觉系统则可以从图像中 提取丰富的三维信息 ,如面、线段和孔等的定性和 定量描述. 如图 2 所示 ,基于 CAD 模型的目标位置 和运动估计就是通过对图像和模型特征的比较和 匹配 ,辨识和提取目标位置和运动信息的方法. 详 细的理论可参见文献[3 ]的论述. 图 2 　基于 CAD 模型的机器视觉框图 2. 2 　模型和图像的特征提取 最常用的 CAD 模型表达形式是边界表示法 (B2 Rep) .通常可以认为模型的基元为面 ,采用点 线 面 三级结构来描述模型.有时也可以采用一些易检测、 不存在多解情况的特征或特征组合来表达模型. 在图像上可以通过边缘检测、链码跟踪、霍夫 变换等的方法寻找图像特征. 其最常用的模型特征 为角点和圆孔. 可以通过检测与角点相连的 2 条直 线来获得角点. 这种方法具有较好的鲁棒性. 同时 圆孔由于易辨识 ,测量误差对姿态不敏感 ,也常被 采用. 在此基础上采用点 线 面三级结构的图像数 据快速匹配方法进行模型匹配. 当图像和模型匹配完成后 ,通过跟踪特征的方 法跟踪目标成为视觉跟踪的关键. 通常 ,通过对刚体 上不共线的 3 个特征进行跟踪可以完全确定其空间 位置.但由于光照和图像噪声等的影响可能会造成 图像特征的丢失 ,一般选取 4～7 个特征进行跟踪. 3 　基于扩展卡尔曼滤波的目标跟踪 3. 1 　图像特征扩展卡尔曼滤波器 　　对于一个目标 ,已知其 CAD 模型和模型上的 一个包含 m 个特征点的特征点集 : w o 1 , w o 2 , ⋯, woi , ⋯, wom 其中 , woi = xoi yoi zoi T. 为模型第 i 个特征点在 模型空间的坐标. 通过立体视觉系统 ,可以得到一组时间序列的 空间特征点集 W (1) , W (2) , ⋯, W ( k) , ⋯ 式中 , W( k) = w1 ( k) w2 ( k) ⋯ wm ( k) T , 设 T 为图像采样周期 , W( k) 为 kT 时刻空间特征点集 , wi ( k) = xi ( k) yi ( k) zi ( k) T , i ∈[1 ,2 , ⋯, m ] , 代表 kT 时刻第 i 个特征点空间位置测量值. 已知特征点的集合以及其对应的 CAD 模型可 以通过各种方法求取目标的空间位置 ,如Arun[7 ] 提 出的一种基于奇异值分解的方法. 对于视觉伺服机 器人系统来讲 ,图像处理本身是一个随机过程 ,掺杂 了各种噪声 ,同时图像处理存在延时 ,因此 ,滤波和 预测是必不可少的. 本文采用扩展卡尔曼滤波器对 目标的特征点集序列进行预测、滤波. 从而直接得到 893 东南大学学报 (自然科学版) 第 32 卷 目标的运动状态. 关于扩展卡尔曼滤波的理论可参 见文献[8 ] ,图 3 为扩展卡尔曼滤波器的框图. 图 3 　基于扩展卡尔曼滤波的视觉跟踪框图 　　 取系统的状态向量为目标的运动状态 S = x Ûx y Ûy z ÛzαÛαβÛβγÛγ T ∈ R12 式中 , x , y , z 分别代表模型坐标系原点在参考坐标 系中的位置 ;α,β,γ分别代表模型坐标系的偏转、 俯仰、横滚角度. 离散系统的动力学状态模型为 S ( k + 1) = AS ( k) + u ( k) (3) 式中 A = 1 T 0 0 ⋯ 0 0 0 1 0 0 ⋯ 0 0 0 0 1 T ⋯ 0 0 0 0 0 1 ⋯ 0 0 … … … … … … 0 0 0 0 ⋯ 1 T 0 0 0 0 ⋯ 0 1 12×12 u 为零均值高斯分布的过程噪声 ,具有正定协 方差矩阵 Q . 输出模型为测量值和目标运动状态之间的关 系.在本系统中 ,这一关系是通过特征点在工作空间 的坐标与特征点在模型空间坐标的变换关系表达的 wi ( k) = gi ( S ( k) ) = x + cαcβx o i + ( cαsβsγ - sαcγ) yoi + ( cαsβcγ + sαsγ) zoi y + sαcβxoi + ( sαsβsγ + cαcγ) yoi + ( sαsβcγ - cαsγ) zoi z - sβx o i + cβsγyoi + cβcγzoi 式中 , s·= sin (·) ; c·= cos (·) . 假设每个时间间隔内图像特征点运动速度不 变. 以 W ( k) 为测量向量 ,测量噪声为 v ,呈零均值 高斯分布 ,具有正定协方差矩阵 Ro 输出方程为 W ( k) = G ( S ( k) ) + v ( k) (4) 式中 , G ( S ( k) ) 表达目标运动状态与空间特征点 集序列的约束关系 G( S ( k) ) = g1 ( S ( k) ) g2 ( S ( k) ) … gm ( S ( k) ) 初始状态 S (0) 为高斯分布变量 , 均值为 S^ (0 0) ,表示根据 0 时刻的输出对 0 时刻状态的估 计值 ,余类推. 协方差为 P^ (0 0) . 预测方程 S^ ( k k - 1) = AS^ ( k - 1 k - 1) P ( k k - 1) = AP ( k - 1 k - 1) AT + Q ( k) (5) 输出方程局部线性化 H ( k) = 5 G ( S ( k) )5 ( S ( k) ) T S^ ( k k - 1) = H1 ( k)H2 ( k)…Hi ( k) … Hm ( k) (6) 式中 Hi ( k) = hi ,1 ,1 ( k) hi ,1 ,2 ( k) ⋯ hi ,1 ,12 ( k) hi ,2 ,1 ( k) hi ,2 ,2 ( k) ⋯ hi ,2 ,12 ( k) hi ,3 ,1 ( k) hi ,3 ,2 ( k) ⋯ hi ,3 ,12 ( k) 　hi ,1 ,1 ( k) = hi ,2 ,3 ( k) = hi ,3 ,5 ( k) = 1 　hi ,1 ,7 ( k) = - sαcβxoi + ( - sαsβsγ - cαcγ) yoi + ( - sαsβcγ + cαsγ) zoi 　hi ,1 ,9 ( k) = - cαsβxoi + cαcβsγyoi + cαcβcγzoi 　hi ,1 ,11 ( k) = ( cαsβcγ + sαsγ) yoi + ( - cαsβsγ + sαcγ) zoi 　hi ,2 ,7 ( k) = cαcβxoi + ( cαsβsγ - sαcγ) yoi + ( cαsβcγ + sαsγ) zoi 　hi ,2 ,9 ( k) = - sαsβxoi + sαcβsγyoi + sαcβcγzoi 　hi ,2 ,11 ( k) = ( sαsβcγ - cαsγ) yoi + ( - sαsβsγ - cαcγ) zoi 　hi ,3 ,9 ( k) = - cβxoi - sβsγyoi - sβcγzoi 　hi ,3 ,11 ( k) = cβcγyoi - cβsγzoi Kalman 增益 K( k) = P ( k k - 1) HT ( k) ( R ( k) + H ( k) P ( k k - 1) HT ( k) ) - 1 (7) 估计值滤波 S^ ( k k) = S^ ( k k - 1) + 　　　　K( k) ( W ( k) - G ( S^ ( k k - 1) ) ) (8) P ( k k) = P ( k k - 1) - K( k) H ( k) P ( k k - 1) (9) 由上式即可得到下一时刻该特征点的估计位置 S^ ( k k) . 3. 2 　图像特征点位置预测 为了减少图像处理的计算量 ,可利用以上结果 进一步预测下一时刻图像特征点的位置. 使得系统 能够利用比较小的窗口来确定图像特征点的位置 变化. 达到减少图像处理计算量的目的. 具体做法 是 :首先 ,通过运动状态预测值求各特征点在工作 993第 3 期 罗 　翔等 :基于 CAD 模型的立体视觉目标运动估计 空间的位置预测值 W^ ( k + 1 k) = G( AS^ ( k k) ) (10) 然后 ,根据立体视觉系统的投影关系式 (1) 得到各 特征点在两摄像机图像平面的位置预测值. 4 　伺服轨迹生成器 对于一个视觉反馈机器人系统来讲 ,其视觉反 馈系统带宽比关节伺服系统低得多. 有必要通过轨 迹生成器生成适合关节伺服控制的目标运动轨迹 信息. 轨迹生成算法为 S^ ( t) = B ( t) S^ ( k k) (11) 式中 B ( t) = 1 ( t - kT) 0 0 ⋯ 0 0 0 1 0 0 ⋯ 0 0 0 0 1 ( t - kT) ⋯ 0 0 0 0 0 1 ⋯ 0 0 … … … … … … 0 0 0 0 ⋯ 1 ( t - kT) 0 0 0 0 ⋯ 0 1 t = t s ,2 t s , ⋯, kT ≤ t ≤ ( k + 1) T. t s 为伺服系统 的采样频率. 这样就可以生成一组满足伺服系统频 率的离散运动轨迹. 5 　数值仿真 建立一套由 2 个焦距为 50 mm ,光轴相互平 行 ,间距为 200 mm 的摄像机组成的立体视觉 数学 数学高考答题卡模板高考数学答题卡模板三年级数学混合运算测试卷数学作业设计案例新人教版八年级上数学教学计划 模型 ;跟踪目标为一边长为 200 mm 的正方体 ,其中 心为模型坐标系原点. 初始状态时模型坐标系与立 体视觉坐标系 (摄像机 1 坐标系)平行 ,模型坐标系 原点坐标为 (0 ,0 ,500) . 仿真过程中 ,目标包含 3 种 运动 :沿 x 方向作频率为 0. 32 Hz ,振幅为 100 mm 的往复运动 ;绕 y 轴作频率为 0. 80 Hz ,幅度为 1 rad 的往复摆动 ;绕 z 轴作变加速旋转 ,加速度的幅度 为 2 rad/ s2 ,频率为 0. 32 Hz 的正弦函数. 所有位移 运动加入均方差为 10 的过程噪声 ;旋转方向加入 均方差为 0. 1 的过程噪声. 仿真算法分为两步 ,首先由图像特征点求出目 标特征点的空间位置 ;然后由已知其匹配模型的特 征点集 ,通过扩展卡尔曼滤波方法估计其运动参 数. 图 4 为仿真结果. 图 4 　仿真结果 004 东南大学学报 (自然科学版) 第 32 卷 6 　结　论 本文介绍了一种基于机器人立体视觉系统的 目标运动信息处理方法. 该方法是基于 CAD 模型 , 即对象的特征是精确知道的. 目标的运动估计分为 两步 :首先由图像特征求出目标特征点的空间位置 时间序列 ;然后由已知其匹配模型的特征点集 ,通 过扩展卡尔曼滤波方法估计其运动参数. 基于上述 参数 ,设计了与伺服系统频率匹配、减轻图像处理 延时影响的目标轨迹生成器. 该方法对于运动目标运动估计 ,基于位置的视 觉伺服机器人系统 ;机器人与机器人或人与机器人 协调控制系统均有实际的意义. 参考文献 ( References) [1 ] Hutchinson S , Hager G D , Corke P I. A tutorial on visual servo control[J ] . IEEE Trans Robot Automat ,1996 ,12 (5) : 651 670. [2 ] Papanikolopoulos N P , Khosla P K, Kanade T. Visual track2 ing of a moving target by a camera mounted on a robot : a combination of control and vision[J ] . IEEE Trans Robot Au2 tomat ,1993 ,9 (1) :14 35. [3 ] 郑南宁. 计算机视觉与模式识别 [M] . 北京 :国防工业 出版社 ,1998. 248 277. Zhen Nanning. Computer vision and pattern recognition[M] . Beijing: Publishing House of Defence Industry ,1998. 248 277. (in Chinese) [4 ] Janabi2Sharifi F , Wilson W J . Automatic selection of image features for visual servoing[J ] . IEEE Trans Robot Automat , 1997 ,13 (6) :890 902. [5 ] Wilson W J , Hulls C W , Bell G S. Relative end2effector control using Cartesian position based visual servoing [ J ] . IEEE Trans Robot Automat ,1996 ,12 (5) :684 696. [6 ] Blostein S D , Huang T S. Error analysis in stereo determina2 tion of 32D point position[J ] . IEEE Trans Pattern Anal Ma2 chine Intell ,1987 ,PAMI29 (6) :752 765. [ 7 ] Arun K S , Huang T S , Blostein S D. Least2squares of two 32 D point sets[J ] . IEEE Trans on Pattern Anal Machine Intell , 1987 ,PAMI29 (6) :698 700. [8 ] Goodwin G C , 孙贵生. 自适应滤波、预测与控制 [M] . 北京 :科学出版社 ,1992. 200 246. Goodwin G C , Sun Guisheng. Adaptive filtering , forecast and control [ M ] . Beijing : Publishing House of Science , 1992. (in Chinese) 104第 3 期 罗 　翔等 :基于 CAD 模型的立体视觉目标运动估计