CN103862476B - 一种六自由度机械手的位置逆解方法 - Google Patents

一种六自由度机械手的位置逆解方法 Download PDF

Info

Publication number
CN103862476B
CN103862476B CN201410125310.6A CN201410125310A CN103862476B CN 103862476 B CN103862476 B CN 103862476B CN 201410125310 A CN201410125310 A CN 201410125310A CN 103862476 B CN103862476 B CN 103862476B
Authority
CN
China
Prior art keywords
angle
steering wheels
steering wheel
mechanical hand
steering
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
CN201410125310.6A
Other languages
English (en)
Other versions
CN103862476A (zh
Inventor
孙采鹰
兰孝文
颉新春
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Inner Mongolia University of Science and Technology
Original Assignee
Inner Mongolia University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Inner Mongolia University of Science and Technology filed Critical Inner Mongolia University of Science and Technology
Priority to CN201410125310.6A priority Critical patent/CN103862476B/zh
Publication of CN103862476A publication Critical patent/CN103862476A/zh
Application granted granted Critical
Publication of CN103862476B publication Critical patent/CN103862476B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Abstract

本发明专利公开了一种六自由度机械手的位置逆解方法。本发明包括：通过摄像头获取圆形目标物体的图像位置，用OPENCV对图像进行处理得到中心坐标和半径信息，通过比较直接的数值方法，对机械手需要到达的目标位置进行逆解，建立数学模型求出机械手各个舵机需要转过的角度，乘以各个舵机每转过一度所对应的脉宽，得到各个舵机将目标物品送达目的地需要的脉宽值，根据脉宽值控制机械手各个关节动作，实现机械手臂的按颜色自主物品抓取。本发明计算过程简便，在本方法下实现了对机械手精确自主抓物的控制，很好地避免了传统位置逆解算法中的复杂运算和多解问题，有很好的市场应用前景。

Description

一种六自由度机械手的位置逆解方法
技术领域
本发明涉及一种六自由度机械手的位置逆解方法，属于机械手自动控制领域。
背景技术
位置逆解问题是机械手机构学乃至机械手学中的最基础也是最重要的研究问题之一，它直接关系到机械手运动分析、离线编程、轨迹规划和实时控制等工作。因为速度和加速度分析都要在进行位置分析的基础之上才能进行，所以位置逆解问题是机械手运动规划和轨迹规划的基础，只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机械手末端执行器的控制。而从工程应用的角度出发，位置逆解问题的研究成果可以很容易地应用到机械手上面，因此更加促进了对位置逆解问题的研究。
对于运动学正解来说，它的解是唯一确定的，即各个关节变量给定之后，机械手的末端抓手和工具的位姿是唯一确定的;而运动学反解往往具有多重解，也可能不存在解。位置逆解的复杂程度往往与机械手的结构有很大关系。由于一般情况下，六个自由度便可满足机械手在工作空间内可达任一位姿，因此六自由度机械手的位置逆解最具有研究价值和实用价值。
对六自由度机械手运动学的位置逆解问题，已经有很多学者作了大量的分析和研究。常用的数值方法主要包括牛顿拉夫森法、优化算法，区间算法，遗传算法和同伦算法等方法。数值方法求解一般是先建立包括若干个未知量的一个方程组，然后提供一组初始值，再利用各种优化法进行迭代，使之逐步收敛于机构的一组解。这一类数值方法的优点是求解过程比较简单，但是在计算中需要提供适当的初始值，因此涉及到初始值的选取问题；另外，这些方法不能根据方程组的情况来确定机械手机构有多少组解，也很难得到全部解。
发明内容
本发明的目的在于通过提供一种直接的数值方法进行机械手臂的位置逆解，建立一个角度与长度对应关系的数学模型，采用迭代法和反解法算出各个舵机需要转过的角度，再转换成对应的脉宽，控制机械手进行自主抓物。
为了解决上述技术特征，现提出一种六自由度机械手的位置逆解方法，包括以下步骤：
A.初始化步骤；
首先让舵机处于竖直状态，调试出各个舵机对应的脉宽；
B.逆解步骤；
然后根据角度与机械手臂长度对应的关系建立数学模型，再反解出各个舵机到达目标物需要转过的角度，具体步骤为：
首先通过测量知道各个舵机间的舵机臂长度；
所述机械手具有零号舵机、一号舵机、二号舵机、三号舵机、四号舵机、五号舵机；通过测量获得三号舵机与四号舵机之间的距离la，以及二号舵机与三号舵机之间的距离lc；采用摄像头采集目标物品的x、y坐标，采集回来的坐标经过上位机OPENCV处理将其转化成坐标参考物从左到右的坐标，坐标从零开始算起，中间位置坐标为x0；求出各个舵机需要转过的角度cr2、cr3、cr4、cr5，根据三角形的正余弦和正余切定理，先算出角crD的大小和边lb的长度；
根据三角形正切定理：
tan(crD)= (3.3+z)/(rad-4.85)； (1）
由于rad为机械手从中心坐标（x0，0）到目标物品的距离，算出其大小为：
rad= sqrt((x-x0)*(x-x0)+y*y)； (2）
得到知角crD的大小为：
crD= atan((3.3+z)/(rad-4.85))； (3）
求出了角crD的大小，根据勾股定理算出未知边lb的大小为：
lb=sqrt((rad-4.85)*(rad-4.85)+(3.3+z)*(3.3+z))； (4）
由此得到la、lb和lc的大小，跟据三角形余弦和反余弦定理得到三角形任意一个角的大小，把三角形的三个内角设为crA、crB、 crC，由余弦定理得：
cos(crA)= (lb*lb+lc*lc-la*la)/(2*lb*lc); (5）
cos(crB)= (la*la+lc*lc-lb*lb)/(2*la*lc); (6）
cos(crC)= (la*la+lb*lb-lc*lc)/(2*la*lb); (7）
再根据反余弦定理，得出三角形各个角的大小为：
crA=acos((lb*lb+lc*lc-la*la)/(2*lb*lc))； (8）
crB=acos((la*la+lc*lc-lb*lb)/(2*la*lc))； (9）
crC=acos((la*la+lb*lb-lc*lc)/(2*la*lb))； (10）
公式（8）（9）（10）算出了三角形各个内角的大小，再由舵机转过角度与各个角的关系算出舵机转过角度的大小；设定二号舵机转过的角度为cr2，三号舵机转过的角度为cr3，四号舵机转过的角度为cr4，五号舵机转过的角度为cr5，其计算公式如下所示：
cr2=90+crD-crA； （11）
cr3=180-crB； （12）
cr4=90-crC-crD； （13）
通过算式（11）（12）（13）得到各个舵机到达目标物需要转过的角度；
C. 转换步骤；
将各个舵机到达目标物需要转过的角度转换成相应的脉宽传给舵机控制板，控制机械手进行自主抓物。
作为进一步优选的技术方案，舵机控制板由舵机信号输出电路和舵机脉冲发生电路组成。
作为进一步优选的技术方案，所述舵机信号输出电路对舵机的角度进行控制，通过给舵机提供相应的脉冲值，完成对舵机的角度控制。
作为进一步优选的技术方案，所述舵机脉冲信号发生电路主要是通过上位机给舵机控制板发送需要产生的脉宽值，来决定产生多少脉冲，然后经过处理输送给舵机输出电路。
作为进一步优选的技术方案，所述坐标参考物为A4纸。
本发明的有益效果是，通过摄像头获取圆形目标物体的图像位置，用OPENCV对图像进行处理得到中心坐标和半径信息，根据中心坐标和半径对机械手臂的运动学进行分析，通过比较直接的数值方法，对机械手需要到达的目标位置进行逆解，建立数学模型求出机械手各个舵机需要转过的角度，并根据求出的解来控制机械手各个关节动作，实现机械手臂的按颜色自主物品抓取和分类。
附图说明
图1是本发明的机械手数学模型；
图2是本发明的舵机信号输出电路；
图3是本发明的舵机脉冲信号发生电路；
图4是本发明机械手位置逆解算法程序流程；
图5是基于本发明所实现的机械手按颜色自主抓物实物图。
具体实施方式
本发明将通过下面的实例进行更详细的描述，但是本发明不受限于以下实施例，坐标参考物也不局限于A4纸，形状规则、边缘容易区分的物品都可以用来作为坐标参考物品。
本发明一种六自由度机械手的位置逆解方法，提出新的控制机械手到达目标物的位置逆解方法。
一种六自由度机械手的位置逆解方法：从本发明建立的机械手臂数学模型中，可以知道每个舵机臂的长度，z为待给定值即机械手臂的抬升高度，rad为机械手从中心坐标（x0，0）到目标物品的距离。首先通过测量知道各个舵机间的舵机臂长度，舵机号与舵机臂对应长度关系见下表。
舵机对应的序号 舵机臂长度（cm）
0～2 14.7
2～3（lc） 9.8
3～4（la） 10.45
4～5 11.4
根据舵机臂长度表可以知道la=10.45，lc=9.8。本发明采用摄像头采集目标物品的x、y坐标，采集回来的坐标经过上位机OPENCV处理将其转化成A4纸从左到右的坐标，坐标从零开始算起，中间位置坐标为x0。要求出各个舵机需要转过的角度cr2、cr3、cr4、cr5，根据三角形的正余弦和正余切定理，先算出角crD的大小和边lb的长度。首先三角形两边的长度分别为：rad-2.15-2.7、14.7-11.4+z，即rad-4.85、14.7-11.4+z，根据三角形正切定理可知：
tan(crD)= (3.3+z)/(rad-4.85)； (1）
由于rad为机械手从中心坐标（x0，0）到目标物品的距离，可以算出其大小为：
rad= sqrt((x-x0)*(x-x0)+y*y)； (2）
这样就可以得知角crD的大小为：
crD= atan((3.3+z)/(rad-4.85))； (3）
求出了角crD的大小，已知本发明所构建的模型三角形是直角三角形，根据勾股定理算出未知边lb的大小为：
lb=sqrt((rad-4.85)*(rad-4.85)+(3.3+z)*(3.3+z))； (4）
通过上述过程知道了la、lb和lc的大小，跟据三角形余弦和反余弦定理可以得到三角形任意一个角的大小，把三角形的三个内角设为crA、crB、 crC，由余弦定理得：
cos(crA)= (lb*lb+lc*lc-la*la)/(2*lb*lc); (5）
cos(crB)= (la*la+lc*lc-lb*lb)/(2*la*lc); (6）
cos(crC)= (la*la+lb*lb-lc*lc)/(2*la*lb); (7）
再根据反余弦定理，可以得出三角形各个角的大小为：
crA=acos((lb*lb+lc*lc-la*la)/(2*lb*lc))； (8）
crB=acos((la*la+lc*lc-lb*lb)/(2*la*lc))； (9）
crC=acos((la*la+lb*lb-lc*lc)/(2*la*lb))； (10）
从以上的公式算出了三角形各个内角的大小，再由舵机转过角度与各个角的关系算出舵机转过角度的大小。设定二号舵机转过的角度为cr2，三号舵机转过的角度为cr3，四号舵机转过的角度为cr4，五号舵机转过的角度为cr5，其计算公式如下所示：
cr2=90+crD-crA； （11）
cr3=180-crB； （12）
cr4=90-crC-crD； （13）
五号舵机转过的角度cr5是最底下舵机的角度，其转过后角度对应的机械手的方向应指向物体。如果x-x0的误差小于万分之一时，五号舵机对应的角度为cr5=90；当x-x0大于零时，cr5=atan(y/(x-x0))；当x-x0小于零时，cr5=cr5+180。一号舵机为控制机械爪转成什么方向的舵机，只需在使用范围内转动即可，不用算出其具体需要转过的角度。零号舵机为机械爪夹东西时的张度，只要测试好张开和抓紧物品的脉宽就可以算出需要转动的角度。
本发明通过以上计算，已经算出了各个舵机对应的角度，下一步需要确定舵机转过的角度与舵机控制器对应脉宽的大小。首先要知道每个舵机的脉宽范围，由于每个舵机制作工艺的误差，造成了舵机的使用范围也不同，因此应该算出舵机转过每一度时对应的脉宽。由于舵机的角度范围为0～185度，其每一角度对应的脉宽y计算方法为：有效脉宽范围的量程除以角度范围的量程，各个舵机每一度对应的脉宽见下表。
舵机序号 有效脉宽范围 每一度对应的脉宽
0 500～2240 9.4054
1 500～2320 9.8378
2 500～2320 9.8378
3 500～2290 9.6757
4 500～2290 9.6757
5 500～2230 9.3514
将目标物品在A4纸上的相对位置坐标代入前述公式，计算出每个舵机需要转过的具体角度值后，根据上表中各个舵机每转过一度所对应的脉宽，两者相乘，就可以算出舵机控制器需要给每个舵发送的脉宽值。上述计算过程均由下位机ARM单片机编程实现，从而达到分类抓取物品的要求。

Claims (5)

1.一种六自由度机械手的位置逆解方法，包括以下步骤：
初始化步骤；
首先让舵机处于竖直状态，调试出各个舵机对应的脉宽；
逆解步骤；
然后根据角度与机械手臂长度对应的关系建立数学模型，再反解出二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度，具体步骤为：
首先通过测量知道各个舵机间的舵机臂长度；
所述机械手具有零号舵机、一号舵机、二号舵机、三号舵机、四号舵机以及五号舵机；通过测量获得三号舵机与四号舵机之间的距离la，以及二号舵机与三号舵机之间的距离lc；采用摄像头采集目标物品的x、y坐标，采集回来的坐标经过上位机OPENCV处理将其转化成坐标参考物从左到右的坐标，坐标从零开始算起，中间位置坐标为x0；求出所述二号舵机、三号舵机、四号舵机、五号舵机需要转过的角度cr2、cr3、cr4、cr5，根据三角形的正余弦和正余切定理，先算出角crD的大小和边lb的长度；z为机械手臂的抬升高度；rad为机械手从中心坐标（x0，0）到目标物品的距离；
根据三角形正切定理：
tan(crD)= (3.3+z)/(rad-4.85)； (1）
由于rad为机械手从中心坐标（x0，0）到目标物品的距离，算出其大小为：
rad= sqrt((x-x0)*(x-x0)+y*y)； (2）
得到角crD的大小为：
crD= atan((3.3+z)/(rad-4.85))； (3）
求出了角crD的大小，根据勾股定理算出未知边lb的大小为：
lb=sqrt((rad-4.85)*(rad-4.85)+(3.3+z)*(3.3+z))； (4）
由此得到la、lb和lc的大小，跟据三角形余弦和反余弦定理得到三角形任意一个角的大小，把三角形的三个内角设为crA、crB、 crC，由余弦定理得：
cos(crA)= (lb*lb+lc*lc-la*la)/(2*lb*lc); (5）
cos(crB)= (la*la+lc*lc-lb*lb)/(2*la*lc); (6）
cos(crC)= (la*la+lb*lb-lc*lc)/(2*la*lb); (7）
再根据反余弦定理，得出三角形各个角的大小为：
crA=acos((lb*lb+lc*lc-la*la)/(2*lb*lc))； (8）
crB=acos((la*la+lc*lc-lb*lb)/(2*la*lc))； (9）
crC=acos((la*la+lb*lb-lc*lc)/(2*la*lb))； (10）
公式（8）（9）（10）算出了三角形各个内角的大小，再由舵机转过角度与各个角的关系算出舵机转过角度的大小；设定二号舵机转过的角度为cr2，三号舵机转过的角度为cr3，四号舵机转过的角度为cr4，五号舵机转过的角度为cr5，其计算公式如下所示：
cr2=90+crD-crA； （11）
cr3=180-crB； （12）
cr4=90-crC-crD； （13）
所述crD指的是二号舵机与四号舵机之间的连线与水平方向的夹角，所述lb指的是二号舵机与四号舵机之间的距离；
通过算式（11）（12）（13）得到所述二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度；
转换步骤；
将所述二号舵机、三号舵机、四号舵机以及五号舵机到达目标物需要转过的角度转换成相应的脉宽传给舵机控制板，控制机械手进行自主抓物。
2.根据权利要求1所述的一种六自由度机械手的位置逆解方法，其特征在于：舵机控制板由舵机信号输出电路和舵机脉冲发生电路组成。
3.根据权利要求2所述的一种六自由度机械手的位置逆解方法，其特征在于：所述舵机信号输出电路对舵机的角度进行控制，通过给舵机提供相应的脉冲值，完成对舵机的角度控制。
4.根据权利要求2所述的一种六自由度机械手的位置逆解方法，其特征在于：所述舵机脉冲信号发生电路主要是通过上位机给舵机控制板发送需要产生的脉宽值，来决定产生多少脉冲，然后经过处理输送给舵机输出电路。
5.根据权利要求1至4中任一项所述的一种六自由度机械手的位置逆解方法，其特征在于：所述坐标参考物为A4纸。
CN201410125310.6A 2014-03-31 2014-03-31 一种六自由度机械手的位置逆解方法 Expired - Fee Related CN103862476B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410125310.6A CN103862476B (zh) 2014-03-31 2014-03-31 一种六自由度机械手的位置逆解方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410125310.6A CN103862476B (zh) 2014-03-31 2014-03-31 一种六自由度机械手的位置逆解方法

Publications (2)

Publication Number Publication Date
CN103862476A CN103862476A (zh) 2014-06-18
CN103862476B true CN103862476B (zh) 2016-10-26

Family

ID=50901796

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410125310.6A Expired - Fee Related CN103862476B (zh) 2014-03-31 2014-03-31 一种六自由度机械手的位置逆解方法

Country Status (1)

Country Link
CN (1) CN103862476B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105005656B (zh) * 2015-07-13 2018-02-23 广州霞光技研有限公司 一种6自由度机械手自主抓取逆解工程算法
CN107953340B (zh) * 2018-01-16 2021-09-24 广东凯宝机器人科技有限公司 一种通用型六自由度机械手逆解工程算法
CN111113431A (zh) * 2020-01-03 2020-05-08 武汉科技大学 一种六自由度串联机器人逆解优化方法
CN114770461B (zh) * 2022-04-14 2023-12-01 深圳技术大学 一种基于单目视觉的移动机器人及其自动抓取方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5293461A (en) * 1991-11-20 1994-03-08 The University Of British Columbia System for determining manipulator coordinates
JP4894657B2 (ja) * 2007-07-17 2012-03-14 トヨタ自動車株式会社 逆運動学計算方法及び逆運動学計算装置
DE102008057142B4 (de) * 2008-04-29 2016-01-28 Siemens Aktiengesellschaft Verfahren zur rechnergestützten Bewegungsplanung eines Roboters
KR101234519B1 (ko) * 2011-03-11 2013-02-19 전자부품연구원 역키네마틱스를 이용하여 위치를 제어하는 곤돌라 로봇 및 그의 위치 제어 방법
CN102509025A (zh) * 2011-11-25 2012-06-20 苏州大学 一种六自由度仿人灵巧臂逆运动学的快速求解方法
CN102609002B (zh) * 2012-03-05 2015-06-03 浙江工业大学 一种六自由度串联机械臂的位置逆解控制方法
CN102637158B (zh) * 2012-04-28 2015-05-06 谷菲 一种六自由度串联机器人运动学逆解的求解方法

Also Published As

Publication number Publication date
CN103862476A (zh) 2014-06-18

Similar Documents

Publication Publication Date Title
Zhao et al. Dual-arm robot design and testing for harvesting tomato in greenhouse
CN102219051B (zh) 基于人机交互技术的四旋翼飞行器系统控制方法
CN103862476B (zh) 一种六自由度机械手的位置逆解方法
US9990685B2 (en) Automated guidance system and method for a coordinated movement machine
CN106041927A (zh) 结合eye‑to‑hand和eye‑in‑hand结构的混合视觉伺服系统及方法
CN106003034B (zh) 一种主从式机器人控制系统及控制方法
CN110216674B (zh) 一种冗余自由度机械臂视觉伺服避障系统
CN113814986B (zh) 一种基于机器视觉控制scara机器人的方法及系统
JP2013186088A5 (zh)
JP6826069B2 (ja) ロボットの動作教示装置、ロボットシステムおよびロボット制御装置
CN104325268A (zh) 一种基于智能学习的工业机器人三维空间自主装配方法
CN103970071B (zh) 铸件表面清理过程中加工路径的检测及纠正方法
CN105436252A (zh) 一种基于视觉测量的折弯机加工控制方法及装置
Li et al. Neural learning and kalman filtering enhanced teaching by demonstration for a baxter robot
CN114347008A (zh) 基于工业机器人的无序抓取工件方法、装置及智能终端
Han et al. Grasping control method of manipulator based on binocular vision combining target detection and trajectory planning
CN105773661A (zh) 水平机器人固定相机下工件平移旋转标定方法
Ge Programming by demonstration by optical tracking system for dual arm robot
CN103737603B (zh) 一种流水线上机械臂精确控制系统及控制方法
Matsuda et al. Control system for object transportation by a mobile robot with manipulator combined with manual operation and autonomous control
Wang et al. Dynamic obstacle avoidance for application of human-robot cooperative dispensing medicines
CN110421565A (zh) 一种用于实训的机器人全局定位与测量系统和方法
Abut et al. Sliding Mode Control of the Haptic-Teleoperation System Based on a Real and Virtual Robot
CN202607670U (zh) 带有机器视觉目标精准检测的物流转载机器人控制系统
Zhou et al. Visual servo control system of 2-DOF parallel robot

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20161026

Termination date: 20170331