【KOA三维路径规划】开普勒算法求解复杂山地环境下无人机三维路径规划【含Matlab源码 3648期】

⛄一、蜘蛛蜂算法无人机避障三维航迹规划简介

1 无人机航迹规划问题的数学模型
建立三维航迹规划问题的数学模型时, 不但考虑无人机基本约束, 还考虑复杂的飞行环境, 包括山体地形和雷暴威胁区。

1.1 无人机基本约束
规划的无人机三维航迹, 通常需要满足一些基本约束, 包括最大转弯角、最大爬升角或下滑角、最小航迹段长度、最低和最高飞行高度, 以及最大航迹长度等约束。其中, 最大转弯角约束, 是指无人机只能在水平面内小于或等于指定的最大转弯角内转弯;最大爬升角或下滑角约束, 是指无人机只能在垂直平面内小于或等于指定的最大爬升角或下滑角内爬升或下滑;最小航迹段长度约束, 要求无人机改变飞行姿态之前, 按目前的航迹方向飞行的最短航程;最低和最高飞行高度约束, 要求无人机在指定的飞行高度区间飞行;最大航迹长度约束, 是指无人机的航迹长度小于或等于指定的阈值。

记q (x, y, z, θ, ψ) 为无人机的飞行位置与姿态, 其中, (x, y, z) 为无人机的位置, θ为无人机的水平转弯角, ψ为无人机的竖直爬升角或下滑角, 进而建立上述基本约束的数学表达式。

1.2 飞行环境障碍物和威胁区建模
在飞行环境中, 高耸的山体近似采用圆锥体等效表示, 用以e为底的自然指数图形生成, 那么, 山体地形可以通过多个位置不同的圆锥体叠加而成。若将参考海拔基准高度设置为xOy平面, 记 (x, y, z) 为山体地形中的点, 那么

式中:N为山体个数;xk0和yk0为第k座山体中心对称轴的横坐标和纵坐标;hk为第k座山体的最大高度;xki和yki为第k座山体的横向斜度和纵向斜度。

在飞行环境中, 山体附近通常存在雷暴等极端气象, 本文视为飞行威胁区, 并通过球体近似等效表示, 且记第k座山体附近飞行威胁区的球心坐标为 (xks0, yks0, zks0) , 半径为rk。

1.3 目标函数及航迹表示
在本文中, 执行任务的某型无人机, 其航迹规划的目标函数是生成一条由起始点到目标点的无碰撞可行航迹。采用q (x, y, z, θ, ψ) 表示无人机在飞行空域中某特定位置的特定姿态, 那么 (x, y, z) 则表示无人机所在航迹点, θ表示无人机的水平转弯角, ψ表示无人机的竖直爬升角或下滑角。采用r (q) 表示由起始点qinitial到目标点qgoal的无碰撞可行航迹, 那么航迹规划的过程可以写成如下形式:

2 开普勒算法
开普勒算法（Kepler’s Method）是一种用于计算行星轨道的数值方法，由德国天文学家约翰内斯·开普勒在16世纪发明。该算法基于开普勒第一定律，即行星绕太阳运动的轨道是一个椭圆，太阳位于椭圆的一个焦点上。开普勒算法可以通过已知的行星位置和速度，计算出行星在轨道上的位置和速度，从而预测行星的运动轨迹。

开普勒算法的具体步骤如下：
（1）根据开普勒第一定律，确定行星轨道的椭圆参数，包括半长轴、离心率、倾角等。
（2）根据行星的初始位置和速度，计算出行星在轨道上的位置和速度。
（3）根据牛顿运动定律，计算出行星在下一个时间步长内的位置和速度。
（4）重复步骤3，直到计算出所需的时间范围内的行星位置和速度。

⛄二、部分源代码

close all
clear
clc
addpath(‘./Algorithm/’)%添加算法路径
warning off;
%% 三维路径规划模型定义
global startPos goalPos N
N=2;%待优化点的个数(可以修改)
startPos = [10, 10, 80]; %起点(可以修改)
goalPos = [80, 90, 150]; %终点(可以修改)
SearchAgents_no=30; % 种群大小(可以修改)
Function_name=‘F2’; %F1:随机产生地图 F2：导入固定地图
Max_iteration=100; %最大迭代次数(可以修改)
% Load details of the selected benchmark function
[lb,ub,dim,fobj]=Get_Functions_details(Function_name);
[Best_score,Best_pos,curve]=KOA(SearchAgents_no,Max_iteration,lb,ub,dim,fobj);%算法优化求解
AlgorithmName=‘KOA’;%算法名字
figure
semilogy(curve,‘Color’,‘r’,‘linewidth’,3)
xlabel(‘迭代次数’);
ylabel(‘飞行路径长度’);
legend(AlgorithmName)
display(['算法得到的最优适应度: ‘, num2str(Best_score)]);
Position=[Best_pos(1:dim/3); Best_pos(1+dim/3:2*(dim/3)); Best_pos(1+(2*dim/3):end)]’; %优化点的XYZ坐标（每一行是一个点）
plotFigure(Best_pos,AlgorithmName)%画最优路径

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]田疆,李二超.用于无人机三维航迹规划改进连接型快速扩展随机树算法[J].航空工程进展. 2018,9(04)

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