RTB matlab工具箱（2）——工作空间可视化与轨迹规划

最新推荐文章于 2023-08-09 16:23:48 发布

hongliyu_lvliyu

最新推荐文章于 2023-08-09 16:23:48 发布

阅读量3.1k

点赞数

分类专栏：机器人工具文章标签：机器人 matlab RTB

本文链接：https://blog.csdn.net/hongliyu_lvliyu/article/details/123589867

版权

机器人同时被 2 个专栏收录

7 篇文章 7 订阅

订阅专栏

工具

6 篇文章 1 订阅

订阅专栏

笔记为B站教学视频（https://www.bilibili.com/video/BV1q44y1x7WC?p=5）的记录，非常感谢UP主的精彩分享！

一、工作空间可视化

整体思路：

在关节空间中随机生成变量；
通过正运动学函数fkine得到变换矩阵；
变换矩阵通过transl函数得到三维坐标。
在点足够多的情况下，不断标出的三维坐标拼接后则形成了动作空间。

生成随机变量：

rand 
//在（0，1）内均匀生成随机数字

若想在[m,n]内随机生成一个数字：

num = m+rand*(m-n)

生成随机的关节空间变量：

q=qmin + rand*(qmax-qmin);
// qmin,qmax为该关节的角度上下限

在程序中定义关节限制：
使用Link的属性Link.qlim
定义关节限制：

L(1) = Link('revolute','d',0.216,'a',0,'alpha',pi/2);
L(1).qlim=[-150,150]/180*pi;
//等价于：
L(1)=Link('revolute','d',0.216,'a',0,'alpha',pi/2,'qlim',[-150,150]/180*pi)

查看某关节的限制可以用：

L(1).qlim          //L(1)已经被定义为一个Link

二、轨迹规划

2.1 五次多项式

在考虑轨迹的时间、位置、速度、加速度的约束条件时，通过五次多项式，可以是路径平滑，且起点终点不突变。
$y_5 = Ax^{5}+Bx^{4}+Cx^{3}+Dx^{2}+Ex+F$

在工具箱内有五次多项式轨迹相关函数：tpoly
例如：

t = linspace(0,2,51); %等价于==0:0.04:2        %0-2s,进行51次插值
[P,dP,ddP]=tpoly(0,3,t);
![在这里插入图片描述](https://img-blog.csdnimg.cn/07fb485ee2b942ce9ea355ec5eae06c9.png?x-oss-process=image/watermark,type_d3F5LXplbmhlaQ,shadow_50,text_Q1NETiBAaG9uZ2xpeXVfbHZsaXl1,size_13,color_FFFFFF,t_70,g_se,x_16)

% 指定初末速度
[P,dP,ddP]=tpoly(0,3,51,0.02,0.01);
% 0-3s内进行51次插值

在这里插入图片描述

2.2 其它常用函数

混合曲线轨迹： lspb
例如:

t=linspace(0,2,51);   
[P,dP,ddP]=lspb(0,3,t);
%指定最大速度
[P,dP,ddP]=lspb(0,3,51,0,1);

在这里插入图片描述

多维轨迹：mtraj

t=linspace(0,2,51);
[P,dP,ddP]=mtraj(@tpoly,[0,0],[3,4],t);   %用@衔接插值方式
% 初始位置：[0,0] 末位：[3,4]

在这里插入图片描述

多维多段轨迹：mstraj
例如从[0,0] 2s到[3,4]，再京过2s到[1,2]

TRAJ=mstraj(WP,QDMAX,TSEG,Q0,DT,TACC,OPTIONS)
% WP: 经过路径点       QDMAX：最大速度     TSEG：每段的时间间隔
%（最大速度和时间间隔只能固定一个；如果确定了一个，另一个需要置为空）
% Q0：初始位置（第一个wp）   DT：插值间隔     TACC：加速时间

例如：

wp=[0,0;3,4;1,2];
P1=mstraj(wp,[],[2,1],[],0.04,0);
P2=mstraj(wp,[],[2,1],[],0.04,0.5);

在这里插入图片描述

2.3 位置轨迹规划

整体思路：
在这里插入图片描述
demo:

P1=[0.7,-0.5,0];
P2=[0.7,0.5,0.5];
t=linspace(0,2,51);
Traj = mtraj(@tpoly,P1,P2,t);
n = size(Traj,1);
T = zeros(4,4,n);
for i=1:n
    T(:,:,i) = transl(Traj(i,:))*trotx(180);
end

Qtraj  = Five_dof.ikunc(T);
Five_dof.plot(Qtraj);                   %只运动，不显示轨迹
% Five_dof.plot(Qtraj,'trail','b');           %录制轨迹
% Five_dof.plot(Qtraj,'trail','b','movie','trail.gif');       %保存录像

位姿插值函数trinterp (线性插值)

trinterp(T0,T1,M)      %T0:初始状态的变换矩阵     T1：末端的变换矩阵    M：插值次数（正整数）

例如：

T=trinterp(T1,T2,51);                %得到的t-position结果是直线，因为使用的是线性插值
T=trinterp((T1,T2,tpoly(0,2,50)/2);       %归一化，可以得到光滑曲线

笛卡尔轨迹ctraj （默认梯形速度）

ctraj(T0,T1,N)     %T0:初始状态的变换矩阵     T1：末端的变换矩阵    N：插值次数（正整数）

例如：

T=ctraj(T1,T2,51);               
T=ctraj((T1,T2,tpoly(0,2,50)/2);

2.4 姿态轨迹规划

在这里插入图片描述
旋转矩阵转 RPY角（横滚、俯仰、偏航）；
生成多维轨迹（mtraj）；
RPY角转变换矩阵T；

%% 位姿轨迹规划 matlab     (有突变)
figure(4)
% rpy1 = tr2rpy(T1)/pi*180;      %旋转矩阵转转角，并转换为角度制
% rpy2 = tr2rpy(T2)/pi*180;
rpy1 = [0,150,0];
rpy2 = [200,0,0];

rpy_traj = mtraj(@tpoly,rpy1,rpy2,t);             %5次多项式插值
T_traj_rot=rpy2tr(rpy_traj);                    %反得到变换矩阵（仅有旋转部分）
P1 = transl(T1);
P2 = transl(T2);
P_traj = mtraj(@tpoly,P1',P2',t);
T_traj_transl = transl(P_traj);                   %有位移部分的变换矩阵

n = length(t);
T_traj = zeros(4,4,n);
for i=1:n
    T_traj(:,:,i) = T_traj_transl(:,:,i)*T_traj_rot(:,:,i);        %将带位移的变换矩阵与带旋转的相乘
end

q_traj = Five_dof.ikunc(T_traj);
Five_dof.plot(q_traj,'trail','r');

2.5 用关节变量做轨迹规划

在初末位置的变换矩阵求逆，得到关节变量；
再使用关节变量进行轨迹规划

关节变量轨迹规划函数:[q,qd,qdd]=jtraj(q0,qf,M)

[q,qd,qdd]=jtraj(q0,qf,M)    
%q0,qf ：初末关节变量    M ：插值次数

[q,qd,qdd]=jtraj(q0,qf,T,qd0,qdf)    
%T: 时间 例如：T = linspace(0,2,51)
% qd0,qdf: 初末关节速度要求

优势：在关节空间中顺滑，运行起来相对平稳

3. 显示三维模型

可以导入STL模型
函数：serialLink/plot3d
其中，path是导入STL模型（可以是ascii或者二进制）
！：每个STL文件的命名方式是LinkN.stl，N=0，1，…,N

只能用于DH的标准型建模（坐标系连接杆的末端）
在这里插入图片描述

例如：
在这里插入图片描述

clear;
clc;
L(1) = Link('revolute','d',0.216,'a',0,'alpha',pi/2);
L(2) = Link('revolute','d',0,'a',0.5,'alpha',0,'offset',pi/2);
L(3) = Link('revolute','d',0,'a',sqrt(0.145^2+0.42726^2),'alpha',0,'offset',-atan(427.46/145));
L(4) = Link('revolute','d',0,'a',0,'alpha',pi/2,'offset',atan(427.46/145));
L(5) = Link('revolute','d',0.258,'a',0,'alpha',0);

Five_dof = SerialLink(L,'name','5-dof');
Five_dof.base = transl(0,0,0.28);

q0 = [0 0 0 0 0];
v = [35 20];
w = [-1 1 -1 1 0 2];
Five_dof.plot3d(q0.'tilesize',0.1,'workspace',w,'path','D:\XX\XX','nowrist','view',v)

light('Position',[1 1 1],'color','w');     %加光源，使效果更清晰