MATLAB下机器人可视化与控制—simulink篇（3）_人工智能

1、前记：

续写MATLAB下机器人可视化与控制—simulink篇（2），继续完善基于MATLAB/simulink的机器人控制—位置控制。这里主要是PID和GUI部分。PID作为单关节伺服控制部分，GUI作为上位机完成简单的基本运动指令。

2、位置控制：

在预先指定的坐标系上,对机器人末端执行器（end effector）的位置和姿态（方向）的控制。如图所示，末端执行器的位置和姿态是在三维空间描述的，包括三个平移分量和三个旋转分量，它们分别表示末端执行器坐标在参考坐标中的空间位置和方向（姿态）。因此，必须给它指定一个参考坐标，原则上这个参考坐标可以任意设置，但为了规范化和简化计算，通常以机器人的基坐标作为参考坐标。

机器人的位置控制主要有直角坐标和关节坐标两种控制方式。个人理解是直角坐标是逆运动学的位置控制，关节坐标的位置控制是正运动学的控制。

即：直角坐标位置控制：是对机器人末端执行器坐标在参考坐标中的位置和姿态的控制。通常其空间位置主要由腰关节、肩关节和肘关节确定，而姿态（方向）由腕关节的两个或三个自由度确定。通过解逆运动方程，求出对应直角坐标位姿的各关节位移量，然后驱动伺服结构使末端执行器到达指定的目标位置和姿态。

关节坐标位置控制：直接输入关节位移给定值，控制伺服机构

3、根据上图很容易在simulink中建立机器人的位置控制系统—独立关节位置控制。

GUI运动学部分：1)界面包括简单的关节坐标控制（正）和直角坐标控制（逆）。正逆运动部分参考PUMA762中的形式编写。

以下是部分代码：关节1轴运动回调函数–正运动。


function slider1_Callback(hObject, eventdata, handles)

ModelName = 'complete_arm';

a1 = 0;

a2 = 0;

a3 = 104;

a4 = 94;

a5=119;

%get the angle

theta1=get(handles.slider1,'value');

set(handles.edit2,'string',num2str(theta1));

theta2=get(handles.slider2,'value');

set(handles.edit3,'string',num2str(theta2));

theta3=get(handles.slider3,'value');

set(handles.edit4,'string',num2str(theta3));

theta4=get(handles.slider4,'value');

set(handles.edit5,'string',num2str(theta4));

%Data transmission to simulink

set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1));

set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2+180));

set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3+170));

set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4));

 

T1 = [ cosd(theta1) 0   -sind(theta1)    0;

       sind(theta1) 0   -cosd(theta1)    0;

       0            -1       0           486;

       0            0       0            1];

 

T2 = [ cosd(theta2) -sind(theta2)   0   a2*cosd(theta2);

       sind(theta2) cosd(theta2)    0   a2*sind(theta2);

       0            0               1       0;

       0            0               0        1];

   

T3 = [ cosd(theta3) -sind(theta3)   0   a3*cosd(theta3);

       sind(theta3) cosd(theta3)    0   a3*sind(theta3);

       0            0               1     0;

       0            0               0       1];

   

T4 = [ cosd(theta4) -sind(theta4)   0   a4*cosd(theta4);

       sind(theta4) cosd(theta4)    0   a4*sind(theta4);

       0            0               1   0;

       0            0               0     1];

T = T1*T2*T3*T4;

px=T(1,4);

py=T(2,4);

pz=T(3,4);

set(handles.edit6,'string',num2str(px));

set(handles.edit7,'string',num2str(py));

set(handles.edit8,'string',num2str(pz));

逆运动部分：


function INVERSE_Callback(hObject, eventdata, handles)

ModelName = 'complete_arm';

global var;

px=get(handles.slider5,'value');

set(handles.edit9,'string',num2str(px));

py=get(handles.slider6,'value');

set(handles.edit10,'string',num2str(py));

pz=get(handles.slider7,'value');

set(handles.edit11,'string',num2str(pz));

set(handles.edit6,'string',num2str(px));

set(handles.edit7,'string',num2str(py));

set(handles.edit8,'string',num2str(pz));

a1 = 486;

a2 = 0;

a3 = 104;

a4 = 94;

a5=119;

%%rignal

theta1=atan2d(py,px);

theta234= 0; 

d=sqrt(px^2+py^2);%DB = OA

xd=d*cosd(theta1);

yd=d*sind(theta1);

r4=d-a4*cosd(theta234);

z4=pz-a4*sind(theta234);%

s=sqrt((z4-a1)^2+r4^2);

theta3=acosd((s^2-a2^2-a3^2)/(2*a2*a3));

beta=atan2d(a3*sind(theta3),a2+a3*cosd(theta3));

alpha=atan2d(z4-a1,r4);

theta2=alpha+beta;

theta4=theta234-theta2-theta3; 

guidata(hObject,handles);

set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1));

set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2));

set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3));

set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4));

set(handles.edit2,'string',num2str(theta1));

set(handles.edit3,'string',num2str(theta2));

set(handles.edit4,'string',num2str(theta3));

set(handles.edit5,'string',num2str(theta4));