1 机器人模型及问题描述

1.1 领航者运动学模型

作者给出了如下动力学模型方程式：
[x˙y˙z˙]=[cos⁡θ0sin⁡θ001][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θ0001⎦⎤[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 跟随者运动学模型

符号说明：
RFR_FRF：跟随者机器人
LFL_FLF：领航者机器人
vLv_LvL：领航者机器人的线速度
ωL\omega_LωL：领航者机器人的角速度
θL\theta_LθL：领航者机器人的线速度与水平方向的夹角
vFv_FvF：跟随者机器人的线速度
ωF\omega_FωF：跟随者机器人的角速度
θF\theta_FθF：跟随者机器人的线速度与水平方向的夹角
λL−F\lambda_{L-F}λL−F：两机器人参考点之间的距离
φL−F\varphi_{L-F}φL−F：领航者机器人前进方向与两机器人参考点连线的夹角

λL−Fd\lambda_{L-F}^dλL−Fd：最终目标
φL−Fd\varphi_{L-F}^dφL−Fd：最终目标

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

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

{xV=xL+λL−Fdcos⁡(φL−Fd+θL)yV=yL+λL−Fdsin⁡(φL−Fd+θ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}⎩⎪⎨⎪⎧xVyVθV=xL+λL−Fd cos(φL−Fd+θL)=yL+λL−Fd sin(φL−Fd+θL)=θL(2)

跟随者（FFF）与领航者（LLL）之间的位置关系为：

{xF=xL+λL−Fcos⁡(φL−F+θL)yF=yL+λL−Fsin⁡(φ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}⎩⎪⎨⎪⎧xFyFθF=xL+λL−F cos(φL−F+θL)=yL+λL−F sin(φL−F+θL)=θL−F(3)

虚拟机器人（VVV）与跟随者之间（FFF）的表达式为：

{xe=xV−xFye=yV−yFθ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}⎩⎪⎨⎪⎧xeyeθe=xV−xF=yV−yF=θV−θF(4)

通过转移矩阵，将其转换到跟随者机器人（FFF）自身的坐标系 xF−yFx_F - y_FxF−yF 下的误差表达式为：

[exeyeθ]=[cos⁡θFsin⁡θF0−sin⁡θFcos⁡θF0001][xeyeθ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}⎣⎡exeyeθ⎦⎤=⎣⎡cosθF−sinθF0sinθFcosθF0001⎦⎤⎣⎡xeyeθe⎦⎤(5)

还是展开一下多一层理解：
{ex=cos⁡(θF)xe+sin⁡(θF)yeey=−sin⁡(θF)xe+cos⁡(θF)yeeθ=θ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. ⎩⎪⎨⎪⎧exeyeθ=cos(θF)xe+sin(θF)ye=−sin(θF)xe+cos(θF)ye=θe

继续反推回去：
{ex=cos⁡(θF)xe+sin⁡(θF)ye=cos⁡(θF)(xV−xF)+sin⁡(θF)(yV−yF)ey=−sin⁡(θF)xe+cos⁡(θF)ye=−sin⁡(θF)(xV−xF)+cos⁡(θF)(yV−yF)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. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧exeyeθ=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

将式（3）（4）代入到（5）中得：（这里借用了式子（2））
ex=cos⁡(θF)(xV−xF)+sin⁡(θF)(yV−yF)=cos⁡(θF)(xV−xL−λL−Fcos⁡(φL−F+θL))+sin⁡(θF)(yV−yL−λL−Fsin⁡(φL−F+θL))=cos⁡(θF)(xL+λL−Fdcos⁡(φL−Fd+θL)−xL−λL−Fcos⁡(φL−F+θL))+sin⁡(θF)(yL+λL−Fdsin⁡(φL−Fd+θL)−yL−λL−Fsin⁡(φL−F+θL))=cos⁡(θF)(λL−Fdcos⁡(φL−Fd+θL)−λL−Fcos⁡(φL−F+θL))+sin⁡(θF)(λL−Fdsin⁡(φL−Fd+θL)−λL−Fsin⁡(φL−F+θL))=λL−Fdcos⁡(φL−Fd+θL)cos⁡(θF)−λL−Fcos⁡(φL−F+θL)cos⁡(θF)+λL−Fdsin⁡(φL−Fd+θL)sin⁡(θF)−λL−Fsin⁡(φL−F+θL)sin⁡(θF)=λL−Fdcos⁡(φL−Fd+θL)cos⁡(θF)+λL−Fdsin⁡(φL−Fd+θL)sin⁡(θF)−λL−Fcos⁡(φL−F+θL)cos⁡(θF)−λL−Fsin⁡(φ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))\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−Fcos(φL−F+θL))+sin(θF)(yV−yL−λL−Fsin(φL−F+θL))cos(θF)(xL+λL−Fd cos(φL−Fd+θL)−xL−λL−Fcos(φL−F+θL))+sin(θF)(yL+λL−Fd sin(φL−Fd+θL)−yL−λL−Fsin(φL−F+θL))cos(θF)(λL−Fd cos(φL−Fd+θL)−λL−Fcos(φL−F+θL))+sin(θF)(λL−Fd sin(φL−Fd+θL)−λL−Fsin(φL−F+θL))λL−Fd cos(φL−Fd+θL)cos(θF)−λL−Fcos(φL−F+θL)cos(θF)+λL−Fd sin(φL−Fd+θL)sin(θF)−λL−Fsin(φL−F+θL)sin(θF)λL−Fd cos(φL−Fd+θL)cos(θF)+λL−Fd sin(φL−Fd+θL)sin(θF)−λL−Fcos(φL−F+θL)cos(θF)−λL−Fsin(φ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⁡(φL−F+θL−θF)=cos⁡(φL−F+θL)cos⁡(θF)+sin⁡(φL−F+θL)sin⁡(θF)\cos(\varphi_{L-F} + \theta_L - \theta_F) = \cos(\varphi_{L-F} + \theta_L) \cos(\theta_F) + \sin(\varphi_{L-F} + \theta_L) \sin(\theta_F)cos(φL−F+θL−θF)=cos(φL−F+θL)cos(θF)+sin(φL−F+θL)sin(θF)

[exeyeθ]=[λL−Fdcos⁡(φL−Fd+eθ)−λL−Fcos⁡(φL−F+eθ)λL−Fdsin⁡(φL−Fd+eθ)−λL−Fsin⁡(φ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}⎣⎡exeyeθ⎦⎤=⎣⎡λL−Fdcos(φL−Fd+eθ)−λL−Fcos(φL−F+eθ)λL−Fdsin(φL−Fd+eθ)−λL−Fsin(φL−F+eθ)θL−θF⎦⎤(6)

求导得：

{e˙x=vLcos⁡eθ−vF+ωLλL−Fdsin⁡(φL−F+eθ)e˙y=vLsin⁡eθ−ωFex+ωLλL−Fdcos⁡(φ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˙xe˙ye˙θ=vLcoseθ−vF+ωLλL−Fdsin(φL−F+eθ)=vLsineθ−ωFex+ωLλL−Fdcos(φL−F+eθ)=ωL−ωF(7)

注意，式（7）中第三个角度误差的式子，也可以为 eθ=θL−θFe_\theta = \theta_L - \theta_Feθ=θL−θF。

至此，机器人编队控制问题转化为跟随机器人 RFR_FRF 对虚拟机器人 RVR_VRV 的轨迹跟踪问题，即寻找合适的控制律（vF,ωFv_F, \omega_FvF,ωF）使得式（7）描述的闭环系统渐近稳定.

2 控制器设计

设计控制器如下：
vF=vLcos⁡eθ+γvF+ϕ1(9)v_F = v_L \cos e_{\theta} + \gamma_{vF} + \phi_1 \tag{9}vF=vLcoseθ+γvF+ϕ1(9)

ωF=ωL+kvLey1+ex2+ey2+γωF+ϕ2(10)\omega_F = \omega_L + \frac{k v_L e_y}{\sqrt{1 + e^2_x + e^2_y}} + \gamma_{\omega F} + \phi_2 \tag{10}ωF=ωL+1+ex2+ey2kvLey+γωF+ϕ2(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 Timet(1,i+1) = t(1,i) + dT;% Updta LeaderthetaL(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]);

【Paper】2018_多机器人领航-跟随型编队控制相关推荐

领航-跟随型编队（六）避障问题综述
领航-跟随型编队避障问题指编队在运动过程中,领航机器人根据某种方式获取与识别前方障碍物,同时编队整体采取一定方法及时规避障碍物与防止内部碰撞,涉及到障碍物检测.编队避障规划.编队避碰协调,运动控制等问 ...
领航跟随型编队（十）编队实验视频
实验一:圆形轨迹下编队生成与保持实验如图 5-19 所示,两个机器人完成从随机状态形成编队并沿圆形轨迹保持编队运行, 且图中下方的窗口动态显示编队的运行情况.领航机器人初始信息:坐标(0.5m,0m ...
领航跟随型编队（十四）室内定位技术概述
室外定位技术提出早.发展快且成果显著.室内定位技术相比而言起步较晚,该领域还有很多空白,但人们对室内定位技术的关注从未中断.美国联邦通信委员会FCC(Federal Communications Co ...
【Paper】2022_多无人机系统的分布式最优编队控制
分享自己的一篇文章,发布在人工生命与机器人ICAROB2022,欢迎各位引用. A Distributed Optimal Formation Control for Multi-Agent Syst ...
【Paper】2017_水下潜航器编队海洋勘测的协调控制方法研究
友情链接:[paper]2019_Consensus Control of Multiple AUVs Recovery System Under Switching Topologies and T ...
【Paper】2022_基于无人驾驶地面车辆的多Agent系统仿真平台的设计及编队控制协议的研究
分享自己的一篇文章,发布在人工生命与机器人ICAROB2022,欢迎各位引用. A Design of Multi-Agent System Simulation Platform Based on ...
#基于ROS的编队控制
前言研究生新生,着手研究编队控制.本文为一个记录:在ROS自带的小乌龟环境下实现基于领航者的三角形编队控制. 提示:以下是本篇文章正文内容,下面案例可供参考一.基于领航者-跟随者的编队控制方法实现 ...
论文研读笔记（三）——基于障碍函数的移动机器人编队控制安全强化学习
基于障碍函数的移动机器人编队控制安全强化学习(Barrier Function-based Safe Reinforcement Learning for Formation Control of M ...
论文研读笔记（二）——通过深度强化学习避免碰撞的编队控制
通过深度强化学习避免碰撞的编队控制(Formation Control with Collision Avoidance through Deep Reinforcement Learning) 文献 ...

【Paper】2018_多机器人领航-跟随型编队控制

文章目录