【无人机编队】基于一致性算法和人工势场算法实现多机器人协同编队避障路径规划matlab代码

1 简介

本文提出一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法.通过引入与障碍物移动方向垂直的辅助牵引加速度信息,可消除局部优化解的影响.同时,为了实现无人机机群快速编队和避障,本文将有限时间一致性算法与人工势场方法相结合,提出一种编队及其避障控制方法.

2 部分代码

clc
clear all
close all
%%%%一阶算法仿真 非完整型约束
%%% 设置好重心偏移量和gama，权重，基本可行
%% 初始化 位置pose、速度V、加速度控制量control
init_f=[-3 -6 0; %%%[x y th]
   -5 6 0;
   2 4 pi/4;
   5 -3 -pi/4;
   3 0 pi/2];
pose_x=init_f(:,1);
pose_y=init_f(:,2);
pose_th=init_f(:,3);
%% follower相对leader的位置
delta_x=[-2 -6 -2 -6 0];   % 相对间隔误差
delta_y=[4 4 -4 -4 0