用matlab写个简单人工势场避障函数

以下是一个简单的Matlab代码实现人工势场避障函数：

function [v_x, v_y] = artificial_potential_field(x, y, obs_x, obs_y, obs_r, goal_x, goal_y, k_att, k_rep, rep_range)
% x, y：机器人当前位置坐标
% obs_x, obs_y, obs_r：障碍物的位置和半径
% goal_x, goal_y：目标位置坐标
% k_att, k_rep：引力和斥力系数
% rep_range：斥力范围

% 计算机器人到目标点的引力
d_x = goal_x - x;
d_y = goal_y - y;
dist = sqrt(d_x^2 + d_y^2);
att_x = k_att * d_x / dist;
att_y = k_att * d_y / dist;

% 计算机器人到障碍物的斥力
rep_x = 0;
rep_y = 0;
for i = 1:length(obs_x)
    obs_dist = sqrt((x-obs_x(i))^2 + (y-obs_y(i))^2);
    if obs_dist < rep_range
        rep_d_x = x - obs_x(i);
        rep_d_y = y - obs_y(i);
        rep_x = rep_x + k_rep * (1/obs_dist - 1/rep_range) * (rep_d_x / obs_dist^3);
        rep_y = rep_y + k_rep * (1/obs_dist - 1/rep_range) * (rep_d_y / obs_dist^3);
    end
end

% 计算机器人的速度
v_x = att_x + rep_x;
v_y = att_y + rep_y;

这个函数使用了两个参数：机器人当前位置坐标（x，y）和目标位置坐标（goal_x，goal_y）。障碍物的位置和半径（obs_x，obs_y，obs_r）也需要提供。引力系数k_att和斥力系数k_rep用于控制引力和斥力的大小，rep_range是斥力的范围。函数返回机器人在当前位置的速度v_x和v_y，可以用于控制机器人运动。

这个函数的原理是：机器人会受到一个来自目标点的引力，以及来自障碍物的斥力。当机器人越靠近目标点时，引力越大，斥力越小；当机器人越靠近障碍物时，斥力越大，引力越小。通过调整k_att和k_rep的值，可以控制机器人的行动方式