基于改进的人工势场法的机器人路径规划

基于改进的人工势场法的机器人路径规划，加入bug算法避免机器人陷入局部最小

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%   全空间人工势场说明   %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all
clc

x_goal=100;    y_goal=100;    % 小车目标点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%            相关参数            %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k_att=1;       k_rep=20;    % 势场参数 （ 引力系数 斥力系数 ）
L0=100;                     % L0为障碍物区域可对机器人的运动产生影响的最大距离
LO_min=2;                   % L0_min为沿墙行走的安全距离
U_max=5000;    FF_max=200;
obs_maxNum=4;               % 矩形障碍物的最大数目

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%      读取障碍物、绘制障碍物    %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DataDir     =[‘.\dataRect‘];                      % directory of data
DataOrgName =[‘\dataObs\obs11.txt‘];              % File of Obstacle
copyfile（[ DataDir DataOrgName ] ‘.\dataRect\temp\obs.txt‘）;
load -ascii .\dataRect\temp\obs.txt;
delete（‘.\dataRect\temp\obs.txt‘）;
for i=1:obs_maxNum
    x_q（i）=obs（i1）;    y_q（i）=obs（i2）;    x_L（i）=obs（i3）;    y_w（i）=obs（i4）;    theta（i）=obs（i5）;
end
Rect_x=zeros（obs_maxNum5）;  Rect_y=zeros（obs_maxNum5）;
for i=1:obs_maxNum                          %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%???????????????
    x1_（i）=x_q（i）;                             y1_（i）=y_q（i）;
    x2_（i）=x_q（i）+x_L（i）*cos（theta（i））;        y2_（i）=y_q（i）+x_L（i）*sin（theta（i））;
    x3_（i）=x_q（i）+x_L（i）*cos（theta（i））+y_w（i）*cos（pi/2+theta（i））;
    y3_（i）=y_q（i）+x_L（i）*sin（theta（i））+y_w（i）*sin（pi/2+theta（i））;
    x4_（i）=x_q（i）+y_w（i）*cos（pi/2+theta（i））;   y4_（i）=y_q（i）+y_w（i）*sin（pi/2+theta（i））;
    Rect_x（i:）=[x1_（i） x2_（i） x3_（i） x4_（i） x1_（i）];  
    Rect_y（i:）=[y1_（i） y2_（i） y3_（i） y4_（i） y1_（i）];
end

x_min=0.0;  x_max=100.0;  y_min=0.0;   y_max=100.0;         % 绘制点阵区域
point_NumX=100;    point_NumY=100;                          % 绘制点阵密度
result_x=zeros（point_NumX+11）;  result_y=zeros（point_NumY+11）;
result_U=zeros（point_NumY+1point_NumX+1）;  result_sF=zeros（point_NumY+1point_NumX+1）;
result_Dx=zeros（point_NumX+11）;  result_Dy=zeros（point_NumY+11）;
for point_i=1:（point_NumX+1）
    x=x_min+（x_max-x_min）/point_NumX*（point_i-1）;
	for point_j=1:（point_NumY+1）
        y=y_min+（y_max-y_min）/point_NumY*（point_j-1）;        % 绘制点阵

        % 机器人与障碍物碰撞情况
        obs_flag=zeros（1obs_maxNum）;
        for obs_i=1:obs_maxNum 
            if inpolygon（xyRect_x（obs_i:）Rect_y（obs_i:）） > 0    % 点（xy）在矩形障碍物内（有碰撞发生）
                obs_flag（i）=1;                           % 机器人撞到第 i 个障碍物
            end
        end
        % 引力场计算
        L_goal=sqrt（（x-x_goal）^2+（y-y_goal）^2）;          % 机器人到目标位置的距离
        if L_goal<1e-10
            L_goal=1e-10;                                % 防止除零错误发生
        end
        U_att=1/2*k_att*L_goal^2;                        % 机器人引力势场
        F_att=-k_att*[（x-x_goal）（y-y_goal）]

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       7182  2014-03-21 15:04  新建文件夹\BUG_Potential.m

     文件      20901  2013-05-31 10:13  新建文件夹\Bug_Rect_3.m

     文件       3148  2013-06-27 15:34  新建文件夹\Bug_Rect_Plot.m

     文件       4325  2012-12-06 19:47  新建文件夹\calculate_obs1.m

     文件       2881  2014-03-21 15:12  新建文件夹\Point_to_Rect.m

     文件       9915  2013-06-27 17:21  新建文件夹\try_Bug_Rect_Plot.m

     文件       9920  2013-06-27 15:55  新建文件夹\try_Bug_Rect_Plot2.m

     目录          0  2014-05-05 08:59  新建文件夹

----------- ---------  ---------- -----  ----

                58272                    8