俯視相機輔助的地面機器人導(dǎo)航實驗平臺

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

（同濟(jì)大學(xué)電子與信息工程學(xué)院，上海 200092）

0 引言

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

1 系統(tǒng)功能架構(gòu)設(shè)計

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

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

2 實驗平臺構(gòu)建實現(xiàn)

2.1 地面機器人實驗對象

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

圖2 兩輪差速底盤基本結(jié)構(gòu)及其運動學(xué)分析數(shù)學(xué)模型

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

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

2.2.1 視頻圖像預(yù)處理

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

2.2.2 相關(guān)濾波器的跟蹤定位

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

圖3 基于相關(guān)濾波器的跟蹤定位

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

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

2.3.1 代價地圖

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

2.3.2 路徑規(guī)劃

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

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

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

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

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

2.4.1 導(dǎo)航路徑跟蹤控制架構(gòu)

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

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

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

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

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

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

3 實驗測試與結(jié)果分析

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

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

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

表1 部分綜合評價指標(biāo)和測試結(jié)果

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

3.2 定位測試結(jié)果及分析

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

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

3.3 路徑跟蹤測試結(jié)果及分析

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

4 結(jié)語

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