兩關(guān)節(jié)機(jī)器人運(yùn)動分析MATLAB源程序

1、function = DAR( % 名稱：兩關(guān)節(jié)機(jī)器人(DAR-Double Arm Robot % 功能：計(jì)算兩關(guān)節(jié)機(jī)器人末端位置坐標(biāo)及速度 % 軟件平臺版本：MATLAB 2011b % 日期：2014年3月16日 %*% % 前期準(zhǔn)備 % %*% clear all; clc syms t; %創(chuàng)建符號對象t，t表示兩關(guān)節(jié)機(jī)器人運(yùn)行的時間。 l1=10; %l1表示機(jī)械臂1的長度為10。 l2=5; %l2表示機(jī)械臂2的長度為5。 omiga1=pi/90; %omiga1表示機(jī)械臂1的角速度為pi/90。 omiga2=pi/8; %omiga2表示機(jī)械臂2的角速度為pi/8。 the

2、ta1=pi/6+omiga1*t; %theta1表示機(jī)械臂1相對于機(jī)架轉(zhuǎn)過的角度，起始角度為pi/6。 theta2=pi/6+omiga2*t; %theta2表示機(jī)械臂2相對于機(jī)械臂1轉(zhuǎn)過的角度，起始角度為pi/6。 %*% % 坐標(biāo)的建立與變換 % %*% T10=cos(theta1 -sin(theta1 0;sin(theta1 cos(theta1 0; 0 0 1*1 0 l1;0 1 0;0 0 1; %建立機(jī)械臂1末端相對于機(jī)架的坐標(biāo)。 T21=cos(theta2 -sin(theta2 0;sin(theta2 cos(theta2 0; 0 0 1*1 0 l2;

3、0 1 0;0 0 1; %建立機(jī)械臂2末端相對于機(jī)械臂1末端的坐標(biāo)。 T20=T10*T21; %計(jì)算機(jī)械臂2末端相對于機(jī)架的坐標(biāo)。 DT20=diff(T20,t; %對機(jī)械臂2末端相對于機(jī)架的坐標(biāo)進(jìn)行求導(dǎo)，計(jì)算機(jī) %械臂2末端的速度。 %*% % 顯示兩關(guān)節(jié)機(jī)器人末端某一時刻的位置坐標(biāo)和速度矢量 % %* *% disp('* *' disp(' 提示 ' disp('雙臂機(jī)器人的運(yùn)動時間為10秒鐘，若想計(jì)算中間某一時刻的 ' %顯示提示信息。 disp('具體位置和速度，請鍵盤輸入對應(yīng)時間，按回車鍵結(jié)束 ' %顯示提示信息

4、。 disp('*' while 1>0 disp(' ' %顯示空格。 t=input('請鍵盤輸入對應(yīng)時間,t大于等于0，小于等于10，t=' %輸入數(shù)據(jù)。 disp(' ' %顯示空格。 while 1>0 %內(nèi)部while循環(huán)用于判斷輸入數(shù)據(jù)是否正確；若輸入數(shù)據(jù)不再范圍內(nèi)，則重新輸入；否則，循環(huán)結(jié)束。 if t>=0&&t<=10 break; %若輸入正確，break跳出內(nèi)部循環(huán)。 else t=input('輸入時間超出范圍，請重新鍵盤輸入對應(yīng)時間t=' %若輸入

5、錯誤，重新輸入數(shù)據(jù)。 disp(' ' end end Xp=subs(T20(1,3; %計(jì)算機(jī)械臂2末端相對于機(jī)架的坐標(biāo)。 Yp=subs(T20(2,3; %計(jì)算機(jī)械臂2末端相對于機(jī)架的坐標(biāo)。 Xv=subs(DT20(1,3; %計(jì)算機(jī)械臂2末端相對于機(jī)架的坐標(biāo)。 Yv=subs(DT20(2,3; %計(jì)算機(jī)械臂2末端相對于機(jī)架的坐標(biāo)。 fprintf('第%f秒時，機(jī)械臂2末端的數(shù)據(jù)信息如下所示：',t; fprintf('n機(jī)械臂2末端位置橫坐標(biāo)為Xp=%fn機(jī)械臂2末端位置縱坐標(biāo)為Yp=%f',Xp,Yp; %顯示兩關(guān)節(jié)機(jī)器人末端給

6、定時刻的位置坐標(biāo)。 fprintf('n機(jī)械臂2末端位置X方向速度分量為Xv=%fn機(jī)械臂2末端位置Y方向速度分量為Yv=%fn',Xv,Yv; %顯示兩關(guān)節(jié)機(jī)器人末端給定時刻的速度分量。 disp(' ' %顯示空格。 disp('若繼續(xù)查看給定時刻的位置與速度，請鍵盤輸入1,回車結(jié)束' %顯示提示信息。 disp('若查看各類曲線，請鍵盤輸入非1的數(shù)字，回車結(jié)束' %顯示提示信息。 disp(' ' %顯示空格。 temp=input('請鍵盤輸入數(shù)據(jù)，temp=' if temp=1 %條件判

7、斷。 break; %若查看曲線，br eak跳出外部循環(huán)。 end end %* *% % 繪制曲線 % %*% t=0:0.1:10; %雙臂機(jī)器人運(yùn)動的時間范圍。 subplot(2,2,1; %繪制第1個子圖。 plot(subs(T20(1,3,subs(T20(2,3; %繪制子圖1的位置曲線。 title('雙臂機(jī)器人末端“位置”曲線' %給子圖1添加標(biāo)題。 xlabel('Xp' %子圖1的X軸標(biāo)題。 ylabel('Yp' %子圖1的Y軸標(biāo)題。 grid on; %顯示網(wǎng)格。 subplot(2,2,2; %繪制第2個子圖。 p

8、lot(subs(DT20(1,3,subs(DT20(2,3; %繪制子圖2的速度曲線。 title('雙臂機(jī)器人末端“速度”曲線' %給子圖2添加標(biāo)題。 xlabel('Xv' %子圖2的X軸標(biāo)題。 ylabel('Yv' %子圖2的Y軸標(biāo)題。 grid on; %顯示網(wǎng)格 subplot(2,2,3; %繪制第3個子圖。 plot(t,subs(T20(1,3,'-.r',t,subs(T20(2,3 %繪制子圖3的位置曲線 title('雙臂機(jī)器人末端“位置時間”曲線' %給子圖3添加標(biāo)題。 legend('Xp-t','Yp-t' %子圖3添加圖例。 xlabel('t' %子圖3的X軸標(biāo)題。 ylabel('Xp、Yp' %子圖3的Y軸標(biāo)題。 grid on; %顯示網(wǎng)格 subplot(2,2,4; %繪制第4個子圖。 plot(t,subs(DT20(1,3,'-.r',t,subs(DT20(2,3 %繪制子圖4的速度-時間曲線 title('雙臂機(jī)器人末端“速度時間”曲線' %