Cyberbotics 机器人课程/高级编程练习

本章需要计算机科学方面的先进知识。它将通过一系列基于 e-puck 机器人的练习，引导你学习一些高级主题。在这个层面上，主题变得越来越丰富和复杂，因此我们不会试图面面俱到。相反，我们将重点关注五个热门话题，让你深入了解当今的机器人技术。第一个练习是关于使用里程计进行位置估计。第二个是关于使用 NF1 和势场进行路径规划。第三个是关于使用人工神经网络和监督学习进行模式识别。最后，我们将讨论使用粒子群优化 (PSO) 进行无监督学习，并研究同步定位和地图构建问题 (SLAM)。

里程计 [高级]

我们在之前的练习中看到，e-puck 机器人有两个直流电机，每个电机都有一个编码器。这些编码器统计电机完成的步数（车轮旋转一圈为 1000 步）。通过记录这些信息，可以估计机器人的轨迹。这被称为 *里程计*。在下面的练习中，我们将学习里程计的基础知识。

上述过程的主要缺点是误差累积。在每一步（每次你进行编码器测量时），位置更新都将包含一些误差。这种误差会随着时间的推移而累积，因此无法在长距离内进行准确的跟踪。然而，在短距离内，里程计可以提供非常精确的结果。为了获得这样的结果，校准机器人至关重要。车轮直径的微小差异将在几米后导致重大误差，如果这些差异没有得到适当考虑。请注意，校准必须针对每个机器人进行，最好是在它以后将要使用的表面上进行。该程序也适用于模拟的 e-puck。

一些理论

对于差动驱动机器人，可以通过观察编码器值的变化来估计机器人的位置：$\Delta s_{r}$ 和 $\Delta s_{l}$。利用这些值，我们可以更新机器人的位置$p=\left[{\begin{array}{c}x\\y\\\theta \end{array}}\right]$ 以找到其当前位置 $p'=p+\left[{\begin{array}{c}\Delta x\\\Delta y\\\Delta \theta \end{array}}\right]$。为了最大限度地提高整个方法的精度，需要尽可能频繁地进行此更新。我们表示 $\Delta \theta ={\frac {\Delta s_{r}-\Delta s_{l}}{b}}$ 和 $\Delta s={\frac {\Delta s_{r}+\Delta s_{l}}{2}}$。因此我们有 $\Delta x=\Delta s\cdot cos(\theta +{\frac {\Delta \theta }{2}})$ 和 $\Delta y=\Delta s\cdot sin(\theta +{\frac {\Delta \theta }{2}})$ 其中 b 是两个轮子之间的距离。总结一下，我们有

$p=f(x,y,\theta ,\Delta s_{r},\Delta s_{l})=\left[{\begin{array}{c}x\\y\\\theta \end{array}}\right]+\left[{\begin{array}{c}{\frac {\Delta s_{r}+\Delta s_{l}}{2}}\cdot cos(\theta +{\frac {\Delta s_{r}-\Delta s_{l}}{2b}})\\{\frac {\Delta s_{r}+\Delta s_{l}}{2}}\cdot sin(\theta +{\frac {\Delta s_{r}-\Delta s_{l}}{2b}})\\{\frac {\Delta s_{r}-\Delta s_{l}}{b}}\end{array}}\right]$

这些方程在 的第 5.2.4 节中给出。您将在同一参考文献中找到有关误差传播的更多信息。

校准您的 e-puck

打开以下世界文件

.../worlds/advanced_odometry.wbt

这个世界只有一个机器人，并且已经实现了里程计方程。里程计（.h 和 .c） 模块跟踪机器人的位置，里程计_goto（.h 和 .c） 模块旨在为机器人提供目的地并计算将其引导到那里的所需电机速度。

校准程序改编自 针对 e-puck 机器人，其目标是计算以下三个参数

• 左轮每增量距离（左转换系数）
• 右轮每增量距离（右转换系数）
• 两轮之间的距离（轴长）

这些参数可以相当容易地估计，但我们无法直接测量它们，以达到足够的精度。因此，我们将改为测量另外 4 个参数，并进行一些数学运算以获得我们需要的数值。每个测量都是该过程中的一个步骤

1. 每旋转增量 是每个轮子旋转的增量数
2. 轴轮比率 是轴长与平均轮直径的比率
3. 左直径 和 右直径 是两个轮子的直径
4. 缩放系数 不言而喻

获得这些参数后，我们可以按如下方式计算先前的参数

$distancePerIncrementLeft={\frac {diameterLeft\cdot scalingFactor\cdot 2\cdot \pi }{incrementsPerTour}}$

$distancePerIncrementRight={\frac {diameterRight\cdot scalingFactor\cdot 2\cdot \pi }{incrementsPerTour}}$

$axisLength={\frac {axisWheelRatio\cdot (diameterLeft+diameterRight)}{2\cdot scalingFactor}}$

步骤 1 - 每圈增量

每圈增量的数量可以通过实验找到。在本测试中，机器人可以加速，稳定其速度并执行给定数量的电机增量。此值可以通过 advanced_odometry.c 中的常量 INCREMENT_TEST 进行修改。当然，代码中的任何更改都必须伴随着构建和仿真还原。

[P.1] 使用键 '1' 运行第一步，并将 INCREMENT_TEST 值设置为 1000。通过在轮子上放置一个标记来检查轮子是否恰好转了一圈。如果不是，则找到一个值使得轮子完美转动，并通过进行 10 圈而不是 1 圈来检查它。

步骤 2 - 轴轮比

如果你在 e-puck 的文档中搜索，你会发现每个轮子的直径应该为 41 毫米，轮子之间的距离为 53 毫米，这给出了 1.293 的比例。为了验证这一点，我们将命令机器人原地旋转多次。为了完成一圈，我们将需要 1000 * 1.293 = 1293 个增量。步骤数量可以通过常量 RATIO_TEST 进行调整。

[P.2] 使用键 '2' 运行第二步，并像之前一样调整值以获得一个完美的转动。然后用 10 圈或更多圈来验证它。一旦你得到一个好的值，就反过来计算出轴轮比。

步骤 3 - 轮子直径

为了通过实验找到两个轮子之间直径的差异，我们将加载我们当前的里程计模型到机器人上，然后让它在你的竞技场中移动，同时跟踪其位置。最后，我们让机器人回到它认为的起始位置，并记下它与实际初始位置的偏移量。我们将给机器人四个路点，使其沿着一个正方形移动。默认情况下，正方形的边长为 0.2 米，但如果你有更多空间，你可以增加这个尺寸以获得更好的结果。

[P.3] 在 odometry.c 文件中输入每圈增量的数量和轴轮比的值。这将提供我们完成校准所需的精度。

[P.4] 使用键 '3' 运行第三个测试，机器人将移动并回到其起始位置附近。如果正方形没有闭合，如名为“开放和闭合正方形轨迹”的图所示，请在 odometry.c 中增加左侧直径并减小右侧直径。这意味着：$diameterLeft=diameterLeft+\delta$ 和 $diameterRight=diameterRight-\delta$。相反，如果轨迹太闭合，如同一张图所示，则减小左侧直径并增加右侧直径。

开放和闭合正方形轨迹

步骤 4 - 缩放因子

缩放因子是最容易找到的，我们只需让机器人沿直线行驶一段给定距离（代码中为 0.3 米），并测量实际行驶的距离。

[P.5] 使用键盘键 '4' 启动第四个测试，机器人将开始移动。测量行驶距离并计算缩放因子，如下所示： $scalingFactor={\frac {real\ distance}{parameter\ distance}}$。同样，将此值报告到 odometry.c 中。

现在您的机器人已经过校准。

[Q.1] 我们进行了一整套程序来限制里程计过程中的误差，但这些误差从何而来？是什么原因导致的？

使用里程计

现在我们已经拥有了一个完整且经过校准的里程计模块，我们将使用它。控制机器人移动的命令汇总在下表中。

本练习的命令摘要

键盘按键	操作
1	开始校准步骤 1
2	开始校准步骤 2
3	开始校准步骤 3
4	开始校准步骤 4
向上箭头	增加机器人速度
向下箭头	降低机器人速度
向左箭头	向左转
向右箭头	向右转
S	停止机器人
R	重置编码器

[P.6] 使用里程计可以计算蓝色六边形的面积吗？

这是一个里程计的简单应用，但在接下来的练习中，您将看到我们严重依赖里程计来了解机器人的位置。我们还使用它来精确地移动机器人。使用里程计的练习包括路径规划 和 SLAM。

路径规划 [高级]

概述

本练习的目标是实现和实验两种路径规划方法：势场法和 NF1。我们将对它们进行比较，以找出两种方法的优缺点以及在使用它们时需要注意的事项。路径规划在工业领域得到广泛应用，例如，用于移动具有 6 个自由度的机械臂。移动机器人，特别是在我们所做的二维环境中，所使用的技术要简单得多。

势场法

本练习基于 的第 6.2.1.3 节。目标是通过提供的环境为 e-puck 实现简单的势场控制。

理论

势场法的基本思想是在地图上创建一个场（或梯度），引导机器人到达目标。我们将机器人视为一个受人工势场 $U(q)$ 影响的点。机器人通过跟随场移动，就像一个球沿坡向下滚动一样。这可以通过给目标施加一个吸引力（即最低势能）以及给障碍物施加非常高的排斥力（场中的峰值）来实现。在图中，您看到的是单个三角形障碍物（蓝色）和目标（红色）的等势线图，相同的设置在下一张图中被绘制为 3D 场。

势场的等势线图

3D 中的势场

由此产生的力可以在每个位置计算

$q=(x,y):F(q)=-\nabla U(q)$

其中 $\nabla U(q)$ 表示 $U$ 在位置 $q$ 处的梯度

$\nabla U(q)=\left[{\begin{array}{c}{\frac {\partial U}{\partial x}}\\{\frac {\partial U}{\partial y}}\end{array}}\right]$

然后我们可以将势能和力分成两个不同的部分，一个吸引部分和一个排斥部分。然后我们可以分别计算它们

$U_{att}={\frac {1}{2}}k_{att}\cdot \rho _{goal}^{^{2}}(q)$

$F_{att}(q)=-k_{att}\cdot (q-q_{goal})$

$U_{rep}=\left\{{\begin{array}{cc}{\frac {1}{2}}k_{rep}\left({\frac {1}{\rho (q)}}-{\frac {1}{\rho _{0}}}\right)^{^{2}}&if\ \rho (q)\leq \rho _{0}\\0&if\ \rho (q)\geq \rho _{0}\end{array}}\right\}$

$F_{rep}(q)=\left\{{\begin{array}{cc}{\frac {1}{2}}k_{rep}\left({\frac {1}{\rho (q)}}-{\frac {1}{\rho _{0}}}\right){\frac {q-q_{obstacle}}{\rho ^{^{3}}(q)}}&if\ \rho (q)\leq \rho _{0}\\0&if\ \rho (q)\geq \rho _{0}\end{array}}\right\}$

应用

打开以下世界文件

.../worlds/advanced_path_planning.wbt

[P.1] 在控制器 advanced_path_planning_potential_field.c 中根据公式 $U_{att}$ 到 $F_{att}(q)$ 实现目标吸引势场和力。

[P.2] 根据公式 $U_{rep}$ 到 $F_{rep}(q)$ 实现排斥障碍势场和力。

[P.3] 使用当前位置作为参数调用您刚刚实现的函数，并根据计算出的力进行控制。提供的代码结构中清楚地标明了缺少的代码。

[P.4] 在仿真和真实 e-puck 上测试您的代码。在真实 e-puck 上运行时，您可以打印 .../path_planning_obstacle.pdf 中给出的 A3 纸张副本。

[Q.1] 您是否注意到任何差异？哪些差异？为什么会出现这些差异？

[P.5] 测试其他势函数（例如，线性吸引势而不是二次吸引势）。

[Q.2] 有些方法可以改善机器人的轨迹（例如扩展势场），想想如何才能获得更短的路径。

NF1

NF1 算法，也称为草火算法，是路径规划的另一种方法。它在 的第 6.2.1.2 节和图 6.15 中有描述。该算法已在提供的代码中实现。

理论

该算法可以这样解释

• 将计划划分为称为单元格的简单几何区域。在本例中，我们通过创建带有方形单元格的网格来实现，并且可以根据需要更改分辨率。
• 确定哪些单元格被占用（被障碍物）以及哪些单元格是空闲的。同时找到起点所在的单元格和目标所在的单元格。
• 在空闲单元格中找到一条从起点到目标位置的路径。在 NF1 中，这是通过为每个单元格分配一个距离来完成的，目标距离为 0，然后每次移动都会增加到目标的距离。此分配可以在整个网格上递归进行。

此过程的结果可以在名为“使用 NF1 的单元格分解和到目标的距离”的图中看到。我们得到一个离散梯度，类似于我们之前拥有的势场。

使用 NF1 的单元格分解和到目标的距离

应用

[P.7] 浏览 NF1 控制器代码控制器 advanced_path_planning_NF1.c 并在仿真和真实 e-puck 上运行它。

[Q.3] 你注意到什么？行为是最优的吗？注意它如何基于在里程计校准过程的步骤 3 中使用的运动控制算法：步骤 3。

[P.8] 如果你在文件 .../lib/odometry_goto.c 中增加运动控制算法的配置值（k_alpha、k_beta 和 k_rho）会发生什么？你能解释一下吗？（提示：Webots 是一个物理上真实的模拟器）

[P.9] 你可以修改运动控制算法以获得更平滑的行为吗？

比较

现在我们将比较两种路径规划方法，以突出每种方法的优缺点。

[P.10] 使用文件 .../path_planning_blank.pdf 中给出的空白 A3 世界表，设计一个势场应该失败而 NF1 不应该失败的世界。

[P.11] 在两个控制器中输入障碍物坐标，并在模拟中运行它们以确认预测。

[Q.4] 基于这些练习，列出每种方法的优点和缺点。

使用反向传播算法的模式识别 [高级]

机器人学习是计算机科学和机器人领域最令人兴奋的主题之一。本练习的目的是展示你的机器人能够学习。首先，将介绍监督机器学习的基础知识。然后，你将详细使用一种高级技术来识别 e-puck 摄像机图像中的模式：反向传播算法。本练习涵盖了两个主题：图像处理和人工神经网络。

练习说明

一般来说，图像中的模式识别有许多应用，例如许多扫描仪中使用的光学字符识别 (OCR) 或许多网络摄像头中使用的面部检测。在机器人领域，它可以用于定位环境中的机器人。

本练习的目的是简单地识别墙壁上的某些地标。你的 e-puck 应该能够在一个迷宫中随机移动（参见图），并且每次其摄像头刷新时，识别它看到的标志。名为“要识别的 4 个标志”的图描绘了本练习中使用的标志。请注意，摄像头值难以处理。首先，有大量的数值需要处理（此处：52*39*3=6084 个介于 0 和 255 之间的整数）。此外，这些值周围还有一些噪声。

墙壁上涂有标志的迷宫

要识别的 4 个标志

关于模式识别的直觉

[Q.1] 考虑上一节中描述的问题。描述一种在摄像机图像值中检测蓝色标志的方法，即找到标志在图像中的 x-y 坐标和大小。（提示：一个图像中可能存在多个标志。）

[Q.2] 一旦知道标志的位置和大小，描述一种识别它们的方法，即确定它们属于哪一类标志（l1、l2、l3 或 l4）。

[Q.3] 你的识别方法易于维护吗？（提示：如果你修改标志的形状以获得水平交叉、对角线交叉、圆形和棋盘格，你的方法是否可以直接应用？）

机器学习

你在上一节中观察到模式识别并非易事。可以通过经验性编程获得一些结果。但程序随后会针对给定问题而特定。

有没有什么方法可以让你的机器人能够学习任何标志？在接下来的练习中，将介绍监督学习方法。监督学习是机器学习主题的一个子集。它主要由一个函数构成，该函数将机器人的输入映射到其输出。该函数可以使用由专家填充的数据库进行训练。该数据库包含一对输入和专家期望的相应输出。在本练习中使用的方法中，该函数由一个人工神经网络 (ANN) 表示，并且使用反向传播算法来训练该函数。这些特性将在以下部分介绍。

神经元

目的是模拟学习能力。哪种更好的解决方案可以从我们知道的关于智力的最佳例子中汲取灵感：大脑。很久以前，生物学家试图了解大脑的机制。简而言之，大脑由大量（约 10^{11} 个）基本细胞组成，称为神经元。这些细胞专门负责传播信息（电脉冲）。每个神经元平均有 7000 个与其他神经元的连接（称为突触连接）。这些连接是化学的。一个成年人的大脑平均有 10^{15} 个这样的连接。这个难以置信的网络将我们的感官映射到肌肉，并使我们能够思考、感受、记忆信息等。在下面的图中，你看到的是生物神经元的表示。来自其他神经元的电信息通过突触连接进入。根据此信号，末端按钮会激发另一个神经元。

下一步是模拟一个人工神经元。文献中存在多种神经元模型。但有一点是肯定的：模型越接近生物学现实，就越复杂。在本练习中，我们将使用文献中常用的一个模型：McCulloch&Pitts 神经元（见图）。本文档不解释每个术语的生物学意义，只解释人工神经元的功能。神经元通过多个加权输入和一个计算输出的函数来建模。输入（x 向量）表示突触连接，以浮点数形式表示。这些值的生物学意义是电峰的速率。它们来自另一个神经元或来自系统的输入。权重（w 向量）平衡输入。由于这些权重的存在，给定的输入对输出的影响或多或少。您可能在图中注意到第一个输入设置为 1。其对应的权重称为偏差 (b)。神经元的 y 输出被计算为输入加权和的函数 $y=\varphi (\sum _{i=0}^{m}w_{j}x_{j})$. $\varphi$ 函数可以是 sigmoid 函数 $\varphi (z)={\frac {1}{1+e^{-z}}}$.

单个神经元已经可以对输入进行分类。但它只能对线性可分输入进行分类。为了获得更好的结果，人工神经元像我们大脑的神经网络一样相互连接。结果被称为人工神经网络 (ANN)。

生物神经元的表示

McCulloch&Pitts 神经元

反向传播算法

为了训练 ANN（找到合适的权重）并知道输入和期望输出（监督学习），存在一种叫做反向传播的算法。要应用它，需要特定的 ANN 形状。ANN 必须由层构成，并且每一层的每个神经元都必须与前一层和下一层的所有神经元严格连接。该算法在算法 1 中作为参考写入。但它的解释超出了本练习的范围。然而，关于这个算法的信息可以在互联网上找到， 是一个很好的起点。

Backpropagation algorithm

1. Initialize the weights in the network (often randomly)
2. repeat * foreach example e in the training set do
  1. O = neural-net-output(network, e) ; forward pass
  2. T = teacher output for e
  3. Calculate error (T - O) at the output units
  4. Compute delta_wi for all weights from hidden layer to output layer ; backward pass
  5. Compute delta_wi for all weights from input layer to hidden layer ; backward pass continued
  6. Update the weights in the network * end
3. until all examples classified correctly or stopping criterion satisfied

提出的方法

如图所示，所提方法的第一步是提取相机图像中的样本，并对其进行预处理，以便直接发送到 ANN。其结果必须是一个大小固定的 0 到 1 之间的浮点值数组。因此，只保留蓝色通道，并对重采样图像进行重新采样以获得另一个分辨率。对图像进行采样的方法是最近邻插值。此方法可以快速实现，但得到的样本质量非常差。然而，对于本练习中使用的地标，此方法就足够了。

样本提取。首先，在图像中检测模式（地标），然后将找到的模式采样到固定大小，并将它们的蓝色分量在 0 到 1 之间归一化。

该方法的第二步是将采样图像的值作为 ANN 的输入发送（见图）。ANN 的输入层必须与采样图像像素的大小相同。ANN 的隐藏层可以具有任意大小。最后一层与地标数量相同。

ANN 的内部结构及其与输出和输入的连接。只有一个隐藏层。网络的输入直接是采样图像。因此，输入层的大小等于预处理图像的大小。输出层有 4 个神经元。此数字对应于地标的数量。

自己训练你的机器人并测试它

本练习的命令摘要

键盘按键	操作
1	将机器人移到模式 1 前面，并选择模式 1 进行学习
2	将机器人移到模式 2 前面，并选择模式 2 进行学习
3	将机器人移到模式 3 前面，并选择模式 3 进行学习
4	将机器人移到模式 4 前面，并选择模式 4 进行学习
向上箭头	选择下一个要学习的模式
向下箭头	选择上一个要学习的模式
L	启用（或禁用）学习模式
T	启用（或禁用）测试模式
O	加载存储在 weights.w 文件中的权重
S	保存存储在 weights.w 文件中的权重
B	停止 e-puck 的电机
R	e-puck 执行旋转
W	e-puck 随机行走
D	e-puck 跳舞

打开以下世界文件

.../worlds/advanced_pattern_recognition.wbt

该表列出了练习的命令。第一组命令使您能够选择要学习的模式。第二个命令使您能够学习选定的模式（对 ANN 进行一次反向传播迭代，以采样图像作为输入，以选定的模式作为输出）并测试采样图像（将其作为 ANN 的输入并观察其输出）。第三个命令使您能够保存和加载 ANN 的权重。最后，最后一组命令使您能够移动 e-puck。e-puck 的舞蹈特别有用，可以在训练阶段添加噪声。

要训练 e-puck，请选择要学习的模式，将 e-puck 放在模式前面，然后启用学习模式。

要测试 e-puck，将 e-puck 放在任何地方，然后启用测试模式。

[P.1] 按下“1”和“B”键，然后使用“L”键启用学习模式。在不移动 e-puck 的情况下，交替按下“1”到“4”键，以训练其他地标，直到这些地标的误差都小于 10% (<0.1)。您应该随机按下数字键，以使每个地标都能得到公平的演变，否则权重可能会偏向其中一个输出。然后按下“T”按钮以测试结果。

[Q.4] 当您使用“1”到“4”键将 e-puck 放置到位时，您的 e-puck 能否很好地识别地标？它在大型棋盘上随机移动时识别情况又如何？问题是什么？如何修正？

[P.2] 使用之前的结果重新尝试 e-puck 训练。（提示：还原模拟以删除 ANN 的权重。）

ANN 的参数

ann.h 文件定义了 ANN 的结构（隐藏层的大小、层数等）、采样图像的大小以及学习率。

[P.3] 学习率对 e-puck 学习有什么影响？如果学习率过高或过低会发生什么？

[P.4] 隐藏层的大小有什么影响？

[P.5] 添加至少一个其他的隐藏层。它对 e-puck 学习有什么影响？

[P.6] 改善当前的图像子采样方法。（提示：存在几种插值方法。在网上搜索：多元插值）

[P.7] 修改样本图像的分辨率。它对 e-puck 学习有什么影响？

[P.8] 将四个初始地标替换为其他一些地标（例如：你在网上找到的一些图标，你自己的创作，数字或 Rat's Life 地标）。修改 ANN、反向传播算法和图像插值的参数，以便以小于 3% 的误差识别它们。

ANN 的其他应用 [挑战]

[Q.5] 列出在不使用摄像机的情况下，ANN 和反向传播算法在 e-puck 上的一些可能的应用。

[P.9] 实现你列表中的一个项目。

更进一步

这项练习的灵感来自关于使用反向传播算法进行 OCR 的论文，更多信息可以在 中找到。

事实上，存在其他模式识别方法。所提出的方法的优点是受生物学启发，并且相当直观。该方法可以通过两种方式扩展：使用更逼真的神经元模型以及使用训练算法的一些扩展。另一方面，可以使用一些完全不同的方法，例如基于统计的方法。以下维基百科页面总结了监督学习的现有解决方案：监督学习

使用粒子群优化 (PSO) 的无监督学习 [高级]

在之前的练习中，我们看到了如何赋予机器人学习的能力。这是使用人工神经网络和反向传播算法完成的，这是一种监督学习方法。现在使用 PSO，我们将学习无监督学习的基础知识。我们希望机器人能够像避障模块 中的避障模块一样避开障碍物，但这次我们将让它自己学习。当我们没有训练集，即没有一组输入及其预期输出时，就会使用这种技术，即当不存在良好的模型时，以及当设计空间太大（无限）而无法系统地搜索时。工程师的作用是指定性能要求和问题编码，算法将完成其余工作。

关于 PSO 的一些见解

PSO 是一种相对年轻但很有前途的技术，它是由 Eberhart 和 Kennedy 在 1995 年的一篇论文中提出的。从那时起，该算法经历了许多演变，更多信息可以在 和 中找到。该算法背后的关键思想是使用粒子群来探索搜索空间。这些粒子中的每一个都代表一个候选解决方案，并以 n 维搜索空间中的位置和速度向量为特征。这两个向量都是随机初始化的，粒子在搜索空间中飞行。为了评估候选解决方案的性能，我们使用适应度函数。该适应度函数必须专门为给定的问题而设计，它必须对解决方案的性能进行排名。通常，我们设计一个返回 [0,1] 范围内的值的函数，其中 0 是最糟糕的结果，1 是完美的分数。每个粒子都会跟踪其个人最佳解决方案（称为 pbest）和全局最佳解决方案（称为 gbest）。然后，在每次迭代中，速度会朝着其 pbest 和 gbest 改变（加速）。流程图显示了主要的 PSO 进化循环。

由于我们在多维空间中工作，因此我们将粒子的位置表示为 $x_{i,j}$，速度表示为 $v_{i,j}$，其中 i 是粒子的索引，j 代表搜索空间中的维度。 $x_{i,j}^{*}$ 然后是粒子 i 达成的个人最佳解决方案，而 $x_{i',j}^{*}$ 是全局最佳解决方案。主要方程很容易理解，并以这种形式表达

$v_{i,j}=w\cdot (v_{i,j}+pw\cdot rand()\cdot (x_{i,j}^{*}-x_{i,j})+nw\cdot rand()\cdot (x_{i',j}^{*}-x_{i,j}))$

$x_{i,j}=x_{i,j}+v_{i,j}$

我们注意到 $rand()$ 函数，它生成一个介于 0 和 1 之间的均匀分布随机数。我们还注意到三个可以修改的参数，以调整算法的性能：$w$，$pw$ 和 $nw$.

PSO 使用的进化优化循环

实现

打开以下世界文件

.../worlds/advanced_particle_swarm_optimization.wbt

在本练习中，目标是使某些机器人能够自我进化。为此，我们使用 PSO 和人工神经网络（ANN，参见 使用反向传播算法的模式识别）。每个机器人将运行相同的控制器，但 ANN 的权重不同。PSO 将作用于权重以进化每个控制器，这意味着搜索空间是一个 N 维搜索空间，其中 N 是权重的数量。该图显示了具有一个隐藏层及其输入和输出的网络表示。

本练习中使用的神经网络

[Q.1] 我们如何指定机器人应避免障碍物作为适应度函数？

[Q.2] 策略“当我的传感器未被激活时保持静止”是否对您的功能有效？如何避免这种情况，即如何鼓励机器人快速移动？

[Q.3] 使用您的新功能，如果机器人以全速转向自身会发生什么？如果此策略产生了良好的结果，请重新设计您的功能以阻止这种行为。

[Q.4] 为什么我们将之前的速度作为 ANN 的输入之一？偏差也是一样。

如果您在之前的问答中没有设计适应度函数，则可以使用 中提出的函数，并在 中再次使用。它以这种方式呈现

$F=V\cdot (1-{\sqrt {\Delta v}})\cdot (1-i)$

$0\leq V\leq 1$

$0\leq \Delta v\leq 1$

$0\leq i\leq 1$

其中 $V$ 是两个车轮的平均绝对车轮速度，$\Delta v$ 是车轮速度在绝对值上的平均差，$i$ 是最活跃的红外传感器的平均值。

[Q.5] 为什么这个适应度函数适用于问题 1-3 中的每个点？

[P.1] 在机器人控制器中实现您的适应度函数或此函数。(提示：在代码中查找 TODO)

[P.2] 在监督控制器的控制器中，您需要设置运行次数及其持续时间，良好的值是在至少 30 秒的持续时间内运行 30 到 100 次。现在您可以第一次以快速模式运行模拟... 并喝杯咖啡，学习是一个漫长的过程。

[Q.6] 在模拟结束时，模拟进入 DEMO 模式，这意味着所有机器人将全局最佳结果作为其控制器。分析它们的行為，这是一个好的避障算法吗？一段时间后是否有任何机器人卡住了？我们如何改进解决方案的质量？

PSO 修改

我们提出了一种改进解决方案质量的方法，我们将修改主 PSO 循环以使其对噪声更具鲁棒性。在“评估新的粒子位置”步骤中，我们还将重新评估每个粒子的个人最佳位置。这有助于消除偶然的幸运运行，其中一个糟糕的解决方案偶然获得了良好的适应度。在代码中，这是通过一起评估所有个人最佳解决方案来完成的，并且每三次运行执行一次。

[P.3] 您可以在监督控制器的代码中执行此操作，将 NOISE_RESISTANT 定义为 1。现在构建并再次运行模拟。

[Q.7] 生成的解决方案更好吗？由于我们没有更改运行次数，因此我们通过启用抗噪声版本来划分进化运行次数。为什么这样做有意义？

在 pso.c 文件中，您可能已经注意到三个参数 w、nw 和 pw。这些是 PSO 进化函数中使用的参数。

[Q.8] 参数 w 的作用是什么？如果将其设置为大于 1 的值会发生什么？

更进一步

要获取有关 PSO 的更多信息，您可以查看此练习中提出的论文，，，，。

遗传算法 (GA) [高级]

介绍

遗传算法是由约翰·霍兰德在 70 年代发明的优化技术，其灵感来自达尔文的进化论。达尔文的理论基于三个基本原则，即变异、遗传和选择。在自然界中，这三个原则结合在一起形成了强大的优化机制，最大限度地提高了基因生存的机会，并解释了复杂生命形式的存在和适应。约翰·霍兰德的伟大想法是在计算机模拟中使用相同的达尔文原理来解决工程或数学问题。遗传算法是模拟世代中种群的人工进化的一种。种群由特定领域问题的候选解决方案组成。在每一代中，候选者的适应度根据一个或多个特定领域的目标进行评估。选择最佳候选者并允许它们繁殖；这是选择原则。繁殖根据两个亲本的遗传物质产生新的后代；这是遗传原则。新后代以一定概率发生变异，因此与它们的亲本不同；这是变异原则。可以在维基百科上找到基本信息和指针。

模拟

打开以下世界文件

.../worlds/advanced_genetic_algorithm.wbt

在这个示例中，您可以观察到一个 e-puck 机器人推着一块负载（小紫色圆柱体），同时避开一个障碍物（大棕色圆柱体）。模拟无限次重复相同的动作，直到用户按下 O 键。按下 O 键时，监督器开始 GA 优化。GA 适应度函数的定义是为了尽可能地将负载推离其起点（欧几里德距离）。遗传算法使用 50 个个体的种群，这些个体通过线性排名选择。从一代到下一代，由 10% 最适合个体组成的精英群体保持不变（克隆），这确保了最好的个体不会丢失。剩余的 90% 的种群通过有性繁殖产生，使用两点交叉和变异。每个基因的变异概率为 0.06，这大约代表每个基因组的一个变异。变异幅度取自平均值为 0、标准差为 0.2 的正态分布。GA 优化运行了 50 代，然后监督器将最佳个体的基因型保存在名为“fittest.txt”的文件中。当重新启动模拟时，此文件用于演示先前优化的最佳个体的行为。

基因型编码

此模拟使用两种不同的控制器；监督控制器运行 GA 优化，而机器人控制器执行机器人的行为。机器人的行为实现为直接的传感器到执行器矩阵，该矩阵定义每个车轮的速度受每个接近传感器影响的程度。e-puck 有 8 个接近传感器和 2 个车轮，因此传感器到执行器矩阵是一个 8*2 矩阵。基因直接映射到传感器到执行器矩阵，因此每个基因型都有 16 个基因。每个基因都编码为一个 double 精度浮点数。

问题

[Q.1] 代码如何指定应该尽可能地推动负载？

[Q.2] 机器人是否需要所有接近传感器来完成此任务？

[Q.3] 你如何确定对于这个问题，交叉操作比变异操作更重要/不重要？

[P.1] 尝试实现一个适应度函数，让机器人尽可能多地绕障碍物旋转，同时推动负载。

双侧对称

用于推动负载的机器人行为完全是左右对称的。因此，可以在每个基因型中只编码矩阵的一半（左或右）。然后，当基因型实例化为表现型时，可以从另一部分复制矩阵的缺失一半，同时尊重机器人的对称性。通过考虑双侧对称性，基因数量可以除以二，因此搜索空间将缩小两倍，GA 最终会更快地收敛到一个解。

[P.2] 修改监督器和机器人控制器代码以使用双侧对称性来进行基因型编码？

更进一步 *[挑战]*

如果你想更进一步，可以阅读关于进化机器人的论文，很好的参考是， 和。

[P.3] 尝试修改这个例子，使得负载可以由两个 e-puck 机器人而不是一个机器人同时推动。

SLAM [高级]

SLAM 是 Simultaneous Localization And Mapping（同步定位与地图构建）的首字母缩写。SLAM 问题是指移动机器人同时估计其环境地图及其相对于地图的位置。这是一个当前的研究课题，所以我们不会深入太多细节，但我们会尝试理解这个问题背后的关键思想，并为感兴趣的人提供参考。

地图构建，第一次尝试

为了说明这个问题的难度，我们将从一个简单的办法开始，并展示其局限性。为此，打开以下世界文件

.../worlds/advanced_slam.wbt

我们将使用一个简单的网格来模拟地图。机器人将从网格的中间开始，如果它看到障碍物，它将能够修改它。每个单元格中的网格被初始化为零，这意味着它最初是空的。机器人在找到障碍物的每个单元格中都会写入一个 1。当然，这将使用里程计来了解机器人的位置，我们将重复使用与里程计练习相同的模块。然后，我们将把它显示在屏幕上，以便我们能够检查它。

[P.1] 运行模拟并注意机器人的行为，它正在进行墙壁跟踪。现在让它完成这个简化竞技场的整个旋转，停止模拟，并查看屏幕上显示的地图。黑点是障碍物，白点是自由空间，红点是机器人当前位置。

[Q.1] 你能注意到这张地图的什么？特别注意轨迹的宽度，循环闭合的位置和角落。

[Q.2] 如果你让机器人完成竞技场的 5 圈，地图中会发生什么？在模拟中检查你的预测。为什么这个结果是一个问题？想象一下如果完成 10 圈甚至 100 圈会发生什么！

在图片中你可以看到四次这样的运行轨迹。顶行显示了如果你的里程计没有完美校准，循环没有闭合或者至少没有正确闭合，你可能会得到什么。在第三张图片中，你可以看到在良好校准的情况下返回的结果。它更好，但即使现在，它也不是一个完美的矩形，因为竞技场是。最后，第四张图片显示了使用我们发现的最佳校准进行的非常长运行的结果。由于里程计不可能是完美的，在运行过程中始终会存在一个小的误差增加。从长远来看，这将导致绘制路径的旋转，从非常长远来看（几分钟到一个小时），我们会得到由近似矩形的旋转产生的圆圈。

这个里程计误差是一个问题，但是如果你仔细想想，你肯定会想出一些关于这种映射技术的其他问题。在反思的开始，我们可以强调一个大矩阵在内存方面非常贪婪。存在多种方法来改进地图表示，从数据结构开始，你可能想搜索拓扑表示、线特征提取或几何约束 SLAM。

4 次映射控制器运行

机器人定位

本节基于 EPFL 移动机器人课程的实验室 D。现在我们已经看到，地图构建比看起来更难，我们将探索定位问题。机器人被给定一张地图，它必须找出自己在地图上的哪个位置。这是通过将红外传感器测量值与预先建立的地图进行比较以及使用里程计来完成的。机器人将不得不随着时间的推移整合传感器测量值，因为单个测量值通常不足以确定位置。

我们可以区分几个定位问题，所以我们将定义其中一些问题，并限制自己只解决一个问题。必须做的第一个区分是静态环境与动态环境，在我们的例子中，环境将是固定的，只有机器人会移动。然后，方法可以是局部或全局的，我们可以只进行位置跟踪，这意味着我们知道机器人位于哪个位置，只需要跟踪它即可，或者从头开始在整个地图上定位它。在本练习中，我们将做一个全局定位的例子。第三个对立是主动算法与被动算法，被动算法只处理传感器数据，主动算法将控制机器人以最小化位置不确定性，我们将使用被动算法。

我们方法的基本思想是将每个网格单元格分配一个概率（或置信度），并使用贝叶斯规则来更新这个概率分布。贝叶斯规则陈述如下

$p(x\,|\,y)={\frac {p(y\,|\,x)\cdot p(x)}{p(y)}}$

由于 $p(y)$ 不依赖于 $x$，我们将只把它看作一个归一化因子，我们可以把这个规则重新表述为

$p(x\,|\,y)=\eta \cdot p(y\,|\,x)\cdot p(x)$

• $p(x)$ 是先验概率分布，它提供了我们关于 $X$ 的信息，在加入数据 $y$ 之前。
• $p(x\,|\,y)$ 被称为后验概率分布，它是关于 $X$ 的信息，在知道数据 $y$ 之后。
• $p(y\,|\,x)$ 通常被称为似然函数或生成模型，因为它描述了状态变量 $X$ 如何导致传感器测量值 $Y$。

现在，我们将跳过马尔可夫假设，并在给出算法之前提供一些术语。

• $x_{t}$ 是机器人和环境在时刻 $t$ 的状态。在本例中，它将只包含机器人的位置，但它可以包含更多信息（机器人的速度，环境中移动物体的 位置等）。
• $u_{t}$ 是在状态 $x_{t}$ 之前给出的控制动作。它将是电机命令。
• $z_{t}$ 是状态 $x_{t}$ 中的测量数据。
• $m$ 是给机器人的地图，它没有索引，因为它不会变化。

所有这些量都可以表示为一个集合，$z_{t1:t2}=z_{t1},\,z_{t1+1},\,z_{t1+2},\,...\,,\,z_{t2}$。现在我们可以定义状态转移概率，它是给出状态演化的概率规律

$p(x_{t}\,|\,x_{t-1},\,u_{t})$

我们也可以定义测量概率，它是测量值 $z_{t}$ 的概率，已知我们处于状态 $x_{t}$。

$p(z_{t}\,|\,x_{t})$

最后，我们必须定义置信度，它是状态变量在可用数据上的后验概率。

$bel(x_{t})=p(x_{t}\,|\,z_{1:t},\,u_{1:t})$

如果我们在包含最新测量值之前计算它，我们称之为预测。

${\overline {bel}}(x_{t})=p(x_{t}\,|\,z_{1:t-1},\,u_{1:t})$

从 ${\overline {bel}}(x_{t})$ 计算 $bel(x_{t})$ 被称为测量更新。现在我们可以给出马尔可夫定位的通用算法（这只是将贝叶斯规则应用于移动机器人定位问题的改编）。

  Markov localization($bel(x_{t-1})$, $u_{t}$, $z_{t}$, $m$)
    for all $x_{t}$ do
      ${\overline {bel}}(x_{t})=\int p(x_{t}\,|\,u_{t},\,x_{t-1},\,m)\cdot bel(x_{t-1})\ dx_{t-1}$
      $bel(x_{t})=\eta \cdot p(z_{t}\,|\,x_{t},\,m)\cdot {\overline {bel}}(x_{t})$
    end for
  return $bel(x_{t})$

现在这个算法不能应用于我们的基于网格的地图表示，所以我们将以另一种形式给出它，作为离散贝叶斯滤波器。这将使我们能够实现该算法。

  Discrete Bayes filter($\{p_{k,t-1}\}$, $u_{t}$, $z_{t}$)
    for all $k$ do
      ${\overline {p}}_{k,t}=\sum _{t\ }p(X_{t}=x_{t}\,|\,u_{t},\,X_{t-1}=x_{i})\cdot p_{i,t-1}$
      $p_{k,t}=\eta \cdot p(z_{t}\,|\,X_{t}=x_{t})\cdot {\overline {p}}_{k,t}$
    end for
  return $\{p_{k,t}\}$

我们可以想象用直方图替换曲线，如果我们从一个维度考虑它，这就是发生的情况，我们离散化了概率空间。此算法已经实现，您可以使用与映射部分相同的 world 文件，但您需要更改 e-puck 的控制器。

[P.2] 在场景树中查找 e-puck 节点，并将字段控制器更改为 advanced_slam_2 而不是 advanced_slam_1，然后保存 world 文件。

在相机窗口中，我们将绘制机器人位置的置信度，用红色阴影表示，墙壁用黑色表示，最有可能的位置用青色表示（可能有多个最佳位置，尤其是在模拟开始时）。在开始时，概率分布在整个地图上是均匀的，并且随着时间的推移，它会根据红外传感器测量值和里程计信息而变化。

[P.3] 运行模拟并让机器人自行定位。为什么我们在地图上没有一个红色的点，而是一个更高值的区域呢？

[Q.3] 你能解释一下为什么我们在这个算法中只使用前一个网格的一小部分吗？

你可能已经注意到，当机器人定位好后，它只有一个可能的位置。既然矩形是对称的，这是怎么发生的？这是因为我们进一步简化了算法。我们只需要在二维空间 $(x,y)$ 中定位机器人，而不是三维空间 $(x,y,\theta )$。这意味着机器人知道它的初始方向，并消除了歧义。

[Q.4] 阅读代码，你理解如何更新置信度吗？

[Q.5] 为什么我们要在传感器模型和里程计测量值上应用模糊？如果我们不这样做会发生什么？

[P.4] 尝试在机器人自行定位之前移动它（只执行一步模拟），机器人当然应该能够自行定位！

我们已经介绍了一种执行机器人定位的算法，但还有很多其他算法！如果你有兴趣，你可以看看蒙特卡罗定位，它也是基于贝叶斯规则，但用一组粒子而不是直方图来表示概率分布。扩展卡尔曼滤波 (EKF) 定位也是一种有趣的算法，但它稍微复杂一些，如果你想更深入地了解，你可以看看 Rao-Blackwellized 滤波器。

SLAM 中的关键挑战

正如你在前面的两个例子中所看到的，为了解决 SLAM 问题，需要克服一些挑战。我们将在这里列出其中一些，但可能还会出现很多其他挑战。我们考虑到 e-puck 在固定环境中运行，环境变化与我们无关。

• 我们遇到的第一个问题是传感器有噪声。我们从关于 里程计 的练习中知道，里程计的精度会随着时间的推移而下降。这意味着机器人位置的不确定性会增加。我们必须在算法的某个阶段找到降低此误差的方法。但里程计并非唯一产生噪声的传感器，红外传感器也必须考虑在内。然后，如果我们使用相机基于视觉来执行 SLAM，我们将不得不处理相机的噪声。
• 解决之前问题的一种方法会引出另一个问题，即对应问题，这可能是最难的问题。该问题在于确定在不同时间点获得的两个测量值是否对应于物理世界中的同一物体。当机器人完成一个循环并再次回到空间中的同一位置时，就会发生这种情况，这也称为“闭环”。如果我们能够依靠两个测量值是同一物理物体的这一事实，我们就能显着减少机器人位置的不确定性。此问题包括定位过程，它本身就是一个挑战。
• 另一个可能会出现的问题是地图的高维性。对于二维网格地图，我们已经使用 200×200 整数，想象一下，如果我们要对整个建筑物的三维地图做同样的事情。这就是地图表示问题。由于我们还需要探索一个可能无限的空间，因此内存和计算能力也可能成为问题。
• 最后一个挑战是探索地图，因为机器人不仅要构建地图并定位自身，而且还应尽可能完整地探索其环境。这意味着以最短的时间探索最大的表面积。此问题与导航和路径规划问题有关；通常称为自主探索。

现在，您对我们在能够进行 SLAM 之前必须面对的挑战有了更清晰的认识！

但 SLAM 并不局限于红外传感器，可以使用相机来执行基于视觉的 SLAM。我们可以将路标放置在迷宫的墙壁上，并重用练习 [sec:backprop] 中的人工神经网络来识别它们并改进我们的算法。如果有人想参加“老鼠人生”比赛，就可以做到这一点。许多研究人员还使用激光测距仪来获得单个位置的精确数据扫描。我们还可以在 3D 环境中找到飞行或水下车辆的 SLAM 工作。SLAM 的当前研究主题包括限制计算复杂度，改进数据关联（在观察到的路标和地图之间）以及环境表示。

更进一步 *[挑战]*

如果您想进一步了解，可以阅读有关 SLAM 的论文，好的参考资料是 和。 展示了基于 EKF 的 SLAM 问题的解决方案。 展示了使用稀疏传感的 SLAM，您也可以查看 ，它展示了一种称为 FastSLAM 的高级算法。其他资源可以在互联网上找到，http://www.openslam.org/ 为 SLAM 研究人员提供了一个平台，使他们有机会发布他们的算法。

[P.5] 在“老鼠人生”框架中实现完整的 SLAM 算法。