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
优质代码,推荐下载,方便在其基础上改成你要的算法。