【无人机】基于RRT算法实现四旋翼无人机的安全和最小能量轨迹规划附matlab代码和报告

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

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

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

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

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

⛄ 内容介绍

The main goal of this work is to find a solution to one of the major limitations that characterizes the Unmanned Aerial Vehicles, namely the reduced flight endurance. In fact, this is typically between 15 and 30 minutes, depending on the model, and different applications were proposed in the literature to alleviate this problem like weight reduction and the improvement of power-to-weight ratio of the brushless DC motors.

In this work, this problem linked to the limited power of the UAVs’ batteries is faced proposing a novel algorithm-level solution: a flatness based trajectory planning strategy is proposed for a quadrotor unmanned aerial vehicle. A feasible trajectory is a trajectory that lies inside the admissible state domain and that does not violate the input constraints. If system constraints are not considered, the trajectory may be unfeasible, and the defined mission may not be accomplished.

The purpose of this work is to implement a safe and minimum energy trajectory since the energy-optimal paths for a rotorcraft has received much less attention in the aerial robotics literature. The safety concept was developed under two perspectives: the first is about the internal mechanisms of the UAV, in fact the input bounds are realistically interpreted as the actuators limits, because when they are hit the actuators cannot deliver the actuation inputs desired by the controller. The second point of view is about the environment where the UAV has to move: if we consider an empty and unbounded environment, the safety is always accomplished, but if we consider boundaries and obstacles in the environment, then a method to generate a path that does not provide collisions is needed.

Indeed, we can define three main objectives for our trajectory planning problem: internal safety, external safety and minimum energy. For these three tasks, we adopted different solutions and then we integrate them in a unique final system able to satisfy all of them. In the next sections, we will show all the steps that lead to the final solution.

⛄ 部分代码

clear

close all

syms tend

tic

%Set start and final point in the usage function which runs the rrt method and returns the path

obstacle = 'no_obstacles.txt';

p = 0.2;

path = RRT(obstacle);

%Test repeated points

dim=size(path);

path_temp=[];

for row=1:dim(1)

    if row ~= dim(1)

        if (norm(path(row,:) - path(row+1,:))) > 2.2

            disp((norm(path(row,:) - path(row+1,:))))

            path_temp = [path_temp ; path(row,:)];

        else

            continue

        end

    else

        path_temp = [path_temp ; path(row,:)];

    end

        

end

path=path_temp;

disp(path)

len_row = size(path);

stringa = ['Total iterations : ', num2str(len_row(1)-1)];

disp(stringa)

%Start optimization process for each section of the path

list_time=[];

%Optimization problem parameters

lb =0.01;

A = [];

b=[];

Aeq= [];

beq=[];

x0 = 2.5;

ub=18;

%Options parameters

options = optimoptions('fmincon','Display','iter','Algorithm','active-set');

%%%%%%%%%%%%%%%%%%%%%%% DEFINE TEST PARAMETERS %%%%%%%%%%%%%%%%%%%%%%%%%%%%

options.MaxFunctionEvaluations =30;

options.ConstraintTolerance =1e-10;

options.StepTolerance =0;

opt_CT = options.ConstraintTolerance;

opt_ST = options.StepTolerance;

opt_MFE = options.MaxFunctionEvaluations;

%Flag:0 inizio; 1 mezzo; 2 fine

passaggio=0;

fin2=size(path);

fin = fin2(1)-1;

for i=1:(len_row(1)-1)

    switch i

        case 1

            passaggio = 0;

        case (len_row(1)-1)

            passaggio = 2;

        otherwise

            passaggio = 1;

    end

    if (i == 1) && (i ==(fin))

        passaggio = 3;

    end

    disp(['Iteration number :',num2str(i)])

    disp(passaggio)

    init_point2 = path(i,:);

    init_point = [init_point2(1) init_point2(2) init_point2(3) 0];

    final_point2 = path(i+1,:);

    final_point = [final_point2(1) final_point2(2) final_point2(3) 0];

    fun = @(tf) double(Function_obj(tf,init_point,final_point,passaggio));

    nonlcon = @(tf) Function_cond(tf,init_point,final_point,p,passaggio);

    disp('Computing...')

    tf = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon,options);

    list_time = [ list_time tf];

    disp(list_time)%19.06

end

tempo_ex =toc;

disp(list_time)

%Build the trajectory point by point

Build_trajectory(path,list_time,p,opt_CT,opt_ST,obstacle);

final_energy = 0;

final_time = sum(list_time);

%Compute total time and energy

for w=1:size(path)-1

    switch w

        case 1

            passaggio = 0;

        case size(path)-1

            passaggio = 2;

        otherwise

            passaggio = 1;

    end

    if (w == 1) && (w ==(fin))

        passaggio = 3;

    end

    init_point2 = path(w,:);

    init_point = [init_point2(1) init_point2(2) init_point2(3) 0];

    final_point2 = path(w+1,:);

    final_point = [final_point2(1) final_point2(2) final_point2(3) 0];

    energy = Function_obj(list_time(w),init_point,final_point,passaggio);

    disp(energy)

    final_energy = final_energy + energy;

end

disp(' Time of the optimization execution')

disp(tempo_ex)

disp('Total time of the trajectory')

disp(final_time)

disp('Total energy employeed')

disp(final_energy)

⛄ 运行结果

⛄ 参考文献

Chamseddine, Abbas & Zhang, Youmin & Rabbath, Camille & Join, Cédric & Theilliol, Didier. (2012).

Flatness-Based Trajectory Planning/Replanning for a Quadrotor Unmanned Aerial Vehicle. IEEE

Transactions on Aerospace Electronic Systems. 48. 2832-2848. 10.1109/TAES.2012.6324664.

Sudhakara, Priyanka & Ganapathy, V. & Sundaran, Karthika. (2017).

Optimal trajectory planning

based on bidirectional spline-RRT

∗ for wheeled mobile robot. 10.1109/SSPS.2017.8071566.

Omerdic, Edin & Roberts, Geoff. (2004). Thruster fault diagnosis and accommodation for open

frame underwater vehicles. Control Engineering Practice. 12. 1575-1598.

10.1016/j.conengprac.2003.12.014.

Fabio Morbidi, Roel Cano, David Lara. Minimum-Energy Path Generation for a Quadrotor UAV. IEEE

International Conference on Robotics and Automation, May 2016, Stockholm, Sweden.

Abdilla, Analiza & Richards, Arthur & Burrow, S.G.. (2015). Power and endurance modelling of

battery-powered rotorcraft. 675-680. 10.1109/IROS.2015.7353445.

Jongerden, Marijn & Haverkort, Boudewijn. (2009). Which battery model to use?. IET Software. 3.

445-457.

DuToit Ryan, Holt Matt, Lyle Megan, Biaz Saad. UAV Collision Avoidance Using RRT* and LOS

Maximization Technical Report #CSSE12 – 03.

⛄ Matlab代码关注​

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

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