CN105988420B - 一种机械手双轴轨迹插补算法 - Google Patents
一种机械手双轴轨迹插补算法 Download PDFInfo
- Publication number
- CN105988420B CN105988420B CN201510060767.8A CN201510060767A CN105988420B CN 105988420 B CN105988420 B CN 105988420B CN 201510060767 A CN201510060767 A CN 201510060767A CN 105988420 B CN105988420 B CN 105988420B
- Authority
- CN
- China
- Prior art keywords
- equation
- cylinder
- arm
- oil cylinder
- big arm
- 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.)
- Active
Links
Landscapes
- Manipulator (AREA)
Abstract
本发明公开了一种机械手双轴轨迹插补算法，用于控制机械手按照预定轨迹进行运动。其特征是：通过双轴插补算法能够得到机械手大臂油缸和小臂油缸的流量方程，通过将流量方程嵌入到计算机控制程序中，调节电液比例流量方向阀即可实现机械手的运动。这种算法具有控制灵活、适应性强、又有控制精度高、稳定性强等特点，能够显著提高作业效率，降低人身伤亡事故发生的概率。
Description
技术领域
本发明涉及机械手技术领域，特别是涉及一种机械手双轴轨迹插补算法。
背景技术
机械手作为钻井作业专门配备的机械，可以减轻繁重的体力劳动，改善劳动条件和安全生产，能够通过自动控制按照预定轨迹移动，在提高钻井作业工作效率，增强企业竞争力等方面起到极其重要的作用。现有机械手在控制方面存在的缺陷是：机械手一般是用单片机作为主要控制单元，由于单片机的编程工作比较复杂，其内部程序固化简单，一旦出现故障或者机械手工作任务改变需要修改程序的情况，则需要由专业人员对单片机的内部程序进行重新编写，利用单片机对机械手进行控制的操作灵活性和通用性差、成本高、开发难度大，且控制精度和效率不高。因此，研究出操作方便且实用、生产效率高且成本低的机械手控制算法成为一种需求。
发明内容
为了克服上述现有技术的不足，本发明提供了一种高效准确、生产成本低的机械手双轴轨迹插补算法。
本发明所采用的技术方案是：推导出机械手比较理想的轨迹运动，按照预先设计的轨迹运动求得了其数学方程，根据机械手的轨迹运动确定两个驱动液压缸的位置和速度方程(确定了液压系统中液压缸的工作流量方程)，根据液压缸的工作流量方程嵌入到计算机控制程序中，调节电液比例流量方向阀从而实现机械手的运动。
与现有技术相比，本发明的有益效果是能够简单有效地控制机械手按照预定轨迹运动，稳定可靠，控制反应灵敏，程序维护成本低，生产效率高。
附图说明
图1为机械手坐标定位图
图2为机械手坐标定位系统计算软件界面图
图3为抓取提升算法界面图
图4为水平回收算法界面图
图5为置最远端算法界面图
图6为置最近端算法界面图
图7为抓取提升油缸速度曲线图
图8为水平回收油缸速度曲线图
图9为最大位置放杆水平运动油缸速度曲线
图10为最大位置放杆垂直运动油缸速度曲线
图11为最小位置放杆水平运动油缸速度曲线
图12为最小位置放杆垂直运动油缸速度曲线
具体实施方式
图1机械手坐标定位图中，图(1)为空间总坐标系，坐标系原点建立在平台导轨左端极限位置的中心，其中h为导轨中间平面到机械手大臂L1的竖直距离(由于h未知，公式中h暂取为0)。由图中可知，C点坐标为(x1+lx，ly，lz)。
图(2)中将坐标系原点平移到O’，可知O、A、B、C在同一平面内，则可在x’oz平面坐标系内求解lx’，如图(3)所示，可得lx’
lx′＝a+AB·cosθ1-BC·cos(θ1-θ2)
同时可得lz
l2′＝BC·sin(θ1+θ2)-AB·sinθ1
同样，由图(4)可解得lx、ly
lx＝lx′·cosθ3＝(a+AB·cosθ1-BC·cos(θ1+θ2))·cosθ1
ly＝lx′·sinθ3＝(a+AB·cosθ1-BC·cos(θ1+θ2))·sinθ1
将上面求得的结果代入C点坐标(x1+lx，ly，lz)中，即得到C点坐标公式：
x＝x1+(a+L1·cosθ1-L2·cos(θ1+θ2))·cosθ1
y＝-(a+L1·cosθ1-L2·cos(θ1+θ2))·sinθ1
z＝L2·cos(θ1+θ2)-L1sinθ1
式中L1＝AB，L2＝BC
根据对以上机械手坐标定位图的分析，我们设计了机械手坐标定位系统计算软件(如图2至图6所示)，更加方便、准确地计算机械手运动的位置方程，再将机械手运动的位置方程分别转化成大臂油缸和小臂油缸的位置方程，对各个运动阶段的位置方程求一阶导数即获得大臂液压缸和小臂液压缸的速度方程，这个方程中含有较复杂的三角关系，为了便于后期计算机运算求解，对大臂液压缸和小臂液压缸的速度方程进行采用了多项式拟合，得到了多项式方程，由于大臂和小臂液压缸的尺寸是确定的，即可以得到大臂液压缸和小臂液压缸的流 量方程，具体分析如下：
1、垂直抓取提升：提升范围0～500mm，最大速度100mm/s，运行时间≤5s；图7抓取提升油缸速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油缸速度曲线。其中大臂油缸速度方程：
V1＝1.0e+005*(3*0.00000000001482*(-1700+100*t)^2+2*0.00000018273633*(-1700+100*t)+0.00064385076391) (1-1)
(1-1)可以求得：
V1max＝26.93mm/s
此时大臂油缸的无杆腔进油，
Q1max＝A11*V1max＝19.8L/min
小臂油缸速度方程：
V2＝1.0e+004*(3*0.00000000005127*(-1700+100*t)^2+2*0.00000032211200*(-1700+100*t)-0.00083823451613) (1-2)
由(1-2)可以求得：
V2max＝14.9mm/s
此时小臂油缸的有杆腔进油，
Q2max＝A22*V2max＝5.27L/min
注：A11-大臂油缸的无杆腔面积
A21-大臂油缸的有杆腔面积
A12-小臂油缸的无杆腔面积
A22-小臂油缸的有杆腔面积
2、水平回收：回收范围1450～390mm，最大速度106mm/s，运行时间≤10s；图8水平回收油缸速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油缸速度曲线。其中大臂油缸速度方程：
V1＝1.06e+004*(3*0.00000000058491*(1450-106*t)^2-2*0.00000204110142*(1450-106*t)+0.00345935632275) (1-3)
由(1-3)可以求得：
V1max＝22.62mm/s
此时大臂油缸的有杆腔进油，
Q1max＝A21*V1max＝8.0L/min
小臂油缸速度方程：
V2＝1.06e+004*(-3*0.00000000021918*(1450-106*t)^2+2*0.00000092982377*(1450-106*t)+0.00002793357367) (1-4)
由(1-4)可以求得：
V2max＝14.22mm/s
此时小臂油缸的有杆腔进油，
Q2max＝A22*V2max＝5.03L/min
3、最大位置放杆水平运动：水平放杆范围2500mm，最大速度214mm/s，运行时间≤10；图9最大位置放杆水平运动速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油缸速度曲线。其中大臂油缸速度方程：
V1＝2.14e+004*(3*0.00000000013803*(360+214*t)^2-2*0.00000073219526*(360+214*t)+0.00228666149523) (1-5)
由(1-5)可以求得：
V1max＝38.8mm/s
此时大臂油缸的无杆腔进油，
Q1max＝A11*V1max＝28.55L/min
小臂油缸速度方程：
V2＝2.14e+004*(-3*0.00000000008640*(360+214*t)^2+2*0.00000053946243*(360+214*t)+0.00037900721752) (1-6)
由(1-6)可以求得：
V2max＝32.14mm/s
此时小臂油缸的无杆腔进油，
Q2max＝A21*V2max＝23.65L/min
4、最大位置放杆垂直运动：放杆位置2500mm，放杆高度范围500mm，最大速度100mm/s，运行时间≤5；图10最大位置放杆垂直运动速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油缸速度曲线。其中大臂油缸速度方程：
V1＝1.0e+005*(-3*0.00000000000896*(-1200-100*t)^2-2*0.00000000199410*(-1200-100*t)+0.00015599459336) (1-7)
由(1-7)可以求得：
V1max＝12.21mm/s
此时大臂油缸的有杆腔进油，
Q1max＝A21*V1max＝4.3L/min
小臂油缸速度方程：
V2＝1.0e+005*(3*0.00000000000689*(-1200-100*t)^2+2*0.00000004849141*(-1200-100*t)+0.00000581005899) (1-8)
由(1-8)可以求得：
V2max＝8.08mm/s
此时小臂油缸的无杆腔进油，
Q2max＝A21*V2max＝5.95L/min
5、最小位置放杆水平运动：放杆位置600mm，最大速度80mm/s，运行时间≤3s；图11最小位置放杆水平运动速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油缸速度曲线。其中大臂油缸速度方程：
V1＝8.0e+003*(3*0.00000000504425*(360+80*t)^2-2*0.00000985143308*(360+80*t)+0.00799300230043) (1-9)
由(1-9)可以求得：
V1max＝22.89mm/s
此时大臂油缸的无杆腔进油，
Q1max＝A11*V1max＝21.46L/min
小臂油缸速度方程：V2＝
8.0e+003*(-3*0.00000000146616*(360+80*t)^2+2*0.00000311665514*(360+80*t)-0.00124306225744) (1-10)
由(1-10)可以求得：
V2max＝7.63mm/s
此时小臂油缸的无杆腔进油，
Q2max＝A12*V2max＝5.61L/min
6、最小位置放杆垂直运动：放杆位置600mm，放杆高度范围500mm，最大速度100mm/s，运行时间≤5s；图12最小位置放杆垂直运动速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油缸速度曲线。其中大臂油缸速度方程：
V1＝1.0e+005*(3*0.00000000077004*(-1200-100*t)^2+2*0.00000386889305*(-1200-100*t)+0.00668233394726) (1-11)
由(1-11)可以求得：
V1max＝72.36mm/s
此时大臂油缸的有杆腔进油，
Q1max＝A21*V1max＝25.62L/min
小臂油缸速度方程：
V2＝1.0e+004*(-3*0.00000000177228*(-1200-100*t)^2-2*0.00000855144543*(-1200-100*t)-0.01570993294590) (1-12)
由(1-12)可以求得：
V2max＝28.43mm/s
此时小臂油缸的无杆腔进油，
Q2max＝A12*V2max＝20.9L/min
Claims (4)
1.一种机械手双轴轨迹插补算法，用于控制机械手按照预定轨迹运动,其特征在于：通过双轴插补算法能够得到机械手大臂油缸和小臂油缸的流量方程，通过将流量方程嵌入到计算机控制程序中，调节电液比例流量方向阀即可实现机械手的运动；所述双轴插补算法根据大臂和小臂液压缸的速度和尺寸确定大臂液压缸和小臂液压缸的流量方程，大臂和小臂液压缸的速度和尺寸通过以下运动阶段的大臂和小臂液压缸的速度方程以及尺寸方程计算；
1)垂直抓取提升阶段：
大臂油缸速度方程：
V1＝1.0e+005*(3*0.00000000001482*(-1700+100*t)^2+2*0.00000018273633*(-1700+100*t)+0.00064385076391)；
大臂油缸尺寸方程：
大臂油缸流量方程：
Q1max＝A11*V1max；
小臂油缸速度方程：
V2＝1.0e+004*(3*0.00000000005127*(-1700+100*t)^2+2*0.00000032211200*(-1700+100*t)-0.00083823451613)；
小臂油缸尺寸方程：
小臂油缸流量方程：
Q2max＝A22*V2max；
2)水平回收：
大臂油缸速度方程：
V1＝1.06e+004*(3*0.00000000058491*(1450-106*t)^2-2*0.00000204110142*(1450-106*t)+0.00345935632275)；
大臂油缸尺寸方程：
大臂油缸流量方程：
Q1max＝A21*V1max；
小臂油缸速度方程：
V2＝1.06e+004*(-3*0.00000000021918*(1450-106*t)^2+2*0.00000092982377*(1450-106*t)+0.00002793357367)；
小臂油缸尺寸方程：
小臂油缸流量方程：
Q2max＝A22*V2max；
3)最大位置放杆水平运动：
大臂油缸速度方程：
V1＝2.14e+004*(3*0.00000000013803*(360+214*t)^2-2*0.00000073219526*(360+214*t)+0.00228666149523)；
大臂油缸尺寸方程：
大臂油缸流量方程：
Q1max＝A11*V1max；
小臂油缸速度方程：
V2＝2.14e+004*(-3*0.00000000008640*(360+214*t)^2+2*0.00000053946243*(360+214*t)+0.00037900721752)；
小臂油缸尺寸方程：
小臂油缸流量方程：
Q2max＝A21*V2max；
4)最大位置放杆垂直运动：
大臂油缸速度方程：
V1＝1.0e+005*(-3*0.00000000000896*(-1200-100*t)^2-2*0.00000000199410*(-1200-100*t)+0.00015599459336)
大臂油缸尺寸方程：
大臂油缸流量方程：
Q1max＝A21*V1max；
小臂油缸速度方程：
V2＝1.0e+005*(3*0.00000000000689*(-1200-100*t)^2+2*0.00000004849141*(-1200-100*t)+0.00000581005899)；
小臂油缸尺寸方程：
小臂油缸流量方程：
Q2max＝A21*V2max；
5)最小位置放杆水平运动：
大臂油缸速度方程：
V1＝8.0e+003*(3*0.00000000504425*(360+80*t)^2-2*0.00000985143308*(360+80*t)+0.00799300230043)；
大臂油缸尺寸方程：
大臂油缸流量方程：
Q1max＝A11*V1max；
6)最小位置放杆垂直运动：
大臂油缸速度方程：
V1＝1.0e+005*(3*0.00000000077004*(-1200-100*t)^2+2*0.00000386889305*(-1200-100*t)+0.00668233394726)；
大臂油缸尺寸方程：
大臂油缸流量方程：
Q1max＝A21*V1max；
小臂油缸速度方程：
V2＝1.0e+004*(-3*0.00000000177228*(-1200-100*t)^2-2*0.00000855144543*(-1200-100*t)-0.01570993294590)；
小臂油缸尺寸方程：
小臂油缸流量方程：
Q2max＝A12*V2max；
其中，t为油缸的运行时间。
2.根据权利要求1所述的机械手双轴轨迹插补算法，其特征在于：准确地计算机械手运动的位置方程。
3.根据权利要求1所述的机械手双轴轨迹插补算法，其特征在于：将机械手运动的位置方程分别转化成大臂油缸和小臂油缸的位置方程。
4.根据权利要求1所述的机械手双轴轨迹插补算法，其特征在于：对各个运动阶段的位置方程求一阶导数获得大臂液压缸和小臂液压缸的速度方程。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510060767.8A CN105988420B (zh) | 2015-02-06 | 2015-02-06 | 一种机械手双轴轨迹插补算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510060767.8A CN105988420B (zh) | 2015-02-06 | 2015-02-06 | 一种机械手双轴轨迹插补算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105988420A CN105988420A (zh) | 2016-10-05 |
| CN105988420B true CN105988420B (zh) | 2019-04-05 |
Family
ID=57037639
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510060767.8A Active CN105988420B (zh) | 2015-02-06 | 2015-02-06 | 一种机械手双轴轨迹插补算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105988420B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN202089693U (zh) * | 2011-05-17 | 2011-12-28 | 徐州昊意工程机械科技有限公司 | 一种基于总线技术的随车起重机有线遥控电器系统 |
| JP2012196749A (ja) * | 2011-03-23 | 2012-10-18 | Seiko Epson Corp | ロボット装置 |
| CN103147577A (zh) * | 2013-02-27 | 2013-06-12 | 中联重科股份有限公司 | 多关节类机械臂架的控制方法、设备、系统及工程机械 |
| CN103383572A (zh) * | 2013-08-07 | 2013-11-06 | 中联重科股份有限公司 | 一种工程机械及其臂架的振动控制方法、控制装置和控制系统 |
| CN104133424A (zh) * | 2014-08-19 | 2014-11-05 | 福州大学 | 一种基于可编程控制器的工业机器人直线插补方法 |
| CN104191428A (zh) * | 2014-08-27 | 2014-12-10 | 深圳科瑞技术股份有限公司 | 一种基于scara机械手的运动轨迹规划方法及系统 |
-
2015
- 2015-02-06 CN CN201510060767.8A patent/CN105988420B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2012196749A (ja) * | 2011-03-23 | 2012-10-18 | Seiko Epson Corp | ロボット装置 |
| CN202089693U (zh) * | 2011-05-17 | 2011-12-28 | 徐州昊意工程机械科技有限公司 | 一种基于总线技术的随车起重机有线遥控电器系统 |
| CN103147577A (zh) * | 2013-02-27 | 2013-06-12 | 中联重科股份有限公司 | 多关节类机械臂架的控制方法、设备、系统及工程机械 |
| CN103383572A (zh) * | 2013-08-07 | 2013-11-06 | 中联重科股份有限公司 | 一种工程机械及其臂架的振动控制方法、控制装置和控制系统 |
| CN104133424A (zh) * | 2014-08-19 | 2014-11-05 | 福州大学 | 一种基于可编程控制器的工业机器人直线插补方法 |
| CN104191428A (zh) * | 2014-08-27 | 2014-12-10 | 深圳科瑞技术股份有限公司 | 一种基于scara机械手的运动轨迹规划方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105988420A (zh) | 2016-10-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107357294B (zh) | 一种砌砖机器人的直线墙体的砌筑算法 | |
| CN203840822U (zh) | 一种新型的移动式绿化带修剪装置 | |
| SE539323C2 (en) | Improved control of remote demolition robot | |
| CN105988420B (zh) | 一种机械手双轴轨迹插补算法 | |
| CN105351263A (zh) | 一种差动液压缸控制回路 | |
| CN202577399U (zh) | 中型液压挖掘机负流量系统中动臂提升优先控制系统 | |
| CN111828406B (zh) | 一种高效节能的tbm水平姿态调整液压系统 | |
| CN103737603B (zh) | 一种流水线上机械臂精确控制系统及控制方法 | |
| CN103921147B (zh) | 一种加工汽轮机大叶片汽道的夹具 | |
| WO2021077950A1 (zh) | 基于关节工况多象限耦合的双足机器人数字液压驱动方法 | |
| CN204892781U (zh) | 矫直机用自动送料系统 | |
| CN205111454U (zh) | 一种新型数控机床轨迹控制系统 | |
| CN107553485A (zh) | 一种人机交互过程中动态虚拟夹具的生成方法 | |
| CN108915007B (zh) | 一种挖掘机动臂斗杆节能系统 | |
| CN102385386A (zh) | 一种水火弯板智能机器人路径规划方法 | |
| CN206123696U (zh) | 一种重型机械手用可调节机构 | |
| CN206493123U (zh) | 一种用于汽车配件加工或者检验中的双工位治具 | |
| CN103419197A (zh) | 横行三轴伺服机械手 | |
| CN104176511A (zh) | 一种码砖机器人 | |
| CN204566146U (zh) | 一种应用于机械臂上的夹取工具 | |
| CN204505274U (zh) | 机械手夹爪机构 | |
| CN208885279U (zh) | 一种盾构机自动纠偏防脱轨装置 | |
| CN206967058U (zh) | 机床自动工作台装置 | |
| CN205571619U (zh) | 智能翻面定齿机构 | |
| CN204979984U (zh) | 一种防堵塞下料管 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 100083 Haidian District, Xueyuan Road, No. 606, West building, room 30, group of rooms, room six Applicant after: Beijing long Intelligent Technology Co., Ltd. Address before: 100083 Haidian District, Xueyuan Road, No. 606, West building, room 30, group of rooms, room six Applicant before: Beijing long letter transmission and Control Technology Co., Ltd. |
|
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20211124 Address after: 215200 north side of building e, No. 1868 Guangming Road, Jiangling street, Wujiang District, Suzhou City, Jiangsu Province Patentee after: Suzhou Langxin Intelligent Technology Co.,Ltd. Address before: Room 606, West 6th floor, Kequn building, No. 30 Xueyuan Road, Haidian District, Beijing 100083 Patentee before: Beijing long Intelligent Technology Co.,Ltd. |