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

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

0 引言

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

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

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

1 總體方案

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

圖1 系統(tǒng)總體框架

2 硬件設(shè)計

2.1 機(jī)器人底盤主控芯片

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

2.2 電機(jī)驅(qū)動電路

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

圖2 BTN電機(jī)驅(qū)動電路

3 構(gòu)建地圖

3.1 Gmapping

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

圖3 Gmapping構(gòu)建地圖的數(shù)據(jù)流程

圖4 構(gòu)建出來的室內(nèi)二維地圖

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

由此可知，RBPF 將定位和建圖分離，即先進(jìn)行定位再進(jìn)行建圖。RBPF 存在所用粒子數(shù)多和頻繁執(zhí)行重采樣的缺點，導(dǎo)致計算量和內(nèi)存消耗變大，以及粒子退化，粒子多樣性減少。

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

當(dāng)Neff小于某個閾值，說明粒子差異性過大，進(jìn)行重采樣；否則，不進(jìn)行。

3.2 Laser_scan_matcher

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

4 路徑規(guī)劃

4.1 Move_base

基于Gmapping 算法構(gòu)建好的地圖基礎(chǔ)上， 采用Move_base[10]開源算法包實現(xiàn)路徑規(guī)劃，圖5 所示為ROS 提供的該包框架，主要包括global_planner和local_planner，一個負(fù)責(zé)全局最優(yōu)路徑的搜索查找，一個負(fù)責(zé)機(jī)器人每個周期的速度控制規(guī)劃，分別用于實現(xiàn)全局規(guī)劃和本地規(guī)劃。

圖5 Move_base框架

4.2 全局規(guī)劃

global_planner主要包括Dijkstra[11-12]和A*，本文采用Dijks?tra最優(yōu)路徑算法，實現(xiàn)機(jī)器人的全局路徑規(guī)劃，找出機(jī)器人到達(dá)目的地的最短路徑。

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

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

4.3 本地規(guī)劃

Local_planner 主要包括DWA 和TEB[13]，本文在全局規(guī)劃好的路徑基礎(chǔ)上，采用TEB 算法對機(jī)器人附近的障礙物進(jìn)行躲避，實現(xiàn)本地路線規(guī)劃。如圖6所示，運(yùn)用該算法搜索到達(dá)目標(biāo)的多條路徑，并且通過評價是否撞擊障礙物等風(fēng)險選出最優(yōu)路徑。

圖6 TEB規(guī)劃出多條路徑

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

圖7 路徑風(fēng)險評價

圖8 轉(zhuǎn)向成本

通過全局路徑規(guī)劃之后，接著通過本地路徑規(guī)劃在全局路線上規(guī)劃出避開障礙物的最優(yōu)路線，進(jìn)一步對路線進(jìn)行更改，得出完整的路徑再發(fā)布給機(jī)器人。

5 機(jī)器人控制

通過計算圖9 所示兩點之間的偏差將直角坐標(biāo)轉(zhuǎn)換成極坐標(biāo)：

圖9 機(jī)器人定位導(dǎo)航示意圖

其中θ為目標(biāo)點相對機(jī)器人的偏角，即機(jī)器人到達(dá)目標(biāo)點的方向偏差，可通過PID 控制算法，對機(jī)器人的方向環(huán)進(jìn)行控制，消除偏差，使機(jī)器人精準(zhǔn)地對準(zhǔn)目標(biāo)點行駛。本文采用增量式PI算法控制：

式中：kP為比例系數(shù)；kI為積分系數(shù)；分別為當(dāng)前偏差、上次偏差。

機(jī)器人在向目標(biāo)點行駛的過程中，系統(tǒng)會不斷地更新誤差，調(diào)整修正，使車頭朝向目標(biāo)點行進(jìn)，當(dāng)與目標(biāo)點的距離ρ=0時，即到達(dá)目的地。

6 調(diào)試評估

圖10 所示為本系統(tǒng)配套的電腦端上位機(jī)，是基于QT[14]采用C ++ 開發(fā)的，用戶可以在這里點擊按鈕實現(xiàn)控制機(jī)器人、機(jī)器人構(gòu)建地圖、發(fā)布目標(biāo)點等功能。點擊相關(guān)按鈕會運(yùn)行相應(yīng)的Linux 命令行，啟動ROS 工程下的Launch 文件，以啟動相應(yīng)的節(jié)點，比如構(gòu)建地圖、自主導(dǎo)航等，避免了輸入命令行才能啟動機(jī)器人的麻煩。

圖10 上位機(jī)界面

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

圖11 規(guī)劃的運(yùn)動軌跡

圖12 向目標(biāo)位置移動

7 結(jié)束語

本文根據(jù)Gmapping 開源算法、PID 控制算法等原理設(shè)計室內(nèi)導(dǎo)盲機(jī)器人，闡述了硬件電路工作原理、Gmapping 算法處理過程、ROS的工作原理、PID自動控制算法、機(jī)器人控制流程、上位機(jī)設(shè)計以及機(jī)器人機(jī)械結(jié)構(gòu)設(shè)計。經(jīng)過機(jī)器人的改裝，硬件制作，軟件算法調(diào)試，基本實現(xiàn)了構(gòu)建地圖，定位導(dǎo)航，引導(dǎo)盲人等功能。