matlab仿真小车路径规划代码、

以下是一个简单的matlab仿真小车路径规划代码，使用了A*算法实现路径规划。

%% 定义地图
map = zeros(20,20); % 20x20的地图
map(5:15,5:15) = 1; % 障碍物
start = [1,1]; % 起点
goal = [20,20]; % 终点

%% A*算法实现路径规划
path = A_star(map,start,goal);

%% 绘制地图和路径
figure;
imagesc(map); colormap(flipud(gray)); hold on;
plot(start(2),start(1),'bo','MarkerSize',10,'LineWidth',3);
plot(goal(2),goal(1),'go','MarkerSize',10,'LineWidth',3);
if ~isempty(path)
    plot(path(:,2), path(:,1), 'r', 'LineWidth', 2);
end

A*算法的实现如下：

function path = A_star(map,start,goal)
% A*算法实现路径规划

% 初始化
[nrow, ncol] = size(map);
closed = zeros(nrow, ncol);
came_from = zeros(nrow, ncol);
g_score = Inf(nrow, ncol);
f_score = Inf(nrow, ncol);

% 起点的g_score和f_score为0
g_score(start(1), start(2)) = 0;
f_score(start(1), start(2)) = heuristic(start, goal);

% 对于每个节点，记录它的f_score和g_score
open = [start, f_score(start(1), start(2))];

while ~isempty(open)
    % 选出f_score最小的节点
    [~, idx] = min(open(:,3));
    current = open(idx,:);
    
    % 到达终点，返回路径
    if isequal(current(1:2), goal)
        path = reconstruct_path(came_from, current);
        return;
    end
    
    % 从open列表中删除当前节点
    open(idx,:) = [];
    
    % 将当前节点加入closed列表中
    closed(current(1), current(2)) = 1;
    
    % 遍历当前节点的邻居
    for i = -1:1
        for j = -1:1
            % 跳过当前节点和对角线上的邻居
            if i == 0 && j == 0 || abs(i) == abs(j)
                continue;
            end
            
            % 邻居节点的位置
            neighbor = current(1:2) + [i,j];
            
            % 邻居节点在地图范围内，且不是障碍物
            if neighbor(1) >= 1 && neighbor(1) <= nrow && ...
                    neighbor(2) >= 1 && neighbor(2) <= ncol && ...
                    map(neighbor(1), neighbor(2)) == 0
                tentative_g_score = g_score(current(1), current(2)) + ...
                    euclidean_distance(current(1:2), neighbor);
                
                % 如果邻居节点已经在closed列表中，跳过
                if closed(neighbor(1), neighbor(2))
                    continue;
                end
                
                % 如果邻居节点不在open列表中，加入open列表
                if ~ismember(neighbor(1:2), open(:,1:2), 'rows')
                    open = [open; neighbor, Inf];
                end
                
                % 更新邻居节点的g_score和f_score
                if tentative_g_score < g_score(neighbor(1), neighbor(2))
                    came_from(neighbor(1), neighbor(2)) = sub2ind([nrow, ncol], current(1), current(2));
                    g_score(neighbor(1), neighbor(2)) = tentative_g_score;
                    f_score(neighbor(1), neighbor(2)) = g_score(neighbor(1), neighbor(2)) + heuristic(neighbor, goal);
                    idx = find(open(:,1) == neighbor(1) & open(:,2) == neighbor(2));
                    open(idx,3) = f_score(neighbor(1), neighbor(2));
                end
            end
        end
    end
end

% 没有找到路径
path = [];

end

function h = heuristic(a, b)
% 计算启发式函数，这里使用欧几里得距离
h = euclidean_distance(a, b);
end

function d = euclidean_distance(a, b)
% 计算欧几里得距离
d = norm(a - b);
end

function path = reconstruct_path(came_from, current)
% 从came_from列表中重建路径
path = [current(1), current(2)];
while came_from(current(1), current(2)) ~= 0
    current = ind2sub(size(came_from), came_from(current(1), current(2)));
    path = [current(1), current(2); path];
end
end