CN105014677A - 基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置及方法 - Google Patents

Abstract

一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂装置，利用Camshift视觉跟踪算法对机械臂上USB相机采集到的图像信息进行分析处理，获取到待抓取和组装工件的精确定位信息，并采用机器人正运动学D-H建模算法实现对视觉机械臂、目标工件和工作台等多目标的准确定位，随后针对视觉机械臂建立以各舵机关节为坐标系原点的多坐标系描述模型，针对机械臂的不同动作，通过逆动力学计算推导出封闭解析解，从而确定机械臂多个舵机关节运动的唯一控制参数，实现对视觉机械臂的精确控制，完成机械臂对工件的抓取、加工、装配和运送等多种动作。

Description

基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置及方法

技术领域

本发明属于机械臂领域，涉及一种视觉机械臂装置，特别涉及一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置及方法。

背景技术

近些年来，随着科技的快速发展和工业生产规模的日益扩大，机械臂凭借其生产速度快、稳定性强、加工精度高、生产成本低等优势，已逐步替代各种人工操作，广泛地应用到工业各个生产流程中。与此同时，随着机械臂的广泛普及，越来越多的生产流程需要机械臂具有较强的环境适应能力和人工智能，使其可以独立、高效地完成各种生产流程，满足生产过程中的各种要求。而视觉机械臂可以满足这样的需求，因此视觉机械臂成为了当今机器人领域的研究热点之一。

视觉机械臂是一种具有计算机视觉、可独立完成各种动作的机械臂。视觉机械臂通过对相机采集到的图像序列进行分析和处理，针对不同工作环境和加工流程进行判断和决策，采用人工智能完成相应的生产动作，从而替代人类完成零件抓取、加工和组装等各种过程。但是，设计一种简易、高效、准确的视觉机械臂装置以及对视觉机械臂精确控制的研究，一直是亟待解决和研究的困难和问题。

发明内容

为了克服上述现有技术的缺点，本发明的目的在于提供一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置及方法，解决了背景中无法对视觉机械臂进行准确控制的难题，具备灵活性强、定位准确、定位目标不易丢失、控制精度高、节能等多个优点。

为了实现上述目的，本发明采用的技术方案是：

一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法，机械臂为六自由度机械臂，包括基部舵机4、肩部舵机5、肘部舵机6、腕部舵机7、握手舵机8和腕部转动舵机9；

采用Camshift视觉跟踪方法精确定位目标工件的位置坐标信息，具体步骤如下：

步骤1.1)，根据待加工工件的特征信息，创建表示各种工件的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}和目标坐标系{G}实现对视觉机械臂、目标工件和工作台在同一坐标系下的准确定位；(如图3)

步骤2.2)，对视觉机械臂建立以各舵机关节为坐标系原点的多坐标系模型(如图4)，根据步骤2.1得到的机械臂的初始位置信息和期望目标位置信息，采用逆运动学对机械臂的各个关节进行解耦运算，从而得到机械臂各个运动关节的控制参数，实现对机械臂的移动控制。

所述步骤1.1)中，待加工工件的特征信息包括其形状、尺寸和颜色信息。

所述步骤2.1)中：

基坐标系{B}位于操作臂的基座上,x轴与操作臂转轴重合(方向任意)，z轴垂直于水平面向上，y轴通过右手定则确定；

工作台坐标系{S}位于工作台的一个角上，是一个通用的坐标系，z轴垂直于水平面向上，xy轴根据实际工作环境标定(需符合右手定则)，机械臂的所有运动均相对于该坐标系执行，工作台坐标系{S}根据基坐标系{B}确定，即表示从工作台坐标系{S}到基坐标系{B}的变换矩阵；

腕部坐标系{W}位于机械臂的末端连杆，x轴与腕部旋转轴重合(方向任意)，y轴与腕部连杆重合(方向任意)，z轴通过右手定则确定；

工具坐标系{T}附于机械臂所夹持工具的末端，x轴与腕部连杆重合(方向任意)，y轴垂直于x轴(方向任意，)z轴通过右手定则确定，当手部没有夹持工具时，工具坐标系{T}的原点位于机械臂的指端之间，工具坐标系{T}根据腕部坐标系{W}确定，即

目标坐标系{G}是机械臂移动工具时对工具位置的描述，在机械臂运动结束时，工具坐标系{T}与目标坐标系{G}重合，目标坐标系{G}根据工作台坐标系{S}由人为指定或根据实际情况初始化进行确定；

同时，确定相机坐标系{C}在腕部坐标系{W}中的定义

本发明通过如下方式对视觉机械臂建立以各舵机关节为坐标系原点的多坐标系模型：(如图4所示)

步骤1)，找出各关节轴，并标出这些轴线的延长线，在下面的步骤2)至步骤5)中，仅考虑两个相邻的轴线，即关节轴i和i+1；其中，i表示连杆和关节轴的编号，连杆i和连杆i+1之间通过关节i+1相连

步骤2)，找出关节轴i和i+1之间的公垂线或交点，以该公垂线或交点与关节轴i的交点作为连杆坐标系{i}的原点；

步骤3)，规定连杆坐标系{i}中轴为沿关节轴i的指向；

步骤4)，规定连杆坐标系{i}中轴沿公垂线的指向，如果关节轴i和i+1相交，则规定轴垂直于关节轴i和i+1所在的平面；

步骤5)，按右手定则确定连杆坐标系{i}的轴；

将关节i对应坐标系{i-1}，用4个连杆参数来描述，具体如下：

1)a_i表示第i个连杆的长度；

2)α_i表示第i关节轴与第i+1关节轴线的夹角；

3)θ_i表示第i个连杆和第i+1个连杆的夹角；

4)d_i表示在第i个关节轴线上相邻两连杆的距离；

由坐标系{i-1}到坐标系{i}的变换顺序为：

1)绕轴旋转α_i-1角度；

2)沿轴平移a_i-1距离；

3)绕轴旋转θ_i角度；

4)沿轴平移d_i距离。

所述步骤2.2)中，先进行坐标变换求出相对于基坐标系{B}的腕部坐标系{W}的变换矩阵然后应用逆运动学求关节角。

所述腕部坐标系{W}相对于基坐标系{B}的位置 ^BP_WORG＝[^Bx_WORG ^By_WORG ^Bz_WORG]^T(^BP_WORG表示腕部坐标系{W}的坐标原点P在基坐标系{B}的坐标位置)，第1个连杆和第2个连杆的夹角第2个连杆和第3个连杆的夹角θ₂＝-(β+ψ)(其中， 第3个连杆和第4个连杆的夹角 第4个连杆和第5个连杆的夹角θ₄＝-(θ₃+θ₂)，其中a_i表示第i个连杆的长度。

综上所述，通过几何解法计算得到机械臂各关节舵机角的准确值，总结如下：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=\arctan \frac{{y_B}_{WO}}{{x_B}_{WO}}\\ {\mathrm{\θ}}_2=-(\arctan \frac{{z_1}_{WO}}{{x_1}_{WO}}+\arccos (\frac{x_{WO}^2{}_1+z_{WO}^2{}_1+a_2^2-a_3^2}{2a_2\sqrt{z_{WO}^2{}_1+z_{WO}^2{}_1}}\left)\right)\\ {\mathrm{\θ}}_3=\arccos \left(\frac{z_{WO}^2{}_1+z_{WO}^2{}_1-a_2^2-a_3^2}{2a_2{a}_3}\right)\\ {\mathrm{\θ}}_4=-({\mathrm{\θ}}_3+{\mathrm{\θ}}_2)\\ {\mathrm{\θ}}_5={\mathrm{\θ}}_1\end{array}\right.$

其中，θ₁表示基部舵机4的转动角度，θ₂表示肩部舵机5的转动角度，θ₃表示肘部舵机6的转动角度，θ₄表示腕部舵机7的转动角度，θ₅表示腕部转动舵机9的转动角度，握手舵机8的转动角度根据零件大小通过初始化设定来确定。

本发明利用关节空间规划法规划机械臂运动，设其轨迹为一条确定的光滑运动曲线θ(t)＝a₀+a₁t+a₂t²+a₃t³。考虑在一定时间内将工具从初始位置移动到目标位置的问题。应用逆运动学可以计算出对应于目标位姿的各个关节角。记其在t₀时刻的值为初始位置，在t_f时刻为目标位置。

根据约束条件为：

$\left\{\begin{array}{c}\mathrm{\θ}\left(0\right)={\mathrm{\θ}}_0\\ \mathrm{\θ}\left(t_f\right)={\mathrm{\θ}}_f\\ \stackrel{\·}{\mathrm{\θ}}\left(0\right)=0\\ \stackrel{\·}{\mathrm{\θ}}\left(t_f\right)=0\end{array}\right.$

最终得到方程中未知量a_i为：

a₀＝θ₀

a₁＝0

$a_2=\frac{3}{t_f^2}({\mathrm{\θ}}_f-{\mathrm{\θ}}_0)$

$a_3=-\frac{2}{t_f^3}({\mathrm{\θ}}_f-{\mathrm{\θ}}_0).$

本发明还提供了一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂 控制装置，其中，机械臂为六自由度机械臂，包括基部舵机4、肩部舵机5、肘部舵机6、腕部舵机7、握手舵机8和腕部转动舵机9，控制装置包括：

USB相机1，位于机械臂腕部，扫描工作台上的各种零件，获取各个零件的定位信息；

卡片电脑2，接收USB相机1发送的定位信息，根据机械臂待完成动作指令，获取机械臂各舵机的控制参数；

机械臂舵机控制器3，接收卡片电脑2发送的各舵机的控制参数，分别传输至各舵机执行相应的动作。

与现有技术相比，本发明可实现视觉机械臂的精确控制。

附图说明

图1是视觉机械臂系统装置示意图。

图2是视觉机械臂控制流程示意图。

图3是视觉机械臂坐标系描述示意图。

图4是机械臂多机构坐标系建模示意图。

图5是腕部坐标系相对于基坐标系的位置。

图6是视觉机械臂在多坐标系描述中的各连杆的平面几何关系。

具体实施方式

下面结合附图和实施例详细说明本发明的实施方式。

如图1所示，本发明基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置，包括USB相机1、卡片电脑2和机械臂舵机控制器3等。其中，机械臂为六自由度机械臂，由基部舵机4、肩部舵机5、肘部舵机6、腕部舵机7、握手舵机8和腕部转动舵机9组成。

具体实施方式如图2所示，包括如下步骤：

(1)将各种待加工零件放置在工作台平面上；

(2)位于机械臂腕部的USB相机1自动扫描工作台上的各种零件，通过Camshift视觉定位算法，得到各个零件的定位信息(X、Y、Z坐标，相机坐标系{C})，并将定位信息通过USB串口传输到上位机卡片电脑2；

(3)卡片电脑2接收到USB串口信息后，根据上位机软件输入的机械臂待完成动作指令，利用D-H建模算法，计算得到机械臂各舵机的控制参数；

(4)卡片电脑2将机械臂各舵机的控制参数通过UART串口传输至机械臂舵机控制器3；

(5)机械臂舵机控制器3将各舵机控制参数，依次传输至基座舵机4、肩部舵机5、肘部舵机6、腕部舵机7、握手舵机8和腕部转动舵机9；

(6)机械臂完成指定动作后，向卡片电脑2发出响应，等待卡片机电脑2输入下一步指令；

(7)不断重复步骤(2)(3)(4)(5)(6)，直到装配工作全部完成。

本发明的关键在于采用Camshift视觉跟踪技术精确定位目标工件的位置坐标信息，并采用D-H建模算法实现对视觉机械臂的精确控制。

采用Camshift视觉跟踪技术精确定位目标工件的过程，可以由以下四个步骤组成：

1、根据待加工工件的形状、尺寸和颜色等特征信息，创建表示各种工件的Camshifit直方图，用于跟踪和匹配。

2、在相机摄取的第一帧图像中，以上述各种工件的Camshift直方图为特征，对图像中的所有像素进行匹配，搜索到工件的像素区域，得到该工件中心相对相机坐标中心的位置信息(X和Y坐标，相机坐标系{C})。

3、根据工件所占像素点多少、相机焦距以及工件真实大小，利用小孔成像原理，计算工件在图像中的深度，得到工件距离相机的高度位置信息(Z坐标，相机坐标系{C})。

4、以第一帧图像中工件的中心为起始点，对下一帧图像中的工件进行搜索和定位，不断重复2和3步骤。从而实现在连续的图像中对工件精确位置信息的准确获取。

而采用机器人正运动学D-H建模算法实现对视觉机械臂、目标工件和工作台等多目标的准确定位，如图3所示。

1)基坐标系{B}

基坐标系{B}位于操作臂的基座上，x轴与操作臂转轴重合(方向任意)，z轴垂直于水平面向上，y轴通过右手定则确定

2)工作台坐标系{S}

工作台坐标系{S}位于工作台的一个角上，是一个通用的坐标系，机械臂的所有运动都是相对于该坐标系来执行的。工作台坐标系通常是根据基坐标系来确定的，即(表示从工作台坐标系{S}到基坐标系{B}的变换矩阵，以下相似符号同理表示)，

3)腕部坐标系{W}

腕部坐标系{W}位于机械臂的末端连杆，x轴与腕部旋转轴重合(方向任意)，y轴与腕部连杆重合(方向任意)，z轴通过右手定则确定。

4)工具坐标系{T}

工具坐标系{T}附于机械臂所夹持工具的末端。x轴与腕部连杆重合(方向任意)，y轴垂直于x轴(方向任意，)z轴通过右手定则确定。当手部没有夹持工具时，工具坐标系的原点位于机械臂的指端之间。工具坐标系{T}通常根据腕部坐标系{W}来确定，即

5)目标坐标系{G}

目标坐标系{G}是机械臂移动工具时对工具位置的描述。特指在机械臂运动结束时，工具坐标系{T}应当与目标坐标系{G}重合。目标坐标系{G}通常根据根据工作台坐标系{S}由人为指定或根据实际情况初始化进行确定。

此外，对于视觉机械臂，在机械臂的末端附有相机用于图像采集。图像处理程序可计算出目标在相机坐标系中的位置^CP。要计算出目标在工作台坐标系中{T}的位置，需要知道相机坐标系{C}相对基坐标系{B}的转移矩阵通常情况下，相机固定于腕部，容易测量出相机坐标系{B}在腕部坐标系中的定义由此可得 $T_C^B=T_W^{B}T_C^W.$

关于针对视觉机械臂，建立以各舵机关节为坐标系原点的多坐标系进行模型描述，可以按照下面的步骤正确地建立连杆坐标系，如图4所示。

1)找出各关节轴，并标出这些轴线的延长线。在下面的步骤2至步骤5中，仅考虑两个相邻的轴线(关节轴i和i+1)。

2)找出关节轴i和i+1之间的公垂线或交点，以关节轴i和i+1的交点或公垂线与关节轴i的交点作为连杆坐标系{i}的原点。

3)规定轴沿关节轴i的指向。

4)规定轴沿公垂线的指向，如果关节轴i和i+1相交，则规定轴垂直于关节轴i和i+1所在的平面。

5)按右手定则确定轴。

在标准D-H法中，关节i对应坐标系{i-1}。用4个连杆参数来描述，具体如下：

1)a_i表示第i个连杆的长度；

2)α_i表示第i关节轴与第i+1关节轴线的夹角；

3)θ_i表示第i个连杆和第i+1个连杆的夹角；

4)d_i表示在第i个关节轴线上相邻两连杆的距离；

由坐标系{i-1}到坐标系{i}的变换顺序为：

1)绕轴旋转α_i-1角度；

2)沿轴平移a_i-1距离；

3)绕轴旋转θ_i角度；

4)沿轴平移d_i距离。

由此可得转移矩阵如下所示。

$\begin{array}{c}T_i^{i-1}=Rot(x_{i-1},{\mathrm{\α}}_{i-1})Trans(x_{i-1},a_{i-1})Rot(z_i,{\mathrm{\θ}}_i)Trans(z_i,d_i)\\ =\left[\begin{array}{cccc}1& 0& 0& a_{i-1}\\ 0& {\mathrm{cos\α}}_{i-1}& -{\mathrm{sin\α}}_{i-1}& 0\\ 0& {\mathrm{sin\α}}_{i-1}& {\mathrm{cos\α}}_{i-1}& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& 0\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& a_{i-1}\\ {\mathrm{sin\θ}}_i{\mathrm{sin\α}}_{i-1}& {\mathrm{cos\θ}}_i{\cos \mathrm{\α}}_{i-1}& -{\mathrm{sin\α}}_{i-1}& -d_i{\mathrm{sin\α}}_{i-1}\\ {\mathrm{sin\θ}}_i{\mathrm{sin\α}}_{i-1}& {\mathrm{cos\θ}}_i{\mathrm{cos\α}}_{i-1}& {\mathrm{cos\α}}_{i-1}& d_i{\mathrm{cos\α}}_{i-1}\\ 0& 0& 0& 1\end{array}\right]\end{array}$

使用D-H参数法，可求得各连杆变换距阵为：

$T_1^0=\left[\begin{array}{cccc}{\mathrm{c\θ}}_1& -{\mathrm{s\θ}}_1& 0& 0\\ {\mathrm{s\θ}}_1& {\mathrm{c\θ}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$T_2^1=\left[\begin{array}{cccc}{\mathrm{c\θ}}_2& -{\mathrm{s\θ}}_2& 0& 0\\ 0& 0& 1& 0\\ -{\mathrm{s\θ}}_2& -{\mathrm{c\θ}}_2& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

$T_3^2=\left[\begin{array}{cccc}{\mathrm{c\θ}}_3& -{\mathrm{s\θ}}_3& 0& a_2\\ {\mathrm{s\θ}}_3& {\mathrm{c\θ}}_3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$T_4^3=\left[\begin{array}{cccc}{\mathrm{c\θ}}_4& -{\mathrm{s\θ}}_4& 0& a_3\\ {\mathrm{s\θ}}_4& {\mathrm{c\θ}}_4& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$T_5^4=\left[\begin{array}{cccc}{\mathrm{c\θ}}_5& -{\mathrm{s\θ}}_5& 0& 0\\ 0& 0& 1& 0\\ -{\mathrm{s\θ}}_5& -{\mathrm{c\θ}}_5& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

由于本专利选择的视觉机械臂型号，关节2、3、4为相互平行(如图4所示)，所以的乘积用二角和公式可以得到一个简化的表达式。

$T_4^1=\left[\begin{array}{cccc}{c}_{234}& -s_{234}& 0& c_2{a}_2+c_{23}{a}_3\\ 0& 0& 1& 0\\ -s_{234}& -c_{234}& 0& -s_2{a}_2-s_{23}{a}_3\\ 0& 0& 0& 1\end{array}\right]$

其中，为了简单表示，令s_i＝sinθ_i，s_ij＝sin(θ_i+θ_j)，s_ijk＝sin(θ_i+θ_j+θ_k)，余弦也用类似的表示。

$T_5^0=\left[\begin{array}{cccc}{c}_1{c}_{234}{c}_5+s_1+s_5& s_1{c}_5-c_1{c}_{234}{s}_5& -c_1{s}_{234}& c_1(a_2{c}_2+a_3{c}_{23})\\ s_1{c}_{234}{c}_5-c_1{s}_5& -c_1{c}_5-s_1{c}_{234}{s}_5& -s_1{s}_{234}& s_1(a_2{c}_2+a_3{c}_{23})\\ -s_{234}{c}_5& s_{234}{s}_5& -c_{234}& -a_2{s}_2-a_3{s}_{23}\\ 0& 0& 0& 1\end{array}\right]$

在机模臂的控制中，已知工具坐标系相对于工作台坐标系的期望位置和姿态，需要计算出一系列满足期望要求的关节角。为了实现这一要求，首先进行坐标变换求出相对于基坐标系{B}的腕部坐标系{W}，然后应用逆运动学求关节角。

由于机械臂的逆运动学的求解方法很多，针对不同的机械臂，各求解方法的复杂性也不尽相同。此处针对机械臂使用几何解法进行求解。

已知工具坐标系{T}相对于工作台坐标系{S}的期望位置和姿态由于工具坐标系{T}相对于腕部坐标系{W}是固定的(即已知，为常数矩阵)，因此容易求得腕部坐标系相对于基坐标系的位置和姿态

首先研究腕部坐标系{W}相对于基坐标系{B}的位置 ^BP_WORG＝[^Bx_WORG ^By_WORG ^Bz_WORG]^T。(矢量^BP_WORG表示{W}的原点P相对于{B}的位置)

如图2所示，在机械臂的运动过程中，如果第1个关节轴不动，那么机械臂的工作空间是一个平面，即^BP_WORG和所确定的平面。只有第1个关节轴的转动才能使得机械臂到达这一平面。由此可得

${\mathrm{\θ}}_1=arctan\frac{{y_B}_{WO}}{{x_B}_{WORG}}$

在计算得到θ₁后，研究腕部坐标系相对于第1个关节轴的位置。此时其位置关系是二维的，在一个平面内描述即可。由图5容易计算出腕部坐标系原点在 坐标系{1}中的位置：

${P_1}_{WORG}=\left[\begin{array}{c}{x_1}_{WORG}\\ {y_1}_{WORG}\\ {z_1}_{WORG}\end{array}\right]=\left[\begin{array}{c}\sqrt{{\left({x_B}_{WORG}\right)}^2+{\left({y_B}_{WORG}\right)}^2}\\ 0\\ {z_B}_{WORG}\end{array}\right]$

在图6中，对于描述出的三角形，应用余弦定理求解θ₃

$x_W^2+z_W^2=a_2^2+a_3^2-2a_2{a}_{3}cos(180-{\mathrm{\θ}}_3)$

所以有

${\mathrm{cos\θ}}_3=\frac{x_W^2+z_W^2-a_2^2-a_3^2}{2a_2{a}_3}$

${\mathrm{\θ}}_3=arccos\left(\frac{x_W^2+z_W^2-a_2^2-a_3^2}{2a_2{a}_3}\right)$

为了使得三角形成立，必须满足到目标点的距离小于或等于第二个连杆和第三个连杆的长度之和a₂+a₃。在计算之前可使用上述条件进行检验以判断该解是否存在。假设解存在，那么上式所解得的θ₃应在0～140°的范围内。

为了求得θ₂，需要求得图中的β和ψ角。首先，β可以位于任意象限，这是由x_W和z_W的符号决定的。

$\mathrm{\β}=arctan\frac{z_W}{x_W}$

应用余弦定理可以计算出ψ：

$cos\mathrm{\ψ}=\frac{x_W^2+z_W^2+a_2^2-a_3^2}{2a_2\sqrt{x_W^2+z_W^2}}$

$\mathrm{\ψ}=arccos\left(\frac{x_W^2+z_W^2+a_2^2-a_3^2}{2a_2\sqrt{x_W^2+z_W^2}}\right)$

为使该几何关系成立，所解得的ψ应在0～180°的范围内。由图中关系有

|θ₂|＝β+ψ

由于θ₂的范围在[-180°,0]，所以

θ₂＝-(β+ψ)

θ₄的角度根据腕部的姿态来确定，如果末端连杆与轴平行时，可计算得：

|θ₄|＝|θ₃|-|θ₂|

由于θ₂、θ₄的范围在[-180°,0]，所以

θ₄＝-(θ₃+θ₂)

综上所述，通过几何解法计算得到机械臂各关节舵机角的准确值，总结如下：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=\arctan \frac{{y_B}_{WO}}{{x_B}_{WO}}\\ {\mathrm{\θ}}_2=-(\arctan \frac{{z_1}_{WO}}{{x_1}_{WO}}+\arccos (\frac{x_{WO}^2{}_1+z_{WO}^2{}_1+a_2^2-a_3^2}{2a_2\sqrt{z_{WO}^2{}_1+z_{WO}^2{}_1}}\left)\right)\\ {\mathrm{\θ}}_3=\arccos \left(\frac{z_{WO}^2{}_1+z_{WO}^2{}_1-a_2^2-a_3^2}{2a_2{a}_3}\right)\\ {\mathrm{\θ}}_4=-({\mathrm{\θ}}_3+{\mathrm{\θ}}_2)\\ {\mathrm{\θ}}_5={\mathrm{\θ}}_1\end{array}\right.$

其中，θ₁表示基部舵机(4)的转动角度，θ₂表示肩部舵机(5)的转动角度，θ₃表示肘部舵机(6)的转动角度，θ₄表示腕部舵机(7)的转动角度，θ₅表示腕部转动舵机(9)的转动角度，握手舵机(8)的转动角度根据零件大小通过初始化设定来确定。

机械臂运动的基本问题是将机械臂从一个位置移动到另一个位置，也就是将工具坐标系从当前值{T_initial}移动到期望值{T_final}。机械臂的基本运动是各个关节角的动运，而最终的运动则是机械臂前端的运动。因此有两种空间的规划方法：关节空间和笛卡尔空间规划法。

机械臂运动经过的路径点通常是用工具坐标系相对于工作台坐标系来描述的。应用逆运动学理论，这些路径点均可转换成一组对应的关节角。这样就得到了经过各路径点的n个关节的光滑函数。对于每个关节而言，由于各路径段所需的时间是相同的，因此所有关节将同时到达各路径点，从而得到工具坐标系在每个路径点上期望的笛卡尔位置。而且对某个特定的关节角而言，其期望的关节角函数与其他的关节函数无关。尽管各路径点在关节空间的描述非常简单，但在笛卡尔空间中的描述却很复杂。因此采用关节空间的规划方法来进行机械 臂的轨迹描述。

考虑在一定时间内将工具从初始位置移动到目标位置的问题。应用逆运动学可以解出对应于目标位姿的各个关节角。其在t₀时刻的值为初始位置，在t_f时刻的值为该关节的目标位置。为了获得一条确定的光滑运动曲线，至光需要对θ(t)施加四个约束条件。由于初始值和目标值已知，并且在初始时刻和最终时刻关节的速度为零，因此可得到这四个约束条件为

θ(0)＝θ₀

θ(t_f)＝θ_f

$\stackrel{\·}{\mathrm{\θ}}\left(0\right)=0$

$\stackrel{\·}{\mathrm{\θ}}\left(t_f\right)=0$

这些约束条件唯一确定了一个三次多项式，该三次多项式有如下形式：

θ(t)＝a₀+a₁t+a₂t²+a₃t³

由四个约束条件可解出方程中的未知量a_i为：

a₀＝θ₀

a₁＝0

$a_2=\frac{3}{t_f^2}({\mathrm{\θ}}_f-{\mathrm{\θ}}_0)$

$a_3=-\frac{2}{t_f^3}({\mathrm{\θ}}_f-{\mathrm{\θ}}_0).$

Claims (8)

1.一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法，其中，机械臂为六自由度机械臂，包括基部舵机(4)、肩部舵机(5)、肘部舵机(6)、腕部舵机(7)、握手舵机(8)和腕部转动舵机(9)，其特征在于：

采用Camshift视觉跟踪方法精确定位目标工件的位置坐标信息，具体步骤如下：

步骤1.1)，根据待加工工件的特征信息，创建表示各种工件的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}和目标坐标系{G}实现对视觉机械臂、目标工件和工作台在同一坐标系下的准确定位；

步骤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}确定，即 表示从工作台坐标系{S}到基坐标系{B}的变换矩阵；

腕部坐标系{W}位于机械臂的末端连杆，x轴与腕部旋转轴重合，方向任意，y轴与腕部连杆重合，方向任意，z轴通过右手定则确定；

工具坐标系{T}附于机械臂所夹持工具的末端，x轴与腕部连杆重合，方向任意，y轴垂直于x轴，方向任意，z轴通过右手定则确定，当手部没有夹持工具时，工具坐标系{T}的原点位于机械臂的指端之间，工具坐标系{T}根据腕部坐标系{W}确定，即表示从工具坐标系{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的交点作为连杆坐标系{i}的原点；

步骤3)，规定连杆坐标系{i}中轴为沿关节轴i的指向；

步骤4)，规定连杆坐标系{i}中轴沿公垂线的指向，如果关节轴i和i+1相交，则规定轴垂直于关节轴i和i+1所在的平面；

步骤5)，按右手定则确定连杆坐标系{i}的轴；

将关节i对应坐标系{i-1}，用4个连杆参数来描述，具体如下：

1)a_i表示第i个连杆的长度；

2)α_i表示第i关节轴与第i+1关节轴线的夹角；

3)θ_i表示第i个连杆和第i+1个连杆的夹角；

4)d_i表示在第i个关节轴线上相邻两连杆的距离；

由坐标系{i-1}到坐标系{i}的变换顺序为：

1)绕轴旋转α_i-1角度；

2)沿轴平移a_i-1距离；

3)绕轴旋转θ_i角度；

4)沿轴平移d_i距离。

5.根据权利要求3所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法，其特征在于，所述步骤2.2)中，先进行坐标变换求出相对于基坐标系{B}的腕部坐标系{W}的变换矩阵 然后应用逆运动学求关节角。

6.根据权利要求5所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法，其特征在于，所述腕部坐标系{W}相对于基坐标系{B}的位置^BP_WORG＝[^Bx_WORG ^By_WORG ^Bz_WORG]^T，^BP_WORG表示腕部坐标系{W}的坐标原点P在基坐标系{B}的坐标位置，第1个连杆和第2个连杆的夹角第2个连杆和第3个连杆的夹角θ₂＝-(β+ψ)，其中， 第3个连杆和第4个连杆的夹角第4个连杆和第5个连杆的夹角θ₄＝-(θ₃+θ₂)，其中a_i表示第i个连杆的长度；

综上，通过几何解法计算得到机械臂各关节舵机角的准确值，总结如下：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=\arctan \frac{y_{\mathrm{WO}}^B}{x_{\mathrm{WO}}^B}\\ {\mathrm{\θ}}_2=-(\arctan \frac{z_{\mathrm{WO}}^1}{x_{\mathrm{WO}}^1}+\arccos \left(\frac{x_{\mathrm{WO}}^2{}_1+z_{\mathrm{WO}}^2{}_1+a_2^2-a_3^2}{{2a}_2\sqrt{z_{\mathrm{WO}}^2{}_1+z_{\mathrm{WO}}^2{}_1}}\right))\\ {\mathrm{\θ}}_3=\arccos \left(\frac{z_{\mathrm{WO}}^2{}_1+z_{\mathrm{WO}}^2{}_1-a_2^2-a_3^2}{{2a}_2{a}_3}\right)\\ {\mathrm{\θ}}_4=-({\mathrm{\θ}}_3+{\mathrm{\θ}}_2)\\ {\mathrm{\θ}}_5={\mathrm{\θ}}_1\end{array}\right.$

其中，θ₁表示基部舵机(4)的转动角度，θ₂表示肩部舵机(5)的转动角度，θ₃表示肘部舵机(6)的转动角度，θ₄表示腕部舵机(7)的转动角度，θ₅表示腕部转动舵机(9)的转动角度，握手舵机(8)的转动角度根据零件大小通过初始化设定来确定。

7.根据权利要求6所述基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制方法，其特征在于，利用关节空间规划法规划机械臂运动，设其轨迹为一条确定的光滑运动曲线θ(t)＝a₀+a₁t+a₂t²+a₃t³，记其在t₀时刻的值为初始位置，在t_f时刻为目标位置；

根据约束条件为：

$\left\{\begin{array}{c}\mathrm{\θ}\left(0\right)={\mathrm{\θ}}_0\\ \mathrm{\θ}\left(t_f\right)={\mathrm{\θ}}_f\\ \stackrel{.}{\mathrm{\θ}}\left(0\right)=0\\ \stackrel{.}{\mathrm{\θ}}\left(t_f\right)=0\end{array}\right.$

最终得到方程中未知量a_i为：

a₀＝θ₀

a₁＝0

$a_2=\frac{3}{t_f^2}({\mathrm{\θ}}_f-{\mathrm{\θ}}_0)$

$a_3=-\frac{2}{t_f^3}({\mathrm{\θ}}_f-{\mathrm{\θ}}_0)$

。

8.一种基于Camshift视觉跟踪和D-H建模算法的视觉机械臂控制装置，其中，机械臂为六自由度机械臂，包括基部舵机(4)、肩部舵机(5)、肘部舵机(6)、腕部舵机(7)、握手舵机(8)和腕部转动舵机(9)，其特征在于，控制装置包括：

USB相机(1)，位于机械臂腕部，扫描工作台上的各种零件，获取各个零件的定位信息；

卡片电脑(2)，接收USB相机(1)发送的定位信息，根据机械臂待完成动作指令，获取机械臂各舵机的控制参数；

机械臂舵机控制器(3)，接收卡片电脑(2)发送的各舵机的控制参数，分别传输至各舵机执行相应的动作。