个关节角的动运， 而最终的运动则是机械臂前端的运动。因此有两种空间的规划方法：关节空间和笛卡尔空 间规划法。
[0150] 机械臂运动经过的路径点通常是用工具坐标系相对于工作台坐标系来描述的。应 用逆运动学理论，运些路径点均可转换成一组对应的关节角。运样就得到了经过各路径点 的n个关节的光滑函数。对于每个关节而言，由于各路径段所需的时间是相同的，因此所有 关节将同时到达各路径点，从而得到工具坐标系在每个路径点上期望的笛卡尔位置。而且 对某个特定的关节角而言，其期望的关节角函数与其他的关节函数无关。尽管各路径点在 关节空间的描述非常简单，但在笛卡尔空间中的描述却很复杂。因此采用关节空间的规划 方法来进行机械臂的轨迹描述。
[0151] 考虑在一定时间内将工具从初始位置移动到目标位置的问题。应用逆运动学可W 解出对应于目标位姿的各个关节角。其在t。时刻的值为初始位置，在tf时刻的值为该关 节的目标位置。为了获得一条确定的光滑运动曲线，至光需要对0 (t)施加四个约束条件。 由于初始值和目标值已知，并且在初始时刻和最终时刻关节的速度为零，因此可得到运四 个约束条件为
[0152] 0 (0) = 0 0
[0153] 0 (tf) = 0f
[0154] 台(0)二0
[0155] 刮式）=〇
[0156] 运些约束条件唯一确定了一个=次多项式，该=次多项式有如下形式：
[0157] 0 (t) =a〇+ait+a2t2+a3t3
[015引 由四个约束条件可解出方程中的未知量曰1为：
[0159] a〇= 00
[0160] 曰1= 0
【主权项】
1. 一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法，其中，机械臂 为六自由度机械臂，包括基部舵机（4)、肩部舵机（5)、肘部舵机（6)、腕部舵机（7)、握手舵 机（8)和腕部转动舵机（9)，其特征在于： 采用Camshift视觉跟踪方法精确定位目标工件的位置坐标信息，具体步骤如下： 步骤1. 1)，根据待加工工件的特征信息，仓ll建表示各种工件的Camshifit直方图，用于 跟踪和匹配； 步骤1.2)，利用相机扫描各工件，在相机摄取的第一帧图像中，以所述各种工件的Camshift直方图为特征，对图像中的所有像素进行匹配，搜索到某一工件的像素区域，得到 该工件中心相对相机坐标中心的位置信息，即X和Y坐标； 步骤1.3)，根据工件所占像素点多少、相机焦距以及工件真实大小，利用小孔成像原 理，计算工件在图像中的深度，得到工件距离相机的高度位置信息，即Z坐标；其中X、Y、Z坐 标均是在相机坐标系{C}中的值； 步骤1. 4)，以第一帧图像中工件的中心为起始点，对下一帧图像中的工件进行搜索和 定位，不断重复步骤2)和步骤3)，从而实现在连续的图像中对工件精确位置信息的准确获 取； 采用D-H建模算法实现对视觉机械臂、目标工件以及工作台的准确定位具体步骤如 下： 步骤2.1)，利用基坐标系{B}、工作台坐标系{S}、腕部坐标系{W}、工具坐标系{T}和 目标坐标系{〇}实现对视觉机械臂、目标工件和工作台在同一坐标系下的准确定位； 步骤2. 2)，对视觉机械臂建立以各舵机关节为坐标系原点的多坐标系模型，根据步骤 2. 1得到的机械臂的初始位置信息和期望目标位置信息，采用逆运动学对机械臂的各个关 节进行解耦运算，从而得到机械臂各个运动关节的控制参数，实现对机械臂的移动控制。2. 根据权利要求1所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方 法，其特征在于，所述步骤1. 1)中，待加工工件的特征信息包括其形状、尺寸和颜色信息。3. 根据权利要求1所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方 法，其特征在于，所述步骤2. 1)中： 基坐标系{B}位于操作臂的基座上，x轴与操作臂转轴重合（方向任意），z轴垂直于 水平面向上，y轴通过右手定则确定； 工作台坐标系{S}位于工作台的一个角上，是一个通用的坐标系，z轴垂直于水平面向 上，xy轴根据实际工作环境标定，但需符合右手定则，机械臂的所有运动均相对于该坐标系 执行，工作台坐标系{S}根据基坐标系{B}确定，即^r，^r表示从工作台坐标系{S}到基 坐标系{B}的变换矩阵； 腕部坐标系{W}位于机械臂的末端连杆，X轴与腕部旋转轴重合，方向任意，y轴与腕部 连杆重合，方向任意，Z轴通过右手定则确定； 工具坐标系{T}附于机械臂所夹持工具的末端，X轴与腕部连杆重合，方向任意，y轴垂 直于x轴，方向任意，z轴通过右手定则确定，当手部没有夹持工具时，工具坐标系{T}的原 点位于机械臂的指端之间，工具坐标系{T}根据腕部坐标系{W}确定，即fr，fr表示从工 具坐标系{T}到腕部坐标系{W}的变换矩阵； 目标坐标系{G}是机械臂移动工具时对工具位置的描述，在机械臂运动结束时，工具 坐标系{T}与目标坐标系{G}重合，目标坐标系{G}根据工作台坐标系{S}由人为指定或 根据实际情况初始化进行确定； 同时，确定相机坐标系{C}在腕部坐标系{W}中的定义方，表示从相机 坐标系{C}到基坐标系{B}的变换矩阵。4. 根据权利要求3所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方 法，其特征在于，通过如下方式对视觉机械臂建立以各舵机关节为坐标系原点的多坐标系 丰旲型： 步骤1)，找出各关节轴，并标出这些轴线的延长线，在下面的步骤2)至步骤5)中，仅 考虑两个相邻的轴线，即关节轴i和i+1 ;其中，i表示连杆和关节轴的编号，连杆i和连杆 i+1之间通过关节i+1相连； 步骤2)，找出关节轴i和i+1之间的公垂线或交点，以该公垂线或交点与关节轴i的交 点作为连杆坐标系U}的原点； 步骤3)，规定连杆坐标系{i}中之轴为沿关节轴i的指向； 步骤4)，规定连杆坐标系{i}中龙.轴沿公垂线的指向，如果关节轴i和i+1相交，则规 定矣軸垂直于关节轴i和i+1所在的平面； 步骤5)，按右手定则确定连杆坐标系{i}的轴； 将关节i对应坐标系U-1}，用4个连杆参数来描述，具体如下： 1) %表示第i个连杆的长度； 2. ai表示第i关节轴与第i+1关节轴线的夹角； 3) 0i表示第i个连杆和第i+1个连杆的夹角； 4) 山表示在第i个关节轴线上相邻两连杆的距离； 由坐标系U-1}到坐标系U}的变换顺序为： 1) 绕之,轴旋转aii角度； 2) 沿轴平移&11距离； 3) 绕之轴旋转0i角度； 4) 沿；釉平移山距离。5. 根据权利要求3所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方 法，其特征在于，所述步骤2. 2)中，先进行坐标变换求出相对于基坐标系{B}的腕部坐标系 IW}的变换矩阵#，#= 然后应用逆运动学求关节角。6. 根据权利要求5所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方 法，其特征在于，所述腕部坐标系{W}相对于基坐标系{B}的位置BP胃e= [Bx胃eByw(]ReBz胃e] T，BP胃<；表示腕部坐标系{W}的坐标原点P在基坐标系{B}的坐标位置，第1个连杆和第2 B 个连杆的夹角3 第2个连杆和第3个连杆的夹角02=-(0 +il〇,其中， ^wom第3个连杆和第4个连杆的夹角，第4个连杆和第5个连杆的夹角0 4= - ( 0 3+ 0 2)，其 中％表示第i个连杆的长度； 综上，通过几何解法计算得到机械臂各关节舵机角的准确值，总结如下：其中，0:表示基部舵机（4)的转动角度，0 2表示肩部舵机（5)的转动角度，0 3表示 肘部舵机（6)的转动角度，04表示腕部舵机（7)的转动角度，0 5表示腕部转动舵机（9)的 转动角度，握手舵机（8)的转动角度根据零件大小通过初始化设定来确定。7.根据权利要求6所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方 法，其特征在于，利用关节空间规划法规划机械臂运动，设其轨迹为一条确定的光滑运动曲 线0 (t)zao+ait+aj^+a；^3,记其在t。时刻的值为初始位置，在tf时刻为目标位置； 根据约束条件为： .心.\最终得到方程中未知量为：8. -种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置，其中，机械臂 为六自由度机械臂，包括基部舵机（4)、肩部舵机（5)、肘部舵机（6)、腕部舵机（7)、握手舵 机（8)和腕部转动舵机（9)，其特征在于，控制装置包括： USB相机（1)，位于机械臂腕部，扫描工作台上的各种零件，获取各个零件的定位信息； 卡片电脑（2)，接收USB相机（1)发送的定位信息，根据机械臂待完成动作指令，获取机 械臂各舵机的控制参数； 机械臂舵机控制器（3)，接收卡片电脑（2)发送的各舵机的控制参数，分别传输至各舵 机执行相应的动作。
【专利摘要】一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂装置，利用Camshift视觉跟踪算法对机械臂上USB相机采集到的图像信息进行分析处理，获取到待抓取和组装工件的精确定位信息，并采用机器人正运动学D-H建模算法实现对视觉机械臂、目标工件和工作台等多目标的准确定位，随后针对视觉机械臂建立以各舵机关节为坐标系原点的多坐标系描述模型，针对机械臂的不同动作，通过逆动力学计算推导出封闭解析解，从而确定机械臂多个舵机关节运动的唯一控制参数，实现对视觉机械臂的精确控制，完成机械臂对工件的抓取、加工、装配和运送等多种动作。
【IPC分类】B25J9/16, B25J13/08
【公开号】CN105014677
【申请号】CN201510394465
【发明人】韩九强, 李俊杰, 刘俊, 刘瑞玲, 张新曼
【申请人】西安交通大学
【公开日】2015年11月4日
【申请日】2015年7月7日