算法的视觉机械臂控制装 置，包括USB相机1、卡片电脑2和机械臂舱机控制器3等。其中，机械臂为六自由度机械 臂，由基部舱机4、肩部舱机5、肘部舱机6、腕部舱机7、握手舱机8和腕部转动舱机9组成。
【具体实施方式】 [0065] 如图2所示，包括如下步骤：
[0066](1)将各种待加工零件放置在工作台平面上；
[0067] (2)位于机械臂腕部的USB相机1自动扫描工作台上的各种零件，通过Camshift 视觉定位算法，得到各个零件的定位信息狂、Y、Z坐标，相机坐标系忙})，并将定位信息通 过USB串口传输到上位机卡片电脑2;
[006引（3)卡片电脑2接收到USB串口信息后，根据上位机软件输入的机械臂待完成动作 指令，利用D-H建模算法，计算得到机械臂各舱机的控制参数；
[0069] (4)卡片电脑2将机械臂各舱机的控制参数通过UART串口传输至机械臂舱机控制 器3;
[0070] (5)机械臂舱机控制器3将各舱机控制参数，依次传输至基座舱机4、肩部舱机5、 肘部舱机6、腕部舱机7、握手舱机8和腕部转动舱机9;
[0071] (6)机械臂完成指定动作后，向卡片电脑2发出响应，等待卡片机电脑2输入下一 步指令；
[0072] (7)不断重复步骤（2) (3) (4) (5)化），直到装配工作全部完成。
[0073] 本发明的关键在于采用Camshift视觉跟踪技术精确定位目标工件的位置坐标信 息，并采用D-H建模算法实现对视觉机械臂的精确控制。
[0074] 采用Camshift视觉跟踪技术精确定位目标工件的过程，可W由W下四个步骤组 成：
[0075] 1、根据待加工工件的形状、尺寸和颜色等特征信息，创建表示各种工件的 Camshifit直方图，用于跟踪和匹配。
[0076] 2、在相机摄取的第一帖图像中，W上述各种工件的Camshift直方图为特征，对图 像中的所有像素进行匹配，捜索到工件的像素区域，得到该工件中屯、相对相机坐标中屯、的 位置信息狂和Y坐标，相机坐标系{C})。
[0077] 3、根据工件所占像素点多少、相机焦距W及工件真实大小，利用小孔成像原理，计 算工件在图像中的深度，得到工件距离相机的高度位置信息狂坐标，相机坐标系忙})。
[0078] 4、W第一帖图像中工件的中屯、为起始点，对下一帖图像中的工件进行捜索和定 位，不断重复2和3步骤。从而实现在连续的图像中对工件精确位置信息的准确获取。
[0079] 而采用机器人正运动学D-H建模算法实现对视觉机械臂、目标工件和工作台等多 目标的准确定位，如图3所示。
[0080] 1)基坐标系{6}
[0081] 基坐标系{B}位于操作臂的基座上，X轴与操作臂转轴重合（方向任意），Z轴垂 直于水平面向上，y轴通过右手定则确定
[0082] 2)工作台坐标系怯}
[0083] 工作台坐标系怯}位于工作台的一个角上，是一个通用的坐标系，机械臂的所有 运动都是相对于该坐标系来执行的。工作台坐标系通常是根据基坐标系来确定的，即賢 (Sgr表示从工作台坐标系怯}到基坐标系巧}的变换矩阵，W下相似符号同理表示），
[0084]如腕部坐标系{W}
[0085] 腕部坐标系{W}位于机械臂的末端连杆，X轴与腕部旋转轴重合（方向任意），y 轴与腕部连杆重合（方向任意），Z轴通过右手定则确定。
[008引 4)工具坐标系讯
[0087]工具坐标系{T}附于机械臂所夹持工具的末端。X轴与腕部连杆重合（方向任 意），y轴垂直于X轴（方向任意，）Z轴通过右手定则确定。当手部没有夹持工具时，工具 坐标系的原点位于机械臂的指端之间。工具坐标系{T}通常根据腕部坐标系{W}来确定， 即攻。
[008引W目标坐标系脚
[0089]目标坐标系{G}是机械臂移动工具时对工具位置的描述。特指在机械臂运动结束 时，工具坐标系{T}应当与目标坐标系{G}重合。目标坐标系{G}通常根据根据工作台坐 标系怯}由人为指定或根据实际情况初始化进行确定。
[0090] 此外，对于视觉机械臂，在机械臂的末端附有相机用于图像采集。图像处理程序可 计算出目标在相机坐标系中的位置中。要计算出目标在工作台坐标系中{T}的位置，需要 知道相机坐标系忙}相对基坐标系{B}的转移矩阵狂。通常情况下，相机固定于腕部，容易 测量出相机坐标系{B}在腕部坐标系中的定义踩，由此可得其T岩T。
[0091] 关于针对视觉机械臂，建立W各舱机关节为坐标系原点的多坐标系进行模型描 述，可W按照下面的步骤正确地建立连杆坐标系，如图4所示。
[0092] 1)找出各关节轴，并标出运些轴线的延长线。在下面的步骤2至步骤5中，仅考虑 两个相邻的轴线（关节轴i和i+1)。
[0093] 2)找出关节轴i和i+1之间的公垂线或交点，W关节轴i和i+1的交点或公垂线 与关节轴i的交点作为连杆坐标系U}的原点。
[0094] 3)规定《轴沿关节轴i的指向。
[0095] 4)规定义，轴沿公垂线的指向，如果关节轴i和i+1相交，则规定怎轴垂直于关节 轴i和i+1所在的平面。
[0096] 5)按右手定则确定 轴。
[0097] 在标准D-H法中，关节i对应坐标系U-1}。用4个连杆参数来描述，具体如下：
[0098] 1)曰1表示第i个连杆的长度；
[009引 。a康示第i关节轴与第i+1关节轴线的夹角；
[0100] 如0 1表示第i个连杆和第i+1个连杆的夹角；
[0101] 4)di表示在第i个关节轴线上相邻两连杆的距离；
[010引 由坐标系a-1}到坐标系{i}的变换顺序为：
[010引1)绕iVi轴旋转曰11角度；
[0104]。沿疋1轴平移曰11距离；
[0105]3)绕Z抽旋转01角度；
[0106] 4)沿i,轴平移di距离。
[0107] 由此可得转移矩阵如下所示。
[0115] 由于本专利选择的视觉机械臂型号，关节2、3、4为相互平行巧日图4所示），所W 终、梦、班的乘积用二角和公式可W得到一个简化的表达式。
[0116]
[0117]其中，为了间单表不，令Si=sin目i，Sij=sin(目i+ 目j)，Sijk二sin(目i+ 目j+ 目k)， 余弦也用类似的表示。
[011 引
[0119] 在机模臂的控制中，已知工具坐标系相对于工作台坐标系的期望位置和姿态，需 要计算出一系列满足期望要求的关节角。为了实现运一要求，首先进行坐标变换求出相对 于基坐标系巧}的腕部坐标系{W}，然后应用逆运动学求关节角。
[0120] 由于机械臂的逆运动学的求解方法很多，针对不同的机械臂，各求解方法的复杂 性也不尽相同。此处针对机械臂使用几何解法进行求解。
[0121] 已知工具坐标系{T}相对于工作台坐标系怯}的期望位置和姿态，:由于工 具坐标系{T}相对于腕部坐标系{W}是固定的（即"才已知，为常数矩阵），因此容易求得腕 部坐标系相对于基坐标系的位置和姿态其^ =许奸"；ri。
[0122] 首先研究腕部坐标系{W}相对于基坐标系巧}的位置Bp胃。=[Bxwk(；By胃。Bz胃。] T。（矢量Bp胃康示{W}的原点P相对于脚的位置）
[0123] 如图2所示，在机械臂的运动过程中，如果第1个关节轴不动，那么机械臂的工作 空间是一个平面，即Bp胃和么所确定的平面。只有第1个关节轴的转动才能使得机械臂到 达运一平面。由此可得
[0124]
[0125]在计算得到01后，研究腕部坐标系相对于第1个关节轴的位置。此时其位置关 系是二维的，在一个平面内描述即可。由图5容易计算出腕部坐标系原点在坐标系{1}中 的位置：
[0126]
[0127] 在图6中，对于描述出的S角形，应用余弦定理求解0 3
[0132] 为了使得S角形成立，必须满足到目标点的距离^3音+4小于或等于第二个连 杆和第=个连杆的长度之和曰2+曰3。在计算之前可使用上述条件进行检验W判断该解是否 存在。假设解存在，那么上式所解得的9 3应在0~140°的范围内。
[0133] 为了求得0 2，需要求得图中的P和角。首先，P可^位于任意象限，运是由 Xw和ZW的符号决定的。
[0134]
[0135] 应用余弦定理可W计算出:
[013引为使该几何关系成立，所解得的应在0~180°的范围内。由图中关系有
[0139]021 = 0 + 1])
[0140] 由于02的范围在[-180°，0]，所W
[0141] 白 2= -(0 + 1]〇
[0142] 04的角度根据腕部的姿态来确定，如果末端连杆与^轴平行时，可计算得：
[0143] I目J=I目sl-l白 2
[0144] 由于 0 2、0 4的范围在[-180°，0]，所W [01 45]白 4二-（目 3+ 白 2)
[0146] 综上所述，通过几何解法计算得到机械臂各关节舱机角的准确值，总结如下：
[0147]
[014引其中，0康示基部舱机（4)的转动角度，0康示肩部舱机妨的转动角度，03表示肘部舱机化）的转动角度，94表示腕部舱机（7)的转动角度，0 5表示腕部转动舱机 (9)的转动角度，握手舱机（8)的转动角度根据零件大小通过初始化设定来确定。
[0149] 机械臂运动的基本问题是将机械臂从一个位置移动到另一个位置，也就是将工具 坐标系从当前值移动到期望值{TfmJ。机械臂的基本运动是各