[发明专利]一种基于梯形规划的机械臂多关节轨迹时间同步方法

说明书

本发明涉及一种基于梯形规划的机械臂多关节轨迹时间同步方法，其针对多关节机械臂的初始位姿和终点位姿，首先使用逆解算法计算出各个关节的初始角度序列和重点角度序列，然后采用经典的梯形轨迹规划方法求出每个关节的短时梯形轨迹参数，然后计算每个关节的总运行时间，并从中选择出总运行时间最大的关节；最后结合所选出的总运行时间最大的关节的梯形轨迹参数对其他关节的短时梯形轨迹进行修正，将不同的运动时间修正为相同的运动时间，实现各个关节之间同时启动同时停止的目的。

技术领域

本发明涉及机械臂控制领域，具体涉及一种基于梯形规划的机械臂多关节轨迹时间同步方法。

背景技术

关节插补功能是机械臂的控制系统的一个重要功能，它的效果是使机械臂从初始位姿运行到终点位姿，运行过程中需满足多个关节从特定的初始角度运行到特定的终点角度。如下图1所示，以3关节为例。

机械臂的多关节运动中，各个关节的初始角度和终点角度的差值，即角度行程，一般是不一样的，机械臂的关节插补功能要求各个关节在初始角度同时启动，在终点角度同时停止，即时间同步。

使用经典的梯形轨迹规划算法，在规划不同的角度行程时，算出的时间是不一样的，即各个关节的时间不同步。如果各个关节的运动不同步，那么机械臂在执行一条关节插补运动指令的时候，多个关节是在不同的时间停止运动的，当调试人员在对机械臂编程时，调试人员无法正确判定运动指令是否已经停止或即将停止，这会造成误判，严重情况下会造成机械臂碰撞到其它外围设备的情况，所以对关节的时间同步已成为机械臂控制系统开发中的一个基础功能。

发明内容

针对现有技术存在的问题，本发明的目的在于提供一种基于梯形规划的机械臂多关节轨迹时间同步方法。

为实现上述目的，本发明采用的技术方案是：

一种基于梯形规划的机械臂多关节轨迹时间同步方法，其包括以下步骤：

步骤1、对于包含n个关节的机械臂，其的初始位姿T₀和终点位姿T₁，使用逆解算法计算各个关节的初始角度序列α₁,…,α_n和终点角度序列β₁,…,β_n，其中，n＞1；

步骤2、根据上述计算得到的每个关节的参数：初始角度、终点角度、极限速度和极限加速度，经过经典的梯形轨迹规划方法后，求出每个关节的梯形轨迹参数：加速时间t1₁,…,t1_n、匀速时间t2₁,…,t2_n、减速时间t3₁,…,t3_n、可达最大速度wm₁,…,wm_n、可达最大加速度αm₁，…,αm_n和速度轨迹类型P₁,…,P_n；

P₁,…,P_n为2或者3，表示速度轨迹是2段式轨迹或3段式轨迹；使用梯形轨迹规划方法时设定的加速时间和减速时间相同；

步骤3、计算每个关节的总运行时间，求得n个关节的运行时间T₁,…,T_n；取出n个关节的总运行时间T₁,…,T_n的最大值，记为T_max，设这个总运行时间最长的关节的编号为k；

步骤4、对所有关节梯形轨迹参数进行时间同步处理：