师五喜,王栋伟,李宝全.多机器人领航-跟随型编队控制[J].天津工业大学学报,2018,37(02):72-78.
文章目录
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θ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 跟随者运动学模型
符号说明：
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} ⎩⎪⎨⎪⎧xVyVθ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˙xe˙ye˙θ=vLcoseθ−vF+ωLλL−Fdsin(φL−F+eθ)=vLsineθ−ωFex+ωLλL−Fdcos(φ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=vLcoseθ+γ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 kvLey+γωF+ϕ2(10)
3 仿真与实验3.1 仿真
