stewart平台 波浪补偿 振动补偿 运动学基础

逆解算法（IK）：逆解算法则是指根据给定的位姿，计算出机器人关节长度。逆解问题的求解过程用数值法求解相对比较简单，使用的公式也只有一个

相比较传统的inverse计算，这里将上平台视为姿态不变体，来求解腿长，姿态补偿的关键其实就是求解 传感器得到的姿态变化以后，六个支链需要改变的长度，所以从公式上来说这个课题并不难理解。

clc
clear all
%close all

tic
% 上平台初始位置坐标  500
X = 0;   
Y = 0;
Z = 500;

XP = 0; %  下平台  相对  上平台  的初始位置下移300，这里可以设置xyz三个方向位移量
YP = 0;
ZP = -300;



ROLL  = 0.1; % 相对静平台的恣态   参数类型为 角度  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 ); Z];
bR2 = [ R*cosd( up_angle1 ); R*sind( up_angle1 ); Z];
bR3 = [ R*cosd( up_angle2 ); R*sind( up_angle2 ); Z];
bR4 = [ R*cosd( up_angle3 ); R*sind( up_angle3 ); Z ];
bR5 = [ R*cosd( up_angle4 ); R*sind( up_angle4 ); Z ];
bR6 = [ R*cosd( up_angle5 ); R*sind( up_angle5 ); Z ];

%----------静平台的6个铰点，在静平台坐标系中的位置矢量---------------------
Br1 = [ r*cosd( down_angle0 ) ; r*sind( down_angle0 ); 0 ];
Br2 = [ r*cosd( down_angle1 ) ; r*sind( down_angle1 ); 0 ];
Br3 = [ r*cosd( down_angle2 ) ; r*sind( down_angle2 ); 0 ];
Br4 = [ r*cosd( down_angle3 ) ; r*sind( down_angle3 ); 0 ];
Br5 = [ r*cosd( down_angle4 ) ; r*sind( down_angle4 ); 0 ];
Br6 = [ r*cosd( down_angle5 ) ; r*sind( down_angle5 ); 0 ];

%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个位置矢量，得到杆长矢量
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), '.m', 'MarkerSize', 30); %  画出动平台上的6个铰点
plot3(BR2(1),BR2(2),BR2(3), '.m', 'MarkerSize', 30);
plot3(BR3(1),BR3(2),BR3(3), '.m', 'MarkerSize', 30);
plot3(BR4(1),BR4(2),BR4(3), '.m', 'MarkerSize', 30);
plot3(BR5(1),BR5(2),BR5(3), '.m', 'MarkerSize', 30);
plot3(BR6(1),BR6(2),BR6(3), '.m', 'MarkerSize', 30);

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),bR1(3)], '-k', 'LineWidth', 2 ); % 动、静平台的铰点连接，也就是连杆
plot3( [BR2(1),bR2(1)], [BR2(2),bR2(2)], [BR2(3),bR2(3)], '-k', 'LineWidth', 2 );
plot3( [BR3(1),bR3(1)], [BR3(2),bR3(2)], [BR3(3),bR3(3)], '-k', 'LineWidth', 2 );
plot3( [BR4(1),bR4(1)], [BR4(2),bR4(2)], [BR4(3),bR4(3)], '-k', 'LineWidth', 2 );
plot3( [BR5(1),bR5(1)], [BR5(2),bR5(2)], [BR5(3),bR5(3)], '-k', 'LineWidth', 2 );
plot3( [BR6(1),bR6(1)], [BR6(2),bR6(2)], [BR6(3),bR6(3)], '-k', 'LineWidth', 2 );

%-------------------------设置3D图的一些格式，方便观察---------------------
xlim([ -500,600 ]); %  X轴固定显示范围
ylim([ -500,500 ]); %  Y轴固定显示范围
zlim([    0,900 ]); %  Z轴固定显示范围
    
xlabel('X-axis / mm'); %  X轴标识
ylabel('Y-axis / mm'); %  Y轴标识
zlabel('Z-axis / mm'); %  Z轴标识

str = ['LenL1 = ',num2str(LenL1)];      %  在图上显示每条连杆的长度，mm为单位
text(-300,200,900,[str,' mm']);
str = ['LenL2 = ',num2str(LenL2)];
text(-300,200,800,[str,' mm']);
str = ['LenL3 = ',num2str(LenL3)];
text(-300,200,700,[str,' mm']);
str = ['LenL4 = ',num2str(LenL4)];
text(-300,200,600,[str,' mm']);
str = ['LenL5 = ',num2str(LenL5)];
text(-300,200,500,[str,' mm']);
str = ['LenL6 = ',num2str(LenL6)];
text(-300,200,400,[str,' mm']);

text( Br1(1), Br1(2)-50, '1' ); %  在静平台的铰点旁边，显示连杆的序号
text( Br2(1)+50, Br2(2)-50, '2' );
text( Br3(1)+50, Br3(2)-50, '3' );
text( Br4(1)+50, Br4(2)+50, '4' );
text( Br5(1)+50, Br5(2)+50, '5' );
text( Br6(1)-50, Br6(2)-50, '6' );