【Paper】2018_多机器人领航-跟随型编队控制_多车编队 跟随者如何跟踪领航者的速度

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

1 机器人模型及问题描述

1.1 领航者运动学模型

作者给出了如下动力学模型方程式：
[ x ˙ y ˙ z ˙ ] = [ cos ⁡ θ 0 sin ⁡ θ 0 0 1 ] [ v ( t ) ω ( t ) ] (1) \left[

$$\begin{matrix} \dot{x} \\ \dot{y} \\ \dot{z} \\ \end{matrix}$$ \right]= \left[ $$\begin{matrix} \cos \theta & 0 \\ \sin \theta & 0 \\ 0 & 1 \\ \end{matrix}$$ \right] \left[ $$\begin{matrix} v(t) \\ \omega(t) \\ \end{matrix}$$ \right] \tag{1} ⎣⎡​x˙y˙​z˙​⎦⎤​=⎣⎡​cosθsinθ0​001​⎦⎤​[v(t)ω(t)​](1)

展开方便理解
{ x ˙ = cos ⁡ θ ⋅ v ( t ) y ˙ = sin ⁡ θ ⋅ v ( t ) θ ˙ = ω ( t ) \left\{

$$\begin{aligned} \dot{x} &= \cos \theta \cdot v(t) \\ \dot{y} &= \sin \theta \cdot v(t) \\ \dot{\theta} &= \omega(t) \\ \end{aligned}$$ \right. ⎩⎪⎨⎪⎧​x˙y˙​θ˙​=cosθ⋅v(t)=sinθ⋅v(t)=ω(t)​

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$）之间的位置关系为：

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

{ x V = x L + λ L − F d   cos ⁡ ( φ L − F d + θ L ) y V = y L + λ L − F d   sin ⁡ ( φ L − F d + θ L ) θ V = θ L (2) \left\{

$$\begin{aligned} x_V &= x_L + \lambda_{L-F}^d ~\cos(\varphi_{L-F}^{d} + \theta_L) \\ y_V &= y_L + \lambda_{L-F}^d ~\sin(\varphi_{L-F}^{d} + \theta_L) \\ \theta_V &= \theta_L \\ \end{aligned}$$ \right. \tag{2} ⎩⎪⎨⎪⎧​xV​yV​θV​​=xL​+λL−Fd​ cos(φL−Fd​+θL​)=yL​+λL−Fd​ sin(φL−Fd​+θL​)=θL​​(2)

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

{ x F = x L + λ L − F   cos ⁡ ( φ L − F + θ L ) y F = y L + λ L − F   sin ⁡ ( φ L − F + θ L ) θ F = θ L − F (3) \left\{

$$\begin{aligned} x_F &= x_L + \lambda_{L-F} ~\cos(\varphi_{L-F} + \theta_L) \\ y_F &= y_L + \lambda_{L-F} ~\sin(\varphi_{L-F} + \theta_L) \\ \theta_F &= \theta_{L-F} \\ \end{aligned}$$ \right. \tag{3} ⎩⎪⎨⎪⎧​xF​yF​θF​​=xL​+λL−F​ cos(φL−F​+θL​)=yL​+λL−F​ sin(φL−F​+θL​)=θL−F​​(3)

---

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

{ x e = x V − x F y e = y V − y F θ e = θ V − θ F (4) \left\{

$$\begin{aligned} x_e &= x_V - x_F \\ y_e &= y_V - y_F \\ \theta_e &= \theta_{V} - \theta_{F} \\ \end{aligned}$$ \right. \tag{4} ⎩⎪⎨⎪⎧​xe​ye​θe​​=xV​−xF​=yV​−yF​=θV​−θF​​(4)

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

[ e x e y e θ ] = [ cos ⁡ θ F sin ⁡ θ F 0 − sin ⁡ θ F cos ⁡ θ F 0 0 0 1 ] [ x e y e θ e ] (5) \left[

$$\begin{matrix} e_x \\ e_y \\ e_\theta \\ \end{matrix}$$ \right]= \left[ $$\begin{matrix} \cos \theta_F & \sin \theta_F & 0 \\ -\sin \theta_F & \cos \theta_F & 0 \\ 0 & 0 & 1 \\ \end{matrix}$$ \right] \left[ $$\begin{matrix} x_e \\ y_e \\ \theta_e \\ \end{matrix}$$ \right] \tag{5} ⎣⎡​ex​ey​eθ​​⎦⎤​=⎣⎡​cosθF​−sinθF​0​sinθF​cosθF​0​001​⎦⎤​⎣⎡​xe​ye​θe​​⎦⎤​(5)

还是展开一下多一层理解：
{ e x = cos ⁡ ( θ F ) x e + sin ⁡ ( θ F ) y e e y = − sin ⁡ ( θ F ) x e + cos ⁡ ( θ F ) y e e θ = θ e \left\{

$$\begin{aligned} 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{aligned}$$ \right. ⎩⎪⎨⎪⎧​ex​ey​eθ​​=cos(θF​)xe​+sin(θF​)ye​=−sin(θF​)xe​+cos(θF​)ye​=θe​​

继续反推回去：
{ e x = cos ⁡ ( θ F ) x e + sin ⁡ ( θ F ) y e = cos ⁡ ( θ F ) ( x V − x F ) + sin ⁡ ( θ F ) ( y V − y F ) e y = − sin ⁡ ( θ F ) x e + cos ⁡ ( θ F ) y e = − sin ⁡ ( θ F ) ( x V − x F ) + cos ⁡ ( θ F ) ( y V − y F ) e θ = θ e = θ V − θ F \left\{

$$\begin{aligned} 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{aligned}$$ \right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​ex​ey​eθ​​=cos(θF​)xe​+sin(θF​)ye​=cos(θF​)(xV​−xF​)+sin(θF​)(yV​−yF​)=−sin(θF​)xe​+cos(θF​)ye​=−sin(θF​)(xV​−xF​)+cos(θF​)(yV​−yF​)=θe​=θV​−θF​​

---

{ x F = x L + λ L − F   cos ⁡ ( φ L − F + θ L ) y F = y L + λ L − F   sin ⁡ ( φ L − F + θ L ) θ F = θ L − F (3) \left\{

$$\begin{aligned} x_F &= x_L + \lambda_{L-F} ~\cos(\varphi_{L-F} + \theta_L) \\ y_F &= y_L + \lambda_{L-F} ~\sin(\varphi_{L-F} + \theta_L) \\ \theta_F &= \theta_{L-F} \\ \end{aligned}$$ \right. \tag{3} ⎩⎪⎨⎪⎧​xF​yF​θF​​=xL​+λL−F​ cos(φL−F​+θL​)=yL​+λL−F​ sin(φL−F​+θL​)=θL−F​​(3)

{ x e = x V − x F y e = y V − y F θ e = θ V − θ F (4) \left\{

$$\begin{aligned} x_e &= x_V - x_F \\ y_e &= y_V - y_F \\ \theta_e &= \theta_{V} - \theta_{F} \\ \end{aligned}$$ \right. \tag{4} ⎩⎪⎨⎪⎧​xe​ye​θe​​=xV​−xF​=yV​−yF​=θV​−θF​​(4)

[ e x e y e θ ] = [ cos ⁡ θ F sin ⁡ θ F 0 − sin ⁡ θ F cos ⁡ θ F 0 0 0 1 ] [ x e y e θ e ] (5) \left[

$$\begin{matrix} e_x \\ e_y \\ e_\theta \\ \end{matrix}$$ \right]= \left[ $$\begin{matrix} \cos \theta_F & \sin \theta_F & 0 \\ -\sin \theta_F & \cos \theta_F & 0 \\ 0 & 0 & 1 \\ \end{matrix}$$ \right] \left[ $$\begin{matrix} x_e \\ y_e \\ \theta_e \\ \end{matrix}$$ \right] \tag{5} ⎣⎡​ex​ey​eθ​​⎦⎤​=⎣⎡​cosθF​−sinθF​0​sinθF​cosθF​0​001​⎦⎤​⎣⎡​xe​ye​θe​​⎦⎤​(5)

将式（3）（4）代入到（5）中得：（这里借用了式子（2））
e x = cos ⁡ ( θ F ) ( x V − x F ) + sin ⁡ ( θ F ) ( y V − y F ) = cos ⁡ ( θ F ) ( x V − x L − λ L − F cos ⁡ ( φ L − F + θ L ) ) + sin ⁡ ( θ F ) ( y V − y L − λ L − F sin ⁡ ( φ L − F + θ L ) ) = cos ⁡ ( θ F ) ( x L + λ L − F d   cos ⁡ ( φ L − F d + θ L ) − x L − λ L − F cos ⁡ ( φ L − F + θ L ) ) + sin ⁡ ( θ F ) ( y L + λ L − F d   sin ⁡ ( φ L − F d + θ L ) − y L − λ L − F sin ⁡ ( φ L − F + θ L ) ) = cos ⁡ ( θ F ) ( λ L − F d   cos ⁡ ( φ L − F d + θ L ) − λ L − F cos ⁡ ( φ L − F + θ L ) ) + sin ⁡ ( θ F ) ( λ L − F d   sin ⁡ ( φ L − F d + θ L ) − λ L − F sin ⁡ ( φ L − F + θ L ) ) = λ L − F d   cos ⁡ ( φ L − F d + θ L ) cos ⁡ ( θ F ) − λ L − F cos ⁡ ( φ L − F + θ L ) cos ⁡ ( θ F ) + λ L − F d   sin ⁡ ( φ L − F d + θ L ) sin ⁡ ( θ F ) − λ L − F sin ⁡ ( φ L − F + θ L ) sin ⁡ ( θ F ) = λ L − F d   cos ⁡ ( φ L − F d + θ L ) cos ⁡ ( θ F ) + λ L − F d   sin ⁡ ( φ L − F d + θ L ) sin ⁡ ( θ F ) − λ L − F cos ⁡ ( φ L − F + θ L ) cos ⁡ ( θ F ) − λ L − F sin ⁡ ( φ L − F + θ L ) sin ⁡ ( θ F ) = λ L − F d   ( cos ⁡ ( φ L − F d + θ L ) cos ⁡ ( θ F ) +   sin ⁡ ( φ L − F d + θ L ) sin ⁡ ( θ F ) ) − λ L − F ( cos ⁡ ( φ L − F + θ L ) cos ⁡ ( θ F ) + sin ⁡ ( φ L − F + θ L ) sin ⁡ ( θ F ) )

$$\begin{aligned} 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(\varphi_{L-F} + \theta_L)) \\ &+ \sin(\theta_F) (y_V - y_L - \lambda_{L-F} \sin(\varphi_{L-F} + \theta_L)) \\ =& \cos (\theta_F) (x_L + \lambda_{L-F}^d ~\cos(\varphi_{L-F}^{d} + \theta_L) - x_L- \lambda_{L-F} \cos(\varphi_{L-F} + \theta_L)) \\ &+ \sin(\theta_F) (y_L + \lambda_{L-F}^d ~\sin(\varphi_{L-F}^{d} + \theta_L) - y_L - \lambda_{L-F} \sin(\varphi_{L-F} + \theta_L)) \\ =& \cos (\theta_F) (\lambda_{L-F}^d ~\cos(\varphi_{L-F}^{d} + \theta_L) - \lambda_{L-F} \cos(\varphi_{L-F} + \theta_L)) \\ &+ \sin(\theta_F) (\lambda_{L-F}^d ~\sin(\varphi_{L-F}^{d} + \theta_L) - \lambda_{L-F} \sin(\varphi_{L-F} + \theta_L)) \\ =& \lambda_{L-F}^d ~\cos(\varphi_{L-F}^{d} + \theta_L) \cos (\theta_F) - \lambda_{L-F} \cos(\varphi_{L-F} + \theta_L) \cos (\theta_F) \\ &+ \lambda_{L-F}^d ~\sin(\varphi_{L-F}^{d} + \theta_L) \sin(\theta_F) - \lambda_{L-F} \sin(\varphi_{L-F} + \theta_L) \sin(\theta_F) \\ =& \lambda_{L-F}^d ~\cos(\varphi_{L-F}^{d} + \theta_L) \cos (\theta_F) + \lambda_{L-F}^d ~\sin(\varphi_{L-F}^{d} + \theta_L) \sin(\theta_F) \\ &- \lambda_{L-F} \cos(\varphi_{L-F} + \theta_L) \cos (\theta_F)- \lambda_{L-F} \sin(\varphi_{L-F} + \theta_L) \sin(\theta_F) \\ =& \lambda_{L-F}^d ~(\cos(\varphi_{L-F}^{d} + \theta_L) \cos (\theta_F) + ~\sin(\varphi_{L-F}^{d} + \theta_L) \sin(\theta_F)) \\ &- \lambda_{L-F} ( \cos(\varphi_{L-F} + \theta_L) \cos (\theta_F)+ \sin(\varphi_{L-F} + \theta_L) \sin(\theta_F)) \\ \end{aligned}$$ ex​=======​cos(θF​)(xV​−xF​)+sin(θF​)(yV​−yF​)cos(θF​)(xV​−xL​−λL−F​cos(φL−F​+θL​))+sin(θF​)(yV​−yL​−λL−F​sin(φL−F​+θL​))cos(θF​)(xL​+λL−Fd​ cos(φL−Fd​+θL​)−xL​−λL−F​cos(φL−F​+θL​))+sin(θF​)(yL​+λL−Fd​ sin(φL−Fd​+θL​)−yL​−λL−F​sin(φL−F​+θL​))cos(θF​)(λL−Fd​ cos(φL−Fd​+θL​)−λL−F​cos(φL−F​+θL​))+sin(θF​)(λL−Fd​ sin(φL−Fd​+θL​)−λL−F​sin(φL−F​+θL​))λL−Fd​ cos(φL−Fd​+θL​)cos(θF​)−λL−F​cos(φL−F​+θL​)cos(θF​)+λL−Fd​ sin(φL−Fd​+θL​)sin(θF​)−λL−F​sin(φL−F​+θL​)sin(θF​)λL−Fd​ cos(φL−Fd​+θL​)cos(θF​)+λL−Fd​ sin(φL−Fd​+θL​)sin(θF​)−λL−F​cos(φL−F​+θL​)cos(θF​)−λL−F​sin(φL−F​+θL​)sin(θF​)λL−Fd​ (cos(φL−Fd​+θL​)cos(θF​)+ sin(φL−Fd​+θL​)sin(θF​))−λL−F​(cos(φL−F​+θL​)cos(θF​)+sin(φL−F​+θL​)sin(θF​))​

$$\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)$$

[ e x e y e θ ] = [ λ L − F d cos ⁡ ( φ L − F d + e θ ) − λ L − F cos ⁡ ( φ L − F + e θ ) λ L − F d sin ⁡ ( φ L − F d + e θ ) − λ L − F sin ⁡ ( φ L − F + e θ ) θ L − θ F ] (6) \left[

$$\begin{matrix} e_x \\ e_y \\ e_\theta \\ \end{matrix}$$ \right]= \left[ $$\begin{matrix} \lambda_{L-F}^{d} \cos(\varphi_{L-F}^{d} + e_\theta) - \lambda_{L-F} \cos(\varphi_{L-F} + e_\theta) \\ \lambda_{L-F}^{d} \sin(\varphi_{L-F}^{d} + e_\theta) - \lambda_{L-F} \sin(\varphi_{L-F} + e_\theta) \\ \theta_L - \theta_F \\ \end{matrix}$$ \right] \tag{6} ⎣⎡​ex​ey​eθ​​⎦⎤​=⎣⎡​λL−Fd​cos(φL−Fd​+eθ​)−λL−F​cos(φL−F​+eθ​)λL−Fd​sin(φL−Fd​+eθ​)−λL−F​sin(φL−F​+eθ​)θL​−θF​​⎦⎤​(6)

求导得：

{ e ˙ x = v L cos ⁡ e θ − v F + ω L λ L − F d sin ⁡ ( φ L − F + e θ ) e ˙ y = v L sin ⁡ e θ − ω F e x + ω L λ L − F d cos ⁡ ( φ L − F + e θ ) e ˙ θ = ω L − ω F (7) \left\{

$$\begin{aligned} \dot{e}_x &= v_L \cos e_\theta - v_F + \omega_L \lambda_{L-F}^{d} \sin(\varphi_{L-F} + e_\theta) \\ \dot{e}_y &= v_L \sin e_\theta - \omega_F e_x + \omega_L \lambda_{L-F}^{d} \cos(\varphi_{L-F} + e_\theta) \\ \dot{e}_\theta &= \omega_L - \omega_F \\ \end{aligned}$$ \right. \tag{7} ⎩⎪⎨⎪⎧​e˙x​e˙y​e˙θ​​=vL​coseθ​−vF​+ωL​λL−Fd​sin(φL−F​+eθ​)=vL​sineθ​−ωF​ex​+ωL​λL−Fd​cos(φL−F​+eθ​)=ωL​−ωF​​(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]);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40