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

一、代码体系总览

本套Matlab代码聚焦于2D平面内多机器人（1个领航者+4个跟随者）的编队控制与避障任务，核心融合改进人工势场法与领航者-跟随者拓扑结构，同时支持队形动态切换、攻击检测与冲突规避。代码集包含21个文件，覆盖工具函数、核心控制逻辑、仿真演示三大模块，形成从底层算法支撑到上层仿真验证的完整技术链，可复现机器人从初始位置出发、保持编队队形、规避静态障碍物、向目标点运动的全流程，部分场景还能模拟攻击环境下的自适应控制。

二、模块划分与文件功能详解

（一）基础工具函数模块：算法支撑基石

基础工具函数模块提供通用计算、几何分析、图形绘制、数据处理等底层能力，是核心控制逻辑的“基础设施”，各文件功能高度内聚且可复用。

文件名称	核心功能	技术细节与应用场景
cal_mse.m	位置误差量化	输入pose（实际位置）与idealpose（理想位置），通过欧氏距离公式((pose(1)-idealpose(1))²+(pose(2)-ideal_pose(2))²)^(1/2)计算位置偏差，作为编队精度的核心评价指标，用于后续误差曲线绘制与攻击检测阈值判断
compute_area.m	仿真视口动态适配	理论上根据领航者x/y坐标与步长range（默认10）计算显示区域，但实际固定为[-5, 30, -5, 30]，确保机器人运动全程在视口内可见，避免因位置偏移导致的可视化缺失
compute_repulsion.m	人工势场核心：斥力计算	1. 遍历所有障碍物/其他机器人，计算目标与当前机器人的欧氏距离； 2. 当距离≤探测半径detectR时，通过公式temp=1.0(1/distance-1/detectR)/(distance³)(distance⁵)计算斥力系数，确保距离越近斥力越强； 3. 分解斥力为x/y方向分量，求和后输出，是避障功能的核心算法
confine.m	运动学约束控制	1. 输入当前速度current、期望速度next、运动学参数Kinematic（最大速度、最大加速度）与时间步长dt； 2. 分别对x/y方向速度增量进行约束：加速度不超过最大值（Kinematic(3)/4*dt）、速度不超过最大值（Kinematic(1)/2）； 3. 避免速度突变导致的编队溃散，保证机器人运动平滑性

| detect2.m | 扇形区域障碍物探测 | 1. 输入自身位置（x1,y1）、目标位置（x2,y2）、自身航向角th、扫描角度阈值threldth与探测半径radius；
2. 计算目标与自身的距离r及相对角度ang（通过seekang.m归一化到[-pi,pi]）；
3. 当r≤radius且|ang|≤threldth时，判定目标在探测范围内，用于局部避障决策 |

| 图形绘制类（draw*.m） | 仿真可视化 | - drawcircle.m：按机器人编号分配颜色（color='ybgcrkr'），绘制机器人轮廓，区分不同角色；
- drawcircle2.m：支持自定义颜色，专用于绘制攻击者（红色 -r），视觉上与正常机器人区分；
- drawsecor.m：绘制扇形探测区域，直观展示机器人感知范围；
- draw_square.m：以黑色填充正方形绘制障碍物，清晰标记静态障碍区域 |

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

| motion.m | 机器人运动学模型 | 1. 输入当前位姿x=[x;y;th]、控制量u=[vt;wt]（线速度、角速度）与时间步长dt；
2. 采用微分运动模型：deltath=u(2)dt，deltax=u(1)dtcos(x(3)+deltath/2)，deltay=u(1)dt*sin(x(3)+delta_th/2)；
3. 更新下一时刻位姿，比近似模型更精准，适用于高精度编队控制 |

| 数据处理类（normalization.m / seekang.m / 角度转换类） | 数据标准化与角度计算 | - normalization.m：将误差数据映射到[lower,upper]区间（如[0,1]），消除量纲影响，为攻击检测的SVM模型提供标准化输入；
- seekang.m：计算两个弧度角的差值并归一化到[-pi,pi]，避免角度计算出现2pi跳变；
- toDegree.m/toRadian.m：实现弧度与角度的双向转换，适配不同计算场景（如可视化用角度，控制用弧度） |

（二）核心控制模块：编队逻辑中枢

核心控制模块以defend.m为核心，整合编队控制、避障策略、攻击检测与应对逻辑，是整个代码集的“大脑”，实现从感知到决策再到控制的完整闭环。

1. 核心功能拆解

• 系统初始化：定义机器人数量（N=5）、迭代次数（countmax=800）、时间步长（dt=0.1）、运动学参数（最大速度0.4m/s、最大加速度0.4m/s²）、目标点（[20,20]）、障碍物坐标（obtemp）等基础参数；同时初始化机器人初始位姿（initf）、相对位置偏差（deltax/deltay）、初始速度（Vx/Vy）与攻击者初始状态（init_att）。
• 拓扑结构定义：通过邻接矩阵A定义机器人间通信关系，例如初始矩阵：
  matlab
A=[0 1 1 1 1; % 跟随者1与领航者（第5行）、其他跟随者通信
0 0 0 0 1; % 跟随者2仅与领航者通信
0 0 0 1 1; % 跟随者3与领航者、跟随者4通信
0 0 1 0 1; % 跟随者4与领航者、跟随者3通信
0 0 0 0 0]; % 领航者无上级节点

  矩阵元素A(i,j)=1表示机器人i可接收机器人j的位置信息，是编队协同的基础。
• 领航者控制逻辑：
  1. 目标趋近：计算领航者到目标点的距离与角度，当距离>2m时固定距离为2m，通过Vx(N,k+1)=KNdistancecos(th)、Vy(N,k+1)=KNdistancesin(th)生成朝目标点的基础速度；
  2. 避障调整：调用computerepulsion.m计算障碍物（含攻击者）的斥力，通过beta*repulsion叠加到基础速度，实现避障；
  3. 局部极小值逃逸：当距离目标点>0.1m且速度接近零（|Vx|≤0.1、|V_y|≤0.1）时，施加-1~1的随机速度，避免机器人陷入“无合力”区域。
• 攻击者建模与控制：
  1. 目标追踪：计算编队中心（所有机器人位置平均值）或特定目标（通过q循环切换攻击对象），生成攻击者的目标位置posexattpurpose/poseyattpurpose；
  2. 避障适配：将正常机器人与静态障碍物视为避障对象，调用computerepulsion.m计算斥力（探测半径attdetect_R=0.6m），调整自身速度避免碰撞；
  3. 动态调整：当编队误差之和（attmse(6,:)）超过阈值时，切换攻击目标，模拟动态攻击行为。
• 跟随者协同控制：
  1. 编队误差计算：遍历邻居机器人，通过sumdeltax=sum(A(i,j)((posex(j,k)-posex(i,k))-(deltax(j)-deltax(i))))计算位置偏差，确保跟随者与邻居保持理想相对位置；
  2. 速度合成：跟随者速度由三部分组成——领航者速度分量（K0Vx(N,k)）、编队协同速度（gamaerrordistancecos(th)）、避障斥力速度（beta*repulsion）；
  3. 运动学约束：调用confine.m限制速度增量，更新位姿与航向角，确保编队运动稳定。
• 攻击检测与应对：
  1. 误差监测：通过attmse矩阵记录各机器人位置误差与理想值（0.2）的偏差，当误差之和>0.3时判定编队受攻击；
  2. 拓扑重构：攻击发生后（flag_att=1），重新定义邻接矩阵A与领航者角色（N=1），调整跟随者通信关系；
  3. 攻击识别：通过normalization.m归一化误差数据，调用SVM模型（预留label=svm(1)接口）识别受攻击机器人，标记后调整控制策略。

2. 关键变量说明

变量名称	作用	关键取值
deltax/deltay	跟随者相对领航者的理想位置偏差	初始为[-1.5,-3,-1.5,-3,0]/[1.5,1.5,-1.5,-1.5,0]，决定初始编队队形
beta	斥力影响因子	取值10，控制避障力度，过大会导致运动震荡
gama	编队协同因子	取值0.5，影响跟随者对编队误差的响应速度
attmse	攻击检测误差矩阵	6行（4跟随者+1领航者+1误差和），记录误差偏离值，用于攻击判定

（三）仿真演示模块：功能验证载体

仿真演示模块包含demo1.m~demo4.m，针对不同应用场景设计可直接运行的案例，验证核心算法的有效性，同时提供可视化结果与数据输出。

文件名称	核心场景	特色功能与参数配置
demo1.m	基础编队避障	1. 无攻击场景，验证编队稳定性与静态避障能力； 2. 障碍物设置为[10,9;10,14;13,12]，运动学参数为最大速度0.7m/s、最大加速度0.4m/s²； 3. 输出运动轨迹图、位置误差曲线，计算仿真耗时，量化编队精度
demo2.m	队形动态切换	1. 设定count=415时切换为deltax=[-1,-3,-2,-4,0]/deltay=[-1,-3,-2,-4,0]，count=620时恢复初始队形； 2. 新增矩形障碍物（通过fill函数绘制黑色填充区域），提升避障场景复杂度； 3. 到达目标点后驻停100步（m_count>100），验证队形保持稳定性
demo3.m	领导者-跟随者优化	1. 优化邻接矩阵，强化领航者主导作用（跟随者主要接收领航者信息）； 2. 障碍物设置为[5,4;5,8;8,5]，验证稀疏障碍物场景下的编队控制； 3. 记录路径中关键节点（300/570/760步）的机器人位置，直观展示队形切换过程
demo4.m	多障碍物复杂场景	1. 障碍物数量增加到4个（[3,3;10,9;10,14;13,12]），模拟密集障碍环境； 2. 增大斥力影响因子beta=25，确保避障反应及时； 3. 输出多组关键节点轨迹（180/490/650步），验证算法在复杂环境下的鲁棒性

三、核心算法原理深度解析

（一）改进人工势场法：突破传统算法局限

传统人工势场法存在局部极小值（机器人陷入引力与斥力平衡的“死区”）和目标不可达（目标点附近斥力叠加导致无法靠近）问题，本代码通过三重改进实现优化：

1. 斥力公式优化

传统斥力公式为Urep = 0.5eta(1/distance - 1/detectR)²（距离≤detect_R时），存在近距离斥力增长平缓的问题。本代码采用改进公式：

temp=1.0*(1/distance-1/detect_R)/(distance³)*(distance⁵)

化简后为temp= (1/distance - 1/detect_R)*distance²，斥力随距离减小呈二次函数增长，确保近距离时避障力度充足，避免机器人与障碍物碰撞。

2. 局部极小值主动逃逸

当机器人满足“距离目标点>0.1m”且“速度接近零”时，判定陷入局部极小值，通过施加-1+2*rand(1)的随机速度（x/y方向独立），打破引力与斥力的平衡，帮助机器人脱离“死区”。该策略无需额外传感器，仅通过运动状态判断，实现简单且高效。

3. 合力合成策略

机器人最终速度由“目标引力”“编队协同力”“避障斥力”三部分合成：

• 领航者：以“目标引力”为主，“避障斥力”为辅，确保朝目标点运动的同时规避障碍；
• 跟随者：以“编队协同力”为主，“领航者速度分量”保证跟随性，“避障斥力”避免碰撞，三者权重通过K0/gama/beta调节，兼顾编队稳定性与灵活性。

（二）领航者-跟随者拓扑：实现高效编队协同

1. 拓扑结构优势

相较于“分布式无领航者”结构，领航者-跟随者拓扑具有两大优势：

• 决策集中：领航者负责目标路径规划与避障决策，跟随者仅需跟随邻居与领航者，降低计算复杂度；
• 队形可控：通过deltax/deltay定义相对位置，可灵活切换直线、矩形、菱形等队形，适配不同场景需求。

2. 队形保持机制

跟随者通过“邻居位置误差反馈”维持队形：

1. 计算自身与邻居机器人的实际位置差（posex(j,k)-posex(i,k)）；
2. 减去理想位置差（deltax(j)-deltax(i)），得到位置偏差；
3. 基于偏差生成协同速度，通过gama调节响应强度，确保偏差快速收敛到零。

（三）攻击检测与应对：提升系统鲁棒性

1. 攻击判定逻辑

通过“误差偏离度”量化攻击影响：

1. 计算各机器人位置误差与理想误差（0.2）的绝对偏差（abs(att_mse(j)-0.2)）；
2. 求和得到编队总偏差（attmse(6,:)）；
3. 当总偏差>0.3时，判定编队受到攻击，触发应对策略。

2. 自适应应对策略

• 拓扑重构：重新定义邻接矩阵，切断受攻击机器人与其他机器人的通信，避免攻击扩散；
• 角色切换：将未受攻击的机器人设为新领航者（如N=1），维持编队控制中枢；
• 数据归一化：通过normalization.m将误差数据映射到[0,1]，为SVM模型提供标准化输入，精准识别受攻击对象。

四、仿真流程与结果分析

（一）仿真核心流程（以demo1.m为例）

1. 初始化阶段（1~50行）：
   - 配置机器人数量、迭代次数、时间步长、运动学参数、目标点、障碍物坐标；
   - 定义初始位姿（initf）、相对位置偏差（deltax/deltay）、初始速度（Vx/V_y）；
   - 设置颜色（color='ybgcrkr'）、线型（type=[2,1,0.5,0.5,2,2]）等可视化参数。

1. 循环迭代阶段（51~200行）：
   - 领航者控制：计算朝目标点的速度，叠加避障斥力，处理局部极小值；
   - 跟随者控制：计算邻居位置偏差，合成协同速度与避障速度，调用confine.m限制速度；
   - 位姿更新：根据约束后速度更新机器人x/y坐标与航向角（poseth=atan2(Vy,Vx)）；
   - 可视化绘制：调用quiver绘制机器人航向箭头，drawcircle绘制机器人轮廓，draw_square绘制障碍物，axis(area)固定视口，drawnow实时更新动画；
   - 终止判断：当领航者到目标点距离<0.2m时，输出“Arrive Goal!!”并结束循环。

1. 结果输出阶段（201~250行）：
   - 绘制机器人运动轨迹图（标记起点、关键节点、终点）；
   - 计算并绘制位置误差曲线（error=sqrt(errorx.²+errory.²)）；
   - 保存误差数据（attmse.mat），便于后续分析。

（二）关键结果指标

1. 编队精度：通过cal_mse.m计算跟随者与理想位置的偏差，稳定后误差应<0.1m，说明编队精度达标；
2. 避障效果：机器人运动轨迹应绕开障碍物，无碰撞发生，且避障后能快速恢复编队队形；
3. 收敛速度：仿真迭代次数应<1500步（时间<150s），说明控制算法响应迅速，无明显震荡；
4. 攻击应对：defend.m中攻击发生后，编队总误差（attmse(6,:)）应快速下降，新领航者能正常引导编队向目标点运动。

五、代码特色与扩展方向

（一）核心特色

1. 算法创新性：改进人工势场法解决传统算法的局部极小值问题，结合领航者-跟随者拓扑，兼顾编队稳定性与避障灵活性；
2. 场景覆盖全面：从基础避障到队形切换，从无攻击环境到攻击检测，覆盖多机器人编队控制的核心场景；
3. 模块化程度高：工具函数与核心逻辑分离，可单独调用（如compute_repulsion.m可复用于其他避障项目），便于维护与扩展；
4. 可视化丰富：支持机器人航向、探测范围、障碍物、运动轨迹、误差曲线等多维度可视化，直观展示算法效果。

（二）扩展方向

1. 动态障碍物适配：在循环中实时更新obtemp矩阵（如随机生成移动障碍物坐标），结合detect2.m实现动态避障；
2. 多目标路径规划：修改领航者目标点设置，支持多目标点依次运动（如从[10,10]到[20,20]再到[30,10]）；
3. 智能攻击检测优化：完善SVM模型训练流程（添加训练数据生成、模型验证模块），提高攻击识别准确率；
4. 3D场景扩展：增加z轴维度，扩展compute_repulsion.m、motion.m等函数，实现3D空间内的多机器人编队控制；
5. 硬件联调适配：将Matlab代码转换为C++代码，结合ROS（机器人操作系统），实现与实体机器人的联调，验证算法在实际场景中的有效性。

六、使用建议与注意事项

（一）运行环境要求

• 软件：Matlab R2018b及以上版本（需支持quiver、fill、save等基础函数，无需额外工具箱）；
• 硬件：CPU i5及以上，内存8GB及以上（确保大规模迭代时运行流畅）。

（二）参数调整建议

1. 障碍物密集场景：增大beta（如25）和detect_R（如1.2m），增强避障反应；减小gama（如0.65），避免编队震荡；
2. 队形切换场景：延长队形切换时间窗口（如count=400~600），逐步调整deltax/deltay，避免队形突变；
3. 局部极小值频发场景：增大随机扰动幅度（如-1.5+3*rand(1)），或减小detect_R（如0.8m），降低斥力叠加导致的“死区”概率。

（三）常见问题解决

1. 机器人碰撞：检查detect_R是否过小（应≥1m），beta是否过小（应≥13），确保斥力足够；
2. 编队溃散：检查gama是否过小（应≥0.5），confine函数中最大加速度是否过大（应≤0.4m/s²），避免速度突变；
3. 仿真卡顿：减小countmax（如1500），或降低可视化更新频率（如每5步更新一次动画）。

七、总结

本套代码基于改进人工势场法与领航者-跟随者拓扑，构建了一套完整的多机器人编队避障控制方案，涵盖算法设计、代码实现、仿真验证全流程。代码架构清晰、算法鲁棒性强、场景适配性广，可作为多机器人系统研发的基础框架，适用于无人机编队、地面机器人协同作业、仓储机器人调度等领域。通过扩展动态障碍物、智能攻击检测、硬件联调等模块，可进一步提升代码的工程应用价值，为实际多机器人项目提供技术支撑。