基于RTOS的機器人巡線控制算法

明子成+++印元+葛六波

摘要：機器人循線移動是當前機器人導航的一種常用方法。關于巡線算法的研究有很多，但多是研究算法本身，有不少算法設計精巧，實際應用中運行快速流暢，但可靠性并未有明顯改善。而我們設計的機器人，巡線采用離散PID算法，實驗測試和機器人競賽結果表明，該算法具有響應速度快、巡線可靠、動作快速流暢和偏離航線率低等優點。

關鍵詞：機器人；離散PID RTOS；傳感器；巡線算法

● 概述

自主巡線機器人可廣泛應用于物流、超市等的倉庫、工廠自動料車、機場等的貨物無人轉移技術領域，但要達到實用化，首先需要解決機器人可靠導航的問題。對于在一定區域內實現機器人導航，巡線是目前較理想的選擇，因此成為各學段學生機器人比賽活動中幾乎必有的項目。縱觀近年來各級各類機器人競賽活動中巡線機器人的表現，最薄弱的環節仍然是在基本的巡線過程中，機器人會不經意間脫離引導線而不得不中止比賽。PID算法在巡線中已經得到一定程度的使用，盡管改善了巡線的質量，但由于控制程序運行過程中的時序同步問題，機器人在移動過程中還是會隨機出現脫離引導線的問題。即使采用多個傳感器信息進行糾偏，但問題仍會出現，即機器人在巡線過程中，某個糾偏傳感器明明“看”到引導線了，卻未按設計要求轉去執行糾偏程序。“視”而不見，表明在傳感器處于引導線區域內時，CPU正在處理其他事務。在機器人MCU等硬件之上植入嵌入式實時操作系統，傳感器的數據采集、環境監測、信息處理與輸出控制都可作為獨立的任務進行管理。讓監視與控制程序同時進行，避免了在等待過程中造成的信息“無視”，控制器實現了對監測到的異常進行實時處理，并即時進行輸出控制，化解了單任務程序設計中的實時響應難題。

● 總體設計

μC/OS-III中，任務可以是基于優先級的搶占式調度，但也支持同等優先級的輪轉調度。機器人的結構設計如圖1所示，安裝了7個灰度傳感器，其中S4用于巡線數據的采集，其他傳感器用于偏離航線的檢測。在執行任務的過程中，機器人始終不允許脫線運行，這就意味著脫線檢測和處理必須優先于其他事件。因此，我們將傳感器S1和S7的信息采集與處理作為任務1（如下頁圖2所示）；在確定S1和S7沒有檢測到標志線的情況下，需要考慮機器人是否處于較大的偏轉狀態，根據傳感器S2和S6采集的信息進行處理的任務2，可以優先執行；而檢測較小偏轉的傳感器S3和S5是為了保證機器人盡量以較小的偏轉沿著引導線平滑移動，設置為任務3；傳感器S4用于巡線移動信息采集，故在這四個應用任務中設置為最低優先級，為任務4。兩個任務之間的數據通信考慮到巡線過程中對傳感器信息的即時性要求，可通過全局變量實現。

圖1

● 關鍵設計

1.離散PID算法實現巡線設計

機器人巡線實際上僅使用一個灰度傳感器S4，即圖1中的3號傳感器。對采集的地面數據運用離散PID算法，通過設定合適的比例、積分和微分參數計算出驅動馬達的速度控制值，以確保其能快速平穩前進。該算法需要從傳感器檢測的地面和引導線不同點的值中找出最大值（Vmax）和最小值（Vmin），并計算出最大值與最小值的平均值，作為判斷在線與否的閾值（Vmid）。計算出最大值與最小值的差值（VΔ），作為比例參量（Kp）計算的依據之一。根據這些數據，分別計算出比例P、積分I和微分D的調節值。

運用比例可以計算出機器人對當前狀況需要作出的反應。比例調節值的算式：

（1）

積分是對已經發生的偏差(V_err)進行累積，以達到快速調節的目的。積分調節值的算式為:

（2）

微分用于對機器人移動方向的趨勢進行預測和加強，比較前一次的偏差與當前的偏差，得到機器人移動的趨勢，用微分調節值來強化該趨勢，計算式為：

（3）

馬達速度調節值：

（4）

馬達的速度控制值：

（5）

（6）

馬達的移動控制函數：

Move（Pwr_Left，Pwr_Right）； （7）

通過實時的地面環境感知和計算結果，即時調整機器人左右馬達的運行速度，從而達到控制機器人沿著引導線移動的目的。同時，設計者需要根據機器人實際運行狀況進行參數Kp、Ki和Kd的值的設定。具體整定方法是：先單獨采用比例算法整定Kp，讓機器人巡線，尋找到合適的Kp和Pwr_Base。需要注意的是，Pwr_Base的值建議與機器人的驅動電池電量形成關聯，可以使系統運行的穩定性更高且適應性更廣。

2.解決時序問題

盡管以上設計在邏輯上似乎解決了巡線和保障問題，但在實際調試時，還是難以處理好巡線的問題。究其原因，在于盡管機器人可以隨時讀取傳感器值，但CPU如果在執行其他指令，而檢測異常的傳感器盡管檢測到了引導線，CPU也會熟視無睹，而當指令執行到讀取該傳感器值時，已經物是人非了，這就會讓人無所適從，明明程序邏輯上沒有錯誤，為什么還是“不聽話”呢？要讓機器人對傳感器信息“隨時”保持敏感，可以引入多任務管理。讓傳感器的采集數據在盡量短的時間內（3ms或更低，具體需視機器人的運行速度、傳感器的采集周期等而定）轉化成驅動馬達的控制指令。據此，將傳感器采集和數據處理作為一個個獨立的任務，根據任務的緊迫性可分為：脫線、較大偏離、較小偏離和巡線四個由高到低不同優先級的任務。

圖2

● 實驗測試與結果分析

機器人自主控制程序的設計所面臨的一個難題是運行環境信息復雜和機器人可感知信息的客觀限制，任何理論上似乎完美的算法，在實際運行時都很難保證100%的成功率，設計中刻意繞開和避免的問題往往還是會發生。基于此，本設計中不是回避問題，而是采用多傳感器信息融合算法，一旦機器人遇到問題就能夠通過傳感器檢測確認，并即時采取糾偏措施。在多次大型機器人競賽巡線相關項目比賽中，筆者均采用了此算法。實踐證明，機器人運行的穩定性、快速性和問題的自糾偏表現均有明顯改善。

參考文獻：

[1]J.J.Labrosse，μC/OS-III：the Real—Time Kernel for the STMicroelectronics STM32[M/OL].2013.http://www.micrium.com/books/ucosiii.

[2]明子成.家庭護理機器人的設計[J].機器人技術與應用，2010(09).

[3]明子成.游覽機器人的設計與實現[J].機器人技術與應用，2008(10).

基金項目：南京曉莊學院2013年度科研項目《基于RTOS的機器人控制器開發》，項目編號2013NXY44。

endprint

摘要：機器人循線移動是當前機器人導航的一種常用方法。關于巡線算法的研究有很多，但多是研究算法本身，有不少算法設計精巧，實際應用中運行快速流暢，但可靠性并未有明顯改善。而我們設計的機器人，巡線采用離散PID算法，實驗測試和機器人競賽結果表明，該算法具有響應速度快、巡線可靠、動作快速流暢和偏離航線率低等優點。

關鍵詞：機器人；離散PID RTOS；傳感器；巡線算法

● 概述

自主巡線機器人可廣泛應用于物流、超市等的倉庫、工廠自動料車、機場等的貨物無人轉移技術領域，但要達到實用化，首先需要解決機器人可靠導航的問題。對于在一定區域內實現機器人導航，巡線是目前較理想的選擇，因此成為各學段學生機器人比賽活動中幾乎必有的項目。縱觀近年來各級各類機器人競賽活動中巡線機器人的表現，最薄弱的環節仍然是在基本的巡線過程中，機器人會不經意間脫離引導線而不得不中止比賽。PID算法在巡線中已經得到一定程度的使用，盡管改善了巡線的質量，但由于控制程序運行過程中的時序同步問題，機器人在移動過程中還是會隨機出現脫離引導線的問題。即使采用多個傳感器信息進行糾偏，但問題仍會出現，即機器人在巡線過程中，某個糾偏傳感器明明“看”到引導線了，卻未按設計要求轉去執行糾偏程序。“視”而不見，表明在傳感器處于引導線區域內時，CPU正在處理其他事務。在機器人MCU等硬件之上植入嵌入式實時操作系統，傳感器的數據采集、環境監測、信息處理與輸出控制都可作為獨立的任務進行管理。讓監視與控制程序同時進行，避免了在等待過程中造成的信息“無視”，控制器實現了對監測到的異常進行實時處理，并即時進行輸出控制，化解了單任務程序設計中的實時響應難題。

● 總體設計

μC/OS-III中，任務可以是基于優先級的搶占式調度，但也支持同等優先級的輪轉調度。機器人的結構設計如圖1所示，安裝了7個灰度傳感器，其中S4用于巡線數據的采集，其他傳感器用于偏離航線的檢測。在執行任務的過程中，機器人始終不允許脫線運行，這就意味著脫線檢測和處理必須優先于其他事件。因此，我們將傳感器S1和S7的信息采集與處理作為任務1（如下頁圖2所示）；在確定S1和S7沒有檢測到標志線的情況下，需要考慮機器人是否處于較大的偏轉狀態，根據傳感器S2和S6采集的信息進行處理的任務2，可以優先執行；而檢測較小偏轉的傳感器S3和S5是為了保證機器人盡量以較小的偏轉沿著引導線平滑移動，設置為任務3；傳感器S4用于巡線移動信息采集，故在這四個應用任務中設置為最低優先級，為任務4。兩個任務之間的數據通信考慮到巡線過程中對傳感器信息的即時性要求，可通過全局變量實現。

圖1

● 關鍵設計

1.離散PID算法實現巡線設計

機器人巡線實際上僅使用一個灰度傳感器S4，即圖1中的3號傳感器。對采集的地面數據運用離散PID算法，通過設定合適的比例、積分和微分參數計算出驅動馬達的速度控制值，以確保其能快速平穩前進。該算法需要從傳感器檢測的地面和引導線不同點的值中找出最大值（Vmax）和最小值（Vmin），并計算出最大值與最小值的平均值，作為判斷在線與否的閾值（Vmid）。計算出最大值與最小值的差值（VΔ），作為比例參量（Kp）計算的依據之一。根據這些數據，分別計算出比例P、積分I和微分D的調節值。

運用比例可以計算出機器人對當前狀況需要作出的反應。比例調節值的算式：

（1）

積分是對已經發生的偏差(V_err)進行累積，以達到快速調節的目的。積分調節值的算式為:

（2）

微分用于對機器人移動方向的趨勢進行預測和加強，比較前一次的偏差與當前的偏差，得到機器人移動的趨勢，用微分調節值來強化該趨勢，計算式為：

（3）

馬達速度調節值：

（4）

馬達的速度控制值：

（5）

（6）

馬達的移動控制函數：

Move（Pwr_Left，Pwr_Right）； （7）

通過實時的地面環境感知和計算結果，即時調整機器人左右馬達的運行速度，從而達到控制機器人沿著引導線移動的目的。同時，設計者需要根據機器人實際運行狀況進行參數Kp、Ki和Kd的值的設定。具體整定方法是：先單獨采用比例算法整定Kp，讓機器人巡線，尋找到合適的Kp和Pwr_Base。需要注意的是，Pwr_Base的值建議與機器人的驅動電池電量形成關聯，可以使系統運行的穩定性更高且適應性更廣。

2.解決時序問題

盡管以上設計在邏輯上似乎解決了巡線和保障問題，但在實際調試時，還是難以處理好巡線的問題。究其原因，在于盡管機器人可以隨時讀取傳感器值，但CPU如果在執行其他指令，而檢測異常的傳感器盡管檢測到了引導線，CPU也會熟視無睹，而當指令執行到讀取該傳感器值時，已經物是人非了，這就會讓人無所適從，明明程序邏輯上沒有錯誤，為什么還是“不聽話”呢？要讓機器人對傳感器信息“隨時”保持敏感，可以引入多任務管理。讓傳感器的采集數據在盡量短的時間內（3ms或更低，具體需視機器人的運行速度、傳感器的采集周期等而定）轉化成驅動馬達的控制指令。據此，將傳感器采集和數據處理作為一個個獨立的任務，根據任務的緊迫性可分為：脫線、較大偏離、較小偏離和巡線四個由高到低不同優先級的任務。

圖2

● 實驗測試與結果分析

機器人自主控制程序的設計所面臨的一個難題是運行環境信息復雜和機器人可感知信息的客觀限制，任何理論上似乎完美的算法，在實際運行時都很難保證100%的成功率，設計中刻意繞開和避免的問題往往還是會發生。基于此，本設計中不是回避問題，而是采用多傳感器信息融合算法，一旦機器人遇到問題就能夠通過傳感器檢測確認，并即時采取糾偏措施。在多次大型機器人競賽巡線相關項目比賽中，筆者均采用了此算法。實踐證明，機器人運行的穩定性、快速性和問題的自糾偏表現均有明顯改善。

參考文獻：

[1]J.J.Labrosse，μC/OS-III：the Real—Time Kernel for the STMicroelectronics STM32[M/OL].2013.http://www.micrium.com/books/ucosiii.

[2]明子成.家庭護理機器人的設計[J].機器人技術與應用，2010(09).

[3]明子成.游覽機器人的設計與實現[J].機器人技術與應用，2008(10).

基金項目：南京曉莊學院2013年度科研項目《基于RTOS的機器人控制器開發》，項目編號2013NXY44。

endprint

摘要：機器人循線移動是當前機器人導航的一種常用方法。關于巡線算法的研究有很多，但多是研究算法本身，有不少算法設計精巧，實際應用中運行快速流暢，但可靠性并未有明顯改善。而我們設計的機器人，巡線采用離散PID算法，實驗測試和機器人競賽結果表明，該算法具有響應速度快、巡線可靠、動作快速流暢和偏離航線率低等優點。

關鍵詞：機器人；離散PID RTOS；傳感器；巡線算法

● 概述

自主巡線機器人可廣泛應用于物流、超市等的倉庫、工廠自動料車、機場等的貨物無人轉移技術領域，但要達到實用化，首先需要解決機器人可靠導航的問題。對于在一定區域內實現機器人導航，巡線是目前較理想的選擇，因此成為各學段學生機器人比賽活動中幾乎必有的項目。縱觀近年來各級各類機器人競賽活動中巡線機器人的表現，最薄弱的環節仍然是在基本的巡線過程中，機器人會不經意間脫離引導線而不得不中止比賽。PID算法在巡線中已經得到一定程度的使用，盡管改善了巡線的質量，但由于控制程序運行過程中的時序同步問題，機器人在移動過程中還是會隨機出現脫離引導線的問題。即使采用多個傳感器信息進行糾偏，但問題仍會出現，即機器人在巡線過程中，某個糾偏傳感器明明“看”到引導線了，卻未按設計要求轉去執行糾偏程序。“視”而不見，表明在傳感器處于引導線區域內時，CPU正在處理其他事務。在機器人MCU等硬件之上植入嵌入式實時操作系統，傳感器的數據采集、環境監測、信息處理與輸出控制都可作為獨立的任務進行管理。讓監視與控制程序同時進行，避免了在等待過程中造成的信息“無視”，控制器實現了對監測到的異常進行實時處理，并即時進行輸出控制，化解了單任務程序設計中的實時響應難題。

● 總體設計

μC/OS-III中，任務可以是基于優先級的搶占式調度，但也支持同等優先級的輪轉調度。機器人的結構設計如圖1所示，安裝了7個灰度傳感器，其中S4用于巡線數據的采集，其他傳感器用于偏離航線的檢測。在執行任務的過程中，機器人始終不允許脫線運行，這就意味著脫線檢測和處理必須優先于其他事件。因此，我們將傳感器S1和S7的信息采集與處理作為任務1（如下頁圖2所示）；在確定S1和S7沒有檢測到標志線的情況下，需要考慮機器人是否處于較大的偏轉狀態，根據傳感器S2和S6采集的信息進行處理的任務2，可以優先執行；而檢測較小偏轉的傳感器S3和S5是為了保證機器人盡量以較小的偏轉沿著引導線平滑移動，設置為任務3；傳感器S4用于巡線移動信息采集，故在這四個應用任務中設置為最低優先級，為任務4。兩個任務之間的數據通信考慮到巡線過程中對傳感器信息的即時性要求，可通過全局變量實現。

圖1

● 關鍵設計

1.離散PID算法實現巡線設計

機器人巡線實際上僅使用一個灰度傳感器S4，即圖1中的3號傳感器。對采集的地面數據運用離散PID算法，通過設定合適的比例、積分和微分參數計算出驅動馬達的速度控制值，以確保其能快速平穩前進。該算法需要從傳感器檢測的地面和引導線不同點的值中找出最大值（Vmax）和最小值（Vmin），并計算出最大值與最小值的平均值，作為判斷在線與否的閾值（Vmid）。計算出最大值與最小值的差值（VΔ），作為比例參量（Kp）計算的依據之一。根據這些數據，分別計算出比例P、積分I和微分D的調節值。

運用比例可以計算出機器人對當前狀況需要作出的反應。比例調節值的算式：

（1）

積分是對已經發生的偏差(V_err)進行累積，以達到快速調節的目的。積分調節值的算式為:

（2）

微分用于對機器人移動方向的趨勢進行預測和加強，比較前一次的偏差與當前的偏差，得到機器人移動的趨勢，用微分調節值來強化該趨勢，計算式為：

（3）

馬達速度調節值：

（4）

馬達的速度控制值：

（5）

（6）

馬達的移動控制函數：

Move（Pwr_Left，Pwr_Right）； （7）

通過實時的地面環境感知和計算結果，即時調整機器人左右馬達的運行速度，從而達到控制機器人沿著引導線移動的目的。同時，設計者需要根據機器人實際運行狀況進行參數Kp、Ki和Kd的值的設定。具體整定方法是：先單獨采用比例算法整定Kp，讓機器人巡線，尋找到合適的Kp和Pwr_Base。需要注意的是，Pwr_Base的值建議與機器人的驅動電池電量形成關聯，可以使系統運行的穩定性更高且適應性更廣。

2.解決時序問題

盡管以上設計在邏輯上似乎解決了巡線和保障問題，但在實際調試時，還是難以處理好巡線的問題。究其原因，在于盡管機器人可以隨時讀取傳感器值，但CPU如果在執行其他指令，而檢測異常的傳感器盡管檢測到了引導線，CPU也會熟視無睹，而當指令執行到讀取該傳感器值時，已經物是人非了，這就會讓人無所適從，明明程序邏輯上沒有錯誤，為什么還是“不聽話”呢？要讓機器人對傳感器信息“隨時”保持敏感，可以引入多任務管理。讓傳感器的采集數據在盡量短的時間內（3ms或更低，具體需視機器人的運行速度、傳感器的采集周期等而定）轉化成驅動馬達的控制指令。據此，將傳感器采集和數據處理作為一個個獨立的任務，根據任務的緊迫性可分為：脫線、較大偏離、較小偏離和巡線四個由高到低不同優先級的任務。

圖2

● 實驗測試與結果分析

機器人自主控制程序的設計所面臨的一個難題是運行環境信息復雜和機器人可感知信息的客觀限制，任何理論上似乎完美的算法，在實際運行時都很難保證100%的成功率，設計中刻意繞開和避免的問題往往還是會發生。基于此，本設計中不是回避問題，而是采用多傳感器信息融合算法，一旦機器人遇到問題就能夠通過傳感器檢測確認，并即時采取糾偏措施。在多次大型機器人競賽巡線相關項目比賽中，筆者均采用了此算法。實踐證明，機器人運行的穩定性、快速性和問題的自糾偏表現均有明顯改善。

參考文獻：

[1]J.J.Labrosse，μC/OS-III：the Real—Time Kernel for the STMicroelectronics STM32[M/OL].2013.http://www.micrium.com/books/ucosiii.

[2]明子成.家庭護理機器人的設計[J].機器人技術與應用，2010(09).

[3]明子成.游覽機器人的設計與實現[J].機器人技術與應用，2008(10).

基金項目：南京曉莊學院2013年度科研項目《基于RTOS的機器人控制器開發》，項目編號2013NXY44。

endprint