【路径规划】基于梯度下降算法求解自定义起点终点障碍路径规划问题附matlab代码

1 内容介绍

首先来看看梯度下降的一个直观的解释。比如我们在一座大山上的某处位置，由于我们不知道怎么下山，于是决定走一步算一步，也就是在每走到一个位置的时候，求解当前位置的梯度，沿着梯度的负方向，也就是当前最陡峭的位置向下走一步，然后继续求解当前位置梯度，向这一步所在位置沿着最陡峭最易下山的位置走一步。这样一步步的走下去，一直走到觉得我们已经到了山脚。当然这样走下去，有可能我们不能走到山脚，而是到了某一个局部的山峰低处。

从上面的解释可以看出，梯度下降不一定能够找到全局的最优解，有可能是一个局部最优解。当然，如果损失函数是凸函数，梯度下降法得到的解就一定是全局最优解。

​

2 仿真代码

clear; close all; clc

%% 0 获取区域数据

% 已准备区域分布(MAP)、起点(sp)、终点(tp)

idx = 1;                        % 总共有4个模型

[MAP,sp,tp] = getMAP(idx);

showflag = 0;                   % 是否显示中间过程

%% 1 生成水扩散模型

diffconst = 0.1;                % 扩散速度权值: 建议0.1~0.4

WDM = getWaterDiffusionModel(MAP,sp,diffconst,showflag);

%% 2 梯度下降找路径

stepLen = 1;

RouteWithoutModify = getRoute(MAP,WDM,sp,tp,stepLen,showflag);

%% 3 路径优化

MAXtier = 10000;

epsilon = 1e-6*stepLen;

force = 0.001*stepLen;

Route = optimizeRoute(MAP,RouteWithoutModify,force,MAXtier,epsilon,showflag);

%% 显示

figure(1)

subplot(222),imshow(mat2gray(WDM)),colormap jet,title('\fontsize{16}1 水流模型')

subplot(221),imshow(MAP),hold on,title('\fontsize{16}0 区域图')

plot(sp(2),sp(1),'r*');

plot(tp(2),tp(1),'ro');

subplot(223),imshow(MAP),hold on,title('\fontsize{16}2 可行路径')

plot(RouteWithoutModify(1,2),RouteWithoutModify(1,1),'r*');

plot(RouteWithoutModify(end,2),RouteWithoutModify(end,1),'ro');

plot(RouteWithoutModify(:,2),RouteWithoutModify(:,1),'b-','LineWidth',2);

subplot(224),imshow(MAP),hold on,title('\fontsize{16}3 最短路径')

plot(Route(1,2),Route(1,1),'r*');

plot(Route(end,2),Route(end,1),'ro');

plot(Route(:,2),Route(:,1),'r-','LineWidth',2);

% -----------------------------子函数--------------------------------

% 0 读取模型数据

function [MAP,sp,tp] = getMAP(idx)

load('MAP.mat')

MAP = M{idx}.MAP;

sp = M{idx}.sp;

tp = M{idx}.tp;

end

% 1 生成水扩散模型函数

function WDM = getWaterDiffusionModel(MAP,sp,diffconst,showflag)

% {初始化模型} Water Diffusion Model

[M,N] = size(MAP);

WDM = zeros(M,N);               % 扩展顺序的记录

WDMtmp = zeros(M,N);

WDMtmp(sp(1),sp(2)) = 1;

% {初始化水流参数}

SAN = sum(WDMtmp(:)>1);         % 记录水流过的区域面积 Scaned Areas Number

SANtmp = SAN;                   % 记录水流过的区域面积（上一步）

NCC = 0;                        % 面积不变计数器 No Changed Counter

iter = 1;                       % 扩散迭代次数

while NCC < 2/diffconst         % 连续迭代多次面积不发生变化则停止

    % 水流四向扩展

    WDMtmp(1:end-1 ,:) = WDMtmp(1:end-1 ,:) + diffconst * WDMtmp(2:end,:);

    WDMtmp(2:end,:) = WDMtmp(2:end,:) + diffconst * WDMtmp(1:end-1 ,:);

    WDMtmp(:,1:end-1 ) = WDMtmp(:,1:end-1 ) + diffconst * WDMtmp(:,2:end );

    WDMtmp(:,2:end) = WDMtmp(:,2:end) + diffconst * WDMtmp(:,1:end-1 );

    % 障碍物处禁止扩展

    WDMtmp(MAP) = 0;

    % 记录本次扩展的区域

    WDM(logical(WDMtmp>1 & ~WDM)) = iter;

    % 更新已扫描过的区域的面积

    SAN = sum(WDMtmp(:)>1);

    % 判断

    if abs(SAN-SANtmp) > 0

        iter = iter + 1;

        if showflag

            imshow(WDM)

            drawnow

        end

    else

        NCC = NCC + 1;

    end

    SANtmp = SAN;

end

WDM(WDM == 0) = nan;            % 无法抵达的区域除去

% {处理边缘值}

% 扩展矩阵

HB = nan*ones(size(WDM)+2);

HB(2:end-1,2:end-1) = WDM;

% 最大值滤波

HM = max(cat(3,HB(1:end-2,1:end-2),...

    HB(1:end-2,2:end-1),...

    HB(1:end-2,3:end  ),...

    HB(2:end-1,1:end-2),...

    HB(2:end-1,3:end  ),...

    HB(3:end  ,1:end-2),...

    HB(3:end  ,2:end-1),...

    HB(3:end  ,3:end  )),[],3);

% 水墙替换原始边缘

WDM(isnan(WDM)) = HM(isnan(WDM))+1;

end

% 2 梯度下降找路径

function Route = getRoute(MAP,WDM,sp,tp,stepLen,showflag)

% {初始化路径}

[M,N] = size(MAP);

Route = ones(numel(MAP),2)*nan;

Route(1,:) = tp;

% {计算梯度场}

[Gy,Gx] = gradient(-WDM);

iter = 0;

continflag = 1;

if showflag

    imshow(MAP),hold on

    plot(Route(1,2),Route(1,1),'b.')

    axis([0 M 0 N])

end

3 运行结果

4 参考文献

[1]陶志远. 带电作业机器人及其路径规划研究. 

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。