智能车避障路径规划算法研究(matlab)

智能车避障路径规划算法研究(matlab)

基于matlab仿真平台来实现的
蚁群算法


代码（matlab）


人工势场算法

clc;
close all;
clear all;

%%读取图片信息
x = imread('test.png');
map = im2bw(x,0.9);
imshow(map);

%%相关参数初始化设置
start_point = [88 101];
terminal_point = [616 384];
robot_size = 10;
initial_direction = pi/8;
initial_speed = 10;
speed_max = 10;
distanceThreshold=15;
acc_max = 10;
turn_max = pi/18;
current_position = start_point;
current_speed = initial_speed ;
current_direction = initial_direction;
repulsivePotentialScaling=300000;
k=2.8; %计算势能时的参数,
t=1;%用于M（t）=getframe;t++；中
is_find_path=false;
plot_robot(start_point,current_direction,robot_size,start_point,terminal_point);
plot_robot(terminal_point,current_direction,robot_size,start_point,terminal_point);

while ~is_find_path
   
    
    distance_to_terminal =sqrt( (current_position(1)-terminal_point(1))^2 +  (current_position(2)-terminal_point(2))^2);
    angle_to_terminal = atan2(terminal_point(2)-current_position(2) , terminal_point(1)-current_position(1));
    
    if distance_to_terminal<distanceThreshold
        disp('the path has been found');
        break;
    end
    
    Front_distance= front_distance(current_position,current_direction,robot_size,map);
    Left_distance= left_distance(current_position,current_direction,robot_size,map);
    Right_distance= right_distance(current_position,current_direction,robot_size,map);
    Left_front_distance= left_front_distance(current_position,current_direction,robot_size,map);
    Right_front_distance= right_front_distance(current_position,current_direction,robot_size,map);
    
    %计算排斥势能Repulsive potential
    Repulsive_potential1=[(1.0/ Front_distance)^k*cos(current_direction) (1.0/ Front_distance)^k*sin(current_direction)];
    Repulsive_potential2=[(1.0/ Left_distance)^k*sin(current_direction) -(1.0/ Left_distance)^k*cos(current_direction)];
    Repulsive_potential3=[-(1.0/ Right_distance)^k*sin(current_direction) (1.0/ Right_distance)^k*cos(current_direction)];
    Repulsive_potential4=[(1.0/ Left_front_distance)^k*cos(pi/4-current_direction) -(1.0/ Left_front_distance)^k*sin(pi/4-current_direction)];
    Repulsive_potential5=[(1.0/ Right_front_distance)^k*sin(pi/4-current_direction) (1.0/ Right_front_distance)^k*cos(pi/4-current_direction)];
    Repulsive_potential = Repulsive_potential1+Repulsive_potential2+Repulsive_potential3+Repulsive_potential4+Repulsive_potential5;
    
    %计算吸引势能attraction potential
    Attraction_potential =[ distance_to_terminal^k *.000001*cos(angle_to_terminal)   distance_to_terminal^k *.000001*sin(angle_to_terminal)];
    
    %计算总势能
    totalPotential=Attraction_potential -Repulsive_potential*repulsivePotentialScaling;
    
    %计算路径拐角
    current_speed_x = current_speed*cos(current_direction);
    current_speed_y = current_speed*sin(current_direction);
    Steer_setpoint= atan2(current_speed_y+totalPotential(2),current_speed_x+totalPotential(1)) - current_direction;
    if Steer_setpoint>pi, Steer_setpoint=Steer_setpoint-2*pi; end % check to get the angle between -pi and pi
    if Steer_setpoint<-pi, Steer_setpoint =Steer_setpoint+2*pi; end % check to get the angle between -pi and pi
    Steer_setpoint=min([turn_max Steer_setpoint]);
    Steer_setpoint=max([-turn_max Steer_setpoint]);
    current_direction=current_direction+Steer_setpoint;
    
    %计算新的速度值
    current_speed_x = current_speed*cos(current_direction);
    current_speed_y = current_speed*sin(current_direction);
    speed_setpoint= sqrt((current_speed_y+totalPotential(2))^2 + (current_speed_x+totalPotential(1))^2);
     speed_setpoint=min([current_speed+acc_max speed_setpoint]);
     speed_setpoint=max([current_speed-acc_max speed_setpoint]);
     speed_setpoint=min([current_speed speed_max]);
     speed_setpoint=max([current_speed 0]);
     current_speed=speed_setpoint;
    
     new_position=current_position+speed_setpoint*[cos(current_direction) sin(current_direction)];
     current_position=new_position;
     
     
     if is_valid(current_position,map)==0
        error('the point is invalid');
     end
    
     plot_robot(current_position,current_direction,robot_size,start_point,terminal_point);
     m(t)=getframe;
     t=t+1;

end

毕设实现了上述算法基于matlab的路径规划（qq3343067895）