领航跟随型编队（十一）五角形编队生成同构队形避障

 仿真实验 ：五角形编队自主规划及生成同构队形避障。  编队选择原始队形 2，机器人初始化参数以及障碍物参数分别如表 5-1 和表 5-2 所示。

其中领航机器人的初始位置坐标为(0,9)，跟随机器人follower1的初 始位置坐标为(0,8)，跟随机器人follower2的初始位置坐标为(0,10)，跟随机器 人follower3的初始位置坐标为(0,11)，跟随机器人follower4的初始位置坐标为 (0,7)，没有遇到障碍物时候，领航机器人的线速度和角速度分别为 =0.75。

截取的部分MATLAB代码如下：

 for k=1:1:J
        Distance_up2(k)=( follower2.x(i)-Xsum_up(k,1))^2+( follower2.y(i)-Xsum_up(k,2))^2;      
        distance_up2(k)=sqrt(Distance_up2(k));
                Distance_down2(k)=( follower2.x(i)-Xsum_down(k,1))^2+( follower2.y(i)-Xsum_down(k,2))^2;
              distance_down2(k)=sqrt(Distance_down2(k));                
        if ( distance_up2(k)=dm2)          
            if(Xsum_up(k,1)>follower2.x(i)&&Xsum_down(k,1)>follower2.x(i) )
              follower2_ideal.beta(i+1)= sign(follower2.beta(i)) *asin(2*d_follower2/c31);
                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)) ) ;
                Distance_collision=(follower2_ideal.y(i+1)-follower1_ideal.y(i+1))^2+(follower2_ideal.y(i+1)-follower1_ideal.y(i+1))^2;
                distance_collision=sqrt(Distance_collision);                
                if  distance_collision<2*rs                    
                    h1 =((2*rs)^2-(  d_follower1+d_follower2)^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)+follower2_ideal.beta(i));
                    follower2_ideal.y(i+1)=leader.y(i)-L31*sin(leader.theta(i)+follower2_ideal.beta(i));
                    follower2_ideal.theta(i+1)=atan(2*(follower2_ideal.y(i+1)-follower2.y(i))/(follower2_ideal.x(i+1)-follower2.x(i)) ) ;
                end
            end
            
        else
            if ( distance_up2(k)>=dm2&&distance_down2(k)<=dm2)
                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.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)) ) ;
                    Distance_collision=(follower2_ideal.y(i+1)-follower1_ideal.y(i+1))^2+(follower2_ideal.y(i+1)-follower1_ideal.y(i+1))^2;
                    distance_collision=sqrt(Distance_collision);
                    

 编队在线协同避障的运动轨迹图如图 5-6 所示，在初始的编队未遇到障碍物 阶段，整个编队从离散状态形成预设的编队队形（如图中的 A 所示），并且保持 该队形前行。当编队行进到一定位置时候，领航机器人的传感器探测到路径上有 障碍物，此时编队内部开始进入在线协同避障的流程，基于多机器人协同避障策 略，整个队形在领航机器人的带领下进行同构队形的变化（如图中的 B 所示）， 编队将以新形成的同构队形绕过障碍物（如图中 C 所示）。当领航机器人检测到
 
整个编队已经绕过障碍物后，编队开始进行队形恢复的变换（如图中 D 所示）， 编队恢复到原始队形后将保持队形继续前行（如图中 E 所示）。通过仿真可以看 出，多机器人系统的避障需要领航机器人的反应式导航，其他机器人根据环境约 束实时调整与领航者的相对位置关系，逐步实现编队队形的自主变化，从而使整 个多机器人系统无碰撞地穿越障碍物。 

 
以下是分镜头情况：

标题 标题 标题