Paper2018_多机器人领航-跟随型编队控制

Posted 2021-11-23 Zhao_Jichao

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

文章目录

• ​​1 机器人模型及问题描述​​
• 1.1 领航者运动学模型
• 1.2 跟随者运动学模型
• 
  
• ​​2 控制器设计​​
• ​​3 仿真与实验​​
• 3.1 仿真
• 
  

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 R_F RF​：跟随者机器人

L F L_F LF​：领航者机器人

v L v_L vL​：领航者机器人的线速度

ω L \\omega_L ωL​：领航者机器人的角速度

θ L \\theta_L θL​：领航者机器人的线速度与水平方向的夹角

v F v_F vF​：跟随者机器人的线速度

ω F \\omega_F ωF​：跟随者机器人的角速度

θ F \\theta_F θF​：跟随者机器人的线速度与水平方向的夹角

λ L − F \\lambda_{L-F} λL−F​：两机器人参考点之间的距离

φ L − F \\varphi_{L-F} φL−F​：领航者机器人前进方向与两机器人参考点连线的夹角

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

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

---

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

{ 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)

---

{ 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 θ = θ L − θ F e_\\theta = \\theta_L - \\theta_F eθ​=θL​−θF​。

至此，机器人编队控制问题转化为跟随机器人 R F R_F RF​ 对虚拟机器人 R V R_V RV​ 的轨迹跟踪问题，即寻找合适的控制律（ v F , ω F v_F, \\omega_F vF​,ωF​）使得式（7）描述的闭环系统渐近稳定.

2 控制器设计

设计控制器如下：

v F = v L cos ⁡ e θ + γ v F + ϕ 1 (9) v_F = v_L \\cos e_{\\theta} + \\gamma_{vF} + \\phi_1 \\tag{9} vF​=vL​coseθ​+γvF​+ϕ1​(9)

ω F = ω L + k v L e y 1 + e x 2 + e y 2 + γ ω 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​+ey2​ ​kvL​ey​​+γωF​+ϕ2​(10)

3 仿真与实验

3.1 仿真