人工势场法避障三维

人工势场法3维路径规划的matlab程序，用网上流传最多的2维人工势场路径规划改的 效果一般 有时间还会改进的

function [Theta] = compute_angel（PTnThetat）
%UNtitleD4 此处显示有关此函数的摘要
%   此处显示详细说明
for i = 1:n+1
    deltaX（i） = T（1i）-P（1）;
    deltaY（i） = T（2i）-P（2）;
    deltaZ（i） = T（3i）-P（3）;
    r（i） = sqrt（deltaX（i）^2+deltaY（i）^2+deltaZ（i）^2）;
    
    if deltaX（i）>0
        Theta（1ti） = acos（deltaX（i）/r（i））;
    else
        Theta（1ti） = pi-acos（deltaX（i）/r（i））;
    end
    
    if deltaY（i）>0
        Theta（2ti） = acos（deltaY（i）/r（i））;
    else
        Theta（2ti） = pi-acos（deltaY（i）/r（i））;
    end
    
    if deltaZ（i）>0
        Theta（3ti） = acos（deltaZ（i）/r（i））;
    else
        Theta（3ti） = pi-acos（deltaZ（i）/r（i））;
    end
    
end
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        706  2019-04-08 16:26  人工势场法（3维）\compute_angel.m

     文件        431  2019-04-09 15:03  人工势场法（3维）\compute_angel_F.m

     文件        246  2019-04-08 16:44  人工势场法（3维）\compute_Attract.m

     文件       1248  2019-04-09 16:04  人工势场法（3维）\compute_repulsion.m

     文件       2375  2019-04-09 16:05  人工势场法（3维）\main.m

     文件       1579  2019-04-09 15:23  人工势场法（3维）\main_test.m

     目录          0  2019-04-09 16:11  人工势场法（3维）

----------- ---------  ---------- -----  ----

                 6585                    7