基于领航者人工势场法的队形变化避障控制matlab代码仿真，路径规划，改进人工势场法，拓扑结构，集群，变换队形，基于领航者与人工势场法相结合的编队控制算法，可随意变换队形 增加机器人个数

一、代码整体概述

本套代码基于Matlab开发，聚焦于多机器人（5个机器人，含1个领航者和4个跟随者）的编队控制、队形切换及避障功能实现。核心采用改进人工势场法结合领航者-跟随者拓扑结构，同时融入冲突避免机制、局部极小值逃逸策略，部分场景还支持攻击检测与自适应应对。代码包含21个文件，涵盖工具函数、核心控制逻辑、仿真演示等模块，可实现机器人从初始位置出发，沿规划路径向目标点运动，过程中保持编队队形、规避障碍物及动态调整运动状态的完整流程。

二、核心模块与文件分工

代码体系可划分为工具函数模块、核心控制模块、仿真演示模块三大类，各模块文件分工明确，协同完成多机器人编队控制任务。

（一）工具函数模块

工具函数模块提供基础算法支撑，包括误差计算、几何运算、图形绘制、数据处理等通用功能，为核心控制逻辑提供底层支持。

文件名称	核心功能	详细说明
cal_mse.m	位置误差计算	输入机器人实际位置（pose）与理想位置（ideal_pose），计算二维平面内的欧氏距离，作为位置精度评价指标
compute_area.m	仿真区域界定	根据领航者当前位置动态计算仿真显示区域，默认设置为[-5, 30, -5, 30]，确保机器人运动过程完整可视化
compute_repulsion.m	斥力计算（人工势场核心）	基于机器人与障碍物（或其他机器人、攻击者）的距离，计算x、y方向的斥力合力。当距离小于探测半径（detectR）时，斥力随距离减小而增大，公式为temp=1.0(1/distance-1/detectR)/(distance^3)(distance^5)，确保近距离时避障力度充足
confine.m	速度与加速度约束	基于机器人运动学参数（最大速度、最大加速度），限制下一时刻速度增量，避免速度突变导致的编队不稳定，同时防止超调
detect_2.m	扇形区域探测	判断目标点（如障碍物）是否落在机器人的扇形扫描范围内，输入参数包括自身位置、目标位置、方位角、扫描角度阈值及半径，输出探测结果、夹角及距离
drawcircle.m / drawcircle2.m	圆形绘制	用于在仿真中绘制机器人（drawcircle按编号分配颜色）或攻击者（drawcircle2支持自定义颜色）的轮廓，增强可视化效果
draw_secor.m	扇形区域绘制	绘制机器人的探测扇形区域，直观展示探测范围
draw_square.m	障碍物绘制	以正方形填充形式绘制障碍物，便于仿真中区分障碍物与机器人
interpoint.m	交点计算	求解两条直线的交点，用于路径规划中的几何分析（如障碍物边界交点计算）
motion.m	运动模型	基于速度（vt）和角速度（wt）的运动学模型，计算下一时刻机器人的位置（x、y）和航向角（th），考虑航向角增量对位移的影响
move_2.m	位置更新	根据当前位姿、障碍夹角及运动距离，计算机器人避障后的目标位置和航向角
normalization.m	数据归一化	将特征数据映射到指定区间（lower~upper），用于攻击检测中的数据预处理，消除量纲影响
seek_ang.m	角度差计算	计算两个弧度制角度的差值，确保结果在[-pi, pi]范围内，用于航向角调整和障碍物方向判断
toDegree.m / toRadian.m	角度单位转换	实现弧度与角度的相互转换，适配不同计算场景的角度表示需求

（二）核心控制模块

核心控制模块以defend.m为代表，实现多机器人编队控制的核心逻辑，包括领航者决策、跟随者协同、避障策略、攻击检测与应对等功能。

文件名称	核心功能	详细说明
defend.m	带攻击检测的编队控制	1. 系统初始化：设置机器人数量、初始位姿、速度、运动学参数、目标点、障碍物坐标等； 2. 拓扑结构定义：通过邻接矩阵A定义机器人间通信关系（如跟随者与领航者、跟随者之间的信息交互）； 3. 领航者控制：朝目标点运动，结合斥力实现避障，添加局部极小值随机扰动逃逸； 4. 攻击者建模：根据机器人编队误差动态调整攻击目标，同时具备自身避障能力； 5. 跟随者控制：基于一致性算法，结合领航者速度、邻居机器人影响及斥力，保持编队队形； 6. 攻击检测：通过计算各机器人位置误差（attmse），结合SVM模型识别受攻击机器人，切换控制策略

（三）仿真演示模块

仿真演示模块包含demo1.m~demo4.m，针对不同场景（无攻击、队形切换、多障碍物等）提供可直接运行的仿真案例，验证核心算法的有效性。

文件名称	核心功能	场景特点
demo1.m	基础编队避障仿真	无攻击场景，验证机器人编队运动与避障能力，输出路径图和位置误差曲线
demo2.m	带队形切换的编队控制	设定特定时刻（count=415、620）切换编队队形（修改deltax、deltay），验证队形动态调整的平滑性
demo3.m	领导者-跟随者法编队避障	强化领航者主导作用，结合拓扑图与跟随者协同，支持队形切换与多障碍物规避
demo4.m	多障碍物复杂场景仿真	增加障碍物数量（如[3,3;10,9;10,14;13,12]），提高避障算法的测试强度，验证算法鲁棒性

三、关键算法原理与实现

（一）改进人工势场法

传统人工势场法易出现局部极小值（机器人陷入无合力区域无法前进）和目标不可达问题，本代码通过以下改进实现优化：

1. 斥力计算优化：采用非线性斥力公式，在障碍物附近（距离
2. 局部极小值逃逸：当机器人距离目标点较远（distance>1）且速度接近零（|Vx|≤0.1、|Vy|≤0.1）时，施加随机扰动速度（-1~1之间随机值），帮助机器人脱离局部极小区域；
3. 合力合成：领航者与跟随者的最终速度由目标引力（朝目标点运动的驱动力）、障碍物斥力（避障力）、邻居协同力（保持编队）合成，确保运动方向兼顾目标性与安全性。

（二）领航者-跟随者编队控制

1. 拓扑结构：通过邻接矩阵A定义机器人间通信关系，例如demo1.m中A矩阵设置为：

A=[0 0 0 0 1;     % 跟随者1仅受领航者（第5行）影响
   1 0 0 0 1;     % 跟随者2受跟随者1和领航者影响
   0 0 0 0 1;     % 跟随者3仅受领航者影响
   0 0 1 0 1;     % 跟随者4受跟随者3和领航者影响
   0 0 0 0 0];    % 领航者无上级通信节点

1. 队形保持：通过deltax、deltay定义跟随者相对于领航者的理想位置偏差，跟随者通过计算与邻居机器人的位置差，动态调整速度，维持相对位置稳定；
2. 速度协同：跟随者速度由三部分组成：领航者速度分量（K0Vx(N,k)、K0Vy(N,k)）、编队协同速度（基于邻居位置差的一致性速度）、避障斥力速度（beta*repulsion）。

（三）障碍物与冲突避免

1. 障碍物类型：支持静态点障碍物（如ob_temp定义的坐标点）和静态区域障碍物（如demo2.m中通过fill函数绘制的黑色填充矩形区域）；
2. 探测范围：机器人通过detectR（默认1m）设定障碍物探测半径，攻击者探测半径为attdetect_R（默认0.6m），仅对探测范围内的障碍物计算斥力；
3. 冲突避免：将其他机器人（包括攻击者）视为动态障碍物，在计算斥力时纳入考虑，避免机器人间碰撞。

（四）攻击检测与应对

defend.m中实现了攻击检测与自适应控制逻辑：

1. 攻击建模：攻击者（attacker）动态追踪编队中心或特定目标机器人，通过调整自身速度逼近目标；
2. 误差监测：通过attmse矩阵记录各机器人的位置误差偏离值（与理想误差0.2的差值），当误差之和超过阈值（0.3）时，判定编队受到攻击；
3. 应对策略：攻击发生后（flag_att=1），切换邻接矩阵A和领航者角色（N=1），重新分配跟随者的通信关系，通过SVM模型识别受攻击机器人并标记（label），调整控制参数以维持编队稳定性。

四、仿真流程与关键参数

（一）仿真核心流程

以demo1.m为例，完整仿真流程如下：

1. 初始化配置：设置机器人数量（N=5）、最大迭代次数（countmax=2000）、时间步长（dt=0.1）、运动学参数（最大速度0.7m/s、最大加速度0.4m/s²）、目标点（[25,25]）、障碍物坐标等；
2. 初始状态设置：定义机器人初始位姿（initf）、相对位置偏差（deltax、deltay）、初始速度（Vx、V_y均为0）；
3. 循环迭代：
   - 领航者计算朝目标点的速度，结合斥力调整运动状态，处理局部极小值；
   - 跟随者基于邻居机器人位置差和领航者速度，计算协同速度与避障速度；
   - 调用confine函数限制速度与加速度，更新机器人位姿；
   - 绘制仿真动画（机器人、障碍物、运动轨迹）；
4. 终止条件：当领航者到达目标点附近（距离<0.2m）时，输出"Arrive Goal!!"并结束仿真；
5. 结果输出：绘制机器人运动轨迹图、位置误差曲线，保存误差数据（attmse.mat）。

（二）关键参数说明

参数名称	物理意义	典型值	影响
gama	机器人协同影响因子	0.65~1.5	过大会导致编队抖动，过小则队形调整缓慢
beta	障碍物斥力影响因子	13~25	越大避障反应越强烈，需与gama匹配避免运动震荡
K0	领航者速度权重	1	控制跟随者对领航者速度的跟随程度
KN	领航者目标趋近系数	0.2	影响领航者朝目标点运动的速度大小
detect_R	障碍物探测半径	1m	过小可能遗漏障碍物，过大则斥力干扰正常运动
d_max	最大位置误差阈值	2m	限制跟随者与理想位置的最大偏差，避免编队溃散

五、代码特色与优势

1. 模块化设计：工具函数与核心逻辑分离，代码复用性强，便于扩展新功能（如新增队形、障碍物类型）；
2. 算法鲁棒性：改进人工势场法解决了传统算法的局部极小值问题，结合速度约束和冲突避免机制，确保编队运动稳定；
3. 可视化效果丰富：支持机器人轨迹、实时位姿、探测范围、障碍物、位置误差等多维度可视化，便于算法调试与结果分析；
4. 场景适配性强：通过4个demo案例覆盖基础避障、队形切换、多障碍物、攻击检测等场景，可根据实际需求调整参数适配不同应用场景。

六、使用说明与注意事项

（一）运行环境

• 编程语言：Matlab（建议R2018b及以上版本）；
• 运行方式：直接运行demo1.m~demo4.m或defend.m，无需额外安装工具箱（核心依赖Matlab基础计算与绘图函数）。

（二）参数调整建议

1. 障碍物密集场景：增大beta（如25）和detect_R（如1.2），确保避障反应及时；
2. 队形切换平滑性：减小gama（如0.65），延长队形切换时间窗口（如count=400~600）；
3. 局部极小值频发：增大随机扰动幅度（如将-1+2rand(1)改为-1.5+3rand(1)）。

（三）扩展方向

1. 新增队形：修改deltax、deltay数组，定义直线、三角形、菱形等自定义队形；
2. 动态障碍物：在循环中实时更新ob_temp矩阵，实现动态障碍物避障；
3. 优化攻击检测：完善SVM模型训练与验证逻辑，提高攻击识别准确率；
4. 3D场景扩展：基于现有算法框架，增加z轴维度的位置与力计算，适配3D多机器人编队。

七、总结

本套代码基于改进人工势场法与领航者-跟随者拓扑结构，完整实现了多机器人编队控制、避障、队形切换及攻击检测等核心功能。代码架构清晰、算法鲁棒性强、可视化效果丰富，可作为多机器人系统研发的基础框架，适用于无人机编队、地面机器人协同作业等场景。通过调整关键参数或扩展模块，可进一步适配更复杂的实际应用需求。

基于领航者人工势场法的队形变化避障控制matlab代码仿真，路径规划，改进人工势场法，拓扑结构，集群，变换队形，基于领航者与人工势场法相结合的编队控制算法，可随意变换队形 增加机器人个数