基于牛优化（ OX Optimizer，OX）算法的多个无人机协同路径规划（可以自定义无人机数量及起始点），MATLAB代码

一、牛优化算法

牛优化（ OX Optimizer，OX）算法由 AhmadK.AlHwaitat 与 andHussamN.Fakhouri于2024年提出，该算法的设计灵感来源于公牛的行为特性。公牛以其巨大的力量而闻名，能够承载沉重的负担并进行远距离运输。这种行为特征可以被转化为优化过程中的优势，即在探索广阔而复杂的搜索空间时保持强大的鲁棒性。公牛不仅强壮，还具有灵活性、稳健性、适应性和协作能力等特点。这些特点使得OX优化器能够在不断变化的环境和优化需求中有效地找到最优解。

算法步骤：

参数初始化 ：首先，定义问题的解空间范围、种群规模以及最大迭代次数等关键参数，并根据需要选择合适的适应度函数来评估个体的优劣。

种群初始化 ：生成一个初始种群，其中每个个体代表问题的一个候选解。这些个体的位置在解空间中随机分布，以确保初始搜索的多样性。

适应度评估 ：计算种群中每个个体的适应度值，该值反映了个体在当前解空间中的优劣程度。

繁殖选择 ：

精英策略：保留适应度最高的个体，确保优质解不会因后续操作而丢失。

杂交操作：从当前种群中选择个体进行杂交。杂交过程通过交换两个父代个体的基因信息，生成新的子代个体。这一操作有助于引入新的解特征，扩大搜索范围。

变异操作：对子代个体进行变异。每个个体有较高的概率随机选择一个维度进行微小的随机扰动，这种变异操作能够进一步增强种群的多样性，避免过早收敛。

局部优化器 ：

置换探索：从当前最优解或随机子集中选取个体，通过局部优化器进行置换探索。例如，随机暴露置换群中的元素，并通过应用均匀分布的随机值来修正这些元素，从而生成新的置换。这一过程能够精化解的质量，提高局部搜索的效率。

整体最优解集中消去与鲁棒优化 ：在每次迭代中，跟踪由局部优化器生成的置换，并将这些置换中的所有解集中到集合S中。对于每个新的解x，检查是否存在另一个解x’∈S，其分量在设定的容差范围内。通过持续集中消去，有效保持算法的推进，确保搜索过程的高效性和鲁棒性。

迭代更新 ：重复上述步骤，直到达到最大迭代次数或满足其他停止条件。在整个优化过程中，种群逐渐进化，适应度较高的个体更有可能被保留下来，而适应度较低的个体则被淘汰。

参考文献：

[1]Al Hwaitat AK, Fakhouri HN. The OX Optimizer: A Novel Optimization Algorithm and Its Application in Enhancing Support Vector Machine Performance for Attack Detection. Symmetry. 2024; 16(8):966.

2. 无人机路径规划数学模型

2.1 路径最优性

为了提高无人机的操作效率，规划的路径需要在特定的应用标准下达到最优。在我们的研究中，主要关注空中摄影、测绘和表面检查，因此选择最小化路径长度作为优化目标。由于无人机通过地面控制站（GCS）进行控制，飞行路径

X i X_i

X

i

​

被表示为无人机需要飞越的一系列

n n

n 个航路点的列表。每个航路点对应于搜索地图中的一个路径节点，其坐标为

P i j

( x i j , y i j , z i j ) P_{ij} = (x_{ij}, y_{ij}, z_{ij})

P

ij

​

=

(

x

ij

​

,

y

ij

​

,

z

ij

​

) 。通过表示两个节点之间的欧几里得距离为 $| \overrightarrow{P_{ij}P_{i,j+1}} |，与路径长度相关的成本

F 1 F_1

F

1

​

可以计算为：

F 1 ( X )

∑ j

1 n − 1 ∥ P i j P i , j + 1 → ∥ F_1(X) = \sum_{j=1}^{n-1} | \overrightarrow{P_{ij}P_{i,j+1}} |

F

1

​

(

X

)

=

j

=

1

∑

n

−

1

​

∥

P

ij

​

P

i

,

j

1

​

​

∥

2.2 安全性和可行性约束

除了最优性之外，规划的路径还需要确保无人机的安全操作，引导其避开操作空间中可能出现的威胁，这些威胁通常由障碍物引起。设

K K

K 为所有威胁的集合，每个威胁被假设为一个圆柱体，其投影的中心坐标为

C k C_k

C

k

​

，半径为

R k R_k

R

k

​

，如下图 所示。

对于给定的路径段

∥ P i j P i , j + 1 → ∥ | \overrightarrow{P_{ij}P_{i,j+1}} |

∥

P

ij

​

P

i

,

j

1

​

​

∥ ，其相关的威胁成本与它到

C k C_k

C

k

​

的距离

d k d_k

d

k

​

成正比。考虑到无人机的直径

D D

D 和到碰撞区域的危险距离

S S

S ，威胁成本

F 2 F_2

F

2

​

在障碍物集合

K K

K 上计算如下：

F 2 ( X i )

∑ j

1 n − 1 ∑ k

1 K T k ( P i j P i , j + 1 → ) , F_2(X_i) = \sum_{j=1}^{n-1} \sum_{k=1}^K T_k(\overrightarrow{P_{ij}P_{i,j+1}}),

F

2

​

(

X

i

​

)

=

j

=

1

∑

n

−

1

​

k

=

1

∑

K

​

T

k

​

(

P

ij

​

P

i

,

j

1

​

​

)

,

其中

T k ( P i j P i , j + 1 → )

{ 0 , if  d k

S + D + R k ( S + D + R k ) − d k , if  D + R k < d k ≤ S + D + R k ∞ , if  d k ≤ D + R k T_k(\overrightarrow{P_{ij}P_{i,j+1}}) = \begin{cases} 0, & \text{if } d_k > S + D + R_k \ (S + D + R_k) - d_k, & \text{if } D + R_k < d_k \leq S + D + R_k \ \infty, & \text{if } d_k \leq D + R_k \end{cases}

T

k

​

(

P

ij

​

P

i

,

j

1

​

​

)

=

⎩

⎨

⎧

​

0

,

(

S

D

R

k

​

)

−

d

k

​

,

∞

,

​

if

d

k

​

S

D

R

k

​

if

D

R

k

​

<

d

k

​

≤

S

D

R

k

​

if

d

k

​

≤

D

R

k

​

​

在操作过程中，飞行高度通常被限制在给定的最小和最大高度之间，例如在调查和搜索应用中，需要相机以特定的分辨率和视场收集视觉数据，从而限制飞行高度。设最小和最大高度分别为

h min h_{\text{min}}

h

min

​

和

h max h_{\text{max}}

h

max

​

。与航路点

P i j P_{ij}

P

ij

​

相关的高度成本计算为：

H i j

{ ∣ h i j − h max + h min 2 ∣ , if  h min ≤ h i j ≤ h max ∞ , otherwise H_{ij} = \begin{cases} |h_{ij} - \frac{h_{\text{max}} + h_{\text{min}}}{2}|, & \text{if } h_{\text{min}} \leq h_{ij} \leq h_{\text{max}} \ \infty, & \text{otherwise} \end{cases}

H

ij

​

=

{

∣

h

ij

​

−

2

h

max

​

h

min

​

​

∣

,

∞

,

​

if

h

min

​

≤

h

ij

​

≤

h

max

​

otherwise

​

其中

h i j h_{ij}

h

ij

​

表示相对于地面的飞行高度，如下图所示。

可以看出，

H i j H_{ij}

H

ij

​

保持平均高度并惩罚超出范围的值。对所有航路点求和得到高度成本：

F 3 ( X )

∑ j

1 n H i j F_3(X) = \sum_{j=1}^n H_{ij}

F

3

​

(

X

)

=

j

=

1

∑

n

​

H

ij

​

平滑成本评估转弯率和爬升率，这对于生成可行路径至关重要。如下图 所示。

转弯角

ϕ i j \phi_{ij}

ϕ

ij

​

是两个连续路径段

P i j ′ P i , j + 1 ′ → \overrightarrow{P’{ij}P’{i,j+1}}

P

ij

′

​

P

i

,

j

1

′

​

​

和

P i , j + 1 ′ P i , j + 2 ′ → \overrightarrow{P’{i,j+1}P’{i,j+2}}

P

i

,

j

1

′

​

P

i

,

j

2

′

​

​

在水平面 Oxy 上的投影之间的角度。设

k → \overrightarrow{k}

k

是 z 轴方向的单位向量，投影向量可以计算为：

P i j ′ P i , j + 1 ′ →

k → × ( P i j P i , j + 1 → × k → ) \overrightarrow{P’{ij}P’{i,j+1}} = \overrightarrow{k} \times (\overrightarrow{P_{ij}P_{i,j+1}} \times \overrightarrow{k})

P

ij

′

​

P

i

,

j

1

′

​

​

=

k

×

(

P

ij

​

P

i

,

j

1

​

​

×

k

)

因此，转弯角计算为：

ϕ i j

arctan ⁡ ( ∥ P i j ′ P i , j + 1 ′ → × P i , j + 1 ′ P i , j + 2 ′ → ∥ P i j P i , j + 1 ′ → ⋅ P i , j + 1 ′ P i , j + 2 ′ → ) \phi_{ij} = \arctan\left( \frac{| \overrightarrow{P’{ij}P’{i,j+1}} \times \overrightarrow{P’{i,j+1}P’{i,j+2}} |}{\overrightarrow{P_{ij}P’{i,j+1}} \cdot \overrightarrow{P’{i,j+1}P’_{i,j+2}}} \right)

ϕ

ij

​

=

arctan

​

P

ij

​

P

i

,

j

1

′

​

​

⋅

P

i

,

j

1

′

​

P

i

,

j

2

′

​

​

∥

P

ij

′

​

P

i

,

j

1

′

​

​

×

P

i

,

j

1

′

​

P

i

,

j

2

′

​

​

∥

​

​

爬升角

ψ i j \psi_{ij}

ψ

ij

​

是路径段

P i j P i , j + 1 → \overrightarrow{P_{ij}P_{i,j+1}}

P

ij

​

P

i

,

j

1

​

​

与其在水平面上的投影

P i j ′ P i , j + 1 ′ → \overrightarrow{P’{ij}P’{i,j+1}}

P

ij

′

​

P

i

,

j

1

′

​

​

之间的角度，由下式给出：

ψ i j

arctan ⁡ ( z i , j + 1 − z i j ∥ P i j ′ P i , j + 1 ′ → ∥ ) \psi_{ij} = \arctan\left( \frac{z_{i,j+1} - z_{ij}}{| \overrightarrow{P’{ij}P’{i,j+1}} |} \right)

ψ

ij

​

=

arctan

​

∥

P

ij

′

​

P

i

,

j

1

′

​

​

∥

z

i

,

j

1

​

−

z

ij

​

​

​

然后，平滑成本计算为：

F 4 ( X )

a 1 ∑ j

1 n − 2 ϕ i j + a 2 ∑ j

1 n − 1 ∣ ψ i j − ψ j − 1 ∣ F_4(X) = a_1 \sum_{j=1}^{n-2} \phi_{ij} + a_2 \sum_{j=1}^{n-1} |\psi_{ij} - \psi_{j-1}|

F

4

​

(

X

)

=

a

1

​

j

=

1

∑

n

−

2

​

ϕ

ij

​

a

2

​

j

=

1

∑

n

−

1

​

∣

ψ

ij

​

−

ψ

j

−

1

​

∣

其中

a 1 a_1

a

1

​

和

a 2 a_2

a

2

​

分别是转弯角和爬升角的惩罚系数。

2.3 总体成本函数

2.3.1 单个无人成本计算

考虑到路径

X X

X 的最优性、安全性和可行性约束， **第

i i

i 个无人机总体成本函数** 可以定义为以下形式：

f i ( X )

∑ k

1 4 b k F k ( X i ) f_i(X) = \sum_{k=1}^4 b_k F_k(X_i)

f

i

​

(

X

)

=

k

=

1

∑

4

​

b

k

​

F

k

​

(

X

i

​

)

其中

b k b_k

b

k

​

是权重系数，

F 1 ( X i ) F_1(X_i)

F

1

​

(

X

i

​

) 到

F 4 ( X i ) F_4(X_i)

F

4

​

(

X

i

​

) 分别是路径长度、威胁、平滑度和飞行高度相关的成本。决策变量是

X X

X ，包括

n n

n 个航路点

P i j

( x i j , y i j , z i j ) P_{ij} = (x_{ij}, y_{ij}, z_{ij})

P

ij

​

=

(

x

ij

​

,

y

ij

​

,

z

ij

​

) 的列表，使得

P i j ∈ O P_{ij} \in O

P

ij

​

∈

O ，其中

O O

O 是无人机的操作空间。根据这些定义，成本函数

F F

F 是完全确定的，可以作为路径规划过程的输入。

2.3.2 多无人机总成本计算

若共有

m m

m 个无人机，其总成本为单个无人机成本和，计算公式如下：

f i t n e s s ( X )

∑ i

1 m f i ( X ) fitness(X) = \sum_{i=1}^mf_i(X)

f

i

t

n

ess

(

X

)

=

i

=

1

∑

m

​

f

i

​

(

X

)

参考文献：

[1] Phung M D , Ha Q P .Safety-enhanced UAV path planning with spherical vector-based particle swarm optimization[J].Applied Soft Computing, 2021(2):107376.DOI:10.1016/j.asoc.2021.107376.

三、部分代码及结果

close all
clear
clc
% dbstop if all error
pop=100;%种群大小（可以修改）
maxgen=200;%最大迭代（可以修改）

%% 模型建立
model=Create_Model();
UAVnum=4;%无人机数量(可以修改)  必须与无人机的起始点保持一致

%% 初始化每个无人机的模型
for i=1:UAVnum
    ModelUAV(i).model=model;
end

%% 第一个无人机 起始点
start_location = [120;200;100];
end_location = [800;800;150];
ModelUAV(1).model.start=start_location;
ModelUAV(1).model.end=end_location;
%% 第二个无人机 起始点
start_location = [400;100;100];
end_location = [900;600;150];
ModelUAV(2).model.start=start_location;
ModelUAV(2).model.end=end_location;
%% 第三个无人机 起始点
start_location = [200;150;150];
end_location =[850;750;150];
ModelUAV(3).model.start=start_location;
ModelUAV(3).model.end=end_location;
%% 第四个无人机 起始点
start_location = [100;100;150];
end_location = [800;730;150];
ModelUAV(4).model.start=start_location;
ModelUAV(4).model.end=end_location;
%% 第5个无人机 起始点
% start_location = [500;100;130];
% end_location = [850;650;150];
% ModelUAV(5).model.start=start_location;
% ModelUAV(5).model.end=end_location;
% %% 第6个无人机 起始点
% start_location = [100;100;150];
% end_location =   [800;800;150];
% ModelUAV(6).model.start=start_location;
% ModelUAV(6).model.end=end_location;

四个无人机：

五个无人机：

四、完整MATLAB代码见下方名片