基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置及方法
【技术领域】
[0001] 本发明属于机械臂领域，设及一种视觉机械臂装置，特别设及一种基于Camshift 视觉跟踪和D-H建模算法的视觉机械臂控制装置及方法。
【背景技术】
[0002] 近些年来，随着科技的快速发展和工业生产规模的日益扩大，机械臂凭借其生产 速度快、稳定性强、加工精度高、生产成本低等优势，已逐步替代各种人工操作，广泛地应用 到工业各个生产流程中。与此同时，随着机械臂的广泛普及，越来越多的生产流程需要机械 臂具有较强的环境适应能力和人工智能，使其可W独立、高效地完成各种生产流程，满足生 产过程中的各种要求。而视觉机械臂可W满足运样的需求，因此视觉机械臂成为了当今机 器人领域的研究热点之一。
[0003] 视觉机械臂是一种具有计算机视觉、可独立完成各种动作的机械臂。视觉机械臂 通过对相机采集到的图像序列进行分析和处理，针对不同工作环境和加工流程进行判断和 决策，采用人工智能完成相应的生产动作，从而替代人类完成零件抓取、加工和组装等各种 过程。但是，设计一种简易、高效、准确的视觉机械臂装置W及对视觉机械臂精确控制的研 究，一直是亟待解决和研究的困难和问题。
【发明内容】
[0004] 为了克服上述现有技术的缺点，本发明的目的在于提供一种基于Camshift视觉 跟踪和D-H建模算法的视觉机械臂控制装置及方法，解决了背景中无法对视觉机械臂进行 准确控制的难题，具备灵活性强、定位准确、定位目标不易丢失、控制精度高、节能等多个优 点。
[0005] 为了实现上述目的，本发明采用的技术方案是：
[0006] -种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法，机械臂为六 自由度机械臂，包括基部舱机4、肩部舱机5、肘部舱机6、腕部舱机7、握手舱机8和腕部转 动舱机9 ;
[0007] 采用Camshift视觉跟踪方法精确定位目标工件的位置坐标信息，具体步骤如下：
[0008]步骤1. 1)，根据待加工工件的特征信息，创建表示各种工件的Camshifit直方图， 用于跟踪和匹配；
[0009]步骤1. 2)，利用相机扫描各工件，在相机摄取的第一帖图像中，W所述各种工件的 Camshift直方图为特征，对图像中的所有像素进行匹配，捜索到某一工件的像素区域，得到 该工件中屯、相对相机坐标中屯、的位置信息，即X和Y坐标；
[0010]步骤1. 3)，根据工件所占像素点多少、相机焦距W及工件真实大小，利用小孔成像 原理，计算工件在图像中的深度，得到工件距离相机的高度位置信息，即Z坐标；其中X、Y、 Z坐标均是在相机坐标系忙}中的值；
[0011] 步骤1.4)，w第一帖图像中工件的中屯、为起始点，对下一帖图像中的工件进行捜 索和定位，不断重复步骤2)和步骤3)，从而实现在连续的图像中对工件精确位置信息的准 确获取；
[0012] 采用D-H建模算法实现对视觉机械臂、目标工件化及工作台等多目标的准确定位 具体步骤如下：
[0013] 步骤2. 1)，利用基坐标系巧}、工作台坐标系怯}、腕部坐标系{W}、工具坐标系m 和目标坐标系{G}实现对视觉机械臂、目标工件和工作台在同一坐标系下的准确定位；巧曰 图3)
[0014]步骤2. 2)，对视觉机械臂建立W各舱机关节为坐标系原点的多坐标系模型巧曰图 4)，根据步骤2. 1得到的机械臂的初始位置信息和期望目标位置信息，采用逆运动学对机 械臂的各个关节进行解禪运算，从而得到机械臂各个运动关节的控制参数，实现对机械臂 的移动控制。
[0015] 所述步骤1. 1)中，待加工工件的特征信息包括其形状、尺寸和颜色信息。
[001引所述步骤2.1)中：
[0017] 基坐标系{B}位于操作臂的基座上，X轴与操作臂转轴重合（方向任意），Z轴垂 直于水平面向上，y轴通过右手定则确定；
[0018] 工作台坐标系怯}位于工作台的一个角上，是一个通用的坐标系，Z轴垂直于水平 面向上，xy轴根据实际工作环境标定（需符合右手定则），机械臂的所有运动均相对于该坐 标系执行，工作台坐标系怯}根据基坐标系{B}确定，即|7 ,梦^表示从工作台坐标系怯} 到基坐标系巧}的变换矩阵；
[0019] 腕部坐标系{W}位于机械臂的末端连杆，X轴与腕部旋转轴重合（方向任意），y 轴与腕部连杆重合（方向任意），Z轴通过右手定则确定；
[0020] 工具坐标系{T}附于机械臂所夹持工具的末端，X轴与腕部连杆重合（方向任 意），y轴垂直于X轴（方向任意，）Z轴通过右手定则确定，当手部没有夹持工具时，工具坐 标系{T}的原点位于机械臂的指端之间，工具坐标系{T}根据腕部坐标系{W}确定，即
[0021] 目标坐标系{G}是机械臂移动工具时对工具位置的描述，在机械臂运动结束时， 工具坐标系{T}与目标坐标系{G}重合，目标坐标系{G}根据工作台坐标系怯}由人为指 定或根据实际情况初始化进行确定；
[002引同时，确定相机坐标系似在腕部坐标系{W}中的定义奸，奸=袜抒。
[0023] 本发明通过如下方式对视觉机械臂建立W各舱机关节为坐标系原点的多坐标系 模型：巧日图4所示）
[0024] 步骤1)，找出各关节轴，并标出运些轴线的延长线，在下面的步骤2)至步骤5)中， 仅考虑两个相邻的轴线，即关节轴i和i+1 ;其中，i表示连杆和关节轴的编号，连杆i和连 杆i+1之间通过关节i+1相连
[00巧]步骤2)，找出关节轴i和i+1之间的公垂线或交点，W该公垂线或交点与关节轴i的交点作为连杆坐标系{i}的原点；
[0026] 步骤3)，规定连杆坐标系U}中Z轴为沿关节轴i的指向；
[0027] 步骤4)，规定连杆坐标系{i}中V轴沿公垂线的指向，如果关节轴i和i+1相交， 则规定A>,;轴垂直于关节轴i和i+1所在的平面；
[002引步骤5)，按右手定则确定连杆坐标系W的；f;轴；
[0029] 将关节i对应坐标系U-1}，用4个连杆参数来描述，具体如下：
[0030] 1)曰1表示第i个连杆的长度；
[003。 。a康示第i关节轴与第i+1关节轴线的夹角；
[003引扣0康示第i个连杆和第i+1个连杆的夹角；
[0033] 4)di表示在第i个关节轴线上相邻两连杆的距离；
[0034] 由坐标系a-u至挫标系w的变换顺序为：
[0035] 1)绕衣1轴旋转a1 1角度；
[003引。沿和，洋由平移曰1 1距离；
[0037]扣绕為轴旋转0 1角度；
[003引 4)沿之.轴平移di距离。
[0039] 所述步骤2. 2)中，先进行坐标变换求出相对于基坐标系{B}的腕部坐标系{W}的 变换矩降
然后应用逆运动学求关节角。
[0040] 所述腕部坐标系{W}相对于基坐标系脚的位置
表示腕部坐标系{W}的坐标原点P在基坐标系巧}的坐标位置），第1个连杆和第2个 连杆的夹角
第2个连杆和第3个连杆的夹角02=-(P+ilO(其中，
，第3个连杆和第4个连杆的夹角
第4个连杆和第5个连杆的夹角0 4= - ( 0 3+e2)，其 中曰1表示第i个连杆的长度。
[0041] 综上所述，通过几何解法计算得到机械臂各关节舱机角的准确值，总结如下：
[0042]
[004引其中，0康示基部舱机4的转动角度，0康示肩部舱机5的转动角度，0 3表示 肘部舱机6的转动角度，0 4表示腕部舱机7的转动角度，0 5表示腕部转动舱机9的转动 角度，握手舱机8的转动角度根据零件大小通过初始化设定来确定。
[0044] 本发明利用关节空间规划法规划机械臂运动，设其轨迹为一条确定的光滑运动曲 线0 (t) =au+ait+a2t2+a3t3。考虑在一定时间内将工具从初始位置移动到目标位置的问题。 应用逆运动学可W计算出对应于目标位姿的各个关节角。记其在t。时刻的值为初始位置， 在tf时刻为目标位置。
[0045] 根据约束条件为：
[0046]
[0047] 最终得到方程中未知量曰1为：
[0052] 本发明还提供了一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制 装置，其中，机械臂为六自由度机械臂，包括基部舱机4、肩部舱机5、肘部舱机6、腕部舱机 7、握手舱机8和腕部转动舱机9,控制装置包括：
[0053]USB相机1，位于机械臂腕部，扫描工作台上的各种零件，获取各个零件的定位信 息；
[0054] 卡片电脑2,接收USB相机1发送的定位信息，根据机械臂待完成动作指令，获取机 械臂各舱机的控制参数；
[0055] 机械臂舱机控制器3,接收卡片电脑2发送的各舱机的控制参数，分别传输至各舱 机执行相应的动作。
[0056]与现有技术相比，本发明可实现视觉机械臂的精确控制。
【附图说明】
[0057] 图1是视觉机械臂系统装置示意图。
[0058] 图2是视觉机械臂控制流程示意图。
[0059] 图3是视觉机械臂坐标系描述示意图。
[0060] 图4是机械臂多机构坐标系建模示意图。
[0061] 图5是腕部坐标系相对于基坐标系的位置。
[0062] 图6是视觉机械臂在多坐标系描述中的各连杆的平面几何关系。
【具体实施方式】
[0063] 下面结合附图和实施例详细说明本发明的实施方式。
[0064] 如图1所示，本发明基于Camshift视觉跟踪和D-H建模