多无人机协同多无人机协同目标运输任务附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法       神经网络预测       雷达通信       无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机 

⛄ 内容介绍

This work aims to design two drones able to pick up objects and take them to a new location through a hierarchical control.  For each of the two drones, a route search algorithm has been implemented that allows it to avoid obstacles of the environment. The two UAVs must be synchronized because the second UAV takes the object from the place where the first UAV has brought it. The mass of the objects to be picked is not known and it is estimated using an estimator in run time.This project was chosen due to the interest of both candidates in Aerial Robotics. More and more companies are investing in this sector, understanding the great development margins that this technology can bring. The fields of application of this type of robotics are various: transport, defense, cinema, etc., etc. Both candidates think that the in a close future a great part of the delivery work will be made by drones and also there will be a high request from various companies for unmanned systems capable of moving packages in safe conditions. The presence of many UAVs in the space implies the necessity of coordination between them, and this project tries to solve this issue. The project has set itself the objective not only to apply the topics studied during the Field and Service Robotics course but also to allow us to deepen and acquire skills useful for the working world.

⛄ 部分代码

%procedura per utilizzare il workspace salvato:

� riga 7 clear a riga 63 end 2: ESEGUI

%metti il workspace salvato nel workspace

� 189 takeoffenu a 205 put2: ESEGUI

� 154 tempo a 170 timetake2: ESEGUI

� 176 a 186: esegui

clear

clc

close all

hold off

takeoff = [0,0,0.5];

take = [17,10,14.5];

put = [10,15,16.5];

takeoff2 = [0 20 0.5];

take2 = put;

put2 = [10 3 12.5];

travel=[takeoff take; take put; put takeoff];

travel2=[takeoff2 take2; take2 put2;put2 takeoff2];

Scenario = uavScenario("UpdateRate",100,"ReferenceLocation",[0 0 0]);

InitialPosition=[0 0 0];

InitialOrientation=[0 0 0];

platUAV = uavPlatform("UAV",Scenario, ...

                      "ReferenceFrame","NED", ...

                      "InitialPosition",InitialPosition, ...

                      "InitialOrientation",eul2quat(InitialOrientation));

platUAV2 = uavPlatform("UAV2",Scenario, ...

                      "ReferenceFrame","NED", ...

                      "InitialPosition",InitialPosition, ...

                      "InitialOrientation",eul2quat(InitialOrientation));

updateMesh(platUAV,"quadrotor",{1.2},[0 0 1],eul2tform([0 0 pi]));

updateMesh(platUAV2,"quadrotor",{1.2},[0 0 1],eul2tform([0 0 pi]));

ObstaclePositions = [2 16;5 5;6 2;15 16; 10 10;10 3; 17 10; 10 15;17 4]; 

% Locations of the obstacles

ObstacleHeight = 4;                      % Height of the obstacles

ObstaclesWidth = 1.5;                       % Width of the obstacles

for i = 1:size(ObstaclePositions,1)

    addMesh(Scenario,"polygon", ...

        {[ObstaclePositions(i,1)-ObstaclesWidth*i/7 ...

        ObstaclePositions(i,2)-ObstaclesWidth*i/7; ...

        ObstaclePositions(i,1) ObstaclesWidth*i/7 ...

        ObstaclePositions(i,2)-ObstaclesWidth*i/7; ...

        ObstaclePositions(i,1) ObstaclesWidth*i/7 ...

        ObstaclePositions(i,2) ObstaclesWidth*i/7; ...

        ObstaclePositions(i,1)-ObstaclesWidth*i/7 ...

        ObstaclePositions(i,2) ObstaclesWidth*i/7], ...

        [1.3*i ObstacleHeight*i/2]},0.651*ones(1,3));

end

meshes_vector=[];

for i=1:size(Scenario.Meshes,2)

    meshes_vector=[meshes_vector ...

        collisionMesh(Scenario.Meshes{1,i}.Vertices)];

end

a=4000; %number of internal iterations (increased if iter is increased)

delta=5; %distance for the RRT algorithm

range_goal=3; %radius around the GOAL where we check for the q_new

check_line=5; %number of points to control in a segment connecting 2 nodes

TOTAL_COORD_PATH=[];

TOTAL_COORD_PATH2=[];

%construction of the seventh order polynomial

numpts_for_segment=1000; %number of points for each segment

tf=1; %final time

lin=linspace(0,1,numpts_for_segment); %time vector

des_vel_0=0; �sired velocity at time 0

des_vel_1=0; �sired velocity at time 1

des_acc_0=0; �sired acceleration at time 1

des_acc_1=0; �sired acceleration at time 1

des_jerk_0=0; �sired jerk at time 1

plot3(takeoff2enu(1),takeoff2enu(2),takeoff2enu(3),'o','Color','black',...

    'MarkerSize',10,'MarkerFaceColor','g')

plot3(take2enu(1),take2enu(2),take2enu(3),'o','Color','black','MarkerSize',10,...

    'MarkerFaceColor','y')

plot3(put2enu(1),put2enu(2),put2enu(3),'o','Color','black','MarkerSize',10,...

    'MarkerFaceColor','b')

plot3(takeoffenu(1),takeoffenu(2),takeoffenu(3),'o','Color','black','MarkerSize',10,...

    'MarkerFaceColor','r')

plot3(takeenu(1),takeenu(2),takeenu(3),'o','Color','black','MarkerSize',10,...

    'MarkerFaceColor','m')

for i=1:size(Scenario.Meshes,2)

    show(meshes_vector(i));

end

figure(11)

hold on

title('s')

plot(s_t)

function[NODELIST,PATH,COORD_PATH,found]=RRT(start,goal,a,...

    meshes_vector,delta,check_line,range_goal)

    ADJ=[1]; �jacency matrix at the beginning (1x1 matrix)

    NODELIST=[start]; %list of nodes and their coordinates (X,Y)

    N=1; %number of nodes at the beginning (1)

    found=0; %flag: if a connection with the GOAL node is found

    iteration=1; %INDICE ITERAZIONE GRANDE

    while (iteration <a & found==0)

        iteration=iteration 1;

        x_rand=(rand*22); "x22x22 is the size of the environment

        y_rand=(rand*22);

        z_rand=(rand*22);

    

        q_rand=[x_rand y_rand z_rand]; %a random point in the map

    

        best=10000; �ST EUCLIDEAN DISTANCE FOUND AT THE BEGINNING

        %it must be large, it's a minimum searching algorithm

    

        %rearching q_near

        for j=1:N %search for the closer point to q_rand in the graph

            if(norm([NODELIST(j,:)-q_rand])<best) 

                 %norm: euclidean distance

                 best=norm([NODELIST(j,:)-q_rand]);

                 q_near_index=j;

            end

        end

           

        Dx=x_rand-NODELIST(q_near_index,1); %difference along x y and z

        Dy=y_rand-NODELIST(q_near_index,2);

        Dz=z_rand-NODELIST(q_near_index,3);

        %we choose the distance between q_near and q_new

        %leng<=delta

        if(norm([NODELIST(q_near_index,:)-q_rand])<delta) 

            leng=norm([NODELIST(q_near_index,:)-q_rand]);

        else

            leng=delta; 

        end

        %the unit vector is built

        Vx=Dx/norm([NODELIST(q_near_index,:)-q_rand]);

        Vy=Dy/norm([NODELIST(q_near_index,:)-q_rand]);

        Vz=Dz/norm([NODELIST(q_near_index,:)-q_rand]);

        vnorm=norm([Vx Vy Vz]); %Always one

        %we find q_new on the segment by multiplying the unit vector with

        %the chosen leng and we add the offset given by the coordinates of

        %q_near

        q_new=[NODELIST(q_near_index,1) leng*Vx ...

            NODELIST(q_near_index,2) leng*Vy ...

            NODELIST(q_near_index,3) leng*Vz];

        %we check if the q_new is in collision

        collision_ext=controlloCollisione(q_new,meshes_vector,0.5);

        %we check if the q_new is in a good place

        if(q_new(1,1)>0 & q_new(1,1)<22 ...

            & q_new(1,2)>0 & q_new(1,2)<22 ...

            & q_new(1,3)>0 & q_new(1,3)<22 ...

            & not(collision_ext))

            

            ok=1;

            %we beck a certain number of point on the segment between q_new

            %and q_near by increasing an index that is multiplied by a

            %vector that is long fraction of the distance between the 

            % two points.

            for k=1:check_line

                coord_pt=[(NODELIST(q_near_index,1) leng/check_line*k*Vx)...

                    (NODELIST(q_near_index,2) leng/check_line*k*Vy)...

                    (NODELIST(q_near_index,3) leng/check_line*k*Vz)];

                collision_int=controlloCollisione(coord_pt,meshes_vector,...

                    0.2);

                if(collision_int==1)

                    ok=0;

                end

            end

            %if the point is good we add it to the tree

            if ok==1

                %we enlarge the adjacency matrix by adding a row and a col

                ADJ=[ADJ zeros(N,1)];

                ADJ=[ADJ ; zeros(1,N 1)];

               

                %we put one where is needed

                ADJ(N 1,q_near_index)=1;

                ADJ(q_near_index,N 1)=1;

                ADJ(N 1,N 1)=1;

                N=N 1;

⛄ 运行结果

⛄ 参考文献

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料

这篇好文章是转载于：学新通技术网