HuskyLens攝像頭系列 | 寫給小學生看的視覺PID巡線算法

Hello，大家好，光天化日之下我又來擼狗了。距離上次擼狗已經過去了個把月時間了，那麼這次又有什麼新驚喜呢？

先來看一下本期的演示視頻吧。

https://www.bilibili.com/video/av79068760/

什麼？巡線？這也太簡單了吧。不要急，聽我慢慢道來。

哈士奇圖形模塊來了

上次視頻裏說了，DF 只給我提供了 HuskyLens （中文名：哈士奇）人工智能攝像頭的通信協議，所以只能靠擼代碼來體驗哈士奇。你還不知道哈士奇是什麼？更應該看一看上期的教程了。

上期回顧：擼狗初體驗 | 手把手教你上手 HuskyLens 哈士奇人工智能攝像頭

但是這次，我拿到了哈士奇的新狗糧！那就是哈士奇的 Arduino 庫和 Mind+ 圖形模塊（Mind+ v1.6.2 測試版）。也就是說，我們現在只要輕鬆拖動圖形模塊，就可以很輕鬆地玩轉哈士奇了，而且哈士奇的界面，終於支持中文了！

注意：哈士奇的圖形模塊和 Arduino 代碼庫目前處於工程測試階段，正式版很有可能會發生調整和改動，請對照本文描述的原理酌情修改程序使用。

巡線介紹

既然有了這些功能，那麼玩哈士奇就簡單多了。所以這一期我將帶大家來玩一玩哈士奇的視覺巡線功能！

爲了配合完成巡線功能，我用激光切割給麥昆小車做了一個可調節支架，將哈士奇安裝到了麥昆小車上，如下圖所示。其中哈士奇的角度是可以旋轉調節的。

智能車巡線，相信大家都已經很熟悉了，就是讓小車沿着規定的軌道（通常是黑線，下圖視覺效果圖用了藍線）、按照一定的速度進行移動。一般情況下，我們只需要使用普通的巡線傳感器，就可以完成相應的功能。既然麥昆裝上了哈士奇，我們這次就不用麥昆自帶的巡線傳感器來進行巡線了，我們來試試哈士奇視覺巡線有什麼奇特的功效。

視覺巡線跟普通巡線傳感器巡線有什麼區別呢？採用普通巡線傳感器時，一般根據線寬、巡線傳感器的間距、巡線傳感器的數量等，會有好幾種巡線模式，我們這裏以麥昆小車自帶的雙巡線傳感器爲例，這也是最常見的巡線模式。如下圖所示，我們可以很容易就可以根據巡線傳感器在黑線（視覺效果圖用了藍線）和白線上的位置來判斷麥昆小車什麼時候需要直行、什麼時候需要左轉、什麼時候需要右轉。

那麼哈士奇視覺巡線的邏輯又是怎麼樣的呢？其實跟巡線傳感器的邏輯非常類似，我們只需要知道小車相對黑線是什麼位置就行了：

• 當麥昆小車在黑線的偏左位置時，控制小車向右轉彎；
• 當麥昆小車在黑線的偏右位置時，控制小車向左轉彎；
• 當麥昆小車與黑線相對居中時，控制小車直行。

在本文中，我們希望麥昆小車以儘可能快的恆定速度進行巡線，即麥昆小車的幾何中心速度保持不變。

開始巡線吧

我們先來看一下，麥昆小車帶着哈士奇巡線時，哈士奇屏幕上顯示的信息。需要注意的是，爲了巡線，我們需要將攝像頭斜向下調節，這樣能夠看到離麥昆小車更近距離的黑線，巡線效果更佳。

爲了方便說明問題，我們將上圖中哈士奇屏幕上的信息剝離出來，抽象成下圖的幾何數學模型。

我們知道哈士奇屏幕的分辨率是 320×240，屏幕左上角的頂點爲屏幕的座標原點（0, 0），水平向右方向爲 X 軸正方向，豎直向下方向爲 Y 軸正方向，因此屏幕右下角的座標爲（320, 240）。上圖中紅色虛線爲屏幕的中軸線，這條線的橫座標 x = 160。上圖中黑色的線，爲哈士奇攝像頭“看到”的巡線地圖，藍色箭頭爲哈士奇計算出來的線條方向。藍色箭頭的起點座標爲**（x1, y1），終點座標爲（x2, y2）**。

巡線算法1

經過這樣的數學抽象之後，其實巡線邏輯就很簡單了。當哈士奇檢測到黑線在屏幕的左邊時，也就是藍色箭頭的座標 x1 < 160 時，控制小車左轉；當黑線在屏幕的右邊時，此時 x1 > 160 時，控制小車右轉；當黑線在屏幕的中間時，此時 x1 = 160 時，控制小車直行。

邏輯是不是很簡單？我們馬上來編寫個簡單的巡線程序試試吧。

首先我們可以在 Mind+ 軟件或者 Arduino 軟件中，通過一個簡單的圖形模塊或者一行簡單的代碼，就可以獲取（x1, y1）、（x2, y2）的座標。

完整的巡線程序如下圖所示。

在上面的程序中，我們定義了一個麥昆運動控制函數，可以分別接收左、右輪的速度參數，然後控制麥昆的運動（下同，不再贅述）。在主程序或者巡線算法中，我們只要調用這個函數並改變速度就可以了。

巡線效果如下面的動圖所示，如果覺得 GIF 動圖有點卡頓，可以看文末的對比視頻。

這時的巡線的平均速度是 60 左右，走完上面地圖 1 圈需要 62 秒。試試改變巡線速度，看看你能以多快的速度巡線。我在編程時嘗試了更大的速度，發現巡線效果不是很理想。

在改變速度的過程中，發現什麼問題了麼？

1. 小車在前進中明顯左右晃動，速度變化不連貫，不能穩定直行運動；
2. 速度不能設置太快，速度太快很容易脫線；
3. 不同的轉彎角度，需要的轉彎速度也不一樣，當巡線地圖中有好幾種轉彎角度時很容易脫線。

我們先來看第 1 個問題：小車在前進中明顯左右晃動，不能穩定直行運動。這是爲什麼呢？因爲小車直行的位置區間太小了，只有當 x1 = 160 時，才能直行。由於小車在運動過程中帶有慣性，所以這是很難達到的條件。

巡線算法2

那應該怎麼辦呢？很簡單，增大直行的位置區間就好了。如下圖所示，我們將 150 ≤ x1 ≤ 170 的區間設置爲小車直行區間，當檢測到黑線的起點座標 x1 在這個區間內時，控制小車直行；當 x1 < 150 時，控制小車左轉；當 x1 > 170 時，控制小車右轉。

這樣一來，是不是直行區間變大了？我們改變一下程序試試看效果吧。

巡線效果如下面的動圖所示，如果覺得 GIF 動圖有點卡頓，可以看文末的對比視頻。

通過這些調整，我們發現，麥昆小車的巡線速度快了一些，這時的巡線的平均速度是 70 左右，走完上面地圖 1 圈需要 55 秒，而且在直行路徑上，速度變化也順滑了一些，但是仍然會左右輕微晃動。

可是這樣還是不能改善第 3 個問題，同一種轉彎速度並不能適應所有的轉彎角度，一旦設置的速度太快了，在轉彎角度變化比較大的巡線地圖中，還是很容易脫線。

巡線算法3

我們繼續來調節，既然可以將檢測到的黑線位置分成 3 個區間，我們是不是能如法炮製，將區間繼續分爲 5 個呢？黑線位置越靠兩邊，轉彎速度越大；黑線位置越趨近於中間，越趨向於直行運動。

或者更進一步，直接分爲 7 個調速區間呢？

我們來改編程序試試效果吧。

巡線效果如下面的動圖所示，如果覺得動圖有點卡頓，可以看文末的對比視頻。

這時，麥昆小車的巡線速度可以更快了，此時的巡線的平均速度是 80 左右，走完上面地圖 1 圈需要 51 秒，而且不管是轉彎還是直行，速度變化都順滑很多了。

PID來了

經過上面的調速過程，你發現什麼？

既然麥昆巡線時的運動狀態或巡線區間可以分爲 2 檔、3 檔、5 檔、7 檔，那是不是還能繼續細分呢？9 檔？11 檔？……直到無限細分。調速區間越細分，巡線效果越好。但是這樣寫程序會越來越長，有什麼辦法呢？這裏就要引出 PID 調速算法了。

PID 算法，是控制理論中最常用、最經典的算法，在我們的生產和生活中應用的非常廣泛。它並不是什麼很神聖的東西，大家一定都見過 PID 的實際應用：比如四軸飛行器，再比如平衡小車，還有汽車的定速巡航、3D 打印機上的溫度控制器、恆溫熱水器等。就是類似於這種：需要將某一個物理量“保持穩定”的場合（比如維持平衡，穩定溫度、轉速等），PID 都會派上大用場。

它的公式爲：

或者我們在編程時更常用的是數字式的 PID 公式：

PID 控制系統是一種閉環控制系統，閉環控制是根據控制對象輸出的反饋來進行校正的控制方式。它能夠根據測量出的實際值與計劃值之間的誤差 Error，按一定的標準來進行糾正。

根據公式，可以看到 PID 分爲 3 個部分：比例調節（P）、積分調節（I）、微分調節（D）。那麼這三者之間是什麼關係呢？每個部分的調節對巡線會產生什麼樣的影響呢？我們一個一個來看。

注意：如標題所說，本文是寫給小學生看的 PID 調速算法，所以下文中描述的是已經簡化的 PID 算法，默認沒有去計算每個程序的循環時間，也就是沒有去計算積分和微分時間。主要有幾個原因：一、是因爲程序功能比較單一，每個循環時間差不多，在調節 Kd、Ki 參數時，將時間參數 T 當成一個整體進行調試了；二、省略了時間參數後，程序編寫會更加簡單，容易看懂與理解。

P：調節當前的誤差

以視覺巡線爲例，測量出來的實際值爲黑線的起點座標 x1，但是我們希望黑線的起點座標能夠在屏幕中央，也就是位於 x = 160 的位置。這時，誤差 Error = x1 - offset = x1 - 160。那麼接下來，我們就可以根據這個誤差大小來調節轉速的大小了，也就是我們上面分析的，黑線越是遠離屏幕中心，誤差越大，這時需要的轉速也越大。根據 PID 公式，轉速 V_turn 的計算公式爲：

V_turn = Vp_turn = Kp × Error = Kp × (x1 - 160)

我們知道，物體的運動一般可以由直線運動和轉彎運動組成，所以速度也可以由直線運動速度 V_straught 與轉彎速度 V_turn 組成。爲了使麥昆小車保持一定的運動速度，我們可以分別給左右輪加上或減去一個轉彎速度 V_turn，這樣麥昆小車中心的速度就能夠保持不變。

V_left = V_straught + V_turn
V_right = V_straught - V_turn

根據這個公式，我們可以簡單的理解爲，P 控制器的作用是根據誤差的大小產生一個轉彎速度 V_turn，用於疊加在左右輪的速度上。

那麼 Kp 的值取多少合適呢？這就需要我們根據實際巡線情況，手動進行調節與設置了。多嘗試幾次，應該就能設置出一個比較合適的 Kp 值了。

編寫程序並調節 Kp 值如下。程序中的的offset變量就是屏幕中軸線的座標，其他參數前面都有說明了，這裏不再解釋。

巡線效果如下面的動圖所示，如果覺得 GIF 動圖有點卡頓，可以看文末的對比視頻。

此時的巡線的平均速度是 120 左右，走完上面地圖 1 圈需要 39 秒。不僅在速度變快的情況下還能正常巡線，而且速度的變化明顯順滑多了。

到這裏，你已經比大部分巡線程序實現的效果好多了。還能怎麼繼續完善呢？

D：預測未來的誤差

上述程序增加了 P 比例調節之後，我們發現巡線效果好多了，速度的變化非常平滑，但是仍然會有晃動情況。這是因爲慣性，速度越大，慣性越大，當檢測到的黑線越靠近屏幕邊緣時，V_turn 就越大，導致慣性就越大。能不能想辦法改善這個問題呢？

這個時候就是 D 微分調節要上場了。微分調節的作用如下圖，它可以理解爲一個彈簧阻力，當慣性越大的時候，阻力或拉力也就越大，直到將慣性方向的速度變爲 0，也就是下圖中彈簧下面的方塊，始終會被限制在一個小範圍內運動，不會產生太大範圍的抖動變化。加上 D 微分調節之後，我們就可以有效的抑制晃動。但是也要去適當的調節 D 微分調節的參數，不然限制的阻力太大，麥昆小車轉彎速度 V_turn 被限制的太小也不行，這會導致轉不過彎來。

可以說，D 的功能，就是根據之前的變化趨勢，去防範於未來可能發生的失控問題。之前的變化趨勢越大（慣性越大），接下來阻力的作用越明顯。根據 PID 公式，D 調節也要計算一個 Vd_turn 的分量：

Vd_turn = Kd × derivative = Kd × (Error - lastError)

其中，Kd 是需要我們手動調節的參數，Error 爲當前測量的誤差，lastError 爲上次測量的誤差。

總的 V_turn 計算公式爲：

V_turn = Vp_turn + Vd_turn = Kp × Error + Kd × derivative

編寫 PD 調速程序，並調節 Kd 參數至合適的數值，程序如下：

巡線效果如下面的動圖所示，如果覺得 GIF 動圖有點卡頓，可以看文末的對比視頻。

此時的巡線的平均速度是 120 左右，走完上面地圖 1 圈需要 38 秒。巡線過程中的晃動已經明顯減弱了很多。

I：糾正過去的誤差

雖然在本文的效果中不是很明顯，但是單單 P 比例調節，可能會出現一個問題：調節力度不夠，永遠存在一個穩定的誤差。因爲實際情況中可能會存在各種各種的阻力或者影響輸出的情況，具體例子可以看文末的參考文章：我對『PID算法』的理解 —— 原理介紹

在這種情況下，我們就需要引入 I 積分調節了。積分調節可以產生一個新的轉速 Vi_turn。它的計算公式爲：

Vi_turn = Ki × integral

積分調節的作用就是可以消除那個相對穩定的誤差。它能將過去誤差累計起來，只要存在誤差，這個累計值 integral 就會越來越大，就會使調節力度越來愈大，逐漸就能抵消阻力產生的穩定誤差的影響。

總的 V_turn 的計算公式如下：

V_turn = Vp_turn + Vi_turn + Vd_turn

即：V_turn = Kp × Error + Ki × integral + Kd × derivative

編寫 PID 調速程序，並調節 Ki 參數至合適的數值。由於本示例中，幾乎沒有穩定誤差的影響，所以 Ki 的值很小。程序如下：

巡線效果如下面的動圖所示，如果覺得 GIF 動圖有點卡頓，可以看下面的對比視頻。

此時的巡線的平均速度是 120 左右，走完上面地圖 1 圈需要 38 秒。跟 PD 算法巡線效果差不多。

總結

最後放一個視頻，做一下文中幾個算法整體的效果對比：

https://www.bilibili.com/video/av79068862/

當然本文講的是最基本的 PID 算法，這個算法在實際應用中還有很多可以進一步完善的地方。基於 PID 控制理論設計的新算法也很多，但是最基本的原理都是類似的。

最後用一張動圖來總結一下本文講的 PID 算法。從下圖中，我們可以直觀理解 PID 中三個參數 Kp、Ki、Kd 的作用。圖中紅色虛線爲目標位置，藍色實線爲系統輸出。

• 當調節 Kp 參數時，可以讓系統輸出越來越接近目標位置，但是存在抖動、以及存在穩定誤差調節力度不夠的問題；
• 調節 Ki 參數，可以彌補 Kp 參數調節力度不夠的問題，但是也會增加抖動；
• 調節 Kd 參數，會讓抖動減小，變化曲線更加平滑。
  

補充思考

在本文中，我們利用哈士奇攝像頭得到了黑線位置，這裏我們只用了黑線的其中一個位置參數：起點座標 x1。但是另外三個參數都沒有用上（起點座標 y1，終點座標 x2、y2）。

請你思考，是否將另外幾個參數利用起來效果會更好呢？我們在巡線過程中，發現哈士奇屏幕中的黑線經常是斜着的，是否讓黑線整體保持豎直，也就是讓黑線與哈士奇屏幕的中軸線重合效果會更好呢？不僅去調節黑線與屏幕中間位置的偏差，還要去調節黑線的方向。

趕緊用本文講的 PID 算法試試吧，你能挑戰更快的速度麼？

代碼下載

代碼與哈士奇激光切割源文件下載鏈接：https://t.zsxq.com/qZVzrbQ

參考文章

• 我對『PID算法』的理解 —— 原理介紹
• 用於樂高機器人的PID控制器