热门标签 | HotTags
当前位置:  开发笔记 > 编程语言 > 正文

Matlab机器人工具箱

因为需要用到和机器人相关的东西,就用到了这个工具箱,作者官网http:www.petercorke.comRobotics_Toolbox.html老爷子很厉害,那本《Robotics,V

因为需要用到和机器人相关的东西,就用到了这个工具箱,作者官网 http://www.petercorke.com/Robotics_Toolbox.html 

老爷子很厉害,那本《Robotics, Vision & Control》就是他本人写的,可以看做是工具箱的一个详细说明书。另外,在网站那里提到了rvctools/robot/robot.pdf这个pdf可以看做一个函数的使用说明文档,有不懂的函数可以在pdf中查查它的API。

关于安装方法可以参考http://blog.sina.com.cn/s/blog_a16714bf0101hygu.html

首先,输入rtbdemo可以看到一个列表,这个列表包含了常用的一些功能。我们也可以通过学习这个来快速上手这个工具箱。


  1. 旋转

%二维平面内的姿态(x, y, theta), Special Euclidean(2)
clear;
clc;
T1 = se2(1, 2, 30*pi/180)
trplot2(T1, 'frame', '1', 'color', 'b')
T2 = se2(2, 1, 0)
hold on
trplot2(T2, 'frame', '2', 'color', 'r');
T3 = T1*T2
trplot2(T3, 'frame', '3', 'color', 'g');
T4 = T2*T1
trplot2(T4, 'frame', '4', 'color', 'c');
P = [3 ; 2 ];
plot_point(P, '*');% 画出点的方位(world)
P1 = inv(T1) * [P; 1] % 点P在坐标系{1}中的方位,P齐次,原始式 h2e(inv(T1) * e2h(P))
axis([0 5 0 5]);

P2 = homtrans( inv(T2), P) % 点P在坐标系{2}中的方位

注意:点为列向量



>> R = rotx(pi/2) 

R =

 

    1.0000         0         0

         0   0.0000   -1.0000

         0   1.0000    0.0000

 >> tranimate(R) 动画显示

>> det(R) 行列式

>> R = rotx(30, 'deg') * roty(50, 'deg') * rotz(10, 'deg')

>> trplot(R)    最终形态

>> [theta,vec] = tr2angvec(R)   绕空间中的轴vec旋转theta角

>> eul = tr2eul(R)    转化为Eular角(Z,Y,Z)

>> rpy = tr2rpy(R)  转化为RPY(X,Y,Z),相对于上一个坐标系

>> q = Quaternion(R)

>> q.R

>>q1 = Quaternion( rotx(pi/2) )

>>q2 = Quaternion( roty(pi/2) )

>> q1 * q2


=======================三维坐标==============================

clc;
clear;
close all;


t1 = eye(4);
trplot(t1,'frame','R','arrow','width', '1', 'color', 'r', 'text_opts', {'FontSize', 10, 'FontWeight', 'light'},'view', [-0.3 0.5 0.6],'thick',0.9,'dispar',0.8 );
hold on;

robotHcam =[ -120.2117270943047, -700.033498810312, 759.864364912089, -3.137243821744305, -0.007451761425653, -0.207643158709979 ];
robotHcam(1,1) = robotHcam(1,1) /1000.0;
robotHcam(1,2) = robotHcam(1,2) /1000.0;
robotHcam(1,3) = robotHcam(1,3) /1000.0;
robotHcam1 = transl(robotHcam(1,1), robotHcam(1,2), robotHcam(1,3)) * trotx(robotHcam(1,4)) * troty(robotHcam(1,5))* trotz(robotHcam(1,6))
trplot(robotHcam1,'frame','C');

eef_real = [-0.0190283,-0.63227,-0.101349,-3.09773,-0.0104572,-0.33805];% -179.545 15.2654 -4.47882
robotHobj_real = transl(eef_real(1,1), eef_real(1,2), eef_real(1,3)) * trotx(eef_real(1,4)) * troty(eef_real(1,5))* trotz(eef_real(1,6))
trplot(robotHobj_real,'frame','E2', 'color', 'magenta')%cyan

camHobj = [ 0.082430 -0.075731 0.858318 -3.124834 -3.139422 3.051156];
camHobj1 = transl(camHobj(1,1), robotHcam(1,2), robotHcam(1,3)) * trotx(robotHcam(1,4)) * troty(robotHcam(1,5))* trotz(robotHcam(1,6));

robotHobj = robotHcam1 * camHobj1
trplot(robotHobj,'frame','O','arrow','width', '1', 'color', 'g');
trplot(robotBobj,'frame','O', 'color', 'g')


2. 平移

>> transl(0.5, 0.0, 0.0)

 

ans =

 

    1.0000         0         0   0.5000

         0   1.0000         0         0

         0         0   1.0000         0

         0         0         0   1.0000

>> troty(pi/2)   绕y轴旋转pi/2

 

ans =

 

    0.0000        0    1.0000         0

         0   1.0000         0         0

   -1.0000         0   0.0000         0

         0         0         0   1.0000

>> t = transl(0.5, 0.0, 0.0) * troty(pi/2) * trotz(-pi/2)


% If thistransformation represented the origin of a new coordinate frame with respect

% to the worldframe origin (0, 0, 0), that new origin would be given by

 >> t * [0 0 0 1]'

 

ans =

 

    0.5000

         0

         0

    1.0000

3. 轨迹

3.1 五次多项式轨迹规划

通常的轨迹规划限制条件:起始终止速度、加速度为0,设定起点和终点,一共6个条件,所以最容易想到的是5次多项式轨迹规划。

clear;
clc;
p0 = -1;% 定义初始点及终点位置
p1 = 2;
p = tpoly(p0, p1, 50);% 取步长为50
figure(1);

%%
plot(p);%绘图,可以看到在初始点及终点的一、二阶导均为零
[p,pd,pdd] = tpoly(p0, p1, 50);%得到位置、速度、加速度 tpoly为五阶多项式(这里的50步长并非限制条件)
figure(2);

subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p');
title('初始和终止速度都为0');
grid on;
subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd');
grid on;
subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd');
grid on;

%%
[s,sd,sdd] = tpoly(0, 1, 50, 0.5, 0); % 初始速度0.5,终点速度0

figure(3);
subplot(3,1,1); plot(s); xlabel('Time'); ylabel('s');
title('初始和终止速度为0.5和0')
hold on;
n = find(s == max(s));
plot(n, s(n),'o');
cell_string{1} = ['smax = ' num2str(s(n))]; % 多行文本
cell_string{2} = ['time = ' num2str(n)];

text(n, s(n) -3, cell_string);
hold off;
grid on;
subplot(3,1,2); plot(sd); xlabel('Time'); ylabel('sd');
grid on;
subplot(3,1,3); plot(sdd); xlabel('Time'); ylabel('sdd');
grid on;
下图为路径曲线,速度曲线和加速度曲线。


可以从图中看出初始和终止速度都为0时(左图),平均速度只有52%达到了峰值,也就是说机器人运行效率不高。而当初始和终止速度分别为0.5和0时(右图),除了平均速度的问题外,它的轨迹漂移比较大,从位置0到1的运动,最大达到了5.062。因此就引入了抛物线轨迹规划。

3.2 抛物线轨迹规划
抛物线轨迹规划就是起始段和终止段为抛物线,中间有部分为直线匀速段。限制条件为起始终止位置、起始终止速度、总时间。匀速段的速度在一定条件也可以作为限制条件。
[l,ld,ldd] = lspb(p0, p1, 50); % Linear Segment(匀速) with Parabolic(抛物线) Blends(过渡)注意这里的总步长是一个限制条件
figure(4);
subplot(3,1,1); plot(l); xlabel('Time'); ylabel('l');
grid on;
subplot(3,1,2); plot(ld); xlabel('Time'); ylabel('ld');% 可以看到速度是呈梯形
grid on;
subplot(3,1,3); plot(ldd); xlabel('Time'); ylabel('ldd');
grid on;
当设定匀速段的速度时,轨迹会有不同的反应。随着速度增加,他的匀速时间变短了,而且加速度会增大,也就是说震动会加大。匀速段的速度不能太大也不能太小,否则会无解。因为他是一个Over constraied system,这里有5个constrains(总时间,初始终止位置,初始终止速度)但是有6个freedom(抛物线过渡时间,抛物线方程的三个量,直线方程的两个量)。相当于有5个未知数但是有6个方程,无解。 在《机器人学导论--分析、控制及应用》——Saeed B.Niku 孙富春这本书P145提到,因为加速减速是对称的,vmax = 2(lf - l0)/ tf, lf和l0指终止和初始位置,tf是总时间。 这里vmax = 2(1-0)/ 50 = 0.04, 匀速段最大速度为0.04.不然就会没有匀速段的时间
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');
title('初始和终止速度都为0');
grid on;
subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd');
grid on;
subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd');
grid on;

%%
[s,sd,sdd] = tpoly(0, 1, 50, 0.5, 0); % 初始速度0.5,终点速度0

figure(3);
subplot(3,1,1); plot(s); xlabel('Time'); ylabel('s');
title('初始和终止速度为0.5和0')
hold on;
n = find(s == max(s));
plot(n, s(n),'o');
cell_string{1} = ['smax = ' num2str(s(n))]; % 多行文本
cell_string{2} = ['time = ' num2str(n)];

text(n, s(n) -3, cell_string);
hold off;
grid on;
subplot(3,1,2); plot(sd); xlabel('Time'); ylabel('sd');
grid on;
subplot(3,1,3); plot(sdd); xlabel('Time'); ylabel('sdd');
grid on;
%%
[l,ld,ldd] = lspb(0, 1, 50); % Linear Segment(匀速) with Parabolic(抛物线) Blends(过渡)
figure(4);

subplot(3,1,1); plot(l); xlabel('Time'); ylabel('l');
grid on;
hold on;
subplot(3,1,2); plot(ld); xlabel('Time'); ylabel('ld');% 可以看到速度是呈梯形
grid on;
hold on;
subplot(3,1,3); plot(ldd); xlabel('Time'); ylabel('ldd');
grid on;
hold on;

[l,ld,ldd] = lspb(0, 1, 50, 0.025);
subplot(3,1,1); plot(l,'c'); xlabel('Time'); ylabel('l');
grid on;

subplot(3,1,2); plot(ld, 'c'); xlabel('Time'); ylabel('ld');
grid on;

subplot(3,1,3); plot(ldd,'c'); xlabel('Time'); ylabel('ldd');
grid on;

[l,ld,ldd] = lspb(0, 1, 50, 0.04);
subplot(3,1,1); plot(l,'r'); xlabel('Time'); ylabel('l');
grid on;
hold off;
legend('normal','0.025','0.04','location','southeast');

subplot(3,1,2); plot(ld,'r'); xlabel('Time'); ylabel('ld');
grid on;
hold off;
legend('normal','0.025','0.04','location','northeast');

subplot(3,1,3); plot(ldd,'r'); xlabel('Time'); ylabel('ldd');
grid on;
hold off;
legend('normal','0.025','0.04','location','northeast');
3.3 多重分割轨迹规划

>> via = [4,1; 4,4; 5,2; 2,5];
>> q = mstraj(via, [2,1], [], [4, 1], 0.05, 0);% 每个坐标系上的最大速度,分段时间(与前面最大速度二者取一个),初始速度,样本间隔,加速时间,All axes reach their via points at the same time.
>> plot(q,'DisplayName','q')


% mstraj的位移、速度和加速度曲线P48
clc;
clf;
close all;
d=0.05;
% t=0:d:400;
via = [ 4,1; 4,4; 5,2; 2,5 ];
p=mstraj(via, [2,1], [], [4,1], 0.05, 2);
mstraj(via, [2,1], [], [4,1], 0.05, 2);

pd(:,1)=gradient(p(:,1))/d;
pd(:,2)=gradient(p(:,2))/d;
pdd(:,1)=gradient(pd(:,1))/d;
pdd(:,2)=gradient(pd(:,2))/d;
subplot(3,1,1);plot([t,t],p,'Linewidth',2);xlabel('Time');ylabel('p');grid on;
subplot(3,1,2);plot([t,t],pd,'Linewidth',2);xlabel('Time');ylabel('pd');grid on;
hold on;

max1=find(abs(pd(:,1))==max(abs(pd(:,1))))
max2=find(abs(pd(:,2))==max(abs(pd(:,2))))
plot(t(max1),pd(max1,1),'*');%描点画出关节1的最大速度点
plot(t(max2),pd(max2,2),'o','markersize',12);%描点画出关节2的最大速度点

text(t(max1,1)-1,pd(max1,1)-0.3,['(',num2str(t(max1,1)),',',num2str(pd(max1,1)),')']);% 标注极值点
text(t(max2,1)-1,pd(max2,1)+1.3,['(',num2str(t(max2,1)),',',num2str(pd(max2,2)),')']);

subplot(3,1,3);plot([t,t],pdd,'Linewidth',2);xlabel('Time');ylabel('pdd');grid on;


相比抛物线轨迹规划,匀速段和加速度段较好,但是他牺牲了位置点的精确性,只是趋近于途经点。

平移与旋转复合运动


%% P49

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); % 范围是【0,1】
about(Ts);
Ts(:, :, 1)
P = transl(Ts);% 平移部分
about(P)
figure(5);
subplot(5,1,1);plot(P,'DisplayName','Translate1');grid on;

rpy = tr2rpy(Ts);% 旋转部分
subplot(5,1,2);
plot(rpy,'DisplayName','RPY1');grid on;

Ts2 = trinterp(T0, T1, lspb(0, 1, 50));% 平滑平移部分
subplot(5,1,3);plot(transl(Ts2),'DisplayName','Translate2');grid on;set(gca,'XTick',0:10:50, 'YTick',[-0.5, -0.25, 0, 0.25, 0.5]);

subplot(5,1,4);plot(tr2rpy(Ts2),'DisplayName','RPY2');grid on;

Ts3 = ctraj(T0, T1, 50);% ctraj替代trinterp(T0, T1, lspb(0, 1, 50));
subplot(5,1,5);plot(transl(Ts3),'DisplayName','Translate2');grid on;





推荐阅读
  • 本文详细介绍了Java中org.eclipse.ui.forms.widgets.ExpandableComposite类的addExpansionListener()方法,并提供了多个实际代码示例,帮助开发者更好地理解和使用该方法。这些示例来源于多个知名开源项目,具有很高的参考价值。 ... [详细]
  • 使用 Azure Service Principal 和 Microsoft Graph API 获取 AAD 用户列表
    本文介绍了一段通用代码示例,该代码不仅能够操作 Azure Active Directory (AAD),还可以通过 Azure Service Principal 的授权访问和管理 Azure 订阅资源。Azure 的架构可以分为两个层级:AAD 和 Subscription。 ... [详细]
  • 深入解析Spring Cloud Ribbon负载均衡机制
    本文详细介绍了Spring Cloud中的Ribbon组件如何实现服务调用的负载均衡。通过分析其工作原理、源码结构及配置方式,帮助读者理解Ribbon在分布式系统中的重要作用。 ... [详细]
  • 本文详细介绍了中央电视台电影频道的节目预告,并通过专业工具分析了其加载方式,确保用户能够获取最准确的电视节目信息。 ... [详细]
  • 毕业设计:基于机器学习与深度学习的垃圾邮件(短信)分类算法实现
    本文详细介绍了如何使用机器学习和深度学习技术对垃圾邮件和短信进行分类。内容涵盖从数据集介绍、预处理、特征提取到模型训练与评估的完整流程,并提供了具体的代码示例和实验结果。 ... [详细]
  • 本文详细介绍了Java编程语言中的核心概念和常见面试问题,包括集合类、数据结构、线程处理、Java虚拟机(JVM)、HTTP协议以及Git操作等方面的内容。通过深入分析每个主题,帮助读者更好地理解Java的关键特性和最佳实践。 ... [详细]
  • ServiceStack与Swagger的无缝集成指南
    本文详细介绍了如何在ServiceStack项目中集成Swagger,以实现API文档的自动生成和在线测试。通过本指南,您将了解从配置到部署的完整流程,并掌握如何优化API接口的开发和维护。 ... [详细]
  • 本文详细介绍了Java中的访问器(getter)和修改器(setter),探讨了它们在保护数据完整性、增强代码可维护性方面的重要作用。通过具体示例,展示了如何正确使用这些方法来控制类属性的访问和更新。 ... [详细]
  • 本文详细探讨了VxWorks操作系统中双向链表和环形缓冲区的实现原理及使用方法,通过具体示例代码加深理解。 ... [详细]
  • 基因组浏览器中的Wig格式解析
    本文详细介绍了Wiggle(Wig)格式及其在基因组浏览器中的应用,涵盖variableStep和fixedStep两种主要格式的特点、适用场景及具体使用方法。同时,还提供了关于数据值和自定义参数的补充信息。 ... [详细]
  • 本文介绍如何使用JPA Criteria API创建带有多个可选参数的动态查询方法。当某些参数为空时,这些参数不会影响最终查询结果。 ... [详细]
  • 本文将介绍网易NEC CSS框架的规范及其在实际项目中的应用。通过详细解析其分类和命名规则,探讨如何编写高效、可维护的CSS代码,并分享一些实用的学习心得。 ... [详细]
  • 本文将深入探讨如何在不依赖第三方库的情况下,使用 React 处理表单输入和验证。我们将介绍一种高效且灵活的方法,涵盖表单提交、输入验证及错误处理等关键功能。 ... [详细]
  • 利用决策树预测NBA比赛胜负的Python数据挖掘实践
    本文通过使用2013-14赛季NBA赛程与结果数据集以及2013年NBA排名数据,结合《Python数据挖掘入门与实践》一书中的方法,展示如何应用决策树算法进行比赛胜负预测。我们将详细讲解数据预处理、特征工程及模型评估等关键步骤。 ... [详细]
  • 本文探讨了如何在 F# Interactive (FSI) 中通过 AddPrinter 和 AddPrintTransformer 方法自定义类型(尤其是集合类型)的输出格式,提供了详细的指南和示例代码。 ... [详细]
author-avatar
可靠的我还是一个人
这个家伙很懒,什么也没留下!
PHP1.CN | 中国最专业的PHP中文社区 | DevBox开发工具箱 | json解析格式化 |PHP资讯 | PHP教程 | 数据库技术 | 服务器技术 | 前端开发技术 | PHP框架 | 开发工具 | 在线工具
Copyright © 1998 - 2020 PHP1.CN. All Rights Reserved | 京公网安备 11010802041100号 | 京ICP备19059560号-4 | PHP1.CN 第一PHP社区 版权所有