以下是将上述 **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 |
|------|--------|--------|---|
| 开发效率 | 高 | 中 | 低 |
| 图形可视化 | 强 | 强 | 弱(需额外库) |
| 实时性能 | 一般 | 一般 | 高(适合嵌入式) |
| 移植性 | 好 | 依赖工具箱 | 极佳 |
| 适用阶段 | 仿真验证 | 教学/原型 | 工程部署 |
---