两关节机器人运动分析MATLAB源程序

1、function = DAR( % 名称：两关节机器人(DAR-Double Arm Robot % 功能：计算两关节机器人末端位置坐标及速度 % 软件平台版本：MATLAB 2011b % 日期：2014年3月16日 %*% % 前期准备 % %*% clear all; clc syms t; %创建符号对象t，t表示两关节机器人运行的时间。 l1=10; %l1表示机械臂1的长度为10。 l2=5; %l2表示机械臂2的长度为5。 omiga1=pi/90; %omiga1表示机械臂1的角速度为pi/90。 omiga2=pi/8; %omiga2表示机械臂2的角速度为pi/8。 the

2、ta1=pi/6+omiga1*t; %theta1表示机械臂1相对于机架转过的角度，起始角度为pi/6。 theta2=pi/6+omiga2*t; %theta2表示机械臂2相对于机械臂1转过的角度，起始角度为pi/6。 %*% % 坐标的建立与变换 % %*% T10=cos(theta1 -sin(theta1 0;sin(theta1 cos(theta1 0; 0 0 1*1 0 l1;0 1 0;0 0 1; %建立机械臂1末端相对于机架的坐标。 T21=cos(theta2 -sin(theta2 0;sin(theta2 cos(theta2 0; 0 0 1*1 0 l2;

3、0 1 0;0 0 1; %建立机械臂2末端相对于机械臂1末端的坐标。 T20=T10*T21; %计算机械臂2末端相对于机架的坐标。 DT20=diff(T20,t; %对机械臂2末端相对于机架的坐标进行求导，计算机 %械臂2末端的速度。 %*% % 显示两关节机器人末端某一时刻的位置坐标和速度矢量 % %* *% disp('* *' disp(' 提示 ' disp('双臂机器人的运动时间为10秒钟，若想计算中间某一时刻的 ' %显示提示信息。 disp('具体位置和速度，请键盘输入对应时间，按回车键结束 ' %显示提示信息

4、。 disp('*' while 1>0 disp(' ' %显示空格。 t=input('请键盘输入对应时间,t大于等于0，小于等于10，t=' %输入数据。 disp(' ' %显示空格。 while 1>0 %内部while循环用于判断输入数据是否正确；若输入数据不再范围内，则重新输入；否则，循环结束。 if t>=0&&t<=10 break; %若输入正确，break跳出内部循环。 else t=input('输入时间超出范围，请重新键盘输入对应时间t=' %若输入

5、错误，重新输入数据。 disp(' ' end end Xp=subs(T20(1,3; %计算机械臂2末端相对于机架的坐标。 Yp=subs(T20(2,3; %计算机械臂2末端相对于机架的坐标。 Xv=subs(DT20(1,3; %计算机械臂2末端相对于机架的坐标。 Yv=subs(DT20(2,3; %计算机械臂2末端相对于机架的坐标。 fprintf('第%f秒时，机械臂2末端的数据信息如下所示：',t; fprintf('n机械臂2末端位置横坐标为Xp=%fn机械臂2末端位置纵坐标为Yp=%f',Xp,Yp; %显示两关节机器人末端给

6、定时刻的位置坐标。 fprintf('n机械臂2末端位置X方向速度分量为Xv=%fn机械臂2末端位置Y方向速度分量为Yv=%fn',Xv,Yv; %显示两关节机器人末端给定时刻的速度分量。 disp(' ' %显示空格。 disp('若继续查看给定时刻的位置与速度，请键盘输入1,回车结束' %显示提示信息。 disp('若查看各类曲线，请键盘输入非1的数字，回车结束' %显示提示信息。 disp(' ' %显示空格。 temp=input('请键盘输入数据，temp=' if temp=1 %条件判

7、断。 break; %若查看曲线，br eak跳出外部循环。 end end %* *% % 绘制曲线 % %*% t=0:0.1:10; %双臂机器人运动的时间范围。 subplot(2,2,1; %绘制第1个子图。 plot(subs(T20(1,3,subs(T20(2,3; %绘制子图1的位置曲线。 title('双臂机器人末端“位置”曲线' %给子图1添加标题。 xlabel('Xp' %子图1的X轴标题。 ylabel('Yp' %子图1的Y轴标题。 grid on; %显示网格。 subplot(2,2,2; %绘制第2个子图。 p

8、lot(subs(DT20(1,3,subs(DT20(2,3; %绘制子图2的速度曲线。 title('双臂机器人末端“速度”曲线' %给子图2添加标题。 xlabel('Xv' %子图2的X轴标题。 ylabel('Yv' %子图2的Y轴标题。 grid on; %显示网格 subplot(2,2,3; %绘制第3个子图。 plot(t,subs(T20(1,3,'-.r',t,subs(T20(2,3 %绘制子图3的位置曲线 title('双臂机器人末端“位置时间”曲线' %给子图3添加标题。 legend('Xp-t','Yp-t' %子图3添加图例。 xlabel('t' %子图3的X轴标题。 ylabel('Xp、Yp' %子图3的Y轴标题。 grid on; %显示网格 subplot(2,2,4; %绘制第4个子图。 plot(t,subs(DT20(1,3,'-.r',t,subs(DT20(2,3 %绘制子图4的速度-时间曲线 title('双臂机器人末端“速度时间”曲线' %