一種面向移動機器人的通用回座算法

摘? 要：當前家用機器人的主流回座方法大都是基于紅外信號，也就是說，大多數家用機器人（如掃地機器人、陪伴機器人等）采用的是能夠發射紅外信號的充電座。由于各廠家的移動機器人模具不同，充電座紅外信號不同，導致機器人的回座方法也千差萬別。文章介紹一種面向移動機器人、依賴紅外信號回座的通用回座方法，該回座方法提供一套完整的軟件架構，用以規范回座算法，降低回座算法的開發難度，使得任何一個軟件工程師都可以進行回座功能的開發。

關鍵詞：家用機器人；紅外信號；充電座；軟件架構

中圖分類號：TP242；TP18? 文獻標識碼：A? 文章編號：2096-4706（2023）23-0171-06

A Universal Reseating Algorithm for Mobile Robots

LI Ming

（Amicro Semiconductor Co.， Ltd.， Zhuhai? 519000， China）

Abstract： The current mainstream reseating methods for household robots are mostly based on infrared signals， which mean that most household robots （such as sweeping robots， companion robots， etc.） use charging stations that can emit infrared signals. Due to the different molds of mobile robots from different manufacturers and the different infrared signals of charging stations， the methods of robot resetting also vary greatly. This paper introduces a universal reseating method for mobile robots that relies on infrared signals for reseating. This reseating method provides complete software architecture to standardize the reseating algorithm， reduce the difficulty of developing reseating algorithms， and enable any software engineer to develop reseating functions.

Keywords： household robot; infrared signal; charging station; software architecture

0? 引? 言

機器人作為一個完全獨立自主的系統，自動回座是其不可或缺的功能。當前市面上大多數家用機器人主流的回座都是依靠紅外引導的方式實現。市面上常見的小米、石頭、追覓等掃地機器人廠商均有使用該方案。本文介紹一種適用于依靠紅外信號回座的移動機器人通用回座方法，將回座算法進行歸納分解，搭建一套完整的回座算法軟件架構，此架構可規范回座算法，使軟件結構清晰，大大降低了回座算法的開發難度。同時，此軟件架構具有通用性，適用于基于紅外引導回座的所有機器人。

1? 模塊介紹

1.1? 硬件介紹

Dock由充電座和機器人兩部分組成。充電座發出紅外信號，機器人根據所接收的紅外信號回座充電。通常，充電座的紅外信號分為左信號、右信號、中間信號（有的是左右信號疊加）、護欄信號（圓泡信號）。

充電座的信號分布如圖1所示。機器人有數目不等的紅外接收頭，正中間的一個或兩個紅外接收頭用來對準座子上座，左右通常都會有一個紅外接收頭以保證樣機側面也能收到座子信號。個別機器人的機身后面還有一個紅外接收頭，確保樣機360度都可以接收到紅外信號。有些機器人頭頂有個360度的紅外接收頭，用以代替機身左右及后方的紅外接收頭。

1.2? 軟件架構

Dock架構如圖2所示。軟件分為兩層：Core層和驅動層。Core層實現回座行為的切換。當前最多支持32個回座行為（可配置），最高優先級為0，最低優先級為31。各個行為根據其進入條件，會進行實時搶占式調度。Core層不對外開放，驅動層實現回座的行為。各個行為完全獨立，根據自身設定的進入條件及優先級完成啟動。

2? 架構實現

2.1? Debouncer介紹

由于紅外信號具有瞬時性，加入Debouncer功能，可實現信號去抖動。

定時輪詢對應的紅外信號，只要收到一次即認為此信號有效。連續多次輪詢沒有收到時才認為信號失效。

2.1.1? 數據介紹

predicate指針指向對應的觸發函數，返回True時代表收到對應的紅外信號碼值，否則返回False。

tigger_on記錄on門限值，連續N次觸發函數返回True，且N大于等于tigger_on時，current_state為True。

tigger_off記錄off門限值，連續N次觸發函數返回False，且N大于等于tigger_off時，current_state為False。

on_count、off_count表示Core層內部使用，記錄on與off的次數。

current_state表示Debouncer當前的狀態。

set_dock_context表示設置上下文環境。用于在特定環境下設置樣機對應的參數。如靠近充電座，對應的Debouncer為True時，提高cliff閾值，關閉light touch等。遠離充電座時，再恢復其對應的參數。

2.1.2? 更新邏輯

Debouncer更新邏輯：每10 ms調用輪詢一次。

每個Debouncer根據其predicate函數的返回值，更新其on_count和off_count的值。predicate函數返回True時，其on_count加1，off_count設為零。predicate函數返回False時，其off_count加1，on_count設為零。

當on_count大于trigger_on時，Debouncer的current_

state為True，當off_count大于trigger_off時，Debouncer的current_state為False。

2.2? 行為介紹

行為behavior分為兩種，分別為邊沿觸發行為和電平觸發行為。

對于邊沿觸發行為，只要是觸發信號由False變為True，行為即被觸發，啟動運行，需要自己主動退出行為。

對于電平觸發行為，當觸發信號為True時，行為運行，當觸發信號為False時，行為退出。

2.2.1? 數據結構

Dock_Data結構體用來管理behavior，其有如下幾個變量：

current_function函數指針指向對應的behavior實現函數。

start_when函數指針指向邊沿觸發函數，當此函數的返回值由False變為True時，行為觸發。

run_when函數指針指向電平觸發函數，此函數返回True時，行為啟動，返回False時，行為結束。

abort_when函數指針指向的函數返回True時，行為退出，與start_when配合使用。

abort_code函數指針在行為退出時，執行其指向的函數。

last_start_state記錄上一次start_when的狀態。

priorty代表此behavior的優先級，0為最高優先級，數值越大，優先級越低。

一個行為具有start_when和run_when兩種函數，只有一個函數有效，另一個函數應為空函數。

2.2.2? 行為切換

行為的調度采用搶占式調度策略，從所有行為中選擇優先級高的行為運行。

1）行為篩選。當前行為為空或為電平觸發行為時，所有行為參與篩選。當前行為為邊沿觸發行為，且其不滿足abort條件的情況下，只有優先級高的行為才能強制它。因此選擇下一個行為時，比它優先級低的行為不參與篩選。從當前行為及其他參與篩選的行為中，選擇優先級最高的行為作為下一個要執行的行為。

2）行為切換。行為運行在一個嵌入式操作系統task中。每次進行行為切換，都要先刪除這個task，然后將其運行的行為指針指向新的行為，最后重新創建并啟動task。每次行為切換都要刪除task，重新創建的原因是保證每次都是一個新的行為運行。

2.3? 歸納總結

此Dock架構有兩個好處：

1）Dock行為之間獨立，沒有耦合，有利于多個項目重用。

2）實時性好，行為的切換由單獨的Core層完成。Core層的行為切換判斷代碼在定時器的中斷服務函數中執行。

3? 一種回座算法思路

將充電座信號劃分為四種區域，如圖3所示。

1）區域1。機器人什么信號都收不到，因此機器人進行隨機模式行走，搜索信號。

如圖4（a）所示，機器人在區域1時接收不到任何紅外信號，機器人進行隨機模式行走，尋找座子發出的紅外信號。

2）區域2。機器人收到了左信號或右信號，說明充電座的正前方就在附近。

如圖4（c）所示，機器人在區域2時，實現如上圖中的一種行為，去尋找中間線，并將機器人姿態調整為正對著充電座。一種是向中間線走，邊走邊轉向充電座方向，以確保行走在中間線上。一種是朝充電座方向弧形搖擺前進，直至走到中間線內或收到了圓泡信號。

3）區域3。機器人可以同時收到左信號和右信號或中間信號，表明充電座就在機器人的前方。

圖4（b）所示，機器人在區域3時根據正前方的紅外接收頭接收到左信號、右信號兩個信號或者是接收到中間信號，實時調整樣機姿態，使得機器人能夠精準地對接充電座，進行充電。

4）區域4。機器人收到了圓泡信號，說明當前機器人離充電座很近，有可能撞到充電座。

圖4（d）所示，機器人可以繞圓泡信號行走，收到中間信號，直接旋轉掉頭直至樣機正前方的紅外接收頭接收到中間信號，然后切換到區域3的行為對準上座。

4? 回座算法實現方法

可根據不同的模具、紅外信號特點，設計自己的回座行為。本節介紹當前我們實現的一套算法行為，可適配公司大部分掃地機器人，由于不同模具具有不同的紅外信號，只需微調Debouncer及觸發函數start_when、run_when。

4.1? 區域1找信號

當機器人在區域1時，收不到任何充電座信號，此時進行隨機模式行走尋找充電座信號。隨機跑由兩個行為組成：docking_line和docking_line_bounce。docking_line走直線，docking_line_bounce遇到障礙物時隨機轉一個角度。

4.1.1? docking_line

docking_line行為為電平觸發方式。

觸發條件及行為實現如下：

1）觸發條件。電平觸發函數run_when永遠有效，但其在所有行為中的優先級最低。因此當其他行為都無效時，它會運行。

2）行為實現。行為實現函數current_function控制機器人走直線。start_when函數指針、abort_when函數指針、abort_code函數指針都為空指針。

4.1.2? docking_line_bounce

docking_line_bounce行為為邊沿觸發方式。

觸發條件、退出條件及行為實現如下：

1）觸發條件。run_when函數指針為空指針。start_when函數的實現邏輯為遇到障礙物時獲取一個隨機角度，并設置旋轉方向為障礙物的反方向。當發現障礙物時返回True觸發docking_line_bounce行為，其優先級高于docking_line。

2）退出條件。用一個全局變量docking_line_bounce_abort記錄是否有行為要退出。abort_when函數用于判斷此全局變量，當此全局變量被設置時abort_when函數返回True，docking_line_bounce行為退出。abort_code函數復位docking_line_bounce_abort變量，為下次行為進入做準備。

3）行為實現。根據start_when函數給出的旋轉角度、旋轉方向，控制機器人旋轉，當旋轉到目標角度時設置docking_line_bounce_abort變量主動退出docking_line_bounce行為。

4.2? 區域2找中線

當機器人處于區域2時，可以收到左信號或右信號，此時觸發docking_left_right行為。此行為將找到區域3，并將機器人的正前方對準充電座，然后此行為退出，將控制權交給區域3的行為。

觸發條件、退出條件及行為實現如下：

1）觸發條件。有接收頭收到左信號或右信號，前方正中間紅外接收頭沒有收到中間信號（包括同時收到左信號和右信號）。

為了解決紅外信號的瞬時性，相關Debouncer定義如下：

recently_left_left代表機器人左邊接收頭接收左邊信號。收到1次就有效，連續50次未收到失效。

recently_right_left代表機器人左邊接收頭接收右邊信號。收到1次就有效，連續50次未收到失效。

recently_right_right代表機器人右邊接收頭接收右邊信號。收到1次就有效，連續50次未收到失效。

recently_left_right代表機器人右邊接收頭接收左邊信號。收到1次就有效，連續50次未收到失效。

recently_center_focus代表中間接收頭接收左邊或右邊信號。收到1次就有效，連續10次未收到失效。

run_when函數指針為空指針。

start_when函數實現邏輯如下：

當recently_left_left、recently_left_right為True，且recently_center_focus為False，start_when函數返回True，且記錄機器人當前在充電座左邊的區域2。

當recently_right_left、recently_right_right為True，且recently_center_focus為False，start_when函數返回True，且記錄機器人當前在充電座右邊的區域2。否則，start_when函數返回False。

2）退出條件。abort_when函數實現原理為，正中間紅外接收頭同時接收到左信號和右信號時退出。

3）行為實現。樣機直接擺動著向中線靠近。如樣機前方紅外接收頭接收到左信號就向右擺動，接收到右信號就向左擺動，且擺動弧度要大一些。如若擺動過程中收不到信號，就原地反方向轉圈或小弧度反方向擺動，直至重新接收到信號，再接著做大弧度擺動。

4.3? 區域3正面上座

正面對著上座，由docking_go_forward、docking_left、docking_right三個行為完成。

docking_go_forward行為表示直行；docking_left行為表示向左走弧形；docking_right行為表示向右走弧形。

三個行為的優先級順序為docking_go_forward最高，docking_left最低。這三個行為根據樣機正中間的紅外接收頭實時收到信號，進行實時任務切換，使樣機能夠正對著上到充電座上。

4.3.1? docking_go_forward

docking_go_forward為電平觸發行為，樣機正中間的紅外接收頭同時接收到左信號和右信號或者是接收到中間信號時運行。

觸發條件、退出條件及行為實現如下：

1）觸發條件。Debouncer定義正中間的紅外接收頭，同時接收到左信號和右信號，或者是接收到中間信號。只要滿足條件1次就有效，連續2次不滿足條件則失效。

此Debouncer有效，則run_when函數返回True，docking_go_forward行為生效。

start_when函數指針、abort_when函數指針、abort_code函數指針都為空指針。

2）退出條件。無。

3）行為實現。向左右輪流發送同樣的速度命令，使機器人直行。

4.3.2? docking_right

docking_right為電平觸發行為，樣機正中間的紅外接收頭只接收到左信號時運行。

觸發條件、退出條件及行為實現如下：

1）觸發條件。Debouncer定義如下。如若正中間的紅外接收頭只接收到左信號，1次就有效，連續20次未收到則失效。此Debouncer有效，則run_when函數返回True，docking_right行為生效。start_when函數指針、abort_when函數指針、abort_code函數指針都為空指針。

2）退出條件。無。

3）行為實現。將機器人的右輪子設置為較慢的速度，將機器人的左輪子設置為較快的速度，使樣機向右走弧線。

4.3.3? docking_left

docking_left為電平觸發行為，樣機正中間的紅外接收頭只接收到右信號時運行。docking_left行為與docking_right行為類似，這里不再贅述。

4.4? 區域4避開座子

機器人不是正對著座子，且機器人接收到了圓泡信號，說明機器人有可能撞到座子。此時應觸發避座行為docking_force_field。

docking_force_field行為為邊沿觸發方式，可實現繞座功能。

觸發條件、退出條件及行為實現如下：

1）觸發條件。中間紅外接收頭沒有收到左右信號，但樣機收到圓泡信號時觸發。

Debouncer定義如下：

recently_near_dock代表樣機是否接收到圓泡信號，收到1次就有效，連續20次沒有收到則無效。

recently_force_field代表樣機一段時間內多次收到圓泡信號。recently_near_dock有效23次，其有效；recently_near_dock失效1次，其失效。

recently_no_force_field代表中間接收頭是否收到左右信號。收到1次就有效，連續100次沒有收到則無效。

start_when函數，當recently_force_field有效、recently_no_force_field無效時，返回True，docking_force_field行為運行。

退出條件及行為實現如下：

1）退出條件。docking_force_field行為自己設置docking_force_field_abort變量，主動退出docking_force_field行為。用一個全局變量docking_force_field_abort記錄是否有行為要退出。abort_when函數判斷此全局變量，當其被設置時返回True，docking_force_field行為退出。abort_code函數復位docking_force_field_abort變量，為下次行為進入做準備。

2）行為實現。docking_force_field繞座行為。當樣機接收頭接收到圓泡時，遠離座子走弧線；當樣機接收頭接收不到圓泡信號時，靠近座子走弧線；直至機器人有接收頭同時接收到座子發出的左信號和右信號或者是中間信號。調整樣機方向，使樣機正前方的中間接收頭可以收到左信號和右信號或者是中間信號。

4.5? 優先級

各行為優先級從高到低分別為：DOCKING_FORCE_FIELD、DOCKING_LEFT_RIGHT、DOCKING_LINE_BOUNCE、DOCKING_GO_FORWARD、DOCKING_RIGHT、DOCKING_LEFT、DOCKING_LINE。

5? 實驗數據

5.1? 測試方法

試驗臺長為5 000 mm，寬為3 000 mm，圍欄高度至少為300 mm。

清潔機器人放置在以充電座為中心，半徑分別為1 000 mm和2 000 mm的半圓弧上。每個位置點和充電座的連線成0°、45°、90°、135°和180°。將清潔機器人放置在離充電座1 000 mm的位置上啟動返回鍵運行，記錄其是否能在3 min內成功對接充電。在每個位置試驗4次，清潔機器人的頭部放置方位分別和充電座連線逆時針成0°、90°、180°和270°。在離充電座2 000 mm的位置重復以上試驗，運行時間為5 min。在10個位置進行試驗40次，按照公式計算對接成功率。

注意：如果在試驗過程中，清潔機器人將充電座推離原位置0.5 m遠，則記為未對接成功。測試方法演示圖如圖5所示。

5.2? 測試結果

如圖6所示，采用兩種不同模具的掃地機器人來驗證此回座算法的通用性、可行性。

（a）樣機一? ? ? ? ? ? ? ? （b）樣機二

5.2.1? 樣機一測試結果

如圖6（a）所示，左側黑色樣機為樣機一，其測試結果如表1所示。

滿足國標3 min內成功對接充電的要求。

5.2.2? 樣機二測試結果

如圖6（b）所示，右側白色樣機為樣機二，其測試結果如表2所示。

滿足國標3 min內成功對接充電的要求。

6? 結? 論

此Dock架構旨在規范回座算法，使軟件結構清晰，算法思路明了，降低回座算法的開發難度，使得任何一個軟件工程師都可以進行回座功能的開發。

參考文獻：

[1] 魏銀.掃地機器人自動回充系統的設計與實現 [D].合肥：中國科學技術大學，2019.

[2] 白茶-清歡.移動機器人自主回充技術理論與實踐 [EB/OL].[2023-04-10].https：//blog.csdn.net/zhao_ke_xue/article/details/122156201.

[3] 中華人民共和國工業和信息化部.家用和類似用途清潔機器人行業標準：QB_T 4833—2015 [S]．北京：中國輕工業出版社，2016.

[4] 肖奇軍，鄭健聰，陳斯鵬，等.一種基于地圖構建與角度傳感器的掃地機器人自動回充方法 [J].機械與電子，2019，37（2）：78-80.

[5] 谷冀嘉．基于ROS系統的自主充電技術研究 [D].成都：電子科技大學，2020.

[6] 劉甲賓.一種基于激光雷達的機器人自主充電方法 [J].鐵路通信信號工程技術，2022，19（4）：19-23.

[7] 李斌，賴志林，王淼.基于醫院智能配送機器人的自主充電控制系統 [J].中國信息化，2021（11）：14-16.

[8] 孟虎．移動機器人路徑規劃與自主充電技術研究 [D].南京：南京理工大學，2023.

作者簡介：李明（1983.10—），男，漢族，山西大同人，工程師，碩士，研究方向：智能家用機器人SLAM算法及運動控制。