基于ROS和SLAM的無人消殺機器人系統設計

宋鑫鵬，趙 倩

(曲阜師范大學工學院，山東 日照 276800)

0 引言

全球新冠感染疫情防控仍不容懈怠，公共場所疫情防控的消殺環(huán)節(jié)是防控重點。然而，國內外的防疫工作主要采用人工消殺。人工消殺作業(yè)過程繁瑣、消殺覆蓋率低、危險場所無法保障人員安全等弊端逐漸顯現。國內外公司均已開發(fā)出多款面向疫情防控的消殺防疫機器人[1]。這些機器人雖存在一定的短板，但其擴展性高、路徑規(guī)劃可選擇，優(yōu)點十分明顯。

為了簡化作業(yè)流程、提高消殺覆蓋率和保障人員安全，本文設計了1種基于機器人系統(robot operating system,ROS)的無人消殺機器人系統。該系統結合了市場上一些消殺機器人系統的擴展性高和路徑規(guī)劃可選擇的特點，可自主調節(jié)消殺藥液濃度，通過自主導航進行環(huán)境消殺，并通過上位機調度軟件對機器人進行實時監(jiān)控，以實現公共場所消殺的無人化和智能化。

1 系統結構設計

基于ROS的無人消殺機器人系統由上位機調度軟件、搭建在Jeston nano上的ROS機器人操作系統、STM32系列單片機、激光雷達、九軸慣性測量單元(inertial measurement unit，IMU)、編碼器、藥液箱和霧化噴射裝置組成。上位機調度軟件使用Pyside2進行可視化界面設計，并利用Socket與ROS中的調度節(jié)點進行通信，以實現數據的雙向傳輸。在ROS中，利用基于粒子濾波的Gmapping算法對未知環(huán)境進行地圖構建。地圖的精度直接影響了在執(zhí)行消殺任務時的導航精度。Gmapping算法構建的地圖精度與里程計的精度有關。為了提高里程計的精度和魯棒性，本文采用了基于擴展卡爾曼濾波的里程計融合方案，對激光雷達里程計、輪式里程計和IMU進行融合，以提高定位精度。同時，本文通過基于粒子濾波的自適應蒙特卡洛定位(adaptive Monte Carlo localization，AMCL)發(fā)布的Map坐標系到Odom坐標系的坐標變換，進一步提高了定位精度。在消殺導航階段：全局路徑規(guī)劃采用帶有啟發(fā)式搜索的A*算法；局部路徑規(guī)劃采用動態(tài)窗口法(dynamic window approach，DWA)。藥液噴灑部分由霍爾流量計、可調速吸泵、射流器和霧化噴頭組成。上位機調度軟件通過Socket與機器人上位機Jeston nano通信。Jeston nano與機器人下位機STM32單片機進行串口通信，從而實現遠程操控機器人的目的。系統結構如圖1所示。

圖1 系統結構框圖

2 子系統設計

無人消殺系統主要包括任務調度系統、定位系統、地圖構建系統、自主導航系統和藥液噴灑系統這5個子系統。

2.1 任務調度系統

任務調度系統主要用于監(jiān)測并控制機器人，通過遠程通信觀察每個機器人的運行狀態(tài)和消毒液剩余量，并對不同編號的機器人進行任務分配。使用任務調度系統可達到減少人工參與、降低人工成本、保障工作人員安全的目的。調度軟件界面如圖2所示。

圖2 調度軟件界面

調度軟件界面采用Pyside2設計。在界面內可觀察到當前機器人在線數量，并可選擇當前操作機器人的編號、調整消毒液濃度、發(fā)送指令使機器人前往指定區(qū)域消殺。與機器人通信時，采用Socket編程中的面向連接編程，即傳輸控制協議/網際協議(transmission control protocol/internet protocal,TCP/IP)。此通信方法比較固定，需要在調度軟件上建立服務端、在機器人上建立客戶端。在建立服務端的同時，設置監(jiān)聽多客戶端，以實現一對多的通信。在每個機器人機載電腦的ROS中分別單獨建立1個節(jié)點，用于與調度軟件的通信。該節(jié)點實時監(jiān)聽來自調度軟件的指令，通過自定義話題發(fā)出控制信息并實時上傳機器人的運行狀態(tài)。

2.2 定位系統

機器人定位系統包括里程計定位與AMCL全局定位。里程計定位用于估計機器人位姿。AMCL全局定位用于減小里程計誤差。目前，3種主流里程計分別是基于IMU的里程計、輪式里程計和視覺里程計。基于IMU的里程計是較為理想的估計空間方向的里程計，但在測量時無法抵消測量的重力分量，會隨著時間的推移產生里程漂移。輪式里程計[2]被廣泛應用于輪式或腿式機器人的運動估計，但在建立機器人模型時經常因為打滑而導致里程不準確。視覺里程計則是運動估計中較優(yōu)秀的解決方案，適用于不同運動模型的機器人。但視覺里程計受環(huán)境影響嚴重，且計算量較大。除以上3種主流方式外，本文設計還采用了1種效率高、準確度高且適用性強的里程計方案——激光雷達里程計。

本文設計中使用的里程計和AMCL全局定位大致分為5個部分，分別為激光雷達里程計、輪式里程計、輪式里程計標定、擴展卡爾曼濾波和AMCL全局定位。

2.2.1 激光雷達里程計

激光雷達里程計具有快速性和準確性，可為機器人的里程計提供較快的響應和較為準確的信息。本文設計采用的rf2o_laser_odometry是1種基于距離流約束方程的估計2D激光雷達位姿的方法[3]。該方法不依賴于機器人的運動類型，適用于激光雷達的測繪、避障或定位，并且在平移和旋轉方面都優(yōu)于點對點掃描的極軸掃描匹配(polar scan match，PSM)算法和點對線迭代最近點(point to line iterative closest points，PL-ICP)掃描匹配算法。

2.2.2 輪式里程計

激光雷達里程計的原理是利用掃描點建立距離流約束方程。若將機器人放置于長走廊中，激光輪廓scan_match匹配機制就會失效，導致機器人無法精確估計走廊方向的位姿變化。但輪式里程計對環(huán)境的依賴程度較小，即便在長走廊環(huán)境下也可對機器人位姿進行估計。因此，在里程計方案中引入輪式里程計[4]，可提高里程計的準確性和魯棒性。輪式里程計由底盤的運動學模型和每個編碼器數值計算得到。底盤模型如圖3所示。

圖3 底盤模型圖

2.2.3 輪式里程計標定

理想情況下，激光雷達里程計所估計出的機器人運動狀態(tài)[vLωL]與輪式里程計計算的機器人運動狀態(tài)[vω]應相等，則有：

(1)

式中：T為采樣周期；ni為圖3中第i號(i=1,2,3,4)輪子編碼器采樣結果；γ為編碼器每米脈沖數；μ為原地旋轉1周的機器人左右2列輪子編碼器累計值差值的絕對值。

假設雷達里程計估計姿態(tài)服從高斯分布，則以最小二乘法估計參數γ和μ，以提高里程計計算精度。

2.2.4 擴展卡爾曼濾波

由前文分析可知，激光雷達里程計與輪式里程計可以進行優(yōu)勢互補。輪式里程計本身容易存在較大的累積誤差，若標定精度高，則里程計定位精度也會得到大幅提高。本文設計利用擴展卡爾曼濾波對激光雷達里程計、輪式里程計和IMU這3種檢測信息進行融合[5]。卡爾曼濾波器是1種狀態(tài)最優(yōu)估計算法，采用卡爾曼增益在預測值的基礎上進行修正。

robot_localization功能包中的擴展卡爾曼濾波節(jié)點以式(2)中的15維狀態(tài)向量表示機器人運動狀態(tài)：

(2)

在功能包的配置文件中，可自定義每個被融合信息的輸入。融合節(jié)點根據配置文件從激光雷達里程計、輪式里程計和IMU中選擇相應數據，對擴展卡爾曼濾波預測值進行校正，以實現里程計融合。

2.2.5 AMCL全局定位

導航任務的實現離不開精準的機器人位姿估計。AMCL是移動機器人在二維環(huán)境中的概率定位系統[6]，采用粒子濾波器跟蹤已知的機器人位姿，根據粒子的分布估計機器人位姿的概率分布，被廣泛應用于解決機器人的全局定位問題。AMCL是在蒙特卡羅定位算法的基礎上改進而成的，解決了機器人綁架問題和粒子冗余問題。在本文設計中：AMCL在機器人導航階段提供了Map與Odom坐標系之間的坐標變換關系；擴展卡爾曼濾波里程計融合器提供了Odom與Base_link之間的坐標變換。在理想情況下，Map坐標系與Odom坐標系時刻保持重合，即里程計與真實值之間無誤差。在實際情況下，外部環(huán)境及傳感器自身存在誤差，因此里程計信息往往與真實值之間存在誤差。AMCL提供了Map與Odom之間的坐標變換，進一步提高了定位精度，為本文設計的導航功能提供了前提和保障。

2.3 地圖構建系統

與Hector_slam算法和Cartographer算法對比，Gmapping算法對激光雷達掃描頻率要求更低，且在中小型地圖中有著更高的準確度。因此，本文設計采用Gmapping算法構建二維柵格地圖。

在同步定位與地圖構建(simulteneaus localrzation and mapping,SLAM)時需要解決2個問題，分別是機器人的定位，以及對地圖的特征評估。基于Rao-Blackwellized粒子濾波(Rao-Blackwellized particle filters，RBPF)的SLAM方案先解決機器人定位問題，再根據機器人定位解決建圖問題。本文設計使用的SLAM方案是基于RBPF的Gmapping算法。該算法基于RBPF，在定位部分完成了2個改進，分別為改進提議分布和選擇性重采樣[7]。

粒子濾波(particle filters，PF)原理在Gmapping中用于估計軌跡的后驗概率。1個粒子需要保存所有歷史時刻的機器人位姿和整個地圖信息，對機器人位姿有較高要求，并且頻繁的重采樣會導致粒子耗竭。若機器人的里程計誤差較大，那么提議分布與目標分布誤差也會較大，將需要更多的粒子才能更好地估計機器人位姿。這會占用更多的內存。RBPF僅依靠里程計數據計算提議分布(位姿的概率分布)。Gmapping利用激光雷達最近1幀的觀測值將提議分布限制在1個有效的狹窄區(qū)域，得到改進提議分布，再對改進提議分布進行采樣。這樣可以使用更少的粒子覆蓋機器人位姿的概率分布。同時，Gmapping算法通過選擇性重采樣的方法來減少重采樣次數。在重采樣步驟前增加一步判斷，引入式(3)計算粒子之間的權重差距：

(3)

式中：Neff為權重分散度量值；W(i)為單個粒子權重。

Neff的值越大，表示粒子之間權重差距越小，有越多的粒子能反應真實值，粒子也越集中。當Neff小于設定閾值時，意味著較少的粒子能反映真實值，此時進行重采樣可有效減少重采樣次數、避免粒子耗竭。Gmapping在完成上述2種改進后，則更適合在中小型地圖場景下解決SLAM問題。

2.4 自主導航系統

無人駕駛系統在獲得目的地信息后，首先經過全局路徑規(guī)劃器規(guī)劃出1條大致可行的路線，而后調用局部路徑規(guī)劃器根據這條全局路線和局部代價地圖信息在局部作出運動策略的調整。由于消殺任務的不同，導致需要使用不同的全局路徑規(guī)劃器。在定點消殺任務中使用A*全局路徑規(guī)劃算法能保證規(guī)劃路徑較短，在區(qū)域消殺任務中需要使路徑盡可能覆蓋指定區(qū)域。因此，本文選用全覆蓋全局路徑規(guī)劃(complete coverage path planning，CCPP)算法。

A*算法的代價函數式為：

F=H+G

(4)

式中：H為從當前節(jié)點到目標節(jié)點的代價；G為從開始節(jié)點到當前節(jié)點的代價。

A*算法通過尋找父節(jié)點，以代價函數式計算待檢查列表中節(jié)點的代價，再作代價比較并尋找新的父節(jié)點，不斷維護待檢查列表與已檢查列表，直至父節(jié)點為目標點。所有父節(jié)點相連即為所規(guī)劃路徑[8]。

局部路徑規(guī)劃對系統能否安全且準確到達目標點有著重要的影響。當局部代價地圖中出現障礙物時，局部路徑規(guī)劃器可作出相應的調整，以實現動態(tài)避障。此設計采用的局部路徑規(guī)劃器是動態(tài)窗口法(dynamic window approach，DWA)。

DWA流程如圖4所示。

圖4 DWA流程圖

DWA實現的大致步驟為速度采樣、軌跡采樣和評價[9]。在速度采樣的過程中，對速度的取樣范圍進行了限制。假設在初始時刻的車體線速度為v0，在參數配置中設置最大加速度為am，則Δt后速度的范圍為v0±amΔt。同理可得角速度的范圍為ω0±αmΔt。在所設定的范圍內完成取樣后，組合不同的線速度和角速度，可以得到不同的運動狀態(tài)。軌跡采樣根據當前速度采樣結果對車體的軌跡進行延伸。評價函數用于對得到的多組軌跡進行評價。若目標點為可達目標點，則選擇分數最高的軌跡，發(fā)布此條軌跡速度信息，從而實現對車體的控制。

在A*全局路徑規(guī)劃器與DWA局部路徑規(guī)劃器的配合下，機器人可完成定點導航的功能。但實際應用時，消殺機器人需定時對環(huán)境進行消殺，或對環(huán)境進行循環(huán)消殺。為了盡可能地覆蓋消殺區(qū)域，需要使用CCPP算法規(guī)劃全局路徑。因此，在調用全局路徑規(guī)劃器時需先判斷當前任務為定點消殺還是區(qū)域消殺，再根據任務需求選擇A*全局路徑規(guī)劃器或CCPP。路徑規(guī)劃流程如圖5所示。

圖5 路徑規(guī)劃流程圖

為解決全覆蓋路徑問題，眾多學者采用了隨機覆蓋法，即讓機器人在環(huán)境中隨意移動，檢測到障礙物后旋轉一定角度并繼續(xù)移動，由此循環(huán)。雖然該方法可行，但覆蓋率低，難以滿足消殺機器人全覆蓋的需求。此外，該方法耗時過長，無法保證消殺效率。基于激勵神經網絡的全覆蓋路徑規(guī)劃算法可用于較為復雜的環(huán)境。但該算法會出現路徑過長、重復率高、轉彎次數多且容易陷入死區(qū)的問題。移動規(guī)則法可通過對機器人移動方向加以引導，使得全覆蓋路徑重復率和轉彎次數降低，在路徑陷入死區(qū)后使用A*算法尋找最短路徑逃離死區(qū)[10]。因此，移動規(guī)則法的全覆蓋路徑規(guī)劃算法更適用于消殺機器人的全覆蓋消殺任務。

2.5 藥液噴灑系統

藥液噴灑系統由藥液噴灑裝置與水泵控制系統組成。噴灑裝置由可調速水泵、霍爾流量計、射流器和霧化噴頭組成[11]。該系統可將純消毒液和水以一定速度吸出，流經霍爾流量計后在射流器充分混合，在霧化噴頭處霧化噴出。

噴灑裝置結構如圖6所示。

圖6 噴灑裝置結構圖

在不同場景下，消毒液濃度不同。可在上位機調度系統中對機器人的消毒液濃度進行選擇，以適應不同的場景。由于水泵控制系統對快速性要求不高，因此可采用閉環(huán)控制系統。水泵控制系統結構如圖7所示。

圖7 水泵控制系統結構圖

根據圖2所示的調度軟件界面，可在右側調節(jié)區(qū)域中消毒液濃度欄中設定機器人的期望消毒液濃度。調度系統將與局域網下對應編號的機器人進行Socket通信，將期望消毒液濃度發(fā)送至機器人端。STM32單片機接收來自調度系統的控制信息，使用可調速吸泵將藥液箱中的藥品和水以一定流速吸出。混合后的液體可經過霧化噴頭噴出，從而完成消毒液濃度的遠程調節(jié)。

3 系統創(chuàng)新點

本文所設計的基于ROS的無人消殺機器人系統具有以下創(chuàng)新點。

①本文系統地設計無人消殺任務的解決方案，利用上位機調度軟件、通過Socket通信來遠程操控和觀察機器人，提高了消殺任務的效率。

②本設計將SLAM技術與消殺任務相結合，減少了消殺作業(yè)中的人工參與，節(jié)約了人工成本，保障了工作人員的安全。

③本設計通過上位機調度軟件與機載電腦的遠程通信，調整了消毒液濃度，以適應不同環(huán)境。

④本設計根據不同任務類型選用不同全局路徑規(guī)劃器，提高了系統靈活性，保證了消殺區(qū)域覆蓋率。

⑤本設計設置了定時消殺，可以按要求進行按時消殺。

4 結論

本文設計的基于ROS和SLAM的地面無人消殺機器人系統，將SLAM與消殺任務有機結合，利用上位機調度軟件對機器人的參數進行設置，提高了運作效率，減少了人工參與。SLAM方案采用了基于PF的Gmapping算法，提高了精度，為精準導航提供了前提。在里程計設計方面，本文采用了擴展卡爾曼濾波對激光雷達里程計、輪式里程計和IMU數據進行了融合，并利用自適應蒙特卡洛定位為機器人提供全局定位，有效保障了機器人的定位精度。在點到點任務中，機器人的全局路徑規(guī)劃算法采用A*算法，在區(qū)域消殺任務中使用了全覆蓋路徑規(guī)劃算法。局部路徑規(guī)劃算法采用了DWA，可靠地提供了機器人的任務路徑，保障了消殺任務的覆蓋率。藥液噴灑裝置由STM32單片機進行PID閉環(huán)控制，保證了消毒液濃度。機器人與調度軟件的協調運作提高了消毒殺菌任務的效率和消殺的覆蓋率，保障了人員安全。