一种基于梯形规划的机械臂多关节轨迹时间同步方法与流程

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


背景技术：

2.关节插补功能是机械臂的控制系统的一个重要功能，它的效果是使机械臂从初始位姿运行到终点位姿，运行过程中需满足多个关节从特定的初始角度运行到特定的终点角度。如下图1所示，以3关节为例。
3.机械臂的多关节运动中，各个关节的初始角度和终点角度的差值，即角度行程，一般是不一样的，机械臂的关节插补功能要求各个关节在初始角度同时启动，在终点角度同时停止，即时间同步。
4.使用经典的梯形轨迹规划算法，在规划不同的角度行程时，算出的时间是不一样的，即各个关节的时间不同步。如果各个关节的运动不同步，那么机械臂在执行一条关节插补运动指令的时候，多个关节是在不同的时间停止运动的，当调试人员在对机械臂编程时，调试人员无法正确判定运动指令是否已经停止或即将停止，这会造成误判，严重情况下会造成机械臂碰撞到其它外围设备的情况，所以对关节的时间同步已成为机械臂控制系统开发中的一个基础功能。


技术实现要素：

5.针对现有技术存在的问题，本发明的目的在于提供一种基于梯形规划的机械臂多关节轨迹时间同步方法。
6.为实现上述目的，本发明采用的技术方案是：
7.一种基于梯形规划的机械臂多关节轨迹时间同步方法，其包括以下步骤：
8.步骤1、对于包含n个关节的机械臂，其的初始位姿t0和终点位姿t1，使用逆解算法计算各个关节的初始角度序列α1,
…
,αn和终点角度序列β1,
…
,βn，其中，n＞1；
9.步骤2、根据上述计算得到的每个关节的参数：初始角度、终点角度、极限速度和极限加速度，经过经典的梯形轨迹规划方法后，求出每个关节的梯形轨迹参数：加速时间t11,
…
,t1n、匀速时间t21,
…
,t2n、减速时间t31,
…
,t3n、可达最大速度wm1,
…
,wmn、可达最大加速度αm1，
…
,αmn和速度轨迹类型p1,
…
,pn；
10.p1,
…
,pn为2或者3，表示速度轨迹是2段式轨迹或3段式轨迹；使用梯形轨迹规划方法时设定的加速时间和减速时间相同；
11.步骤3、计算每个关节的总运行时间，求得n个关节的运行时间t1,
…
,tn；取出n个关节的总运行时间t1,
…
,tn的最大值，记为t
max
，设这个总运行时间最长的关节的编号为k；
12.步骤4、对所有关节梯形轨迹参数进行时间同步处理：
13.输入为所有关节的短时梯形轨迹参数：加速时间t11,
…
,t1n、匀速时间t21,
…
,t2n、减速时间t31,
…
,t3n、可达最大速度wm1,
…
,wmn、可达最大加速度αm1，
…
,αmn和速度轨
迹类型p1,
…
,pn；
14.输出为所有关节的长时梯形轨迹参数，即修正后的参数值：修正加速时间t1'1,
…
,t1'n、修正匀速时间t2'1,
…
,t2'n、修正减速时间t3'1,
…
,t3'n、修正可达最大速度wm'1,
…
,wm'n、修正可达最大加速度αm'1，
…
,αm'n和速度轨迹类型p'1,
…
,p'n；
15.对于输入的每一个关节参数，判断其是否属于第k个关节，若其为第k个关节，那么该关节的长时梯形轨迹参数等于短时梯形轨迹参数；若输入的关节不属于第k个关节，判断其速度轨迹类型；当输入关节的速度轨迹为2段式时，该输入关节的长时轨迹参数设置如下：t2'i＝0；t3'i＝t1'i；p'i＝pi；
16.当输入关节的速度轨迹为3段式时，该输入关节的长时轨迹参数设置如下：
17.αm'i＝αmi；
18.t3'i＝t1'i；p'i＝pi19.ω1，
…
,ωn为极限加速度，是预设值；
20.步骤5、将步骤4得到的所有关节的长时梯形轨迹参数输入到插补模块并下发到相应的电机即可。
21.采用上述方案后，本发明针对多关节机械臂的初始位姿和终点位姿，首先使用逆解算法计算出各个关节的初始角度序列和重点角度序列，然后采用经典的梯形轨迹规划方法求出每个关节的短时梯形轨迹参数，然后计算每个关节的总运行时间，并从中选择出总运行时间最大的关节；最后结合所选出的总运行时间最大的关节的梯形轨迹参数对其他关节的短时梯形轨迹进行修正，将不同的运动时间修正为相同的运动时间，实现各个关节之间同时启动同时停止的目的。
附图说明
22.图1为多关节机械臂的关节运动示意图；
23.图2为本发明的时间同步方法流程图；
24.图3为多关节时间同步示意图(以3段式为例)；
25.图4为步骤2的流程示意图。
具体实施方式
26.如图2-3所示，本发明揭示了一种基于梯形规划的机械臂多关节轨迹时间同步方法，其基于以下前提进行：
27.(1)本发明可用于任意的多关节机械臂，假设n个关节，n＞1；
28.(2)各关节的轨迹参数，包括极限速度ψ1，
…
,ψn，极限加速度ω1，
…
,ωn是预设值，属于提前给定的；
29.(3)使用梯形轨迹规划方法时设定的加速时间和减速时间相同。
30.基于以上，本发明具体包括以下步骤：
31.步骤1、对于给定机械臂的初始位姿t0和终点位姿t1，使用逆解算法计算各个关节的初始角度序列α1,
…
,αn和终点角度序列β1,
…
,βn。
32.步骤2、根据上述计算得到的每个关节的参数：初始角度、终点角度、极限速度和极限加速度，经过经典的梯形轨迹规划方法后，求出每个关节的梯形轨迹参数：加速时间t11,
…
,t1n、匀速时间t21,
…
,t2n、减速时间t31,
…
,t3n、可达最大速度wm1,
…
,wmn、可达最大加速度αm1，
…
,αmn和速度轨迹类型p1,
…
,pn；
33.p1,
…
,pn只能为2或者3，表示速度轨迹是2段式轨迹或3段式轨迹。对于每个关节，上述的6个参数确定了以后，即决定了一个关节的梯形轨迹，如图4所示。
34.步骤3、计算每个关节的总运行时间(例如第i个关节，ti＝t1i+t2i+t3i)求得n个关节的运行时间t1,
…
,tn；取出n个关节的总运行时间t1,
…
,tn的最大值，记为t
max
，设这个总运行时间最长的关节的编号为k。
35.步骤4、对所有关节梯形轨迹参数进行时间同步处理：
36.输入为所有关节的短时梯形轨迹参数(即步骤2得到的关节参数)：加速时间t11,
…
,t1n、匀速时间t21,
…
,t2n、减速时间t31,
…
,t3n、可达最大速度wm1,
…
,wmn、可达最大加速度αm1，
…
,αmn和速度轨迹类型p1,
…
,pn。
37.输出为所有关节的长时梯形轨迹参数，即修正后的参数值：修正加速时间t1'1,
…
,t1'n、修正匀速时间t2'1,
…
,t2'n、修正减速时间t3'1,
…
,t3'n、修正可达最大速度wm'1,
…
,wm'n、修正可达最大加速度αm'1，
…
,αm'n和速度轨迹类型p'1,
…
,p'n。
38.对于输入的每一个关节参数，判断其是否属于第k个关节，若其为第k个关节，那么该关节的长时梯形轨迹参数等于短时梯形轨迹参数；若输入的关节不属于第k个关节，判断其速度轨迹类型；当输入关节的速度轨迹为2段式时，该输入关节的长时轨迹参数设置如下：
39.t2'i＝0；t3'i＝t1'i；p'i＝pi；
40.当输入关节的速度轨迹为3段式时，该输入关节的长时轨迹参数设置如下：
41.αm'i＝αmi；
42.t3'i＝t1'i；p'i＝pi43.该步骤运算具体如下：
[0044][0045]
步骤5、将步骤4得到的所有关节的长时梯形轨迹参数输入到插补模块并下发到相应的电机即可。
[0046]
综上，本发明针对多关节机械臂的初始位姿和终点位姿，首先使用逆解算法计算出各个关节的初始角度序列和重点角度序列，然后采用经典的梯形轨迹规划方法求出每个关节的短时梯形轨迹参数，然后计算每个关节的总运行时间，并从中选择出总运行时间最大的关节；最后结合所选出的总运行时间最大的关节的梯形轨迹参数对其他关节的短时梯形轨迹进行修正，将不同的运动时间修正为相同的运动时间，实现各个关节之间同时启动同时停止的目的。
[0047]
以上所述，仅是本发明实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。