3轴机械臂动力学辨识-牛顿欧拉线性化(Matlab)

 

Matlab代码写成τ=Yp的形式(τ-关节扭矩 p-参数集)

% 三连杆机械臂瞬态运动的牛顿-欧拉递归逆动力学求解：
% 参数：（运动指令）各关节运动角度， 关节速度， 关节加速度（3*1矩阵）
% 返回值：各关节力矩（3*1矩阵）

function tau = min_Three_link_manipulator(theta,theta_d,theta_dd)
% 改进D-H参数
th(1)=theta(1)*pi/180;d(1)=0;a(1)=0;alp(1)=0;
th(2)=theta(2)*pi/180;d(2)=0;a(2)=4;alp(2)=0;
th(3)=theta(3)*pi/180;d(3)=0;a(3)=3;alp(3)=0;
% base_link的各项初始值
w00=[0;0;0]; v00=[0;0;0];w00d=[0;0;0];v00d=[0;9.8;0];
% 各关节p及各link质心pc的距离(假设质心在几何中心)
p10=[0;0;0];p21=[4;0;0];p32=[3;0;0];p43=[2;0;0];
pc11=[2;0;0];pc22=[1.5;0;0];pc33=[1;0;0];z=[0;0;1];
% 各连杆质量
m1=20;m2=15;m3=10;
% 惯性张量
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