一知半解|MATLAB机器人建模与仿真控制(5)

242
0
2020年6月24日 14时44分

前言:续写一知半解系列,在上一篇中相信对大家对GUI的控件操作和与Simulink数据传递比较熟悉了。这篇主要解决下上次留下的问题。主要包括 :(1)GUI界面其他控件说明。包括运行时间,工具箱机器人显示和末端位姿计算显示,利用工具箱逆解函数ikine对末端进行操作。(2)PID参数的调节。

 

1、GUI界面其他控件说明

 

1)GUI显示运行时间,需要一个静态文本和可编辑文本

a、在guide界面拖入静态文本用于运行时间的标识,进行简单设置,一般设置如下。因为静态文本只作为标识,所以这里不需要对其进行回调函数编写。

1

b、在guide界面拖入可编辑文本用于当前时间的运行,进行简单设置如下。

2

注意这里的标签Tag自定义为Time,方便后面对句柄对象进行操作。由于运行时间是在运行GUI就开始显示。所以需要在GUI自动生成的OpeningFcn中编写代码用于时间的触发;并编写函数disptime用于时间显示,如下:

OpeningFcn函数:

function PUMA560_OpeningFcn(hObject, eventdata, handles, varargin)
clc
handles.output = hObject;
%% 运行时间
h=timer;   %定时器
handles.T=h;   %将定时器放到全局变量中
%set(handles.T,'ExecutionMode','singleShot');  %定时器只执行一次,定一次时。
set(handles.T,'ExecutionMode','fixedRate');   %定时器,循环执行,循环定时。
set(handles.T,'Period',1);    %定时器,定时间隔 1秒
set(handles.T,'TimerFcn',{@disptime,handles});  %定时器,定时会触发 TimerFcn 函数
%定时函数(TimerFcn)触发用户自定义的函数(disptime函数)
start(handles.T);   %开启定时器
%% 运行立即显示工具箱机器人模型
axes(handles.plot)%指定显示区域
mdl_puma560   %导入机器人模型
assignin('base','Robot',p560);%将p560赋给基础工作空间中的Robot变量
Robot= evalin('base','Robot');%将基础工作空间中的变量指定到GUI中,方便数据的共享
view(3)      %三维视角
Robot.plot([0 0 0 0 0 0]);%显示机器人构型为[0 0 0 0 0 0]
guidata(hObject, handles);

disptime函数:

function disptime(hObject, eventdata, handles)

% 自定义的函数,将edit控件的内容改成当前时间。

set(handles.Time,'String',datestr(now));   % 将edit控件的内容改成当前时间。

2)GUI显示机器人工具箱模型

机器人显示在坐标区控件上,控件大小调节到合适大小,双击修改打开属性检测器,这里只修改标签Tag为plot(命名自定,最好是有意义的名字,方便操作)。

3

由于一运行GUI机器人模型就显示在坐标区,所以初始的显示代码同样写在了OpeningFcn函数中,在   1)中的OpeningFcn函数中可查。对应代码axes(handles.plot)%指定显示区域

3)末端位姿计算显示

需要控件和运行时间的控件一样,静态文本和可编辑文本,静态文本用于标识,可编辑文本用于计算结果的显示,过程大致如下。

4

如关节1滑块移动后对机器人单关节运动和末端位姿计算显示代码如下【其他同】

function Joint1_Callback(hObject, eventdata, handles)
ModelName = 'TestPuma560';
%get the angle
theta1=get(handles.Joint1,'value');
set(handles.edit1,'string',num2str(theta1));  
set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1));

theta2=get(handles.Joint2,'value');
set(handles.edit2,'string',num2str(theta2));
theta3=get(handles.Joint3,'value');
set(handles.edit3,'string',num2str(theta3));
theta4=get(handles.Joint4,'value');
set(handles.edit4,'string',num2str(theta4));
theta5=get(handles.Joint5,'value');
set(handles.edit5,'string',num2str(theta5));
theta6=get(handles.Joint6,'value');
set(handles.edit6,'string',num2str(theta6));
%Data transmission to simulink
set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2));
set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3));
set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta4));
set_param([ModelName '/Slider Gain5'],'Gain',num2str(theta5));
set_param([ModelName '/Slider Gain6'],'Gain',num2str(theta6));
%Forword kinematics calculation
rpy=zeros(1,3);
        a2 = 0.4318;
        a3 = 0.0203;
        d3 = 0.1501;
        d4 = 0.4318;
t1=theta1;t2=theta2; t3=theta3; 
t4=theta4; t5=theta5; t6=theta6;     
        T_01 = tmat(0, 0, 0, t1);
        T_12 = tmat(90, 0, 0, t2);
        T_23 = tmat(0, a2, d3, t3);
        T_34 = tmat(-90, a3, d4, t4);
        T_45 = tmat(90, 0, 0, t5);
        T_56 = tmat(-90, 0, 0, t6);
 % Each link fram to base frame transformation
        T_02 = T_01*T_12;
        T_03 = T_02*T_23;
        T_04 = T_03*T_34;
        T_05 = T_04*T_45;
        T_06 = T_05*T_56;     
X=T_06(1,4);Y=T_06(2,4);Z=T_06(3,4);%position
R=T_06;
    if abs(abs(R(1,3)) - 1) < eps  % when |R13| == 1
        % singularity
        rpy(1) = 0;  % roll is zero
        if R(1,3) > 0
        rpy(3) = atan2( R(3,2), R(2,2));   % R+Y
        else
             rpy(3) = -atan2( R(2,1), R(3,1));   % R-Y
        end
        rpy(2) = asin(R(1,3));
    else
        rpy(1) = -atan2(R(1,2), R(1,1));
        rpy(3) = -atan2(R(2,3), R(3,3));
        rpy(2) = atan(R(1,3)*cos(rpy(1))/R(1,1));
    end
    RPY=rpy*180/pi;
    Rall=round(RPY(1));Pitch=round(RPY(2));Yaw=round(RPY(3));
    set(handles.X,'string',num2str(X));
    set(handles.Y,'string',num2str(Y));
    set(handles.Z,'string',num2str(Z));
    set(handles.R,'string',num2str(Rall));
    set(handles.P,'string',num2str(Pitch));
    set(handles.Yr,'string',num2str(Yaw));
    q=[theta1 theta2 theta3 theta4 theta5 theta6];
  Robot= evalin('base','Robot');
  handles.plot;
Robot.plot(q*pi/180);
%   Robot.plot3d(q*pi/180, 'view',[-138 8]);
  T=Robot.fkine(q*pi/180);
  hold on
plot3(T.t(1,1),T.t(2,1),T.t(3,1),'.','LineWidth',3,'color','r');

上面的正运动学计算末端位姿调用的tmat函数为:

function T = tmat(alpha, a, d, theta)%改进DH方法,坐标变换过程看出Craig
        alpha = alpha*pi/180;    %Note: alpha is in radians.
        theta = theta*pi/180;    %Note: theta is in radians.
        c = cos(theta);
        s = sin(theta);
        ca = cos(alpha);
        sa = sin(alpha);
        T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];

4)利用工具箱逆解函数ikine对末端进行笛卡尔空间操作

这里选用控件为静态文本和按钮。静态文本依然只作为标识,按钮按下作为末端位置矢量的输入,通过逆解得各关节角度,将关节角度再传递给机器人。

5

按钮的回调函数基本相同,下面以改变机器人末端X为列。

X-按钮回调代码:

function pushbutton3_Callback(hObject, eventdata, handles)
Robot= evalin('base','Robot');
theta1=get(handles.Joint1,'value');
theta2=get(handles.Joint2,'value');
theta3=get(handles.Joint3,'value');
theta4=get(handles.Joint4,'value');
theta5=get(handles.Joint5,'value');
theta6=get(handles.Joint6,'value');
q0=[theta1 theta2 theta3 theta4 theta5 theta6];
T0=Robot.fkine(q0*pi/180);R=[T0.n T0.o  T0.a];%直接调用正运动学函数
numMats = size(R,3);H = zeros(4,4,numMats,'like',R);
H(1:3,1:3,:) = R;
H(4,4,:) = ones(1,1,numMats,'like',R);
H(1:3,4)=T0.t;%SE3 xyzMovement To Homogeneous matrix
Hinit=H;
offset=str2double(get(handles.MoveAccuracy,'string'));%运动的步长,精度
H(1:1 ,4:4)=Hinit(13)+offset;%以当前末端为参考进行末端运动
%Hinit(13)为矩阵第13个位置上的数,在齐次矩阵中为X坐标。
Hmove=H;
Q=Robot.ikine(Hmove);%调用逆运动学函数得到各关节值
handles.plot;
Robot.plot(Q);%显示末端运动
theta1=Q(1)*180/pi;theta2=Q(2)*180/pi;theta3=Q(3)*180/pi;
theta4=Q(4)*180/pi;theta5=Q(5)*180/pi;theta6=Q(6)*180/pi;
%设置滑块变动和对应角度显示
set(handles.Joint1,'value',theta1);set(handles.Joint2,'value',theta2);   
set(handles.Joint3,'value',theta3); set(handles.Joint4,'value',theta4); 
set(handles.Joint5,'value',theta5); set(handles.Joint6,'value',theta6); 
set(handles.edit1,'string',num2str(theta1)); set(handles.edit2,'string',num2str(theta2)); 
set(handles.edit3,'string',num2str(theta3)); set(handles.edit4,'string',num2str(theta4)); 
set(handles.edit5,'string',num2str(theta5)); set(handles.edit6,'string',num2str(theta6)); 
%对Simulink的操作
ModelName = 'TestPuma560';
set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1));
set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2));
set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3));
set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta4));
set_param([ModelName '/Slider Gain5'],'Gain',num2str(theta5));
set_param([ModelName '/Slider Gain6'],'Gain',num2str(theta6));
%显示末端运动轨迹,X为红色
  T=Robot.fkine(Q);
  hold on
plot3(T.t(1,1),T.t(2,1),T.t(3,1),'.','LineWidth',3,'color','r');
X=T.t(1,1);Y=T.t(2,1);Z=T.t(3,1);
    set(handles.X,'string',num2str(X));
    set(handles.Y,'string',num2str(Y));
set(handles.Z,'string',num2str(Z));

对逆运动学的控制在GUI中设置了末端位置矢量的变化步长,控件选择使用一个静态文本作为标识,静态文本拖入修改名字即可。可编辑文本可在运行时修改数据,并在位置按钮回调中获得。上面代码中的offset=str2double(get(handles.MoveAccuracy,’string’));可编辑文本修改如下:

6

效果:(这里只对位置矢量进行控制,若要对姿态进行控制可以添加控件按同样的方式完成)

GIF1

 

2、PID参数的调节

 

双击打开PID控制器,默认参数如下。

8

在这种情况下用一个正弦波模块替换关节1的角度输入Sider Gian模块进行测试。

9

使用Scope示波器对其进行角度跟踪情况如下,很明显存在误差:

10

因此对PID参数进行调节。这里使用PID自动参数调节工具。先运行一次模型,然后双击PID控制模块点击Tune按钮。

11

调节下面的响应时间和鲁棒性(超调量),实线为调节后的响应会自动更新。

12

得到一个较满意的响应后,我们选择update,更新PID参数到simulink中。

13

如下PID参数自动生成之后,点击应用。

14

现在运行模型。可以看到关节1的角度跟踪情况变好了很多,误差已经很小了,说明关节1的PID参数调节就完成了,其他关节类似的方式。

15

到目前为止,我们将一知半解4里面留下的问题基本解决了。

后记:作者能力有限,如果有错误的地方请大家及时的指出。

(1)PID参数调节相关可参考:https://zhuanlan.zhihu.com/p/74131690

                                                          https://zhuanlan.zhihu.com/p/139945063

                                                         书籍:《先进PID控制MATLAB仿真 》刘金琨

(2)非guide方式的GUI设计学习可以参考:https://www.bilibili.com/video/BV1mK411p7c3

(3)GUI设计参考书籍:《MATLAB GUI设计入门与实战》 

(4)机器人工具箱Robotics Toolbox学习可参考:https://www.bilibili.com/video/BV1MK4y1k7je

—以上

发表评论

后才能评论