基于烟花算法（Fireworks Algorithm，FWA）及三次样条的机器人路径规划，50个场景任意选择，完整MATLAB代码

一、烟花算法

烟花算法（Fireworks Algorithm，FWA）是一种受烟花爆炸产生火星启发的群体智能优化算法，由谭营教授等人于2010年提出。以下是其详细信息：

算法原理

• 初始化：在可行解空间中随机产生一定数量的烟花，每个烟花代表一个可行解。
• 适应度计算：计算每个烟花的适应度，适应度越高的烟花表示其位置越优。
• 爆炸行为：每个烟花根据其适应度产生一定数量的火花，适应度越高的烟花产生的火花数量越多，但爆炸半径越小；反之，适应度越低的烟花产生的火花数量越少，但爆炸半径越大。这样可以在全局和局部之间进行平衡搜索。
• 变异操作：对部分火花进行高斯变异，以增加种群的多样性，避免算法陷入局部最优。
• 选择策略：根据适应度选择下一代烟花，保留适应度较高的烟花和火花，淘汰适应度较低的个体。

算法特点

• 随机性：烟花的位置和火花的产生都具有一定的随机性，这使得算法具有较强的全局搜索能力。
• 局部性：适应度较高的烟花在较小的范围内产生较多的火花，有利于局部搜索。
• 爆发性：烟花爆炸时产生大量的火花，能够快速地扩展搜索范围。
• 隐并行性：多个烟花同时进行爆炸和搜索，具有一定的并行性。
• 多样性和瞬时性：火花的多样性和烟花的瞬时性使得算法能够不断探索新的解空间。

参考文献：

[1]Tan, Y. and Y. Zhu. Fireworks Algorithm for Optimization. in Advances in Swarm Intelligence. 2010. Berlin, Heidelberg: Springer Berlin Heidelberg.

二、路径规划问题的适应度-奖励函数

路径从起点 $(x_s,y_s)$ 开始，到终点 $(x_t,y_t)$ 结束，并包括 $n$ 个航路点。路径上的每个点表示为 $(x_i,y_i)$，其中 $i$ 从 1（起点）到 $n$（终点）。目标是求解约束优化问题，以找到起点和终点之间的最短安全路径。最短路径最小化连续点之间的总欧几里得距离，这是欧几里得空间中最直接的路线。

除了最小化距离外，路径必须满足两个约束条件。首先，所有路径点必须保持在边界内，即 $x_{\text{min}}\le x_i\le x_{\text{max}}$ 和 $y_{\text{min}}\le y_i\le y_{\text{max}}$。其次，路径必须避开障碍物，这通过条件 $g(x_i,y_i,b)\ge 0$ 强制执行，其中 $g(x_i,y_i,b)$ 是评估与第 $b$ 个障碍物碰撞条件的一般约束函数，并确保不发生碰撞。优化问题如下所示：

$$\begin{array}{rl}\text{minimize}& f(x,y)=\sum _{i=1}^{n-1}\sqrt{(x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2}\end{array}$$

subject to x min ≤ x i ≤ x max , ∀ i ∈ { 1 , … , n } y min ≤ y i ≤ y max , ∀ i ∈ { 1 , … , n } g ( x i , y i , b ) ≥ 0 , ∀ i ∈ { 1 , … , n } , ∀ b ∈ Obstacles \begin{aligned} \text{subject to} \quad & x_{\text{min}} \le x_i \le x_{\text{max}}, \quad \forall i \in \{1, \ldots, n\} \\ & y_{\text{min}} \le y_i \le y_{\text{max}}, \quad \forall i \in \{1, \ldots, n\} \\ & g(x_i, y_i, b) \ge 0, \quad \forall i \in \{1, \ldots, n\}, \quad \forall b \in \text{Obstacles} \end{aligned} subject to​xmin​≤xi​≤xmax​,∀i∈{1,…,n}ymin​≤yi​≤ymax​,∀i∈{1,…,n}g(xi​,yi​,b)≥0,∀i∈{1,…,n},∀b∈Obstacles​

适应度（成本）函数通过考虑路径的总距离和与障碍物碰撞产生的任何惩罚来评估给定路径的质量。惩罚包括两个部分：点惩罚（路径点与障碍物之间的碰撞）和线段惩罚（连续航路点之间的线段与障碍物之间的碰撞）。此公式确保路径既短又安全，有效指导优化过程。

线段惩罚对于识别点惩罚无法检测到的问题至关重要。即使所有航路点都位于障碍物外部，个别路径线段仍可能与障碍物相交；因此，线段惩罚对于捕捉这些点惩罚会遗漏的违规行为至关重要。虽然线段惩罚足以检测障碍物碰撞，但也会执行点检查以评估或惩罚各个航路点。任何位于障碍物内的航路点都表明严重违规，因为该航路点无法到达，从而使路径不可行。

相比之下，如果所有航路点都位于障碍物外部，但连接它们的某些线段与障碍物相交，则该路径仍可被视为候选路径。这些路径通常只需要在后续迭代中进行 minor 优化即可消除交点。这种惩罚组合确保对不可达航路点的路径施加更严厉的惩罚，而对 minor 问题的路径仍可进行优化。此外，点惩罚允许快速初步检查，帮助跳过不必要的线段惩罚计算，当路径因航路点位于障碍物内而明显无效时，这降低了计算复杂性。

成本函数由两个主要部分组成：路径长度 $L$ 和总惩罚 $p^T$，如下所示。路径长度 $L$ 表示路径上所有点之间的欧几里得距离，定义如下：

$$L=\sum _{i=1}^{n-1}\sqrt{(x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2}$$

总惩罚 $p^T$ 考虑碰撞情况，并按权重 $\beta$ 缩放，权重设为 100 以优先考虑安全性，对不安全路径施加严厉惩罚。对于无障碍物的路径，总成本等于路径长度 $L$，代表可实现的最小成本。

总惩罚 $p^T$ 表示所有障碍物的平均惩罚，包括基于点和线段的惩罚，如下所示，其中 $B$ 是障碍物的总数。对于每个障碍物 $b$，惩罚包括两个项：${\sum }_{i=1}^n{p}_i^b$，评估涉及 $n$ 个路径点 $(x_i,y_i)$ 的碰撞惩罚；以及 ${\sum }_{j=1}^{n-1}{p}_{\text{line},j}^b$，评估涉及 $n-1$ 个线段 $L_j=[(x_j,y_j),(x_{j+1},y_{j+1})]$ 的碰撞惩罚。惩罚通过除以 $B$（障碍物总数）进行归一化，以确保惩罚反映碰撞的总体分布和严重程度。

$$\text{cost}=L\times (1+\beta p^T)$$

对于基于网格的障碍物，两个组件 $p_i^b$ 和 $p_{\text{line},j}^b$ 使用网格表示进行计算。搜索空间表示为基于网格的环境，网格由其原点 $(x_g,y_g)$ 和分辨率 $(x_{\text{res}},y_{\text{res}})$ 定义。x 坐标从 $x_{\text{min}}$ 到 $x_{\text{max}}$ 以 $x_{\text{res}}$ 为步长进行离散化，y 坐标同样从 $y_{\text{min}}$ 到 $y_{\text{max}}$ 以 $y_{\text{res}}$ 为步长进行离散化。路径上的每个点 $(x_i,y_i)$ 被映射到网格中的一个对应单元格，索引为 $(x{c}_i,y{c}_i)$，计算方法如下：

$$x{c}_i=\text{round}\left(\frac{x_i-x_{og}}{x_{\text{res}}}\right),y{c}_i=\text{round}\left(\frac{y_i-y_{og}}{y_{\text{res}}}\right)$$

网格中的每个单元格被分配一个成本 $M(x{c}_i,y{c}_i)$，其中 0 表示空闲单元格，100 表示完全被占据的单元格，如 SLAM 等映射算法所确定。对于基于点的惩罚 $p_i$，对应于点 $(x_i,y_i)$ 的单元格的成本 $M(x{c}_i,y{c}_i)$ 通过除以 100 进行归一化，得到范围为 $[0,1]$ 的惩罚，如下所示：

$$p_i=\frac{M(x{c}_i,y{c}_i)}{100}$$

线段惩罚 $p_{\text{line},j}$ 通过确定由两个连续航路点连接的线段 $L_j$ 相交的所有网格单元格来计算。使用 Bresenham 线算法确定这些单元格，其中 $n_j$ 是这些单元格的数量。

上图展示了线段与网格单元格之间的相交情况。然后通过对这些相交单元格的归一化占用成本进行平均来聚合惩罚，如下所示：

$$p_{\text{line},j}=\frac{1}{n_j}\sum _{k=1}^{n_j}\frac{M(x{c}_k,y{c}_k)}{100}$$

方程 被修改为在基于网格的环境中计算总体路径惩罚 $p^T$，如下所示，其中成本直接从网格中获得。

$$p^T=\sum _{i=1}^n{p}_i+\sum _{j=1}^{n-1}{p}_{\text{line},j}$$

成本函数支持多种障碍物类型，重点在于基于网格的障碍物，关于与圆形和矩形障碍物相关的 $p_i^b$ 和 $p_{\text{line},j}^b$ 的详细公式以及这些类型之间的转换机制详细参考文献。
参考文献：
[1]Reda, M., Onsy, A., Haikal, A.Y. et al. A novel reinforcement learning-based multi-operator differential evolution with cubic spline for the path planning problem. Artif Intell Rev 58, 142 (2025). https://doi.org/10.1007/s10462-025-11129-6

三、部分MATLAB及结果

50个椭圆或矩阵障碍物场景任意选择

clear;
clc;
close all;
warning off all;
AlgorithmName='';%算法名称
N=40;%选择地图-可以选择1-50;  1-16 椭圆障碍物，17-50 矩形障碍物
SearchAgents_no=50;    %种群大小
Max_iteration=200;    %最大迭代次数
[lb,ub,dim,fobj,Fobj,model] = Get_F(N);%获取参数
[Best_score,Best_pos,cg_curve]=(SearchAgents_no,Max_iteration,lb,ub,dim,fobj);%算法求解
GlobalBest=Fobj(Best_pos);%计算最优个体的详细路径