CN103522305B - 一种使移动机械臂趋近并抓取目标物体的方法 - Google Patents

Abstract

本发明公开了一种使移动机械臂趋近与抓取目标物体的方法，目标物体为被双环标识图案环绕的有色圆柱，放置于贴有双十字标识图案的参照物的上方，其中，当移动机械臂的移动本体距目标物体较远时，移动机械臂的控制单元得到移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e；将e与e的变化率ec作为模糊控制器的输入，进而得到输出量u控制移动本体，使其趋近目标物体；当移动本体距目标物体较近时，它径直趋近，机械臂进行姿态调整；当目标物体在机械臂可操作范围内，移动本体停止运动，机械臂基于逆向运动学实现目标物体抓取。本发明只根据视觉信息实时决策，为移动机械臂在危险物排除、服务业等方面的应用提供技术保障。

Description

一种使移动机械臂趋近并抓取目标物体的方法

技术领域

本发明涉及机器人技术领域，特别是一种使移动机械臂趋近并抓取目标物体的方法。本发明特别适用于小型的移动机械臂系统。

背景技术

随着机器人应用领域的不断扩展，人们对机器人的能力需求也在不断的提高。机器人除了能够安全无碰的运动外，操作功能变得日益重要。移动机械臂兼具移动机器人的移动功能和机械臂的操作功能，适合代替人工作在许多危险的场合下，受到了广泛的关注。

趋近与抓取目标物体是一类典型的移动机械臂任务，在日常生活、危险物排除等方面具有重要的应用前景。对于一些低矮、狭窄的环境，装有CCD摄像头与图像采集卡的移动机械臂在体积等方面难以满足需求，因此，有必要对采用嵌入式视觉进行环境感知的移动机械臂开展深入的研究。

发明内容

(一)要解决的技术问题

本发明的目的是面向采用嵌入式视觉的移动机械臂的趋近与抓取目标物体问题，提供了一种使移动机械臂趋近与抓取目标物体的方法，使得移动机械臂基于嵌入式视觉获取的信息趋近目标物体，并保持其运动方向尽可能指向目标物体直至目标物体在机械臂的可操作范围内，而后机械臂实施抓取，达到令人满意的控制效果。

(二)技术方案

为解决上述技术问题，本发明提出一种使移动机械臂趋近并抓取目标物体的方法，所述移动机械臂包括移动本体、机械臂和控制单元，所述机械臂包括三个连杆和一个末端抓手，所述机械臂通过腰关节角、肩关节角、肘关节角、腕关节的俯仰关节角和腕关节的旋转关节角实现机械臂姿态的调整，通过调节末端抓手关节角对目标物体进行操作；在所述机械臂的末端抓手的上方固定一个图像撷取装置；所述目标物体为被双环标识图案环绕的有色圆柱，环绕有色圆柱的双环标识图案由不相邻的两同色环形区域上下排布而成，双环标识图案的两个环形区域的颜色与有色圆柱的颜色能够从视觉上相互区分，并且，所述目标物体放置于贴有双十字标识图案的参照物的上方；所述移动机械臂的控制单元实时获取来自所述图像撷取装置的图像信息，对目标物体上的双环标识图案和参照物上的双十字标识图案进行识别，如果只识别到双十字标识图案或者识别到的双环标识图案的两环形区域的中心像素间距d_pixel小于一个给定阈值d_T1时，表明所述移动机械臂的移动本体距所述目标物体较远；如果识别到的双环标识图案的两环形区域的中心像素间距d_pixel不小于给定阈值d_T1同时小于给定阈值d_T2时，表明所述移动机械臂的移动本体距所述目标物体较近；如果识别到的双环标识图案的两环形区域的中心像素间距d_pixel不小于给定阈值d_T2时，表明所述目标物体在所述机械臂的可操作范围内。

根据本发明的一种具体实施方式，所述抓取目标物体的方法包括如下步骤：步骤S1、当所述移动机械臂的移动本体距所述目标物体较远时，所述机械臂的姿态保持不变，所述控制单元基于所述参照物上的双十字标识图案控制所述移动本体趋近目标物体；步骤S2、当所述移动机械臂的移动本体距所述目标物体较近时，所述控制单元控制所述移动本体以一给定的速度径直趋近所述目标物体，所述移动机械臂根据所述目标物体在图像中的中心点以及所述双环标识图案的两环形区域的中心像素间距进行所述机械臂的姿态调整；步骤S3、当所述目标物体在所述机械臂的可操作范围内时，所述移动本体停止运动，所述控制单元基于逆向运动学控制机械臂抓取目标物体。

根据本发明的一种具体实施方式，所述步骤S1包括：步骤S11、所述控制单元基于所述图像撷取装置获取的所述参照物上的双十字标识图案提取双十字标识图案中各十字标识图案的中心，进而提取移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e；步骤S12、将移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e以及e的变化率ec作为模糊控制器的输入，结合设定的e和ec的量化等级和隶属函数，对模糊控制器的输入进行模糊化；步骤S13、设定模糊控制器输出变量u的量化等级和隶属函数；步骤S14、设置模糊控制规则；步骤S15、进行模糊推理；步骤S16、采用重心法进行解模糊，将模糊控制量转为对应的精确控制量，该精确控制量乘以比例因子k_u后作为最终输出量u；步骤S17、基于模糊控制器的最终输出量u，根据两轮驱动的移动本体的运动学模型，在移动本体速度V_p1大小一定的前提下，求取左驱动轮的运动速度v_l和右驱动轮的运动速度v_r，用于控制移动本体的运动方向，使其趋近目标物体。

根据本发明的一种具体实施方式，所述步骤S11包括：步骤S111、所述控制单元根据十字标识图案的中心点亮度较暗和中心对称特征，排除非目标的图像像素点，得到图像像素点集Ω_p1；步骤S112、控制单元对图像像素点集Ω_p1中的任意图像像素点p_c，检测一个识别模板中标记为“×”的图像像素点，根据图像像素点p_c的灰度值g(p_c)和设定阈值T_h对该识别模板进行二值化 $G_{\mathrm{pc}}\left(i\right)=\left\{\begin{array}{cc}0& g\left({\&times;}_i\right)<g\left(p_c\right)+T_h\\ 1& \mathrm{else}\end{array}\right.,$ 获取关于p_c的0-1模板数组G_pc，其中，g(×_i)为第i个模板点×_i的灰度值，所述识别模板为一个正方形，正方形的每条边包含T_n个待检测的图像像素点，T_n的初始值记为T_n1；步骤S113、统计数组G_pc中0和1的分布情况，满足十字标识图案的辐条数目为4、任意两辐条的图像像素宽度之差的绝对值不超过一个设定阈值T_a以及任意相邻两辐条之间的图像像素间隔之差的绝对值不超过一个设定阈值T_b的特征，则将图像像素点p_c作为候选图像像素点，如此，得到候选图像像素点集Ω_p2；步骤S114、根据像素坐标对候选图像像素点集Ω_p2进行滤波，并聚成两类，用均值法得到双十字标识图案中各十字标识图案的中心，记为(u₁，v₁)和(u₂，v₂)；步骤S115、根据所述双十字标识图案中的两十字标识图案的中心的像素距离d_p更新T_n： $T_n=\left\{\begin{array}{cc}{T}_{n1}& d_p<41\\ T_{n2}& 41\&le;d_p<83\\ T_{n3}& d_p\&GreaterEqual;83\end{array}\right.;$ 步骤S116、控制单元提取移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e：式中u₀为图像撷取装置的光心的横轴图像坐标，f为图像撷取装置的焦距。

根据本发明的一种具体实施方式，所述步骤S2包括：步骤S21、所述控制单元控制移动本体以一给定的速度径直趋近目标物体；移动本体以给定的速度V_p2向目标物体径直趋近，其中左驱动轮的运动速度v_l＝V_p2，右驱动轮的运动速度v_r＝V_p2；步骤S22、所述控制单元根据图像撷取装置获取的目标物体的视觉信息调整机械臂的姿态。

根据本发明的一种具体实施方式，所述步骤S22包括：步骤S221、图像撷取装置提取目标物体的视觉信息；步骤S222、控制单元计算目标物体相对于移动本体的位置信息；步骤S223、控制单元根据目标物体在图像中的中心点以及双环标识图案的两环形区域的中心像素间距控制机械臂调整腰关节角和腕关节的俯仰关节角。

根据本发明的一种具体实施方式，所述步骤S221为：根据颜色特征提取目标物体图像的ROI区域，获取ROI的中心点(u_c，v_c)，并将其作为目标物体在图像中的中心点；在ROI区域内从左到右逐列扫描，提取出双环标识图案的各环形区域，用均值法计算得到双环标识图案的两环形区域的中心像素间距d_pixel。

根据本发明的一种具体实施方式，所述步骤S222为：根据双环标识图案的两环形区域的中心的实际间距D和小孔成像模型D_OC=D*f/d_pixel，确定目标物体相对于图像撷取装置的位置[X_OC，Y_OC，Z_OC]：

$\left\{\begin{array}{c}{X}_{\mathrm{OC}}=D_{\mathrm{OC}}\cos {\mathrm{\&theta;}}_v\sin {\mathrm{\&theta;}}_h\\ Y_{\mathrm{OC}}=D_{\mathrm{OC}}\cos {\mathrm{\&theta;}}_v{\cos \mathrm{\&theta;}}_h\\ Z_{\mathrm{OC}}=D_{\mathrm{OC}}\sin {\mathrm{\&theta;}}_v\end{array}\right.,$ 式中，θ_v=θ₁+θ₂+θ₃-atan((v₀-v_c)／f)为目标物体与图像撷取装置连线的竖直偏差角，θ_h=θ₅+atan((u₀-u_c)／f)为目标物体与图像撷取装置连线的水平偏差角，v₀为图像撷取装置的光心的纵轴图像坐标；θ₅为腰关节角，θ₁为肩关节角、θ₁为肘关节角，θ₃为腕关节的俯仰关节角；图像撷取装置相对于移动本体的位置可根据下式计算： $\left\{\begin{array}{c}{X}_{\mathrm{CR}}=\left(\underset{p=1}{\overset{3}{\mathrm{\&Sigma;}}}{l}_p\sin \left(\underset{k=1}{\overset{p}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_k\right)\right)\sin {\mathrm{\&theta;}}_5\\ Y_{\mathrm{CR}}=\left(\underset{p=1}{\overset{3}{\mathrm{\&Sigma;}}}{l}_p\sin \left(\underset{k=1}{\overset{p}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_k\right)\right)\cos {\mathrm{\&theta;}}_5\\ Z_{\mathrm{CR}}=\underset{p=1}{\overset{3}{\mathrm{\&Sigma;}}}{l}_p\sin \left(\underset{k=1}{\overset{p}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_k\right)\end{array}\right.,$ 式中，l_p(p=1，2，3)是机械臂的三个连杆的长度；机械臂末端需要到达的位置(X_OR，Y_OR，Z_OR)为：X_OR=X_OC+X_CR，Y_OR=Y_OC+Y_CR，Z_OR=Z_OC+Z_CR。

根据本发明的一种具体实施方式，在步骤S223中，所述机械臂的腰关节角进行如下调整：Δθ₅=atan2(X_OR，Y_OR)-θ₅；机械臂的腕关节的俯仰关节角进行如下调整：Δθ₃=atan2(u_c-u₀，2u₀)。

根据本发明的一种具体实施方式，在步骤S3中，通过肩关节、肘关节、腕关节的俯仰关节和腰关节的运动以使得机械臂到达抓取位置；结合抓取时末端抓手关节水平的约束，即θ₁+θ₂+θ₃=0，基于逆向运动学求解关节角：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_5=a\tan 2(X_{\mathrm{OR}},Y_{\mathrm{OR}})\\ {\mathrm{\&theta;}}_2=-a\cos ((A^2+B^2-{l_1}^2-{l_2}^2)/\left(2l_1{l}_2\right))\\ {\mathrm{\&theta;}}_1=a\cos \left(\frac{(l_1+l_2\cos {\mathrm{\&theta;}}_2)A+l_2\sin {\mathrm{\&theta;}}_{2}B}{{l_1}^2+{l_2}^2+2l_1{l}_2\cos {\mathrm{\&theta;}}_2}\right)\\ {\mathrm{\&theta;}}_3=-{\mathrm{\&theta;}}_1-{\mathrm{\&theta;}}_2\end{array}\right.,$ 其中，A=Y_OR／cosθ₅-l₃，B=Z_OR；在机械臂的肩关节角、肘关节角、腕关节的俯仰关节角和腰关节角到达逆向运动学解算的位置后，末端抓手缩进完成抓取操作。

(三)有益效果

本发明一种移动机械臂趋近与抓取目标物体的方法，通过嵌入式的方式处理图像撷取装置获取的视觉信息，实时性好，环境适应性较好。

附图说明

图1是本发明所应用的移动机械臂的示意图；

图2是本发明所应用的被双环标识图案环绕的目标物体的示例的示意图；

图3是本发明所应用的目标物体放置于贴有双十字标识图案的参照物的上方的示意图；

图4是使移动机械臂趋近与抓取目标物体的控制框图；

图5是本发明的十字标识图案的识别模板示意图；

图6是本发明的模糊控制器的输入夹角e的隶属函数图；

图7是本发明的模糊控制器的输入夹角e的变化率ec的隶属函数图；

图8是本发明的模糊控制器的输出u的隶属函数图；

图9是本发明的实施例的移动机械臂对目标物体的趋近轨迹图。

具体实施方式

为使本发明的目的、技术方案更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。虽然本文可提供包含特定值的参数的示范，但应了解，参数无需确切等于相应的值，而是可在可接受的误差容限或设计约束内近似于所述值。

本发明给出了一种移动机械臂趋近与抓取目标物体的方法，该方法能够使一个移动机械臂趋近并抓取一个目标物体。

本发明的移动机械臂是指具有移动能力和操作能力的移动机器人，包括两轮驱动的移动本体、机械臂和控制单元，如图1所示，机械臂包括三个连杆l₁，l₂，l₃和一个末端抓手，通过腰关节角θ₅、肩关节角θ₁、肘关节角θ₂、腕关节的俯仰关节角θ₃、腕关节的旋转关节角θ₄实现机械臂姿态的调整，通过调节末端抓手关节角θ₆对目标物体进行操作；在所述机械臂的末端抓手的上方固定一个图像撷取装置，例如CMOS摄像头；控制单元用于根据所述机械臂的图像撷取装置获得的图像信息来控制所述移动本体的运动以及机械臂的动作。

本发明的移动机械臂所要抓取的目标物体为被双环标识图案环绕的有色圆柱，环绕有色圆柱的双环标识图案由不相邻的两同色环形区域上下排布而成，双环标识图案的两环形区域的颜色与有色圆柱的颜色应当能够从视觉上相互区分，例如有色圆柱的颜色为红色，双环标识图案的颜色为黑色。图2给出了一个典型的被双环标识图案环绕的目标物体的示例的示意图。如图2所示，该示例中的所述目标物体为被黑色的双环标识图案环绕的红色圆柱。

本发明的目标物体放置于贴有双十字标识图案的参照物的上方，如图3所示，每个十字标识图案的辐条数目都为4。

下面以图1所示的移动机械臂、图2所示的目标物体以及图3所示的目标物体位于贴有双十字标识图案的参照物的上方为例来说明本发明的方法。

本发明的移动机械臂的控制单元实时获取来自图像撷取装置的图像信息，对目标物体上的双环标识图案和参照物上的双十字标识图案进行识别，如果只识别到双十字标识图案或者识别到的双环标识图案的两环形区域的中心像素间距d_pixel小于一个给定阈值d_T1时，表明移动本体距目标物体较远；如果识别到的双环标识图案的两环形区域的中心像素间距d_pixel不小于给定阈值d_T1同时小于给定阈值d_T2时，表明移动本体距目标物体较近；如果识别到的双环标识图案的两环形区域的中心像素间距d_pixel不小于给定阈值d_T2时，表明目标物体在机械臂的可操作范围内。

当移动本体距目标物体较远时，机械臂的姿态保持不变，移动机械臂的控制单元识别参照物上的双十字标识图案，得到移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e；将e与e的变化率ec作为模糊控制器的输入并进行模糊化，依据模糊推理得出模糊控制量，进而依据重心法解模糊后给出精确控制量，精确控制量乘以比例因子得到模糊控制器的输出量u；结合输出量u，根据两轮驱动的移动本体的运动学模型，在移动本体速度V_p1大小一定的前提下，得到左驱动轮的运动速度v_l和右驱动轮的运动速度v_r，用于控制移动本体的运动方向，使其趋近目标物体。当移动本体距目标物体较近时，移动本体径直趋近，同时控制机械臂进行姿态调整尽可能使得目标物体位于图像撷取装置的视野的中心，机械臂姿态的调整通过对机械臂腰关节和腕关节的俯仰关节的调整实现。当目标物体在机械臂的可操作范围内，移动本体停止运动，同时机械臂基于逆向运动学实现目标物体抓取。

下面参照图4来具体论述移动机械臂的趋近与抓取目标物体的方法的流程。图4所示为整个方法的控制框图，其中k_e，k_ec为量化因子，k_u为比例因子。如图4所示，本发明的方法包括如下步骤：

步骤S1、当移动机械臂的移动本体距目标物体较远时，机械臂的姿态保持不变，控制单元基于参照物上的双十字标识图案控制移动本体趋近目标物体。该步骤S1具体包括如下分步骤：

步骤S11、控制单元基于图像撷取装置获取的参照物上的双十字标识图案提取双十字标识图案中各十字标识图案的中心，进而提取移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e。该步骤S11又具体包括：

步骤S111、控制单元根据十字标识图案的中心点亮度较暗和中心对称特征，排除非目标的图像像素点，得到图像像素点集Ω_p1;

步骤S112、控制单元对图像像素点集Ω_p1中的任意图像像素点p_c，检测一个识别模板中标记为“×”的图像像素点，根据图像像素点p_c的灰度值g(p_c)和设定阈值T_h对该识别模板进行二值化 $G_{\mathrm{pc}}\left(i\right)=\left\{\begin{array}{cc}0& g\left({\&times;}_i\right)<g\left(p_c\right)+T_h\\ 1& \mathrm{else}\end{array}\right.,$ 获取关于p_c的0-1模板数组G_pc，其中，g(×_i)为第i个模板点×_i的灰度值；

图5显示了识别模板。如图5所示，识别模板为一个正方形，正方形的每条边包含T_n个待检测的图像像素点，T_n的初始值记为T_n1。

步骤S113、统计数组G_pc中0和1的分布情况，满足十字标识图案的辐条数目为4、任意两辐条的图像像素宽度之差的绝对值不超过一个设定阈值T_a以及任意相邻两辐条之间的图像像素间隔之差的绝对值不超过一个设定阈值T_b的特征，则将图像像素点p_c作为候选图像像素点，如此，得到候选图像像素点集Ω_p2；

步骤S114、根据像素坐标对候选图像像素点集Ω_p2进行滤波，并聚成两类，用均值法得到双十字标识图案中各十字标识图案的中心，记为（u₁，v₁)和(u₂，v₂)；

步骤S115、根据双十字标识图案中的两十字标识图案的中心的像素距离d_p更新T_n：

$T_n=\left\{\begin{array}{cc}{T}_{n1}& d_p<41\\ T_{n2}& 41\&le;d_p<83\\ T_{n3}& d_p\&GreaterEqual;83\end{array};\right.$

步骤S116、控制单元提取移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e。

$e=a\tan 2(\frac{u_1+u_2}{2}-u_0,f)$

式中u₀为图像撷取装置的光心的横轴图像坐标，f为图像撷取装置的焦距。

步骤S12、将移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e以及e的变化率ec作为模糊控制器的输入，结合设定的e和ec的量化等级和隶属函数，对模糊控制器的输入进行模糊化。

移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e，量化因子定为k_e=6／e_th；将e量化为论域[-1，1]中的值，并按一定隶属度关系使该值与模糊变量E的7个语言取值{NB，NM，NS，Z，PS，PM，PB}相对应，隶属函数如图6所示。

移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e的变化率为ec，量化因子定为k_ec=1／ec_th；将ec量化为论域[-1，1]中的值，并按一定隶属度关系使该值与模糊变量E_C的5个语言取值{NB，NS，Z，PS，PB}相对应，隶属函数如图7所示。

步骤S13、设定模糊控制器输出变量u的量化等级和隶属函数。

输出变量u为移动本体需要偏转的角度，以移动本体当前运动方向为基准，左转为负，右转为正。比例因子定为k_u=1／u_th；将u量化为论域[-1，1]中的值，并按一定隶属度关系使该值与模糊变量U的7个语言取值{NB，NM，NS，ZE，PS，PM，PB}相对应，隶属函数如图8所示。

步骤S14、设置模糊控制规则

根据模糊控制规则的设计原理，结合对移动本体方向调整的控制经验，总结35条模糊控制规则(表1)。

表1模糊控制规则表

步骤S15、进行模糊推理

计算被激活的模糊规则的隶属度值如下：

其中，μ_i(E_j，E_Ck，U_h，e，ec)是第i条被激活的模糊规则的隶属度值，m是规则库中被激活的模糊规则个数。

步骤S16、采用重心法进行解模糊，将模糊控制量转为对应的精确控制量，该精确控制量乘以比例因子k_u后作为最终输出量u。

解模糊即清晰化，采用重心法解模糊得到精确控制量为式中，b_h是模糊输出语言变量的隶属度函数中心。

依据上述图6、图7、图8和表1的各输入变量、输出变量及其相互间的模糊控制规则，通过解模糊可确定表2所示的模糊控制查询表。

表2模糊控制查询表

在给定e和ec量化后的论域值的基础上，查询表2可获得相应的模糊控制的精确控制量，该量值乘以比例因子k_u后作为最终输出量u。当各输入变量、控制量的隶属函数或其相互间的模糊控制规则发生变化时，应重新计算并确定模糊控制查询表。

步骤S17、基于模糊控制器的最终输出量u，根据两轮驱动的移动本体的运动学模型，在移动本体速度V_p1大小一定的前提下，求取左驱动轮的运动速度v_l和右驱动轮的运动速度v_r，用于控制移动本体的运动方向，使其趋近目标物体。

左驱动轮的运动速度v_l和右驱动轮的运动速度v_r计算如下：

$\left\{\begin{array}{c}{v}_r=2*V_{p1}/(2+k_{\mathrm{ue}}*u)\\ v_l=2*V_{p1}-v_r\end{array}\right.$

其中，k_ue为比例系数。

步骤S2、当移动机械臂的移动本体距目标物体较近时，控制单元控制移动本体以一给定的速度径直趋近目标物体，移动机械臂根据目标物体在图像中的中心点以及双环标识图案的两环形区域的中心像素间距进行机械臂的姿态调整。

步骤S21、控制单元控制移动本体以一给定的速度径直趋近目标物体。

移动本体以给定的速度V_p2向目标物体径直趋近，其中左驱动轮的运动速度v_l＝V_p2，右驱动轮的运动速度v_r＝V_p2。

步骤S22、控制单元根据图像撷取装置获取的目标物体的视觉信息调整机械臂的姿态。

为了尽可能使得目标物体位于图像撷取装置的视野的中心，需要对机械臂的姿态进行调整。步骤S22包括如下分步骤：

步骤S221、图像撷取装置提取目标物体的视觉信息。

根据颜色特征提取目标物体图像的ROI(RegionOfInterest)区域，获取ROI的中心点(u_c，v_c)，并将其作为目标物体在图像中的中心点；在ROI区域内从左到右逐列扫描，提取出双环标识图案的各环形区域，用均值法计算得到双环标识图案的两环形区域的中心像素间距d_pixel。

步骤S222、控制单元计算目标物体相对于移动本体的位置信息。

根据双环标识图案的两环形区域的中心的实际间距D和小孔成像模型D_OC=D*f/d_pixel，确定目标物体相对于图像撷取装置的位置[X_OC，Y_OC，Z_OC]：

$\left\{\begin{array}{c}{X}_{\mathrm{OC}}=D_{\mathrm{OC}}\cos {\mathrm{\&theta;}}_v\sin {\mathrm{\&theta;}}_h\\ Y_{\mathrm{OC}}=D_{\mathrm{OC}}\cos {\mathrm{\&theta;}}_v{\cos \mathrm{\&theta;}}_h\\ Z_{\mathrm{OC}}=D_{\mathrm{OC}}\sin {\mathrm{\&theta;}}_v\end{array}\right.$

式中，θ_v=θ₁+θ₂+θ₃-atan((v₀-v_c)／f)为目标物体与图像撷取装置连线的竖直偏差角，θ_h=θ₅+atan((u₀-u_c)／f)为目标物体与图像撷取装置连线的水平偏差角，v₀为图像撷取装置的光心的纵轴图像坐标。

由于图像撷取装置固定于机械臂的末端抓手上方，于是，图像撷取装置相对于移动本体的位置可根据下式计算：

$\left\{\begin{array}{c}{X}_{\mathrm{CR}}=\left(\underset{p=1}{\overset{3}{\mathrm{\&Sigma;}}}{l}_p\sin \left(\underset{k=1}{\overset{p}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_k\right)\right)\sin {\mathrm{\&theta;}}_5\\ Y_{\mathrm{CR}}=\left(\underset{p=1}{\overset{3}{\mathrm{\&Sigma;}}}{l}_p\sin \left(\underset{k=1}{\overset{p}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_k\right)\right)\cos {\mathrm{\&theta;}}_5\\ Z_{\mathrm{CR}}=\underset{p=1}{\overset{3}{\mathrm{\&Sigma;}}}{l}_p\sin \left(\underset{k=1}{\overset{p}{\mathrm{\&Sigma;}}}{\mathrm{\&theta;}}_k\right)\end{array}\right.$

式中，l_p(p=1，2，3)是机械臂的三个连杆的长度。

进而得到机械臂末端需要到达的位置(X_OR，Y_OR，Z_OR)，其中，X_OR=X_OC+X_CR，Y_OR=Y_OC+Y_CR，Z_OR=Z_OC+Z_CR。

步骤S223、控制单元根据目标物体在图像中的中心点以及双环标识图案的两环形区域的中心像素间距控制机械臂调整腰关节角和腕关节的俯仰关节角。

机械臂的腰关节角进行如下调整：Δθ₅=atan2(X_OR，Y_OR)-θ₅；机械臂的腕关节的俯仰关节角进行如下调整：Δθ₃=atan2(u_c-u₀，2u₀)。

步骤S3、当目标物体在机械臂的可操作范围内时，移动本体停止运动，控制单元基于逆向运动学控制机械臂抓取目标物体。

通过肩关节、肘关节、腕关节的俯仰关节和腰关节的运动以使得机械臂到达抓取位置。结合抓取时末端抓手关节水平的约束，即θ₁+θ₂+θ₃=0，基于逆向运动学求解关节角：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_5=a\tan 2(X_{\mathrm{OR}},Y_{\mathrm{OR}})\\ {\mathrm{\&theta;}}_2=-a\cos ((A^2+B^2-{l_1}^2-{l_2}^2)/\left(2l_1{l}_2\right))\\ {\mathrm{\&theta;}}_1=a\cos \left(\frac{(l_1+l_2\cos {\mathrm{\&theta;}}_2)A+l_2\sin {\mathrm{\&theta;}}_{2}B}{{l_1}^2+{l_2}^2+2l_1{l}_2\cos {\mathrm{\&theta;}}_2}\right)\\ {\mathrm{\&theta;}}_3=-{\mathrm{\&theta;}}_1-{\mathrm{\&theta;}}_2\end{array}\right.$

其中，A=Y_OR／cosθ₅-l₃，B=Z_OR。

在机械臂的肩关节角、肘关节角、腕关节的俯仰关节角和腰关节角到达逆向运动学解算的位置后，末端抓手缩进完成抓取操作。

在具体实施时，本发明的移动机械臂的图像撷取装置可以是CMOS摄像头OV9653，控制单元可以是嵌入式控制单元，嵌入式控制单元可以主要包括基于ARM9的S3C2440A处理器和ATmega128；移动本体采用两轮驱动方式，包括两个电机驱动器、左电机、右电机、两个驱动轮和一个万向轮，其中电机驱动器可采用铭朗科技的MLDS3605-A，电机采用Portescap的直流电机22N48-210E-308。图像撷取装置的焦距f＝350，图像撷取装置的光心的图像坐标[u₀，v₀]=[160，120]。移动机械臂参数为l₁=9.7cm，l₂=8.9cm，1₃=9.55cm。双环标识图案的两环形区域的中心的实际间距D=1.45cm。机械臂的关节角初始值如下：θ₁=118°，θ₂=-26°，θ₃=-111°，θ₄=0°，θ₅=0°，末端抓手关节处于完全张开状态。取移动本体距离目标物体较远时移动本体的速度V_p1＝15cm／s，移动本体距离目标物体较近时移动本体的速度V_p2=3.08cm／s，d_T1=15，d_T2=45，T_h=30，T_n1=6，T_n2=16，T_n3=30，T_a=3，T_b=5，e_th=1.43，ec_th=5，u_th=1，k_ue=1。

采用本发明所提供的方法，能够满足实际的需求，图9给出了移动机械臂对目标物体的趋近轨迹，移动机械臂顺利的从起始位置S_r运动到终止位置E_r。而后，移动机械臂的移动本体停止运动，机械臂执行抓取操作进而完成对目标物体的抓取。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种使移动机械臂趋近并抓取目标物体的方法，

所述移动机械臂包括移动本体、机械臂和控制单元，所述机械臂包括三个连杆和一个末端抓手，所述机械臂通过腰关节角、肩关节角、肘关节角、腕关节的俯仰关节角和腕关节的旋转关节角实现机械臂姿态的调整，通过调节末端抓手关节角对目标物体进行操作；在所述机械臂的末端抓手的上方固定一个图像撷取装置；

所述目标物体为被双环标识图案环绕的有色圆柱，环绕有色圆柱的双环标识图案由不相邻的两同色环形区域上下排布而成，双环标识图案的两个环形区域的颜色与有色圆柱的颜色能够从视觉上相互区分，并且，所述目标物体放置于贴有双十字标识图案的参照物的上方；其特征在于：

所述移动机械臂的控制单元实时获取来自所述图像撷取装置的图像信息，对目标物体上的双环标识图案和参照物上的双十字标识图案进行识别，如果只识别到双十字标识图案或者识别到的双环标识图案的两环形区域的中心像素间距d_pixel小于一个给定阈值d_T1时，表明所述移动机械臂的移动本体距所述目标物体较远；如果识别到的双环标识图案的两环形区域的中心像素间距d_pixel不小于给定阈值d_T1同时小于给定阈值d_T2时，表明所述移动机械臂的移动本体距所述目标物体较近；如果识别到的双环标识图案的两环形区域的中心像素间距d_pixel不小于给定阈值d_T2时，表明所述目标物体在所述机械臂的可操作范围内。

2.如权利要求1所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，所述抓取目标物体的方法包括如下步骤：

步骤S1、当所述移动机械臂的移动本体距所述目标物体较远时，所述机械臂的姿态保持不变，所述控制单元基于所述参照物上的双十字标识图案控制所述移动本体趋近目标物体；

步骤S2、当所述移动机械臂的移动本体距所述目标物体较近时，所述控制单元控制所述移动本体以一给定的速度径直趋近所述目标物体，所述移动机械臂根据所述目标物体在图像中的中心点以及所述双环标识图案的两环形区域的中心像素间距进行所述机械臂的姿态调整；

步骤S3、当所述目标物体在所述机械臂的可操作范围内时，所述移动本体停止运动，所述控制单元基于逆向运动学控制机械臂抓取目标物体。

3.如权利要求2所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，所述步骤S1包括：

步骤S11、所述控制单元基于所述图像撷取装置获取的所述参照物上的双十字标识图案提取双十字标识图案中各十字标识图案的中心，并分别记为(u₁,v₁)和(u₂,v₂)，进而提取移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e：

$e=atan2(\frac{u_1+u_2}{2}-u_0,f)$

式中u₀为图像撷取装置的光心的横轴图像坐标，f为图像撷取装置的焦距；

步骤S12、将移动本体与双十字标识图案中心连线的水平方向和移动本体运动方向的夹角e以及e的变化率ec作为模糊控制器的输入，结合设定的e和ec的量化等级和隶属函数，对模糊控制器的输入进行模糊化；

步骤S13、设定模糊控制器输出变量u的量化等级和隶属函数；

步骤S14、设置模糊控制规则；

步骤S15、进行模糊推理；

步骤S16、采用重心法进行解模糊，将模糊控制量转为对应的精确控制量，该精确控制量乘以比例因子k_u后作为最终输出量u；

步骤S17、基于模糊控制器的最终输出量u，根据两轮驱动的移动本体的运动学模型，在移动本体速度V_p1大小一定的前提下，求取左驱动轮的运动速度v_l和右驱动轮的运动速度v_r，用于控制移动本体的运动方向，使其趋近目标物体。

4.如权利要求3所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，所述步骤S11包括：

步骤S111、所述控制单元根据十字标识图案的中心点亮度较暗和中心对称特征，排除非目标的图像像素点，得到图像像素点集Ω_p1；

步骤S112、所述控制单元对图像像素点集Ω_p1中的任意图像像素点p_c，检测一个识别模板中标记为“×”的图像像素点，根据图像像素点p_c的灰度值g(p_c)和设定阈值T_h对该识别模板进行二值化 $G_{pc}\left(i\right)=\left\{\begin{array}{cc}0& g\left({\&times;}_i\right)<g\left(p_c\right)+T_h\\ 1& else\end{array}\right.,$ 获取关于p_c的0-1模板数组G_pc，其中，g(×_i)为第i个模板点×_i的灰度值，所述识别模板为一个正方形，正方形的每条边包含T_n个待检测的图像像素点，T_n的初始值记为T_n1；

步骤S113、统计数组G_pc中0和1的分布情况，满足十字标识图案的辐条数目为4、任意两辐条的图像像素宽度之差的绝对值不超过一个设定阈值T_a以及任意相邻两辐条之间的图像像素间隔之差的绝对值不超过一个设定阈值T_b的特征，则将图像像素点p_c作为候选图像像素点，如此，得到候选图像像素点集Ω_p2；

步骤S114、根据像素坐标对候选图像像素点集Ω_p2进行滤波，并聚成两类，用均值法得到双十字标识图案中各十字标识图案的中心；

步骤S115、根据所述双十字标识图案中的两十字标识图案的中心的像素距离d_p更新T_n：

$T_n=\left\{\begin{array}{cc}{T}_{n1}& d_p<41\\ T_{n2}& 41\&le;d_p<83\\ T_{n3}& d_p\&GreaterEqual;83\end{array}\right..$

5.如权利要求2所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，所述步骤S2包括：

步骤S21、所述控制单元控制移动本体以一给定的速度径直趋近目标物体；移动本体以给定的速度V_p2向目标物体径直趋近，其中左驱动轮的运动速度v_l＝V_p2，右驱动轮的运动速度v_r＝V_p2；

步骤S22、所述控制单元根据图像撷取装置获取的目标物体的视觉信息调整机械臂的姿态。

6.如权利要求5所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，所述步骤S22包括：

步骤S221、图像撷取装置提取目标物体的视觉信息；

步骤S222、控制单元计算目标物体相对于移动本体的位置信息；

步骤S223、控制单元根据目标物体在图像中的中心点以及双环标识图案的两环形区域的中心像素间距控制机械臂调整腰关节角和腕关节的俯仰关节角。

7.如权利要求6所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，所述步骤S221为：根据颜色特征提取目标物体图像的ROI区域，获取ROI的中心点(u_c,v_c)，并将其作为目标物体在图像中的中心点；在ROI区域内从左到右逐列扫描，提取出双环标识图案的各环形区域，用均值法计算得到双环标识图案的两环形区域的中心像素间距d_pixel。

8.如权利要求6所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，所述步骤S222为：

根据双环标识图案的两环形区域的中心的实际间距D和小孔成像模型D_OC＝D*f/d_pixel，确定目标物体相对于图像撷取装置的位置[X_OC,Y_OC,Z_OC]：

$\left\{\begin{array}{c}{X}_{OC}=D_{OC}cos{\mathrm{\&theta;}}_{v}sin{\mathrm{\&theta;}}_h\\ Y_{OC}=D_{OC}{\mathrm{cos\&theta;}}_v{\mathrm{cos\&theta;}}_h\\ Z_{OC}=D_{OC}{\mathrm{sin\&theta;}}_v\end{array}\right.$

式中，θ_v＝θ₁+θ₂+θ₃-atan((v₀-v_c)/f)为目标物体与图像撷取装置连线的竖直偏差角，θ_h＝θ₅+atan((u₀-u_c)/f)为目标物体与图像撷取装置连线的水平偏差角，v₀为图像撷取装置的光心的纵轴图像坐标；θ₅为腰关节角，θ₁为肩关节角、θ₂为肘关节角，θ₃为腕关节的俯仰关节角，u₀为图像撷取装置的光心的横轴图像坐标，f为图像撷取装置的焦距，u_c和v_c为ROI区域的中心点的横、纵坐标；

图像撷取装置相对于移动本体的位置根据下式计算：

$\left\{\begin{array}{c}{X}_{CR}=\left(\underset{p=1}{\overset{3}{\&Sigma;}}{l}_{p}sin\left(\underset{k=1}{\overset{p}{\&Sigma;}}{\mathrm{\&theta;}}_k\right)\right)sin{\mathrm{\&theta;}}_5\\ Y_{CR}=\left(\underset{p=1}{\overset{3}{\&Sigma;}}{l}_p\sin \left(\underset{k=1}{\overset{p}{\&Sigma;}}{\mathrm{\&theta;}}_k\right)\right)\cos {\mathrm{\&theta;}}_5\\ Z_{CR}=\underset{p=1}{\overset{3}{\&Sigma;}}{l}_p\sin \left(\underset{k=1}{\overset{p}{\&Sigma;}}{\mathrm{\&theta;}}_k\right)\end{array}\right.$

式中，l_p(p＝1,2,3)是机械臂的三个连杆的长度；

机械臂末端需要到达的位置(X_OR,Y_OR,Z_OR)为：X_OR＝X_OC+X_CR，Y_OR＝Y_OC+Y_CR，Z_OR＝Z_OC+Z_CR。

9.如权利要求6所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，在步骤S223中，所述机械臂的腰关节角进行如下调整：Δθ₅＝atan2(X_OR,Y_OR)-θ₅；机械臂的腕关节的俯仰关节角进行如下调整：Δθ₃＝atan2(u_c-u₀,2u₀)，其中X_OR、Y_OR分别为机械臂末端需要到达的位置的X、Y方向上的坐标，u₀为图像撷取装置的光心的横轴图像坐标，u_c为ROI区域的中心点的横坐标。

10.如权利要求2所述的使移动机械臂趋近并抓取目标物体的方法，其特征在于，在步骤S3中，通过肩关节、肘关节、腕关节的俯仰关节和腰关节的运动以使得机械臂到达抓取位置；结合抓取时末端抓手关节水平的约束，即θ₁+θ₂+θ₃＝0，基于逆向运动学求解关节角：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_5=atan2(X_{OR},Y_{OR})\\ {\mathrm{\&theta;}}_2=-acos\left(\right(A^2+B^2-{l_1}^2-{l_2}^2)/(2l_1{l}_2\left)\right)\\ {\mathrm{\&theta;}}_1=acos\left(\frac{(l_1+l_2{\mathrm{cos\&theta;}}_2)A+l_2{\mathrm{sin\&theta;}}_{2}B}{{l_1}^2+{l_2}^2+2l_1{l}_2{\mathrm{cos\&theta;}}_2}\right)\\ {\mathrm{\&theta;}}_3=-{\mathrm{\&theta;}}_1-{\mathrm{\&theta;}}_2\end{array}\right.$

其中，A＝Y_OR/cosθ₅-l₃，B＝Z_OR，θ₅为腰关节角、θ₁为肩关节角、θ₂为肘关节角，θ₃为腕关节的俯仰关节角，l₁、l₂、l₃为机械臂的三个连杆的长度，X_OR、Y_OR、Z_OR分别为机械臂末端需要到达的位置的X、Y、Z方向上的坐标；

在机械臂的肩关节角、肘关节角、腕关节的俯仰关节角和腰关节角到达逆向运动学解算的位置后，末端抓手缩进完成抓取操作。