《robotic》由会员分享,可在线阅读,更多相关《robotic(16页珍藏版)》请在金锄头文库上搜索。
1、General/Rotations 角度弧度互换 deg2rad() rad2deg()%绕 x 轴旋转 pi/2得到的旋转矩阵 (1)r = rotx(pi/2);%matlab 默认的角度单位为弧度,这里可以用度数作为单位(2)R = rotx(30, deg) * roty(50, deg) * rotz(10, deg);%求出 R 等效的任意旋转变换的旋转轴矢量 vec 和转角 theta(3)theta,vec = tr2angvec(R);%旋转矩阵用欧拉角表示,R = rotz(a)*roty(b)*rotz(c)(4)eul = tr2eul(R);%旋转矩阵用 roll-p
2、itch-yaw 角表示, R = rotx(r)*roty(p)*rotz(y)(5)rpy = tr2rpy(R);%旋转矩阵用四元数表示(6)q = Quaternion(R);%将四元数转化为旋转矩阵(7)q.R; %界面,可以是“rpy” , “eluer”角度单位为度。(8)tripleangle(rpy);General:transfirmations 位置变化 转换矩阵Trot ()Transl( x y z)类型 3C 代表 cos s 代表 sinD-H 位置方程 R xyz0 0 0 1R,)(,)(,)001010100RyRxcscssc csscsccscss cs
3、cs (General/Trajectory 设置轨迹 clear;clc;p0 = -1;% 定义初始点及终点位置p1 = 2;p = tpoly(p0, p1, 50);% 取步长为50 测试各点的参数figure(1);plot(p);%绘图,可以看到在初始点及终点的一、二阶导均为零p,pd,pdd = tpoly(p0, p1, 50);%得到位置、速度、加速度%p 为五阶多项式,速度、加速度均在一定范围内figure(2);subplot(3,1,1); plot(p); xlabel(Time); ylabel(p);subplot(3,1,2); plot(pd); xlabel
4、(Time); ylabel(pd);subplot(3,1,3); plot(pdd); xlabel(Time); ylabel(pdd);%另外一种方法:p,pd,pdd = lspb(p0, p1, 50);figure(3);subplot(3,1,1); plot(p); xlabel(Time); ylabel(p);subplot(3,1,2); plot(pd); xlabel(Time); ylabel(pd);% 可以看到速度是呈梯形subplot(3,1,3); plot(pdd); xlabel(Time); ylabel(pdd);%三维的情况:p = mtraj(
5、tpoly, 0 1 2, 2 1 0, 50);%点对点变换空间轨迹figure(4);polt(p)%对于齐次变换矩阵的情况T0 = transl(0.4, 0.2, 0) * trotx(pi);% 定义初始点和目标点的位姿T1 = transl(-0.4, -0.2, 0.3) * troty(pi/2) * trotz(-pi/2);T = ctraj(T0, T1, 50); %矩阵旋转变换空间轨迹first=T(:,:,1);%初始位姿矩阵tenth=T(:,:,10);%第十个位姿矩阵figure(5);tranimate(T);%动画演示坐标系自初始点运动到目标点的过程Ctr
6、aj矩阵4*4 笛卡尔规划 TC = ctraj(T0,T1,N)是一个笛卡尔轨迹(4x4xn)从姿态 T0 T1与 N 个点沿着路径梯形速度剖面。笛卡尔的轨迹是一个齐次变换序列和最后一个下标被点指数,即,T(:,:,i)是沿路径的 i 点。TC = ctraj(T0,T1,S)但上面的 S 元素(资料片)指定沿路径的分数距离,和这些值的范围为1 0 。第 i 点对应的距离 S(I)沿路径。T0 = transl(0.4, 0.2, 0) * trotx(pi);% 定义初始点和目标点的位姿T1 = transl(-0.4, -0.2, 0.3) * troty(pi/2) * trotz(-
7、pi/2);T = ctraj(T0, T1, 50); %矩阵旋转变换空间轨迹first=T(:,:,1);%初始位姿矩阵tenth=T(:,:,10);%第十个位姿矩阵figure(5);tranimate(T);%动画演示一个坐标系自初始点运动到目标点的过程Jtraj点位置1*n矩阵 关节空间规划 q 代表空间姿态Q,qd,qdd = jtraj( Q0,QF ,M)是一种关节空间轨迹 Q(MXN)在关节坐标从Q0(1N)对 QF(1N) 。一个五阶(第五阶)多项式使用默认的零边界条件的速度和加速度。时间是变化从0 M 步骤1。 关节速度和加速度可随意返回 QD(MXN)和需求(MXN)
8、分别。轨迹 Q,qd 和需求是 mxn 矩阵,每个时间步一行,每一列的联合。(M 直接打入一个数)Q,qd,qdd = jtraj(Q0 , QF,M,QD0,QDF)但上面还指定的初始和最终的关节轨迹速度。定义时间 t t=0初始时间:0.05间隔时间:2 结束时间Q,qd,qdd = jtraj(Q0,QF,T)但上面的轨迹长度的时间 T 载体中定义的长度(1) 。Q,qd,qdd = jtraj(Q0,QF,T,QD0,QDF)但上面指定的初始和最终的轨迹和时间向量的关节速度mdl_puma560t=0:0.05:2; q = jtraj(qz, qr, t);p560.plot(q);
9、mdl_puma560q = jtraj(qz, qr, 50);p560.plot(q);lspb直线与抛物线混合段s,sd,sdd= lspb(s0,st,m)是一个标量轨迹(Mx1)不同顺利从盲到科幻小说在 m 的步骤使用匀速段和抛物型混合(梯形路径 )。速度和加速度可以选择返回 sd(Mx1)和 sdd(Mx1)。s,sd,sdd= lspb(s0,st,m, v)如上所述,但指定的线性段的速度通常是自动计算的。s,sd,sdd= lspb(s0,st,T)如上但指定轨迹的长度时间向量 T(Mx1)。s,sd,sdd= lspb(s0,st,T,v)如上所述,但指定的线性段的速度通常是
10、自动计算向量。s1=0,s2=1;s,sd,sdd=lspb(s1,s2,50);Plot(s)Tpoly标量多项式轨迹生成s,sd,sdd = tpoly(S0,SF,M)是一个标量轨迹(MX1)是来自不同需求平稳在 M 步骤S0到 SF(5阶)采用五次多项式。速度和加速度可以选择返回 SD(MX1)和SDD(MX1) 。s,sd,sdd= tpoly(S0,SF,t)在指定的轨迹,但在条款的时间长度 T(MX1)向量。p0 = -1;p1 = 2;p = tpoly(p0, p1, 50);% 取步长为 50figure(1);plot(p);MtrajQ 是一个坐标多轴轨迹的两个点之间的
11、q,qd,qdd=mtraj(tfunc,Q0 , QF,M)是一个多轴轨迹从 Q0的变化状态(MN ) (N)(N Q)根据标量函数 tfunc 轨迹在 M 步骤。关节的速度和加速度可以选择返回1(n)和(n)的分别。在输出的每一行有一个轨道和一个时间步,每个轴柱。的轨迹的形状由标量函数,给出了 tfunc 轨迹s,sd,sdd= tfunc(S0,SF,m) ;与可能的值是“lspb tfunc 包括梯形轨迹” ,或是 tpoly 多项式轨迹。q,qd,qdd=mtraj(tfunc,Q0,QF,T)上的轨迹,但在条款的长度指定的时间长度T(MX1 )向量。q,qd,qdd=mtraj(t
12、poly,0,1,2,3,6,8,100);plot(q)Tfunc 可以是 tpoly 或 lspb 以一个标量为单位变化的函数MStraj连接多轴轨迹traj = mstraj(p,qdmax q0,dt,tacc选项) 是一种多轨迹(KxN) 基于通过点p( 以麦根)和轴速度限制qdmax(1 xn)。由多项式混合线性段的道路。输出轨迹矩阵每个时间步一行,每轴和一个列。P(以麦根)是一个通过点的矩阵,每通过一行,一列每轴。最后通过点目标。QDMAX(1 xn)轴不能超过速度限制,或QDMAX(Mx1)的持续时间为每个片段Q0(1 xn)初始轴坐标DT是时间步TACC这个加速时间(1 x1
13、)是应用于所有段过渡TACC xm(1)为每个段加速时间,TACC(我)的过渡段的加速度时间我来段+ 1。TACC(1) 也是加速时间在1段的开始。traj = mstraj(segments,qdmax q0,dt,tacc qd0,qdf,选项 )如上所述,但另外指定了初始的和最终的轴速度(1 xn)。p=1 1 1 12 3 3 2 4 6 4 67 7 7 7;traj=mstraj(p,3,3,3,3,p(1,:),0.01,0);plot(traj)xf = 5; xb = -xf; % forward and backward limits for foot on groundy
14、 = 5; % distance of foot from body along y-axiszu = 2; zd = 5; % height of foot when up and down% define the rectangular path taken by the footsegments = xf y zd; xb y zd; xb y zu; xf y zu;xf y zd * 0.01;tseg = 0 3 0.25 0.5 0.25;x = mstraj(segments, ,tseg, segments(1,:), 0.01, 0);Trinterp插入均匀转换T = t
15、rinterp(T0,T1,s)是一个齐次变换插值在 T0 = 0 时 T1s= 1。旋转是使用四元数插值球形线性插值。如果年代(n*1),那么 T(4 x4xn)是一个对应的齐次变换序列插值值。T = trinterp(T,s)是一个不同身份的转换矩阵时年代= 0 T R = 1。如果 s(n*1),那么 T(4 x4xn)是一个对应的齐次变换序列插值值t0=transl(0.4,0.2,0)*trotx(pi);t1=transl(1,2,3)*troty(pi/2);tr=trinterp(t0,t1,0.4)Tr表示一个4*4d-h 矩阵Link L(1)=Link(theta,d,a,alpha,0) 最后的 0 代表转动关节左右两种写法一样l1=Link(0,0,0,pi/2,0);% l(1) = Link(d, 0, a, 0, alpha,pi/2);l2=Link(0,0,1,0,0);% l(2) = Link(d, 0, a, 1, alpha,0);bot=SerialLink(l1,l2);% b