基于matlab 的串并联机构运动学求解

在现代工业和科学领域中，机器人技术的发展已经取得了巨大的进步，为生产和研究提供了许多便利。其中，串并联机器人是一种广泛应用于各种领域的机器人系统。它具有高精度、高刚度和高可靠性的特点，被广泛应用于飞行模拟器、手术机器人和运动仿真等领域。

然而，要使串并联机器人能够实现复杂的运动任务，我们需要了解其运动学逆解。运动学逆解是指根据机器人末端执行器的位置和姿态，计算出各个关节的角度和长度。这对于控制机器人的运动非常重要，因为它可以帮助我们确定机器人各个关节的运动范围和工作空间。

要理解六自由度串并联机器人的运动学逆解，我们首先需要了解其结构和工作原理。串并联机器人机器人由一个固定底座和两个可移动平台组成，底部平台平台上安装有四个执行器，每个执行器由一个伺服电机和一个球铰链机构组成。这种结构使得机器人可以在四个自由度上进行运动，包括平移和旋转。

为了计算出机器人的运动学逆解，我们需要使用一些数学模型和算法。其中最常用的方法是使用正运动学方程和雅可比矩阵。正运动学方程可以将关节角度和长度与末端执行器的位置和姿态相联系，而雅可比矩阵则可以帮助我们计算出关节角度和长度的变化对末端执行器位置和姿态的影响。

在实际应用中，我们通常会使用数值方法来求解运动学逆解。这些方法包括牛顿-拉夫逊法、雅可比转置法和雅可比伪逆法等。这些方法可以通过迭代计算来逐步逼近真实解，从而得到机器人的关节角度和长度。

然而，串并联机器人的运动学逆解并不是一个简单的问题。由于机器人的结构复杂性和运动学方程的非线性性，求解过程可能会非常复杂和耗时。因此，研究人员一直在努力寻找更高效和精确的算法来解决这个问题。

总结起来，串并联机器人的运动学逆解是一个复杂而关键的问题。它对于机器人的运动控制和路径规划至关重要。虽然目前仍存在一些挑战和困难，但随着技术的不断进步和研究的深入，相信我们能够找到更好的解决方案，推动机器人技术的发展和应用。

下面是部分代码：

clc

clear all

tic

%-----------------------初始位置的位姿-------------------------------------

%-------------------------静平台的位恣-------------------------------------

X =0; % 相对初始位置运动的坐标

Y =0;

Z = 0;

%动平台1相对于定平台的坐标

X_01 = 0;

Y_01 = 0;

Z_01 = 450;

%动平台2相对于动平台1的坐标

X_12 = 0;

Y_12 = 0;

Z_12 = 400;

% 动平台1相对静平台的恣态

YAW01 = 15;

PITCH01 = 0;

ROLL01 = 0;

% 动平台1相对静平台的恣态

YAW12 = 0;

PITCH12 = 20;

ROLL12 = 45;

%--------------------------------位姿计算器--------------------------------

TransM01 = rotz(YAW01) * roty(PITCH01) * rotx(ROLL01);% XYZ旋转矩阵

TransM12 = rotz(YAW12) * roty(PITCH12) * rotx(ROLL12);% XYZ旋转矩阵

%-----------------------------动平台坐标-----------------------------

P01 = [X+X_01; Y+Y_01; Z+Z_01];%

P12 = [X+X_12; Y+Y_12; Z+Z_12];%

P02 = P01 + P12;

%----------------------平台的基本尺寸-----------------------------

%---底座---

R = 600;

r1 = 450;

r2 = 400;

r3 = 350;

%各个关节铰点的位置角度

bottom01_angle = 0;

bottom02_angle = 90;

bottom03_angle = 180;

bottom04_angle = -90;

top01_angle = 0;

top02_angle = 90;

top03_angle = 180;

top04_angle = -90;

down11_angle = -45;

down12_angle = 135;

%down13_angle = -105;

up11_angle = -45;

up12_angle = 135;

%up13_angle = -135;

%实际铰点安装位置

%静平台底部

bottomR1 = [ R*cosd( bottom01_angle );R*sind( bottom01_angle );0];

bottomR2 = [ R*cosd( bottom02_angle );R*sind( bottom02_angle );0];

bottomR3 = [ R*cosd( bottom03_angle );R*sind( bottom03_angle );0];

bottomR4 = [ R*cosd( bottom04_angle );R*sind( bottom04_angle );0];

%第一级动平台与静平台链接

topr1 = [ r1*cosd( top01_angle );r1*sind( top01_angle );0];

topr2 = [ r1*cosd( top02_angle );r1*sind( top02_angle );0];

topr3 = [ r1*cosd( top03_angle );r1*sind( top03_angle );0];

topr4 = [ r1*cosd( top04_angle );r1*sind( top04_angle);0];

%第一级动平台与第二级动平台链接

downr1 = [ r2*cosd( down11_angle );r2*sind( down11_angle );0];

downr2 = [ r2*cosd( down12_angle );r2*sind( down12_angle );0];

%downr3 = [ r2*cosd( down13_angle );r2*sind( down13_angle );0];

downr3 = [0;0;0];

%第二级动平台链接

upr1 = [ r3*cosd( up11_angle );r3*sind( up11_angle );0];

upr2 = [ r3*cosd( up12_angle );r3*sind( up12_angle );0];

%upr3 = [ r3*cosd( up13_angle );r3*sind( up13_angle );0];

upr3 = [0;0;0];

%-----------一级动平台的四个铰点，在静平台坐标系中的位置矢量---------------------

Topr1 = TransM01 * topr1 + P01;

Topr2 = TransM01 * topr2+ P01;

Topr3 = TransM01 * topr3 + P01;

Topr4 = TransM01 * topr4 + P01;

%------------第一级动平台与静平台连接的四个关节与第二级静平台连接的3个关节-----------

Downr1 = TransM01 * downr1+ P01;

Downr2 = TransM01 * downr2+ P01;

Downr3 = TransM01 * downr3+ P01;

%-----------二级动平台的三个铰点，在一级平台坐标系中的位置矢量---------------------

Upr1 = TransM12* TransM01* upr1 + P02;

Upr2 = TransM12* TransM01* upr2 + P02;

Upr3 = TransM12* TransM01* upr3 + P02;

%-------------------杆件-第一层的计算------------------

L11 = Topr1 - bottomR1;

L12 = Topr2 - bottomR2;

L13 = Topr3 - bottomR3;

L14 = Topr4 - bottomR4;

%-------------------杆件-第二层的计算------------------

L21 = Upr1 - Downr1;

L22 = Upr2 - Downr2;

L23 = Upr3 - Downr3;

%-----------球杆长----------------

LenL11 = norm(L11);

LenL12 = norm(L12);

LenL13 = norm(L13);

LenL14 = norm(L14);

LenL21 = norm(L21);

LenL22 = norm(L22);

LenL23 = norm(L23);

toc %  显示计算时间

%-------------------------画出3D图-----------------------------------------

h=gca; %  把原来的图删掉，免得新的图和旧的图相互重叠

delete(h);

figure(1); %  把 hold on 开启

hold on;

view([1,1,1]); %  设定3D图的观察视角

axis equal; %  XYZ轴显示比例相等

plot3( [0,200], [0,0],[0,0],'-b', 'LineWidth', 1 );% 在图中画出X轴，方便观察

plot3( 200, 0, 0, '>b' );

plot3( [0,0],[0,200], [0,0], '-r', 'LineWidth', 1 );% 在图中画出Y轴，方便观察

plot3( 0, 200, 0, '>r' );

plot3( [0,0], [0,0], [0,200], '-k', 'LineWidth', 1 );% 在图中画出Z轴，方便观察

plot3( 0, 0, 200, '>k' );

PBx0 = TransM12*TransM01 * [ 0; 0; 0 ] + P01 + P12;

PBx1 = TransM12*TransM01 * [ 0+100; 0; 0 ] + P01 + P12;

PBy0 = TransM12*TransM01 * [ 0;0; 0 ] + P01 + P12;

PBy1 = TransM12*TransM01 * [ 0; 0+100; 0] + P01 + P12;

PBz0 = TransM12*TransM01 * [ 0; 0;0] + P01 + P12;

PBz1 = TransM12*TransM01 * [ 0; 0;0+100 ] + P01 + P12;

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( [ bottomR1(1), bottomR2(1), bottomR3(1), bottomR4(1)], [ bottomR1(2), bottomR2(2), bottomR3(2), bottomR4(2)], [ bottomR1(3), bottomR2(3), bottomR3(3), bottomR4(3)], 'c' );

patch( [ Topr1(1), Topr2(1), Topr3(1), Topr4(1),Downr1(1), Downr2(1), Downr3(1)],[ Topr1(2), Topr2(2), Topr3(2),Topr4(2),Downr1(2), Downr2(2), Downr3(2)],[ Topr1(3), Topr2(3), Topr3(3), Topr4(3),Downr1(3), Downr2(3), Downr3(3)], 'r' );

patch( [Upr1(1),Upr2(1),Upr3(1)], [Upr1(2),Upr2(2),Upr3(2)], [Upr1(3),Upr2(3),Upr3(3)], 'y');

%  ------------画出第一级动平台上的7个铰点-----------------

plot3( Topr1(1),Topr1(2),Topr1(3), '.m', 'MarkerSize', 30);

plot3( Topr2(1),Topr2(2),Topr2(3), '.m', 'MarkerSize', 30);

plot3( Topr3(1),Topr3(2),Topr3(3), '.m', 'MarkerSize', 30);

plot3( Topr4(1),Topr4(2),Topr4(3), '.m', 'MarkerSize', 30);

plot3( Downr1(1),Downr1(2),Downr1(3), '.m', 'MarkerSize', 30);

plot3( Downr2(1),Downr2(2),Downr2(3), '.m', 'MarkerSize', 30);

plot3( Downr3(1),Downr3(2),Downr3(3), '.m', 'MarkerSize', 30);

%-------------------画出第二级动平台上的3个铰点---------------------

plot3( Upr1(1),Upr1(2),Upr1(3), '.m', 'MarkerSize', 30);

plot3( Upr2(1),Upr2(2),Upr2(3), '.m', 'MarkerSize', 30);

plot3( Upr3(1),Upr3(2),Upr3(3), '.m', 'MarkerSize', 30);

plot3( P02(1), P02(2), P02(3), '.k', 'MarkerSize',10); %  画出动、静平台的圆心，方便观察

plot3( P01(1), P01(2), P01(3), '.k', 'MarkerSize',10); %  画出动、静平台的圆心，方便观察

plot3( 0, 0, 0, '.k', 'MarkerSize',10);

plot3( [Topr1(1),bottomR1(1)], [Topr1(2),bottomR1(2)], [Topr1(3),bottomR1(3)], '-k', 'LineWidth', 2 ); % 动、静平台的铰点连接，也就是连杆

plot3( [Topr2(1),bottomR2(1)], [Topr2(2),bottomR2(2)], [Topr2(3),bottomR2(3)], '-k', 'LineWidth', 2 );

plot3( [Topr3(1),bottomR3(1)], [Topr3(2),bottomR1(2)], [Topr3(3),bottomR1(3)], '-k', 'LineWidth', 2 );

plot3( [Topr4(1),bottomR4(1)], [Topr4(2),bottomR4(2)], [Topr4(3),bottomR4(3)], '-k', 'LineWidth', 2 );

plot3( [Downr1(1),Upr1(1)], [Downr1(2),Upr1(2)], [Downr1(3),Upr1(3)], '-k', 'LineWidth', 2 );

plot3( [Downr2(1),Upr2(1)], [Downr2(2),Upr2(2)], [Downr2(3),Upr2(3)], '-k', 'LineWidth', 2 );

plot3( [Downr3(1),Upr3(1)], [Downr3(2),Upr3(2)], [Downr3(3),Upr3(3)], '-k', 'LineWidth', 2 );

%-------------------------设置3D图的一些格式，方便观察---------------------

xlim([ -1000,1000 ]); %  X轴固定显示范围

ylim([ -1000,1000 ]);%  Y轴固定显示范围

zlim([ 0,1200 ]); %  Z轴固定显示范围

xlabel('X-axis / mm');%  X轴标识

ylabel('Y-axis / mm');%  Y轴标识

zlabel('Z-axis / mm');%  Z轴标识

str = ['LenL11 = ',num2str(LenL11)];%  在图上显示每条连杆的长度，mm为单位

text(-400,300,1100,[str,' mm']);

str = ['LenL12 = ',num2str(LenL12)];

text(-400,300,1250,[str,' mm']);

str = ['LenL13 = ',num2str(LenL13)];

text(-400,300,1400,[str,' mm']);

str = ['LenL14 = ',num2str(LenL14)];

text(-400,300,1550,[str,' mm']);

str = ['LenL21 = ',num2str(LenL21)];

text(-400,300,1700,[str,' mm']);

str = ['LenL22 = ',num2str(LenL22)];

text(-400,300,1850,[str,' mm']);

str = ['LenL23 = ',num2str(LenL23)];

text(-400,300,2000,[str,' mm']);

%  在静平台的铰点旁边，显示连杆的序号

text( Topr1(1), bottomR1(2)-50,'L11' );

text( Topr2(1)+50, bottomR2(2)-50,'L12' );

text( Topr3(1)+50, bottomR3(2)-50,'L13' );

text( Topr4(1)+50, bottomR4(2)+50,'L14' );

text( Downr1(1)+650, Upr1(2)+650,'L21' );

text( Downr2(1)-650, Upr2(2)-650,'L22' );

text( Downr3(1)-650, Upr3(2)-650,'L23' );

显示效果图：