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_xinit_f(:,1); pose_yinit_f(:,2); pose_thinit_f(:,3); %% follower相对leader的位置 delta_x[-2 -6 -2 -6 0]; % 相对间隔误差 delta_y[4 4 -4 -4 0]; %领航者与自己无误差 fol_num4; N5; % 4follower and 1 leader countmax2000; dt0.1; gama3; di0.02; %%%重心偏移 单位m K00; %%% 直线K1 K2都设在0.2左右 k30 %%% 圆不考虑K20 K1K3设为0.2左右 K11;%%%位置偏差线速度调节 K21;%%位置偏差角速度调节 K30.01;%%朝向偏差角速度调节 % %% 通信拓扑图:1-4行为follower 最后一行为leader A[0 1 1 1 1; % a(ij) 0 0 0 0 1; 0 0 0 1 1; 0 0 1 0 1; 0 0 0 0 0]; % %% 通信拓扑图:1-4行为follower 最后一行为leader linear_v(:,1)[0;0;0;0;1]; angular_w(:,1)[0;0;0;0;1]; k0; % 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss]] Kinematic[1.5,toRadian(60.0),0.5,toRadian(50.0)];%% 运动限制 error_temp(1:fol_num,1:4)0;%%行为follower个数、列为两个时刻的x,y偏差 in_degreesum(A,2); %% 开始循环 走顺时针圆周 % figure; for count1:countmax kk1; linear_v(N,k1)linear_v(N,k);%领航者速度不变 angular_w(N,k1)angular_w(N,k); % if count500 % linear_v(N,k1)0.5;%领航者速度不变 % angular_w(N,k1)0.5; % end % if count1000 % linear_v(N,k1)0.5;%领航者速度不变 % angular_w(N,k1)0; % end for i1:fol_num sum_delta_x0; sum_delta_y0; for j1:N %%考虑邻居对它的影响 if k1 temp_x0; temp_y0; else temp_x(pose_x(j,k)-pose_x(j,k-1))/dt; temp_y(pose_y(j,k)-pose_y(j,k-1))/dt; end sum_delta_xsum_delta_xA(i,j)*(temp_xgama*((pose_x(j,k)-pose_x(i,k))-(delta_x(j)-delta_x(i)))); sum_delta_ysum_delta_yA(i,j)*(temp_ygama*((pose_y(j,k)-pose_y(i,k))-(delta_y(j)-delta_y(i)))); end sum_delta_xsum_delta_x/in_degree(i); sum_delta_ysum_delta_y/in_degree(i); %% 画误差图 colormgbkr; %%%定义颜色标记 figure % 生成三维平面图 连续 for i1:N plot(pose_x(i,:),pose_y(i,:),color(1,i),LineWidth,2); hold on end for i1:N-1 plot(pose_x(i,1),pose_y(i,1),bp,color,color(1,i),LineWidth,1); hold on end plot(pose_x(N,1),pose_y(N,1),*,color,color(1,N),LineWidth,1); hold on for i1:N-1 plot(pose_x(i,countmax),pose_y(i,countmax),m^,color,color(1,i),LineWidth,2); hold on end plot(pose_x(N,countmax),pose_y(N,countmax),o,color,color(1,N),LineWidth,2); hold on; grid on; xlabel(x); ylabel(y); legend(跟随者1,跟随者2,跟随者3,跟随者4,领航者); xlabel(x(m)); ylabel(y(m)); title(基于一致性的二阶编队算法); %% 画误差图 cx0:0.1:countmax/10; figure % 生成三维平面图 连续 errorsqrt(error_x.^2error_y.^2); for i1:4 plot(cx(1:countmax-1),error(i,1:countmax-1),color(1,i)); hold on; end legend(跟随者1,跟随者2,跟随者3,跟随者4); xlabel(时间(s)); ylabel(位置误差); title(各跟随者与领航者的误差曲线);3 仿真结果4 参考文献[1]李奕铭. 基于人工势场法的移动机器人避障研究[D]. 合肥工业大学, 2013.[2]刘云平等. 一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法., CN111290429A. 2020.部分理论引用网络文献若有侵权联系博主删除。