CN107450376A - 一种基于智能移动平台的服务机械臂抓取姿态角计算方法 - Google Patents

Abstract

本发明公开了一种基于智能移动平台的服务机械臂抓取姿态角计算方法，属于机械臂运动控制领域，尤其涉及机械臂的姿态角分析计算。根据相机确定目标物在相机坐标系中的坐标位置；通过提前标定好的转换矩阵将目标物在相机坐标系中的坐标转换到机械臂坐标系中；通过转换后的目标物的坐标点计算目标物在机械臂坐标系中与机械坐标系中的X轴的夹角θ，通过该夹角θ与机械臂姿态中的三个分量的映射函数关系求得当前机械臂的姿态；将目标物坐标和机械臂末端姿态合成控制机械臂的目标位姿，发送给机械臂，即完成目标物抓取动作。通过目标物与机械臂坐标系X轴的夹角θ来确定姿态角中的各个分量，从而完成机械臂以指定的姿态去抓取目标物。

Description

一种基于智能移动平台的服务机械臂抓取姿态角计算方法

技术领域

本发明属于机械臂运动控制领域，尤其涉及机械臂的姿态角分析计算。机械臂姿态角的计算方法为使用目标物与坐标系的夹角与各个姿态角的映射函数关系式来计算。

背景技术

目前在智能化时代中，我国正处在大力推进工业化进程中，促使国内的工业机器人技术不断革新，而机械臂作为机器人的最主要的执行机构，其应用越来越普及，已逐渐渗透到军事、航天、医疗、日常生活及教育娱乐等各个方面，所以对它的研究越来越受到技术人员的关注。

机械臂的应用包括了机械学、力学、电子学、软件、算法和控制论等各个学科领域的相关知识，其重点研究领域是：机械臂运动学和动力学，机械臂的结构设计以及路径规划等。在具体使用时，需要考虑结构设计、控制***的设计、运动学分析、动力学分析以及轨迹规划研究等多个方面。机械臂性能将影响到整个机器人的设计质量。机械臂的作用是使末端执行器到达所期望的目标位置处完成相应的作业任务。而机械臂的自由度数量与结构形式对末端执行器的灵活度、运动精度以及工作空间有直接的关系。

机械臂可以根据机械臂手臂坐标、驱动方式以及用途进行分类。按照手臂坐标进行分类，可以分为直角坐标结构机械臂、极坐标结构机械臂、关节型机械手机械臂；根据驱动方式分类，可以分为液压传动、气压传动、电力传动、机械传动等机械臂；根据用途分类有专用机械臂、通用机械臂两类。在机械臂的控制技术上，主要分为四类，分别是自适应控制、变结构控制、现代鲁棒控制与智能控制。

目前应用最多的机械臂为六自由度机械臂，其由六个旋转关节通过连杆串联组成，它的六个关节大多采用不同的设计形式，以满足各个关节之间的有效连接和动力传递，从而增加了开发者在控制机械臂的运动姿态时的开发难度。尤其当机械臂在未知环境中执行抓取动作时，机械臂的抓取姿态很难确定，限制了机械臂在日常生活中发挥作用。

实际应用中，机械臂绝大多数都是固定在基座上的，因而机械臂只能固定在某一个位置上进行操作，所以一般情况下只适用于工业生产中的重复性工作。而在实际生活中的未知环境中，机械臂要执行抓取目标物时，不仅需要目标物的坐标，还需要确定机械臂的抓取姿态。而机械臂的抓取姿态需要通过其他途径(如相机)确定，这些方式对设备和算法依赖程度高，导致机械臂姿态控制复杂，阻碍了机械臂在日常生活中的普及，所以机械臂在现实生活中的物体抓取功能尚有待完善。

视觉是人类感知世界和认识世界的最主要方式。人类视野中获取到信息往往非常丰富,这些信息给予视觉***多样化的刺激,通过神经***的支配,它们相互影响、融合,形成了人的视力。通过视觉方式来确定目标物的坐标是非常合理的一种方式。

在图像中，显著区域是图像中最能引起用户兴趣，最能表现图像内容的区域。虽然用户的感兴趣区域是非常主观的，并且由于用户任务和知识背景的不同，对于同一幅图像，不同的用户可能会选择不同的区域作为感兴趣区域。但是，由于人类视觉***和注意机制的共性，又使图像中有些区域总能显著地吸引人的注意，这些区域往往含有丰富的信息。因此可以根据人类视觉***的特点，利用人类认知过程中的一般规律，通过图像的某些底层特征近似地判断图像中的显著区域。这样提取的显著区域比较符合人的主观评价，是视觉上重要的区域。

发明内容

控制机械臂进行抓取目标物时，需要给定机械臂的位姿，即机械臂末端要到达的位置坐标和姿态坐标。其中位置坐标可以由外部相机等设备容易获得，但是姿态坐标是不唯一的，使用外部设备进行确定算法复杂度高，较难开发。

针对六自由度机械臂在抓取目标物时，其姿态计算复杂度高，所以引入一种根据待目标物的位置计算机械臂末端的姿态的低复杂度姿态角映射函数法。该方法能够降低姿态计算的复杂度，鲁棒性强，容易理解，方便开发。

本发明的采用的技术方案为一种基于智能移动平台的服务机械臂抓取姿态角计算方法，该方法的实现过程如下：

首先根据相机确定目标物在相机坐标系中的坐标位置；然后通过提前标定好的转换矩阵将目标物在相机坐标系中的坐标转换到机械臂坐标系中；通过转换后的目标物的坐标点计算目标物在机械臂坐标系中与机械坐标系中的X轴的夹角θ，然后通过该夹角θ与机械臂姿态中的三个分量的映射函数关系求得当前机械臂的姿态；然后将目标物坐标和机械臂末端姿态合成控制机械臂的目标位姿，然后发送给机械臂，即完成目标物抓取动作。通过目标物与机械臂坐标系X轴的夹角θ来确定姿态角中的各个分量，从而完成机械臂以指定的姿态去抓取目标物。

所述方法包括以下步骤：

步骤一，获取目标物的坐标

采用采用显著性提取的方法来提取目标物，然后确定目标物在相机坐标系中的坐标。

步骤二，转换目标物的坐标到机械臂坐标系下。

在控制机械臂去完成抓取目标物的动作时，最终输入到机械臂中的目标点坐标是基于机械臂的坐标系下，所以将目标物的坐标通过转换矩阵变换到机械臂坐标系下。

当机械臂和相机在移动机器人上安装完毕后，有两个坐标系：相机坐标系O_c和机械臂坐标系O_m。而且存在唯一的转换矩阵使得相机坐标系下的点转换为机械臂坐标系下的点。

步骤三，建立角度-姿态映射函数

机械臂抓取目标物时，既需要目标物在O_m坐标系中的坐标，还需要抓取目标物时的抓取姿态。目标物与O_m坐标系的X轴的夹角θ和机械臂末端姿态角R(R_x,R_y,R_z)呈函数关系。

通过相机获得目标物的坐标P_m(X_m,Y_m,Z_m)后，分为以下几步建立上述映射函数：

1)获取不同象限多组机械臂姿态坐标

在机械臂坐标系中，取Z＝a平面，a为常数；将a平面分为四个象限，在第一象限中，将机械臂移动到目标点处，然后保持机械臂腕部坐标不变，即机械臂从原点起的三个自由度保持不变，改变机械臂的姿态N(N取值大于5)次，得到N组不同的机械臂位姿坐标：姿态点R＝{R₁(R_1x,R_1y,R_1z)，R₂(R_2x,R_2y,R_2z)，……,R_N(R_Nx,R_Ny,R_Nz)}，位置点P＝{P₁(X₁,Y₁,Z₁)，P₂(X₂,Y₂,Z₂),……,P_N(X_N,Y_N,Z_N)}；同理，在第二象限，第三象限，第四象限中按照相同方式获取N组不同的机械臂位姿坐标。得到4N组机械臂位姿坐标。

2)计算目标物与X轴的夹角

根据得到的4N组机械臂姿态坐标，取机械臂姿态坐标中的位置坐标P_m(X_m,Y_m,Z_m)计算目标物与O_m坐标系的X轴的夹角θ，计算方法如下：

得到4N个夹角θ＝{θ₁，θ₂，……，θ_N}.

3)确定目标物与X轴夹角与姿态角的对应关系

姿态坐标在不同象限有对应关系，对应关系如图1，即在不同的象限中，姿态角的变化体现在不同的坐标轴中。为了简化计算，取二者的平均值来减少误差，得到在不同象限中各自对应的姿态角。从而有这样的映射关系：一个目标物与X轴的夹角θ就在四个不同象限中存在唯一与之对应的姿态。而目标物所在象限值根据目标物位置坐标获得，从而确定一个姿态坐标。映射关系分别定义为：

第三象限R_z与第四象限R_y和夹角θ的映射关系定义为h(θ)；第二象限R_z与第一象限R_y和夹角θ的映射关系定义为f(θ)；第四象限R_x与第二象限R_y与第一象限R_z以及第三象限R_x和夹角θ的映射关系定义为g(θ)；第三象限R_y与第四象限R_z和夹角θ的映射关系定义为j(θ)；第二象限R_x与第一象限R_x和夹角θ的映射关系定义为k(θ)。

4)确定各个映射函数表达式

确定以上各个映射函数。通过获取到的4N组姿态坐标和求得到的4N组目标物与X轴的夹角θ数据，使用多项式，以夹角θ为自变量，以各个姿态角为变量，进行数据拟合，得到近似表达式。使用matlab得到拟合结果的表达式。由此建立夹角θ与各个象限的姿态角的函数映射关系。

由此，实现由待抓物体的位置计算机械臂末端的姿态。

与现有技术相比较，本发明具有如下有益效果。

通过将机械臂姿态进行函数化，进行抓取目标物时，直接以目标物的坐标为输入量，计算得到机械臂末端姿态，避免通过图像识别物体姿态等复杂的姿态计算方式确定机械臂姿态。从而简化机械臂的开发使用流程，降低机械臂的使用难度。

附图说明

图1不同象限中的姿态角对应关系图

图2数据映射函数关系式，图中F代表第四象限，TH代表第三象限，T代表第二象限，O代表第一象限。小写字母x，y，z分别代表姿态角Rx，Ry，Rz。

图3本发明方法流程图

图4腕部位置固定，夹爪姿态不同的机械臂姿态图

图5使用本发明提出的计算机械臂姿态的方法的实验场景图

具体实施方式

下面结合附图和具体实施方式对本发明做进一步阐释。

本发明是以智能移动服务机器人为硬件平台，该机器人配有凌华MXC-6321D工控机(该工控机采用i7-3610QE处理器，该处理器为四核心，八线程处理器，主频2.3GHz，安装Win8专业版操作***)，Universal Robots公司的UR5机械臂(该机械臂有六自由度，工作范围为850mm,可重复误差为正负0.1mm)，微软XBOX ONE Kinect2.0三维立体摄像机以及RGKREH-64夹爪(质量1.9kg，最大夹持力45N)。

本实施方式的具体过程为，首先通过Kinect三维立体相机获取目标物在相机坐标系下的坐标，然后通过控制UR5机械臂到达目标点，得到目标物在机械臂坐标系下的坐标。改变目标物的位置，重复上述过程。通过得到的这两组坐标，进行计算相机坐标系和机械臂坐标系的转换矩阵。其次，将机械臂末端的Z坐标取固定值，然后在第一象限中，取机械臂腕部位置固定，改变夹爪的姿态N次，这里取N＝10，记录机械臂的位姿坐标。同理在其他三个象限中，重复上述过程，可以得到4N组位姿坐标点。通过matlab等数学工具对这4N组数据进行处理，首先通过位置坐标求取目标物与X轴的夹角，然后拟合该夹角与各个象限中的姿态角的映射函数关系，建立相应的映射函数。如图2，经过数据拟合的映射函数如下：

第三象限R_z与第四象限R_y和夹角θ的映射关系为：

h(θ)＝0.0449θ³-1.0831θ2+0.2362θ+4.6996

第二象限R_z与第一象限R_y和夹角θ的映射关系为：

f(θ)＝-0.1949θ³+1.1312θ²-3.1801θ+0.6266

第四象限R_x与第二象限R_y与第一象限R_z以及第三象限R_x和夹角θ的映射关系为：

g(θ)＝-0.2323θ³+0.3589θ²+1.6923θ-0.214

第三象限R_y与第四象限R_z和夹角θ的映射关系为：

j(θ)＝0.212θ³-0.8481θ²+1.0164θ+2.0847

第二象限R_x与第一象限R_x和夹角θ的映射关系为：

k(θ)＝0.0449θ³-1.0831θ²+0.2362θ+4.6996

根据以上映射函数可以计算出机械臂抓取目标物的姿态角，然后就可以控制机械臂进行抓取目标物动作。图3是本发明的算法流程图，具体包括以下几个步骤：

步骤一，相机和机械臂分别获取目标物的坐标，利用得到的对应的相机坐标系和机械臂坐标系下的坐标点，求取两个坐标系的转换矩阵。

步骤二，在四个象限中，分别获取机械臂腕部坐标相同，夹爪姿态不同的N组机械臂姿态坐标，如图4所示。

步骤三，对获得的数据进行求取目标物与X轴的夹角。

步骤四，拟合夹角与姿态角的函数映射关系。

步骤五，通过相机获取目标物的坐标，通过变换矩阵，得到机械臂坐标系下的坐标。

步骤六，通过得到的坐标，计算目标物与X轴的夹角。

步骤七，将上述计算得到的夹角带入到相应的映射函数中，求取夹爪的姿态角。

步骤八，将最终的机械臂的位姿坐标发送到UR5机械臂中，完成目标物夹取动作。

下面给出本发明的一个应用实例。

选择的实验环境为实验室，在实验室内的一张桌子上放置一个水杯，让UR5机械臂进行抓取动作，通过评估机械臂抓取水杯的准确度来验证本发明提出的机械臂姿态计算方法的有效性。如图4所示，在伴随机器人前方的桌子上有一个水杯，需要UR5机械臂将其抓取，放到旁边的桌子上。图5(a)为实验任务场景图，图5(b)为使用本发明提出的方法根据目标物计算得到的夹爪姿态,可以看到夹爪姿态相对于水杯是非常合理的一个姿态，图5中的(c),(d),(e),(f)为UR5机械臂抓取到目标物的动作。

实验过程中，通过抓取水杯姿态的合理性和准确性验证了本发明提出的计算机械臂姿态角方法的有效性，同时也表明了使用本发明提出的计算机械臂姿态的方法，抓取目标物的姿态合理且准确，而且在程序上，程序的复杂度降低，提高了算法的实时性以及计算机械臂位姿计算的速度，降低了机械臂控制的难度，方便机械臂的姿态控制。

Claims (2)

1.一种基于智能移动平台的服务机械臂抓取姿态角计算方法，其特征在于：该方法的实现过程如下，

首先根据相机确定目标物在相机坐标系中的坐标位置；然后通过提前标定好的转换矩阵将目标物在相机坐标系中的坐标转换到机械臂坐标系中；通过转换后的目标物的坐标点计算目标物在机械臂坐标系中与机械坐标系中的X轴的夹角θ，然后通过该夹角θ与机械臂姿态中的三个分量的映射函数关系求得当前机械臂的姿态；然后将目标物坐标和机械臂末端姿态合成控制机械臂的目标位姿，然后发送给机械臂，即完成目标物抓取动作；通过目标物与机械臂坐标系X轴的夹角θ来确定姿态角中的各个分量，从而完成机械臂以指定的姿态去抓取目标物；

所述方法包括以下步骤：

步骤一，获取目标物的坐标

采用采用显著性提取的方法来提取目标物，然后确定目标物在相机坐标系中的坐标；

步骤二，转换目标物的坐标到机械臂坐标系下；

在控制机械臂去完成抓取目标物的动作时，最终输入到机械臂中的目标点坐标是基于机械臂的坐标系下，所以将目标物的坐标通过转换矩阵变换到机械臂坐标系下；

当机械臂和相机在移动机器人上安装完毕后，有两个坐标系：相机坐标系O_c和机械臂坐标系O_m；而且存在唯一的转换矩阵使得相机坐标系下的点转换为机械臂坐标系下的点；

步骤三，建立角度-姿态映射函数

机械臂抓取目标物时，既需要目标物在O_m坐标系中的坐标，还需要抓取目标物时的抓取姿态；目标物与O_m坐标系的X轴的夹角θ和机械臂末端姿态角R(R_x,R_y,R_z)呈函数关系；

通过相机获得目标物的坐标P_m(X_m,Y_m,Z_m)后，分为以下几步建立上述映射函数：

1)获取不同象限多组机械臂姿态坐标

在机械臂坐标系中，取Z＝a平面，a为常数；将a平面分为四个象限，在第一象限中，将机械臂移动到目标点处，然后保持机械臂腕部坐标不变，即机械臂从原点起的三个自由度保持不变，改变机械臂的姿态N次，N取值大于5，得到N组不同的机械臂位姿坐标：姿态点R＝{R₁(R_1x,R_1y,R_1z)，R₂(R_2x,R_2y,R_2z)，……,R_N(R_Nx,R_Ny,R_Nz)}，位置点P＝{P₁(X₁,Y₁,Z₁)，P₂(X₂,Y₂,Z₂),……,P_N(X_N,Y_N,Z_N)}；同理，在第二象限，第三象限，第四象限中按照相同方式获取N组不同的机械臂位姿坐标；得到4N组机械臂位姿坐标；

2)计算目标物与X轴的夹角

根据得到的4N组机械臂姿态坐标，取机械臂姿态坐标中的位置坐标P_m(X_m,Y_m,Z_m)计算目标物与O_m坐标系的X轴的夹角θ，计算方法如下：

<mrow> <mi>&amp;theta;</mi> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mfrac> <msub> <mi>Y</mi> <mi>m</mi> </msub> <msub> <mi>X</mi> <mi>m</mi> </msub> </mfrac> <mo>)</mo> </mrow> </mrow>

得到4N个夹角θ＝{θ₁，θ₂，……，θ_N}.

3)确定目标物与X轴夹角与姿态角的对应关系

姿态坐标在不同象限有对应关系，即在不同的象限中，姿态角的变化体现在不同的坐标轴中；为了简化计算，取二者的平均值来减少误差，得到在不同象限中各自对应的姿态角；从而有这样的映射关系：一个目标物与X轴的夹角θ就在四个不同象限中存在唯一与之对应的姿态；而目标物所在象限值根据目标物位置坐标获得，从而确定一个姿态坐标；映射关系分别定义为：

第三象限R_z与第四象限R_y和夹角θ的映射关系定义为h(θ)；第二象限R_z与第一象限R_y和夹角θ的映射关系定义为f(θ)；第四象限R_x与第二象限R_y与第一象限R_z以及第三象限R_x和夹角θ的映射关系定义为g(θ)；第三象限R_y与第四象限R_z和夹角θ的映射关系定义为j(θ)；第二象限R_x与第一象限R_x和夹角θ的映射关系定义为k(θ)；

4)确定各个映射函数表达式

确定以上各个映射函数；通过获取到的4N组姿态坐标和求得到的4N组目标物与X轴的夹角θ数据，使用多项式，以夹角θ为自变量，以各个姿态角为变量，进行数据拟合，得到近似表达式；使用matlab得到拟合结果的表达式；由此建立夹角θ与各个象限的姿态角的函数映射关系；

由此，实现由待抓物体的位置计算机械臂末端的姿态。

2.根据权利要求1所述的一种基于智能移动平台的服务机械臂抓取姿态角计算方法，其特征在于：

具体包括以下几个步骤：

步骤一，相机和机械臂分别获取目标物的坐标，利用得到的对应的相机坐标系和机械臂坐标系下的坐标点，求取两个坐标系的转换矩阵；

步骤二，在四个象限中，分别获取机械臂腕部坐标相同，夹爪姿态不同的N组机械臂姿态坐标；

步骤三，对获得的数据进行求取目标物与X轴的夹角；

步骤四，拟合夹角与姿态角的函数映射关系；

步骤五，通过相机获取目标物的坐标，通过变换矩阵，得到机械臂坐标系下的坐标；

步骤六，通过得到的坐标，计算目标物与X轴的夹角；

步骤七，将上述计算得到的夹角带入到相应的映射函数中，求取夹爪的姿态角；

步骤八，将最终的机械臂的位姿坐标发送到UR5机械臂中，完成目标物夹取动作。