人工势场法matlab程序梳理

人工势场法（Artificial Potential Field Method）是一种常用于无人机、机器人等自主导航的算法，其基本思路是将机器人运动看作是在一个势场中运动，机器人会被势场中的力推动。这里提供一个简单的Matlab程序梳理，帮助理解人工势场法的实现。

1. 初始化机器人和目标点的位置。

% 机器人初始位置
r_pos = [0, 0];
% 目标点位置
goal_pos = [10, 10];

2. 定义参数，包括势场系数、安全半径、最大速度等。

% 势场系数
k_att = 1; % 引力系数
k_rep = 100; % 斥力系数
% 安全半径
safety_radius = 2;
% 最大速度
max_speed = 0.5;

3. 定义计算机器人位置的函数，根据机器人当前位置和目标点位置计算机器人下一步位置。

function [next_pos, heading] = compute_next_pos(r_pos, goal_pos, obs_pos, k_att, k_rep, safety_radius, max_speed)
% 计算机器人下一步位置
% r_pos: 机器人当前位置
% goal_pos: 目标点位置
% obs_pos: 障碍物位置
% k_att: 引力系数
% k_rep: 斥力系数
% safety_radius: 安全半径
% max_speed: 最大速度

% 计算机器人到目标点的距离和方向
dist = norm(r_pos - goal_pos);
heading = (goal_pos - r_pos) / dist;

% 计算机器人受到的斥力和引力
repulsion_force = [0, 0];
for i = 1:size(obs_pos, 1)
    obs_dist = norm(r_pos - obs_pos(i, :));
    if obs_dist < safety_radius
        repulsion_force = repulsion_force + k_rep * (1/obs_dist - 1/safety_radius) * (r_pos - obs_pos(i, :)) / obs_dist^3;
    end
end
attraction_force = k_att * (goal_pos - r_pos) / dist;

% 计算机器人下一步位置
next_pos = r_pos + max_speed * (attraction_force + repulsion_force) / norm(attraction_force + repulsion_force);
end

4. 在主程序中循环计算机器人下一步位置，并更新机器人位置。

% 障碍物位置
obs_pos = [2, 2; 3, 4; 5, 6];

% 循环计算机器人下一步位置
for i = 1:100
    % 计算机器人下一步位置和方向
    [next_pos, heading] = compute_next_pos(r_pos, goal_pos, obs_pos, k_att, k_rep, safety_radius, max_speed);
    
    % 更新机器人位置
    r_pos = next_pos;
    
    % 绘制机器人和障碍物
    clf;
    hold on;
    axis([-2 12 -2 12]);
    plot(r_pos(1), r_pos(2), 'ro');
    plot(goal_pos(1), goal_pos(2), 'gx');
    for j = 1:size(obs_pos, 1)
        plot(obs_pos(j, 1), obs_pos(j, 2), 'bx');
    end
    drawnow;
    
    % 判断机器人是否到达目标点
    if norm(r_pos - goal_pos) < safety_radius
        break;
    end
end

以上就是一个简单的人工势场法Matlab程序梳理，实现了机器人在有障碍物的环境中到达目标点的自主导航。