机器人动力学与控制学习笔记（七）

七、基于计算力矩法的滑模控制

计算力矩法是机器人控制中较常用的方法，该方法基于机器人模型中各项的估计值进行控制律的设计。

7.1 系统描述

机器人机械手的模型为：

$\tiny H\left ( q \right )\ddot{q}+C\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )=\tau$ （7.20）

其中 $\tiny H\left ( q \right )$ 为正定质量惯性矩阵， $\tiny C\left ( q,\dot{q} \right )$ 为哥氏力、离心力， $\tiny G\left ( q \right )$ 为重力。

7.2 控制律设计

当不知道机器人的惯性参数时，根据计算力矩法，取控制律为

$\tiny \tau =\hat{H}\left ( q \right )v+\hat{C}\left ( q,\dot{q} \right )\dot{q}+\hat{G}\left ( q \right )$ （7.21）

其中 $\tiny \hat{H}\left ( q \right )$ 、 $\tiny \hat{C}\left ( q,\dot{q} \right )$ 和 $\tiny \hat{G}\left ( q \right )$ 为利用惯性参数估计值 $\tiny \hat{p}$ 计算出的 $\tiny H,C$ 和 $\tiny G$ 的估计值。

则闭环系统方程式（7.20）为

$\tiny H\left ( q \right )\ddot{q}+C\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )=\hat{H}\left ( q \right )v+\hat{C}\left ( q,\dot{q} \right )\dot{q}+\hat{G}\left ( q \right )$ （7.22）

即

$\dpi{200} \tiny \hat{H}\left ( q \right )\ddot{q}=\hat{H}\left ( q \right )v-\begin{bmatrix} \tilde{H}\left ( q \right )\ddot{q}+\tilde{C}\left ( q,\dot{q} \right )\dot{q}+\tilde{G}\left ( q \right )=\hat{H}\left ( q \right )v-Y\left ( q,\dot{q},\ddot{q} \right ) \end{bmatrix}\tilde{p}$ （7.23）

其中 $\tiny \tilde{H}=H-\hat{H},\tilde{C}=C-\hat{C},\tilde{G}=G-\hat{G},\tilde{p}=p-\hat{p}$ 。

若惯性参数的估计值 $\tiny \hat{p}$ 使得 $\tiny \hat{H}\left ( q \right )$ 可逆，则闭环系统方程式（7.23）可写为

$\tiny \ddot{q}=v-\left [ \hat{H}\left ( q \right ) \right ]^{-1}Y\left ( q,\dot{q},\ddot{q} \right )\tilde{p}=v-\varphi \left ( q,\dot{q},\ddot{q} ,\hat{p}\right )\tilde{p}$ （7.24）

定义

$\tiny \varphi \left ( q,\dot{q},\ddot{q} ,\hat{p}\right )\tilde{p}=\tilde{d}$

其中 $\tiny \tilde{d}=\left [ \tilde{d}_{1},\cdots ,\tilde{d}_{n} \right ]^{T},d=\left [ d_{1},\cdots ,d_{n} \right ]^{T}$ 。

取滑动面

$\tiny s=\dot{e}+\Lambda e$

其中 $\tiny e=q_{d}-q,\dot{e}=\dot{q}_{d}-\dot{q},s=\left [ s_{1},\cdots ,s_{n} \right ]^{T}$ ， $\tiny \Lambda$ 为正对角矩阵。则

$\tiny \dot{s }=\ddot{e}+\Lambda \dot{e}=\left ( \ddot{q}_{d}-\ddot{q} \right )+\Lambda \dot{e}=\ddot{q}_{d}-v+d+\Lambda \dot{e}$

取

$\tiny v=\ddot{q}_{d}+\Lambda \dot{e}+d$ （7.25）

式中 $\tiny d$ 为待设计的向量。则

$\tiny \dot{s }=\tilde{d}-d$ （7.26）

选取

$\tiny d=\left ( \tilde{d}+\eta \right )sgn\left ( s \right )$

$\tiny \left \| \tilde{d} \right \|\leqslant \tilde{d}$ （7.27）

其中 $\tiny \eta 0$ 。则

$\tiny \dot{s}s=\left ( \tilde{d}-d \right )s=\tilde{d}s-\tilde{d}sgn\left ( s \right )s-\eta sgn\left (s \right )s\leqslant -\eta \left | s \right |\leqslant 0$

由式（7.21）和式（7.25），得滑模控制律为

$\tiny \tau =\hat{H}\left ( q \right )v+\hat{C}\left ( q,\dot{q} \right )\dot{q}+\hat{G}\left ( q \right )$ （7.28）

其中 $\tiny v=\ddot{q}_{d}+\Lambda \dot{e}+d,d=\left ( \tilde{d}+\eta \right )sgn\left ( s \right )$ 。

由控制律式（7.28）可知，若参数估计值 $\tiny \hat{p }$ 越准确，则 $\tiny \left \|p \right \|$ 越小， $\tiny \tilde{d}$ 越小，滑膜控制产生的抖振越小。

7.3 仿真实例

选二关节机器人力臂系统，其动力学模型为：

$\tiny M\left ( q \right )\ddot{q}+C\left ( q,\dot{q} \right )\dot{q}+G\left ( q \right )+F\left ( \dot{q} \right )+\tau _{d}=\tau$ 其中 $\tiny q=\begin{bmatrix} q_{1} & q_{2} \end{bmatrix}^{T}$ ， $\tiny \tau =\begin{bmatrix} \tau _{1} & \tau _{2} \end{bmatrix}^{T}$ 。取

$\tiny M\left ( q \right )=\begin{bmatrix} \alpha +2\varepsilon cosq_{2} +2\eta sinq_{2}&\beta +\varepsilon cosq_{2}+\eta sinq_{2} \\ \beta +\varepsilon cosq_{2}\eta sinq_{2}& \beta \end{bmatrix}$

$\tiny C\left ( q,\dot{q} \right )=\begin{bmatrix} \left ( -2\varepsilon sinq_{2}+2\eta cosq_{2} \right )\dot{q}_{2} &\left ( -\varepsilon sinq_{2}+\eta cosq_{2} \right )\dot{q}_{2} \\ \left ( \varepsilon sinq_{2}-\eta cosq_{2} \right )\dot{q}_{1}& 0 \end{bmatrix}$

$\tiny G\left ( q \right )=\begin{bmatrix} \varepsilon e_{2}cos\left ( q_{1}+q_{2} \right )+\eta e_{2}sin\left ( q_{1}+q_{2} \right )+\left ( \alpha -\beta +e_{1} \right )e_{2}cosq_{1}\\ \varepsilon e_{2}cos\left ( q_{1}+q_{2} \right )+\eta e_{2}sin\left ( q_{1}+q_{2} \right ) \end{bmatrix}$

其中 $\tiny \alpha =I_{1}+m_{1}l_{e1}^{2}+I_{e}+m_{e}l_{ce}^{2}+m_{e}l_{1}^{2},\beta =I_{e}+m_{e}l_{ce}^{2},\varepsilon =m_{e}l_{1}l_{ce}cos\delta _{e},\eta =m_{e}l_{1}l_{ce}sin\delta _{e}$ 。

机械臂的实际物理参数值见表7-1。

表7-1 双机械臂物理参数

$\tiny m_{1}$ $\tiny l_{1}$ $\tiny l_{e1}$ $\tiny I_{1}$ $\tiny m_{e}$ $\tiny l_{ce}$ $\tiny I_{e}$ $\tiny \delta _{e}$ $\tiny e_{1}$ $\tiny e_{2}$

1kg

1m1/2m1/12kg3kg1m2/5kg0-7/129.81

采用滑模控制律式（7.28），取位置指令分别为 $\tiny q_{d1}=cos\left ( \pi t \right ),q_{d2}=sin\left ( \pi t \right ),\hat{H}=0.6H,\hat{C}=0.6C,\hat{G}=0.6G,\tilde{d}=30,\eta =0.10,\Lambda =\begin{bmatrix} 25 & 0\\ 0& 25 \end{bmatrix}$ 。仿真结果如图7-9和7-10所示。

图7.9 双力臂位置跟踪

图7.10 双力臂控制输入

仿真程序:

Simulink主程序：chap7_4sim.mdl

控制律子程序：chap7_4ctrl.m

function [sys, x0,str,ts]= chap7_1ctrl(t,x,u,flag)switch flagcase 0[sys,x0,str,ts]=mdlInitializeSizes;case 3sys=mdlOutputs(t,x,u);case {2,4,9}sys=[];otherwiseerror(['Unhandled flag = ',num2str(flag)]);endfunction [sys,x0,str,ts] = mdlInitializeSizesglobal nmnnmn = 25*eye(2);sizes = simsizes;sizes.NumContStates = 0;sizes.NumDiscStates = 0;sizes.NumOutputs = 2;sizes.NumInputs = 6;sizes.DirFeedthrough = 1;sizes.NumSampleTimes = 1;sys = simsizes(sizes);x0 =[];str =[];ts = [0 0];function sys = mdlOutputs(t,x,u)global nmnqd1 = u(1);dqd1 = -pi*sin(pi*t);ddqd1 = -pi^2*cos(pi*t);qd2 = u(2);dqd2 = pi*cos(pi*t);ddqd2 = -pi^2*sin(pi*t)ddqd = [ddqd1;ddqd2];dqd = [dqd1;dqd2];ddqd = [ddqd1;ddqd2];q1=u(3);dq1=u(4);q2=u(5);dq2=u(6);dq=[dq1;dq2];e1 = qd1-q1;e2 = qd2-q2;e=[e1;e2];de1 = dqd1 - dq1;de2 = dqd2 - dq2;de = [de1;de2];alfa = 6.7;beta = 3.4;epc = 3.0;eta = 0;m1 = 1;l1 = 1;lc1 = 1/2;I1 = 1/12;g = 9.8;e1 = m1*l1*lc1-I1-m1*l1^2;e2 = g/l1;M = [alfa+2*epc*cos(q2)+2*eta*sin(q2),beta+epc*cos(q2)+eta*sin(q2);beta+epc*cos(q2)+eta*sin(q2),beta];C = [(-2*epc*sin(q2)+2*eta*cos(q2))*dq2,(-epc*sin(q2)+eta*cos(q2))*dq2;(epc*sin(q2) - eta*cos(q2))*dq1,0];G = [ epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)+(alfa-beta+e1)*e2*cos(q1);epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)];M0 = 0.6*M;C0 = 0.6*C;G0 = 0.6*G;s = de+nmn*e;d_up = 30;xite = 0.10;d = (d_up+xite)*sign(s);v = ddqd +nmn*de+d;tol = M0*v+C0*dq+G0;sys(1) =tol(1);sys(2) =tol(2);

被控对象子程序：chap7_4plant.m

function [sys, x0,str,ts]= chap7_1plant(t,x,u,flag)switch flagcase 0[sys,x0,str,ts]=mdlInitializeSizes;case 1sys =mdlDerivatives(t,x,u);case 3sys=mdlOutputs(t,x,u);case {2,4,9}sys=[ ];otherwiseerror(['Unhandled flag = ',num2str(flag)]);endfunction [sys,x0,str,ts] = mdlInitializeSizessizes = simsizes;sizes.NumContStates = 4;sizes.NumDiscStates = 0;sizes.NumOutputs = 4;sizes.NumInputs = 2;sizes.DirFeedthrough = 0;sizes.NumSampleTimes = 0;sys = simsizes(sizes);x0 =[0;0;0;0];str =[];ts = [];function sys = mdlDerivatives(t,x,u)q1 = x(1);dq1 = x(2);q2 = x(3);dq2 = x(4);dq = [dq1;dq2];% The model is given by Slotine and Weiping Li(MIT 1987)alfa = 6.7;beta = 3.4;epc = 3.0;eta = 0;m1 = 1;l1 = 1;lc1 = 1/2; I1=1/12;g = 9.8;e1 = m1*l1*lc1-I1-m1*l1^2;e2 = g/l1;M = [alfa+2*epc*cos(q2)+2*eta*sin(q2),beta+epc*cos(q2)+eta*sin(q2);beta+epc*cos(q2)+eta*sin(q2),beta];C = [(-2*epc*sin(q2)+2*eta*cos(q2))*dq2,(-epc*sin(q2)+eta*cos(q2))*dq2;(epc*sin(q2) - eta*cos(q2))*dq1,0];G = [ epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)+(alfa-beta+e1)*e2*cos(q1);epc*e2*cos(q1+q2)+eta*e2*sin(q1+q2)];tol(1)=u(1);tol(2)=u(2);ddq = inv( M )*( tol'-C*dq-G );sys(1) =x(2);sys(2) =ddq(1);sys(3) =x(4);sys(4) =ddq(2);function sys = mdlOutputs(t,x,u)sys(1) = x(1);sys(2) = x(2);sys(3) = x(3);sys(4) = x(4);

绘图子程序：chap7_4plot.m

close all;t = out.t.Data;y1 = out.y1.Data;y2 = out.y2.Data; tol = out.tol.Data;figure(1);subplot(211);plot(t,y1(:,1),'r',t,y1(:,2),'b','linewidth',2);xlabel('time(s)');ylabel('Angle tracking of joint 1');subplot(212);plot(t,y2(:,1),'r',t,y2(:,2),'b','linewidth',2);xlabel('time(s)');ylabel('Angle tracking of joint 2');figure(2);subplot(211);plot(t,tol(:,1),'r','linewidth',2);xlabel('time(s)');ylabel('Control input 1');subplot(212);plot(t,tol(:,2),'r','linewidth',2);xlabel('time(s)');ylabel('Control input 2');

云奕文章网

机器人动力学与控制学习笔记（七）

相关推荐：