Stewart平台 六自由度运动平台位置逆解实现与仿真研究

做这个课题前期在老师的指导下已经完成了一些利用mpu6050完成姿态解算的工作，后面有时间我再放上来。虽然明知最后只不过是拾人牙慧（一坨shit山）但依旧要去完成。

照搬电子科大梁老师的程序 6-SPS并联机器人反解程序（matlab） - 哔哩哔哩 (bilibili.com)https://www.bilibili.com/read/cv6302531/?spm_id_from=333.999.0.0

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' );