（来点有用的）含障碍的两点最短路径算法

主要思路
0 区域图准备
1 水扩散模型
2 寻找可行路径
3 优化路径
实验结果
其他

by HPC_ZY

障碍物的，水平面两点间最短的距离求法。就相当于计算你从一个地方走到另一个地方，需要的最短路径。
注意：不是图论！不是节点！不是Dijkstra！不是Floyd！

主要思路

获得区域二值图M（0表示可通行区域，1表示障碍物）；
生成水扩散模型WDM（模拟水流填满整个区域）；
利用梯度下降找到可行路径（就像放置一根连接两点但不穿过障碍的绳子）；
优化路径（好比用力拉绳子两端）

经过以上步骤最短路径就找出来啦，效果图如下：
在这里插入图片描述

0 区域图准备

这一步没什么好说的，绘制你自己的模型就可以了。推荐使用PS或画图工具，最后转换为bool（logical）类型就可以了。注意，障碍物为白色（1），可行区域为黑色（0）。
我为大家准备了4个模型，其中包含：

MAP（区域）
sp（起点坐标）
tp（终点坐标）

如下所示：
在这里插入图片描述
这四个模型就包含了四种类型的区域：1）复杂区域； 2）近似对称区域；3）多圆弧区域； 4）完全对称区域。
你可能发现了，没有“无法到达的区域”。

1 水扩散模型

效果如图：
在这里插入图片描述
具体实现方法如下

初始化模型，设置迭代参数
好好理解WDMtmp的作用，以及diffconst取值对结果的影响

% {初始化模型} 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
diffconst = 0.1;                % 扩散速度权值: 建议0.1~0.4

迭代求解

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;
    else
        NCC = NCC + 1;
    end
    SANtmp = SAN;
end
WDM(WDM == 0) = nan;            % 无法抵达的区域除去

边缘优化
由于障碍物设置为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;

这一部分非常巧妙，可以细细体会。

2 寻找可行路径

已经获得扩散模型，就可以利用梯度下降法找出可行路径了，大致分为三步：

梯度场计算与路径初始化

% {初始化路径}
[M,N] = size(MAP);
Route = ones(numel(MAP),2)*nan;		% 路径长度不会超过区域面积
Route(1,:) = tp;

% {计算梯度场}
[Gy,Gx] = gradient(-WDM);

开始求解
注意步长大小对结果的影响，以及代码中如何巧妙的避免陷入局部解。

iter = 0;
continflag = 1;			% 是否继续迭代
stepLen = 1；			% 设置下降步长（移动步长）
% {开始寻路}
while continflag
    iter = iter+1;
    % 获取当前点的梯度向量
    dx = interpn(Gx,Route(iter,1),Route(iter,2),'*linear');
    dy = interpn(Gy,Route(iter,1),Route(iter,2),'*linear');
    % 避免陷入局部
    dx = dx+rand*0.002-0.001;
    dy = dy+rand*0.002-0.001;
    % 向下降最快方向移动
    dv = sqrt(dx^2+dy^2);
    dx = stepLen * dx/dv;
    dy = stepLen * dy/dv;
    Route(iter+1,1) = Route(iter,1) + dx;
    Route(iter+1,2) = Route(iter,2) + dy;
    % 限制范围
    if Route(iter+1,1)<1;Route(iter+1,1) = 1;end
    if Route(iter+1,2)<1;Route(iter+1,2) = 1;end
    if Route(iter+1,1)>M;Route(iter+1,1) = M;end
    if Route(iter+1,2)>N;Route(iter+1,2) = N;end
    % 若接近起始点则停止
    if sum((Route(iter+1,:)-sp).^2)<1
        continflag = 0;
    end
end

结果处理
由于梯度下降过程是从终点向起点，所以最后我们把路径翻转。由于我们无法保证下降过程中能刚好达到端点，所以迭代停止条件是“接近“而非”到达“，所以最后我们要把端点值补上。

% 去掉多余的内存
Route = Route(~isnan(Route(:,1)),:);
% 重新排序（）
Route = Route(end:-1:1,:);
% 路径两端点与‘起、终点’不完全重合，则填上
if ~isequal(Route(1,:),sp)
    Route = [sp;Route];
end
if ~isequal(Route(end,:),tp)
    Route = [Route;tp];
end

3 优化路径

优化路径就是把弯曲的线拉直，但注意不能穿过障碍物。
在这里插入图片描述
方法很简单：

计算P2相邻两点P1、P3的中点坐标，计算距离d。
若d非常小，则P1、P2、P3近似在一直线上，不需要调整；若d较大则向中点方向移动一段距离w*d（0<w<0.5）。

过程演示如下：
在这里插入图片描述

MAXtier = 10000;		% 最大迭代次数
epsilon = 1e-6*stepLen;		% 直线精度（决定最后拉的多紧）
force = 0.001*stepLen;		% 拉力大小（每次位移程度）
continflag = 1;
iter = 0;
while continflag    
    RouteOld = Route;		% 记录原始位置
    % {计算路径弯曲程度}
    % 计算前后两点的中点
    RouteM = [Route(1,:);(Route(1:end-2,:)+Route(3:end,:))/2;Route(end,:)];
    % 计算偏移距离
    dRoute = RouteM-Route;
    dist = sqrt(sum(dRoute.^2,2));
    
    % {优化路径}
    % 拉紧偏移较大的点
    modify = dist>force;
    Route(modify,:) = Route(modify,:)+force*dRoute(modify,:)./dist(modify,:);
    % 防止拉进障碍物中
    tmp = interpn(MAP,Route(:,1),Route(:,2),'*nearest');
    modify = tmp>0.25;
    Route(modify,:) = Route(modify,:)-force*dRoute(modify,:)./dist(modify,:);
       
    % 达到精度或最大迭代次数则终止
    if max(abs(Route(:)-RouteOld(:))) < epsilon
        continflag = 0;
        disp(['达到精度要求,当前迭代',num2str(iter),'次'])
    end
    iter = iter+1;
    if iter>MAXtier
        continflag = 0;
        disp('达到最大迭代次数')
    end
end

注意：可能有人觉得w取1，不就一步到位了吗？那么大家可以思考为什么我不这样做，以及如果w取1代码应该怎么写。

实验结果

四个模型结果如下：
在这里插入图片描述
注：因为第四个模型完全对称，所以结果两侧都可以。

其他

整个算法中diffconst（扩散速度）、stepLen（下降步长）、epsilon（直线精度）、force（拉力）的设置都直接影响计算速度与结果的精度。
上述算法在精度和速度上都有很大优化空间。比如，通过自适应步长，减少直线路径点数增加弯曲路径点数；通过自适应拉力，提高路径收敛速度。（此部分代码不再提供，欢迎交流）
通过简单的修改就能将上述代码改为三维空间最短路径求解。（此部分代码不再提供，欢迎交流）
本文除了算法本身以外，主要想给大家分享一些适合MATLAB特性的实现方法，比如代码中的：最大值滤波、路径弯曲程度计算。巧妙利用MATLAB矩阵运算，可以省掉很多for循环，从而提高运行速度。

关于文中代码有任何问题欢迎讨论，最后还是把测试代码上传
（包含MATLAB测试代码及测试数据）
https://download.csdn.net/download/xsz591541060/11445356
优质代码，推荐下载，方便在其基础上改成你要的算法。

含障碍的两点最短路径算法