作者在读学校Singapore University of Technology and Design
Established under strong support from MIT, Singapore University of Technology and Design (SUTD) plans to do for Singapore what MIT has done for Massachusetts and Silicon Valley, as well as for the world. SUTD is conducting top-tier research and education. In 2017, SUTD was ranked by Clarivate Analytics as the 5th most influential scientific research university in telecommunications in the world. SUTD was also ranked the 6th in electrical engineering research in the world.
代码示例
链接：MATLAB多机器人编队协同基础版
在下载代码包后使用matlab打开并更改路径到文件夹下，每次打开时要先右键点击subfun文件夹并添加文件及子文件到路径下，如下图所示
随后可以直接运行consensus_APF.m或者consensus_DWA.m即可。
觉得使用方便还请星标和关注笔者的git！
理论分析
详细请参考作者相关博客
由于前文里很多人评论或者私信在问有没有代码示例，其实之前笔者已经上传了实例代码在我的git中，只是由于没有详细的讲解所以大家有很多困惑的地方，因笔者写这篇文章来统一讲解代码的基础功能，希望能减少大家的疑惑。
拓扑结构与定义
首先对于机器人编队通信结构可以用有向图或者无向图\mathcal{G}=(\mathcal V,\mathcal E) 表示，其中\mathcal V={1,2,…,N}表示图有N个顶点，\mathcal E则表示由顶点组成的边E\subset \mathcal V\times \mathcal V，无向图即为双向通信，信息可以双向传递，有向图为单向通信，信息只能由父节点传递至子节点。两者示例如下（绿色节点为leader）
该单向通信拓扑对应的邻接矩阵为矩阵如下所示：
\mathcal A_1=\left[
\begin{array}{cccccc}
0 & 0 & 0 & 0 &0 & 1 \\
0 & 0 &0 & 0 &0 & 1\\
1 & 0 & 0 & 0 & 0 & 0\\
0& 0 & 0 &0 &0 & 1\\
0& 0 & 0 & 1 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 &0
\end{array}
\right] 
该双向通信拓扑对应的邻接矩阵为
\mathcal A_2=\left[
\begin{array}{cccccc}
0 & 1 & 1 & 0 &0 & 1 \\
1 & 0 &0 & 0 &0 & 1\\
1 & 0 & 0 & 1 & 0 & 0\\
0& 0 & 1 &0 &1& 1\\
0& 0 & 0 & 1 & 0 & 0 \\
1 & 1 & 0 & 1 & 0 &0
\end{array}
\right]
在确定通信拓扑后需要设定各机器人的初始位置信息（包括x,y坐标以及朝向角度theta）和协同状态达到稳定时机器人之前的相对位置关系。初始位置信息设置为
init_f=[-4 -1.5 0;%%%[x y th]
-2 -2.5 pi/4;
-6 -2.4 -pi/4;
-2.5 -4 pi/2;
-1 -4.5 -pi/2;
-2.5 -3 0];
pose_x=init_f(:,1);
pose_y=init_f(:,2);
pose_th=init_f(:,3);
pose_x(:,2)=init_f(:,1);
pose_y(:,2)=init_f(:,2);
pose_th(:,2)=init_f(:,3);在编队中各个机器人的相对位置误差为
delta_x=[-1.5 1.5 -3 -1.5 1.5 0];
delta_y=[1.5 1.5 0 -1.5 -1.5 0];同时代码中给定了目标点位置信息goal、速度上限Kinematic、控制周期dt、障碍物的位置信息ob_temp、机器人的探测距离detect_R、通信连接的最大距离d_max，当机器人之间位置误差大于d_max时通信连接会自动断掉。下面即是机器人的运动算法。
机器人编队协同及避障
我们采用传统的leader-follower编队结构，设置循环控制500次，每次控制间隔0.1s，即一共运动50s。由于我们使用的规划算法是人工势场法APF，在每次控制时需要先计算领航者机器人位置与目标点的距离distance，它会给机器人一个引力，距离越大行驶速度越大（当然要小于等于速度上限）
distance=sqrt((goal(1)-pose_x(N,k))^2+(goal(2)-pose_y(N,k))^2);
th=atan2(goal(2)-pose_y(N,k),goal(1)-pose_x(N,k));
if distance>d_max
distance=d_max;
end
V_x(N,k+1)=KN*distance*cos(th);
V_y(N,k+1)=KN*distance*sin(th);
同时当障碍物进入到机器人的探测距离时，计算障碍物给机器人的排斥力repulsion，使机器人避免碰撞到障碍物的同时继续向目标点移动。
ob_pose=ob_temp;
repulsion=compute_repulsion([pose_x(N,k),pose_y(N,k)],ob_pose,detect_R);
V_x(N,k+1)=V_x(N,k+1)+beta*repulsion(1);
V_y(N,k+1)=V_y(N,k+1)+beta*repulsion(2);
在计算了领航者的运动信息后我们计算其他五个跟随者的运动信息。首先要根据一阶编队的协同原理计算leader与follower的误差信息并施加给follwer的速度，使其向协同的位置运动
for j=1:N %%One-order Consensus control
if A(i,j)==1
w_ij=2-exp(-((pose_x(j,k-1)-pose_x(i,k)-(delta_x(j)-delta_x(i)))^2+(pose_y(j,k-1)-pose_y(i,k)-(delta_y(j)-delta_y(i)))^2));
sum_delta_x=sum_delta_x+A(i,j)*w_ij*((pose_x(j,k-1)-pose_x(i,k))-(delta_x(j)-delta_x(i)));
sum_delta_y=sum_delta_y+A(i,j)*w_ij*((pose_y(j,k-1)-pose_y(i,k))-(delta_y(j)-delta_y(i)));
sum_edge_weight=sum_edge_weight+w_ij;
end
end
然后与leader类似，需要计算检测范围内障碍物对机器人的影响，两者的速度加权在一起即为follower的速度。
位置更新与画图展示
在每次计算速度后，我们需要计算这0.1s时间里机器人的位置更新
for i=1:N
out=confine([V_x(i,k) V_y(i,k)],[V_x(i,k+1) V_y(i,k+1)],Kinematic,0.1);
V_x(i,k+1)=out(1);
V_y(i,k+1)=out(2);
pose_x(i,k+1)=pose_x(i,k)+dt*V_x(i,k+1);
pose_y(i,k+1)=pose_y(i,k)+dt*V_y(i,k+1);
pose_th(i,k+1)=atan2(V_y(i,k+1),V_x(i,k+1));
end随后以六个机器人的位置为圆心，适当的距离为半径画圆，标注机器人的方向，即为机器人的状态，在每次更新的时候都重新画图即可动态显示。效果如下图所示
在机器人到达目标点后或者更新了500次停止后，我们可以将机器人的位置数组画图显示出机器人的运动轨迹，如下图所示
后记
本文把多机器人编队的算法代码简要呈现出来，此版本是基础版，大家可以在此基础上扩展延伸。觉得有帮助一定要转发点赞关注哦，谢谢！
笔者本科毕业于上海交通大学，现在是SUTD PhD Candidate，有两年多ROS的使用经验及多机器人编队的科研经验，现在是总结之前所学，希望能有所帮助。本系列多机器人编队将结合论文讲解多机器人编队的算法原理、稳定性分析、路径规划等研究领域，并有代码示例，持续不断更新中。如果大家有相关问题或发现作者漏洞欢迎私戳，同时欢迎关注收藏。
同时欢迎关注博主Git和CSDN：
https://github.com/LiHongbo97
https://blog.csdn.net/qq_33742147?spm=1011.2124.3001.5343
评论（39）
您还未登录，请登录后发表或查看评论