室內導盲機器人的設計與實現*

周凱凌，顏禧烽，張建民，陳健恒，譚澤輝，李潮平，龍佳樂

（五邑大學智能制造學部， 廣東江門 529020）

0 引言

盲人由于失去了光明，在生活起居上特別的不方便。為了解決盲人生活上的障礙，人們訓練了導盲犬來做盲人忠實的朋友，陪伴著盲人。盡管導盲犬給盲人帶來很大的便利，但是導盲犬并不是所有場合都能使用，比如一些餐廳、會展廳等特殊公共場合。本文研究和設計了一臺可在室內導航定位的機器人，用于在室內引導盲人[1]。

國際導盲機器人主要有智能拐杖式和移動式。2010年日本曾推出過一款電子導盲拐杖，該導盲拐杖通過超聲波傳感器檢測障礙物，能夠讓用戶感受到與頭部同一平面的障礙物，拐杖通過振動提醒用戶。日本山梨大學曾研制出一款移動式導盲機器犬，不僅有效嫁接了導盲犬的各種功能，在培養和使用成本上也更為低廉，為導盲機器人研究提供了重要成果展示。可見，無論國內外，專家學者、科研團隊們都致力于這方面的開發。然而在現階段的技術研發還不算很成熟，仍然有著很大的欠缺，還不能完全投入日常中使用，在導盲機器人的研究中仍需付出很大的努力。移動式導盲機器人采用機器人先行、盲人跟隨的導盲方式，能大大降低盲人受傷的概率。本文針對移動式導盲機器人，研究和設計可適用于室內的導盲機器人。

該機器人處于室內工作，為了應對有可能出現的室內工作環境狹小的情況，機器人配備4個麥克納姆輪的小車底盤作為機器人的底盤，能進行全方向的運動，使機器人運動更加靈活。機器人具有即時定位與地圖構建（Simultaneous Local?ization And Mapping，SLAM）功能，該機器人采用機器人操作系統(Robot Operating System，ROS) 提供的Gmapping開源軟件包實現機器人的地圖構建和定位[2]，通過PID算法對機器人進行運動控制。

1 總體方案

圖1 所示為該室內導盲機器人系統總體框架圖。利用32位單片機STM32F103RCT6作為機器人底盤的主控芯片。機器人底盤具有4 個麥克納姆輪，可以提供機器人三自由度的運動。STM32F103RCT6 通過BTN 電機驅動板驅動機器人底盤的4 個電機[3]。機器人上還有一塊Mini PC——NVIDIA Jetson Nano，通過USB 獲取激光雷達的數據。STM32F103RCT6 將獲取到的里程計數據通過USB_to_TTL 模塊發送給NVIDIA Jetson Nano，NVIDIA Jetson Nano 上面運行著ROS，通過局域網回傳到PC 端，在PC 端進行SLAM 相關算法的處理。同時，PC 端發出的控制指令也是通過局域網發送回NVIDIA Jetson Nano，再經過USB_to_TTL 模塊發送給STM32F103RCT6 進行機器人的運動控制，實現構建地圖，定位導航等功能。整個機器人由鋰電池組提供電能。

圖1 系統總體框架

2 硬件設計

2.1 機器人底盤主控芯片

該機器人底盤的主控芯片采用意法半導體的32位單片機STM32F103RCT6。該芯片價格便宜，性價比高，工作主頻可達72 MHz，有豐富的引腳和內部資源。其支持CAN、I2C、IrDA、LIN、SPI、UART/USART、USB等通信總線，可滿足單片機與Mini PC 的串口間通信需求。具有8 個定時計數模塊，可提供機器人電機控制、編碼器的脈沖采集等功能。

2.2 電機驅動電路

導航小車底層控制涵蓋電機驅動、速度測量、電機控制系統[4]，其中驅動電路選用BTN方案，BTN系列芯片具備自動保護功能，在檢測到過壓、過流、過溫等致命故障時，可自動關閉或調整內部開關管，強行停止或限流運行。由于智能車電池電壓遠低于BTN 系列最高承受電壓，不會出現過壓故障（過溫問題比較常見）。電機驅動電路原理如圖2 所示，INH為BTN驅動芯片的使能端，其可與單片機I/0口相連，并可配合IS 電流檢測與故障診斷端實施故障保護。在檢測到故障信號時，使能端復位，系統停止工作，從而起到保護作用。IN引腳為信號輸入的引腳，該引腳連接到單片機的PWM信號輸出的IO 口，通過PWM 信號來改變BTN 的通斷頻率。在IN 引腳還加入一個1 kΩ的電阻，防止沒有信號輸入的時候，IN 處于懸空的狀態，這時引腳電平高低不定，有可能開啟BTN 導致電機瘋轉。

圖2 BTN電機驅動電路

3 構建地圖

3.1 Gmapping

Gmapping[5]是基于濾波SLAM 框架的常用開源SLAM 算法，可以實時構建室內地圖，在構建小場景地圖時所需的計算量較小且精度較高。ROS 提供了該開源算法。Gmapping 要獲取機器人的兩個數據——激光雷達的數據和里程計數據。如圖3 所示，Scan 是激光雷達的數據，Odom 是里程計數據，兩個數據投入到Gmapping 中即可得到室內的二維地圖，圖4所示為使用Gmapping構建出來的室內二維地圖。

圖3 Gmapping構建地圖的數據流程

圖4 構建出來的室內二維地圖

Gmapping是基于RBPF粒子濾波的算法[6-7]，RBPF是一種有效解決同時定位和建圖的算法。根據貝葉斯法則有：

由此可知，RBPF 將定位和建圖分離，即先進行定位再進行建圖。RBPF 存在所用粒子數多和頻繁執行重采樣的缺點，導致計算量和內存消耗變大，以及粒子退化，粒子多樣性減少。

Giorgio Grisetti、Cyrill Stachniss、Wolfram Burgard 等人提出改進提議分布和自適應重采樣，從而減少粒子個數和防止粒子退化。激光雷達的匹配比里程計的測量精度高很多，匹配的方差也小很多，若粒子濾波采用激光雷達匹配作為提議分布，將得到一個相對集中的分布，就可以把采樣范圍限制在一個比較小的區域，粒子的數量得以降低。而緩解粒子耗散通過減少重采樣次數實現，直接通過下式評估所有粒子權重的分散程度來決定是否進行粒子重采樣[8-9]：

當Neff小于某個閾值，說明粒子差異性過大，進行重采樣；否則，不進行。

3.2 Laser_scan_matcher

如果里程計的精度不高， 可以使用ROS 自帶的La?ser_scan_matcher，其是一個增量激光掃描注冊工具。該軟件包允許掃描連續傳感器/激光掃描信息之間的匹配，并將激光的估計位置發布為tf 變換。即該開源軟件包可以對機器人的位置進行估算，從而取消對里程計的依賴。其還集成好了Gmapping，使用該包可以不需要里程計也可以使用Gmapping進行建圖。

4 路徑規劃

4.1 Move_base

基于Gmapping 算法構建好的地圖基礎上， 采用Move_base[10]開源算法包實現路徑規劃，圖5 所示為ROS 提供的該包框架，主要包括global_planner和local_planner，一個負責全局最優路徑的搜索查找，一個負責機器人每個周期的速度控制規劃，分別用于實現全局規劃和本地規劃。

圖5 Move_base框架

4.2 全局規劃

global_planner主要包括Dijkstra[11-12]和A*，本文采用Dijks?tra最優路徑算法，實現機器人的全局路徑規劃，找出機器人到達目的地的最短路徑。

Dijkstra是基于貪婪搜索的最短路徑算法，解決的是帶權重的有向圖上單源最短路徑問題。算法的偽代碼如下：

集合S保存所有已知實際最短路徑值的頂點，集合Q=V-S保存其他所有頂點。當集合Q不為空集時，找出Q中最短路徑估計最小的結點u，放進集合S中并從集合Q中刪除，再對所有從結點u發出的邊進行松弛操作，若一條經過結點u的路徑能夠使得從源結點s到結點v的最短路徑權重比當前的估計值更小，則更新u、d等相關的值。經過V次循環后，獲得最短路徑的值d。

4.3 本地規劃

Local_planner 主要包括DWA 和TEB[13]，本文在全局規劃好的路徑基礎上，采用TEB 算法對機器人附近的障礙物進行躲避，實現本地路線規劃。如圖6所示，運用該算法搜索到達目標的多條路徑，并且通過評價是否撞擊障礙物等風險選出最優路徑。

圖6 TEB規劃出多條路徑

如圖7 所示，該算法通過計算小車相對障礙的距離來估算對小車撞擊障礙物的風險度，從而判斷機器人需要投入轉向的成本，即機器人轉向的權重。這里允許到障礙物的最小歐幾里德距離設置為0.2 m。當這個距離小于0.2 m 時，隨著距離的接近，機器人投入的轉向成本隨之增高。如圖8所示，機器人在本地規劃中，根據期望的時間分辨率（參數dt）來布置每兩個連續的姿勢。每個時間周期內（時長dt）判斷機器人相對障礙物的距離，來計算轉向成本。

圖7 路徑風險評價

圖8 轉向成本

通過全局路徑規劃之后，接著通過本地路徑規劃在全局路線上規劃出避開障礙物的最優路線，進一步對路線進行更改，得出完整的路徑再發布給機器人。

5 機器人控制

通過計算圖9 所示兩點之間的偏差將直角坐標轉換成極坐標：

圖9 機器人定位導航示意圖

其中θ為目標點相對機器人的偏角，即機器人到達目標點的方向偏差，可通過PID 控制算法，對機器人的方向環進行控制，消除偏差，使機器人精準地對準目標點行駛。本文采用增量式PI算法控制：

式中：kP為比例系數；kI為積分系數；分別為當前偏差、上次偏差。

機器人在向目標點行駛的過程中，系統會不斷地更新誤差，調整修正，使車頭朝向目標點行進，當與目標點的距離ρ=0時，即到達目的地。

6 調試評估

圖10 所示為本系統配套的電腦端上位機，是基于QT[14]采用C ++ 開發的，用戶可以在這里點擊按鈕實現控制機器人、機器人構建地圖、發布目標點等功能。點擊相關按鈕會運行相應的Linux 命令行，啟動ROS 工程下的Launch 文件，以啟動相應的節點，比如構建地圖、自主導航等，避免了輸入命令行才能啟動機器人的麻煩。

圖10 上位機界面

RVIZ[15]是ROS 自帶的一個圖形化工具，功能強大，可以實現對建立地圖以及導航路徑等的動態觀測。點擊上位機的按鈕啟動Launch文件：將激光雷達和里程計采集到的數據投入到Gmpping建立2D地圖，并采用Dijkstra和TEB算法實現路徑規劃。將建立的2D地圖導入到RVIZ中，在RVIZ中用鼠標點擊目標位置，如圖11 所示，界面中規劃出小車運動的軌跡。如圖12所示，小車沿著規劃的路徑向目標位置靠近。

圖11 規劃的運動軌跡

圖12 向目標位置移動

7 結束語

本文根據Gmapping 開源算法、PID 控制算法等原理設計室內導盲機器人，闡述了硬件電路工作原理、Gmapping 算法處理過程、ROS的工作原理、PID自動控制算法、機器人控制流程、上位機設計以及機器人機械結構設計。經過機器人的改裝，硬件制作，軟件算法調試，基本實現了構建地圖，定位導航，引導盲人等功能。