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 演算法。