基于雷达扫描的路径规划

标签：end idx 路径扫描 current grid 雷达 col row

基于雷达的二维扫描建图，得到csv文件后，通过matlab算法完成独立的路径规划，达到绕开障碍并且以最优路线向终点前进

下面是具体代码实现：

1. 读取数据和定义初始点、终点

data = readtable('xxx.csv');

//这部分csv为建图导出的雷达点云文件，使用串口通讯完成读取

x = data.Points_X;
y = data.Points_Y;

[max_x, max_idx] = max(x);
[min_x, min_idx] = min(x);
start_point = [x(max_idx), y(max_idx)];
end_point = [x(min_idx), y(min_idx)];

这段代码读取CSV文件中的点云数据，并提取其中的X和Y坐标。通过找出最大和最小的X坐标，确定了起始点和终点。

2. 定义障碍物的顶点并计算旋转矩阵

vertices = {
    [-0.44, 1.68; -0.18, 1.51], 
    [0.23, 1.07; 0.51, 0.94],  
    [0.25, 2.12; 0.41, 2.03]  
};

squares = cell(size(vertices));
for i = 1:length(vertices)
    v = vertices{i};
    
    side_length = sqrt(sum((v(1,:) - v(2,:)).^2));
    
    if i == 1
        theta = -30 * pi / 180; 
    else
        theta = -25 * pi / 180; 
    end
    
    R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
    
    right_bottom = v(2, :);
    
    sq_unrotated = [
        right_bottom;
        right_bottom(1), right_bottom(2) + side_length;
        right_bottom(1) - side_length, right_bottom(2) + side_length;
        right_bottom(1) - side_length, right_bottom(2)
    ];
    
    sq_rotated = (R * (sq_unrotated - right_bottom)' + right_bottom')';
    
    squares{i} = sq_rotated;
end

这段代码定义了三个障碍物的顶点，并通过计算旋转矩阵将这些顶点旋转一定角度，以得到旋转后的障碍物形状。

本段代码主要原因为已有的csv文件没有将方形障碍物四边完全读取，通过原本csv文件点的读取，通过算法补全整个方形的障碍物

（如果可以把全部位置扫全，你也可以不用这段算法）

3. 绘制点云数据、起始点、终点和障碍物

figure;
scatter(data.Points_X, data.Points_Y, 1, 'k'); hold on;
scatter(start_point(1), start_point(2), 50, 'r', 'filled');
scatter(end_point(1), end_point(2), 50, 'g', 'filled');

for i = 1:length(squares)
    sq = squares{i};
    fill(sq(:,1), sq(:,2), 'k', 'FaceAlpha', 0.1);
    plot(sq(:,1), sq(:,2), 'k', 'LineWidth', 2);
end

使用散点图绘制点云数据，并标记起始点和终点。然后使用fill和plot函数绘制障碍物。

4. 生成网格地图并标记障碍物

grid_size = 100; 
x_grid = linspace(min(x), max(x), grid_size);
y_grid = linspace(min(y), max(y), grid_size);
[xx, yy] = meshgrid(x_grid, y_grid);

obstacle_map = false(grid_size);
for i = 1:length(squares)
    sq = squares{i};
    kx = inpolygon(xx(:), yy(:), sq(:,1), sq(:,2));
    obstacle_map(:) = obstacle_map(:) | kx;
end
obstacle_map = reshape(obstacle_map, size(xx));

创建一个大小为grid_size x grid_size的网格地图，并使用inpolygon函数检查哪些网格点位于障碍物内。生成一个布尔型矩阵obstacle_map来标记这些网格点。

5. 计算起始点和终点在网格中的索引

start_row = round((start_point(2) - min(y)) / (max(y) - min(y)) * (grid_size - 1)) + 1;
start_col = round((start_point(1) - min(x)) / (max(x) - min(x)) * (grid_size - 1)) + 1;
end_row = round((end_point(2) - min(y)) / (max(y) - min(y)) * (grid_size - 1)) + 1;
end_col = round((end_point(1) - min(x)) / (max(x) - min(x)) * (grid_size - 1)) + 1;

start_row = max(min(start_row, grid_size), 1);
start_col = max(min(start_col, grid_size), 1);
end_row = max(min(end_row, grid_size), 1);
end_col = max(min(end_col, grid_size), 1);

start_idx = sub2ind(size(obstacle_map), start_row, start_col);
end_idx = sub2ind(size(obstacle_map), end_row, end_col);

将起始点和终点的实际坐标转换为网格中的索引，并确保这些索引在有效范围内。

6. AI路径规划算法

[path_x, path_y] = a_star_pathfinding(obstacle_map, start_idx, end_idx, x_grid, y_grid);

plot(path_x, path_y, 'r-', 'LineWidth', 2);

xlabel('Points_X');
ylabel('Points_Y');
title('Path Planning with Obstacle Avoidance');
legend('Point Cloud', 'Start Point', 'End Point', 'Obstacle', 'Planned Path', 'Location', 'Best');
grid on;
hold off;

调用AI路径规划函数，并将生成的路径绘制在图上。路径由红色线条表示

7. AI路径规划函数实现

function [path_x, path_y] = a_star_pathfinding(map, start_idx, end_idx, x_grid, y_grid)
    
    [rows, cols] = size(map);
    open_set = start_idx;
    came_from = NaN(rows, cols);
    g_score = inf(rows, cols);
    g_score(start_idx) = 0;
    f_score = inf(rows, cols);
    f_score(start_idx) = heuristic(start_idx, end_idx, rows, cols);

    while ~isempty(open_set)
        [~, current_idx] = min(f_score(open_set));
        current = open_set(current_idx);
        
        if current == end_idx
            path_x = [];
            path_y = [];
            while ~isnan(came_from(current))
                [current_row, current_col] = ind2sub([rows, cols], current);
                path_x = [x_grid(current_col), path_x];
                path_y = [y_grid(current_row), path_y];
                current = came_from(current);
            end
            return;
        end
        
        open_set(current_idx) = [];
        [current_row, current_col] = ind2sub([rows, cols], current);
        
        neighbors = [
            current_row-1, current_col;
            current_row+1, current_col;
            current_row, current_col-1;
            current_row, current_col+1
        ];
        
        for k = 1:size(neighbors, 1)
            neighbor_row = neighbors(k, 1);
            neighbor_col = neighbors(k, 2);
            
            if neighbor_row > 0 && neighbor_row <= rows && ...
               neighbor_col > 0 && neighbor_col <= cols && ...
               ~map(neighbor_row, neighbor_col)
                
                neighbor = sub2ind([rows, cols], neighbor_row, neighbor_col);
                tentative_g_score = g_score(current) + 1;
                
                if tentative_g_score < g_score(neighbor)
                    came_from(neighbor) = current;
                    g_score(neighbor) = tentative_g_score;
                    f_score(neighbor) = g_score(neighbor) + heuristic(neighbor, end_idx, rows, cols);
                    
                    if ~ismember(neighbor, open_set)
                        open_set = [open_set; neighbor];
                    end
                end
            end
        end
    end
    
    path_x = [];
    path_y = [];
end

function h = heuristic(idx, end_idx, rows, cols)
    [x1, y1] = ind2sub([rows, cols], idx);
    [x2, y2] = ind2sub([rows, cols], end_idx);
    h = sqrt((x1 - x2)^2 + (y1 - y2)^2);
end

这是AI路径规划算法的具体实现。a_star_pathfinding函数通过维护一个开放集来跟踪待处理的节点，并使用启发式函数heuristic来估算从当前节点到终点的距离。函数最终返回从起始点到终点的路径坐标。

下面进行函数实现的具体解释：

1. 函数声明和初始化

function [path_x, path_y] = a_star_pathfinding(map, start_idx, end_idx, x_grid, y_grid)
    [rows, cols] = size(map);
    open_set = start_idx;
    came_from = NaN(rows, cols);
    g_score = inf(rows, cols);
    g_score(start_idx) = 0;
    f_score = inf(rows, cols);
    f_score(start_idx) = heuristic(start_idx, end_idx, rows, cols);

function [path_x, path_y] = a_star_pathfinding(map, start_idx, end_idx, x_grid, y_grid)：函数声明，输入为地图矩阵、起始点索引、终点索引、网格的X坐标和Y坐标，输出为路径的X和Y坐标。

open_set：维护一个开放集，存储待处理的节点，初始时包含起始点。

came_from：用来记录每个节点的前驱节点，以便回溯路径

g_score：从起始点到当前节点的实际代价，初始时所有节点的代价为无穷大，起始点的代价为0。

f_score：启发式估计函数值，表示从起始点经过当前节点到终点的估计代价，初始时所有节点的估计代价为无穷大，起始点的估计代价为启发式估计值。

2. 主循环：处理开放集中的节点

    while ~isempty(open_set)
        [~, current_idx] = min(f_score(open_set));
        current = open_set(current_idx);
        
        if current == end_idx
            path_x = [];
            path_y = [];
            while ~isnan(came_from(current))
                [current_row, current_col] = ind2sub([rows, cols], current);
                path_x = [x_grid(current_col), path_x];
                path_y = [y_grid(current_row), path_y];
                current = came_from(current);
            end
            return;
        end
        
        open_set(current_idx) = [];
        [current_row, current_col] = ind2sub([rows, cols], current);

while ~isempty(open_set)：当开放集不为空时，循环继续。

[~, current_idx] = min(f_score(open_set))：找到f_score值最小的节点current，作为当前节点。

if current == end_idx：如果当前节点是终点，构建路径并返回。

open_set(current_idx) = []：将当前节点从开放集中移除。

[current_row, current_col] = ind2sub([rows, cols], current)：将当前节点的索引转换为行列坐标。

3. 检查邻居节点

        neighbors = [
            current_row-1, current_col;
            current_row+1, current_col;
            current_row, current_col-1;
            current_row, current_col+1
        ];
        
        for k = 1:size(neighbors, 1)
            neighbor_row = neighbors(k, 1);
            neighbor_col = neighbors(k, 2);
            
            if neighbor_row > 0 && neighbor_row <= rows && ...
               neighbor_col > 0 && neighbor_col <= cols && ...
               ~map(neighbor_row, neighbor_col)
                
                neighbor = sub2ind([rows, cols], neighbor_row, neighbor_col);
                tentative_g_score = g_score(current) + 1;
                
                if tentative_g_score < g_score(neighbor)
                    came_from(neighbor) = current;
                    g_score(neighbor) = tentative_g_score;
                    f_score(neighbor) = g_score(neighbor) + heuristic(neighbor, end_idx, rows, cols);
                    
                    if ~ismember(neighbor, open_set)
                        open_set = [open_set; neighbor];
                    end
                end
            end
        end
    end
    
    path_x = [];
    path_y = [];
end

neighbors：定义当前节点的四个邻居节点（上、下、左、右）。

for k = 1:size(neighbors, 1)：遍历每个邻居节点。

neighbor_row = neighbors(k, 1); neighbor_col = neighbors(k, 2);：获取邻居节点的行列坐标。

if neighbor_row > 0 && neighbor_row <= rows && ...：检查邻居节点是否在地图范围内且不是障碍物。

neighbor = sub2ind([rows, cols], neighbor_row, neighbor_col)：将邻居节点的行列坐标转换为索引。

tentative_g_score = g_score(current) + 1;：计算从起始点经过当前节点到邻居节点的实际代价。

if tentative_g_score < g_score(neighbor)：如果新的实际代价小于当前记录的邻居节点的代价，更新该邻居节点的信息。

came_from(neighbor) = current;：记录邻居节点的前驱节点。

g_score(neighbor) = tentative_g_score;：更新邻居节点的实际代价。

f_score(neighbor) = g_score(neighbor) + heuristic(neighbor, end_idx, rows, cols);：更新邻居节点的估计代价。

if ~ismember(neighbor, open_set)：如果邻居节点不在开放集中，将其加入开放集。

4. 启发式估计函数

function h = heuristic(idx, end_idx, rows, cols)
    [x1, y1] = ind2sub([rows, cols], idx);
    [x2, y2] = ind2sub([rows, cols], end_idx);
    h = sqrt((x1 - x2)^2 + (y1 - y2)^2);
end

function h = heuristic(idx, end_idx, rows, cols)：函数声明，输入为当前节点索引、终点索引、地图的行列数，输出为启发式估计值。

[x1, y1] = ind2sub([rows, cols], idx); [x2, y2] = ind2sub([rows, cols], end_idx);：将当前节点和终点的索引转换为行列坐标。

h = sqrt((x1 - x2)^2 + (y1 - y2)^2);：计算当前节点到终点的欧几里得距离，作为启发式估计值。

5.总结

AI路径规划算法通过启发式估计和实际代价相结合，逐步扩展从起始点到终点的最优路径。在每一步中，算法选择f_score值最小的节点进行扩展，直到找到终点，并通过came_from矩阵回溯构建最优路径。

标签：end,idx,路径,扫描,current,grid,雷达,col,row
From： https://blog.csdn.net/2301_80136797/article/details/140181650