[发明专利]一种拖动示教机器人的平衡算法

摘要

本发明涉及一种拖动示教机器人的平衡算法，包括静态平衡、动态平衡和误差控制三个部分：静态平衡算法为可计算出机器人第二轴在不同的位姿下的静态压力补偿值set_bal_ax2，和机器人第三轴在不同的位姿下的静态压力补偿值set_bal_ax3；所述动态平衡的算法分为示教机器人的第二轴算法和第三轴算法；所述误差控制采用由信号输入、增益K、比例系数K(·)、电磁阀以及信号输出组成的闭环控制系统，通过调整参数增益K和比例系数K(·)的值，满足机器人拖动的速度连续，同时保证机器人的轨迹精度。本发明通过将空间划分为不同象限，利用不同象限的算法得出补偿值，指导第二轴和第三轴的气缸支撑强度，降低了示教过程中人工移动机械臂的难度，有利于提高示教精确度，保证示教效果。

主权项

1.一种拖动示教机器人的平衡算法，其特征在于：包括静态平衡、动态平衡和误差控制三个部分：静态平衡的算法为：由公式(1)可计算出机器人第二轴在不同的位姿下的静态压力补偿值set_bal_ax2，由公式(2)可计算出机器人第三轴在不同的位姿下的静态压力补偿值set_bal_ax3；set_bal_ax2＝par_2[1]+(1‑cos(robot.axis[2]/180*PI))*par_2[2]  (1)set_bal_ax3＝par_3[0]+par_3[3]*(‑robot.axis[3])/90.0  (2)式中：par_2[1]为机器人第二轴基本压力值，par_2[2]为机器人第二轴静态压力值；par_3[0]为机器人第三轴基本压力值，par_3[3]为机器人第三轴静态压力值；robot.axis[2]为机器人第二轴角度值，robot.axis[3]为机器人第三轴角度值；所述动态平衡的算法分为示教机器人的第二轴算法和第三轴算法，具体为：第二轴的动态平衡计算：当机器人第二轴当前角度为负角度，拖动机器人第二轴向正方向运动时，采用公式(3)进行压力补偿；拖动机器人第二轴向负方向运动时，采用公式(4)进行压力补偿；当机器人第二轴当前角度为正角度，拖动机器人第二轴向正方向运动时，采用公式(5)进行压力补偿；拖动机器人第二轴向负方向运动时，采用公式(6)进行压力补偿；set_bar_ax2＝set_bar_ax2+par_2[5]*cos(robot.axis[2]/180*PI)  (3)set_bar_ax2＝set_bar_ax2‑par_2[6]*cos(robot.axis[2]/180*PI)  (4)set_bar_ax2＝set_bar_ax2‑par_2[7]*cos(robot.axis[2]/180*PI)  (5)set_bar_ax2＝set_bar_ax2+par_2[8]*cos(robot.axis[2]/180*PI)  (6)式中：等式左侧的set_bal_ax2为机器人第二轴的需求动态补偿值，等式右侧的set_bal_ax2为机器人第二轴的当前动态补偿值，par_2[5]为机器人第二轴在负角度时向后运动的基本压力值；par_2[6]为机器人第二轴在负角度时向前运动的动态压力值；par_2[7]为机器人第二轴在正角度时向前运动的动态压力值；par_2[8]为机器人第二轴在正角度时向后运动的动态压力值。