俯視相機輔助的地面機器人導航實驗平臺

曹明宇，袁家遙，張志明，賴志杰，朱子祎

（同濟大學電子與信息工程學院，上海 200092）

0 引言

自主移動機器人在現代工業(yè)、農業(yè)、醫(yī)療、服務等行業(yè)得到了廣泛應用，其感知環(huán)境信息并依此實現定位和導航功能，是機器人實現自主運動的關鍵問題之一［1］。在城市安全、搜索救援等場景應用中，由于環(huán)境復雜難辨，機器人的環(huán)境感知能力受到極大限制，本體搭載的傳感器提供的信息有限，往往無法合理規(guī)劃路徑并導航到特定地點［2-3］。近年來，由無人機搭載俯視相機跟蹤拍攝，直接獲得全局地貌信息后，為地面機器人規(guī)劃路徑并引導到達指定位置的聯合作業(yè)方案越來越受到研究人員的關注［4-6］。但現階段由于無人機成本高、續(xù)航時間短、操控復雜且需要專用場地或空域，在高校新工科工程教育實驗和綜合創(chuàng)新實踐時受到限制，難以開展普遍性的實訓教學。學生在動手實踐過程中需要綜合理解和掌握專業(yè)課程的基礎理論和方法，完成實驗教學任務與創(chuàng)新拓展訓練［7-8］。本實驗項目針對人工智能＋機器人專業(yè)方向人才培養(yǎng)需求，簡化設計方案，在室內環(huán)境中搭建基于俯視相機輔助下的地面機器人導航系統(tǒng)實驗平臺，完成相關核心功能模塊和控制算法的研究與實現，并通過軟件模擬仿真與實際場景測試，驗證其可行性。

1 系統(tǒng)功能架構設計

地面機器人導航系統(tǒng)的設計與開發(fā)是一個典型的復雜工程問題，涉及傳感檢測技術、電子信息處理、嵌入式系統(tǒng)、自動控制原理、計算機控制以及人工智能等多學科的理論知識和綜合實踐技能，完成方案論證、建模仿真、軟硬件設計、算法編程、系統(tǒng)聯調等研究工作。其所要實現的主要功能，是控制機器人在周邊環(huán)境中沿事先規(guī)劃好的最優(yōu)或者次優(yōu)可達路徑運動，并保證過程中其位置偏差始終在一定精度范圍之內。系統(tǒng)實驗平臺的硬件實體對象主要由俯視相機、計算機和地面機器人組成，組合完成周邊環(huán)境感知單元、導航路徑規(guī)劃單元、任務控制執(zhí)行單元和輔助監(jiān)控計算機單元等功能模塊。實驗測試場地中地面機器人導航系統(tǒng)的硬件單元如圖1（a）所示，整體功能架構如圖1（b）所示。俯視相機安裝于實驗場地上方，實時采集地面機器人及其所處環(huán)境的圖像信息，傳送到計算機系統(tǒng)進行圖像處理，檢測周圍障礙物完成全局路徑規(guī)劃，并對機器人進行實時目標跟蹤定位，將解算得到的位置誤差與預期姿態(tài)通過無線方式傳送至移動機器人；地面機器人（內置處理器）利用所接收到的位置誤差和期望姿態(tài)，對導航路徑進行跟蹤，先通過EKF 擴展卡爾曼濾波融合估計得到機器人當前姿態(tài)，再運行復合模糊PID軌跡跟蹤算法，解算得到預期線速度與角速度信息，并發(fā)布給機器人AI-KIT 底層控制系統(tǒng)，控制機器人運動的速度與方向，從而使機器人沿預設導航路徑行駛；監(jiān)控計算機單元是實驗平臺的人機交互界面，主要功能包括系統(tǒng)參數設置、機器人運動狀態(tài)可視化、路徑規(guī)劃和任務控制的狀態(tài)監(jiān)視等功能。

圖1 俯視相機輔助地面機器人導航系統(tǒng)整體功能架構

2 實驗平臺構建實現

2.1 地面機器人實驗對象

地面機器人的移動機構是完成任務的基本保障，考慮到輪式機器人機械結構較為簡單，相對易于控制，且轉彎控制精度高，實驗對象地面機器人選用樂博士公司的AI-KIT 人工智能ROS 機器人通用教學平臺［9］，其采用兩輪差速式底盤，搭載有工控機、激光雷達、深度攝像頭、IMU 等模塊，核心處理器采用Intel NUC（Core i3）／4GB RAM／250GB固態(tài)硬盤，由外部鋰電池供電，采用ROS 機器人操作系統(tǒng)［10］作為軟件環(huán)境，提供人機交互友好的可視化界面，通過訂閱相關話題可以顯示機器人模型、地圖、激光數據、軌跡等信息。機器人的兩個動力輪對稱分布于本體底盤的兩側，由兩個電動機獨立驅動，通過給定不同的速度實現轉向控制；底盤同時配有前后兩個萬向輪，用于輔助支撐。由于移動機器人是一個復雜的非完整約束系統(tǒng)，其動力學模型較為復雜，受干擾情況不穩(wěn)定，建模準確度不足，因此在當前應用中主要基于運動學模型進行理論分析、仿真與調試。地面機器人的兩輪差速底盤基本結構及其運動學分析數學模型如圖2中所示，車體速度為v，左動力輪速度為vl，右動力輪速度為vr，車體自轉速度為ω，轉彎半徑為R，兩輪之間距離為D，兩輪到車中心的距離為d，右輪到圓心距離為L。將機器人整體運動的預期前向速度v和轉動角速度ω解算為左／右動力輪的獨立速度vl和vr后分別控制，滿足約束方程：

圖2 兩輪差速底盤基本結構及其運動學分析數學模型

2.2 周邊環(huán)境感知單元

實驗系統(tǒng)中，周邊環(huán)境感知單元由俯視相機和計算機組成，利用圖像處理技術模擬人類視覺處理功能，從地面環(huán)境事物的圖像中提取足夠豐富的信息，提供地面機器人的實時位置，以及識別機器人周邊環(huán)境的特征。

2.2.1 視頻圖像預處理

由于相比彩色級和灰度級圖像，原始圖像合理二值化后，在壓縮減少信息量的同時，可繼續(xù)保留圖像主要特征，做相關運算時有更好的去噪作用和相關性能［11］。為提高機器人導航系統(tǒng)處理速度，在實際操作中，首先對采集到的單幀周邊環(huán)境圖像進行灰度化、降維和濾波增強等預處理工作，然后參考圖像的灰度直方圖選取二值化閾值，最終實現二值化處理，也便于后期路徑搜索與全局規(guī)劃。

2.2.2 相關濾波器的跟蹤定位

目標跟蹤為地面機器人提供實時定位信息，本實驗平臺中選用基于相關濾波器的跟蹤（Correlation Filter-based Tracking，CFT）算法，典型的CFT 算法有KCF、DSST、STC、SAMF 等［12］。使用相關濾波器進行目標跟蹤的工作過程如圖3（a）所示：在首幀地面環(huán)境圖像中機器人位置處提取圖像塊特征，用余弦窗口平滑邊緣后訓練得到相關濾波器的結構參數；通過離散傅里葉變換（DFT）執(zhí)行相關濾波操作；在后續(xù)到來的每1 幀中，根據前1 幀給出的機器人位置信息提取得到新的圖像塊，再通過傅里葉逆變換（IDFT）得到該幀圖像的置信圖，其最大值所對應的像素坐標即為地面機器人的新位置，并由此訓練更新相關濾波器的參數用于下1 幀的目標檢測。核相關濾波算法（Kernel Correlation Filter，KCF）引入該方法使得算法更具魯棒性，能夠處理非線性分類；訓練過程則被轉化為嶺回歸問題以避免訓練時的過擬合，且具有速度快，效果相對較好，結構和實現簡單的優(yōu)點［13］，實際跟蹤效果示例如圖3（b）所示。

圖3 基于相關濾波器的跟蹤定位

2.3 導航路徑規(guī)劃單元

機器人的導航，即在外部傳感器的輔助下沿規(guī)劃出的路徑安全地運動到達目的地，基于地圖的路徑規(guī)劃是其核心技術之一［14］。為實現機器人的自主導航，該單元首先要獲得環(huán)境相關的地圖信息，然后進行路徑規(guī)劃，最后將結果數據（如多個途經點坐標等）發(fā)送給機器人，使其通過機器人本體中的任務控制執(zhí)行單元實現自主導航的功能。

2.3.1 代價地圖

環(huán)境代價地圖是基于地圖導航的基礎，其表示方法可分為柵格、幾何以及拓撲。常用的柵格地圖表示法將整個環(huán)境分為若干相同大小的柵格單元［15］，每個柵格的權值表示機器人在此處會與障礙物發(fā)生碰撞的概率，即機器人運動到該柵格時需付出的代價。為簡化地圖的構建與維護，盡可能保留地面機器人導航所需的環(huán)境信息，實驗設計采用俯視相機獲得忽略環(huán)境高度信息的二維地圖來近似表征靜態(tài)環(huán)境，機器人可據此地圖進行全局路徑規(guī)劃。

2.3.2 路徑規(guī)劃

路徑規(guī)劃要求機器人根據某種優(yōu)化準則，在地圖上尋找一條從起始點到目標點不經過障礙物的代價最小的路線。經典的全局路徑規(guī)劃算法有Dijkstra 算法和A*算法等［16］，可以靜態(tài)地規(guī)劃出最優(yōu)或次優(yōu)路線。其中A*算法是一種啟發(fā)式搜索算法，規(guī)劃每步路徑的同時會搜索下一步可達的各個節(jié)點并進行評估，評估函數形式如下：

式中：f（n）為對當前點n 的評估函數；g（n）為從初始點到當前點的實際代價值；h（n）為當前點的啟發(fā)值。

通過啟發(fā)值的限制使得那些遠離最優(yōu)路徑區(qū)域的柵格單元不被檢索，與Dijkstra 遍歷搜索算法相比具有較低的時間和空間復雜度。在啟發(fā)值函數設計比較合理的情況下，A*算法所得近似最優(yōu)路徑與Dijkstra算法所得最優(yōu)路徑相差不大，故結合實際應用情況優(yōu)先考慮使用A*算法用于尋路，實現基于地圖的路徑規(guī)劃。

2.4 任務控制執(zhí)行單元

路徑規(guī)劃的最終效果需要運動控制系統(tǒng)的執(zhí)行體現，任務控制執(zhí)行單元負責實現機器人的路徑跟蹤，根據機器人當前的運動狀態(tài)及其與周圍環(huán)境之間相對運動的關系，按照一定的策略綜合控制機器人的運動速度和方向，使實際運動路徑與理想規(guī)劃路徑能夠滿足指標要求［17］，實現真實路徑與規(guī)劃路徑的良好復現。

2.4.1 導航路徑跟蹤控制架構

基于機器人運動學模型的導航路徑跟蹤控制一般由運動模型控制和電動機轉速控制兩層結構構成。其中外層的運動學模型控制器不需要考慮機器人的動力學特性，可以根據當前實際位姿與期望位姿的偏差計算得到地面機器人的期望角速度與線速度。經過解算轉變?yōu)轵寗与妱訖C的預期轉速，將其與當前實際轉速的偏差輸入內層的驅動電動機轉速控制器，解算后輸出電動機控制指令。路徑跟蹤控制的整體結構如圖4（a）所示，AI-KIT 移動機器人的系統(tǒng)底層控制結構如圖4（b）所示。具體實現過程中，機器人運動控制節(jié)點move＿controller 將路徑跟蹤控制中計算出的機器人運動速度信息robot＿vel 通過串口通信發(fā)送給下位機嵌入式控制板。下位機中根據機器人運動學模型進行解算，將移動機器人的預期速度轉換為左／右兩個動力輪的預期轉速，然后通過相關電氣接口將對應占空比的PWM驅動波形發(fā)送給電動機驅動板，控制機器人動力電動機轉動，通過編碼器反饋實際轉速，通過外部傳感器獲取實際位姿，完成雙電動機轉速的閉環(huán)控制，使車輪轉速跟上預期速度。

圖4 基于機器人運動學模型的任務控制執(zhí)行單元

2.4.2 雙閉環(huán)模糊控制器

反饋閉環(huán)控制系統(tǒng)控制精度高，能夠較好地抵抗干擾，但參數設定不合適時會存在穩(wěn)定性問題。機器人底層的速度控制環(huán)結構比較簡單，控制器采用常規(guī)PID算法即可達到較優(yōu)的效果；但上層運動學模型控制器若采用傳統(tǒng)PID控制算法，在軌跡曲率變化較大或連續(xù)變化處，機器人往往難以快速跟蹤，甚至出現不穩(wěn)定的情況。這是因為機器人是復雜的非線性系統(tǒng)，而傳統(tǒng)PID控制必須依賴精確的模型進行參數整定，減弱了機器人對不同路徑的適應能力。實驗中圍繞前述機器人運動學模型，將模糊控制等現代控制理論與方法與PID 控制相結合，實現基于復合模糊PID 算法［18］的路徑跟蹤，參數自整定模糊控制包括模糊化，制定模糊規(guī)則，清晰化等組成部分。在路徑跟蹤控制系統(tǒng)中，計算當前機器人位置與期望路徑的偏差，以及偏差的變化率，根據給定的模糊規(guī)則進行模糊推理得到模糊參數，最后經過清晰化處理輸出實時優(yōu)化后的Kp，Ki，Kd3 個PID參數，完成上層運動學模型控制器。

2.4.3 姿態(tài)定位融合

實驗中基于多傳感器數據融合的方法測量位姿，根據系統(tǒng)物理模型以及噪聲統(tǒng)計特性，利用一定的估計準則來獲得系統(tǒng)的最優(yōu)狀態(tài)估計［19］，在一種傳感器測量數據失真或者無效時，自動切換使用系統(tǒng)中其他可用傳感器的測量數據；另一方面，多傳感器數據融合處理后能得到更真實的測量結果。機器人通過編碼器推算軌跡，直接對距離做分解，累積誤差小，在航向角較準確的情況下做修正，最終精度可達到0.1%以內。為獲取較準確的航向角θ，實驗中調用ROS Navigation stack 中的robot＿pose＿ekf 包，通過擴展卡爾曼濾波器（EKF）對IMU、里程計odom的數據進行融合，估計地面移動機器人的真實姿態(tài)，輸出odom＿combined消息。實際測試發(fā)現，IMU信息的協(xié)方差矩陣中代表機器人航向角分量的協(xié)方差數量級為10－6，而里程計信息的協(xié)方差矩陣中機器人姿態(tài)分量的協(xié)方差數量級為10－3，兩者之間的數量級相差很大。故在進行EKF 融合時，會更“相信”IMU所提供的姿態(tài)信息。如機器人在轉動過程中輪子發(fā)生了打滑，用編碼器推算出的姿態(tài)一直在旋轉，而其實際姿態(tài)并未發(fā)生太大變化，通過EKF 融合后的信息則可以糾正此種情形下的錯誤估計，使姿態(tài)信息更加可信，所以實驗中訂閱使用odom＿combined消息輸出的姿態(tài)信息。

3 實驗測試與結果分析

在實驗室環(huán)境中搭建實驗操作場景，如圖1 所示，包括俯視相機、計算機、地面機器人和試驗場地。針對項目背景中所述問題，為驗證實驗平臺方案的有效性，設計與實現基于俯視相機的圖像采集與地圖處理、基于A*算法的路徑規(guī)劃、基于KCF算法的定位、復合模糊PID路徑跟蹤等功能模塊，并進行了Matlab 仿真測試與在AI-KIT移動機器人平臺上的實際測試。部分綜合評價指標、測試結果（平均值）和測試方法如表1所示。

3.1 基于地圖的導航路徑規(guī)劃

實驗示例中（見圖5），地圖中白色部分為可通行安全區(qū)域，黑色部分為障礙區(qū)域，灰色部分為障礙膨脹區(qū)域，黑色和灰色部分不可通行。在可通行區(qū)域內部選定起點（左上方）和終點（右下方），規(guī)劃路徑用黑色線條表示。如圖5（a）所示，由于A*算法的貪婪性，所得路徑離障礙邊界過近（特別是轉彎處），會因為機器人本身物理尺寸，在運行中發(fā)生碰撞影響通行性。對障礙邊界進行膨脹處理后，效果如圖5（b）所示。此時獲得的路徑基本位于可通行區(qū)域的中央，如圖5（c）所示，避免了障礙碰撞，但仍存在一些角度變化較大的拐點，不利于路徑跟蹤控制。為消除這些拐點，對途經點進行插值采樣，然后通過三次樣條方法得到較為平滑的路徑，如圖5（d）所示。

表1 部分綜合評價指標和測試結果

圖5 基于地圖的導航路徑規(guī)劃

3.2 定位測試結果及分析

測試實驗地面場地為8 m×8 m，在俯視攝像頭正下方區(qū)域約為4.12 m ×4.12 m，定位測試軌跡如圖6（a）所示，圖中綠色軌跡為使用激光雷達SLAM 測定的定位結果，紅色軌跡為基于KCF 算法的定位結果，計算可得全程平均誤差約為80 mm。由于俯視相機標定與坐標系轉換存在一定的誤差，再綜合考慮機器人本體尺寸與地面范圍大小的因素，該定位誤差在修正后可進一步減小到50 mm的可接受范圍內，能夠滿足機器人定位需求。

圖6 地面機器人定位測試及路徑跟蹤控制

3.3 路徑跟蹤測試結果及分析

在測試實驗地面場地中分別選取起始點和終止點，由導航路徑規(guī)劃單元計算得到任務路徑，地面機器人從起始點出發(fā)，在任務控制執(zhí)行單元控制下沿所設定的路徑運動，抵達終止點后停止。機器人的路徑跟蹤控制移動軌跡如圖6（b）所示，圖中綠色軌跡1 和2為理想規(guī)劃路徑，紅色軌跡1 和2 為融合后的實際運動路徑，軌跡數據離線計算后可知，路徑跟蹤過程中的最大誤差為73 mm，全程平均誤差均在48 mm 以內。綜合考慮機器人本體尺寸與場地范圍大小等的因素，該誤差同樣在可接受范圍內，能夠較好地實現路徑跟蹤。

4 結語

Matlab仿真與實際場景測試的結果表明，由俯視觀測器提供全局觀測，可以輔助地面移動機器人的導航，方案有效可行。在此實驗平臺基礎上實現的綜合創(chuàng)新性實驗，可吸引學生積極參與，培養(yǎng)創(chuàng)新思維和工程實踐能力。進一步工作包括：①俯視相機換用RGD-D相機，提供深度信息，對障礙物的感知更加全面和直觀；②將現在固定位置安裝的俯視相機，升級為空中無人機航拍，對復雜環(huán)境中信息獲取處理和地空協(xié)調等提出更高的要求；③結合實際需求開展針對性研究，如室內／室外環(huán)境中的障礙物探測及機器人主動避障技術等，優(yōu)化算法以增強系統(tǒng)的魯棒性和實時性。從而增強實驗功能和效果，更好地營造大學生創(chuàng)新創(chuàng)業(yè)實踐訓練良好環(huán)境，服務教學實踐和科研實踐。