✅作者简介：热爱科研的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:
%da riga 7 clear a riga 63 end 2: ESEGUI
%metti il workspace salvato nel workspace
%da 189 takeoffenu a 205 put2: ESEGUI
%da 154 tempo a 170 timetake2: ESEGUI
%da 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; %desired velocity at time 0
des_vel_1=0; %desired velocity at time 1
des_acc_0=0; %desired acceleration at time 1
des_acc_1=0; %desired acceleration at time 1
des_jerk_0=0; %desired 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]; %Adjacency 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); %22x22x22 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; %BEST 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;
