具有量化反馈的视觉伺服WMR控制研究

Research on WMR Control of Visual Servo with Quantitative Feedback

摘要: 针对具有外部扰动和量化反馈的图像视觉伺服移动机器人轨迹跟踪问题，提出了一种基于超螺旋算法的量化反馈控制方法，首先使用超螺旋滑模控制方案，将扰动项放入滑模高阶导数中，减少滑模控制中的抖振现象；接着将量化误差的界限放入滑模开关函数中，来抑制量化对系统稳定性的影响；通过李雅普诺夫函数证明了系统可以在有限时间收敛至与量化参数相关的较小区域内，最后通过动态调节量化参数，使系统最终可以收敛至零。通过仿真结果验证了控制方案的有效性。

Abstract: A quantitative feedback control method based on super-twisting algorithm is proposed aiming at the trajectory tracking of image visual servo mobile robot with external disturbance and quantitative feedback. Firstly, based on super-twisting sliding mode control plan, the disturbance term is put into the high-order derivative of sliding mode to reduce the chattering in sliding mode control; Secondly, the limit of quantization errors are put into sliding mode switching function to suppress the influence of quantization on system stability; it is proved by Lyapunov function that the system can converge to a small region related to quantization parameters in finite time. Finally, by dynamically adjusting quantization parameters, the system can finally converge to zero. The effectiveness of the control plan is verified by simulation results.

1. 引言

近些年随着科技的发展，图像视觉伺服(Image-Based Visual Servoing)的移动机器人(Wheeled Mobile Robot)备受关注。传统的IBVS中存在图像雅可比矩阵奇异性与局部发散的问题 [1]；Cheah [2] 提出了深度信息的在线估计方法，在此基础上Liu [3] 提出了深度无关雅可比矩阵，使得未知参数可以被线性化。当系统存在未知扰动时，不再满足参数线性化的条件，滑模控制(Sliding Mode Control)成为了更好的选择 [4]，SMC具有对参数摄动和外界扰动不灵敏等特点，张 [5] 使用SMC设计了足式移动机器人的控制方案；陈 [6] 提出了一种全局稳定定理与指数趋近律相结合的滑模控制方法；方 [7] 采用支持向量机耦合改进粒子群算法优化滑模控制器，减小了轨迹跟踪误差提高了鲁棒性。传统SMC中会出现抖振的现象，超螺旋滑模控制(Super Twisting Sliding Mode Control)是高阶滑模的一种，这种方式使产生不连续控制量作用于滑模量的高阶导数上，这样得到的控制率会包含积分项，通过积分的率波功能对抖振起到抑制；STSMC已被应用在无人机 [8]、导弹制导 [9] 等领域。

在IBVS的实际应用中，控制模块、固定摄像机和WMR往往通过一个带宽有限的无线网络连接，量化分辨率很粗糙，应考虑量化对系统稳定性的影响。薛 [10] 针对量化器灵敏度参数不匹配的系统研究了监督策略下的量化反馈滑模镇定控制问题；薛等人 [11] 通过建立量化参数的时变比例模型，设计自适应滑模控制方案；Sun [12] 使用扰动观测器，提出一种量化反馈系统的固定时间收敛控制方案。

针对具有量化和未知扰动的IBVS移动机器人轨迹跟踪问题，提出一种基于超螺旋算法的量化反馈控制方案，将未知扰动放入滑模切换函数的二阶导数中，对扰动连续补偿，接着将量化误差的界限放入滑模切换函数中，采用动态量化方案来使系统稳定，最终通过李雅普诺夫函数证明了系统的稳定性，仿真结果验证了控制方案的有效性。

2. 视觉伺服WMR系统

IBVS系统如图1所示，WMR在天花板具有固定摄像机的全局坐标系下，p为相机捕捉的特征点,将WMR前进速度v与转向角速度w看作控制输入， $\left(x\left(t\right),y\left(t\right)\right)$ 为特征点p全局坐标。

基于非完整约束运动学模型可表示为

$\begin{array}{l}\stackrel{\dot{}}{x}\left(t\right)=v\cos \theta \left(t\right)-wd\sin \theta \left(t\right)\\ \stackrel{\dot{}}{y}\left(t\right)=v\sin \theta \left(t\right)+wd\cos \theta \left(t\right)\\ \stackrel{\dot{}}{\theta }\left(t\right)=w\end{array}$ (1)

IBVS系统为：

$\stackrel{\dot{}}{P}\left(t\right)=\frac{1}{z\left(t\right)}D\left(t\right)u+d\left(t\right)$ (2)

$D\left(t\right)=\left[\begin{array}{cc}{h}_{11}-x_p\left(t\right)h_{31}& h_{12}-x_p\left(t\right)h_{32}\\ h_{21}-y_p\left(t\right)h_{31}& h_{22}-y_p\left(t\right)h_{32}\end{array}\right]\left[\begin{array}{cc}\cos \theta \left(t\right)& -d\sin \theta \left(t\right)\\ \sin \theta \left(t\right)& d\cos \theta \left(t\right)\end{array}\right]$ (3)

$z\left(t\right)$ 为深度， $D\left(t\right)$ 是可逆的 [3] $P\left(t\right)={\left(x_p\left(t\right),y_p\left(t\right)\right)}^{\text{T}}$ 为图像系坐标， $d\left(t\right)={\left(d_x\left(t\right),d_y\left(t\right)\right)}^{\text{T}}$ 为模型不确定、参数不确定和外界扰动的总和，WMR在图像系下的期望轨迹为 $P_r={\left(x_r,y_r\right)}^{\text{T}}$ 图像系下的误差定义为

$e\left(t\right)=P_r-P\left(t\right)$ (4)

设计如下PI型滑模函数

$s=e\left(t\right)+c{\displaystyle {\int }_0^{t}e\left(t\right)\text{d}\tau }$ (5)

$c>0$ 为待设定参数，对(5)求导可得

$\stackrel{\dot{}}{s}=-\frac{1}{z\left(t\right)}D\left(t\right)u+ce\left(t\right)-d\left(t\right)$ (6)

结合超螺旋算法设计如下控制器

$u=z\left(t\right)D{\left(t\right)}^{-1}\left\{ce\left(t\right)+k_1{\left|s\right|}^{\frac{1}{2}}sign\left(s\right)+k_2{\displaystyle {\int }_0^{t}sign\left(s\right)\text{d}\tau }\right\}$ (7)

(6)改写为

$\stackrel{\dot{}}{s}=-k_1{\left|s\right|}^{\frac{1}{2}}sign\left(s\right)-k_2{\displaystyle {\int }_0^{t}sign\left(s\right)\text{d}\tau }+d\left(t\right)$ (8)

假设1： $d\left(t\right)$ 连续可导且 $\stackrel{\dot{}}{d}\left(t\right)=sign\left(s\right)\delta \left(t\right)$，其中 $\delta \left(t\right)$ 是与时间相关的有界函数，满足 $0<\delta \left(t\right)<\eta$。

Figure 1. IBVS system description

图1. IBVS系统描述

3. 量化反馈STSMC

考虑闭环系统通过网络传输信号如图2所示，控制器两端的信号会受到量化的影响，使用均匀量化方案 [13] 对系统状态量化

Figure 2. Quantitative feedback visual servo closed-loop system

图2. 量化反馈的视觉伺服闭环系统

$q\left(s\right)=\mu \times round\left(\frac{s}{\mu }\right)$ (9)

$\mu >0$ 为量化参数， $round\left(\right)$ 表示就近取整运算，量化误差为

$q\left(s\right)-s=e_{qs}$ (10)

由(10)可知

$\left|e_{qs}\right|=\left|q\left(s\right)-s\right|\le \frac{\mu }{2}$ (11)

由图2可知信号传输将受到量化，(8)改写为

$\stackrel{\dot{}}{s}=-k_1{\left|q\left(s\right)\right|}^{\frac{1}{2}}sign\left(q\left(s\right)\right)-k_2{\displaystyle {\int }_0^{t}sign\left(q\left(s\right)\right)\text{d}\tau }+d\left(t\right)$ (12)

根据柯西不等式有如下关系

${\left|q\left(s\right)\right|}^{\frac{1}{2}}={\left|e_{qs}+s\right|}^{\frac{1}{2}}\le {\left|e_{qs}\right|}^{\frac{1}{2}}+{\left|s\right|}^{\frac{1}{2}}\le {\left|s\right|}^{\frac{1}{2}}+\sqrt{\frac{\mu }{2}}$ (13)

当 $\left|s\right|\ge \frac{\mu }{2}$ 时 $sign\left(q\left(s\right)\right)=sign\left(s\right)$，当 $\left|s\right|\le \frac{\mu }{2}$ 时 $q\left(s\right)=0$，将(12)改写为

$\{\begin{array}{l}\stackrel{\dot{}}{s}=-k_1{\left|s\right|}^{\frac{1}{2}}sign\left(s\right)+f\left(e_{qs}\right)+r\\ \stackrel{\dot{}}{r}=-k_{2}sign\left(s\right)+\stackrel{\dot{}}{d}\left(t\right)\end{array}$ (14)

通过计算可得 $\left|f\left(e_{qs}\right)\right|\le k_1\sqrt{\frac{\mu }{2}}$。

定理1：对于系统(14)，如果选择控制增益满足如下形式

$\{\begin{array}{l}{k}_1>2\\ k_2>\frac{k_1^3+{\eta }^2\left(4k_1-8\right)}{k_1\left(4k_1-8\right)}\end{array}$ (15)

系统状态将在有限时间 $t_f$ 内收敛到区域 $\Vert \xi \Vert \le \Omega (\; \beta \; )$

$\xi =\left[\begin{array}{c}{\left|s\right|}^{\frac{1}{2}}sign\left(s\right)\\ k_2{\displaystyle {\int }_0^{t}sign\left(s\right)\text{d}\tau +d\left(t\right)}\end{array}\right]$ (16)

$\Omega \left(\mu \right)=\frac{\sqrt{{\lambda }_{\max }\left(P\right)}}{\sqrt{{\lambda }_{\max }\left(P\right)}}\frac{k_1\sqrt{\frac{\mu }{2}}\sqrt{p_{11}^2+p_{12}^2}}{\left(1-\omega \right){\lambda }_{\min }\left(Q\left(\eta \right)\right)}$ (17)

其中 ${\lambda }_{\min }\left(\right),{\lambda }_{\max }\left(\right)$ 为最小、最大特征根， $\omega \in \left(0,1\right)$，矩阵 $P,Q\left(\eta \right)$ 的形式将在证明中给出， $p_{ij}$ 为P中的元素。

证明：对(16)求导可得

$\begin{array}{c}\stackrel{\dot{}}{\xi }=\left[\begin{array}{c}\frac{1}{2\left|{\xi }_1\right|}\left(-k_1{\left|s\right|}^{\frac{1}{2}}sign\left(s\right)-k_2{\displaystyle {\int }_0^{t}sign\left(s\right)\text{d}\tau }+f\left(e_{qs}\right)\right)\\ k_{2}sign\left(s\right)+\stackrel{\dot{}}{d}\left(t\right)\end{array}\right]\\ =\frac{1}{\left|{\xi }_1\right|}\left[\begin{array}{cc}-\frac{k_1}{2}& \frac{1}{2}\\ -k_2+\delta \left(t\right)& 0\end{array}\right]\xi +\frac{1}{\left|{\xi }_1\right|}\left[\begin{array}{c}f\left(e_{qs}\right)\\ 0\end{array}\right]\\ =\frac{1}{\left|{\xi }_1\right|}A\xi +\frac{1}{\left|{\xi }_1\right|}\left[\begin{array}{c}f\left(e_{qs}\right)\\ 0\end{array}\right]\end{array}$ (18)

定义李雅普诺夫函数

$V\left(\xi \right)={\xi }^{\text{T}}P\xi$ (19)

其中

$P=\left[\begin{array}{cc}\frac{4k_1+k_2^2}{2}& \frac{-k_2}{2}\\ \frac{-k_2}{2}& 1\end{array}\right]$ (20)

对(19)求导

$\begin{array}{c}\stackrel{\dot{}}{V}\left(\xi \right)=\frac{1}{\left|{\xi }_1\right|}{\xi }^{\text{T}}\left(A^{\text{T}}P+PA\right)\xi +\frac{1}{\left|{\xi }_1\right|}\left[f\left(e_{qs}\right),0\right]P\xi \\ =-\frac{1}{\left|{\xi }_1\right|}{\xi }^{\text{T}}Q\left(\delta \left(t\right)\right)\xi +\frac{1}{\left|{\xi }_1\right|}f\left(e_{qs}\right)\left[\begin{array}{cc}{p}_{11}& p_{12}\end{array}\right]\xi \end{array}$ (21)

由假设1可知 $0<\delta \left(t\right)<\eta$，且 $f\left(e_{qs}\right)\left[\begin{array}{cc}{p}_{11}& p_{12}\end{array}\right]\xi \le k_1\sqrt{\frac{\mu }{2}}\sqrt{p_{11}^2+p_{12}^2}\Vert \xi \Vert$，可得

$\stackrel{\dot{}}{V}\left(\xi \right)\le -\frac{1}{\left|{\xi }_1\right|}{\xi }^{\text{T}}Q\left(\eta \right)\xi +\frac{1}{\left|{\xi }_1\right|}{k}_1\sqrt{\frac{\mu }{2}}\sqrt{p_{11}^2+p_{12}^2}\Vert \xi \Vert$ (22)

其中

$Q\left(\eta \right)=\left[\begin{array}{cc}{k}_1{k}_2+\frac{k_2^3}{2}-\frac{k_2^2}{4}-{\eta }^2& \frac{k_2}{2}-\frac{k_2^2}{2}\\ \frac{k_2}{2}-\frac{k_2^2}{2}& \frac{k_2}{2}-1\end{array}\right]$ (23)

由Shur补的性质可知当参数选取满足(16)时 $Q\left(\eta \right)$ 为正定矩阵， ${\xi }^{\text{T}}Q\left(\eta \right)\xi$ 满足不等式

${\lambda }_{\min }\left(Q\left(\eta \right)\right){\Vert \xi \Vert }^2\le {\xi }^{\text{T}}Q\left(\eta \right)\xi \le {\lambda }_{\max }\left(Q\left(\eta \right)\right){\Vert \xi \Vert }^2$ (24)

且 $\left|{\xi }_1\right|\le \Vert \xi \Vert$， $\Vert \xi \Vert \le \sqrt{\frac{V\left(\xi \right)}{{\lambda }_{\min }\left(P\right)}}$ 由此可得

$\begin{array}{c}\stackrel{\dot{}}{V}\left(\xi \right)\le -\frac{1}{\left|{\xi }_1\right|}{\lambda }_{\min }\left(Q\left(\eta \right)\right){\Vert \xi \Vert }^2+\frac{1}{\left|{\xi }_1\right|}{k}_1\sqrt{\frac{\mu }{2}}\sqrt{p_{11}^2+p_{12}^2}\Vert \xi \Vert \\ \le -\frac{\Vert \xi \Vert }{\left|{\xi }_1\right|}\left\{{\lambda }_{\min }\left(Q\left(\eta \right)\right)\Vert \xi \Vert -k_1\sqrt{\frac{\mu }{2}}\sqrt{p_{11}^2+p_{12}^2}\right\}\\ \le -{\lambda }_{\min }\left(Q\left(\eta \right)\right)\Vert \xi \Vert +k_1\sqrt{\frac{\mu }{2}}\sqrt{p_{11}^2+p_{12}^2}\\ \le -{\lambda }_{\min }\left(Q\left(\eta \right)\right)\left[\omega \Vert \xi \Vert -\left(1-\omega \right)\Vert \xi \Vert \right]+k_1\sqrt{\frac{\mu }{2}}\sqrt{p_{11}^2+p_{12}^2}\end{array}$ (25)

当 $\forall \Vert \xi \Vert \ge k_1\sqrt{\frac{\mu }{2}}\frac{\sqrt{p_{11}^2+p_{12}^2}}{\left(1-\omega \right){\lambda }_{\min }\left(Q\left(\eta \right)\right)}$ 时有

$\stackrel{\dot{}}{V}\left(\xi \right)\le \frac{-\omega {\lambda }_{\min }\left(Q\left(\eta \right)\right)}{{\lambda }_{\max }^{\frac{1}{2}}\left(P\right)}{V}^{\frac{1}{2}}\left(\xi \right)$ (26)

由有限时间收敛理论 [14] 可得，存在时间 $t_f$ 使 $t>t_f$ 时有 $\Vert \xi \Vert \le \Omega \left(\mu \right)$，即系统将在有限时间 $t_f$ 内，从任意初始位置收敛至 $\Omega \left(\mu \right)$ 内

$t_f=\frac{2\sqrt{{\lambda }_{\min }\left(P\right)}\left\{V^{\frac{1}{2}}\left(0\right)-k_1\sqrt{\frac{\mu }{2}}\frac{\sqrt{{\lambda }_{\max }\left(P\right)}\sqrt{p_{11}^2+p_{12}^2}}{\left(1-\omega \right){\lambda }_{\min }\left(Q\left(\eta \right)\right)}\right\}}{\omega {\lambda }_{\min }\left(Q\left(\eta \right)\right)}$ (27)

定理2：当满足条件(15)时，使用动态均匀量化方案 ${\mu }_{i+1}={\phi }^{i+1}{\mu }_i$ 调节量化参数，系统状态(14)将会收敛至零，其中为 $\phi \in \left(0,1\right)$ 已知参数， $i=0,1,2,\cdots$。

证明：由(17)可知 $\Omega \left({\mu }_i\right)$ 单调递增且 $\Omega \left(0\right)=0$，即 ${\mu }_{i\text{+}1}<{\mu }_i$ 时 $\Omega \left({\mu }_{i\text{+}1}\right)<\Omega \left({\mu }_i\right)$，动态调节 ${\mu }_{i+1}={\phi }^{i+1}{\mu }_i$， $\phi \in \left(0,1\right)$ 时有 ${\mu }_{i+1}<{\mu }_i$， $i\to \infty$， ${\mu }_{i+1}\to 0$。由定理1可知，系统状态的收敛区域为 $\Omega \left({\mu }_i\right)$，在量化参数的动态调节的过程中，系统状态的收敛域将会不停更新，当 $t_f<t\to \infty$， $i\to \infty$， ${\mu }_{i+1}\to 0$， $\Omega \left({\mu }_{i+1}\right)\to 0$，可得系统状态 $s=\stackrel{\dot{}}{s}\to 0$。

综上所述，在系统状态将收敛至 $\Omega \left({\mu }_i\right)$，通过动态量化方案使收敛区域 $\Omega \left({\mu }_{i+1}\right)\to 0$，最终系统会收敛至零，有 $t_f<t\to \infty$ 时 $s=\stackrel{\dot{}}{s}\to 0$，由(5)可知 $e\left(t\right)=-c{\displaystyle {\int }_0^{t}e\left(t\right)\text{d}\tau }$，当 $c>0$ 时有 $e\left(t\right)\to 0$，扰动 $d\left(t\right)$ 可以在(15)的条件下受到连续的补偿，完成轨迹跟踪的任务。

针对系统控制输入，考虑与系统状态通过同一网络在闭环反馈系统中传输，使用(9)中量化方案对u进行量化， $q\left(u\right)={\mu }^{*}\cdot round\left(\frac{u}{{\mu }^{*}}\right)$，(14)改写为

$\stackrel{\dot{}}{s}=-\frac{1}{z\left(t\right)}D\left(t\right)q\left(u\right)+ce\left(t\right)-d\left(t\right)$ (28)

其中 $q\left(u\right)=u+e_{qu}$， $e_{qu}\le \frac{{\mu }^{*}}{2}$， ${\mu }^{*}$ 为量化参数，(28)被改写

$\{\begin{array}{l}\stackrel{\dot{}}{s}=-k_1{\left|s\right|}^{\frac{1}{2}}sign\left(s\right)+f\left(e_{qs}\right)+e_{qu}+r\\ \stackrel{\dot{}}{r}=-k_{2}sign\left(s\right)+\stackrel{\dot{}}{d}\left(t\right)\end{array}$ (29)

定理2：系统(29)将会在有限时间内收敛至

$\Vert \xi \Vert \le {\Omega }^{\ast }\left(\mu ,{\mu }^{\ast }\right)=\frac{\sqrt{{\lambda }_{\max }\left(P\right)}}{\sqrt{{\lambda }_{\max }\left(P\right)}}\frac{\left(k_1\sqrt{\frac{\mu }{2}+}\frac{{\mu }^{*}}{2}\right)\sqrt{p_{11}^2+p_{12}}}{\left(1-\omega \right){\lambda }_{\min }\left(Q\left(\eta \right)\right)}$ (30)

证明过程同定理1，使用定理2可使 $t\to \infty$ 时有 ${\Omega }^{\ast }\left(\mu ,{\mu }^{*}\right)\to 0$，即系统(29)可渐进收敛至零。

4. 数值与仿真

期望轨迹 $P_r={\left(220+8\cos \left(3t\right),260+8\sin \left(3t\right)\right)}^{\text{T}}$，初始坐标 $P\left(0\right)={\left(240,245\right)}^{\text{T}}$，量化参数 ${\mu }_0={\mu }_0^{\ast }=0.5$， $\phi =0.7$，控制增益 $k_1=50$， $k_2=500$， $c=100$，单应矩阵 $H=\left[63187279,0.3210253,-0.3-0.60.8\right]$， $d=0.2$。仿真时间为3 s，由图3可看出，使用动态量化方案的系统状态可在0.035 s收敛至定理1中的理论边界内做滑模运动，在动态量化参数调节的作用下快速收敛至0，抖振也明显减少；图4为动态调节下的量化输入；图5展示了具有动态量化状态和输入的IBVS移动机器人的轨迹跟踪误差，误差在0.028内；图6展示了在图像系中特征点运动轨迹与期望轨迹，可看出该控制方案拥有良好的轨迹跟踪性能。

Figure 3. Dynamic uniform quantization of the system state

图3. 动态均匀量化的系统状态

Figure 4. Dynamic uniform quantization of control inputs

图4. 动态均匀量化的控制输入

Figure 5. Trajectory tracking error

图5. 轨迹跟踪误差

Figure 6. Real trajectories

图6. 真实轨迹

图7为静态量化下的系统状态，与图3对比可看出，系统状态在超螺旋算法的作用下仍会收敛于定

Figure 7. The state of the system under static quantization

图7. 静态量化下的系统状态

理一中与量化参数相关的较小区域，但存在明显地稳态误差；图8为静态量化的系统输入，在静态调节下控制输入存在明显抖振，图9为静态量化的轨迹跟踪误差，在未经动态调节的控制方案下，量化导致误差无法像图5中良好的收敛于领域。

Figure 8. Control input under static quantization

图8. 静态量化下的控制输入

Figure 9. Systematic error under static quantization

图9. 静态量化下的系统误差

5. 总结

提出一种基于超螺旋算法的量化反馈控制方案；给出存在未知扰动的IBVS系统收敛于与量化参数相关区域的参数条件，通过动态调节量化参数的方法，使系统状态可以收敛于零；通过李雅普诺夫函数证明了系统的稳定性，通过与静态量化控制方案对比的仿真可以看出该控制方案拥有良好的轨迹跟踪性能，具备良好的鲁棒性，收敛速度快且抖振得到有效地抑制。