三自由度机械臂运动学分析+仿真

建立坐标系

1正运动学分析

采用标准的D-h法进行机械腿模型分析：

将连杆坐标系的原点建立在连杆的关节连杆末端。

一、建立D-H连杆坐标系的原则

1.Z,轴沿关节轴i+1的轴向。

2．原点O为Zi.,与Zi轴的交点或其公垂线与关节轴Zi的交点。3.Xi轴沿Zi与Zi轴的公垂线方向，由关节轴i指向关节轴i+1。4.Yi轴按照右手定则确定。

二、D-H参数的含义

1.连杆长度ai:定义为从Zi-1移动到Zi的距离，沿Xi轴指向为正。其实质为公垂线的长度。

2.连杆转角αi:定义为从Z i-1旋转到zi的角度，绕Xi轴正向旋转为正。

3.关节偏距di,:定义为从X i-1移动到Xi的距离，沿Z i-1轴指向为正。其实质为两条公垂线之间的距离。

4.关节角0i,:定义为从X i-1旋转到Xi的角度，绕Zi轴正向旋转为正。

三、相邻坐标系之间的齐次变换矩阵

D-h表如下

（2）通过（1）求解出机器人各位姿变换矩阵后，求解机器人手臂变换矩阵。通过matlab 计算，写出机器人末端位置。

正运动学分析

根据D-H表规定得到如下变换矩阵为：

由此可得机器人相邻两关节位姿分别为：

所以，坐标系{4}相对于基坐标系的变换矩阵为：

相对于基坐标系的旋转矩阵

                              

位置矢量

根据DH参数求解变换矩阵的函数trans：

%输入JD，即6个关节变量的值，求解正运动方程

function [ T ] = trans( theta, d, a, alpha )

T =[

     cos(theta),   -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);

     sin(theta),   cos(theta)*cos(alpha),  -cos(theta)*sin(alpha), a*sin(theta);

     0,            sin(alpha),              cos(alpha),            d;

     0,                       0,                      0,           1            ];

end

正向运动学：

function [ T06 ] = fkine(JD)

%JD为输入的1*6的数组，代表6个关节变量

% trans( theta, d, a, alpha )

if nargin<2; end  

T01 =trans( JD(1), 0,  0.292,   pi/2 );

T12 =trans( JD(2)+pi/2, 0,    0,   pi/2);

T23 =trans( JD(3), 0.328,    0, 0);

T06 =T01*T12*T23;

End

1.%2.%3 机器人逆运动学分析

机器人逆运动学问题采用矩阵逆乘方法进行求解，如下所示：

1. 求关节角：

（1）

对上述方程除以得到如下：

（2）

则

2. 求关节角：

由方程式两端的第一行第三列对应相等，第二行第三列对应相等可得：

通过上述公式运算，整理可得

3. 求关节角

对式子两边连续乘以A1-A2的逆矩阵，可得

让上述公式中第一行第三列对应相等，第二行第三列对应相等可得：

3机器人模型建立

所设计的机器人由多个连杆机构组成，其关节类型包括旋转关节和移动关节两种。利用Matlab中机器人仿真工具箱Robotics Tool中Link和SerialLink两个函数可建立机器人模型[] 。其中，函数表达式如下：

式中，theta为关节变量；d为偏置距离；alpha为扭转角；a为连杆长度；sigma表示关节类型（0为旋转关节，1为移动关节）；前四个参数分别对应表1中的，，，。

具体程序编制如下：

Clear %情况matlab软件的数据缓存，避免影响本次运行

Clc %清空运行窗口的数据

L(1) = Link(  'd',    0,  'a' , 0.292  , 'alpha',   pi/2 ,'offset',0);

L(2) = Link(  'd',     0 ,  'a' ,0 , 'alpha',    pi/2, 'offset',pi/2);

L(3) = Link( 'd',  0.328,  'a' , 0 , 'alpha',0 ,'offset',0);%

robot = SerialLink(L, 'name' , '机械臂');   %建立三自由度模型

robot.teach;       %画出模型并进行调控

robot.display();  %显示建立的机器人的DH参数

运行上述程序，即可得到机器人模型如图 3-3

图 33机器臂模型

运动空间分析

依据机器人三个自由度的运动范围，采用三自由度机器人模型进行计算。采用蒙特卡洛方法，通过Matlab编程，程序如下：

L(1) = Link(  'd',    0,  'a' , 0.292  , 'alpha',   0 ,'offset',0);

L(2) = Link(  'd',     0 ,  'a' ,0 , 'alpha',    pi/2, 'offset',pi/2);

L(3) = Link( 'd',  0.328,  'a' , 0 , 'alpha',0 ,'offset',0);

robot = SerialLink(L, 'name' , '机械臂');   %建立四自由度模型

% RRP.plotopt = {'workspace',[-10,10,-10,10,-10,10],'tilesize',10};  %设置模型空间大小和地砖大小

robot.teach;       %画出模型并进行调控

robot.display();  %显示建立的机器人的DH参数

%%

hold on;

N=10000;    %随机次数

%关节角度限制

limitmax_1 = 0;

limitmin_1 = 90;

limitmax_2 = 0;

limitmin_2 = 120;

limitmax_3 = -45;

limitmin_3 = 45;

theta1=(limitmin_1+(limitmax_1-limitmin_1)*rand(N,1))*pi/180; %关节1限制

theta2=(limitmin_2+(limitmax_2-limitmin_2)*rand(N,1))*pi/180; %关节2限制

theta3=(limitmin_3+(limitmax_3-limitmin_3)*rand(N,1))*pi/180; %关节3限制

qq=[theta1,theta2,theta3];

Mricx=robot.fkine(qq);

Mricx=transl(Mricx);

x=reshape(Mricx(:,1),N,1);

y=reshape(Mricx(:,2),N,1);

z=reshape(Mricx(:,3),N,1);

plot3(x,y,z,'b.','MarkerSize',0.5);%画出落点

hold on;

下载咨询链接

三维模型+word+仿真源代码文件下载见收费内容