遗传模拟退火算法对移动机器人进行路径规划

​1. 问题描述​

​目标​：在二维栅格地图中，为移动机器人规划一条从起点到终点的最优路径，要求：

• 避开障碍物
• 路径长度最短
• 路径平滑（转弯角度小）

​算法选择​：遗传算法（GA）与模拟退火（SA）结合，利用GA的全局搜索能力和SA的局部优化能力。

---

​2. MATLAB代码实现​

clc; clear; close all;

%% 参数设置
mapSize = [50, 50];       % 地图尺寸
start = [5, 5];           % 起点
goal = [45, 45];          % 终点
obstacleDensity = 0.3;    % 障碍物密度
popSize = 100;            % 种群大小
maxGen = 200;             % 最大迭代次数
pc = 0.8;                 % 交叉概率
pm = 0.1;                 % 变异概率
T0 = 100;                 % 初始温度
alpha = 0.95;             % 降温系数

%% 生成环境地图
map = initMap(mapSize, obstacleDensity);
imshow(map);
hold on;
plot(start(2), start(1), 'go', 'MarkerSize', 10); % 起点
plot(goal(2), goal(1), 'ro', 'MarkerSize', 10);   % 终点

%% 初始化种群（路径表示为坐标序列）
population = initPopulation(popSize, start, goal, mapSize);

%% 遗传模拟退火主循环
bestFitness = inf;
bestPath = [];
T = T0;

for gen = 1:maxGen
    % 计算适应度
    fitness = zeros(popSize, 1);
    for i = 1:popSize
        path = population{i};
        fitness(i) = pathFitness(path, map, start, goal);
    end

    % 更新最优解
    [minFitness, idx] = min(fitness);
    if minFitness < bestFitness
        bestFitness = minFitness;
        bestPath = population{idx};
    end

    % 选择
    selectedPop = selection(population, fitness);

    % 交叉
    offspring = crossover(selectedPop, pc);

    % 变异
    mutatedOffspring = mutation(offspring, pm, mapSize);

    % 模拟退火优化（对当前最优解）
    bestPath = simulatedAnnealing(bestPath, map, T);

    % 更新种群
    population = [selectedPop; mutatedOffspring];
    population = population(1:popSize); % 保持种群大小

    % 降温
    T = T * alpha;

    % 显示迭代信息
    fprintf('Generation %d: Best Fitness = %.2f
', gen, bestFitness);
end

%% 绘制最优路径
plot(bestPath(:,2), bestPath(:,1), 'b-', 'LineWidth', 2);
legend('Start', 'Goal', 'Optimal Path');

%% 函数定义
function map = initMap(size, density)
    % 生成随机障碍物地图（0: 可通行, 1: 障碍）
    map = rand(size) < density;
    map(1,1) = 0; % 确保起点无障碍
    map(end,end) = 0; % 确保终点无障碍
end

function population = initPopulation(popSize, start, goal, mapSize)
    % 初始化种群（路径由起点、中间随机点和终点组成）
    population = cell(popSize, 1);
    for i = 1:popSize
        numPoints = randi([3, 6]); % 随机路径点数量
        path = [start; 
                rand(numPoints-2, 2) .* repmat(mapSize-1, numPoints-2, 2);
                goal];
        population{i} = path;
    end
end

function fitness = pathFitness(path, map, start, goal)
    % 计算路径适应度（路径长度 + 障碍惩罚 + 平滑度惩罚）
    path = [start; path; goal]; % 补全起点和终点
    path = unique(path, 'rows', 'stable'); % 去重

    % 路径长度惩罚
    dist = 0;
    for i = 1:size(path,1)-1
        dist = dist + norm(path(i,:) - path(i+1,:));
    end

    % 障碍惩罚（路径经过障碍物区域）
    obstaclePenalty = 0;
    for i = 1:size(path,1)
        if map(round(path(i,1)), round(path(i,2))) == 1
            obstaclePenalty = obstaclePenalty + 1000;
        end
    end

    % 平滑度惩罚（转弯角度）
    smoothPenalty = 0;
    for i = 2:size(path,1)-1
        v1 = path(i) - path(i-1);
        v2 = path(i+1) - path(i);
        angle = acos(dot(v1, v2)/(norm(v1)*norm(v2)));
        smoothPenalty = smoothPenalty + angle;
    end

    fitness = dist + obstaclePenalty + 0.1*smoothPenalty;
end

function selectedPop = selection(population, fitness)
    % 轮盘赌选择
    prob = 1 ./ (fitness + 1e-6); % 适应度越小概率越大
    prob = prob / sum(prob);
    cumProb = cumsum(prob);
    selectedPop = cell(size(population));
    for i = 1:length(population)
        r = rand;
        idx = find(cumProb >= r, 1);
        selectedPop{i} = population{idx};
    end
end

function offspring = crossover(parents, pc)
    % 单点交叉
    offspring = cell(size(parents));
    for i = 1:2:length(parents)
        if rand < pc
            p1 = parents{i};
            p2 = parents{i+1};
            crossPoint = randi([1, min(length(p1), length(p2))-1]);
            c1 = [p1(1:crossPoint, :); p2(crossPoint+1:end, :)];
            c2 = [p2(1:crossPoint, :); p1(crossPoint+1:end, :)];
            offspring{i} = c1;
            offspring{i+1} = c2;
        else
            offspring{i} = parents{i};
            offspring{i+1} = parents{i+1};
        end
    end
end

function mutated = mutation(offspring, pm, mapSize)
    % 随机交换变异
    mutated = cell(size(offspring));
    for i = 1:length(offspring)
        path = offspring{i};
        if rand < pm
            idx1 = randi(length(path));
            idx2 = randi(length(path));
            temp = path(idx1, :);
            path(idx1, :) = path(idx2, :);
            path(idx2, :) = temp;
        end
        mutated{i} = path;
    end
end

function path = simulatedAnnealing(path, map, T)
    % 模拟退火优化路径
    currentPath = path;
    currentCost = pathFitness(currentPath, map, start, goal);
    for iter = 1:100
        newPath = currentPath;
        % 随机扰动（插入/删除点）
        if rand < 0.5
            idx = randi(length(newPath)-2) + 1;
            newPath(idx) = newPath(idx) + randn(1,2)*0.5; % 随机偏移
        else
            if ~isempty(newPath)
                newPath(end) = newPath(end) + randn(1,2)*0.5;
            end
        end
        newCost = pathFitness(newPath, map, start, goal);
        delta = newCost - currentCost;
        if delta < 0 || exp(-delta/T) > rand
            currentPath = newPath;
            currentCost = newCost;
        end
    end
    path = currentPath;
end

可以参考 # 遗传模拟退火算法对移动机器人进行路径规划

​3. 关键步骤解析​

1. ​环境建模​：

• 使用栅格地图表示障碍物（0为可通行，1为障碍）。
• 起点和终点固定在地图边缘。

2. ​路径表示​：

• 路径由一系列坐标点组成，包含起点、中间随机点和终点。

3. ​适应度函数​：

• ​路径长度​：路径总距离的倒数（越短越好）。
• ​障碍惩罚​：路径穿过障碍物时施加高额惩罚。
• ​平滑度惩罚​：路径转弯角度过大会增加惩罚。

4. ​遗传算法操作​：

• ​选择​：轮盘赌选择适应度高的个体。
• ​交叉​：单点交叉生成新路径。
• ​变异​：随机扰动路径点以增加多样性。

5. ​模拟退火优化​：

• 对当前最优路径进行局部扰动（如随机偏移路径点）。
• 根据温度 T 决定是否接受较差解，避免陷入局部最优。

---

​4. 结果分析​

• ​可视化​：最终路径显示为蓝色曲线，绕过红色障碍物区域。
• ​参数调优​：调整 popSize、maxGen、T0 等参数可平衡搜索速度与解质量。
• ​扩展性​：可加入动态障碍物、多目标优化等复杂场景。

---

​5. 改进方向​

1. ​路径平滑​：使用样条插值或贝塞尔曲线优化最终路径。
2. ​动态环境​：结合滚动时域控制（RHC）处理动态障碍物。
3. ​多目标优化​：同时优化路径长度、能耗和安全性。