领航跟随型编队（十三）三角形编队变换异构队形避障

三机三角形编队变换异构队形避障

仿真实验2：在图 4-14所示的仿真实验2中，由三台机器人组成的团队，初始队形选择原始队形1，障碍物宽度选择1.0m，设领航机器人与跟随机器人的初始位姿分别为leader的初始坐标（0，9m，0），follower1的初始坐标（0，8m，π/6） ，follower2的初始坐标为（0，10m, -π/6），无障碍时，leader的线速度与角速度分别为v=0.75m/s ; w=0。

截取的部分代码如下：

if(Xsum_up(k,1)>follower2.x(i)&&Xsum_down(k,1)>follower2.x(i) )                            
                            d_follower2=df-rs;
                            follower2_ideal.beta(i+1)= sign(follower2.beta(i)) *asin(2*d_follower2/c31);
                            follower2_ideal.x(i+1)= leader.x(i)-c31*cos(leader.theta(i)+follower2_ideal.beta(i+1));
                            follower2_ideal.y(i+1)=leader.y(i)-c31*sin(leader.theta(i)+follower2_ideal.beta(i+1));
                            follower2_ideal.theta(i+1)=atan(2*(follower2_ideal.y(i+1)-follower2.y(i))/(follower2_ideal.x(i+1)-follower2.x(i)) )                          
                            do(k)=sqrt(( Xsum_down(k,1)-Xsum_up(k,1))^2+( Xsum_down(k,2)-Xsum_up(k,2))^2);
                                                          
                                d_follower1=do(k)/2-rs;%有待验证
                                h1 =((2*rs)^2-(d_follower1+d_follower2)^2);
                                
                                h2 = (c21)^2- (d_follower1)^2;
                                L31=sqrt((sqrt(h1)+sqrt(h2))^2+(d_follower2)^2);
                                
                                follower2_ideal.beta(i+1)= sign(follower2.beta(1)) *asin(2*d_follower2/L31);
                                follower2_ideal.x(i+1)= leader.x(i)-L31*cos(leader.theta(i+1)+follower2_ideal.beta(i));
                                follower2_ideal.y(i+1)=leader.y(i)-L31*sin(leader.theta(i+1)+follower2_ideal.beta(i));
                                follower2_ideal.theta(i+1)=atan(2*(follower2_ideal.y(i+1)-follower2.y(i))/(follower2_ideal.x(i)-follower2.x(i)) ) ;          
        e3(1) =cos(follower2.theta(i))*(follower2_ideal.x(i)-follower2.x(i))+sin(follower2.theta(i))*( follower2_ideal.y(i)-follower2.y(i));
        e3(2)=-sin(follower2.theta(i))*(follower2_ideal.x(i)-follower2.x(i))+cos(follower2.theta(i))*(follower2_ideal.y(i)-follower2.y(i));
        e3(3)= follower2_ideal.theta(i) -follower2.theta(i);
        
        follower2.xe(i)=e3(1);
        follower2.ye(i)=e3(2);
        follower2.thetae(i)=e3(3);
        follower2_ideal.v(i)=vr;
        follower2_ideal.w(i)=wr;
             
        s1= follower2.xe(i);
        s2= follower2.thetae(i)+atan(follower2_ideal.v(i)* follower2.ye(i));      
        f2w2=1+follower2_ideal.v(i)/(1+(follower2_ideal.v(i)*leader.ye(i))^2);        
        follower2.w(i)=f2w1/f2w2;
        follower2.v(i)= follower2.ye(i)* follower2.w(i)+follower2_ideal)^a1+k12*abs(s1)^b1)*s1/(abs(s1)+theta1)
        follower2.theta(i+1)=follower2.theta(i)+ts*follower2.w(i);
        follower2.x(i+1)=follower2.x(i)+ts*follower2.v(i)*cos( follower2.theta(i));
        follower2.y(i+1)=follower2.y(i)+ts*follower2.v(i)*sin( follower2.theta(i));
        follower2.beta(i+1)=pi/4; 

       
    编队避障运动轨迹，可以看出在无障碍物阶段，整个编队从离散状态到形成编队并保持初始队形1的形态（如图中所示A过程）朝向目标点前进。在B点处leader探测到障碍物，并进行避障路径规划，同时根据多机器人协同避障策略，整个队形进行异构队形的变化，且异构队形的组建过程并不是单纯的变为一字型结构，而是根据环境约束自动调整，其变化过程如图中B阶段所示，最终编队根据环境约束自主组建成斜三角形的异构队形如C过程所示，编队穿越障碍物，在D点时三个机器人依次摆脱障碍物，当最后一个机器人离开障碍域后，编队重新恢复初始正三角队形继续奔向目标位置如E阶段所示；通过仿真结果可看出领航有跟随机器人在避障中相互协调，跟随机器人根据环境约束实时调整与领航者的相对位置关系，实现灵活的编队变化，从而整个多机器人系统无碰撞地通过障碍区域。

避障过程中各个机器人的控制输入量和位置误差曲线，从图中可以看出领航机器人的误差其相对距离误差和相对运动方向误差逐渐减小，最终收敛到零，实现对位姿的成功跟踪，验证了本文所提出的有效性。