【无人机控制】基于RRT算法实现无人机系统在充满障碍物的环境避碰控制附matlab代码

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

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

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

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

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

⛄ 内容介绍

　　经典的路径规划算法大都需要在全局已知空间中对环境进行建模,包括人工势场法,遗传算法,启发式算法,仿生学算法等.由于需要预先构建环境,因此这些方法并不适合解决在高维度空间中的路径规划问题.基于快速扩展随机树(RRT)的路径规划方式其优势在于可以避免对全局环境的构建,通过对状态空间进行随机采样,检测碰撞点,能够有效地解决在平面及三维状态空间下的复杂路径规划问题.通过与人工势场法和A*算法进行比对,确定了RRT算法在复杂环境中解决无人机路径规划问题的优势,在对相关参数进行优化后该方法是概率完备且存在最优解的,同时在固定翼智能集群飞行编队控制及协同项目中应用.

⛄ 部分代码

clearvars

close all

rand('state',1);

x_max = 500;

y_max = 500;

%obstacle = [500,150,200,200];

obstacle1 = [0,200,100,100];

obstacle2 = [200,200,200,100];

obstacle3 = [500,0,100,500];

obstacle4 = [0,500,500,100];

ob_vec=[obstacle1;obstacle2;obstacle3;obstacle4];

EPS = 10;

rho=80;

step=10; %which is EPS in old edition

    BETA=EPS/rho; %my addition

%numNodes = 3000;        

numNodes=2000;

q_start.coord = [10 10];

q_start.dir=pi/3; %My addition

q_start.cost = 0;

q_start.parent = 0;

q_goal.coord = [490 490];

q_goal.cost = 0;

q_goal.dir=0;

nodes(1) = q_start;

figure(1)

set(gcf,'unit','normalized','position',[0.4 0.4 0.5 0.5])

axis equal

axis([0 x_max 0 y_max])

%axis([100 110 100 110])

rectangle('Position',obstacle1,'FaceColor',[0 .5 .5])

hold on

rectangle('Position',obstacle2,'FaceColor',[0 .5 .5])

plot(q_goal.coord(1),q_goal.coord(2),'ro','LineWidth',2)

for i = 1:1:numNodes

    

    i

       

    % Break if goal node is already reached

%     IsReached=0;

%         %     for j = 1:1:length(nodes)

%         %         if dist(nodes(j).coord, q_goal.coord)<5

%         %             IsReached=1;

%         %             break

%         %         end

%         %     end

%     if IsReached

%         break;

%     end

    

    

    if rand(1)<0.3

%         disp('sampling: goal 1')

        q_rand=q_goal;

    else

%         disp('sampling: goal 0')

        q_rand.coord = [floor(rand(1)*x_max) floor(rand(1)*y_max)];

        q_rand.dir=(rand(1)-0.5)*2*pi;

    end

    % Pick the closest node from existing list to branch out from

    ndist = [];

    for j = 1:1:length(nodes)

        n = nodes(j);

        %new func

        tmp = distDubinThree_obs(n, q_rand, rho, ob_vec);

        ndist = [ndist tmp];

    end

    [val, idx] = min(ndist);

    q_near = nodes(idx);

    

    

    q_new.coord = steer_DubinThree(q_near, q_rand, rho, step);

    q_new.dir=steer_DubinThree2(q_near, q_rand, rho, step);

    

    %/* RG-RRT part: the q_goal is closer to q_near or q_new?

    % while(dist(q_rand.coord,q_near.coord)<dist(q_rand.coord,q_new.coord)) || ~noCollision(q_rand.coord, q_near.coord, obstacle1) || ~noCollision(q_new.coord, q_near.coord, obstacle1) || ~noCollision(q_rand.coord, q_near.coord, obstacle2) || ~noCollision(q_new.coord, q_near.coord, obstacle2) 

        

    %/* not RG-RRT    

    while ~noCollision_obs(q_rand.coord, q_near.coord, ob_vec) || ~noCollision_obs(q_new.coord, q_near.coord, ob_vec)  

%         plot(q_rand.coord(1), q_rand.coord(2), 'o', 'Color',  [1 0 0.7410])

        q_rand.coord = [floor(rand(1)*x_max) floor(rand(1)*y_max)];

        q_rand.dir=(rand(1)-0.5)*2*pi;

        % Pick the closest node from existing list to branch out from

        ndist = [];

        for j = 1:1:length(nodes)

            n = nodes(j);

            %jan 28 tmp = dist_Dubin(n, q_rand, BETA, EPS);

            tmp = distDubinThree_obs(n, q_rand, rho, ob_vec);

            ndist = [ndist tmp];

        end

        [val, idx] = min(ndist);

        q_near = nodes(idx);

        

        q_new.coord = steer_DubinThree(q_near, q_rand, rho, step);

        q_new.dir=steer_DubinThree2(q_near, q_rand, rho, step);

    end

%     plot(q_rand.coord(1), q_rand.coord(2), 'x', 'Color',  [0 0.4470 0.7410])

    %*/

    

    % rewiring process

    if noCollision_obs(q_rand.coord, q_near.coord, ob_vec) && noCollision_obs(q_new.coord, q_near.coord, ob_vec)

%         line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], 'Color', 'k', 'LineWidth', 2);

%         drawnow

        hold on

        q_new.cost = distDubinThree(q_near, q_new, rho) + q_near.cost;

        

        % Within a radius of r, find all existing nodes

        q_nearest = [];

        r = 100;

        neighbor_count = 1;

        

        % Initialize cost to currently known value

        q_min = q_near;

        C_min = q_new.cost; 

        

        for j=1:length(nodes)

            if noCollision_obs(nodes(j).coord, q_new.coord,ob_vec) && distDubinThree_obs(nodes(j),q_new, rho, ob_vec)<r

                q_nearest(neighbor_count).coord=nodes(j).coord;

                q_nearest(neighbor_count).dir=nodes(j).dir;

                q_nearest(neighbor_count).cost=nodes(j).cost;

                if nodes(j).cost+distDubinThree_obs(nodes(j),q_new, rho, ob_vec)<C_min-2

                    q_min=nodes(j);

                    C_min=nodes(j).cost+distDubinThree_obs(nodes(j),q_new, rho, ob_vec);

                end

                neighbor_count=neighbor_count+1;

            end

        end 

                

        

        

        

        % Update parent to least cost-from node % near q_near best one

        for j = 1:1:length(nodes)

           if nodes(j).coord == q_min.coord

               q_new.parent = j;

           end

        end

%         q_new.parent=idx; % old q_near

        q_new.cost=C_min; % add by Zhu Wang

%         line([q_min.coord(1),q_new.coord(1)], [q_min.coord(2),q_new.coord(2)],'Color','g');

        % Append to nodes

        nodes = [nodes q_new];

        

        for j=1:length(nodes)-1

            if noCollision_obs(nodes(j).coord, q_new.coord,ob_vec) && distDubinThree_obs(q_new,nodes(j), rho, ob_vec)<r

                if nodes(j).cost>C_min+distDubinThree_obs(q_new, nodes(j),rho,ob_vec)

                    nodes(j).parent=length(nodes);

                    nodes(j).cost=C_min+distDubinThree_obs(q_new, nodes(j),rho,ob_vec);

%                     line([nodes(j).coord(1),q_new.coord(1)], [nodes(j).coord(2),q_new.coord(2)],'Color','y');

                end

            end

        end

        

        

        

    end

    if mod(i,100)==1

        text=['i=',int2str(i-1)];

        title(text)

    end

%     if (mod(i,10)==1)

%         F((i-1)/10+1)=getframe(gcf);

%     end

end

D = [];

for j = 1:1:length(nodes)

    tmpdist = distDubinThree_obs(nodes(j), q_goal, rho,ob_vec);

    D = [D tmpdist];

end

%% Search backwards from goal to start to find the optimal least cost path

path=[q_goal.coord q_goal.dir];

[val, idx] = min(D);

q_final = nodes(idx);

q_goal.parent = idx;

q_end = q_goal;

totaldist=q_final.cost+distDubinThree_obs(q_final,q_goal,rho,ob_vec);

q_goal.cost=totaldist;

nodes = [nodes q_goal];

while q_end.parent ~= 0

    start = q_end.parent;

% %     disp(start)

%     plot(q_end.coord(1), q_end.coord(2),'bv')

%     line([q_end.coord(1), nodes(start).coord(1)], [q_end.coord(2), nodes(start).coord(2)], 'Color', 'r', 'LineWidth', 2);

%     drawnow;

%     hold on;

    q_end = nodes(start);

%     F=[F getframe(gcf)];

    

    path=[q_end.coord q_end.dir; path];

end

% plot(path(:,1),path(:,2),'k--');

⛄ 运行结果

⛄ 参考文献

[1] 王永皎,郑春峰.基于RRT算法的自由飞行空间机器人避碰运动规划[J].农机化研究, 2006(3):3.DOI:10.3969/j.issn.1003-188X.2006.03.017.

[2] 刘强,阳小燕,朱克佳.基于RRT算法的桥检无人机航线规划仿真系统设计[J].计算机科学与应用, 2022(008):012.

[3] 李华忠,梁永生,但唐仁,等.基于RRT的机器人避碰运动规划算法研究[J].深圳信息职业技术学院学报, 2012, 10(3):8.DOI:10.3969/j.issn.1672-6332.2012.03.005.

[4] 马蓉.基于改进RRT算法的无人机航路规划与跟踪方法研究[J].导航定位与授时, 2020, 7(1):6.DOI:CNKI:SUN:DWSS.0.2020-01-004.

⛳️ 代码获取关注我

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

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