matlab 七自由度牛顿欧拉动力学方程(数值版)

最新推荐文章于 2024-07-14 14:53:00 发布

weixin_42824706

最新推荐文章于 2024-07-14 14:53:00 发布

阅读量589

点赞数 22

文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/weixin_42824706/article/details/140302814

版权

%七自由度牛顿欧拉动力学(数值版）
number_of_links=7
% for i=1:number_of_links;
%
% end
%% 调用
q=[0 0 0 0 0 0 0];
qd=[0.1 0.1 0.1 0.1 0.1 0.1 0.1];
qdd=[0.1 0.1 0.1 0.1 0.1 0.1 0.1];
torque=NewtonEuler(q,qd,qdd,1)

function tau = NewtonEuler(q,qd,qdd,G)
%% 输入参数：
% q：广义关节坐标，此处为关节转角，1×7矩阵，每一行向量对应一组关节转角，单位：rad；
% qd：广义关节坐标一阶导数，此处为关节角速度，1×7矩阵，每一行向量对应一组关节角速度，单位：rad/s；
% qdd：广义关节坐标二阶导数，此处为关节角加速度，1×7矩阵，每一行向量对应一组关节角加速度，单位：rad/s^2；
% G：重力项，当G = 1,有重力影响，当G = 0,无重力影响；
% note:：三个输入参数的长度需保持一致。

%%
alpha=[0 0 0 0 0 0 0];
a=[0 0 0 0 0 0 0];
thetaoffset=[0 0 0 0 0 0 0];
theta=q+thetaoffset;
d=[0 0 0 0 0 0 0];
%%

T(:,:,1)=MDH(alpha(1),a(1),theta(1),d(1));%alpha,a,q,d
T(:,:,2)=MDH(alpha(2),a(2),theta(2),d(2));%alpha,a,q,d
T(:,:,3)=MDH(alpha(3),a(3),theta(3),d(3));%alpha,a,q,d
T(:,:,4)=MDH(alpha(4),a(4),theta(4),d(4));%alpha,a,q,d
T(:,:,5)=MDH(alpha(5),a(5),theta(5),d(5));%alpha,a,q,d
T(:,:,6)=MDH(alpha(6),a(6),theta(6),d(6));%alpha,a,q,d
T(:,:,7)=MDH(alpha(7),a(7),theta(7),d(7));%alpha,a,q,d
%% 惯性张量inertia tensor
% inertia_tensor_list：连杆关于质心坐标系的惯性张量inertia tensor，质心坐标系与连杆坐标系方位一致，7个3×3矩阵，单位kg*m^2
% I = Ixx -Ixy -Ixz
% -Ixy Iyy -Iyz
% -Iyz -Iyz Izz

inertia_tensor_list(:,:,1) = [17085955.96 29.13 520.90;
29.13 16299848.40 3041655.32;
520.90 3041655.32 6028717.92]/1e9;

inertia_tensor_list(:,:,2) = [17049081.74 -379.65 -251.02;
-379.65 6095392.93 2836636.56;
-251.02 2836636.56 16245722.28]/1e9;

inertia_tensor_list(:,:,3) = [25077403.67 1373.03 4792.34;
1373.03 23806776.16 -4872887.52;
4792.34 -4872887.52 7607337.29]/1e9;

inertia_tensor_list(:,:,4) = [17049008.27 -2836.83 605.34;
-2836.83 6095457.66 2836684.49;
605.34 2836684.49 16245750.37]/1e9;

inertia_tensor_list(:,:,5) = [10079214.34 -74.51 17.98;
-74.51 8702598.01 3090329.67;
17.98 3090329.67 4469563.66]/1e9;

inertia_tensor_list(:,:,6) = [5094485.23 -96.90 -67.21;
-96.90 3542620.51 -249580.52;
-67.21 -249580.52 4899002.53]/1e9;

inertia_tensor_list(:,:,7) = [198764.02 0.88 -27.71;
0.88 195312.12 0.20;
-27.71 0.20 322516.91]/1e9;
%% mass_center_list：
% mass_center_list：连杆质心在连杆坐标系下的位置，3×7矩阵，单位：m
% x y z
mass_center_list = [0 -34.73 -69.48;
0 -67.33 34.41;
-0.03 29.56 -89.00;
0.03 -67.33 -34.41;
0 -21.39 -140.03;
0 -2.12 0.29;
0.01 0 -25.22]/1000; %注意坐标相对于哪个坐标系来说
% mass_list: 连杆的质量，1×7矩阵，单位：kg
mass_list = [2702.4 2725.8 3175.01 2725.80 1693.85 1836.74 269.17]/1000;
m1 =mass_list(1,1); m2 = mass_list(1,2); m3 = mass_list(1,3); m4 = mass_list(1,4); m5 = mass_list(1,5); m6 = mass_list(1,6);m7 = mass_list(1,7);

z = [0,0,1]'; % 关节轴
if G == 1
g = 9.81; % 重力加速度，单位m/s^2
else
g = 0;
end
%% 提取旋转矩阵并求逆/ 旋转矩阵是正定矩阵RR'=E ,R'=inv(R)
T01=T(:,:,1);T12=T(:,:,2);T23=T(:,:,3);T34=T(:,:,4);
T45=T(:,:,5);T56=T(:,:,6);T67=T(:,:,7);

R01=T01(1:3,1:3); R12=T12(1:3,1:3); R23=T23(1:3,1:3);R34=T34(1:3,1:3);
R45=T45(1:3,1:3); R56=T56(1:3,1:3); R67=T67(1:3,1:3); R78=[1 0 0; 0 1 0; 0 0 1];%R87=[1 0 0; 0 1 0; 0 0 1];代表平移姿态没变

R10=inv(T01(1:3,1:3)); R21=inv(T12(1:3,1:3)); R32=inv(T23(1:3,1:3));R43=inv(T34(1:3,1:3));
R54=inv(T45(1:3,1:3)); R65=inv(T56(1:3,1:3)); R76=inv(T67(1:3,1:3)); R87=[1 0 0; 0 1 0; 0 0 1];%R87=[1 0 0; 0 1 0; 0 0 1];代表平移姿态没变

%% 各关节p及各link质心pc的距离(假设质心在几何中心)
p01 = T01(1: 3, 4); p12 = T12(1: 3, 4); p23 = T23(1: 3, 4);
p34 = T34(1: 3, 4); p45 = T45(1: 3, 4); p56 = T56(1: 3, 4); p67 =T67(1: 3, 4);p78 = [0, 0, 0]';%p78=代表平移7杆长度;

%(假设质心在几何中心)
% pc11 = 0.5 * p12; pc22 = 0.5 * p23; pc33 = 0.5 * p34;
% pc44 = 0.5 * p45; pc55 = 0.5 * p56; pc66 = 0.5 * p67; pc77=0.5*p78;

%% Outward iterations: i: 0->6
% 连杆1到连杆7向外迭代
% i = 0 %1杆
w00 = [0; 0; 0]; v00 = [0; 0; 0]; w00d = [0; 0; 0]; v00d = [0; 0; 9.8];%base_link的各项初始值
w11 = R10*w00 + qd(1)*z;
w11d=R10*w00d+cross(R10*w00,qd(1)*z)+qdd(1)*z;
v11d=R10*(cross(w00d,p01)+cross(w00, cross(w00, p01))+v00d)
vc11d=cross(w11d,mass_center_list(1,1:3)')+cross(w11, cross(w11, mass_center_list(1,1:3)')) + v11d;
%vc11d=cross(w11d, pc11) + cross(w11, cross(w11, pc11)) + v11d; %或者使用pc11
F11 = m1*vc11d;
N11=inertia_tensor_list(:,:,1)*w11d+cross(w11,inertia_tensor_list(:,:,1)*w11); % N11 = I1*w11d + cross(w11, I1*w11);

% i = 1 %2杆
w22 = R21*w11 + qd(2)*z;
w22d=R21*w11d+cross(R21*w11,qd(2)*z)+qdd(2)*z;
v22d=R21*(cross(w11d,p12)+cross(w11, cross(w11, p12))+v11d)
vc22d=cross(w22d,mass_center_list(2,1:3)')+cross(w22, cross(w22, mass_center_list(2,1:3)')) + v22d;
F22 = m2*vc22d;
N22=inertia_tensor_list(:,:,2)*w22d+cross(w22,inertia_tensor_list(:,:,2)*w22); % N22 = I2*w22d + cross(w22, I2*w22);

% i = 2 %3杆
w33 = R32*w22 + qd(3)*z;
w33d=R32*w22d+cross(R32*w22,qd(3)*z)+qdd(3)*z;
v33d=R32*(cross(w22d,p23)+cross(w22, cross(w22, p23))+v22d)
vc33d=cross(w33d,mass_center_list(3,1:3)')+cross(w33, cross(w33, mass_center_list(3,1:3)')) + v33d;
F33 = m3*vc33d;
N33=inertia_tensor_list(:,:,3)*w33d+cross(w33,inertia_tensor_list(:,:,3)*w33); % N33 = I3*w33d + cross(w33, I3*w33);

% i = 3 %4杆
w44 = R43*w33 + qd(4)*z;
w44d=R43*w33d+cross(R43*w33,qd(4)*z)+qdd(4)*z;
v44d=R43*(cross(w33d,p34)+cross(w33, cross(w33, p34))+v33d)
vc44d=cross(w44d,mass_center_list(4,1:3)')+cross(w44, cross(w44, mass_center_list(4,1:3)')) + v44d;
F44 = m4*vc44d;
N44=inertia_tensor_list(:,:,4)*w44d+cross(w44,inertia_tensor_list(:,:,4)*w44); % N44 = I4*w44d + cross(w44, I4*w44);

% i = 4 %5杆
w55 = R54*w44 + qd(5)*z;
w55d=R54*w44d+cross(R54*w44,qd(5)*z)+qdd(5)*z;
v55d=R54*(cross(w44d,p45)+cross(w44, cross(w44, p45))+v44d)
vc55d=cross(w55d,mass_center_list(5,1:3)')+cross(w55, cross(w55, mass_center_list(5,1:3)')) + v55d;
F55 = m5*vc55d;
N55=inertia_tensor_list(:,:,5)*w55d+cross(w55,inertia_tensor_list(:,:,5)*w55); % N55 = I5*w55d + cross(w55, I5*w55);

% i = 5 %6杆
w66 = R65*w55 + qd(6)*z;
w66d=R65*w55d+cross(R65*w55,qd(6)*z)+qdd(6)*z;
v66d=R65*(cross(w55d,p56)+cross(w55, cross(w55, p56))+v55d)
vc66d=cross(w66d,mass_center_list(6,1:3)')+cross(w66, cross(w66, mass_center_list(6,1:3)')) + v66d;
F66 = m6*vc66d;
N66=inertia_tensor_list(:,:,6)*w66d+cross(w66,inertia_tensor_list(:,:,6)*w66); % N66 = I6*w66d + cross(w66, I6*w66);

% i = 6 %7杆
w77 = R76*w66 + qd(7)*z;
w77d=R76*w66d+cross(R76*w66,qd(7)*z)+qdd(7)*z;
v77d=R76*(cross(w66d,p67)+cross(w66, cross(w66, p67))+v66d);
vc77d=cross(w77d,mass_center_list(7,1:3)')+cross(w77, cross(w77, mass_center_list(7,1:3)')) + v77d;
F77 = m7*vc77d;
N77=inertia_tensor_list(:,:,7)*w77d+cross(w77,inertia_tensor_list(:,:,7)*w77); % N77 = I7*w77d + cross(w77, I7*w77);

%% Inward iterations: i: 7->1
f88=[0;0;0];n88=[0;0;0]; %没有八杆，也没有外力，因此为零
%i=7 %7杆
f77=R78*f88+F77;
n77= N77+R78*n88+cross(mass_center_list(7,1:3)',F77)+cross(p78,R78*f88);
tau(7)=n77'*z;

%i=6 %6杆
f66=R67*f77+F66;
n66= N66+R67*n77+cross(mass_center_list(6,1:3)',F66)+cross(p67,R67*f77);
tau(6)=n66'*z;

%i=5 %5杆
f55=R56*f66+F55;
n55= N55+R56*n66+cross(mass_center_list(5,1:3)',F55)+cross(p56,R56*f66);
tau(5)=n55'*z;

%i=4 %4杆
f44=R45*f55+F44;
n44= N44+R45*n55+cross(mass_center_list(4,1:3)',F44)+cross(p45,R45*f55);
tau(4)=n44'*z;

%i=3 %3杆
f33=R34*f44+F33;
n33= N33+R34*n44+cross(mass_center_list(3,1:3)',F33)+cross(p34,R34*f44);
tau(3)=n33'*z;

%i=2 %2杆
f22=R23*f33+F22;
n22= N22+R23*n33+cross(mass_center_list(2,1:3)',F22)+cross(p23,R23*f33);
tau(2)=n22'*z;

%i=1 %1杆
f11=R12*f22+F11;
n11= N11+R12*n22+cross(mass_center_list(1,1:3)',F11)+cross(p12,R12*f22);
tau(1)=n11'*z;

%disp('over')

end
%%
function T = MDH(alpha,a,theta,d)
T(1,1) = cos(theta);
T(1,2) = -sin(theta);
T(1,3) = 0;
T(1,4) = a;
T(2,1) = sin(theta)*cos(alpha);
T(2,2) = cos(theta)*cos(alpha);
T(2,3) = -sin(alpha);
T(2,4) = -d*sin(alpha);
T(3,1) = sin(theta)*sin(alpha);
T(3,2) = cos(theta)*sin(alpha);
T(3,3) = cos(alpha);
T(3,4) = d*cos(alpha);
T(4,1) = 0;
T(4,2) = 0;
T(4,3) = 0;
T(4,4) = 1;
end