orange.m

来自「规则障碍物环境下的机器人路径规划matlab源码！」· M 代码 · 共 29 行

function I=orange(x,y,d)
O=[1 4 14 18;8 13 8 13;14 17 2 5];%障碍物上下左右界
[o1,o2]=size(O);
for i=1:o1
    K(i,1)=(20-O(i,4))/(20-O(i,1));
    K(i,2)=(20-O(i,3))/(20-O(i,2));
    k(i,:)=(O(i,4)-O(i,3))/(O(i,1)-O(i,2));
    
end
a=(d-y)/(d-x);
if isnan(a)==1
    I=0.8;
end
         if a<=K(1,1)|a<=K(2,1)&a>=K(1,2)|a<=K(3,1)&a>=K(2,2)|a>=K(3,2)
         I=0.4;
         end
 for w=1:o1
        
        if a<K(w,2)&a>K(w,1)
            if y-k(w,:)*(x-O(w,1))-O(w,4)>=0
            I=0.8;
            else
              I=0.4;
             end
        end 
 end