协同任务基于matlab二阶一致性算法多无人机协同编队动态仿真含Matlab源码 1740期

Posted 2022-03-02 紫极神光

一、无人机编队控制简介

无人机编队控制是无人机协同控制领域一项重大基础研究课题, 引起了国内外诸多研究人员的兴趣。无人机编队控制方法主要包括领航跟随法、虚拟结构法、基于行为法和图论法。在无人机编队形成的研究方面, 国内外学者已取得一些成果。传统研究无人机编队的方法常常将无人机视为质点目标, 鲜有考虑编队形成过程中, 无人机间的防碰撞问题。

综上可见四旋翼无人机编队形成过程中机间防碰撞的问题仍是一个值得深入研究的课题。针对该问题, 本文将图论法与领航跟随法相结合, 在所有跟随者能够获得领航者状态信息的通信拓扑结构下, 将四旋翼无人机的动力模型描述为二阶积分器动力系统, 利用二阶一致性算法获得编队形成的轨迹, 并借助四旋翼无人机可悬停的特点, 通过调整集结点高度, 避免无人机在编队形成过程中发生碰撞。

1 一致性算法
所谓一致性是指随着时间的演化, 如果网络中每个节点的值或状态趋于一样, 我们就称采用这种协议或者算法的网络能够实现一致, 一致性算法表征四旋翼无人机之间信息传递的规则。根据当系统拓扑结构中包含有向生成树时, 系统能够到达一致。本文在固定连通拓扑G下研究编队形成控制方法, 领航者向外广播自身的状态信息位置和速度, 其余跟随者接收来自领航者的信息, 跟随者之间的拓扑关系是任意的。

为使四旋翼无人机控制作用更准确有效, 将其动力学模型描述为二阶积分器动力系统,

si和vi是第i个的位置和速度信息;控制变量ui= (uix, uiy, uiz) , uiy、uiz分别对应俯仰、横滚方向的控制力矩, uiz对应驱动无人机位姿变化的4个螺旋桨产生的总的升力T。

本文给出以下一致性算法:

2 编队形成控制策略
编队形成具体步骤如下:

二、部分源代码

function [ output_args ] = formation_avoidance3( input_args )
%%%%一阶算法仿真 非完整型约束
%%%  
%%%ren wei论文算法  设置好重心偏移量和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];  %领航者与自己无误差

    fol_num=4;        
    N=5;             % 4follower and 1 leader
    countmax=2000;
    dt=0.1;
    gama=3;
    di=0.02; %%%重心偏移 单位m
    
    
    K0=0;
    %%% 直线K1 K2都设在0.2左右 k3=0
    %%% 圆不考虑K2=0 K1，K3设为0.2左右
    K1=1;%%%位置偏差线速度调节
    K2=1;%%位置偏差角速度调节
    K3=0.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
%     A=[0 0 0 0 1;     % a(ij)
%        0 0 0 0 1;
%        0 0 0 0 1;
%        0 0 0 0 1;
%        0 0 0 0 0];
    A=[0 1 1 1 3;     % a(ij)
       0 0 0 0 3;
       0 0 0 1 3;
       0 0 1 0 3;
       0 0 0 0 0];
   
    linear_v(:,1)=[0;0;0;0;1];
    angular_w(:,1)=[0;0;0;0;1];
    k=0;
    % 最高速度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_degree=sum(A,2);
    
    %% 开始循环 走顺时针圆周
%     figure;
    for count=1:countmax
        k=k+1;
        linear_v(N,k+1)=linear_v(N,k);%领航者速度不变
        angular_w(N,k+1)=angular_w(N,k);       
%         if count==500
%             linear_v(N,k+1)=0.5;%领航者速度不变
%              angular_w(N,k+1)=0.5;
%         end
%         if count==1000
%             linear_v(N,k+1)=0.5;%领航者速度不变
%              angular_w(N,k+1)=0;
%         end
        
        for i=1:fol_num

            sum_delta_x=0;
            sum_delta_y=0;
            for j=1:N %%考虑邻居对它的影响
                if k==1
                    temp_x=0;
                    temp_y=0;
                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_x=sum_delta_x+A(i,j)*(temp_x+gama*((pose_x(j,k)-pose_x(i,k))-(delta_x(j)-delta_x(i))));
                sum_delta_y=sum_delta_y+A(i,j)*(temp_y+gama*((pose_y(j,k)-pose_y(i,k))-(delta_y(j)-delta_y(i))));
            end
            sum_delta_x=sum_delta_x/in_degree(i);
            sum_delta_y=sum_delta_y/in_degree(i);

            linear_v(i,k+1)=cos(pose_th(i,k))*sum_delta_x+sin(pose_th(i,k))*sum_delta_y;
            angular_w(i,k+1)=(-sin(pose_th(i,k))*sum_delta_x+cos(pose_th(i,k))*sum_delta_y)/di;
            u_old=[linear_v(i,k);angular_w(i,k)];
            u=[linear_v(i,k+1);angular_w(i,k+1)];
            %%%加入速度限制
            u=limit(u_old,u,Kinematic);
           
            old_position=[pose_x(i,k);pose_y(i,k);pose_th(i,k)];
            new_position=motion(old_position,u,dt);
            pose_x(i,k+1)=new_position(1)-di*cos(new_position(3));
            pose_y(i,k+1)=new_position(2)-di*sin(new_position(3));
            pose_th(i,k+1)=new_position(3);
        end

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]秦文静,林勇,戚国庆.基于一致性的无人机编队形成与防碰撞研究[J].电子设计工程. 2018,26(09)