导航函数避障形成轨迹的matlab代码

以下是一个简单的导航函数避障形成轨迹的 MATLAB 代码示例：

% 设置起始点和目标点
start = [0, 0];
goal = [10, 10];

% 设置障碍物的坐标
obstacles = [3, 3; 5, 5; 7, 7];

% 设置导航函数的参数
katt = 1; % 引力系数
krep = 100; % 斥力系数
d0 = 1; % 斥力范围

% 生成离散的空间
x = linspace(-2, 12, 100);
y = linspace(-2, 12, 100);

% 计算导航函数
[xx, yy] = meshgrid(x, y);
Uatt = 0.5 * katt * ((xx - goal(1)).^2 + (yy - goal(2)).^2);
Urep = zeros(size(xx));
for i = 1:size(obstacles, 1)
    dist = sqrt((xx - obstacles(i, 1)).^2 + (yy - obstacles(i, 2)).^2);
    Urep(dist < d0) = 0.5 * krep * (1./dist(dist < d0) - 1/d0).^2;
end
Utot = Uatt + Urep;

% 计算梯度
[px, py] = gradient(-Utot);

% 绘制轨迹
xcurr = start(1);
ycurr = start(2);
path = [xcurr, ycurr];
while norm([xcurr, ycurr] - goal) > 0.1
    dx = px(round(ycurr) + 2, round(xcurr) + 2);
    dy = py(round(ycurr) + 2, round(xcurr) + 2);
    xcurr = xcurr + dx * 0.1;
    ycurr = ycurr + dy * 0.1;
    path = [path; xcurr, ycurr];
end
plot(path(:, 1), path(:, 2), 'r-', obstacles(:, 1), obstacles(:, 2), 'ko', start(1), start(2), 'bo', goal(1), goal(2), 'go');

这个代码通过生成一个离散的空间，计算导航函数，然后计算梯度，从起始点开始沿着梯度方向前进，直到到达目标点。在这个过程中，会避开障碍物。这个代码示例中，轨迹是通过在离散空间中以一定步长前进的方式计算得到的。