基于matlab实现改进的人工势场法，apf算法进行路径规划，通过改进斥力函数和引入模拟退火算法，克服局部极小值和目标不可达等问题

于 2025-03-01 06:30:00 发布
阅读量1k 收藏 11
点赞数 12
分类专栏： 模型预测 文章标签： 算法 matlab 模拟退火算法
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/2501_90804007/article/details/145900817
版权

基于matlab实现改进的人工势场法，apf算法进行路径规划，通过改进斥力函数和引入模拟退火算法，克服局部极小值和目标不可达问题。并附送未改进的apf算法，可做改进与未改进的效果比对使用，如图，未改进的算法会陷入局部极小值从而导致路径规划失败。起始点位置，障碍物位置可以根据自己需求随便设置更改，自己设置地图进行路径规划，可自动生成斥力场，引力场，合力场。
为了实现你所描述的基于改进人工势场法（APF）的路径规划，我们可以先构建一个基本的人工势场算法，然后对其进行改进。下面分别给出未改进和改进后的MATLAB代码示例。
在这里插入图片描述

文章目录

未改进的人工势场法（APF）

function [path, U_att, U_rep] = basic_APF(start, goal, obstacles, k_att, k_rep, rr)
    % start: 起始点位置 [x y]
    % goal: 目标点位置 [x y]
    % obstacles: 障碍物位置矩阵，每行表示一个障碍物的位置 [x y]
    % k_att: 引力系数
    % k_rep: 斥力系数
    % rr: 斥力作用半径

    % 初始化参数
    max_iter = 1000;
    step_size = 0.5;
    path = [];
    robot_pos = start;

    for i = 1:max_iter
        % 计算引力场
        U_att = k_att * sqrt(sum((goal - robot_pos).^2));
        F_att = k_att * (goal - robot_pos) / U_att;

        % 计算斥力场
        F_rep = [0; 0];
        for j = 1:size(obstacles, 1)
            dist = norm(obstacles(j,:) - robot_pos);
            if dist <= rr
                F_rep = F_rep + k_rep * ((1/dist - 1/rr) / dist^2) * (robot_pos - obstacles(j,:))';
            end
        end

        % 合力计算
        F_total = F_att + F_rep';

        % 更新机器人位置
        robot_pos = robot_pos + step_size * F_total / norm(F_total);

        % 存储路径
        path = [path; robot_pos];

        % 检查是否到达目标
        if norm(goal - robot_pos) < step_size
            break;
        end
    end
end

改进的人工势场法（引入模拟退火算法和改进斥力函数）

为了克服局部极小值问题，我们可以通过以下方式改进：

  • 改进斥力函数，使其在接近障碍物时产生更强的斥力。
  • 引入模拟退火算法来帮助逃离局部极小值。
function [path, U_att, U_rep] = improved_APF(start, goal, obstacles, k_att, k_rep, rr, T_initial, alpha)
    % start: 起始点位置 [x y]
    % goal: 目标点位置 [x y]
    % obstacles: 障碍物位置矩阵，每行表示一个障碍物的位置 [x y]
    % k_att: 引力系数
    % k_rep: 斥力系数
    % rr: 斥力作用半径
    % T_initial: 初始温度
    % alpha: 温度衰减率

    % 初始化参数
    max_iter = 1000;
    step_size = 0.5;
    T = T_initial;
    path = [];
    robot_pos = start;

    for i = 1:max_iter
        % 计算引力场
        U_att = k_att * sqrt(sum((goal - robot_pos).^2));
        F_att = k_att * (goal - robot_pos) / U_att;

        % 改进后的斥力场计算
        F_rep = [0; 0];
        for j = 1:size(obstacles, 1)
            dist = norm(obstacles(j,:) - robot_pos);
            if dist <= rr
                F_rep = F_rep + k_rep * exp(-dist/rr) * (robot_pos - obstacles(j,:))';
            end
        end

        % 合力计算
        F_total = F_att + F_rep';

        % 模拟退火算法部分
        new_pos = robot_pos + step_size * F_total / norm(F_total);
        delta_U = norm(new_pos - goal) - norm(robot_pos - goal);
        if delta_U < 0 || exp(-delta_U/T) > rand()
            robot_pos = new_pos;
        end

        % 存储路径
        path = [path; robot_pos];

        % 更新温度
        T = alpha * T;

        % 检查是否到达目标
        if norm(goal - robot_pos) < step_size
            break;
        end
    end
end

以上代码展示了如何使用MATLAB实现基本的人工势场法以及通过改进斥力函数和引入模拟退火算法来改进该方法。你可以根据自己的需求调整参数如k_att, k_rep, rr, T_initial, 和 alpha等，以适应不同的场景和要求。
在这里插入图片描述
为了实现你所描述的改进人工势场法（APF）路径规划，并通过引入模拟退火算法来克服局部极小值和目标不可达问题，我们可以分步骤进行。以下是未改进和改进后的MATLAB代码示例。

未改进的人工势场法（APF）

function [path, U_att, U_rep] = basic_APF(start, goal, obstacles, k_att, k_rep, rr)
    % start: 起始点位置 [x y]
    % goal: 目标点位置 [x y]
    % obstacles: 障碍物位置矩阵，每行表示一个障碍物的位置 [x y]
    % k_att: 引力系数
    % k_rep: 斥力系数
    % rr: 斥力作用半径

    % 初始化参数
    max_iter = 1000;
    step_size = 0.5;
    path = [];
    robot_pos = start;

    for i = 1:max_iter
        % 计算引力场
        U_att = k_att * sqrt(sum((goal - robot_pos).^2));
        F_att = k_att * (goal - robot_pos) / U_att;

        % 计算斥力场
        F_rep = [0; 0];
        for j = 1:size(obstacles, 1)
            dist = norm(obstacles(j,:) - robot_pos);
            if dist <= rr
                F_rep = F_rep + k_rep * ((1/dist - 1/rr) / dist^2) * (robot_pos - obstacles(j,:))';
            end
        end

        % 合力计算
        F_total = F_att + F_rep';

        % 更新机器人位置
        robot_pos = robot_pos + step_size * F_total / norm(F_total);

        % 存储路径
        path = [path; robot_pos];

        % 检查是否到达目标
        if norm(goal - robot_pos) < step_size
            break;
        end
    end
end

改进的人工势场法（引入模拟退火算法和改进斥力函数）

function [path, U_att, U_rep] = improved_APF(start, goal, obstacles, k_att, k_rep, rr, T_initial, alpha)
    % start: 起始点位置 [x y]
    % goal: 目标点位置 [x y]
    % obstacles: 障碍物位置矩阵，每行表示一个障碍物的位置 [x y]
    % k_att: 引力系数
    % k_rep: 斥力系数
    % rr: 斥力作用半径
    % T_initial: 初始温度
    % alpha: 温度衰减率

    % 初始化参数
    max_iter = 1000;
    step_size = 0.5;
    T = T_initial;
    path = [];
    robot_pos = start;

    for i = 1:max_iter
        % 计算引力场
        U_att = k_att * sqrt(sum((goal - robot_pos).^2));
        F_att = k_att * (goal - robot_pos) / U_att;

        % 改进后的斥力场计算
        F_rep = [0; 0];
        for j = 1:size(obstacles, 1)
            dist = norm(obstacles(j,:) - robot_pos);
            if dist <= rr
                F_rep = F_rep + k_rep * exp(-dist/rr) * (robot_pos - obstacles(j,:))';
            end
        end

        % 合力计算
        F_total = F_att + F_rep';

        % 模拟退火算法部分
        new_pos = robot_pos + step_size * F_total / norm(F_total);
        delta_U = norm(new_pos - goal) - norm(robot_pos - goal);
        if delta_U < 0 || exp(-delta_U/T) > rand()
            robot_pos = new_pos;
        end

        % 存储路径
        path = [path; robot_pos];

        % 更新温度
        T = alpha * T;

        % 检查是否到达目标
        if norm(goal - robot_pos) < step_size
            break;
        end
    end
end

使用示例

% 参数设置
start = [0, 0]; % 起始点
goal = [10, 10]; % 目标点
obstacles = [2, 2; 4, 4; 6, 6]; % 障碍物位置
k_att = 1; % 引力系数
k_rep = 10; % 斥力系数
rr = 2; % 斥力作用半径
T_initial = 100; % 初始温度
alpha = 0.99; % 温度衰减率

% 运行未改进的APF
[path_basic, U_att, U_rep] = basic_APF(start, goal, obstacles, k_att, k_rep, rr);

% 运行改进的APF
[path_improved, U_att, U_rep] = improved_APF(start, goal, obstacles, k_att, k_rep, rr, T_initial, alpha);

% 绘制路径
figure;
plot(path_basic(:,1), path_basic(:,2), 'b', 'DisplayName', 'Basic APF');
hold on;
plot(path_improved(:,1), path_improved(:,2), 'r', 'DisplayName', 'Improved APF');
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'DisplayName', 'Start');
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'DisplayName', 'Goal');
plot(obstacles(:,1), obstacles(:,2), 'ko', 'MarkerSize', 10, 'DisplayName', 'Obstacles');
legend('show');
title('Comparison of Basic and Improved APF');
xlabel('X');
ylabel('Y');
grid on;
axis equal;

以上代码展示了如何使用MATLAB实现基本的人工势场法以及通过改进斥力函数和引入模拟退火算法来改进该方法。你可以根据自己的需求调整参数如k_att, k_rep, rr, T_initial, 和 alpha等，以适应不同的场景和要求。
在这里插入图片描述
为了实现你所描述的改进人工势场法（APF）路径规划，并通过引入模拟退火算法来克服局部极小值和目标不可达问题，我们可以分步骤进行。以下是未改进和改进后的MATLAB代码示例。

未改进的人工势场法（APF）

function [path, U_att, U_rep] = basic_APF(start, goal, obstacles, k_att, k_rep, rr)
    % start: 起始点位置 [x y]
    % goal: 目标点位置 [x y]
    % obstacles: 障碍物位置矩阵，每行表示一个障碍物的位置 [x y]
    % k_att: 引力系数
    % k_rep: 斥力系数
    % rr: 斥力作用半径

    % 初始化参数
    max_iter = 1000;
    step_size = 0.5;
    path = [];
    robot_pos = start;

    for i = 1:max_iter
        % 计算引力场
        U_att = k_att * sqrt(sum((goal - robot_pos).^2));
        F_att = k_att * (goal - robot_pos) / U_att;

        % 计算斥力场
        F_rep = [0; 0];
        for j = 1:size(obstacles, 1)
            dist = norm(obstacles(j,:) - robot_pos);
            if dist <= rr
                F_rep = F_rep + k_rep * ((1/dist - 1/rr) / dist^2) * (robot_pos - obstacles(j,:))';
            end
        end

        % 合力计算
        F_total = F_att + F_rep';

        % 更新机器人位置
        robot_pos = robot_pos + step_size * F_total / norm(F_total);

        % 存储路径
        path = [path; robot_pos];

        % 检查是否到达目标
        if norm(goal - robot_pos) < step_size
            break;
        end
    end
end

改进的人工势场法（引入模拟退火算法和改进斥力函数）

function [path, U_att, U_rep] = improved_APF(start, goal, obstacles, k_att, k_rep, rr, T_initial, alpha)
    % start: 起始点位置 [x y]
    % goal: 目标点位置 [x y]
    % obstacles: 障碍物位置矩阵，每行表示一个障碍物的位置 [x y]
    % k_att: 引力系数
    % k_rep: 斥力系数
    % rr: 斥力作用半径
    % T_initial: 初始温度
    % alpha: 温度衰减率

    % 初始化参数
    max_iter = 1000;
    step_size = 0.5;
    T = T_initial;
    path = [];
    robot_pos = start;

    for i = 1:max_iter
        % 计算引力场
        U_att = k_att * sqrt(sum((goal - robot_pos).^2));
        F_att = k_att * (goal - robot_pos) / U_att;

        % 改进后的斥力场计算
        F_rep = [0; 0];
        for j = 1:size(obstacles, 1)
            dist = norm(obstacles(j,:) - robot_pos);
            if dist <= rr
                F_rep = F_rep + k_rep * exp(-dist/rr) * (robot_pos - obstacles(j,:))';
            end
        end

        % 合力计算
        F_total = F_att + F_rep';

        % 模拟退火算法部分
        new_pos = robot_pos + step_size * F_total / norm(F_total);
        delta_U = norm(new_pos - goal) - norm(robot_pos - goal);
        if delta_U < 0 || exp(-delta_U/T) > rand()
            robot_pos = new_pos;
        end

        % 存储路径
        path = [path; robot_pos];

        % 更新温度
        T = alpha * T;

        % 检查是否到达目标
        if norm(goal - robot_pos) < step_size
            break;
        end
    end
end

使用示例

% 参数设置
start = [0, 0]; % 起始点
goal = [10, 10]; % 目标点
obstacles = [2, 2; 4, 4; 6, 6]; % 障碍物位置
k_att = 1; % 引力系数
k_rep = 10; % 斥力系数
rr = 2; % 斥力作用半径
T_initial = 100; % 初始温度
alpha = 0.99; % 温度衰减率

% 运行未改进的APF
[path_basic, U_att, U_rep] = basic_APF(start, goal, obstacles, k_att, k_rep, rr);

% 运行改进的APF
[path_improved, U_att, U_rep] = improved_APF(start, goal, obstacles, k_att, k_rep, rr, T_initial, alpha);

% 绘制路径
figure;
plot(path_basic(:,1), path_basic(:,2), 'b', 'DisplayName', 'Basic APF');
hold on;
plot(path_improved(:,1), path_improved(:,2), 'r', 'DisplayName', 'Improved APF');
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'DisplayName', 'Start');
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'DisplayName', 'Goal');
plot(obstacles(:,1), obstacles(:,2), 'ko', 'MarkerSize', 10, 'DisplayName', 'Obstacles');
legend('show');
title('Comparison of Basic and Improved APF');
xlabel('X');
ylabel('Y');
grid on;
axis equal;

生成势场图

为了生成势场图，我们可以使用以下代码：

function plot_potential_fields(start, goal, obstacles, k_att, k_rep, rr)
    % 设置网格
    [X, Y] = meshgrid(linspace(-10, 20, 100), linspace(-10, 20, 100));
    [XX, YY] = meshgrid(X, Y);
    Z = zeros(size(XX));

    % 计算引力场
    for i = 1:length(X)
        for j = 1:length(Y)
            pos = [X(i,j), Y(i,j)];
            U_att = k_att * sqrt(sum((goal - pos).^2));
            Z(i,j) = U_att;
        end
    end

    % 计算斥力场
    for i = 1:length(X)
        for j = 1:length(Y)
            pos = [X(i,j), Y(i,j)];
            U_rep = 0;
            for k = 1:size(obstacles, 1)
                dist = norm(obstacles(k,:) - pos);
                if dist <= rr
                    U_rep = U_rep + k_rep * exp(-dist/rr);
                end
            end
            Z(i,j) = Z(i,j) + U_rep;
        end
    end

    % 绘制势场图
    figure;
    surf(X, Y, Z);
    hold on;
    plot3(start(1), start(2), 0, 'go', 'MarkerSize', 10, 'DisplayName', 'Start');
    plot3(goal(1), goal(2), 0, 'ro', 'MarkerSize', 10, 'DisplayName', 'Goal');
    plot3(obstacles(:,1), obstacles(:,2), 0, 'ko', 'MarkerSize', 10, 'DisplayName', 'Obstacles');
    legend('show');
    title('Total Potential Field');
    xlabel('X');
    ylabel('Y');
    zlabel('Potential');
    grid on;
end

使用示例

% 参数设置
start = [0, 0]; % 起始点
goal = [10, 10]; % 目标点
obstacles = [2, 2; 4, 4; 6, 6]; % 障碍物位置
k_att = 1; % 引力系数
k_rep = 10; % 斥力系数
rr = 2; % 斥力作用半径

% 绘制势场图
plot_potential_fields(start, goal, obstacles, k_att, k_rep, rr);

这些代码片段展示了如何使用MATLAB实现基本的人工势场法以及通过改进斥力函数和引入模拟退火算法来改进该方法。

评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

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

余额充值