【路径规划】基于人工势场求解二维障碍路径规划问题matlab源码

1 简介

 在 1986 年,专家 Oussama Khatib 提出了实时性好、改进空间很大、应用领域也极其广泛的人工势场法去解决机器人避障及路径规划问题,人工势场法的工作原理是:将机器人所在的工作空间虚拟为充满引力势场和斥力势场的空间。目标点与机器人产生的相互作用场为引力势场,障碍物与机器人产生的相互作用场为斥力场,机器人在引力势场和斥力势场的合势场作用下,沿着势函数下降的方向运动,搜索无碰撞最优路径。具体的实现方法是对虚拟的引力场和斥力场分别求负梯度之后,可以得到引力和斥力的大小和方向,机器人在引力和斥力的合力作用下行进至目标点。基于人工势场法的机器人避障最大的特点就是边行走边利用传感器探测可行的避障路径,算法本身的特点就奠定了其相对于其他避障算法对未知环境的适应性较强的优势,并且人工势场法的计算量小、速度快、便于其他人理解,把抽象的机器人避障算法通过建模表现为受力分析、数学计算、向量求解等问题,便于改进和优化,避障轨迹平滑,缺点是会出现目标不可达,局部极小值两种问题,图 1.10 为人工势场法避障原理图,所以人工势场法非常适合应用于未知环境下对避障的实时性要求较高的避障问题求解中去。

img

2 部分代码

 

%% function apf_3

% 人工势场法进行水下机器人路径规划,考虑体积范围

% 计算势函数,画出运动轨迹图像

close all; 

% 设置工作区域

xmin = [0; 0];  

xmax = [50;50];

% Maximum number

Nsteps = 600;

%设置机器人的参数%

% 选定方向上机器人运动步长参数

lambda = 0.1;

Ns=30; 

r = 1; 

xs=0*ones(2,Ns); 

Jo(:,1)=0*ones(Ns,1); 

Jg(:,1)=0*ones(Ns,1); 

J(:,1)=0*ones(Ns,1); 

theta(:,1)=0*ones(Ns,1);

for m=2:Ns

    theta(m,1)=theta(m-1,1)+(pi/180)*(360/Ns); 

end 

  

% 设置目标(Goal/Target)位置坐标

P_Goal=[25; 25];

obstacles = [6 20 11 16 18 19 ;6 16 17 14 11.9 19];

Mat = size(obstacles); %障碍物点数

obNum = Mat(1,2);

nt = 20; % Tar运动步数

nr = 20; % Ro的速度,决定能否跟的上

x1 = 1;

y1 = 1;

g = 1;

h = 0;

distrt = 0; % 计算距离,终止条件

distro = 0*ones(2,obNum); % 计算距离,避让临界

t = 0;

na = 0;

% 设置机器人初始位置坐标

P_Ro=[5; 5]; 

w1 = 1; 

w2 = 5; 

P_Ro(:,2:Nsteps) = 0*ones(2,Nsteps-1);

% 画出势场

xx=0:35/100:35; 

yy=xx; 

% 计算障碍物势函数 

 for jj=1:length(xx) 

    for ii=1:length(yy) 

       op(ii,jj)=obstaclefunction([xx(jj);yy(ii)],w1,obstacles); 

    end 

 end 

% 计算目标势函数

 for jj=1:length(xx) 

    for ii=1:length(yy) 

        gp(ii,jj)=goalfunction([xx(jj);yy(ii)],P_Goal(:,1),w2); 

    end 

 end 

P_RoA = P_Ro(1,1);

P_RoB = P_Ro(2,1);

P_RoC = P_Ro(1,1);

P_RoD = P_Ro(2,1);

potential = gp - op;

figure;

plot(P_Goal(1,1),P_Goal(2,1),'+','MarkerSize',10);

hold on

plot(P_Goal(1,1),P_Goal(2,1),'o','MarkerSize',22);

xlabel('x'); 

ylabel('y'); 

plot(obstacles(1,:),obstacles(2,:),'o', 'MarkerEdgeColor','r','MarkerSize',10); 

contour(xx,yy,potential,90);

axis([0 35 0 35]); 

hold off

figure,

plot(P_Goal(1,1),P_Goal(2,1),'+','MarkerSize',10);

hold on

plot(P_Goal(1,1),P_Goal(2,1),'o','MarkerSize',22);

xlabel('x'); 

ylabel('y'); 

plot(obstacles(1,:),obstacles(2,:),'o','MarkerSize',22); 

plot(obstacles(1,:),obstacles(2,:),'o', 'MarkerEdgeColor','r','MarkerSize',10); 

 contour(xx,yy,op,20);

 axis([0 35 0 35]); 

hold off

figure, 

plot(P_Goal(1,1),P_Goal(2,1),'+','MarkerSize',10);

hold on

plot(P_Goal(1,1),P_Goal(2,1),'o','MarkerSize',22);

xlabel('x'); 

ylabel('y'); 

plot(obstacles(1,:),obstacles(2,:),'o','MarkerSize',22); 

plot(obstacles(1,:),obstacles(2,:),'o', 'MarkerEdgeColor','r','MarkerSize',10); 

 contour(xx,yy,gp,50);

 axis([0 35 0 35]); 

hold off

% Robot运动路径仿真过程

P_Goal_1 = P_Goal(:,1);

for k=1:Nsteps

    % 设定运动边界

    P_Ro(:,k) = min(P_Ro(:,k),xmax); 

    P_Ro(:,k) = max(P_Ro(:,k),xmin); 

    for m=1:Ns 

        xs(:,m) = [P_Ro(1,k)+r*cos(theta(m,1)); P_Ro(2,k)+r*sin(theta(m,1))];      

        % 计算是否进入障碍物的作用范围

        for t = 1:obNum

        distro(:,t) = xs(:,m) - obstacles(:,t);

        end

        sum1 = sum(distro.^2);

        if min(sum1) < 1.69 % 设定障碍物作用距离的平方值

           Jo(m,1) = 100;

        else

           Jo(m,1) = obstaclefunction(xs(:,m),w1,obstacles); 

        end

        Jg(m,1) = goalfunction(xs(:,m),P_Goal_1,w2); 

        J(m,1)= Jg(m,1) - Jo(m,1);

    end 

    

    [val,num] = max(J); 

    distrt = P_Ro(:,k) - P_Goal_1; 

    if sum(distrt.^2) > 0.5        

        P_Ro(:,k+1) = [P_Ro(1,k)+lambda*cos(theta(num,1)); P_Ro(2,k)+lambda*sin(theta(num,1))]; 

    else

        break;

    end

    P_RoA = P_Ro(1,k+1);

    P_RoB = P_Ro(2,k+1);

    P_RoC = P_Ro(1,1:k+1);

    P_RoD = P_Ro(2,1:k+1);

    Deltalambda=0.1*lambda*(2*rand-1); 

    Deltatheta=2*pi*(2*rand-1); 

    P_Ro(:,k+1)=[P_Ro(1,k+1)+Deltalambda*cos(theta(num,1)+Deltatheta); P_Ro(2,k+1)+Deltalambda*sin(theta(num,1)+Deltatheta)]; 

end

figure;

% 障碍物势场

plot(obstacles(1,:),obstacles(2,:),'o','MarkerSize',22);

hold on;

% 机器人运动路径

plot(P_Ro(1,1:k) ,P_Ro(2,1:k) ,'r-'); 

hold on

plot(P_Ro(1,1),P_Ro(2,1),'s', 'MarkerFaceColor','g','MarkerSize',10);

plot(P_Goal(1,1),P_Goal(2,1),'+','MarkerSize',10);

plot(P_Goal(1,1),P_Goal(2,1),'o','MarkerSize',22);

axis([0 35 0 35]); 

xlabel('x'); 

ylabel('y'); 

title('Robot''s path with obstacles'); 

hold off

 

3 仿真结果

4 参考文献

[1]邓学强, DENG, Xue-qiang, School, of, & Computer等. (2014). 基于改进人工势场法的移动机器人路径规划. 山东理工大学学报(自然科学版), 01(v.28;No.134), 42-45.

 

  • 2
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
蚁群算法是一种模拟自然界蚂蚁觅食行为的优化算法,可以应用于路径规划问题求解。下面介绍基于Matlab的蚁群算法三维路径规划。 1. 蚁群算法原理 蚁群算法是一种基于启发式搜索的优化算法,它模拟了蚂蚁觅食的行为。在蚂蚁觅食过程中,蚂蚁会在地面上释放信息素,并根据信息素浓度选择路径。信息素浓度越高的路径,被选中的概率越大。同时,蚂蚁还会在路径上留下新的信息素,增强该路径的信息素浓度。通过这种方式,蚂蚁群体最终找到了最优的食物源。 蚁群算法可以应用于路径规划、组合优化、机器学习等问题求解。 2. 三维路径规划问题 三维路径规划是指在三维空间中寻找一个起点和终点之间的最短路径。在三维空间中,路径可以沿着x、y、z三个方向移动。三维路径规划问题可以应用于航空、机器人导航等领域。 3. 基于Matlab的蚁群算法三维路径规划 下面介绍基于Matlab的蚁群算法三维路径规划的实现步骤。 步骤一:定义问题 定义起点、终点和障碍物。在三维空间中,起点和终点可以用三个坐标表示,障碍物可以用一个三维矩阵表示,矩阵中的元素为1表示该位置为障碍物,为0表示该位置可以通过。 步骤二:初始化信息素和蚂蚁位置 初始化信息素矩阵和蚂蚁位置。信息素矩阵用一个三维矩阵表示,每个元素表示该位置的信息素浓度。蚂蚁位置可以随机初始化。 步骤三:计算路径长度和信息素更新 对于每只蚂蚁,根据信息素浓度选择路径,计算路径长度,并更新路径上的信息素浓度。同时,更新全局最优路径。 步骤四:更新信息素和蚂蚁位置 根据信息素更新公式,更新信息素矩阵。同时,根据蚂蚁数量和信息素浓度选择公式,更新蚂蚁位置。 步骤五:判断是否满足终止条件 判断是否满足终止条件,例如达到最大迭代次数或全局最优路径长度满足给定精度要求。 步骤六:输出结果 输出最优路径和路径长度。 4. Matlab源码 下面是基于Matlab的蚁群算法三维路径规划的代码实现: ```matlab %三维路径规划蚁群算法Matlab代码 clear clc %定义问题 start_point = [1, 1, 1]; %起点 end_point = [10, 10, 10]; %终点 obstacle = ones(10, 10, 10); %障碍物 obstacle(5, 5, 5) = 0; %障碍物中心位置为通路 %参数设置 ant_num = 10; %蚂蚁数量 max_iter = 100; %最大迭代次数 alpha = 1; %信息素重要程度因子 beta = 2; %启发式因子 rho = 0.1; %信息素挥发因子 Q = 1; %信息素常量因子 %初始化信息素和蚂蚁位置 pheromone = ones(10, 10, 10); %信息素矩阵 ant_pos = randi([1, 10], ant_num, 3); %蚂蚁位置 %迭代 best_path = []; %全局最优路径 best_path_length = Inf; %全局最优路径长度 for iter = 1:max_iter for i = 1:ant_num %计算路径长度和信息素更新 path_length = 0; curr_pos = start_point; while ~isequal(curr_pos, end_point) %计算可行的下一步位置 feasible_pos = []; for j = -1:1 for k = -1:1 for l = -1:1 next_pos = curr_pos + [j, k, l]; if any(next_pos < 1) || any(next_pos > 10) %位置越界 continue end if obstacle(next_pos(1), next_pos(2), next_pos(3)) == 1 %位置为障碍物 continue end if ismember(next_pos, ant_pos(i, :), 'rows') %位置已被蚂蚁占据 continue end feasible_pos(end+1, :) = next_pos; end end end %根据信息素浓度和启发式因子选择下一步位置 if isempty(feasible_pos) %无可行位置 break end feasible_pos_pheromone = pheromone(sub2ind(size(pheromone), feasible_pos(:,1), feasible_pos(:,2), feasible_pos(:,3))); feasible_pos_heuristic = 1./((feasible_pos(:,1)-end_point(1)).^2 + (feasible_pos(:,2)-end_point(2)).^2 + (feasible_pos(:,3)-end_point(3)).^2).^beta; pos_prob = feasible_pos_pheromone.^alpha .* feasible_pos_heuristic; pos_prob = pos_prob / sum(pos_prob); next_pos = feasible_pos(randi(size(feasible_pos, 1), 1, 1, [], pos_prob)); %更新路径长度和信息素浓度 path_length = path_length + 1; pheromone(curr_pos(1), curr_pos(2), curr_pos(3)) = (1-rho) * pheromone(curr_pos(1), curr_pos(2), curr_pos(3)) + rho * Q / path_length; curr_pos = next_pos; end %更新全局最优路径 if isequal(curr_pos, end_point) && path_length < best_path_length best_path = [start_point; ant_pos(i, :); end_point]; best_path_length = path_length; end %更新蚂蚁位置 ant_pos(i, :) = curr_pos; end %更新信息素 pheromone = (1-rho) * pheromone; for i = 1:ant_num path_pos = [start_point; ant_pos(i, :)]; for j = 1:size(path_pos, 1)-1 pheromone(path_pos(j,1), path_pos(j,2), path_pos(j,3)) = pheromone(path_pos(j,1), path_pos(j,2), path_pos(j,3)) + Q / best_path_length; end end %判断是否满足终止条件 if best_path_length < eps break end end %输出结果 if isempty(best_path) disp('无可行路径') else disp(['最优路径为:', mat2str(best_path)]) disp(['最优路径长度为:', num2str(best_path_length)]) end ``` 以上是基于Matlab的蚁群算法三维路径规划的实现方法和代码,可以根据具体问题进行修改和调整。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Matlab科研辅导帮

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值