函数库

import %装载
ETS2.* %二维空间
ETS3.* %三维空间
Rz() %绕z轴旋转
Tx() %沿着x轴平移
.fkine() %正运动学
.teach %示教
.structure %关节的数量和类型
Revolute() %创建转动连杆对象
.A() %计算参数为()时的变换
.type %连杆类别
.offset %设置初始偏移值
models %工具箱自带模型
mdl_ %装载
.edit %编辑模型参数
.n %关节数目
.tool %工具
.base %基座
syms ··· real %定义一些实值符号变量
ikine6s()%求逆运动学封闭解
ikine() %求逆运动学数值解
ikunc() %求逆运动学解
mask 遮盖向量
mtraj() %两点间多轴轨迹
jtraj() %计算关节空间轨迹（具有tpoly插值的mtraj）
ctraj() %两个姿态之间的笛卡尔轨迹
mstraj() %多轴多段轨迹
sim_ %运行simlink
qplot() %绘制关节坐标图
maniplty() %路径的可操作性
DHFactor() %转换为DH因子
dh.command()%生成一个机器人
eval() %计算matlab表达式
RevoluteMDH() %创建连杆对象(改进DH参数)

学习记录

正运动学

"关节j带动连杆j"

• fkine 正运动学

二维机械臂

>> import ETS2.* %载入SE2
>> a1 = 1;
>> E = Rz('q1') * Tx(a1) %绕z轴旋转q1然后沿着x轴平移a1
>> E.fkine( 30, 'deg') %q1= 30°时的正运动学
>> E.teach %示教
>> a1 = 1; a2 = 1;
>> E = Rz('q1') * Tx(a1) * Rz('q2') * Tx(a2) %两轴RR机械臂
>> E.fkine( [30, 40], 'deg')
>> E.plot( [30, 40], 'deg')
>> E.structure %关节的数量和类型
ans ='RR'
>> clear import %卸掉SE(2)，装载SE(3)
>> import ETS3.*
>> a1 = 1;
>> E = Rz('q1') * Tx(a1) * Tz('q2') %RP机械臂
>> E.structure

三维机械臂

>> import ETS3.*
>> L1 = 0; L2 = -0.2337; L3 = 0.4318; L4 = 0.0203; L5 = 0.0837; L6 = 0.4318;
>> E3 = Tz(L1) * Rz('q1') * Ry('q2') * Ty(L2) * Tz(L3) * Ry('q3')* Tx(L4) * Ty(L5) * Tz(L6) * Rz('q4') * Ry('q5') * Rz('q6');
>> E3.fkine([0 0 0 0 0 0])

D-H参数法

>> L = Revolute('a', 1) %创建一个a为1的转动关节 Revolute是Link下的子类
L =
Revolute(std): theta=q, d=0, a=1, alpha=0, offset=0
>> L.A(0.5) %q=0.5时的变换
>> L.type %连杆类别
ans ='R'
>> L.a %连杆参数 a=1
ans =1
>> L.offset = 0.5; %设置初始偏移值
>> L.A(0)
%用SerialLink类将Link类对象串联起来
>> robot = SerialLink( [ Revolute('a', 1) Revolute('a', 1) ],'name', 'my robot')
>> robot.fkine([30 40], 'deg') %计算正向运动学
>> models %工具箱自带模型
>> mdl_irb140 %装载
>> robot.edit %编辑模型参数
>> robot.n %关节数目

指数

>> a1 = 1; a2 = 1; %定义连杆长度
>> TE0 = SE2(a1+a2, 0, 0); %计算TE0
>> S1 = Twist( 'R', [0 0] ); %定义两个Twist
>> S2 = Twist( 'R', [a1 0] );
>> TE = S1.T(30, 'deg') * S2.T(40, 'deg') * TE0 %将它们应用于TE0
错误使用  *  (第 395 行)
operands to * cannot be composed   ???????????????

坐标变换微分方程为

，解可表示为

，那么，对于2连杆机械臂可表示为

，对于多连杆

六轴工业机器人

关节坐标向量，典型机器人位型

• qz (0,0, 0, 0,0, 0) 零角度

• qr (0,pi/2,-pi/2,0,0, 0) 就绪状态，机械臂伸直且垂直

• qs (0,0, -pi/2,0,0, 0) 伸展状态，机械臂伸直且水平

• qn (0,pi/4,-pi, 0,pi/4,0) 标准姿态，机械臂处于一个灵巧工作姿态

>> mdl_puma560 %加载工具箱自带的puma560
>> p560
>> p560.plot(qz) %绘制位形图
>> TE = p560.fkine(qz) %计算正运动学
>> p560.tool = SE3(0, 0, 0.2); %定义一个工具变换
>> p560.fkine(qz)
>> p560.base = SE3(0, 0, 30*0.0254); %基座变换
>> p560.fkine(qz)
>> p560.base = SE3(0,0,3) * SE3.Rx(pi);
>> p560.fkine(qz)

逆运动学

• ikine6s：逆运动学封闭解

• ikine：数值解

2D封闭解

>> import ETS2.*
>> a1 = 1; a2 = 1;
>> E = Rz('q1') * Tx(a1) * Rz('q2') * Tx(a2)
>> syms q1 q2 real %定义一些实值符号变量来表示关节角度
>> TE = E.fkine( [q1, q2] ) %求正运动学
>> syms x y real %定义另外两个符号变量来表示期望的末端执行器位置(x, y)
>> e1 = x == TE.t(1) %将它们与正运动学的结果等同起来
>> e2 = y == TE.t(2)
>> [s1,s2] = solve( [e1 e2], [q1 q2] ) %它给出了两个可以同时解的标量方程
>> length(s2) %q2有两个解
ans =2
>> s2(1)
ans =
-2*atan((-(x^2 + y^2)*(x^2 + y^2 - 4))^(1/2)/(x^2 + y^2))

3D封闭解

• 'l','r' 左手或右手

• 'u','d' 肘部在上或在下

• 'f','n' 手腕翻转或不翻转

>> mdl_puma560
>> qn
qn =0    0.7854    3.1416         0    0.7854         0
>> T = p560.fkine(qn)
>> qi = p560.ikine6s(T) %计算逆运动学封闭解
qi =2.6486   -3.9270    0.0940    2.5326    0.9743    0.3734
>> p560.fkine(qi)
>> qi = p560.ikine6s(T, 'ru') %计算逆运动学右手位形解
qi =-0.0000    0.7854    3.1416   -0.0000    0.7854    0.0000
>> p560.ikine6s( SE3(3, 0, 0) ) %存在无法到达的点
警告: point not reachable
>> q = [0 pi/4 pi 0.1 0 0.2];
>> p560.ikine6s(p560.fkine(q), 'ru') %奇异位型
ans =-0.0000    0.7854    3.1416   -3.0409    0.0000   -2.9423
>> q(4)+q(6) %q4与q6的值与原来完全不同，但和始终等于0.3
ans =0.3000

3D数值解

>> T = p560.fkine(qn)
>> qi = p560.ikine(T) %求数值解
qi =-0.0000   -0.8335    0.0940    0.0000   -0.8312   -0.0000
>> p560.fkine(qi) %qi与qn不同，但计算出的位姿相同
>> p560.plot(qi) %ikine获得了手肘向下的解
>> qi = p560.ikine(T, 'q0', [0 0 3 0 0 0]) %如果我们指定初始关节坐标，则可找到手肘向上的杰
qi = -0.0000    0.7854   -3.1416   -0.0000    0.7854    0.0000

ikine的一般数值方法比ikine6s的解析方法慢得多。但是它有一个很大的优势就是能够处理奇点处的机械手以及小于或大于6个关节的机械手。

欠驱动机械臂

>> mdl_cobra600
>> c600
>> T = SE3(0.4, -0.3, 0.2) * SE3.rpy(30, 40, 160, 'deg') %定义一个理想的末端执行器位姿
>> q = c600.ikine(T, 'mask', [1 1 1 0 0 1]) %遮盖向量（关于x轴和y轴的旋转将被忽略）求数值解
>> Ta = c600.fkine(q);
>> Ta.print('xyz')
t = (0.4, -0.3, 0.2), RPY/xyz = (92.7, 0, 180) deg
%它有理想的平移和偏航角，但俯仰和横摇角是不正确的，正如我们所允许的那样。
>> trplot(T, 'color', 'b')
>> hold on
>> trplot(Ta, 'color', 'r')

冗余机械臂

>> mdl_baxter
>> left %7轴
>> TE = SE3(0.8, 0.2, -0.2) * SE3.Ry(pi);
>> q = left.ikine(TE)
>> left.fkine(q).print('xyz')
t = (0.8, 0.2, -0.2), RPY/xyz = (180, -4.2e-10, 180) deg
>> left.plot(q)

轨迹

• tpoly：生成标量多项式轨迹

• lspb：线性段与抛物线混合

• mtraj：两点间多轴轨迹

• jtraj：计算关节空间轨迹（具有tpoly插值的mtraj）

• ctraj：两个姿态之间的笛卡尔轨迹

• mstraj：多段多轴轨迹

关节空间运动

>> startup_rvc
>> mdl_puma560
>> T1 = SE3(0.4, 0.2, 0) * SE3.Rx(pi);
>> T2 = SE3(0.4, -0.2, 0) * SE3.Rx(pi/2);
>> q1 = p560.ikine6s(T1);
>> q2 = p560.ikine6s(T2);
>> t = [0:0.05:2]';
>> q = jtraj(q1, q2, t);
>> [q,qd,qdd] = jtraj(q1,q2,t);
>> q = p560.jtraj(T1, T2 ,t)；
>> p560.plot(q)
>> plot(t, q(:,2))
>> qplot(t, q); %绘制所有随时间变化的关节角
>> T = p560.fkine(q);
>> p = transl(T);//>> p = T.transl;
>> about(p)
>> plot(p(:,1),p(:,2)) %绘制末端执行器在xy平面的轨迹
>> plot(t,tr2rpy(T)) // >> plot(t, T.torpy('xyz')) %绘制末端执行器的姿态随时间变化的图形（横滚-俯仰-偏航角形式）
>> sl_jspace
>> sim('sl_jspace',10);

笛卡尔运动

>> Ts = ctraj(T1,T2,length(t)); %笛卡尔轨迹
>> plot(t, Ts.transl); %提取并绘制位置分量
>> plot(t,Ts.torpy('xyz')); %提取并绘制姿态分量

通过奇异位型的运动

>> T1 = SE3(0.5,  0.3, 0.44) * SE3.Ry(pi/2);
>> T2 = SE3(0.5, -0.3, 0.44) * SE3.Ry(pi/2);
>> Ts = ctraj(T1, T2, length(t)); %得到笛卡尔路径
>> qc =  p560.ikine6s(Ts); %将其转换到关节坐标(封闭解)
>> qplot(t, qc) %绘制关节坐标图a
>> qik2 =  p560.ikunc(Ts);
>> qplot(t,qik2) %绘制关节坐标图b
>> q = p560.jtraj(T1,T2,t); %求关节空间运动的轨迹
>> qplot(t, q)%绘制关节空间运动计算的关节坐标图c
>> m = p560.maniplty(qc); %路径的可操作性
%画出各情况路径的可操作性，如图d
>> mc = p560.maniplty(qc);
>> mk = p560.maniplty(qik2);
>> m = p560.maniplty(q);
>> plot(mc);
>> hold on
>> plot(mk)
>> hold on
>> plot(m)

ikine6s：在t≈0.7s时，q4,q6变化率高，奇异点（失去一个自由度，运动学上仅可以求出q4+q6的值）

ikunc()可以较好地处理奇异性问题

位形转换

>> T = SE3(0.4, 0.2, 0) * SE3.Rx(pi);
>> qr = p560.ikine6s(T, 'ru');
>> ql = p560.ikine6s(T, 'lu');
>> q = jtraj(qr, ql, t); %在这两个关节坐标向量之间创建一个关节空间轨迹
>> p560.plot(q)
>> qplot(q)

机器人仍然执行了一种十分显著的关节空间运动

高级问题

关节角偏移

>> L = Link([0 0 1 0]);
>> L.offset = pi/4
%或
>> p560.links(2).offset = pi/2;

确定D-H参数

>> s = 'Tz(L1) Rz(q1) Ry(q2) Ty(L2) Tz(L3) Ry(q3) Tx(L4) Ty(L5) Tz(L6) Rz(q4) Ry(q5) Rz(q6)' %写入一个字符串>> dh = DHFactor(s); %将字符串输入一个符号代数函数，返回一个DHFactor对象
>> dh %显示一种机器人运动学结构的可读形式，各关节参数顺序分别为θ,d,a,α
>> cmd = dh.command('puma') %创建一个名为“puma”的机器人
>> L1 = 8; L2 = 3; L3 = 8; L4 = 1; L5 = 8; L6 = 1; L7 = 2;
>> robot = eval(cmd); %创建一个名为robot的工作空间变量（SerialLink对象）

改进DH参数

>> L1 = RevoluteMDH('d', 1)
%或
>> L1 = Link([0 1 0 0 0], 'modified')
%与标准形式区分
>> L1 = Revolute('d', 1)
%或
>> L1 = Link([0 1 0 0 0])

应用：绘图

startup_rvc                                                                 %导入工具箱
mdl_puma560                                                                 %导入机器人模型puma 560
load hershey                                                                %导入字符库
B = hershey{'B'}                                                            %定义B，B为一个结构体，描述了字符的尺寸，垂直从0到0.84，水平从0到0.84（由返回值知）
B.stroke                                                                    %B的路径 且为2行23列的矩阵（由返回值知）
path = [ 0.5*B.stroke; zeros(1,numcols(B.stroke))];                         %缩放路径到原来的0.5倍，添加一零行
k = find(isnan(path(1,:)));                                                 %找到含NaNs的纵列
path(:,k) = path(:,k-1); path(3,k) = 0.2;                                   %将含NaNs的纵列替换为前一列，将z坐标设置为0.2以使笔脱离表面
traj = mstraj(path(:,2:end)', [0.5 0.5 0.5], [], path(:,1)', 0.02, 0.2);    %多轴多段轨迹traj,最大速度0.5，初始坐标为第一列，采样间隔0.02，加速时间0.2
about(traj)
numrows(traj) * 0.02                                                        %计算轨迹所需时间
plot3(traj(:,1), traj(:,2), traj(:,3))                                      %绘制轨迹
Tp = SE3(0.4, 0, 0) * SE3(traj) * SE3.oa( [0 1 0], [0 0 -1]);               %接近向量a向下，夹持器o在y方向上，字符放置在工作区中的(0.4,0,0)处
p = Tp.transl;
plot3(p(:,1), p(:,2), p(:,3))                                               %绘制实际轨迹
q = p560.ikine6s(Tp);
p560.plot(q)

应用：一个简单的步行机器人

startup_rvc
%运动学
%旋转方式所建DH参数与书中例子会略有不同，所以直接复制了书中的SerialLink对象
L1 = 0.1; L2 = 0.1;
leg = SerialLink([0, 0, 0, pi/2, 0; 0, 0, L1, 0, 0; 0, 0, -L2, 0, 0; ],'name', 'leg', 'base', eye(4,4),'tool', trotz(pi/2)*trotx(-pi/2)*trotz(-pi/2),'offset', [pi/2 0 -pi/2 ])
transl( leg.fkine([0,0,0]) )                                                %测试
leg.plot([0,0,0], 'nobase', 'noshadow', 'notiles')                          %绘制一条腿
set(gca, 'Zdir', 'reverse'); view(137,48);                                  %Z轴倒置，设置视角
%以下三条均为测试
transl( leg.fkine([0.2,0,0]) )
transl( leg.fkine([0,0.2,0]) )
transl( leg.fkine([0,0,0.2]) )
%单腿运动
xf = 50; xb = -xf; y = 50; zu = 20; zd = 50;                                %xf,xb:x方向上腿部前进和后退运动的极限 y:y方向上脚到机体的距离 zu,zd:z方向上脚抬起和放下时脚的高度
path = [xf y zd; xb y zd; xb y zu; xf y zu; xf y zd] * 1e-3;                %路径
p = mstraj(path, [], [0, 3, 0.25, 0.5, 0.25]', path(1,:), 0.01, 0);         %多段路径轨迹：路径、各段运动时间、初始位置、采样间隔和加速时间
qcycle = leg.ikine( SE3(p), 'mask', [1 1 1 0 0 0] );                        %逆运动学，欠驱动使用ikine，遮盖向量只求位置不求姿态
%leg.plot(qcycle, 'loop')                                                    %动画，loop:轨迹显示一直循环不断，键入Control-C来停止它
leg.plot(qcycle)
W = 0.1; L = 0.2;
legs(1) = SerialLink(leg, 'name', 'leg1');
legs(2) = SerialLink(leg, 'name', 'leg2', 'base', SE3(-L, 0, 0));
legs(3) = SerialLink(leg, 'name', 'leg3', 'base', SE3(-L, -W, 0)*SE3.Rz(pi));
legs(4) = SerialLink(leg, 'name', 'leg4', 'base', SE3(0, -W, 0)*SE3.Rz(pi));
clf
k = 1;
%for k = 1:600
set(gca, 'Zdir', 'reverse'); view(137,48);                                  %Z轴倒置，设置视角
while 1legs(1).plot( gait(qcycle, k, 0, false), 'workspace', [-0.3 0.3 -0.3 0.3 -0.3 0.3],'nobase', 'noshadow', 'notiles');if k == 1, hold on; endlegs(2).plot( gait(qcycle, k, 100, false),'nobase', 'noshadow', 'notiles' );legs(3).plot( gait(qcycle, k, 200, true),'nobase', 'noshadow', 'notiles' );legs(4).plot( gait(qcycle, k, 300, true),'nobase', 'noshadow', 'notiles' );drawnowk = k+1;
end

总结

6DOF且手腕为球关节：可计算解析解ikine6s（结果不唯一）

其他机器人（包括欠驱动机器人，冗余机器人）：数值解ikine

关节空间路径jtraj：非直线路径

笛卡尔路径ctraj:奇异位形可能会导致高关节速度

plot方法

plot算法：

1. 如果具有指定名称的机器人存在于当前图形中，则以指定的关节角度重绘。这时在其他图形中同一名称的机器人也会更新。

1. 如果当前图形中没有这个名称的机器人存在，这时如果hold方法已启动，机器人将被添加到当前图形中，否则将在当前窗口中创建一个指定名称的新机器人。

1. 如果没有图形，则创建一个新的图形。

>> mdl_puma560
% 在不同基座位置创建一个标准Puma机器人的副本
>> p560_2 = SerialLink(p560, 'name', 'puma %2', 'base', transl(0.5, -0.5,0));
>> p560.plot(qr) %绘制第一个机器人
>> hold on %在原视图上叠加
>> p560_2.plot(qr) %绘制第二个机器人
>> p560.plot(qz) %让第一个机器人产生运动
>> figure %新建视图
>> p560.plot(qz) %添加第一个机器人的另外一个视图
>> p560.plot(qr) %两图里的机器人同时转动
>> p560.teach %示教