【路径规划】基于BBO算法的无人机三维路径规划matlab源码

摘要： Alfred Wallace和Charles Darwin在19世纪提出了生物地理学理论，研究生物物种栖息地的分布、迁移和灭绝规律。 Simon受到生物地理学理论的启发，在对生物物种迁移数学模型的研究基础上，于 2008年提出了一种新的智能优化算法 — 生物地理学优化算法（Biogeography-Based Optimization，BBO）。 BBO算法是一种基于生物地理学理论的新型算法，具有良好的收敛性和稳定性，受到越来越多学者的关注。

1.算法原理

BO算法的基本思想来源于生物地理学理论。如图1所示，生物物种生活在多个栖息地（Habitat）上，每个栖息地用栖息适宜指数（Habitat Suitability Index，HSI）表示，与HSI相关的因素有降雨量、植被多样性、地貌特征、土地面积、温度和湿度等，将其称为适宜指数变量（Suitability Index Variables，SIV）。

图1.BBO算法中的多个栖息地

HSI是影响栖息地上物种分布和迁移的重要因素之一。较高 HSI的栖息地物种种类多；反之，较低 HSI的栖息地物种种类少。可见，栖息地的HSI与生物多样性成正比。高 HSI的栖息地由于生存空间趋于饱和等 问题会有大量物种迁出到相邻栖息地，并伴有少量物种迁入；而低 HSI的栖息地其物种数量较少，会有较多物种的迁入和较少物种的迁出。但是，当某一栖息地HSI一直保持较低水平时，则该栖息地上的物种会趋于灭绝，或寻找另外的栖息地，也就是突变。迁移和突变是BBO算法的两个重要操作。栖息地之间通过迁移和突变操作，增强物种间信息的交换与共享，提高物种的多样性。

BBO算法具有一般进化算法简单有效的特性，与其他进化算法具有类似特点。 （1）栖息适宜指数HSI表示优化问题的适应度函数值，类似于遗传算法中的适应度函数。HSI是评价解集好坏的标准。 （2）栖息地表示候选解，适宜指数变量 SIV 表示解的特征，类似于遗传算法中的“基因”。

（3）栖息地的迁入和迁出机制提供了解集中信息交换机制。高 HSI的解以一定的迁出率将信息共享给低HSI的解。 （4）栖息地会根据物种数量进行突变操作，提高种群多样性，使得算法具有较强的自适应能力。

BBO算法的具体流程为： 步骤1 初始化BBO算法参数，包括栖息地数量N NN、迁入率最大值I II和迁出率最大值E EE、最大突变率 m m a x m_{max}mmax 等参数。 步骤2 初始化栖息地，对每个栖息地及物种进行随机或者启发式初始化。 步骤3 计算每个栖息地的适宜指数HSI；判断是否满足停止准则，如果满足就停止，输出最优解；否则转步骤4。 步骤4 执行迁移操作，对每个栖息地计算其迁入率和迁出率，对SIV进行修改，重新计算适宜指数HSI。 步骤5 执行突变操作，根据突变算子更新栖息地物种，重新计算适宜指数HSI。 步骤6 转到步骤3进行下一次迭代。

1.1 迁移操作

如图2所示，该模型为单个栖息地的物种迁移模型。

图2.BBO算法的迁移模型

横坐标为栖息地种群数量 S ，纵坐标为迁移比率 η，λ(s) 和 μ(s) 分别为种群数量的迁入率和迁出率。当种群数量为 0 时，种群的迁出率 μ(s) 为 0，种群的迁入率λ(s) 最大；当种群数量达到 S max 时，种群的迁入率 λ(s)为0，种群迁出率 u(s) 达到最大。当种群数量为 S 0 时，迁出率和迁入率相等，此时达到动态平衡状态。根据图2，得出迁入率和迁出率为： { λ ( s ) = I ( 1 − S / S m a x ) u ( s ) = E S / S m a x (1)

{λ(s)=I(1−S/Smax)u(s)=ES/Smax{λ(s)=I(1−S/Smax)u(s)=ES/Smax

ag{1}{λ(s)=I(1−S/Smax)u(s)=ES/Smax(1) 迁移操作的步骤可以描述为： Step1：for i= 1 to N do Step2：用迁入率λ i λ_iλi 选取x j x^jxj

Step3：if （0，1）之间的均匀随机数小于λ i λ_iλi then Step4：for j= 1 to N do Step5：用迁出率 u i u_iui 选取x j x_jxj Step6：if （0，1）之间的均匀随机数小于 u i u_iui then Step7：从 x j x^jxj中随机选取一个变量SIV Step8：用SIV替换x i x^ixi中的一个随机SIV Step9：end if Step10：end for Step11：end if Step12：end for

1.2 突变（Mutation）操作

突变操作是模拟栖息地生态环境的突变，改变栖息地物种的数量，为栖息地提供物种的多样性，为算法提供更多的搜索目标。栖息地的突变概率与其物种数量概率成反比。即 m s = m m a x ( 1 − P s / P m a x ) (2) m_s = m {max}(1-P_s/P {max}) ag{2}ms=mmax(1−Ps/Pmax)(2) 其中：m m a x m {max}mmax 为最大突变率；P s P_sPs 为栖息地中物种数量为 s ss对应的概率；P m a x P {max}Pmax 为 P s P_sPs 的最大值；m s m_sms 是栖息地中物种数量为 s ss对应的突变概率。

突变操作的步骤可以描述为： Step1：for i= 1 to N do Step2：计算突变概率 P s P_sPs Step3：用突变概率 P s P_sPs 选取一个变量 x i x_ixi Step4：if （0，1）之间的均匀随机数小于 m s m_sms then Step5：随机一个变量代替 x i x^ixi 中的SIV Step6：end if Step7：end for

2 部分代码

close all; clear all; clc; addpath(genpath(’./’)); %% Plan path disp(’Planning ...’); map = load_map(’maps/map4.txt’, 0.1, 0.5, 0.25); start = { [1 -4 1]}; stop = {[0.1 17 3]}; %start = {[0 1 5]}; %stop = {[19 1 5]}; nquad = length(start); for qn = 1:nquad   v = cputime;   path{qn} = bbo(map, start{qn}, stop{qn});   c = cputime - v;   fprintf(’Algo Execution time = %d ’,c); end if nquad == 1   plot_path(map, path{1}); else   % you could modify your plot_path to handle cell input for multiple robots end %% Additional init script init_script; %% Run trajectory trajectory = test_trajectory(start, stop, map, path, true); % with visualization

function [ xtraj , ttraj , terminate_cond ] = test_trajectory ( start , stop , map , path , vis ) % TEST_TRAJECTORY simulates the robot from START to STOP following a PATH % that’s been planned for MAP. % start - a 3d vector or a cell contains multiple 3d vectors % stop - a 3d vector or a cell contains multiple 3d vectors % map   - map generated by your load_map % path - n x 3 matrix path planned by your dijkstra algorithm % vis   - true for displaying visualization %Controller and trajectory generator handles controlhandle = @ controller ; trajhandle   = @ trajectory_generator ; % Make cell if ~ iscell ( start ), start = { start }; end if ~ iscell ( stop ),   stop = { stop }; end if ~ iscell ( path ),   path = { path } ; end % Get nquad nquad = length ( start ); % Make column vector for qn = 1 : nquad     start { qn } = start { qn }(:);     stop { qn } = stop { qn }(:); end % Quadrotor model params = nanoplus (); %% **************************** FIGURES ***************************** % Environment figure if nargin < 5     vis = true ; end fprintf ( ’Initializing figures... ’ ) if vis     h_fig = figure ( ’Name’ , ’Environment’ ); else     h_fig = figure ( ’Name’ , ’Environment’ , ’Visible’ , ’Off’ ); end if nquad == 1     plot_path ( map , path { 1 }); else     % you could modify your plot_path to handle cell input for multiple robots end h_3d = gca ; drawnow ; xlabel ( ’x [m]’ ); ylabel ( ’y [m]’ ); zlabel ( ’z [m]’ ) quadcolors = lines ( nquad ); set ( gcf , ’Renderer’ , ’OpenGL’ ) %% Trying Animation of Blocks NoofBlocks = size ( map (:, 1 ), 1 ); x_0 = map ( 1 , 4 ); x_1 = map ( 2 , 4 ); y_0 = map ( 1 , 5 ); y_1 = map ( 2 , 5 ); z_0 = map ( 1 , 6 ); z_1 = map ( 2 , 6 );     for i = 1 : 2 : NoofBlocks         xb_0 = map ( i , 1 );         xb_1 = map ( i +1 , 1 );         yb_0 = map ( i , 2 );         yb_1 = map ( i +1 , 2 );         zb_0 = map ( i , 3 );         zb_1 = map ( i +1 , 3 );                     B_1 = [ xb_0 yb_0 zb_0 ] ’ ;         B_2 = [ xb_1 yb_0 zb_0 ] ’ ;         B_3 = [ xb_0 yb_0 zb_1 ] ’ ;         B_4 = [ xb_1 yb_0 zb_1 ] ’ ;         B_5 = [ xb_0 yb_1 zb_0 ] ’ ;         B_6 = [ xb_1 yb_1 zb_0 ] ’ ;         B_7 = [ xb_0 yb_1 zb_1 ] ’ ;         B_8 = [ xb_1 yb_1 zb_1 ] ’ ;     %     BlockCoordinatesMatrix(j:j+7,:) = [B_1’;B_2’;B_3’;B_4’;B_5’;B_6’;B_7’;B_8’]; %         BlockCoordinatesMatrix(j:j+1,:) = [B_1’;B_8’]; %         BlockCoordinates(i,:) = {B_1 B_2 B_3 B_4 B_5 B_6 B_7 B_8}; %         j = j+2;         S_1 = [ B_1 B_2 B_4 B_3 ];         S_2 = [ B_5 B_6 B_8 B_7 ];         S_3 = [ B_3 B_4 B_8 B_7 ];         S_4 = [ B_1 B_2 B_6 B_5 ];         S_5 = [ B_1 B_3 B_7 B_5 ];         S_6 = [ B_2 B_4 B_8 B_6 ];         fill3 ([ S_1 ( 1 ,:) ’ S_2 ( 1 ,:) ’ S_3 ( 1 ,:) ’ S_4 ( 1 ,:) ’ S_5 ( 1 ,:) ’ S_6 ( 1 ,:) ’ ],[ S_1 ( 2 ,:) ’ S_2 ( 2 ,:) ’ S_3 ( 2 ,:) ’ S_4 ( 2 ,:) ’ S_5 ( 2 ,:) ’ S_6 ( 2 ,:) ’ ],[ S_1 ( 3 ,:) ’ S_2 ( 3 ,:) ’ S_3 ( 3 ,:) ’ S_4 ( 3 ,:) ’ S_5 ( 3 ,:) ’ S_6 ( 3 ,:) ’ ],[ 1 0 0 ]); %[cell2mat(Block(i,8))/255 cell2mat(Block(i,9))/255 cell2mat(Block(i,10))/255]);         xlabel ( ’x’ ); ylabel ( ’y’ ); zlabel ( ’z’ );         axis ([ min ( x_0 , x_1 ) ( max ( x_0 , x_1 )) min ( y_0 , y_1 ) ( max ( y_0 , y_1 )) min ( z_0 , z_1 ) ( max ( z_0 , z_1 ))])         grid         hold on     end     %% *********************** INITIAL CONDITIONS *********************** fprintf ( ’Setting initial conditions... ’ ) % Maximum time that the quadrotor is allowed to fly time_tol = 50 ;           % maximum simulation time starttime = 0 ;           % start of simulation in seconds tstep     = 0.01 ;       % this determines the time step at which the solution is given cstep     = 0.05 ;       % image capture time interval nstep     = cstep / tstep ; time     = starttime ;   % current time max_iter = time_tol / cstep ;       % max iteration for qn = 1 : nquad     % Get start and stop position     x0 { qn }   = init_state ( start { qn }, 0 );     xtraj { qn } = zeros ( max_iter * nstep , length ( x0 { qn }));     ttraj { qn } = zeros ( max_iter * nstep , 1 ); end % Maximum position error of the quadrotor at goal pos_tol = 0.05 ; % m % Maximum speed of the quadrotor at goal vel_tol = 0.05 ; % m/s x = x0 ;         % state %% ************************* RUN SIMULATION ************************* fprintf ( ’Simulation Running.... ’ ) for iter = 1 : max_iter     timeint = time : tstep : time + cstep ;     tic ;     % Iterate over each quad     for qn = 1 : nquad         % Initialize quad plot         if iter == 1             QP { qn } = QuadPlot ( qn , x0 { qn }, 0.1 , 0.04 , quadcolors ( qn ,:), max_iter , h_3d );             desired_state = trajhandle ( time , qn );             QP { qn } . UpdateQuadPlot ( x { qn }, [ desired_state . pos ; desired_state . vel ], time );             h_title = title ( sprintf ( ’iteration: %d, time: %4.2f’ , iter , time ));         end         % Run simulation       [ tsave , xsave ] = ode45 ( @ ( t , s ) quadEOM ( t , s , qn , controlhandle , trajhandle , params ), timeint , x { qn });         x { qn } = xsave ( end , :) ’ ;         % Save to traj         xtraj { qn }(( iter -1 ) * nstep +1 : iter * nstep ,:) = xsave ( 1 : end -1 ,:);         ttraj { qn }(( iter -1 ) * nstep +1 : iter * nstep )   = tsave ( 1 : end -1 );         % Update quad plot         desired_state = trajhandle ( time + cstep , qn );         QP { qn } . UpdateQuadPlot ( x { qn }, [ desired_state . pos ; desired_state . vel ], time + cstep );     end     set ( h_title , ’String’ , sprintf ( ’iteration: %d, time: %4.2f’ , iter , time + cstep ))     time = time + cstep ; % Update simulation time     t = toc ;     % Pause to make real-time     if ( t < cstep )         pause ( cstep - t );     end     % Check termination criteria     terminate_cond = terminate_check ( x , time , stop , pos_tol , vel_tol , time_tol );     if terminate_cond         break     end end fprintf ( ’Simulation Finished.... ’ ) %% ************************* POST PROCESSING ************************* % Truncate xtraj and ttraj for qn = 1 : nquad     xtraj { qn } = xtraj { qn }( 1 : iter * nstep ,:);     ttraj { qn } = ttraj { qn }( 1 : iter * nstep ); end % Plot the saved position and velocity of each robot if vis     for qn = 1 : nquad         % Truncate saved variables         QP { qn } . TruncateHist ();         % Plot position for each quad         h_pos { qn } = figure ( ’Name’ , [ ’Quad ’ num2str ( qn ) ’ : position’ ]);         plot_state ( h_pos { qn }, QP { qn } . state_hist ( 1 : 3 ,:), QP { qn } . time_hist , ’pos’ , ’vic’ );         plot_state ( h_pos { qn }, QP { qn } . state_des_hist ( 1 : 3 ,:), QP { qn } . time_hist , ’pos’ , ’des’ );         % Plot velocity for each quad         h_vel { qn } = figure ( ’Name’ , [ ’Quad ’ num2str ( qn ) ’ : velocity’ ]);         plot_state ( h_vel { qn }, QP { qn } . state_hist ( 4 : 6 ,:), QP { qn } . time_hist , ’vel’ , ’vic’ );         plot_state ( h_vel { qn }, QP { qn } . state_des_hist ( 4 : 6 ,:), QP { qn } . time_hist , ’vel’ , ’des’ );     end end end

3 仿真结果

4 参考文献

[1]黄宁, 刘刚, 何兵. 基于生物地理学优化的多UCAV协同航迹规划[J]. 计算机仿真, 2013, 30(5):117-120.

5 代码下载

微信扫一扫赞赏作者 赞赏

长按二维码向我转账

受苹果公司新规定影响，微信 iOS 版的赞赏功能被关闭，可通过二维码转账支持公众号。