障碍物的,水平面两点间最短的距离求法。就相当于计算你从一个地方走到另一个地方,需要的最短路径。
注意:不是图论!不是节点!不是Dijkstra!不是Floyd!
这一步没什么好说的,绘制你自己的模型就可以了。推荐使用PS或画图工具,最后转换为bool(logical)类型就可以了。注意,障碍物为白色(1),可行区域为黑色(0)。
我为大家准备了4个模型,其中包含:
如下所示:
这四个模型就包含了四种类型的区域:1)复杂区域; 2)近似对称区域 ;3)多圆弧区域; 4)完全对称区域。
你可能发现了,没有“无法到达的区域”。
% {
初始化模型} 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; % 无法抵达的区域除去
% {
处理边缘值}
% 扩展矩阵
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;
这一部分非常巧妙,可以细细体会。
已经获得扩散模型,就可以利用梯度下降法找出可行路径了,大致分为三步:
% {
初始化路径}
[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
优化路径就是把弯曲的线拉直,但注意不能穿过障碍物。
方法很简单:
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代码应该怎么写。
四个模型结果如下:
注:因为第四个模型完全对称,所以结果两侧都可以。
关于文中代码有任何问题欢迎讨论,最后还是把测试代码上传
(包含MATLAB测试代码及测试数据)
https://download.csdn.net/download/xsz591541060/11445356
优质代码,推荐下载,方便在其基础上改成你要的算法。