机器人动力学与控制学习笔记（七）————基于计算力矩法的滑模控制

最新推荐文章于 2024-05-15 21:46:34 发布

Kocurry

最新推荐文章于 2024-05-15 21:46:34 发布

阅读量9.9k

点赞数 10

分类专栏：机器人学文章标签：自然语言处理

本文链接：https://blog.csdn.net/weixin_38452841/article/details/108773003

版权

机器人学专栏收录该内容

20 篇文章

订阅专栏

本文介绍了基于计算力矩法的滑模控制方法，用于机器人机械手臂系统。通过未知惯性参数的估计，设计了一种控制律（7.28），并通过二关节机器人的仿真实例展示了控制效果。控制精度依赖于参数估计的准确性，能有效减少抖振。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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

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

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	1m	1/2m	1/12kg	3kg	1m	2/5kg	0	-7/12	9.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 flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes;
    case 3
        sys=mdlOutputs(t,x,u);
    case {2,4,9}
        sys=[];
    otherwise
        error(['Unhandled flag = ',num2str(flag)]);
end
function [sys,x0,str,ts] = mdlInitializeSizes

global nmn
nmn = 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 nmn
qd1 = 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 flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes;
    case 1
        sys =mdlDerivatives(t,x,u);
    case 3
        sys=mdlOutputs(t,x,u);
    case {2,4,9}
        sys=[ ];
    otherwise
        error(['Unhandled flag = ',num2str(flag)]);
end
function [sys,x0,str,ts] = mdlInitializeSizes

sizes = 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');