机器人控制系列教程之运动规划(2)

简介

在笛卡尔坐标空间中轨迹规划时，首先用位置矢量和旋转矩阵表示所有相应的机器人节点，其次在所有路径段插值计算相对的位置矢量和旋转矩阵，依次得出笛卡尔坐标空间中的轨迹序列通过求解运动学逆问题得到相应关节位置参数。

优点：

在笛卡尔坐标空间规划的轨迹比较直观，实际运动路径准确度高；
某些工作任务本身对在笛卡尔坐标空间中的轨迹有要求，必须首先在笛卡尔坐标空间规划。

缺点：

计算量远远大于关节空间法，控制时间间隔较长；
不能保证除预定的路径点外轨迹上其他的点都在机器人的运动空间，而关节空间轨迹规划可以；
轨迹路径有可能接近或通过操作空间的盲点。

规划方法

根据机器人的末端执行器的位置和姿态，该方法主要有以下几种：

MATLAB机器人工具箱`ctraj`函数解析

两个位姿之间的笛卡尔轨迹

tc = ctraj(T0, T1, n)表示从位姿T0到T1的笛卡尔轨迹，其中n个点遵循沿路径的梯形速度分布。笛卡尔轨迹是一个齐次变换序列，最后一个下标是点索引，即T(：，：，i)是路径上的第i个点。
tc = ctraj(T0, T1, s)如上所述，但S(Nx1)的元素指定了路径上的分数距离，这些值在[0 1]范围内。第i个点对应于S(I)沿路径的距离。

Note：
如果T0或T1等于[]，则认为它是单位矩阵。在第二种情况下，S可以由标量轨迹生成器生成，如TPOLY(五次多项式)或LSPB(组合轨迹，默认)。

笛卡尔空间轨迹求解

相关代码：

% 导入机器人模型
% 这里输入你的机器人模型
clear,clc,close all;
%% 建立机器人DH参数，初始状态为竖直状态
L1=Link('d',162.50,'a',0,'alpha',0,'modified');
L2=Link('d',0,'a',0,'alpha',pi/2,'offset',pi/2,'modified');
L3=Link('d',0,'a',425,'alpha',0,'modified');
L4=Link('d',126.70+6.6,'a',392.25,'alpha',0,'offset',pi/2,'modified');
L5=Link('d',99.70,'a',0,'alpha',pi/2,'modified');
L6=Link('d',99.60-4.5,'a',0,'alpha',-pi/2,'modified');

robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','UR5')

% M = [1 1 1 1 1 0];
qz = [0 0 0 0 0 0];
t = 0:0.01:2;
T = fkine(robot,qz);
%T(1,4:)  %T(2,4:)  %T(3,4:)
T10 = transl(4, -0.5, 0) * troty(pi/6); % 末端初始位姿
T11 = transl(4, -0.5, -2) * troty(pi/6); % 末端终止位姿
Ts = ctraj(T10, T11, length(t));

% 末端执行器从初始位姿到末端位姿坐标系的平移变化
figure(1); 
TP=transl(Ts);
x=plot(t, TP(:,1));hold on;
y=plot(t, TP(:,2));hold on;
z=plot(t, TP(:,3));
legend([x,y,z],'X轴','Y轴','Z轴')
ylim([-3 5])

%末端执行器从初始位姿到末端位姿坐标系旋转的变化
figure(2); 
Tr=tr2rpy(Ts);
x=plot(t, Tr(:,1),'-*');hold on;
y=plot(t, Tr(:,2),'LineStyle','--');hold on;
z=plot(t, Tr(:,3));
legend([x,y,z],'X轴','Y轴','Z轴')
ylim([-0.002 0.01])

% 末端执行器从初始位姿到末端位姿的空间轨迹的规划图形投影到xOy坐标轴内的变化
u = Ts(1,4,:);v = Ts(2,4,:);w = Ts(3,4,:);
x = squeeze(u);y = squeeze(v);z = squeeze(w);
figure(3); 
subplot(3,1,1);
plot(t,x) 
xlabel('Time')
ylabel('x')
subplot(3,1,2);
plot(t,y) 
xlabel('Time')
ylabel('y')
subplot(3,1,3);
plot(t,z) 
xlabel('Time')
ylabel('z')

末端执行器从初始位姿到末端位姿坐标系的平移变化。