栅格地图路径规划：基于雪橇犬优化算法（Sled Dog Optimizer，SDO）的移动机器人路径规划（提供MATLAB代码）

一、雪橇犬优化算法

雪橇犬优化算法（Sled Dog Optimizer，SDO）是一种于2024年10月发表在JCR1区、中科院1区SCI期刊《Advanced Engineering Informatics》的仿生元启发式算法。它受雪橇犬行为模式启发，通过模拟狗拉雪橇、训练和退役等行为构建数学模型，旨在解决工程领域的优化问题，为复杂优化问题提供了创新的解决思路和技术手段。

算法原理

初始化原理：与多数群优化算法相似，SDO采用随机初始化策略。在搜索空间中，通过公式 $Do g = L b + r an d \cdot (U b - L b)$ 来确定每只“雪橇犬”（代表解空间中的个体）的初始位置。其中， $L b$ 和 $U b$ 分别是各维度变量的下限和上限， $r an d$ 是在 $[0, 1]$ 区间内均匀分布的随机数。这种初始化方式使得算法在开始时能够在整个搜索空间内进行广泛的探索，为后续寻找全局最优解奠定基础。
雪橇犬选择原理：在整个雪橇犬群体中，并非所有个体都参与每次的“拉雪橇”任务（对应于算法中的搜索操作）。通常依据公式 $N_1 = 0.7·N + k_1 ·2k^2$ （ $N$ 为种群数量）来确定参与拉雪橇的犬只数量 $N_1$ 。被选中的雪橇犬通常是群体中各方面能力较强的个体，而剩余的犬只则用于训练以提升其能力，这种选择机制模拟了实际中根据个体能力分配任务的情况，有助于提高算法的搜索效率。
雪橇犬旅行原理
- 正常旅行：在模拟的雪橇犬旅行过程中，它们以两列的队形前进。领头犬在旅途中不仅要持续接收并执行主人发出的命令，还需依据自身训练经验应对突发事件。雪橇犬的速度更新规则因自身位置而异：
  - 当 $i = 1, 2$ 时，速度更新公式为 $V_{i}=\omega \cdot V_{i}+c_{1} \cdot r_{1} \cdot\left(Dog_{G_{i}} - Dog_{i}\right)+c_{2} \cdot r_{2} \cdot\left(Dog_{Z} - Dog_{i}\right)$
  - 当 $\in [3, N_{1}-2]$ 时，公式为 $V_{i}=\omega \cdot V_{i}+c_{1} \cdot r_{1} \cdot\left(Dog_{i - 2}-Dog_{i}\right)+c_{2} \cdot r_{2} \cdot\left(Dog_{i + 2}-Dog_{i}\right)+2 \cdot l \cdot r_{3} \cdot\left(Dog_{t}-Dog_{i}\right)$
  - 当 $i = N_{1}-1, N_{1}$ 时，公式为 $V_{i}=\omega \cdot V_{i}+c_{1} \cdot r_{1} \cdot\left(Dog_{i - 2}-Dog_{i}\right)+c_{2} \cdot r_{2} \cdot\left(Dog_{r}-Dog_{i}\right)+2 \cdot l_{r_{3}} \cdot\left(Dog_{t}-Dog_{i}\right)$
    这里的 $\omega$ 是惯性权重，用于平衡算法的全局探索和局部开发能力； $c_1$ 和 $c_2$ 是学习因子，控制算法向个体最优和全局最优位置学习的程度； $r_1$ 、 $r_2$ 、 $r_3$ 是在 $[0, 1]$ 区间内的随机数； $Dog_{G_{i}}$ 、 $Dog_{Z}$ 、 $Dog_{r}$ 等表示不同参考位置的雪橇犬个体。位置更新则通过公式 $Dog_i = Dog_i + V_i$ 实现，基于更新后的速度来调整雪橇犬在解空间中的位置。
- 避障：在旅行过程中，若遇到障碍，雪橇犬会在主人指挥和日常训练的指导下进行避障操作。其位置更新公式为：
  $Dog_i = \begin{cases} Dog_{i}+0.5 \cdot r_{4} \cdot\left(Dog_{Z}-Dog_{N}\right)+k \cdot p_{2} \cdot r_{5} \cdot\left(Dog_{G_{i}} - Dog_{N}\right), & \text{if } rand \geq 0.5 \\ Dog_{i}+0.5 \cdot r_{4} \cdot\left(Dog_{Z}-Dog_{N}\right)+k \cdot p_{1}^{2} \cdot r_{5} \cdot\left(Dog_{G_{i}} - Dog_{N}\right), & \text{otherwise} \end{cases}$
  其中 $r_4$ 、 $r_5$ 是随机数， $k$ 、 $p_1$ 、 $p_2$ 是算法参数， $Dog_{N}$ 表示种群中的某个参考个体。该公式模拟了雪橇犬在面对障碍时的不同应对策略，确保算法在搜索过程中能够避开局部最优陷阱。
- 迷失方向：当雪橇队在行进过程中迷失方向时，雪橇犬会在主人带领下随机探索周围区域。对应的数学模型为：
  $Dog_{i}=r_{6} \cdot\left(\frac{Dog_{i}+Dog_{z}}{2}\right)+0.5 \cdot C \cdot r_{7} \cdot \zeta \cdot\left(Dog_{r}-\zeta \cdot Dog_{i}\right)$
  此公式中 $r_6$ 、 $r_7$ 是随机数， $C$ 、 $\zeta$ 是算法参数。这种随机探索机制有助于算法在陷入局部最优时跳出当前区域，继续寻找更优解。
雪橇犬训练原理：对于那些不符合要求（例如适应度值较差）的雪橇犬，算法通过特定的训练机制来提升其能力。训练公式为 $Dog_{i}=Dog_{i}+r_{8} \cdot F \cdot\left(r_{9} \cdot D_{1} \cdot X_{1}+r_{10} \cdot D_{2} \cdot X_{2}+r_{11} \cdot D_{3} \cdot X_{3}\right)$ ，其中：
- $D_{1}=\sqrt{\sum_{j = 1}^{Dim }\left(Dog_{Z}^{j}-Dog_{Better }^{j}\right)^{2}}$ ，用于衡量 $Dog_{Z}$ 与 $Dog_{Better}$ 在各维度上的距离。
- $D_{2}=\sqrt{\sum_{j = 1}^{Dim}\left(\frac{Dog_{1}^{j}+Dog_{2}^{j}}{2}-Dog_{Worse }^{j}\right)^{2}}$ ，反映了 $(\frac{Dog_{1}+Dog_{2}}{2})$ 与 $Dog_{Worse}$ 之间的差异程度。
- $D_{3}=\sqrt{\sum_{j = 1}^{Dim}\left(Dog_{Better }^{j}-Dog_{Worse }^{j}\right)^{2}}$ ，表示 $Dog_{Better}$ 和 $Dog_{Worse}$ 之间的距离。
- $X_{1}=Dog_{Z}-Dog_{Better}$ ， $X_{2}=\frac{Dog_{1}+Dog_{2}}{2}-Dog_{Worse}$ ， $X_{3}=Dog_{Better}-Dog_{Worse}$ 。 $Dog_{Better}$ 是从完成任务的雪橇犬中随机选择的个体， $Dog_{Worse}$ 是未被选中执行任务的雪橇犬。通过这种训练方式，使得较差的个体能够向更优的个体学习，从而提升整个种群的质量。
雪橇犬退役原理：考虑到雪橇犬在实际拉雪橇过程中，随着拉雪橇次数增加，受伤可能性增大，当受伤后便难以完成任务，此时需要让其退役。在算法中，通过对适应度值进行排序来实现退役机制。具体过程为：输入雪橇犬种群 $X$ 和适应度值 $f$ ，将适应度值从小到大排序并记录索引数组 $in d e x$ ，然后根据索引重新排列个体的速度、历史最优位置等相关信息，最终输出排序后的种群 $X^{'}$ 和适应度值 $f^{'}$ 。这一机制保证了算法在迭代过程中，不断淘汰较差的个体，保留更优的个体，从而使种群朝着更优的方向进化。

算法模型

速度更新模型：在雪橇犬正常旅行时，根据不同位置的雪橇犬设置了不同的速度更新公式，如上述在 $i = 1, 2$ 、 $\in [3, N_{1}-2]$ 、 $i = N_{1}-1, N_{1}$ 三种情况下的速度更新公式，这些公式综合考虑了惯性、个体最优位置和全局最优位置等因素，引导雪橇犬在解空间中合理移动。
位置更新模型：包括正常旅行、避障和迷失方向三种情况下的位置更新公式。正常旅行时通过 $Dog_i = Dog_i + V_i$ 更新位置；避障时根据随机条件选择不同的公式更新位置；迷失方向时使用特定公式进行随机探索更新位置，这些模型确保了雪橇犬在各种情况下都能在解空间中进行有效的搜索。
训练模型：针对不符合要求的雪橇犬，利用 $Dog_{i}=Dog_{i}+r_{8} \cdot F \cdot\left(r_{9} \cdot D_{1} \cdot X_{1}+r_{10} \cdot D_{2} \cdot X_{2}+r_{11} \cdot D_{3} \cdot X_{3}\right)$ 这一公式进行训练，通过计算不同个体之间的距离和差异，指导较差个体向更优个体学习，实现种群的优化。
退役模型：以适应度值为依据，通过对适应度值排序并重新排列相关信息来实现雪橇犬的退役操作，其核心步骤包括对适应度值排序、根据索引重排个体信息等，保证算法中保留更优的个体。

算法伪代码

Input: N (种群大小), Dim (种群维度), T (最大迭代次数)
Output: Final Value
Randomly generate initial population by Eq.(1).
While t <= TUpdate ω, C_1, C_2, P_1, P_2, C and so on.Sort sled dogs by the retirement mechanism. (Algorithm1)Calculate the dogs selected to pull the sled (N_1).For i = 1:N (Sled teams travelling normally)Update the velocity of dogs using Eq.(3) to Eq.(5).Update the position of dogs using Eq (10).End ForFor i = N_1 + 1:N (Sled dog training)Update the position of sled dogs using Eq. (18).End ForSD = rand If SD <= AFor i = 1:N (Avoiding obstacles)Update the position of sled dogs using Eq. (11).End ForElse If SD > A and SD <= B The sled dogs are not experiencing special circumstances.ElseFor i = 1:N (Disorientation)Update the position of sled dogs using Eq. (14).End ForEnd If
End While

参考文献
[1] GANG HU . SDO: A novel sled dog-inspired optimizer for solving engineering problems[J]. Advanced Engineering Informatics, 2024, 62: Article 102783. DOI:10.1016/j.aei.2024.102783.

二、移动机器人路径规划及栅格地图环境搭建

移动机器人路径规划是移动机器人研究的核心领域之一，旨在为机器人从起点到目标点生成一条安全、无碰撞且最优的路径。路径规划根据环境信息的已知程度，分为全局路径规划和局部路径规划。全局路径规划假设环境信息已知，适用于静态环境；局部路径规划则适用于环境信息未知或部分已知的动态环境。随着机器人技术的发展，路径规划算法不断演进，从传统的栅格法和人工势场法，发展到现代的智能优化算法，如遗传算法、粒子群优化算法（PSO）、麻雀搜索算法（SSA）、灰狼优化算法和鲸鱼优化算法等。

栅格法：将环境划分为二维或三维的栅格，机器人在栅格间移动。该方法简单直观，但存储需求随空间增大而剧增，决策速度下降。
人工势场法：通过模拟引力和斥力引导机器人移动，但容易陷入局部最优解和死锁现象。
智能优化算法：如蚁群算法通过模拟蚂蚁释放信息素的行为，能够找到最优路径，但存在收敛速度慢的问题。改进的蚁群算法通过引入跳点搜索和信息素更新策略，提高了收敛速度和路径平滑度。麻雀搜索算法基于麻雀觅食行为，具有结构简单、收敛速度快的优点，适用于路径优化问题。

栅格地图是路径规划中常用的环境表示方法，通过将工作空间划分为二维栅格来简化环境建模。每个栅格用序号标识，无障碍物的栅格为可行栅格（标记为0），有障碍物的栅格为不可行栅格（标记为1）。栅格的尺寸根据障碍物尺寸和安全距离设置。通过障碍物膨胀，可以确保机器人在路径规划时避开障碍物。

在20x20的栅格环境中，每个栅格的坐标通过以下公式计算：
在这里插入图片描述

$\text{mod}(N_i / N) - 0.5$
$\text{ceil}(N_i / N) + 0.5$
其中， $N_i$ 是栅格的序号， $N$ 是每列的栅格数量。栅格中心位置作为其在坐标系中的坐标。路径规划问题转化为在栅格地图上寻找从起始点到目标点的有序栅格子集，这些栅格子集的中心连线即为规划路径。
参考文献：

[1]史恩秀,陈敏敏,李俊,等.基于蚁群算法的移动机器人全局路径规划方法研究[J].农业机械学报, 2014, 45(6):5.DOI:CNKI:SUN:NYJX.0.2014-06-009.

[2]朱庆保,张玉兰.基于栅格法的机器人路径规划蚁群算法[J].机器人, 2005, 27(2):5.DOI:10.3321/j.issn:1002-0446.2005.02.008.

[3]曹新亮,王智文,冯晶,等.基于改进蚁群算法的机器人全局路径规划研究[J].计算机工程与科学, 2020, 42(3):7.DOI:CNKI:SUN:JSJK.0.2020-03-027.

三、部分MATLAB及结果

clc
clear
close all
tic
%% 地图
global G S E
G=[0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 1 0 0 0 0; 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 1 0 0 0 0; 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 0 1 0 0 0 0 1 1 0 0 0 1 1 1 0 1 1 0 0; 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0; 0 1 1 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0;0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0; 0 1 1 1 0 0 0 0 0 0 1 0 0 0 0 0 1 1 0 0; 0 0 1 0 0 0 0 0 0 0 1 0 1 1 0 0 0 1 0 0; 0 0 1 0 0 0 0 1 1 0 1 0 1 1 0 0 0 1 0 0; 0 0 1 0 0 0 0 1 1 0 0 0 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 1 1 1 0; 1 1 1 1 0 0 0 0 0 0 0 0 0 1 0 1 1 1 1 0; 1 1 1 1 0 0 1 1 0 0 0 1 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 1 0 1 1 1 0 0 0 0 0 1 1 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 1 0; 0 0 1 1 0 0 0 0 0 0 1 1 0 1 1 1 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;];
for i=1:20/2for j=1:20m=G(i,j);n=G(21-i,j);G(i,j)=n;G(21-i,j)=m;end
end
%% 
S = [1 1];   %起点
E = [20 20];  %终点
[ub,dimensions] = size(G);        
dim = dimensions - 2;