您现在的位置是：首页 >技术杂谈 >【APF三维路径规划】基于matlab人工势场算法无人机三维路径规划【含Matlab源码 2519期】网站首页技术杂谈

⛄一、无人机简介

0 引言
随着现代技术的发展，飞行器种类不断变多，应用也日趋专一化、完善化，如专门用作植保的大疆PS-X625无人机，用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机，以及用作水下救援的白鲨MIX水下无人机等，决定飞行器性能主要是内部的飞控系统和外部的路径规划问题。就路径问题而言，在具体实施任务时仅靠操作员手中的遥控器控制无人飞行器执行相应的工作，可能会对操作员心理以及技术提出极高的要求，为了避免个人操作失误，进而造成飞行器损坏的危险，一种解决问题的方法就是对飞行器进行航迹规划。
飞行器的测量精度，航迹路径的合理规划，飞行器工作时的稳定性、安全性等这些变化对飞行器的综合控制系统要求越来越高。无人机航路规划是为了保证无人机完成特定的飞行任务，并且能够在完成任务的过程中躲避各种障碍、威胁区域而设计出最优航迹路线的问题。

1 常见的航迹规划算法

图1 常见路径规划算法
文中主要对无人机巡航阶段的航迹规划进行研究，假设无人机在飞行中维持高度与速度不变，那么航迹规划成为一个二维平面的规划问题。在航迹规划算法中,A算法计算简单，容易实现。在改进A算法基础上，提出一种新的、易于理解的改进A算法的无人机航迹规划方法。传统A算法将规划区域栅格化，节点扩展只限于栅格线的交叉点，在栅格线的交叉点与交叉点之间往往存在一定角度的两个运动方向。将存在角度的两段路径无限放大、细化，然后分别用两段上的相应路径规划点作为切点，找到相对应的组成内切圆的圆心，然后作弧，并求出相对应的两切点之间的弧所对应的圆心角，根据下式计算出弧线的长度

式中：R———内切圆的半径；
α———切点之间弧线对应的圆心角。

⛄三、人工势场算法简介

人工势场算法之所以无法适应复杂的环境, 是因为斥力场系数、引力场系数在计算时是固定不变的。
1 人工势场法基本原理
人工势场法的实质是对无人机的飞行区域人为地定义势场[17], 该势场为地图中出现的障碍物斥力场和终点引力场的向量叠加。传统人工势场的定义如下:
假设无人机的位置为X= (x, y, z) , 则目标点引力场与无人机电子之间的电势场为

式中:Katt为势场增益系数;XG为目标点的位置。
定义Fatt (X) 为引力势场的引力,

定义障碍物的斥力势场为

定义障碍物的斥力为

式中:Frep为障碍物对无人机的斥力势场系数;X-X0的单位是m, 表示移动的无人机到各个障碍物的动态距离;ρ0为障碍物的影响距离。所以移动的无人机像一个电子一样, 它的总势场为无人机与障碍物各个势场的和:

对无人机的作用力Ftotal为

由式 (6) 可以算出无人机的下一步运动轨迹。

虽然人工势场法有很多优点, 但是在实际飞行中, 环境比较复杂的时候, 经常出现障碍物在目标位置附近的情况, 当无人机向目标点飞行时, Fatt减小Frep增大, 此时会出现无人机在终点区域拐弯的情况;当无人机处在障碍物运动时, 可能出现无人机处在合力为零点的情况, 因而无人机不能到达目标点。

⛄三、部分源代码

%Environment code
clf;
close all;
clear;
% 无人机目标位置的确定。
goal = [185,120,20];
% 定义无人机的初始位置。
start = [10,10,0];
%建筑物位置
Cpos = [70,50,60; 20,60,40; 60,90,60; 140,40,50; 180,190,60; 30,180,60;100,20,30; 30,110,20; 150,100,35; 70,160,40; 110,140,20];
figure; hold on
x = 0:4:200;
y = 0:4:200;
xlabel(“x”);
ylabel(“y”);
zlabel(“z”);
xlim([0 200]);
ylim([0 200]);
zlim([0 100]);
radius = [6;4;9;7;5;5;6;4;9;7;5]; % 建筑物的半径
create_cylinder(radius(1,1),Cpos(1,:),[0.25, 0.58, 0.96]) %[Radius, X-position, Y-position, Color]
create_cylinder(radius(2,1),Cpos(2,:),[0.25, 0.58, 0.96])
create_cylinder(radius(3,1),Cpos(3,:),[0.25, 0.58, 0.96])
create_cylinder(radius(4,1),Cpos(4,:),[0.25, 0.58, 0.96])
create_cylinder(radius(5,1),Cpos(5,:),[0.25, 0.58, 0.96])
create_cylinder(radius(6,1),Cpos(6,:),[0.25, 0.58, 0.96])
create_cylinder(radius(7,1),Cpos(7,:),[0.25, 0.58, 0.96]) %[Radius, X-position, Y-position, Color]
create_cylinder(radius(8,1),Cpos(8,:),[0.25, 0.58, 0.96])
create_cylinder(radius(9,1),Cpos(9,:),[0.25, 0.58, 0.96])
create_cylinder(radius(10,1),Cpos(10,:),[0.25, 0.58, 0.96])
create_cylinder(radius(11,1),Cpos(11,:),[0.25, 0.58, 0.96])
grid on;
text(start(1,1)-1, start(1,2), start(1,3)+2,“UAV起点”)
plot3(start(1,1), start(1,2), start(1,3),‘MarkerSize’,10,“Marker”,“*”,“Color”,“cyan”)
text(goal(1,1), goal(1,2), goal(1,3)+2,“UAV终点”)
plot3(goal(1,1), goal(1,2), goal(1,3),‘-s’,‘MarkerSize’,10,‘MarkerFaceColor’,‘green’)

%路径规划
obstacles = transpose(Cpos);
iteration = 350; %迭代次数
current_pos = transpose(start);
goal = transpose(goal);
previous_pos = current_pos; %初始化无人机先前位置
Krep = 0.1; %排斥势场增益因子
Katt = 0.04;
delta = 0;
data_points = zeros(iteration,3); % 存储无人机迭代值位置
F = zeros(3,length(obstacles));
Urep = 0;
figure(1)
title(‘UAV路径’)
for i=1:iteration
p_Fr = 0;
robot_height = current_pos(3,1);
goal_height = goal(3,1);
flag = 0;
Fatt = potential_attraction(Katt, current_pos, goal);
for k = 1: length(obstacles)
% 测量无人机与建筑物中心轴线之间的水平距离
rou = sqrt((current_pos(1,1)-obstacles(1,k))^{2+(current_pos(2,1)-obstacles(2,k))}2);
% 可变柔度差异化
d_rou = [current_pos(1,1)-obstacles(1,k); current_pos(2,1)-obstacles(2,k)]/rou;

    % 阈值判断无人机是否接近建筑物？
    zeta = 3.5*radius(k,1);
    n 
            flag = 1;
            Frep1 = Krep*((1/rou)-(1/zeta))*(1/rou^2)*dist_factor(current_pos, goal, n, flag)*d_rou;
            Frep2 = -(n/2)*Krep*((1/rou)-(1/zeta))^2*dist_factor(current_pos, goal, n-1, flag)*diff_distance_factor(current_pos, goal, n, flag);
            F(:,k) = vertcat(Frep1,0)+Frep2; 
            Fatt = Katt*[goal(1,1)-current_pos(1,1);goal(2,1)-current_pos(2,1); 0];
        else
            F(:,k) = 0;
        end
    elseif rou > zeta
        
        F(:,k) = 0;
    end
end
Frep = sum(F,2); %所有斥力的总和
Ft = Fatt + Frep;
if flag == 1
    flag = 0;
    % 当机器人接近障碍物时，根据无人机的位置改变“ z”轴，机器人只能在 xy 平面内移动。
    Ft(3,1) = 0; 
end
previous_pos = current_pos;
current_pos = current_pos+Ft;
data_points(i,:)=transpose(current_pos);
title(sprintf('迭代 %d', i))
% 实时绘制无人机位置
plot3(current_pos(1,1), current_pos(2,1), current_pos(3,1),"Marker","*","Color","black");
pause(0.07)
drawnow

end
%图的绘制
for i = 1:length(obstacles)
% 绘制其他有用的图来分析无人机的行为
potential_plots(x,y,obstacles(:,i));
end

⛄四、运行结果

⛄五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]焦阳.基于改进蚁群算法的无人机三维路径规划研究[J].舰船电子工程. 2019,39(03)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除