查看: 109|回复: 4|关注: 0

[已答复] 请求帮忙,路径规划问题

[复制链接]

新手

5 麦片

财富积分


050


1

主题

3

帖子

0

最佳答案
发表于 2019-10-31 23:03:00 | 显示全部楼层 |阅读模式
请问各位大佬,这个程序是我弄的dijkstra机器人路径规划,刚刚学习这个,报错不能解决,麻烦有时间的大佬帮忙,谢谢!
>>>dijkstrachange
数组索引必须为正整数或逻辑值。
出错 dijkstrachange>dijkstra (line 63)

            if label(v)>(label(u)+W(u,v))

function main
%构建邻接矩阵
G=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 1 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 1 0;
    0 0 0 0 0 0 1 1 1 0 1 0 0 0 0 0 0 1 1 0;
    0 0 1 1 0 0 1 1 1 0 1 0 0 0 0 0 0 0 0 0;
    0 0 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0;
    0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0;
    0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
    0 1 1 1 0 0 0 0 0 0 1 0 1 1 0 0 0 0 0 0;
    0 0 0 0 0 0 1 1 1 0 1 0 1 1 0 0 0 0 0 0;
    0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];
G1 = G2D(G);
G2 = G1;
L = size(G2,1);
%将栅格中不相邻的栅格距离初始化为inf
for ii = 1:L
    for jj = 1:L
        if G2(ii,jj)==0 && ii~=jj
            G2(ii,jj)=inf;
        end
    end
end
G3 = G2;
start = 381;
terminal = 20;
[minl,path] = dijkstra(G3,start,terminal)
end

function [minl,path]=dijkstra(W,start,termianl)
n = size(W,1);
label(start) = 0; %label记录初始点到每一个点的距离
f(start) = start; %记录最短路线上的点所经过的前一个点,由前一个点u,决定W(u,v)更新label
%初始化label
for i = 1:n
    if i ~= start
        label(i) = inf;
    end
end
%更新label
S(1) = start; % s(1) = 1,s表示遍历每一个点(按距最短路径数组距离最短遍历)的顺序,记录每次访问点的序号
u = start; %u为每次出发点,S记录路径的行走(选点)过程
while length(S)<n
    for i = 1:n
        ins = 0;
        for j = 1:length(S)
            if i == S(j)
                ins = 1;
            end
        end
        if ins == 0
            v = i;
            if label(v)>(label(u)+W(u,v))
               label(v)=(label(u)+W(u,v));
               f(v) = u;
            end
        end %每一次遍历后都更新源点到该点的距离,由中转点u决定.每一次选择u是根据u到label
    end
    %每次都取离选择的路径最近的点,并更新每次出发点
    k = inf;
    v1 = 0;
    for i = 1:n
        ins = 0;
        for j = 1:length(S)
            if i == S(j)
                ins = 1;
            end
        end
        v = i;
        if ins == 0
            if k > label(v)
                k = label(v);
                v1 = v;
            end
        end
    end
    S(length(S)+1) = v1;
    u = v1;
end
%创建最短路线
minl = label(termianl);
path(1) = terminal;
i=1;
while path(i)~=start
    path(i+1)=f(path(i));
    i=i+1 ;
end
path(i)=start; %输出路径
L=length(path); %输出路径的长度
path=path(L:-1:1); %将路径逆序输出
end

function D=G2D(G)
l = size(G,1);
D = zeros(l*l,l*l);
for i = 1:l
    for j = 1:l
        if G(i,j) == 0
            for m = 1:l
                for n = 1:l
                    if G(m,n) == 0
                        im = abs(i-m);
                        jn = abs(j-n);
                        if im+jn==1 || (im==1&&jn==1)
                            D((i-1)*l+j,(m-1)*l+n) = sqrt(im+jn);
                        end
                    end
                end
            end
        end
    end
end
end


MATLAB 基础讨论
版块优秀回答者

中级

535 麦片

财富积分


5001500


0

主题

207

帖子

35

最佳答案
  • 关注者: 2
发表于 2019-11-1 09:59:12 | 显示全部楼层
dijkstra函数中的
S(length(S)+1) = v1;有问题
改为S(length(S)) = v1;试试
运算时间不短,我算了有一会,结果还没出来

新手

5 麦片

财富积分


050


1

主题

3

帖子

0

最佳答案
 楼主| 发表于 2019-11-1 14:59:21 | 显示全部楼层
20141303 发表于 2019-11-1 09:59
dijkstra函数中的
S(length(S)+1) = v1;有问题
改为S(length(S)) = v1;试试

好的马上试试

新手

5 麦片

财富积分


050


1

主题

3

帖子

0

最佳答案
 楼主| 发表于 2019-11-1 15:11:50 | 显示全部楼层
20141303 发表于 2019-11-1 09:59
dijkstra函数中的
S(length(S)+1) = v1;有问题
改为S(length(S)) = v1;试试

好像不太对,单步调试的话后面死循环了。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

关闭

站长推荐上一条 /3 下一条

快速回复 返回顶部 返回列表