本文给出滑模控制在水下机器人运动控制方面的两个实例，主要参考JJE Slotine的著作Applied Nonlinear Control。

基础知识

流形的选择与证明

使\(\widetilde{x} = x - x_d\)作为变量\(x\)的跟踪误差，并使 \[\widetilde{x}=x-x_d= \left[ \begin{matrix} \widetilde{x} & \dot{\widetilde{x}}& \cdots & \widetilde{x}^{(n-1)} \\ \end{matrix} \right]^T\] 定义一个在状态空间\(R^{(n)}\)中时变的平面\(s(x;t)=0\)，其中： \[s(x;t) = (\frac{d}{dt}+\lambda)^{n-1}\widetilde{x}\] 式中的\(\lambda\)是一个正常数。若\(x_d(0)=x(0)\)，那么使得\(x \equiv x_d\)的跟踪问题等价于对任意\(t > 0\)，状态变量保持在流形\(s(t)\)上。即\(n\)阶向量\(x_d\)的跟随问题可以被简化为使标量\(s\)保持为0。且系统一旦到达流形，系统就将保持在流形上。
以一流形为例： \[s = \dot{\widetilde{x}}+\lambda\widetilde{x}\] 系统到达流形上后，由\(s\equiv0\)，则有 \[\dot{\widetilde{x}} = -\lambda \widetilde{x}\] 上式表明系统将渐进趋近零点。
另外，标量\(s\)的界可以转化为对跟踪误差\(\widetilde{x}\)的界，所以可认为标量\(s\)提供了一个对跟踪性能的可靠量度。特别的，假设\(\widetilde{x}(0)=0\)，则有： \[ \forall t \geq 0, |s(t)| \leq \Phi \quad \Longrightarrow \quad \forall t \geq 0,|\widetilde{x}^{(i)}(t)| \leq (2\lambda)^i \varepsilon \qquad \qquad \qquad i=0,\cdots, n-1 \]
证明： \[y_1(t) = \int_0^t e^{-\lambda(t-T)}s(T)dT\] 由\(|s|\leq \Phi\)，可得： \[|y_1 (t)|\leq \Phi \int_0^t e^{-\lambda(t-T)}dT = (\frac{\Phi}{\lambda})(1-e^{-\lambda t}) \leq \frac{\Phi}{\lambda}\] 依次类推，可以得到： \[|\widetilde{x}| \leq \frac{\Phi}{\lambda^{n-1}}=\varepsilon \] 用类似的方式可以得到\(\widetilde{x}^{(i)}\)与\(s\)的关系。假定标量\(s\)通过了\(n-i-1\)个低通滤波器得到了变量\(v_1\)，则按上述证明可知： \[|v_1| \leq \frac{\Phi}{\lambda^{n-i-1}} \] 构造滤波器\(G_f\)：\(\frac{r}{r+\lambda}\)，\(v_1\)经过一次\(G_f\)得到的结果\(v_2\)。上述过程可记为： \[v_2 (t) = v_1 (t) - v_1 (t)\lambda \int_0^t e^{-\lambda(t-T)}dT\] 由此可得： \[v_2 (t) = (1+\frac{\lambda}{\lambda}) v_1 (t) \] 经过\(i\)次\(G_f\)，将得到\(\widetilde{x}^*(i)\)，因此联立两式可得： \[|\widetilde{x^i}| \leq (\frac{\Phi}{\lambda^{n-1-i}})(1+\frac{\lambda}{\lambda})^i = (2\lambda)^i \varepsilon \]

有限时间到达流形的证明

条件： \[\frac{1}{2}\frac{d}{dt} s^2 \leq -\eta |s| \] 其中\(\eta\)是一个严格大于零的常数。满足以上条件则表明系统将在有限时间到达流形。 证明：
令: \[v = s^2 (s \neq 0)\] 则： \[\dot{v} = 2s\dot{s} < -2\eta |s| = -2\eta\sqrt{v}\] 等价于： \[\frac{dv}{\sqrt{v}} < -2\eta dt\] 对上式积分可得： \[\int_{v(0)}^{v(t)}\frac{dv}{v} < -2\eta \int_0^t dt\] 结果为： \[\sqrt{v(t)}-\sqrt{v(0)} < -\eta t\] 由上式可得： \[0 < \sqrt{v(t)} < \sqrt{v(0)}- \eta t\] 由此可以推导出： \[\sqrt{v(0)}-\eta t > 0 \Longrightarrow t < \frac{\sqrt{v(0)}}{\eta}\]

被控对象模型

单自由度艏摇模型

水下机器人的运动模型可用以下形式表述： \[\ddot{x} = -\frac{c}{m}\dot{x}|\dot{x}| + \frac{1}{m}u\] 对航向控制来说，\(u\)表示推进器的输入扭矩，\(x\)表示当前指向的角度。 ## 平面三自由度模型在进行平面轨迹跟踪控制系统设计之前，首先要将前文所述的单自由度模型扩展到平面三自由度，如下图：

图中的\(p_1,p_2\)代表推进器，平面内大地坐标和体坐标的速度转换关系如下： \[ \left\{ \begin{array}{l} \dot{x} = v_x cos(\theta) - v_y sin(\theta) \\ \dot{y} = v_x sin(\theta) + v_y cos(\theta) \\ \dot{\theta} = \omega \end{array} \right. \label{cons:cvtnb} \] 式中的坐标\((x,y)\)表示水下机器人质心在大地坐标系下的位置，\(\theta\)是相对于大地坐标系机器人的转向角。\(v_x,v_y,\omega\)分别是其体坐标系下的纵荡，横荡与艏摇速度。平面内机器人运动除有前文已导出的模型，还有： \[ \left \{ \begin{array}{l} M_{x}\dot{v_x} - M_{y}v_y \omega + c_x v_x |v_x| = u \\ M_{y}\dot{v_y} + M_{x}v_x \omega + c_y v_y |v_y| = 0 \\ \end{array} \right. \label{cons:3dof} \] 式中，\(M_{x}\)与\(M_{y}\)都包括刚体质量和附加质量，尽管在纵荡方向和垂荡方向水下机器人质量没有变化，但二者附加质量的差异导致了\(M_{x} \neq M_{y}\)。其纵荡和横荡的阻尼也存在差异。

控制律推导

由改写的水下机器人动力学方程，二阶系统可描述为： \[ \ddot{x} = f + bu \] 式中\(f,b\)均不能精确测得，但是均已知参数摄动范围。其中\(b\)的范围是： \[ 0 < b_{min} \leq b \leq b_{max} \] 且认为\(b\)的最优估计是: \[ \widehat{b} = \sqrt{b_{min} b_{max}} \] 所以\(b\)的边界条件也可写作： \[ \beta^{-1} \leq \frac{\widehat{b}}{b} \leq \beta \] \(f(x;t)\)的摄动可认为被已知函数\(F(x;t)\)限制，且设对动态的\(f\)最优估计为\(\widehat{f}\)： \[ |\widehat{f}-f|\leq F \] 对输入跟踪问题\(x(t) \equiv x_d(t)\)，定义一个流形： \[ s = (\frac{d}{dt}+\lambda)\widetilde{x} = \dot{\widetilde{x}}+\lambda \widetilde{x} \label{cons:yawsf} \] 可以推出： \[ \dot{s} = \ddot{\widetilde{x}}+\lambda \dot{\widetilde{x}} = \ddot{x}-\ddot{x}_d+\lambda \dot{\widetilde{x}} \] 又由\(\ddot{x} = f + bu\)，则： \[ \dot{s} = f + bu - \ddot{x}_d + \lambda \dot{\widetilde{x}} \] 因此，使得\(\dot{s}=0\)的最优估计\(\widehat{u}\)为： \[ \widehat{u} = \widehat{b}^{-1}(-\widehat{f}+\ddot{x}_d-\lambda\dot{\widetilde{x}}) \label{cons:estu} \] 为满足滑动模态存在条件，在输入\(\widehat{u}\)上需要叠加一个非线性输入。记为： \[ u = \widehat{b}^{-1}[\widehat{u}-k sgn(s)] \label{cons:u} \] 式中\(sgn(s)\)为符号函数。 \[ \left\{ \begin{array}{ll} sgn(s) = +1 \qquad\qquad if \quad s>0 \\ sgn(s) = -1 \qquad\qquad if \quad s<0 \end{array} \right. \] 需选择足够大的\(k\)才能保证满足滑动模态存在条件。

联立，得： \[ \dot{s} = f + b\widehat{b}^{-1}(-\widehat{f}+\ddot{x}_d-\lambda\dot{\widetilde{x}}-k sgn(s))-\ddot{x}_d+\lambda \dot{\widetilde{x}} \] 整理得： \[ \dot{s} = f-b\widehat{b}^{-1}\widehat{f}+(1-b\widehat{b}^{-1})(-\ddot{x}_d+\lambda \dot{\widetilde{x}})-b\widehat{b}^{-1}k sgn(s) \] 又因为： \[ s\dot{s} = [f-b\widehat{b}^{-1}\widehat{f}+(1-b\widehat{b}^{-1})(-\ddot{x}_d+\lambda \dot{\widetilde{x}})]s-b\widehat{b}^{-1}k|s| \leq -\eta |s| \] 所以\(k\)必须满足： \[ k \geq |\widehat{b}b^{-1}f-\widehat{f}+(\widehat{b}b^{-1}-1)(-\ddot{x}_d+\lambda \dot{\widetilde{x}})|+\eta\widehat{b}b^{-1} \] 由\(\beta > \widehat{b}b^{-1}\)： \[ \begin{array}{ll} \Longrightarrow k \geq |\beta f - \widehat{f} + (\beta -1)(-\ddot{x}_d+\lambda\dot{\widetilde{x}})|+\eta \beta\\ \Longrightarrow k \geq |f + (\beta -1)f - \widehat{f}+(\beta -1)(-\ddot{x}_d+\lambda\dot{\widetilde{x}})|+\eta \beta\\ \Longrightarrow k \geq |(f - \widehat{f})+(\beta -1)f +(\beta -1)(-\ddot{x}_d+\lambda\dot{\widetilde{x}})|+\eta \beta \end{array} \] 又因为\(|f-\widehat{f}|\leq F\)，可导出： \[ k \geq |F+(\beta -1)f +(\beta -1)(-\ddot{x}_d+\lambda\dot{\widetilde{x}})|+\eta \beta \] 对于前文所述的水下机器人动力学模型：\(f = -\frac{c}{m}\dot{x}|\dot{x}|\)，由于流体阻力与机器人运动方向相反，所以\(f\leq 0\)。又\(\beta-1 \geq 0\)，所以： \[ k \geq (F+\beta \eta) + (\beta -1)|\ddot{x}_d-\lambda \dot{\widetilde{x}}| \label{cons:k} \] 对水下机器人运动学方程： \[ \left\{\begin{array}{ll} f = -\frac{\widehat{c}}{\widehat{m}}\dot{x}|\dot{x}| \\ b = \frac{1}{\widehat{m}} \end{array}\right. \] 代入前式，得到航向角滑模控制律应为： \[ u = \widehat{c}\dot{x}|\dot{x}|+\widehat{m}(\ddot{x}_d-\lambda\dot{\widetilde{x}})- \widehat{m}ksgn(s) \label{cons:sf1} \] 其中\(k\)应该满足： \[ k \geq (F+\beta \eta) + (\beta -1)|\ddot{x}_d-\lambda \dot{\widetilde{x}}| \label{cons:sf2} \] 类似地，导出轨迹跟踪滑模控制律为：
流形： \(s=\widetilde{v}_x + \lambda \int_0^t \widetilde{v}_x(\tau)d\tau\)： \[ u = -\widehat{M}_y v_y \omega + c_x v_x |v_x| - \widehat{M}_x\dot{s_r} - ksgn(s) \] 其中： \[ k = \beta_y |v_y \omega| + F + \beta_x |\dot{s_r}| + \widehat{M}_x\eta \]

仿真

航向角控制

1
2
3

function u = smc(k, ddot_x_d, e_dot, dot_x, s, lambda)
    u = 1.352*(ddot_x_d-lambda*e_dot)+0.87*dot_x*abs(dot_x)-1.352*k*sign(s)%sat(s,20); 
%#codegen

轨迹跟踪控制

1
2
3

function u = smc_surge(v_y,omega,v_x_d,v_x_dd,x_d,lambda,k,x,v_x)
%#codegen
    u = -42.951*v_y*omega+25*v_x^2+5.379*v_x-34.675*(-v_x_dd+lambda*(v_x-v_x_d))-k*sign(v_x-v_x_d+lambda*(x-x_d));

1 2	function u = fcnK(v_y,omega,v_x_dd,v_x_d,eta,lambda,v_x) u = 1.2abs(v_yomega)+v_x^2+1.2abs(-v_x_dd+lambda(v_x-v_x_d))+34.675*eta;

1
2
3

function [v_x,v_y] = fcnT(theta,v)
v_x = v*cos(theta);
v_y = v*sin(theta);

1
2
3

function y = fcnD(dx,dy)
%#codegen
y = sqrt(dx^2+dy^2);

跟踪轨迹设置

对给定的时变跟踪轨迹曲线可用如下参数方程描述： \[ \left\{ \begin{array}{l} x = \phi(t) \\ y = \psi(t) \end{array} \right. \] 纵荡滑模控制的设定曲线为弧长\(l\)与时间\(t\)的关系，有： \[ ds = \sqrt{(dx)^2+(dy)^2} = \sqrt{\dot{\phi}^2 (t)+\dot{\psi}^2 (t)} dt \] 弧长为： \[ s = \int_0^t \sqrt{\dot{\phi}^2 (t)+\dot{\psi}^2 (t)} dt \] 航向角滑模控制的设定曲线为角度\(\theta\)与时间\(t\)的关系，有： \[ \theta = arctan(\frac{dy}{dx}) \]

%% Input Signal Generator
% By heshiming
%
%% for generate input signal
t = linspace(0,10,15);
input = 2*ones(1,15);
for num = linspace(1,15,15)
   input(num) = 0.1*t(num);
end
c = spcrv([input(1) input input(end)],3,50000)';
time = linspace(0,10,length(c))';
T = 10/length(c);tau = 0.3;
a = T/tau;
x_filt = filter(a, [1 a-1],c);
y_filt = cos(x_filt.^2)-1;
x_t = [time(3:end),x_filt(3:end)];
y_t = [time(3:end),y_filt(3:end)];

参数摄动下的航向角控制

航向角随时间的变化情况

控制器输出

当控制律中含符号函数时，控制器的输出情况：将符号函数更换为饱和函数，以消除控制器抖动：

水下机器人航向/轨迹跟踪控制系统设计--滑模控制