(8條消息) 機(jī)器人學(xué)回爐重造（4）：動力學(xué)仿真（附牛頓-歐拉遞歸逆動力學(xué)算法matlab代碼）_xuuyann的博客-CSDN博客_matlab動力學(xué)仿真

本文機(jī)械臂參數(shù)基于上述文章，利用拉格朗日逆動力學(xué)建模推導(dǎo)，通過已知關(guān)節(jié)的q，dq,ddq獲取每個關(guān)節(jié)所需的tau力矩，仿真有不足或者錯誤之處希望大家多多提出修改意見。

自寫Matlab實(shí)現(xiàn)代碼

? ? clc;clear;th=[10 20 30]*pi/180;I1=[0 0 0;0 0 0;0 0 0.5];I2=[0 0 0;0 0 0;0 0 0.2];I3=[0 0 0;0 0 0;0 0 0.1];m1=20;m2=15;m3=10;g=[0 ;-9.8 ;0];[M,C,G,T]=Lagrange(th);function [M3,C3,G3,T] = Lagrange(theta)syms th1 th2 th3;angle = [th1 th2 th3];d_angle=[1 ;2 ;3];dd_angle=[0.5 ;1 ;1.5];[J1,J2,J3,JW1,JW2,JW3] = jisuan(angle);I1=[0 0 0;0 0 0;0 0 0.5];I2=[0 0 0;0 0 0;0 0 0.2];I3=[0 0 0;0 0 0;0 0 0.1];m1=20;m2=15;m3=10;g=[0 ;-9.8 ;0];%% 求慣性矩陣 MM=m1*J1'*J1+JW1'*I1*JW1 + ? m2*J2'*J2+JW2'*I2*JW2 + ?m3*J3'*J3+JW3'*I3*JW3;M1 = subs(M, th1, theta(1));M2 = subs(M1, th2, theta(2));M3 = subs(M2, th3, theta(3)); ??digits(4)M3 = vpa(M3,8);%% 求科氏力矩陣Vfor i=1:3for j=1:3 ? ? ? ? ??c=0;for k = 1:3c=c+1/2*((diff( M(i,j) ,angle(k)))+(diff( M(i,k) ,angle(j)))-(diff( M(j,k) ,angle(i))))*(d_angle(k));?endC(i,j) = simplify(c);endendC1 = subs(C, th1, theta(1));C2 = subs(C1, th2, theta(2));C3 = subs(C2, th3, theta(3)); ??digits(4)C3 = vpa(C3,8);%% 求重力因素矩陣CG=-(J1'*m1*g +J2'*m2*g+J3'*m3*g);G1 = subs(G, th1, theta(1));G2 = subs(G1, th2, theta(2));G3 = subs(G2, th3, theta(3)); ??digits(4)G3 = vpa(G3,8);%% 力矩T=M3*dd_angle+C3*d_angle+G3;JT1 = subs(J1, th1, theta(1));JT2 = subs(JT1, th2, theta(2));JT3 = subs(JT2, th3, theta(3)); ??digits(4)JV1 = vpa(JT3,8);JT1 = subs(J2, th1, theta(1));JT2 = subs(JT1, th2, theta(2));JT3 = subs(JT2, th3, theta(3)); ??digits(4)JV2 = vpa(JT3,8);JT1 = subs(J3, th1, theta(1));JT2 = subs(JT1, th2, theta(2));JT3 = subs(JT2, th3, theta(3)); ??digits(4)JV3 = vpa(JT3,8);end%% 求雅各比矩陣function [JV1,JV2,JV3,JW1,JW2,JW3]=jisuan(angle)th(1) = angle(1);th(2)=angle(2);th(3)=angle(3);%角度a=[0 4 3];alp=[0 0 0];d=[0 0 0];T0_1=Trans(alp(1), a(1), d(1), th(1));T1_2 = Trans(alp(2), a(2), d(2), th(2));?T2_3 = Trans(alp(3), a(3), d(3), th(3));T0_2 = T0_1 * T1_2;T0_3 = T0_2 * T2_3;T1_2c = Trans(alp(1), a(2)/2, d(1), th(1));T2_3c = Trans(alp(2), a(3)/2, d(2), th(2));?T3_4c = Trans(alp(3), 2/2, d(3), th(3));?T0_1c=T0_1 * T1_2c; %到第一個桿質(zhì)心處的的雅各比矩陣T0_2c=T0_2 * T2_3c; %到第二個桿質(zhì)心處的的雅各比矩陣T0_3c=T0_3 * T3_4c; %到第二個桿質(zhì)心處的的雅各比矩陣JW1=[0 ?0 ?0;0 ?0 ?0;1 ?0 ?0];JW2=[0 ?0 ?0;0 ?0 ?0;1 ?1 ?0];JW3=[0 ?0 ?0;0 ?0 ?0;1 ?1 ?1];for i = 1:3for j = 1:3JV1(j, i) = diff(T0_1c(j, 4), th(i));endendfor i = 1:3for j = 1:3JV2(j, i) = diff(T0_2c(j, 4), th(i));endendfor i = 1:3for j = 1:3JV3(j, i) = diff(T0_3c(j, 4), th(i));endend ?end%% 求齊次變換矩陣 function T = Trans(alpha, a, d, theta) T= [cos(theta) ? ? ? ? ? ? -sin(theta) ? ? ? ? ? ?0 ? ? ? ? ? ?a;sin(theta)*cos(alpha) ?cos(theta)*cos(alpha) ?-sin(alpha) ?-sin(alpha)*d;sin(theta)*sin(alpha) ?cos(theta)*sin(alpha) ?cos(alpha) ? cos(alpha)*d;0 ? ? ? ? ? ? ? ? ? ? ?0 ? ? ? ? ? ? ? ? ? ? ?0 ? ? ? ? ? ?1]; end

結(jié)果輸出

>> TT =851.7 636.7 296.8

總結(jié)

以上是生活随笔為你收集整理的机械臂拉格朗日法逆动力学建模仿真(附MATLAB代码)的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。