一种机械臂末端运动位姿时间同步方法与流程

1.本发明涉及机械臂控制领域，具体涉及一种机械臂末端运动位姿时间同步方法。


背景技术：

2.如图1所示为机械臂的结构示意图，机械臂的一个主要工作是执行机械臂末端运动指令，即给定机械臂末端的工作空间的起点位姿信息(位置和姿态)和工作空间的终点位姿信息(位置和姿态)，让机械臂末端在工作空间中作直线路径或其它形状的路径运动，如下图2所示。机械臂末端的位置行程和姿态行程均可使用固定在机械臂末端的坐标系的变化来表示，如图3和图4所示，其中图3表示的位置行程为坐标系{1}到坐标系{2}的平行运动，绕虚线平移距离1；图4表示的姿态行程为坐标系{1}到坐标系{2}的旋转运动，绕r旋转角度
3.由于位置行程和姿态行程是分开作轨迹规划计算的，位置和姿态的计量单位是不同的，前者是距离，后者是角度(如下图3和图4所示)，经过轨迹规划计算后得到的轨迹参数也是不同的，所以经过轨迹规划算法得到的运动时间一般也是不一样的。
4.但是在实际应用中，机械臂在工作时，是需要让机械臂末端的位置和姿态同时开始启动，同时停止，如图2所示，机械臂从虚线状态运动到实线状态，机械臂末端沿直线运动，坐标系的位置和姿态均一直在变化。这样的一个必要性是为了机械臂调试方便和安全，可消除用户对机械臂工作中动作的误判。所以，机械臂的位姿时间同步成为机械臂控制系统的一个重要的功能。


技术实现要素：

5.本发明的目的在于提供一种机械臂末端运动位姿时间同步方法，以试下机械臂末端的位置和姿态同时开始变化，同时停止的目的。
6.为实现上述目的，本发明采用的技术方案是：
7.一种机械臂末端运动位姿时间同步方法，所述方法基于梯形轨迹规划方法进行，设定位置和姿态的轨迹参数，包括位置的极限线速度ψ
p
，极限线加速度ω
p
，姿态的极限角速度ψr，极限角加速度ωr；同时设定加速时间和减速时间相同；
8.具体包括以下步骤：
9.步骤1、给定机械臂末端的初始位姿t0和终点位姿t1，，其中p0表示初始位置向量，r0表示初始姿态矩阵，p1表示终点位置向量，r1表示终点姿态矩阵；
10.步骤2、进行位置轨迹规划：
11.根据初始位置p0、终点位置p1、极限线速度ψ
p
和极限线加速度ωr，利用梯形轨迹
规划方法求出位置轨迹参数：加速时间t1、匀速时间t2、减速时间t3、可达最大速度vm、可达最大加速度am、速度轨迹类型p
tj
，p
tj
为2或者3，分别表示速度轨迹是2段式轨迹或3段式轨迹；
12.步骤3、进行姿态轨迹规划：
13.根据初始姿态r0、终点姿态r1、极限角速度ψ
p
和极限角加速度ωr，利用“角-轴”法求角度：
14.将初始姿态r0到终点姿态r1的行程，转化为绕姿态旋转轴r的起始姿态角度θ0、终点姿态角度θ1，两角度之差即为姿态的变化行程；
15.再通过经典梯形轨迹规划方法求出旋转角度的轨迹参数：加速时间t1、匀速时间t2、减速时间t3、可达最大角速度wm、可达最大角加速度αm、速度轨迹类型p
θtj
，p
θtj
只能为2或者3，分别表示速度轨迹是2段式轨迹或3段式轨迹；
16.步骤4、计算位置和姿态的总运行时间：
17.位置行程的总时间：t＝t1+t2+t3，姿态行程的总时间：t＝t1+t2+t3，取出t和t的较大值，记为t
max
；
18.步骤5、利用时间延长算法对位置和姿态的梯形轨迹参数进行时间同步处理：输入为位置和姿态的短时梯形轨迹参数：加速时间t1，t1、匀速时间t2，t2、减速时间t3，t3、可达最大速度vm，wm、可达最大加速度am，αm和速度轨迹类型p
tj
，p
θtj
；输出为所有位置和姿态的长时梯形轨迹参数，即修正后的参数值：修正加速时间t1
′
，t1
′
、修正匀速时间t2
′
，t2
′
、修正减速时间t3
′
，t3
′
、修正可达最大速度vm
′
，wm
′
、修正可达最大加速度am
′
，αm
′
和速度轨迹类型p
tj
′
，p
θtj
′
；
19.步骤6、对位置和姿态进行插补，并将修成后的位置和姿态轨迹参数下发到逆解模块中，即完成了位置和姿态的时间同步。
20.所述时间同步处理具体如下：
21.对于输入的位置轨迹参数；
22.如果是轨迹类型是2段式轨迹，那么，
23.修正后的可达最大速度
24.修正后的加速时间
25.修正后的可达最大加速度
26.修正后的匀速时间t2
′
＝0；
27.修正后的减速时间t3
′
＝t1
′
；
28.修正后的速度轨迹类型p
′
tj
＝p
tj
；
29.如果轨迹类型是3段式轨迹，那么，
30.修正后的可达最大速度
31.修正后的加速时间
32.修正后的匀速时间
33.修正后的减速时间t3
′
＝t1
′
；
34.修正后的可达最大加速度am
′
＝am；
35.修正后的速度轨迹类型p
′
tj
＝p
tj
；
36.对于输入的姿态轨迹参数，
37.如果轨迹类型是2段式轨迹，那么，
38.修正后的可达最大角速度
39.修正后的加速时间
40.修正后的可达最大角加速度
41.修正后的匀速时间t2
′
＝0；
42.修正后的减速时间t3
′
＝t1
′
；
43.修正后的速度轨迹类型p
θtj
′
＝p
θtj
；
44.如果轨迹类型是3段式轨迹，那么
45.修正后的可达最大角速度
[0046][0047]
修正后的加速时间
[0048]
修正后的匀速时间
[0049]
修正后的减速时间t3
′
＝t1
′
；
[0050]
修正后的可达最大角加速度αm
′
＝αm；
[0051]
修正后的速度轨迹类型p
θtj
′
＝p
θtj
。
[0052]
采用上述方案后，本发明首先使用梯形轨迹规划方法处理得到位置轨迹参数，使用“角-轴”法和梯形轨迹规划方法结合得到姿态轨迹参数，然后基于位置轨迹参数和位置轨迹参数确定最大运行时间，最后依据该最大运行时间调整位置轨迹参数和姿态轨迹参数，实现位置和姿态的同步处理。
附图说明
[0053]
图1为机械臂的结构示意图；
[0054]
图2为机械臂末端位置和姿态变的同步变化效果图；
[0055]
图3为机械臂末端的位置行程示意图；
[0056]
图4为机械臂末端的姿态行程示意图；
[0057]
图5为本发明的同步方法流程图；
[0058]
图6为梯形轨迹规划时间延长算法流程图。
具体实施方式
[0059]
如图5所示，本发明揭示了一种机械臂末端运动位姿时间同步方法，其基于梯形轨迹规划方法进行，提前设定位置和姿态的轨迹参数，包括位置的极限线速度ψ
p
，极限线加速度ω
p
，姿态的极限角速度ψr，极限角加速度ωr；同时设定加速时间和减速时间相同。基于这些前提，本发明的位姿时间同步方法具体包括以下步骤：
[0060]
步骤1、给定机械臂末端的初始位姿t0和终点位姿t1，，其中p0表示初始位置向量，r0表示初始姿态矩阵，p1表示终点位置向量，r1表示终点姿态矩阵。
[0061]
步骤2、进行位置轨迹规划。根据初始位置p0、终点位置p1、极限线速度ψ
p
和极限线加速度ωr，利用梯形轨迹规划方法求出位置轨迹参数：加速时间t1、匀速时间t2、减速时间t3、可达最大速度vm、可达最大加速度am、速度轨迹类型p
tj
，p
tj
只能为2或者3，分别表示速度轨迹是2段式轨迹或3段式轨迹。对于空间位置，上述的参数确定了以后，即决定了一个位置的梯形曲线轨迹。
[0062]
步骤3、进行姿态轨迹规划。根据初始姿态r0、终点姿态r1、极限角速度ψ
p
和极限角加速度ωr，利用“角-轴”法求角度：
[0063]
由姿态矩阵
[0064]
得到角-轴：
[0065]
若r＝i，则θ＝0，轴r不确定；
[0066]
若tr(r)＝-1，则θ＝π
[0067]
一般情况下：
[0068]
轴：
[0069]
将初始姿态r0到终点姿态r1的行程，转化为绕姿态旋转轴r的起始姿态角度θ0、终点姿态角度θ1，两角度之差即为姿态的变化行程，原理如图4所示。再通过经典梯形轨迹规划方法可以求出旋转角度的轨迹参数：加速时间t1、匀速时间t2、减速时间t3、可达最大角速度wm、可达最大角加速度αm、速度轨迹类型p
θtj
，p
θtj
只能为2或者3，分别表示速度轨迹是2段式轨迹或3段式轨迹。对于空间姿态，上述的参数确定了以后，即决定了一个姿
态的梯形曲线轨迹。
[0070]
步骤4、计算位置和姿态的总运行时间。位置行程的总时间：t＝t1+t2+t3，姿态行程的总时间：t＝t1+t2+t3，取出t和t的较大值，记为t
max
。
[0071]
步骤5、对位置和姿态的梯形轨迹参数进行时间同步处理，输入为位置和姿态的短时梯形轨迹参数：加速时间t1，t1、匀速时间t2，t2、减速时间t3，t3、可达最大速度vm，wm、可达最大加速度am，αm和速度轨迹类型p
tj
，p
θtj
；输出为所有位置和姿态的长时梯形轨迹参数，即修正后的参数值：修正加速时间t1
′
，t1
′
、修正匀速时间t2
′
，t2
′
、修正减速时间t3
′
，t3
′
、修正可达最大速度vm
′
，wm
′
、修正可达最大加速度am
′
，αm
′
和速度轨迹类型p
tj
′
，p
θtj
′
。
[0072]
时间同步处理具体如下：
[0073]
对于输入的位置轨迹参数；
[0074]
如果是轨迹类型是2段式轨迹，那么，
[0075]
修正后的可达最大速度
[0076]
修正后的加速时间
[0077]
修正后的可达最大加速度
[0078]
修正后的匀速时间t2
′
＝0；
[0079]
修正后的减速时间t3
′
＝t1
′
；
[0080]
修正后的速度轨迹类型p
′
tj
＝p
tj
。
[0081]
如果轨迹类型是3段式轨迹，那么，
[0082]
修正后的可达最大速度
[0083]
修正后的加速时间
[0084]
修正后的匀速时间
[0085]
修正后的减速时间t3
′
＝t1
′
；
[0086]
修正后的可达最大加速度am
′
＝am；
[0087]
修正后的速度轨迹类型p
′
tj
＝p
tj
。
[0088]
对于输入的姿态轨迹参数，
[0089]
如果轨迹类型是2段式轨迹，那么，
[0090]
修正后的可达最大角速度
[0091]
修正后的加速时间
[0092]
修正后的可达最大角加速度
[0093]
修正后的匀速时间t2
′
＝0；
[0094]
修正后的减速时间t3
′
＝t1
′
；
[0095]
修正后的速度轨迹类型p
θtj
′
＝p
θtj
。
[0096]
如果轨迹类型是3段式轨迹，那么
[0097]
修正后的可达最大角速度
[0098]
修正后的加速时间
[0099]
修正后的匀速时间
[0100]
修正后的减速时间t3
′
＝t1
′
；
[0101]
修正后的可达最大角加速度αm
′
＝αm；
[0102]
修正后的速度轨迹类型p
θtj
′
＝p
θtj
°
[0103]
上述同步处理过程中，对于调整的位置轨迹参数或者姿态轨迹而言，相当于使用了时间延长算法进行了处理，其原理如图6所示。
[0104]
步骤6、对位置和姿态进行插补，并将修成后的位置和姿态轨迹参数下发到逆解模块中，即完成了位置和姿态的时间同步。
[0105]
本发明的方法可用于任意的多关节机械臂中，主要针对机械臂作单一的直线路径运动的情况，本发明首先使用梯形轨迹规划方法处理得到位置轨迹参数，使用“角-轴”法和梯形轨迹规划方法结合得到姿态轨迹参数，然后基于位置轨迹参数和位置轨迹参数确定最大运行时间，最后依据该最大运行时间调整位置轨迹参数和姿态轨迹参数，实现位置和姿态的同步处理。
[0106]
以上所述，仅是本发明实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。