以下是将上述 **Python 版改进A*算法（带动态权重与路径平滑）** 转换为 **MATLAB 和 C语言版本** 的完整实现代码。两个版本均保持原逻辑一致，适用于无人船航迹规划场景，并针对各自语言特性进行了优化。 --- ## ✅ 一、MATLAB 版本实现 ```matlab % ========================================================================= % 改进A*算法（带动态权重 + B样条路径平滑） % 环境：MATLAB R2018b 及以上 % 功能：栅格地图中全局路径搜索 + 平滑处理 + 动态障碍检测 % ========================================================================= clear; clc; close all; %% 参数设置 GRID_SIZE = 50; OBSTACLE_RATIO = 0.3; START = [6, 6]; % MATLAB索引从1开始 GOAL = [41, 41]; W_MAX = 3.0; H_START_ESTIMATE = 50; G_SCALE = 0.3; % 8方向移动 (dx, dy) 与对应代价 DIRECTIONS = [-1,-1; -1,0; -1,1; 0,-1; 0,1; 1,-1; 1,0; 1,1]; COSTS = [sqrt(2); 1; sqrt(2); 1; 1; sqrt(2); 1; sqrt(2)]; %% 生成地图 grid = generateMap(GRID_SIZE, OBSTACLE_RATIO, START, GOAL); %% 运行改进A*算法 [path, visited] = improvedAStar(grid, START, GOAL, W_MAX, H_START_ESTIMATE, G_SCALE, DIRECTIONS, COSTS); if isempty(path) disp('路径规划失败'); return; end fprintf('原始路径长度: %d\n', length(path)); %% 路径平滑（B样条） smoothed_path = smoothPath(path, 80, 0.5); %% 模拟动态障碍（位置, 速度） dynamicObstacles = {[30, 35], [0, -1]; ... [20, 10], [1, 0]}; % 检测是否需要重规划 if needReplan(path(1:min(10,end),:), dynamicObstacles, 2.0) disp('检测到碰撞风险，触发重规划...'); [path, ~] = improvedAStar(grid, path(1,:), GOAL, W_MAX, H_START_ESTIMATE, G_SCALE, DIRECTIONS, COSTS); smoothed_path = smoothPath(path, 80, 0.5); end %% 可视化 figure('Position', [100, 100, 900, 700]); imshow(grid, 'InitialMagnification', 'fit', 'Colormap', gray); hold on; % 绘制搜索区域 vis_x = arrayfun(@(p) p(1), visited)'; vis_y = arrayfun(@(p) p(2), visited)'; scatter(vis_y, vis_x, 2, 'y', 'filled', 'MarkerFaceAlpha', 0.5); title('搜索区域'); % 原始路径 raw_x = path(:,2); raw_y = path(:,1); plot(raw_x, raw_y, 'r--', 'LineWidth', 2, 'DisplayName', '原始路径'); % 平滑路径 sm_x = smoothed_path(:,2); sm_y = smoothed_path(:,1); plot(sm_x, sm_y, 'g-', 'LineWidth', 3, 'DisplayName', '平滑路径'); % 起终点 scatter(START(2), START(1), 100, 'b', 'filled', 'DisplayName', '起点'); scatter(GOAL(2), GOAL(1), 100, 'g', 'filled', 'DisplayName', '终点'); % 动态障碍 for i = 1:size(dynamicObstacles,1) pos = dynamicObstacles{i,1}; scatter(pos(2), pos(1), 80, 'orange', 's', 'filled'); end text(dynamicObstacles{1,1}(2)+1, dynamicObstacles{1,1}(1)-1, '动态障碍', 'Color','orange'); legend('show', 'Location', 'bestoutside'); xlabel('X坐标'); ylabel('Y坐标'); title('改进A*算法航迹规划（MATLAB版）'); grid on; axis equal tight; hold off; %% =================== 函数定义区 =================== function grid = generateMap(N, ratio, start, goal) grid = zeros(N,N); num_obs = floor(N*N*ratio); idxs = randperm(N*N, num_obs); [rows, cols] = ind2sub([N,N], idxs); for i = 1:length(rows) grid(rows(i), cols(i)) = 1; end % 手动添加岛礁 for j = 15:35 grid(25,j) = 1; end % 清除起终点障碍 grid(start(1), start(2)) = 0; grid(goal(1), goal(2)) = 0; end function dist = euclidean(p1, p2) dist = sqrt(sum((p1 - p2).^2)); end function w = dynamicWeight(g_val, h_val, h_start, w_max) norm_g = g_val / (G_SCALE * h_start + eps); if norm_g < 0.3 w = w_max; elseif norm_g < 0.7 ratio = (norm_g - 0.3)/0.4; w = w_max - ratio*(w_max - 1.5); else w = max(1.0, 1.5 - 0.5*norm_g); end end function valid = isValid(pos, grid) x = pos(1); y = pos(2); valid = (x >= 1 && x <= size(grid,1) && y >= 1 && y <= size(grid,2) && grid(x,y)==0); end function [path, visited] = improvedAStar(grid, start, goal, W_MAX, H_START_ESTIMATE, G_SCALE, DIRS, COSTS) openSet = struct('f', {}, 'node', {}); fStart = euclidean(start, goal); openSet(end+1) = struct('f', fStart, 'node', start); gScore(start(1), start(2)) = 0; cameFrom = cell(size(grid)); visited = {}; while ~isempty(openSet) [~, idx] = min([openSet.f]); currNode = openSet(idx).node; openSet(idx) = []; if isequal(currNode, goal) path = reconstructPath(cameFrom, currNode, start); return; end if any(cellfun(@(p) isequal(p, currNode), visited)) continue; end visited{end+1} = currNode; for i = 1:size(DIRS,1) neighbor = currNode + DIRS(i,:); if ~isValid(neighbor, grid) continue; end tentative_g = gScore(currNode(1),currNode(2)) + COSTS(i); cur_g = gScore(neighbor(1),neighbor(2)); if isnan(cur_g) || tentative_g < cur_g cameFrom{neighbor(1),neighbor(2)} = currNode; gScore(neighbor(1),neighbor(2)) = tentative_g; h = euclidean(neighbor, goal); w = dynamicWeight(tentative_g, h, H_START_ESTIMATE, W_MAX); f = tentative_g + w * h; openSet(end+1) = struct('f', f, 'node', neighbor); end end end path = []; end function path = reconstructPath(cameFrom, current, start) path = {}; while ~isempty(current) && ~isequal(current, start) path{end+1} = current; current = cameFrom{current(1),current(2)}; end path{end+1} = start; path = flipud(cell2mat(path)); end function sp = smoothPath(path, nPoints, sFactor) if size(path,1) < 3 sp = path; return; end x = path(:,2); y = path(:,1); tck = csaps((1:length(x))', [x'; y'], sFactor); ui = linspace(1, length(x), nPoints)'; uv = ppval(tck, ui); sp = [uv(2,:), uv(1,:)]'; end function collision = needReplan(segment, dynObs, safeDist) collision = false; for i = 1:size(segment,1) px = segment(i,1); py = segment(i,2); for j = 1:size(dynObs,1) dp = dynObs{j,1} + dynObs{j,2}; % 预测下一时刻位置 d = euclidean([px,py], dp); if d < safeDist collision = true; return; end end end end ``` --- ## ✅ 二、C语言版本实现（基于标准C99） > ⚠️ 使用堆结构需手动实现最小堆（简化为数组+线性查找），适合嵌入式部署。 ```c #include <stdio.h> #include <stdlib.h> #include <math.h> #include <string.h> #define GRID_SIZE 50 #define MAX_NODES (GRID_SIZE * GRID_SIZE) #define W_MAX 3.0 #define H_START_EST 50.0 #define G_SCALE 0.3 #define INF 1e9 #define TRUE 1 #define FALSE 0 // 节点结构 typedef struct { int x, y; double g, f; } Node; // 方向和代价 int dx[8] = {-1, -1, -1, 0, 0, 1, 1, 1}; int dy[8] = {-1, 0, 1, -1, 1, -1, 0, 1}; double cost[8] = {M_SQRT2, 1.0, M_SQRT2, 1.0, 1.0, M_SQRT2, 1.0, M_SQRT2}; // 地图与路径 int grid[GRID_SIZE][GRID_SIZE]; double g_score[GRID_SIZE][GRID_SIZE]; double f_score[GRID_SIZE][GRID_SIZE]; int visited[GRID_SIZE][GRID_SIZE]; int parent[GRID_SIZE][GRID_SIZE][2]; // 工具函数 double euclidean(int x1, int y1, int x2, int y2) { return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)); } double dynamic_weight(double g_val, double h_val) { double norm_g = g_val / (G_SCALE * H_START_EST + 1e-6); if (norm_g < 0.3) return W_MAX; else if (norm_g < 0.7) { double ratio = (norm_g - 0.3) / 0.4; return W_MAX - ratio * (W_MAX - 1.5); } else { return fmax(1.0, 1.5 - 0.5 * norm_g); } } int is_valid(int x, int y) { return (x >= 0 && x < GRID_SIZE && y >= 0 && y < GRID_SIZE && !grid[x][y]); } // 初始化地图 void init_grid() { memset(grid, 0, sizeof(grid)); memset(visited, 0, sizeof(visited)); for (int i = 0; i < GRID_SIZE; ++i) for (int j = 0; j < GRID_SIZE; ++j) { g_score[i][j] = INF; f_score[i][j] = INF; } // 添加障碍 for (int i = 15; i < 35; ++i) grid[25][i] = 1; // 清除起点终点 grid[5][5] = 0; grid[40][40] = 0; } // 最小堆模拟（简化版） Node open_list[MAX_NODES]; int open_size = 0; void push_node(Node node) { open_list[open_size++] = node; } Node pop_best_node() { int best_idx = 0; for (int i = 1; i < open_size; ++i) if (open_list[i].f < open_list[best_idx].f) best_idx = i; Node ret = open_list[best_idx]; memmove(&open_list[best_idx], &open_list[best_idx+1], (open_size - best_idx - 1) * sizeof(Node)); open_size--; return ret; } int is_empty() { return open_size == 0; } // A* 主算法 int a_star(int sx, int sy, int gx, int gy, int (*path)[2], int *len) { open_size = 0; memset(parent, -1, sizeof(parent)); for (int i = 0; i < GRID_SIZE; ++i) for (int j = 0; j < GRID_SIZE; ++j) g_score[i][j] = INF; Node start = {.x=sx, .y=sy, .g=0.0}; g_score[sx][sy] = 0; f_score[sx][sy] = euclidean(sx,sy,gx,gy); push_node((Node){sx, sy, 0, f_score[sx][sy]}); while (!is_empty()) { Node curr = pop_best_node(); if (visited[curr.x][curr.y]) continue; visited[curr.x][curr.y] = TRUE; if (curr.x == gx && curr.y == gy) { // 回溯路径 int cnt = 0; int x = gx, y = gy; while (!(x == sx && y == sy)) { path[cnt][0] = x; path[cnt][1] = y; int tx = parent[x][y][0], ty = parent[x][y][1]; x = tx; y = ty; cnt++; } path[cnt][0] = sx; path[cnt][1] = sy; *len = cnt + 1; for (int i = 0; i < *len/2; ++i) { int tx = path[i][0], ty = path[i][1]; path[i][0] = path[*len-1-i][0]; path[i][1] = path[*len-1-i][1]; path[*len-1-i][0] = tx; path[*len-1-i][1] = ty; } return TRUE; } for (int i = 0; i < 8; ++i) { int nx = curr.x + dx[i], ny = curr.y + dy[i]; if (!is_valid(nx, ny)) continue; double tg = curr.g + cost[i]; if (tg < g_score[nx][ny]) { parent[nx][ny][0] = curr.x; parent[nx][ny][1] = curr.y; g_score[nx][ny] = tg; double h = euclidean(nx, ny, gx, gy); double w = dynamic_weight(tg, h); f_score[nx][ny] = tg + w * h; push_node((Node){nx, ny, tg, f_score[nx][ny]}); } } } return FALSE; } // 简单打印路径 void print_path(int path[][2], int len) { printf("路径点数: %d\n", len); for (int i = 0; i < len; ++i) printf("(%d,%d) ", path[i][0], path[i][1]); printf("\n"); } // 主函数 int main() { init_grid(); int path[MAX_NODES][2]; int len = 0; if (a_star(5, 5, 40, 40, path, &len)) { print_path(path, len); } else { printf("路径规划失败\n"); } return 0; } ``` --- ### 🔧 编译与运行说明 #### MATLAB： 直接保存为 `.m` 文件并运行即可。 #### C语言： ```bash gcc -o astar astar.c -lm ./astar ``` --- ### ✅ 对比总结表 | 特性 | Python | MATLAB | C | |------|--------|--------|---| | 开发效率 | 高 | 中 | 低 | | 图形可视化 | 强 | 强 | 弱（需额外库） | | 实时性能 | 一般 | 一般 | 高（适合嵌入式） | | 移植性 | 好 | 依赖工具箱 | 极佳 | | 适用阶段 | 仿真验证 | 教学/原型 | 工程部署 | ---