[发明专利]一种工业机器人关节空间同步运动的规划方法

权利要求书

1.一种工业机器人关节空间同步运动的规划方法，其特征在于，包括以下步骤：

【1】启动运动控制器；

【2】获取期望的机器人运动的起始点和末端点；

【3】计算规划所需的最大时长MaxT；

【3.1】根据已知的机器人各关节运动的起始点和末端点，计算每个运动关节需转动的角度θ_i，具体公式为：

θ_i＝sEndP.A_i-sStaP.A_i

其中，sEndP.A_i为第i关节运动的末端点，sStaP.A_i为第i关节运动的起始点；

【3.2】计算每个运动关节要达到期望的关节运动特性所需的加速时间Ta_Acci和从期望的关节速度减速到零的减速时间Ta_Deci，具体公式为：

其中，lrAcc[i]为期望的第i关节的加速度，lrVel[i]为期望的第i关节的速度，lrDec[i]为期望的第i关节的减速度；

【3.3】计算每个运动关节按照期望的关节运动特性需要的加速和减速时转动的角度，具体公式为：

θa_Acci为第i关节加速时转动的角度，θa_Deci为第i关节减速时转动的角度；

【3.4】计算每个运动关节总的运动时长，具体公式为：

其中，MaxV_i代表第i关节实际所能到达的最大速度；

【3.5】在T_i中找出最大的一个值，即就是最大时长MaxT；

【4】计算各关节运动规划所需的5个系数；

其中，a_0i、a_1i、a_2i、a_3i、a_4i代表第i关节运动规划所需的5个系数；

【5】判断运行模式，若为正常模式，则执行步骤【6】，若为减速停止模式，则执行步骤【7】；

【6】正常运行模式下的规划；

计算正常模式下各个运动关节第j个插值点的角度MidAngles.A_i；

其中，lrCycleTime为插值的间隔时间，j＝1,2,…为插值点的序列数；计算过程中lrCycleTime×j≤MaxT，若，lrCycleTime×j＞MaxT，则规划完成；

【7】减速停止模式下的规划；

【7.1】计算当前各个运动关节的转动速度υ_i，具体公式为：

【7.2】计算各个运动关节从当前速度减速到停止需要的时间，具体公式为：

【7.3】在Tstopi中获取最大停止时间MaxT_stop；

【7.4】计算各个运动关节的角度，具体公式为：

A、当在MaxT_stop＞lrCycleTime的情况下，

其中θ_ci是当前的各关节的角度；j＝1,2,…是插值点的序列数；计算过程中需满足lrCycleTime×j≤MaxT_stop，若lrCycleTime×j＞MaxT_stop时，规划结束；

B、当在MaxT_stop≤lrCycleTime情况下，规划结束；

【8】结束。