護士助手機器人多傳感器數據融合定位導航系統研究

陳再勝，王顏霞

（商丘工學院機械工程學院，河南 商丘 476000）

隨著傳感器技術和計算機技術的不斷發展，傳統的單一傳感器信息采集和多傳感器單獨信息采集已不能滿足現階段高精度、高穩定性的機器人市場需求，尤其是在醫院室內移動的護士助手機器人[1]。采用計算機算法將多種傳感器數據進行融合獲取機器人位姿，并實現定位導航，經過多學者[2-3]研究，能相對提高精度，并不斷應用于移動機器人、智能汽車、自動駕駛等領域。

本文在前人[4-5]的基礎上，根據項目條件，研究了一種多傳感器數據融合定位導航系統，主要應用于室內移動的護士助手機器人，其承載能力30kg，最大移動速度υmax為0.6m/s，可實現自主導航、避障、充電和物品運送等[6]。該定位導航系統采用輪式里程計和慣性測量單元IMU 通過EKF 算法實現機器人較為粗糙的實時位姿和速度，采用激光雷達通過ROS 服務包構建環境地圖，采用單目相機校準機器人實時位姿，通過自適應蒙特卡洛定位（AMCL）算法實現機器人精準定位及導航。本定位導航系統為護士助手機器人等室內環境下移動的機器人實現高精度定位導航提供了一種技術方案。

1 定位導航系統設計

本護士助手機器人以ROS 開源系統為基礎，樹莓派為上位機，向下位機分發節點命令，并接收下位機獲取的位姿信息，Arduino 控制板控制底層控制系統，慣性測量單元IMU 和輪式里程計作為內部傳感器，激光雷達和單目相機作為外部傳感器，配合上位機完成機器人SLAM 和路徑規劃系統的實現。護士助手機器人定位系統主要包括以下幾個方面。

1.1 底盤控制系統

該控制系統主要用于控制機器人的運動速度。首先，接收ROS速度命令，驅動電機按初始速度旋轉；其次，控制器接收ROS 速度或角度命令，控制電機基本運動。同時，在該ROS 功能包中讀取電機的運行速度，將其轉換為行駛里程數據，并反饋到樹莓派上位機。

1.2 SLAM（即時定位和地圖構建）系統

本系統主要利用ROS 框架中的SLAM 功能包實現。首先，激光雷達通過360°掃描可檢測到周圍環境障礙物的距離信息；其次，樹莓派通過該功能包將接收到的里程計數據、慣性傳感器數據和雷達數據進行融合處理，從而創建機器人實時位姿和室內環境圖。

1.3 路徑規劃系統

首先，操作者通過可視化界面規定機器人目標點，輸入至樹莓派控制中心；其次，機器人根據給定的目標點，在SLAM 全局地圖中，通過A*算法進行全局路徑的規劃，計算出機器人到目標點的最優路線；對于障礙物，通過局部路徑規劃算法和模糊控制理論選取最優避障路徑，避免與動態出現的障礙物發生碰撞，最終使機器人到達目標點。

2 SLAM 系統實現

2.1 多傳感器數據融合即時定位

本護士助手機器人通過AMCL 算法獲得機器人的實時位置和姿態，并將單目相機獲得的初始位姿與之加權平均獲得機器人即時位姿。

AMCL 算法是一種具有較高魯棒性的護士助手機器人定位算法，其核心算法是基于概率模型的粒子濾波算法。首先，底盤控制系統中的輪式里程計作為內部傳感器，通過航跡推算法基于光電編碼器的脈沖變化量推算出機器人的位姿、角/線速度實時變化數據，慣性測量單元IMU 采用慣性傳感器MPU6050 測算出機器人的角/線速度實時變化數據，兩種數據通過EKF 算法推算出機器人實時位置的坐標和姿態數據，并與機器人基坐標系進行坐標變換后得到機器人即時位姿數據A；激光雷達作為外部傳感器，通過slam_gmapping地圖包構建全局地圖，獲得機器人周圍障礙物的距離數據B；單目相機作為外部傳感器，獲得室內環境初始距離圖像和機器人運行過程中的實時距離圖像，經過圖像處理和位姿估計后得到機器人位姿數據信息C；數據A、數據B 和數據C 通過AMCL 算法創建概率模型，并經測算后得到機器人的即時位姿，同時與單目相機獲得的位姿數據進行加權平均，最后獲得較為準確的機器人即時位姿信息。具體數據融合流程如圖1 所示。

圖1 多傳感器融合流程圖

2.2 SLAM 算法實時地圖構建

本護士助手機器人選擇粒子濾波的SLAM方法（Fast-SLAM）完成地圖構建。Fast-SLAM 算法利用擴展卡爾曼濾波算法（EKF）進行狀態估計，利用粒子濾波算法（PF）進行機器人的位姿估計。若已知機器人的路徑，且各個地標點的位置估計是相互獨立的，則機器人路徑信息和環境路標信息估計模型為：

FastSLAM 算法的基本步驟可以分為以下幾步：

Step 4.計算重要性權值：為新粒子計算其重要性權值ωk。

Step 5.重采樣：用同樣數目的粒子將原來N 個粒子替換，并且對每個粒子按照其對應的重要性權值ωk進行采樣。具體流程如圖2 所示。

圖2 Fast-SLAM 基本流程

3 系統測試與分析

3.1 地圖構建

由于醫院無法實地檢測，為測試本系統的構圖性能，選擇在學校教室和教室走廊兩種環境下，模擬醫院長走廊和多房間的環境，進行實時構建地圖。在教室環境下構建的實時地圖，地圖構建基本符合實際場景。在教室環境下，機器人地圖構建系統跟隨機器人不斷變化，采用綠色邊線表示障礙物，此種障礙物具有兩種特點：一是高度超過了激光雷達掃描的最高高度；二是豎直方向上為平面，即可以遮擋住激光雷達在豎直方向上的掃描光線。而當遇到障礙物豎直方向上不為平面時，激光雷達的掃描光線可以通過該障礙物，掃描到其他位置，中部分地圖邊線明顯，而其他地圖邊線不明顯，甚至有射線射出的情況。同時，我們也檢測了在某些帶有玻璃門的教室環境下地圖構建情況，當機器人經過玻璃門時，激光雷達的射線可以穿透玻璃門，導致環境地圖構建失敗，地圖邊線不存在或者不突出。

機器人在行走過程中在教室長走廊環境下構建的實時地圖，地圖邊線明顯的部分即為墻體，而地圖邊線不明顯，有射線射出的部分表示教室門是開著的，同時，如果是帶有條狀玻璃的教室門或者通體玻璃門，情況也類似。

該測試結果，一方面證明了本文提出的多傳感器數據融合方法地圖構建的有效性，同時也反映出激光雷達在地圖構建方面的缺陷，因此可以考慮增加視覺系統，提高地圖構建的可靠性；另一方面也說明在障礙越明顯的地方，機器人系統構建的地圖越清晰。這里的障礙物的明顯性，比如墻體相對于桌子障礙的明顯性就更突出，激光雷達掃描射線的反射率就越高。

3.2 實時定位

為測試本系統的實時定位功能，選擇重復到點定位精度作為測試指標，實驗過程中先記錄一下機器人到指定位置的坐標值，作為初始位置，并記作0 點，然后讓機器人根據規劃路線離開初始位置0 點，之后再回到初始位置0 點，重復10 次，分別記錄各次機器人的x，y，θ方向的誤差，并計算10 次的平均誤差，作為精度值，最終測試結果見表1。需要指出的是，該機器人在該規劃路線行走的過程中有2 張帶有靠背的椅子和1 張豎直方向為平面的桌子作為障礙物，且椅子和桌子的最高高度均超過機器人的最高高度。由表1可見，本定位導航系統精度較高，效果較理想。并且在機器人按照規劃路線運行的過程中可以很好地檢測到障礙物，并繞過障礙物繼續行進，直至到達測試終點，說明該機器人多傳感器數據融合定位導航系統實時定位功能具有較為可靠的穩定性，能實現較為有效的路徑規劃和自主導航。

表1 本系統定位精度

4 結語

本文介紹了一種應用于室內移動護士助手機器人的多傳感器數據融合定位導航系統，采用里程計和慣性測量單元IMU 作為內部傳感器，激光雷達和單目相機作為外部傳感器，內部傳感器通過EKF 算法進行機器人的即時位姿估計，激光雷達測算出機器人環境距離，并構建環境地圖，單目相機拍攝的圖像經過數據處理得到機器人的實時位姿，最后通過AMCL 算法進行機器人實時定位。經過系統測試，該系統具有較為良好的定位精度，以及較為有效的地圖構建系統，在室內移動機器人定位導航技術方面有一定的借鑒價值。