)机器人轨迹规划

1、实验（5）机器人轨迹规划实验一、实验目的：1） 理解机器人轨迹规划的相关概念；2） 对构建的机器人进行速度分析；3） 能够使用simulink构建机器人仿真模型。二、路径描述和路径生成轨迹描述了操作臂在多维空间中的期望运动。轨迹指每个自由度的位置、速度和加速度的时间历程。由用户通过简单的描述来指定机器人的期望运动，然后由系统来完成详细的计算，确定到达目标的准确路径、时间历程、速度和加速度曲线等。轨迹生成问题：1）通过表达式计算轨迹的问题；2）在轨迹生成的运行时间内要计算位置、速度和加速度；3）计算轨迹点的速率路径更新率（60Hz-2000Hz）。机器人操作臂的运动看作是工具坐标系T相对于工作台

2、坐标系S的运动。工具坐标系从当前值Tinitial移动到最终端期望值Tfinal，其运动包括工具相对于工作台的姿态和位置变化。路径点一般由：初始点、最终点和中间点。图1 机器人操作臂的运动三、轨迹规划方法研究以关节角的函数来描述轨迹（在时间和空间）的轨迹生成方法。每个路径点由工具坐标系T相对于工作台坐标系S的期望位姿来确定；应用逆运动学，将中间点“转换”成一组期望的关节角；得到经过各中间点并终止于目标点的n个关节的光滑函数。对于每个关节，由于各路径段所需的时间是相同的，因此所有的关节将同时到达各中间点，从而得到T在每个中间点上的期望的笛卡尔位姿。（1）一维情况下的轨迹用多项式表示：轨迹所对应的

3、速度和加速度：当t=（0-T）时，可以得到如下的矩阵：在matlab robot toolbox中对应的5次多项式轨迹生成的函数：tpoly函数原型为S,SD,SDD = tpoly(S0, SF, M)其中，S0为初始位置，SF为终止位置，M为步数。S为轨迹，SD为速度，SDD为加速度例1：s,sd,sdd=tpoly(0,1,100);subplot(3,1,1)plot(s)ylabel('s');subplot(3,1,2)plot(sd)ylabel('sd');subplot(3,1,3)plot(sdd)ylabel('sdd');

4、例2：初始化速度为0.5，末端速度为0.s,sd,sdd= tpoly(0, 1, 100, 0.5, 0);subplot(3,1,1)plot(s)ylabel('s');subplot(3,1,2)plot(sd)ylabel('sd');subplot(3,1,3)plot(sdd)ylabel('sdd');LSPB（parabolic blend 抛物线拟合）书本p168LSPB函数原型：S,SD,SDD = lspb(S0, SF, M)（1）S,SD,SDD = lspb(S0, SF, M, V)（2）其中，S0为初始位置，SF

5、为终止位置，M为步数，V为线性段的速度值（不可随便取值，可以先执行函数（1）后获取最大和最小的线性速度，max（sd）和min（sd）；S为轨迹，SD为速度，SDD为加速度例3.s,sd,sdd = lspb(0, 1, 100)subplot(3,1,1)plot(s)ylabel('s');subplot(3,1,2)plot(sd)ylabel('sd');subplot(3,1,3)plot(sdd)ylabel('sdd');例4，不同线性速度情况s,sd,sdd = lspb(0, 1, 100,0.015)subplot(3,1,1

6、)plot(s,'r ')ylabel('s');hold on;subplot(3,1,2)plot(sd,'r ')ylabel('sd');hold on;subplot(3,1,3)plot(sdd,'r ')ylabel('sdd');hold on;s,sd,sdd = lspb(0, 1, 100,0.012)subplot(3,1,1)plot(s,'b ')ylabel('s');subplot(3,1,2)plot(sd,'b ')

7、ylabel('sd');subplot(3,1,3)plot(sdd,'b ')ylabel('sdd');（2）多维情况下的轨迹规划函数mtrajmtraj函数原型：Q,QD,QDD = mtraj(TFUNC, Q0, QF, M)其中，TFUNC为使用一维轨迹规划函数（tploy、lspb等），Q0和QF为起始和结束时的多维位置值。使用方法如下：Q,QD,QDD = mtraj(tpoly, 0 2, 1 -1, 50);subplot(3,1,1)plot(Q)ylabel('s');subplot(3,1,2)plot

8、(QD)ylabel('sd');subplot(3,1,3)plot(QDD)ylabel('sdd');（3）多段轨迹规划函数：mstraj函数原型：TRAJ = mstraj(P, QDMAX, Q0, DT, TACC, OPTIONS)- P (MxN) 中间点矩阵，包括起始点和终点；- QDMAX (1xN) 最大速度，不能超过该速度；- QDMAX (Mx1) 每一个线段的最大速度；- Q0 (1xN) 初始位置；- DT 时间步；- TACC (1x1) 应用到所有线段的加速时间；- TACC (1xM) 每一段的加速时间。例子： via = 4

9、,1; 4,4; 5,2; 2,5 ;q = mstraj(via, 2,1, , 4,1, 0.05, 0);plot(q,'*')via = 4,1; 4,4; 5,2; 2,5 ;q = mstraj(via, 2,1, , 4,1, 0.05, 1);plot(q,'*')（4）姿态插补例子：R0 = rotz(-1) * roty(-1);R1 = rotz(1) * roty(1);rpy0 = tr2rpy(R0); rpy1 = tr2rpy(R1);rpy = mtraj(tpoly, rpy0, rpy1, 50);tranimate( rp

10、y2tr(rpy) );备注：interp （线性插补函数）（5）笛卡尔坐标运动%笛卡尔空间运动采用直线均分情况T0 = transl(0.4, 0.2, 0) * trotx(pi);T1 = transl(-0.4, -0.2, 0.3) * troty(pi/2)*trotz(-pi/2);Ts=trinterp(T0, T1, 0:49/49);figure(1)tranimate(Ts);figure(2)P = transl(Ts);subplot(2,1,1)plot(P)rpy = tr2rpy(Ts);subplot(2,1,2)plot(rpy);%笛卡尔空间运动s曲线情况

11、T0 = transl(0.4, 0.2, 0) * trotx(pi);T1 = transl(-0.4, -0.2, 0.3) * troty(pi/2)*trotz(-pi/2);Ts=trinterp(T0, T1,lspb(0,1,50);figure(1)tranimate(Ts);figure(2)P = transl(Ts);subplot(2,1,1)plot(P)rpy = tr2rpy(Ts);subplot(2,1,2)plot(rpy);上述代码中： Ts=trinterp(T0, T1,lspb(0,1,50);直接采用如下机器人库中的函数实现：Ts = ctraj

12、(T0, T1, 50);ctraj函数为笛卡尔空间规划函数函数原型为：四、机器人关节空间规划%机器人关节空间规划mdl_puma560;T1 = transl(0.4, 0.2, 0) * trotx(pi);T2 = transl(0.4, -0.2, 0) * trotx(pi/2);q1 = p560.ikine6s(T1);q2 = p560.ikine6s(T2);t = 0:0.05:2'q = mtraj(lspb, q1, q2, t); %也可以用q = mtraj(tploy, q1, q2, t);%或者直接调用机器人关节空间规划函数：q = jtraj(q1, q2, t);%或者直接使用机器人库中 q = p560.jtraj(T1, T2, t)p560.plot(q)qplot(t, q);五、机器人笛卡尔空间轨迹规划%机器人笛卡尔空间规划mdl_puma560;T1 = transl(0.4, 0.2, 0) * trotx(pi);T2 = transl(0.4, -0.2, 0) * t