[发明专利]一种机械手双轴轨迹插补算法

摘要

本发明公开了一种机械手双轴轨迹插补算法，用于控制机械手按照预定轨迹进行运动。其特征是：通过双轴插补算法能够得到机械手大臂油缸和小臂油缸的流量方程，通过将流量方程嵌入到计算机控制程序中，调节电液比例流量方向阀即可实现机械手的运动。这种算法具有控制灵活、适应性强、又有控制精度高、稳定性强等特点，能够显著提高作业效率，降低人身伤亡事故发生的概率。

主权项

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为油缸的运行时间。