一种机械手双轴轨迹插补算法
【专利摘要】本发明公开了一种机械手双轴轨迹插补算法，用于控制机械手按照预定轨迹进行运动。其特征是：通过双轴插补算法能够得到机械手大臂油缸和小臂油缸的流量方程，通过将流量方程嵌入到计算机控制程序中，调节电液比例流量方向阀即可实现机械手的运动。这种算法具有控制灵活、适应性强、又有控制精度高、稳定性强等特点，能够显著提高作业效率，降低人身伤亡事故发生的概率。
【专利说明】
-种机械手双轴轨迹插补算法
技术领域
[0001] 本发明设及机械手技术领域，特别是设及一种机械手双轴轨迹插补算法。
【背景技术】
[0002] 机械手作为钻井作业专口配备的机械，可W减轻繁重的体力劳动，改善劳动条件 和安全生产，能够通过自动控制按照预定轨迹移动，在提高钻井作业工作效率，增强企业竞 争力等方面起到极其重要的作用。现有机械手在控制方面存在的缺陷是：机械手一般是用 单片机作为主要控制单元，由于单片机的编程工作比较复杂，其内部程序固化简单，一旦出 现故障或者机械手工作任务改变需要修改程序的情况，则需要由专业人员对单片机的内部 程序进行重新编写，利用单片机对机械手进行控制的操作灵活性和通用性差、成本高、开发 难度大，且控制精度和效率不高。因此，研究出操作方便且实用、生产效率高且成本低的机 械手控制算法成为一种需求。
【发明内容】
[0003] 为了克服上述现有技术的不足，本发明提供了一种高效准确、生产成本低的机械 手双轴轨迹插补算法。
[0004] 本发明所采用的技术方案是：推导出机械手比较理想的轨迹运动，按照预先设计 的轨迹运动求得了其数学方程，根据机械手的轨迹运动确定两个驱动液压缸的位置和速度 方程（确定了液压系统中液压缸的工作流量方程），根据液压缸的工作流量方程嵌入到计 算机控制程序中，调节电液比例流量方向阀从而实现机械手的运动。
[0005] 与现有技术相比，本发明的有益效果是能够简单有效地控制机械手按照预定轨迹 运动，稳定可靠，控制反应灵敏，程序维护成本低，生产效率高。
【附图说明】
[0006] 图1为机械手坐标定位图
[0007] 图2为机械手坐标定位系统计算软件界面图
[0008] 图3为抓取提升算法界面图
[0009] 图4为水平回收算法界面图
[0010] 图5为置最远端算法界面图
[0011] 图6为置最近端算法界面图
[0012] 图7为抓取提升油缸速度曲线图
[0013] 图8为水平回收油缸速度曲线图
[0014] 图9为最大位置放杆水平运动油缸速度曲线
[0015] 图10为最大位置放杆垂直运动油缸速度曲线
[0016] 图11为最小位置放杆水平运动油缸速度曲线
[0017] 图12为最小位置放杆垂直运动油缸速度曲线
【具体实施方式】
[0018] 图1机械手坐标定位图中，图（1)为空间总坐标系，坐标系原点建立在平台导轨左 端极限位置的中屯、，其中h为导轨中间平面到机械手大臂L1的竖直距离（由于h未知，公 式中h暂取为0)。由图中可知，C点坐标为（xi+lx，ly，Iz)。
[001引 图似中将坐标系原点平移到0'，可知0、A、B、C在同一平面内，则可在X'OZ平 面坐标系内求解Ix'，如图（3)所示，可得lx'
[0031] 根据对W上机械手坐标定位图的分析，我们设计了机械手坐标定位系统计算软件 (如图2至图6所示），更加方便、准确地计算机械手运动的位置方程，再将机械手运动的 位置方程分别转化成大臂油缸和小臂油缸的位置方程，对各个运动阶段的位置方程求一阶 导数即获得大臂液压缸和小臂液压缸的速度方程，运个方程中含有较复杂的Ξ角关系，为 了便于后期计算机运算求解，对大臂液压缸和小臂液压缸的速度方程进行采用了多项式拟 合，得到了多项式方程，由于大臂和小臂液压缸的尺寸是确定的，即可W得到大臂液压缸和 小臂液压缸的流量方程，具体分析如下：
[0032] 1、垂直抓取提升：提升范围0~500mm，最大速度lOOmm/s，运行时间《5s ;图7抓 取提升油缸速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油缸速度曲线。 其中大臂油缸速度方程：
[0040]由（1-2)可 W求得：
[0041] V2max = 14. 9mm/s
[0042] 此时小臂油缸的有杆腔进油，
[0043] Q2max = A22*V2max = 5. 2化/min
[0044] 注：A11-大臂油缸的无杆腔面积
[0045] A21-大臂油缸的有杆腔面积
[0046] A12-小臂油缸的无杆腔面积
[0047] A22-小臂油缸的有杆腔面积
[004引 2、水平回收：回收范围1450~390mm，最大速度106mm/s，运行时间《10s ;图8水 平回收油缸速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油缸速度曲线。 其中大臂油缸速度方程：
[0059] Q2max = A22*V2max = 5. 0：3L/min
[0060] 3、最大位置放杆水平运动：水平放杆范围2500mm，最大速度214mm/s，运行时间 《10 ;图9最大位置放杆水平运动速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线 为小臂油缸速度曲线。其中大臂油缸速度方程：
[0061] VI = 2. 14e+004*(3*0. 00000000013803 *(360巧 14村）~2-2*0. 00000073219526*(360+214*t) +0. 00228666149523) (1_巧
[0062] 由（1-5)可 W求得：
[0063] Vlmax = 38. 8mm/s
[0064] 此时大臂油缸的无杆腔进油
[00巧]Qlmax = All*Vlmax = 28. 5f5L/min
[0066] 小臂油缸速度方程：
[0067] V2 = 2. 14e+004*(-3*0. 00000000008640 *(360+214村）~2巧*0. 00000053946243*(360巧 14*t) +0. 00037900721752) (1-6)
[006引 由（1-6)可W求得：
[0069] V2max = 32. 14mm/s
[0070] 此时小臂油缸的无杆腔进油，
[0071] Q2max = A21*V2max = 23. 6f5L/min
[007引 4、最大位置放杆垂直运动：放杆位置2500mm，放杆高度范围500mm，最大速度 lOOmm/s，运行时间《5 ;图10最大位置放杆垂直运动速度曲线图中蓝色曲线为大臂油缸速 度曲线，绿色曲线为小臂油缸速度曲线。其中大臂油缸速度方程：
[0073] VI = 1. Oe+005*(-3*0. 00000000000896*( -1200-100*t)'2-2*0. 00000000199410*(-1200-100*t)+0. 00015599459336) (1-7)
[0074] 由（1-7)可 W求得：
[00巧] Vlmax = 12. 21mm/s
[0076] 此时大臂油缸的有杆腔进油
[0077] Qlmax = A21*Vlmax = 4. 3L/min [007引小臂油缸速度方程：
[0079] V2 = 1. Oe+005*(3*0. 00000000000689* (- 1200-100*t)-2巧*0. 00000004849141*(-1200-100*t)+0. 00000581005899) (1-8)
[0080] 由（1-8)可 W求得：
[0081] V2max = 8. 08mm/s
[0082] 此时小臂油缸的无杆腔进油，
[0083] Q2max = A21*V2max = 5. 9f5L/min
[0084] 5、最小位置放杆水平运动：放杆位置600mm，最大速度80mm/s，运行时间《3s ;图 11最小位置放杆水平运动速度曲线图中蓝色曲线为大臂油缸速度曲线，绿色曲线为小臂油 缸速度曲线。其中大臂油缸速度方程：
[0085] VI = 8. Oe+003*(3*0. 00000000504425 *(360+80村）~2-2*0. 00000985143308*(360+80*t) +0. 00799300230043) (1-9)
[008引 由（1-9)可W求得：
[0087] Vlmax = 22. 89mm/s
[0088] 此时大臂油缸的无杆腔进油
[0089] Qlmax = All*Vlmax = 21. 46L/min
[0090] 小臂油缸速度方程：V2 =
[0091] 8. Oe+003* (-3*0. 00000000146616* (360+80*tr2巧*0. 00000311665514* (360+80 *t)-0.00124306225744) (1-10)
[0092] 由（1-10)可 W求得：
[0093] V2max = 7. 63mm/s
[0094] 此时小臂油缸的无杆腔进油
[0095] Q2max = A12*V2max = 5. 61L/min
[0096] 6、最小位置放杆垂直运动：放杆位置600mm，放杆高度范围500mm，最大速度 lOOmm/s，运行时间《5s ;图12最小位置放杆垂直运动速度曲线图中蓝色曲线为大臂油缸 速度曲线，绿色曲线为小臂油缸速度曲线。其中大臂油缸速度方程：
[0097] VI = 1. Oe+005*(3*0. 00000000077004*(- 1200-100村）~2巧*0. 00000386889305*(-1200-100*t)+0. 00668233394726) (1-11)
[0098] 由（1-11)可 W求得：
[0099] Vlmax = 72. 36mm/s
[0100] 此时大臂油缸的有杆腔进油
[0101] Qlmax = A21*Vlmax = 25. 6化/min
[0102] 小臂油缸速度方程：
[0103] V2 = 1. Oe+004*(-3*0. 00000000177228* ( -1200-100*t)'2-2*0. 00000855144543*(-1200-100*t)-0. 01570993294590) (1-12)
[0104] 由（1-。）可 W求得：
[010引 V2max = 28. 43mm/s
[0106] 此时小臂油缸的无杆腔进油，A12=^ X 0· 1252
[0107] Q2max 二 A12本V2max 二 20.化/min
【主权项】
1. 一种机械手双轴轨迹插补算法，用于控制机械手按照预定轨迹运动。其特征在于： 通过双轴插补算法能够得到机械手大臂油缸和小臂油缸的流量方程，通过将流量方程嵌入 到计算机控制程序中，调节电液比例流量方向阀即可实现机械手的运动。2. 根据权利要求1所述的机械手双轴轨迹插补算法，其特征在于：准确地计算机械手 运动的位置方程。3. 根据权利要求1所述的机械手双轴轨迹插补算法，其特征在于：将机械手运动的位 置方程分别转化成大臂油缸和小臂油缸的位置方程。4. 根据权利要求1所述的机械手双轴轨迹插补算法，其特征在于：对各个运动阶段的 位置方程求一阶导数获得大臂液压缸和小臂液压缸的速度方程5. 根据权利要求1所述的机械手双轴轨迹插补算法，其特征在于：根据大臂和小臂液 压缸的速度和尺寸确定大臂液压缸和小臂液压缸的流量方程。
【文档编号】G05B19/41GK105988420SQ201510060767
【公开日】2016年10月5日
【申请日】2015年2月6日
【发明人】钟海胜, 秦嗣峰, 曹旭东, 赵德华
【申请人】北京朗信传动与控制技术有限公司