师五喜,王栋伟,李宝全.多机器人领航-跟随型编队控制[J].天津工业大学学报,2018,37(02):72-78.

1 机器人模型及问题描述

1.1 领航者运动学模型

作者给出了如下动力学模型方程式：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right]=\left[\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v(t)\\ \omega (t)\end{array}\right]\text{\hspace{1em}(1)}$$

展开方便理解
$$\{\begin{array}{rl}\dot{x}& =\cos \theta \cdot v(t)\\ \dot{y}& =\sin \theta \cdot v(t)\\ \dot{\theta }& =\omega (t)\end{array}$$

1.2 跟随者运动学模型

符号说明：
$R_F$：跟随者机器人
$L_F$：领航者机器人
$v_L$：领航者机器人的线速度
${\omega }_L$：领航者机器人的角速度
${\theta }_L$：领航者机器人的线速度与水平方向的夹角
$v_F$：跟随者机器人的线速度
${\omega }_F$：跟随者机器人的角速度
${\theta }_F$：跟随者机器人的线速度与水平方向的夹角
${\lambda }_{L-F}$：两机器人参考点之间的距离
${\phi }_{L-F}$：领航者机器人前进方向与两机器人参考点连线的夹角

${\lambda }_{L-F}^d$：最终目标
${\phi }_{L-F}^d$：最终目标

---

在世界坐标系中，虚拟机器人（$V$）与领航者（$L$）之间的位置关系为：

注意这里要明确一个事情，就是跟随者最终要达到的位置是虚拟机器人的位置，并不是达到领航机器人的位置，这一点要注意。

$$\{\begin{array}{rl}{x}_V& =x_L+{\lambda }_{L-F}^d\cos ({\phi }_{L-F}^d+{\theta }_L)\\ y_V& =y_L+{\lambda }_{L-F}^d\sin ({\phi }_{L-F}^d+{\theta }_L)\\ {\theta }_V& ={\theta }_L\end{array}\text{\hspace{1em}(2)}$$

跟随者（$F$）与领航者（$L$）之间的位置关系为：

$$\{\begin{array}{rl}{x}_F& =x_L+{\lambda }_{L-F}\cos ({\phi }_{L-F}+{\theta }_L)\\ y_F& =y_L+{\lambda }_{L-F}\sin ({\phi }_{L-F}+{\theta }_L)\\ {\theta }_F& ={\theta }_{L-F}\end{array}\text{\hspace{1em}(3)}$$

---

虚拟机器人（$V$）与跟随者之间（$F$）的表达式为：

$$\{\begin{array}{rl}{x}_e& =x_V-x_F\\ y_e& =y_V-y_F\\ {\theta }_e& ={\theta }_V-{\theta }_F\end{array}\text{\hspace{1em}(4)}$$

通过转移矩阵，将其转换到跟随者机器人（$F$）自身的坐标系 $x_F-y_F$ 下的误差表达式为：

$$\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\theta }\end{array}\right]=\left[\begin{array}{ccc}\cos {\theta }_F& \sin {\theta }_F& 0\\ -\sin {\theta }_F& \cos {\theta }_F& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_e\\ y_e\\ {\theta }_e\end{array}\right]\text{\hspace{1em}(5)}$$

还是展开一下多一层理解：
$$\{\begin{array}{rl}{e}_x& =\cos ({\theta }_F)x_e+\sin ({\theta }_F)y_e\\ e_y& =-\sin ({\theta }_F)x_e+\cos ({\theta }_F)y_e\\ e_{\theta }& ={\theta }_e\end{array}$$

继续反推回去：
$$\{\begin{array}{rl}{e}_x& =\cos ({\theta }_F)x_e+\sin ({\theta }_F)y_e\\ & =\cos ({\theta }_F)(x_V-x_F)+\sin ({\theta }_F)(y_V-y_F)\\ e_y& =-\sin ({\theta }_F)x_e+\cos ({\theta }_F)y_e\\ & =-\sin ({\theta }_F)(x_V-x_F)+\cos ({\theta }_F)(y_V-y_F)\\ e_{\theta }& ={\theta }_e\\ & ={\theta }_V-{\theta }_F\end{array}$$

---

$$\{\begin{array}{rl}{x}_F& =x_L+{\lambda }_{L-F}\cos ({\phi }_{L-F}+{\theta }_L)\\ y_F& =y_L+{\lambda }_{L-F}\sin ({\phi }_{L-F}+{\theta }_L)\\ {\theta }_F& ={\theta }_{L-F}\end{array}\text{\hspace{1em}(3)}$$

$$\{\begin{array}{rl}{x}_e& =x_V-x_F\\ y_e& =y_V-y_F\\ {\theta }_e& ={\theta }_V-{\theta }_F\end{array}\text{\hspace{1em}(4)}$$

$$\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\theta }\end{array}\right]=\left[\begin{array}{ccc}\cos {\theta }_F& \sin {\theta }_F& 0\\ -\sin {\theta }_F& \cos {\theta }_F& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_e\\ y_e\\ {\theta }_e\end{array}\right]\text{\hspace{1em}(5)}$$

将式（3）（4）代入到（5）中得：（这里借用了式子（2））
$$\begin{array}{rl}{e}_x=& \cos ({\theta }_F)(x_V-x_F)+\sin ({\theta }_F)(y_V-y_F)\\ =& \cos ({\theta }_F)(x_V-x_L-{\lambda }_{L-F}\cos ({\phi }_{L-F}+{\theta }_L))\\ & +\sin ({\theta }_F)(y_V-y_L-{\lambda }_{L-F}\sin ({\phi }_{L-F}+{\theta }_L))\\ =& \cos ({\theta }_F)(x_L+{\lambda }_{L-F}^d\cos ({\phi }_{L-F}^d+{\theta }_L)-x_L-{\lambda }_{L-F}\cos ({\phi }_{L-F}+{\theta }_L))\\ & +\sin ({\theta }_F)(y_L+{\lambda }_{L-F}^d\sin ({\phi }_{L-F}^d+{\theta }_L)-y_L-{\lambda }_{L-F}\sin ({\phi }_{L-F}+{\theta }_L))\\ =& \cos ({\theta }_F)({\lambda }_{L-F}^d\cos ({\phi }_{L-F}^d+{\theta }_L)-{\lambda }_{L-F}\cos ({\phi }_{L-F}+{\theta }_L))\\ & +\sin ({\theta }_F)({\lambda }_{L-F}^d\sin ({\phi }_{L-F}^d+{\theta }_L)-{\lambda }_{L-F}\sin ({\phi }_{L-F}+{\theta }_L))\\ =& {\lambda }_{L-F}^d\cos ({\phi }_{L-F}^d+{\theta }_L)\cos ({\theta }_F)-{\lambda }_{L-F}\cos ({\phi }_{L-F}+{\theta }_L)\cos ({\theta }_F)\\ & +{\lambda }_{L-F}^d\sin ({\phi }_{L-F}^d+{\theta }_L)\sin ({\theta }_F)-{\lambda }_{L-F}\sin ({\phi }_{L-F}+{\theta }_L)\sin ({\theta }_F)\\ =& {\lambda }_{L-F}^d\cos ({\phi }_{L-F}^d+{\theta }_L)\cos ({\theta }_F)+{\lambda }_{L-F}^d\sin ({\phi }_{L-F}^d+{\theta }_L)\sin ({\theta }_F)\\ & -{\lambda }_{L-F}\cos ({\phi }_{L-F}+{\theta }_L)\cos ({\theta }_F)-{\lambda }_{L-F}\sin ({\phi }_{L-F}+{\theta }_L)\sin ({\theta }_F)\\ =& {\lambda }_{L-F}^d(\cos ({\phi }_{L-F}^d+{\theta }_L)\cos ({\theta }_F)+\sin ({\phi }_{L-F}^d+{\theta }_L)\sin ({\theta }_F))\\ & -{\lambda }_{L-F}(\cos ({\phi }_{L-F}+{\theta }_L)\cos ({\theta }_F)+\sin ({\phi }_{L-F}+{\theta }_L)\sin ({\theta }_F))\end{array}$$

$$\cos ({\phi }_{L-F}+{\theta }_L-{\theta }_F)=\cos ({\phi }_{L-F}+{\theta }_L)\cos ({\theta }_F)+\sin ({\phi }_{L-F}+{\theta }_L)\sin ({\theta }_F)$$

$$\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\theta }\end{array}\right]=\left[\begin{array}{c}{\lambda }_{L-F}^d\cos ({\phi }_{L-F}^d+e_{\theta })-{\lambda }_{L-F}\cos ({\phi }_{L-F}+e_{\theta })\\ {\lambda }_{L-F}^d\sin ({\phi }_{L-F}^d+e_{\theta })-{\lambda }_{L-F}\sin ({\phi }_{L-F}+e_{\theta })\\ {\theta }_L-{\theta }_F\end{array}\right]\text{\hspace{1em}(6)}$$

求导得：

$$\{\begin{array}{rl}{\dot{e}}_x& =v_L\cos e_{\theta }-v_F+{\omega }_L{\lambda }_{L-F}^d\sin ({\phi }_{L-F}+e_{\theta })\\ {\dot{e}}_y& =v_L\sin e_{\theta }-{\omega }_F{e}_x+{\omega }_L{\lambda }_{L-F}^d\cos ({\phi }_{L-F}+e_{\theta })\\ {\dot{e}}_{\theta }& ={\omega }_L-{\omega }_F\end{array}\text{\hspace{1em}(7)}$$

注意，式（7）中第三个角度误差的式子，也可以为 $e_{\theta }={\theta }_L-{\theta }_F$。

至此，机器人编队控制问题转化为跟随机器人 $R_F$ 对虚拟机器人 $R_V$ 的轨迹跟踪问题，即寻找合适的控制律（$v_F,{\omega }_F$）使得式（7）描述的闭环系统渐近稳定.

2 控制器设计

设计控制器如下：
$$v_F=v_L\cos e_{\theta }+{\gamma }_{vF}+{\varphi }_1\text{\hspace{1em}(9)}$$

$${\omega }_F={\omega }_L+\frac{k{v}_L{e}_y}{\sqrt{1+e_x^2+e_y^2}}+{\gamma }_{\omega F}+{\varphi }_2\text{\hspace{1em}(10)}$$

3 仿真与实验

3.1 仿真

Leader 状态

% Paper: 2018_多机器人领航-跟随型编队控制
% Author: Z-JC
% Data: 2021-11-20
clear
clc


%%
% Leader1's states
xL(1,1)     = 2;
yL(1,1)     = 2;
thetaL(1,1) = 0;
vL = 0.1;
wL = 0.1;

% Parameters
alpha1 = 0.45;
alpha2 = 0.5;
k = 3.0;

% Time states
t(1,1) = 0;
dT = 0.1;

for i=1:999
    % Record Time
    t(1,i+1) = t(1,i) + dT;
    
    % Updta Leader
    thetaL(1,i+1) = thetaL(1,i) + dT * wL;
    xL(1,i+1) = xL(1,i) + dT * vL * cos(thetaL(1,i));
    yL(1,i+1) = yL(1,i) + dT * vL * sin(thetaL(1,i));
    
end



%% 
plot(xL,yL);
xlim([0.5,3.5]); ylim([1.5,4.5]);