做这个课题前期在老师的指导下已经完成了一些利用mpu6050完成姿态解算的工作,后面有时间我再放上来。虽然明知最后只不过是拾人牙慧(一坨shit山)但依旧要去完成。
clc
clear all
%close all
tic
XP = 0; % 动平台 相对 静平台 的初始位置坐标
YP = 0;
ZP = 300;
% 静平台初始位置坐标
X = 0;
Y = 0;
Z = 0;
ROLL = 0; % 相对静平台的恣态 参数类型为 角度 cosd(roll)
PITCH = 0;
YAW = 0;
P = [ X+XP; Y+YP; Z+ZP ]; % 动平台圆心点 相对 静平台 的坐标
%--------------------------平台的基本尺寸----------------------------------
R = 150; % 动平台铰点的外接圆半径
r = 200; % 静平台铰点的外接圆半径
up_angle0 = -133.5; % 动平台铰点的安装角度 87度 33度
up_angle1 = -46.5;
up_angle2 = -13.5;
up_angle3 = 73.5;
up_angle4 = 106.5;
up_angle5 = 193.5;
down_angle0 = -110.0; % 静平台铰点的安装角度 α-down θ1为80° θ2为40度
down_angle1 = -70.0;
down_angle2 = 10.0;
down_angle3 = 50.0;
down_angle4 = 130.0;
down_angle5 = 170.0;
%----------动平台的6个铰点,在动平台坐标系中的位置矢量---------------------
bR1 = [ R*cosd( up_angle0 ); R*sind( up_angle0 ); 0];
bR2 = [ R*cosd( up_angle1 ); R*sind( up_angle1 ); 0];
bR3 = [ R*cosd( up_angle2 ); R*sind( up_angle2 ); 0];
bR4 = [ R*cosd( up_angle3 ); R*sind( up_angle3 ); 0 ];
bR5 = [ R*cosd( up_angle4 ); R*sind( up_angle4 ); 0 ];
bR6 = [ R*cosd( up_angle5 ); R*sind( up_angle5 ); 0 ];
%----------静平台的6个铰点,在静平台坐标系中的位置矢量---------------------
Br1 = [ r*cosd( down_angle0 ) ; r*sind( down_angle0 ); Z ];
Br2 = [ r*cosd( down_angle1 ) ; r*sind( down_angle1 ); Z ];
Br3 = [ r*cosd( down_angle2 ) ; r*sind( down_angle2 ); Z ];
Br4 = [ r*cosd( down_angle3 ) ; r*sind( down_angle3 ); Z ];
Br5 = [ r*cosd( down_angle4 ) ; r*sind( down_angle4 ); Z ];
Br6 = [ r*cosd( down_angle5 ) ; r*sind( down_angle5 ); Z ];
%TransM = rotz(YAW) * roty(PITCH) * rotx(ROLL); % XYZ旋转矩阵
%TransM = rotx(ROLL) * roty(PITCH) * rotz(YAW) % ZYX旋转矩阵
TransM =...
[cosd(YAW), -sind(YAW),0; ...
sind(YAW),cosd(YAW),0; ...
0,0,1 ] * ...
[ cosd(PITCH), 0, sind(PITCH);...
0, 1, 0; ...
-sind(PITCH), 0, cosd(PITCH) ]* ...
[ 1, 0, 0;...
0, cosd(ROLL),-sind(ROLL);...
0, sind(ROLL),cosd(ROLL)];
%----------动平台的6个铰点,在静平台坐标系中的位置矢量,变换矩阵---------------------
br1 = TransM * bR1 + P;
br2 = TransM * bR2 + P;
br3 = TransM * bR3 + P;
br4 = TransM * bR4 + P;
br5 = TransM * bR5 + P;
br6 = TransM * bR6 + P;
%--动平台的6个铰点位置矢量,减去,静平台的6个铰点位置矢量,得到每个杆长矢量
L1 = br1 - Br1;
L2 = br2 - Br2;
L3 = br3 - Br3;
L4 = br4 - Br4;
L5 = br5 - Br5;
L6 = br6 - Br6;
%-----------求模,得到每个杆的杆长-----------------------------------------
LenL1 = norm(L1);
LenL2 = norm(L2);
LenL3 = norm(L3);
LenL4 = norm(L4);
LenL5 = norm(L5);
LenL6 = norm(L6);
toc % 显示计算时间
%-------------------------画出3D图-----------------------------------------
h=gca; % 把原来的图删掉,避免新图和旧的图相互重叠
delete(h);
figure(1); % hold on 开启
hold on;
view([1,1,1]); % 设定3D图的观察视角
axis equal; % XYZ轴显示比例相等
plot3( [0,300], [0,0], [0,0], '-b', 'LineWidth', 1 );% 在图中画出X轴,方便观察
plot3( 300, 0, 0, '>b' );
plot3( [0,0], [0,300], [0,0], '-r', 'LineWidth', 1 );% 在图中画出Y轴,方便观察
plot3( 0, 300, 0, '>r' );
plot3( [0,0], [0,0], [0,300], '-k', 'LineWidth', 1 );% 在图中画出Z轴,方便观察
plot3( 0, 0, 300, '>k' );
PBx0 = TransM * [ 0; 0; 0 ] + P;
PBx1 = TransM * [ 0+100; 0; 0 ] + P;
PBy0 = TransM * [ 0; 0; 0 ] + P;
PBy1 = TransM * [ 0; 0+100; 0 ] + P;
PBz0 = TransM * [ 0; 0; 0 ] + P;
PBz1 = TransM * [ 0; 0; 0+100 ] + P;
plot3( [PBx0(1),PBx1(1)], [PBx0(2),PBx1(2)], [PBx0(3),PBx1(3)], '-g', 'LineWidth', 1 );% 在图中画出X轴,方便观察
plot3( [PBy0(1),PBy1(1)], [PBy0(2),PBy1(2)], [PBy0(3),PBy1(3)], '-r', 'LineWidth', 1 );% 在图中画出Y轴,方便观察
plot3( [PBz0(1),PBz1(1)], [PBz0(2),PBz1(2)], [PBz0(3),PBz1(3)], '-k', 'LineWidth', 1 );% 在图中画出Z轴,方便观察
% 动平台用红色的图块,静平台用蓝色的图块
patch( [Br1(1),Br2(1),Br3(1),Br4(1),Br5(1),Br6(1)], [Br1(2),Br2(2),Br3(2),Br4(2),Br5(2),Br6(2)], [Br1(3),Br2(3),Br3(3),Br4(3),Br5(3),Br6(3)], 'c' );
patch( [br1(1),br2(1),br3(1),br4(1),br5(1),br6(1)], [br1(2),br2(2),br3(2),br4(2),br5(2),br6(2)], [br1(3),br2(3),br3(3),br4(3),br5(3),br6(3)], 'y' );
plot3( br1(1),br1(2),br1(3)-10, '.m', 'MarkerSize', 30); % 画出动平台上的6个铰点
plot3( br2(1),br2(2),br2(3)-10, '.m', 'MarkerSize', 30);
plot3( br3(1),br3(2),br3(3)-10, '.m', 'MarkerSize', 30);
plot3( br4(1),br4(2),br4(3)-10, '.m', 'MarkerSize', 30);
plot3( br5(1),br5(2),br5(3)-10, '.m', 'MarkerSize', 30);
plot3( br6(1),br6(2),br6(3)-10, '.m', 'MarkerSize', 30);
考虑实际模型存在的万向节模块,我将铰点向下偏移10个单位
plot3( P(1), P(2), P(3), '.k', 'MarkerSize',10); % 画出动、静平台的圆心,方便观察
plot3( 0, 0, 0, '.k', 'MarkerSize',10);
plot3( [br1(1),Br1(1)], [br1(2),Br1(2)], [br1(3)-10,Br1(3)], '-k', 'LineWidth', 2 ); % 动、静平台的铰点连接,也就是连杆
plot3( [br2(1),Br2(1)], [br2(2),Br2(2)], [br2(3)-10,Br2(3)], '-k', 'LineWidth', 2 );
plot3( [br3(1),Br3(1)], [br3(2),Br3(2)], [br3(3)-10,Br3(3)], '-k', 'LineWidth', 2 );
plot3( [br4(1),Br4(1)], [br4(2),Br4(2)], [br4(3)-10,Br4(3)], '-k', 'LineWidth', 2 );
plot3( [br5(1),Br5(1)], [br5(2),Br5(2)], [br5(3)-10,Br5(3)], '-k', 'LineWidth', 2 );
plot3( [br6(1),Br6(1)], [br6(2),Br6(2)], [br6(3)-10,Br6(3)], '-k', 'LineWidth', 2 );
%-------------------------设置3D图的一些格式,方便观察---------------------
xlim([ -500,500 ]); % X轴固定显示范围
ylim([ -500,500 ]); % Y轴固定显示范围
zlim([ 0,600 ]); % Z轴固定显示范围
xlabel('X-axis / mm'); % X轴标识
ylabel('Y-axis / mm'); % Y轴标识
zlabel('Z-axis / mm'); % Z轴标识
str1 = ['LenL1 = ',num2str(LenL1)]; % 在图上显示每条连杆的长度,mm为单位
text(-300,200,900,[str1,' mm']);
str2 = ['LenL2 = ',num2str(LenL2)];
text(-300,200,800,[str2,' mm']);
str3 = ['LenL3 = ',num2str(LenL3)];
text(-300,200,700,[str3,' mm']);
str4 = ['LenL4 = ',num2str(LenL4)];
text(-300,200,600,[str4,' mm']);
str5 = ['LenL5 = ',num2str(LenL5)];
text(-300,200,500,[str5,' mm']);
str6 = ['LenL6 = ',num2str(LenL6)];
text(-300,200,400,[str6,' mm']);
text( Br1(1), Br1(2)-30, '1' ); % 在静平台的铰点旁边,显示连杆的序号
text( Br2(1)+50, Br2(2)-30, '2' );
text( Br3(1)+50, Br3(2)-30, '3' );
text( Br4(1)+50, Br4(2)+20, '4' );
text( Br5(1)+50, Br5(2)+20, '5' );
text( Br6(1)-50, Br6(2)-30, '6' );