顧皞偉,黃家才,李毅搏,張 鐸,陳 田,呂思男,唐 安,湯文俊,汪 濤

(南京工程學(xué)院工業(yè)中心、創(chuàng)新創(chuàng)業(yè)學(xué)院, 江蘇 南京 211167)

隨著傳感器技術(shù)與電子信息技術(shù)的飛速發(fā)展,移動(dòng)機(jī)器人發(fā)展迅速、應(yīng)用廣泛,市場(chǎng)上出現(xiàn)了多種移動(dòng)機(jī)器人產(chǎn)品,如掃地機(jī)器人、送餐機(jī)器人、服務(wù)機(jī)器人等.自主導(dǎo)航功能是移動(dòng)機(jī)器人的核心功能[1],而傳感器則是導(dǎo)航系統(tǒng)中的重要組成部分.文獻(xiàn)[2]提出一種基于機(jī)器人操作系統(tǒng)(ROS)的移動(dòng)機(jī)器人導(dǎo)航控制系統(tǒng),該控制系統(tǒng)集成化程度高、可移植性好,但僅采用編碼器作為傳感器來(lái)獲取機(jī)器人的位置信息,在地面打滑的情況下,由編碼器推算出的里程計(jì)數(shù)值將極不準(zhǔn)確,導(dǎo)航精度將會(huì)大大降低;文獻(xiàn)[3]提出一種基于激光的同步定位與建圖(SLAM)移動(dòng)機(jī)器人室內(nèi)巡檢技術(shù),該技術(shù)精度高、穩(wěn)定性好,但采用單一激光雷達(dá)傳感器進(jìn)行環(huán)境感知,在遇到玻璃等透明物體時(shí),激光雷達(dá)將會(huì)失效,安全性低.本文設(shè)計(jì)一種基于多傳感器感知的移動(dòng)機(jī)器人導(dǎo)航系統(tǒng),系統(tǒng)具有兩輪差速輪式底盤(pán),配以激光雷達(dá)、慣性傳感器(IMU)、超聲波雷達(dá)和編碼器多個(gè)傳感器,可有效克服因地面打滑而出現(xiàn)的里程失準(zhǔn)問(wèn)題,實(shí)現(xiàn)導(dǎo)航過(guò)程中的精準(zhǔn)定位;通過(guò)超聲波雷達(dá)補(bǔ)償激光雷達(dá)的方法,實(shí)現(xiàn)對(duì)透明物體的實(shí)時(shí)檢測(cè),提高機(jī)器人的安全性與可靠性;利用SLAM算法[4]、定位算法[5]與路徑規(guī)劃算法[6]實(shí)現(xiàn)移動(dòng)機(jī)器人的自動(dòng)導(dǎo)航功能.

1 系統(tǒng)整體設(shè)計(jì)方案

1.1 系統(tǒng)硬件架構(gòu)

系統(tǒng)的硬件設(shè)計(jì)架構(gòu)如圖1所示,以工控機(jī)為核心控制器實(shí)時(shí)接收IMU、激光雷達(dá)和超聲波雷達(dá)所采集的環(huán)境信息,利用這些信息對(duì)周?chē)h(huán)境進(jìn)行建模;在該模型的基礎(chǔ)上進(jìn)行最優(yōu)路徑規(guī)劃計(jì)算,然后將計(jì)算結(jié)果以速度指令的形式發(fā)送給下位機(jī),下位機(jī)運(yùn)動(dòng)控制器控制移動(dòng)機(jī)器人按照預(yù)設(shè)軌跡移動(dòng),從而實(shí)現(xiàn)移動(dòng)機(jī)器人的自主導(dǎo)航功能;工控機(jī)與運(yùn)動(dòng)控制器采用串口通信,移動(dòng)機(jī)器人自帶電機(jī)驅(qū)動(dòng)器,通過(guò)控制器局域網(wǎng)絡(luò)(CAN)指令與電機(jī)驅(qū)動(dòng)器通信,控制移動(dòng)機(jī)器人運(yùn)動(dòng),各類(lèi)傳感器與工控機(jī)采用串口通信.

圖1 系統(tǒng)硬件架構(gòu)圖

1.2 控制系統(tǒng)軟件架構(gòu)

控制系統(tǒng)的軟件主要分為兩部分:1) 基于ROS系統(tǒng)的上位機(jī)決策層;2) 基于STM32設(shè)計(jì)的下位機(jī)運(yùn)動(dòng)控制器.

上位機(jī)決策層負(fù)責(zé)采集傳感器信息,利用多傳感器信息做出科學(xué)決策.上位機(jī)采用Cartographer建圖算法[7]、A*的改進(jìn)路徑規(guī)劃算法[8]和基于蒙特卡洛的AMCL定位算法[9]為核心設(shè)計(jì)上位機(jī)軟件架構(gòu),見(jiàn)圖2.Cartographer算法利用激光雷達(dá)實(shí)時(shí)獲取環(huán)境中障礙物的信息,依靠里程計(jì)、慣導(dǎo)元件與激光雷達(dá)實(shí)現(xiàn)聯(lián)合定位,從而構(gòu)建完整、精確的室內(nèi)地圖,并在已有室內(nèi)地圖的基礎(chǔ)上進(jìn)行路徑規(guī)劃和實(shí)時(shí)定位.A*算法既能根據(jù)定位信息進(jìn)行全局路徑規(guī)劃,也能根據(jù)雷達(dá)信息進(jìn)行局部路徑規(guī)劃,不論是何種規(guī)劃方法,最終上位機(jī)都會(huì)以速度指令的形式發(fā)送給下位機(jī).基于自適應(yīng)蒙特卡洛的AMCL定位算法可實(shí)時(shí)獲取里程計(jì)、IMU和激光雷達(dá)的信息來(lái)實(shí)現(xiàn)移動(dòng)機(jī)器人的即時(shí)定位.

圖2 上位機(jī)軟件架構(gòu)圖

基于STM32的下位機(jī)運(yùn)動(dòng)控制器軟件流程如圖3所示,通過(guò)串口實(shí)現(xiàn)和上位機(jī)的通信,在實(shí)時(shí)接收上位機(jī)下發(fā)速度指令的同時(shí)上傳移動(dòng)機(jī)器人實(shí)際速度,實(shí)際速度指的是移動(dòng)機(jī)器人前進(jìn)的線(xiàn)速度與轉(zhuǎn)彎的角速度,最終都轉(zhuǎn)化成左右兩輪的轉(zhuǎn)速指令下發(fā)給電機(jī)驅(qū)動(dòng)器,通過(guò)CAN指令下發(fā)左右兩輪速度,同時(shí)通過(guò)外加的輪式編碼器實(shí)時(shí)讀取兩輪的實(shí)際轉(zhuǎn)速,再通過(guò)航跡推演將兩輪轉(zhuǎn)速轉(zhuǎn)化成移動(dòng)機(jī)器人的線(xiàn)速度和角速度,通過(guò)串口將移動(dòng)機(jī)器人實(shí)時(shí)的線(xiàn)速度和角速度上傳至上位機(jī).

圖3 下位機(jī)軟件流程圖

2 多傳感器信息互補(bǔ)

移動(dòng)機(jī)器人在導(dǎo)航的過(guò)程中極其依賴(lài)外部傳感器信息,傳感器的好壞直接決定導(dǎo)航的可靠性與穩(wěn)定性,因此要保證傳感器信息精確而全面.單一的傳感器魯棒性差,原理上也存在缺陷,通過(guò)多個(gè)傳感器相互配合,可全面、準(zhǔn)確地獲取環(huán)境信息.激光雷達(dá)傳感器雖然精度極高,但不能檢測(cè)透明物體；超聲波傳感器雖然精度低,但不受物體透明度影響；編碼器雖然能精確地測(cè)量移動(dòng)機(jī)器人兩輪轉(zhuǎn)速,但移動(dòng)機(jī)器人在旋轉(zhuǎn)運(yùn)動(dòng)過(guò)程中輪胎極易打滑,在旋轉(zhuǎn)過(guò)程中兩輪轉(zhuǎn)速并不能推算出真實(shí)的機(jī)器人旋轉(zhuǎn)速度,而IMU則可以彌補(bǔ)這一不足.因此，多個(gè)傳感器相互補(bǔ)償，能極大地提高導(dǎo)航的穩(wěn)定性.

2.1 IMU修正里程計(jì)信息

在機(jī)器人行駛過(guò)程中,機(jī)器人的旋轉(zhuǎn)角速度由兩輪差速計(jì)算得到,但機(jī)器人在轉(zhuǎn)彎時(shí)輪胎極易打滑,由兩輪差速計(jì)算得到的旋轉(zhuǎn)角度往往與真實(shí)旋轉(zhuǎn)角度相差較大.IMU主要是由加速度計(jì)、陀螺儀組成的測(cè)量裝置,陀螺儀利用內(nèi)部的電容變化得到角速度,通過(guò)對(duì)角速度積分得到旋轉(zhuǎn)角度,該角度不會(huì)因?yàn)檩喬ゴ蚧l(fā)生改變.

本文所選用的移動(dòng)機(jī)器人是兩輪差速移動(dòng)機(jī)器人,該類(lèi)移動(dòng)機(jī)器人是通過(guò)控制左右兩輪的電機(jī)轉(zhuǎn)速來(lái)實(shí)現(xiàn)移動(dòng)機(jī)器人的運(yùn)動(dòng)控制.IMU修正里程計(jì)時(shí)需建立移動(dòng)機(jī)器人的前進(jìn)速度、轉(zhuǎn)向角速度與左輪速度、右輪速度之間的轉(zhuǎn)換關(guān)系.圖4為移動(dòng)機(jī)器人底盤(pán)在兩個(gè)相鄰時(shí)刻的位姿,圖4中:θ1為兩相鄰時(shí)刻移動(dòng)機(jī)器人繞圓弧運(yùn)動(dòng)的角度;θ3為兩相鄰時(shí)刻移動(dòng)機(jī)器人航向角(朝向角head)的變化量;l為左右輪之間的間距;d為右輪比左輪多走的距離;r為移動(dòng)機(jī)器人圓弧運(yùn)動(dòng)的半徑.假設(shè)移動(dòng)機(jī)器人前進(jìn)的線(xiàn)速度為v、角速度為w、左輪速度為vl、右輪速度為vr.

圖4 航跡推演模型

根據(jù)航跡推演算法,移動(dòng)機(jī)器人前進(jìn)的線(xiàn)速度為:

v=(vr+vl)/2

(1)

移動(dòng)機(jī)器人的的前進(jìn)速度等于左右兩輪速度的平均值,由圖4可見(jiàn),移動(dòng)機(jī)器人從第一時(shí)刻到達(dá)第二時(shí)刻的航向角變化量為θ3,利用圖中的幾何關(guān)系得到θ1=θ2=θ3.

移動(dòng)機(jī)器人航向角決定其運(yùn)動(dòng)軌跡圓心旋轉(zhuǎn)角度,移動(dòng)機(jī)器人偏轉(zhuǎn)角度為:

θ3=θ1=d/l=(vr-vl)Δt/l

(2)

角速度為:

w=θ3/Δt=(vr-vl)/l

(3)

通過(guò)式(1)至式(3),能夠利用編碼器推算出里程信息.

編碼器在單位時(shí)間t0內(nèi)采集一次兩輪速度,通過(guò)式(1)計(jì)算出機(jī)器人的線(xiàn)速度為vx,機(jī)器人的角速度為vθ,則單位時(shí)間內(nèi)x方向的位移為:

Dx=vxcosθt0

(4)

y方向的位移為:

Dy=vxsinθt0

(5)

旋轉(zhuǎn)角度為:

Dθ=vθt0

(6)

里程信息可表示為:

Xt=Xt-1+Dx,Yt=Yt-1+Dy,θt=θt-1+Dθ

(7)

將IMU模塊測(cè)量出的機(jī)器人旋轉(zhuǎn)角速度vθ代入式(6),即可實(shí)現(xiàn)IMU對(duì)里程計(jì)的修正.

2.2 超聲波雷達(dá)補(bǔ)償激光雷達(dá)

激光雷達(dá)精度高、抗干擾性好,但是不能檢測(cè)玻璃等透明物體,而在移動(dòng)機(jī)器人實(shí)際導(dǎo)航過(guò)程中,玻璃等透明物體很常見(jiàn).利用超聲波的物理特性能有效檢測(cè)出透明物體,提高系統(tǒng)的安全性.為此,本文對(duì)激光雷達(dá)和超聲波雷達(dá)進(jìn)行聯(lián)合標(biāo)定,將兩者數(shù)據(jù)轉(zhuǎn)化到同一坐標(biāo)系下,實(shí)現(xiàn)超聲波雷達(dá)對(duì)激光雷達(dá)的數(shù)據(jù)補(bǔ)償.

按照?qǐng)D5所示方法安裝激光雷達(dá)與超聲波雷達(dá),設(shè)置激光雷達(dá)起始掃描角度為0°,結(jié)束掃描角度為360°,設(shè)置激光雷達(dá)的分辨率為720,即繞激光雷達(dá)中心旋轉(zhuǎn)360°有720幀激光數(shù)據(jù),每幀激光的間隔為0.5°,將超聲波雷達(dá)安裝在激光雷達(dá)正下方,超聲波的朝向與激光雷達(dá)正前方角度一致,超聲波雷達(dá)的探測(cè)角度為正前方50°區(qū)域,探測(cè)距離為3 m.

圖5 激光雷達(dá)與超聲波雷達(dá)安裝示意圖

在ROS平臺(tái),將激光雷達(dá)數(shù)據(jù)存儲(chǔ)在Range[i]數(shù)組中,i為激光幀的個(gè)數(shù),數(shù)組中每一個(gè)元素都代表其所對(duì)應(yīng)的激光幀長(zhǎng)度,即激光雷達(dá)到物體的距離.激光雷達(dá)測(cè)距長(zhǎng)度設(shè)定為6 m,雷達(dá)補(bǔ)償軟件流程如圖6所示.

圖6 激光雷達(dá)與超聲波雷達(dá)融合軟件流程圖

激光雷達(dá)和超聲波雷達(dá)同時(shí)工作,當(dāng)超聲波雷達(dá)檢測(cè)區(qū)域內(nèi)沒(méi)有障礙物時(shí),不進(jìn)行數(shù)據(jù)補(bǔ)償,直接將激光雷達(dá)數(shù)據(jù)發(fā)布給上位機(jī).當(dāng)超聲波雷達(dá)檢測(cè)區(qū)域內(nèi)存在障礙物則執(zhí)行數(shù)據(jù)補(bǔ)償,將超聲波數(shù)據(jù)轉(zhuǎn)化成激光數(shù)據(jù)并與激光雷達(dá)原始數(shù)據(jù)進(jìn)行拼接.激光雷達(dá)數(shù)據(jù)包括距離數(shù)據(jù)與角度數(shù)據(jù),超聲波雷達(dá)僅能獲取距離數(shù)據(jù),當(dāng)超聲波檢測(cè)到障礙物時(shí),假設(shè)超聲波的波束角為激光雷達(dá)的檢測(cè)角度,超聲波所測(cè)距離d為該檢測(cè)角度內(nèi)第一幀激光幀的長(zhǎng)度.設(shè)任意一幀激光與初始位置的夾角為α,則第i幀激光的長(zhǎng)度為:

li=d/cosα

(8)

激光幀與初始位置夾角為:

α=0.5i

(9)

將li插入Range[i]雷達(dá)數(shù)組中,即可實(shí)現(xiàn)對(duì)激光雷達(dá)數(shù)據(jù)的補(bǔ)償.

3 系統(tǒng)仿真與試驗(yàn)

本文通過(guò)Matlab和ROS平臺(tái)進(jìn)行試驗(yàn)與仿真,ROS系統(tǒng)既可使用自帶的仿真軟件,也可通過(guò)rosbag功能采集數(shù)據(jù)輸入到Matlab內(nèi)進(jìn)行仿真.仿真和試驗(yàn)包括兩輪差速模型仿真、IMU修正里程計(jì)試驗(yàn)、超聲波雷達(dá)補(bǔ)償激光雷達(dá)試驗(yàn)和導(dǎo)航試驗(yàn).

3.1 兩輪差速模型仿真

圖7 兩輪差速模型仿真結(jié)果

3.2 IMU修正里程計(jì)試驗(yàn)

為了驗(yàn)證IMU修正里程計(jì)后的改進(jìn)效果,采集多組修正前與修正后的機(jī)器人實(shí)際旋轉(zhuǎn)角度數(shù)據(jù)進(jìn)行對(duì)比分析(見(jiàn)表1).

表1 IMU修正里程計(jì)試驗(yàn) rad

表1中誤差1代表僅僅依靠編碼器作為里程計(jì)測(cè)量出的旋轉(zhuǎn)角度與實(shí)際角度之間的誤差,誤差2代表IMU修正里程計(jì)后測(cè)量出的旋轉(zhuǎn)角度與實(shí)際角度之間的誤差.誤差1的平均值為0.309 rad,誤差2的平均值為0.091 rad,說(shuō)明IMU修正里程計(jì)后大大提高了機(jī)器人旋轉(zhuǎn)運(yùn)動(dòng)的測(cè)量精度,從而提高了移動(dòng)機(jī)器人導(dǎo)航時(shí)的定位精度.

3.3 超聲波雷達(dá)補(bǔ)償激光雷達(dá)試驗(yàn)

激光雷達(dá)的檢測(cè)范圍為6 m,超聲波雷達(dá)的檢測(cè)范圍為3 m,當(dāng)機(jī)器人正前方3 m內(nèi)出現(xiàn)玻璃等透明障礙物時(shí),補(bǔ)償算法開(kāi)始工作.將超聲波雷達(dá)數(shù)據(jù)轉(zhuǎn)化成激光雷達(dá)數(shù)據(jù)格式,與原始的激光雷達(dá)數(shù)據(jù)進(jìn)行拼接,圖8為雷達(dá)補(bǔ)償前后的試驗(yàn)圖.

(a) 融合前

(b) 融合后

由圖8可見(jiàn),補(bǔ)償前的激光雷達(dá)可以探測(cè)6 m范圍內(nèi)的障礙物,但對(duì)于透明物體則難以檢測(cè),通過(guò)超聲波雷達(dá)補(bǔ)償激光雷達(dá)數(shù)據(jù)后,能檢測(cè)到激光雷達(dá)正前方3 m內(nèi)的透明物體,移動(dòng)機(jī)器人能實(shí)現(xiàn)安全有效的避障導(dǎo)航.

3.4 導(dǎo)航試驗(yàn)

本文基于ROS系統(tǒng)設(shè)計(jì)移動(dòng)機(jī)器人導(dǎo)航系統(tǒng),利用開(kāi)源的SLAM算法建立實(shí)驗(yàn)室內(nèi)的二維柵格地圖,在室內(nèi)地圖的基礎(chǔ)上進(jìn)行路徑規(guī)劃,移動(dòng)機(jī)器人成功地從起點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn),實(shí)現(xiàn)了自動(dòng)導(dǎo)航.在自動(dòng)導(dǎo)航過(guò)程中利用IMU修正里程計(jì)信息，解決了輪胎打滑而使里程失準(zhǔn)的問(wèn)題,并且在終點(diǎn)處利用超聲波雷達(dá)補(bǔ)償激光雷達(dá)的方法檢測(cè)到了預(yù)設(shè)的透明物體障礙物,圖9為移動(dòng)機(jī)器人走過(guò)的路徑和實(shí)時(shí)檢測(cè)到的障礙物,結(jié)果表明試驗(yàn)成功.

圖9 移動(dòng)機(jī)器人導(dǎo)航試驗(yàn)結(jié)果

4 結(jié)語(yǔ)

本項(xiàng)目在ROS平臺(tái)下,設(shè)計(jì)了導(dǎo)航系統(tǒng)的軟硬件框架,利用激光雷達(dá)、IMU、超聲波雷達(dá)、編碼器多個(gè)傳感器來(lái)獲取環(huán)境與姿態(tài)信息,通過(guò)多傳感器互補(bǔ),有效解決了移動(dòng)機(jī)器人因?yàn)槁访娲蚧ㄎ皇?zhǔn)、透明物體難以規(guī)避等問(wèn)題,實(shí)現(xiàn)了移動(dòng)機(jī)器人從起點(diǎn)到目標(biāo)點(diǎn)的自主導(dǎo)航.通過(guò)試驗(yàn)測(cè)試，驗(yàn)證了該系統(tǒng)切實(shí)可行.