王立銀，趙鐵軍

（沈陽工業(yè)大學(xué) 機(jī)械工程學(xué)院，沈陽 110870）

0 引言

隨著制造業(yè)自動(dòng)化水平的不斷提高，以磁條導(dǎo)航、二維碼及激光反光板的傳統(tǒng)AGV具有成本較高、效率低和精度低等缺點(diǎn)，難以滿足當(dāng)代生產(chǎn)實(shí)際需求。一種依靠自主導(dǎo)航并且在空間環(huán)境相對(duì)復(fù)雜的環(huán)境下能夠靈活運(yùn)動(dòng)的智能全方位車應(yīng)運(yùn)而生，而導(dǎo)航和定位技術(shù)是全方位車的核心技術(shù)之一，體現(xiàn)了全方位車的智能化和自動(dòng)化程度[1]。

本文針對(duì)室內(nèi)有光源的工廠對(duì)傳統(tǒng)的AGV進(jìn)行運(yùn)動(dòng)學(xué)建模和SLAM系統(tǒng)框架的算法改造及路徑規(guī)劃算法的改進(jìn)，以實(shí)現(xiàn)對(duì)工廠內(nèi)部各個(gè)零件的自動(dòng)化運(yùn)輸工作。

1 全方位車的運(yùn)動(dòng)學(xué)模型的建立及其整體結(jié)構(gòu)

1.1 全方位車的整體結(jié)構(gòu)

全方位車的機(jī)械結(jié)構(gòu)主要由深度相機(jī)、激光雷達(dá)、控制系統(tǒng)、底盤系統(tǒng)等4部分組成。由于工廠室內(nèi)物件規(guī)格多樣、高度不一，所以對(duì)小車建圖的能力提出了一定的要求。為了保證小車的運(yùn)行效率及兼顧小車的通過性，在系統(tǒng)視覺高度的拾取上進(jìn)行改進(jìn)，所用的傳感器為安裝在小車前上方深度相機(jī)和激光雷達(dá)，小車車體中間則放置了控制系統(tǒng)硅板，車體下方為其車輪驅(qū)動(dòng)電動(dòng)機(jī)、編碼器及電池，小車底部前方為一副萬向輪，后方兩側(cè)為一對(duì)由差速驅(qū)動(dòng)的輪子，該設(shè)計(jì)可以使小車具有小范圍運(yùn)動(dòng)能力較槍的優(yōu)勢(shì)。

1.2 全方位車的運(yùn)動(dòng)模型

以現(xiàn)實(shí)中的某一點(diǎn)作為原點(diǎn)，構(gòu)建世界坐標(biāo)系。假設(shè)兩車輪始終保持平行，以小車底面中心點(diǎn)為小車參考點(diǎn)，同時(shí)假設(shè)小車和地面做無相對(duì)滑動(dòng)摩擦的運(yùn)動(dòng)，則小車t時(shí)刻在世界坐標(biāo)系中的位姿的計(jì)算公式為

式中：x、y為小車在世界坐標(biāo)系中的坐標(biāo)，θ為小車前進(jìn)方向與X軸的夾角。

小車運(yùn)動(dòng)過程如圖1所示。其運(yùn)動(dòng)過程可根據(jù)左右輪編碼器的數(shù)據(jù)來估計(jì)小車的整體運(yùn)動(dòng)狀態(tài)。

圖1 小車運(yùn)動(dòng)模型

車輪的路徑計(jì)算公式為

式中：r為車輪半徑，k為編碼器線數(shù)，n為某段時(shí)間內(nèi)編碼器的脈沖數(shù)，s為車輪的路徑。

根據(jù)單位時(shí)間內(nèi)輪子左右的編碼器脈沖數(shù)量就可以得到短時(shí)間內(nèi)左右輪的速度，繼而可以得到小車的運(yùn)動(dòng)模型，其運(yùn)用的公式如下：

式中：v為小車平均速度，vr為右輪的速度，vl為左輪的速度。

式中：w為小車平均角速度，wr為右輪的角速度，wl為左輪的角速度。

由于短時(shí)間內(nèi)小車直線運(yùn)動(dòng)，故路程和位移做近似處理可以得到小車的運(yùn)動(dòng)方程為

式中：ut為該時(shí)刻小車的控制參數(shù)，wt為該時(shí)刻小車的數(shù)據(jù)誤差項(xiàng)。

將式（5）展開得到：

根據(jù)式（6）便可由機(jī)器人上一時(shí)刻的位姿推算出下一時(shí)刻的位姿，但是在鄰近較短時(shí)間內(nèi)適用，時(shí)間較長后累計(jì)誤差項(xiàng)將會(huì)對(duì)推算結(jié)果造成明顯的影響，但利用電動(dòng)機(jī)編碼器與其他車載傳感器進(jìn)行配合則可以削弱累積誤差。

2 小車的導(dǎo)航系統(tǒng)

不同的引導(dǎo)方式直接影響到全方位車的安全性、穩(wěn)定性、準(zhǔn)確性和經(jīng)濟(jì)性等各個(gè)方面[2]。本小車采用了SLAM框架的導(dǎo)航系統(tǒng)綜合運(yùn)用激光里程計(jì)和視覺里程計(jì)來保證小車導(dǎo)航的正確性和精準(zhǔn)性。

2.1 小車的激光里程計(jì)

激光里程計(jì)所使用的激光傳感器為二維激光雷達(dá)，測距核心由紅外激光發(fā)射點(diǎn)與接收點(diǎn)組成，通過脈沖測量原理實(shí)現(xiàn)距離測量。其中激光雷達(dá)采集的數(shù)據(jù)具有一定的波動(dòng)性，并且雷達(dá)每自轉(zhuǎn)一周就會(huì)獲得n個(gè)相對(duì)獨(dú)立的測距數(shù)據(jù)，因此采用概率模型作為其觀察模型：

式中：zt為雷達(dá)t時(shí)刻的觀測量，Ct為小車的位姿，m為地圖參數(shù)。

2.2 小車的視覺里程計(jì)

視覺里程計(jì)使用的是深度相機(jī)，與單目和雙目相機(jī)相比，深度相機(jī)可以直接獲得圖片的深度值，大大減少了系統(tǒng)運(yùn)行的時(shí)間，可提高系統(tǒng)的實(shí)時(shí)性。而且通過目前成熟的坐標(biāo)系轉(zhuǎn)換就可以將相機(jī)拍攝的實(shí)景坐標(biāo)轉(zhuǎn)化到最終成像的像素坐標(biāo)下[3]。

視覺里程計(jì)的核心就是利用ORB特征點(diǎn)進(jìn)行兩張不同角度的圖片的匹配工作，而ORB特征點(diǎn)由關(guān)鍵點(diǎn)（像素在圖片里的位置）和描述子（該點(diǎn)的周圍像素信息）組成，提取的主要依據(jù)就是該點(diǎn)周圍的像素塊明暗度是否達(dá)到要求的閾值[4]，但是傳統(tǒng)的暴力匹配準(zhǔn)確度差，而快速近似最鄰近算法（FLANN）運(yùn)行時(shí)間長，匹配的正確率不高的缺點(diǎn)制約著其應(yīng)用，因此本文提出基于FLANN的新算法流程一定程度上緩解了當(dāng)前的困境，該流程如圖2所示。

圖2 算法流程圖

基于該算法改進(jìn)后進(jìn)行了程序的實(shí)際運(yùn)行測試，對(duì)于小車運(yùn)行過程拍攝的不同角度的圖片流，通過圖3、圖4來模擬，運(yùn)用算法后的匹配結(jié)果如圖5所示，程序在Linux系統(tǒng)運(yùn)行結(jié)果如圖6所示。

圖3 模擬小車拍攝圖

圖4 模擬小車拍攝圖

圖5 匹配結(jié)果圖

圖6 算法程序運(yùn)行圖

從原算法和本文改進(jìn)算法針對(duì)同一組圖片在本機(jī)運(yùn)行得到的數(shù)據(jù)比較來看，本文改進(jìn)的算法增加了配對(duì)的正確率，且在特征點(diǎn)的提取步驟上加快了速度，彌補(bǔ)了配對(duì)步驟中增加的時(shí)長，使得改進(jìn)的算法整體運(yùn)行時(shí)間也有所縮短。

3 小車的路徑規(guī)劃

小車的路徑規(guī)劃就是在前期小車通過導(dǎo)航系統(tǒng)和融合SLAM框架而建立的地圖基礎(chǔ)上對(duì)小車的行走路徑做合理的規(guī)劃，使小車能夠在運(yùn)行中盡量減少無用路徑，從而提高小車的運(yùn)行效率和降低能耗。傳統(tǒng)的算法是隨機(jī)數(shù)種子法（RRT），RRT算法以設(shè)定的起點(diǎn)為根節(jié)點(diǎn)，在采樣空間中以隨機(jī)采樣的方式進(jìn)行擴(kuò)展，生成隨機(jī)擴(kuò)展樹。當(dāng)采樣點(diǎn)到達(dá)目標(biāo)點(diǎn)或在設(shè)定的目標(biāo)點(diǎn)范圍內(nèi)，則停止擴(kuò)展，隨后隨機(jī)拓展樹快速將起點(diǎn)到終點(diǎn)的點(diǎn)連接起來，形成一條從起點(diǎn)到終點(diǎn)的路徑[5]。

本文在RRT算法的基礎(chǔ)上對(duì)其進(jìn)行了路徑的平滑處理，平滑處理就是在連接同路徑點(diǎn)的基礎(chǔ)上再對(duì)節(jié)點(diǎn)進(jìn)行遍歷同時(shí)繼續(xù)生成新的節(jié)點(diǎn)[6]，并與原路徑進(jìn)行比較，如果符合轉(zhuǎn)彎角度小和路徑變短，在連接線上沒有與障礙物接觸則替換原有的節(jié)點(diǎn)，然后重復(fù)此步驟直到終結(jié)點(diǎn)，重復(fù)次數(shù)可以設(shè)置閾值。其示意圖如圖7所示。

圖7 平滑處理RRT示意圖

平滑處理改進(jìn)的RRT算法基本步驟如下：

1）在工作空間初始化起始點(diǎn)XStart、目標(biāo)點(diǎn)Xgoal、采樣步長a、誤差范圍δ，先執(zhí)行原RRT算法并生成路徑；

2）從起始點(diǎn)XStart開始往外擴(kuò)展，以已生成下一節(jié)點(diǎn)X2作為Xrand點(diǎn)并隨機(jī)生成Xnew1節(jié)點(diǎn)；

3）比較Xnew1節(jié)點(diǎn)與節(jié)點(diǎn)X1到節(jié)點(diǎn)Xrand路徑長度與彎折角度；

4）若沿著點(diǎn)Xnew1到點(diǎn)Xrand的比較項(xiàng)優(yōu)于原節(jié)點(diǎn)，且連線沒有經(jīng)過障礙物，則將該節(jié)點(diǎn)替換原節(jié)點(diǎn)X1；

5）繼續(xù)擴(kuò)展并不斷在其余點(diǎn)選定新的點(diǎn)Xrand，重復(fù)步驟2）到步驟4），直到達(dá)到設(shè)定Xrand迭代閾值次數(shù)。

將改進(jìn)的算法在Matlab上進(jìn)行模擬仿真，通過藍(lán)色的物體模擬障礙物，黑色表示算法得到的路徑，結(jié)果如圖8、圖9所示。

圖8 平滑處理后RRT算法示意圖

圖9 原RRT算法示意圖

通過Matlab軟件對(duì)原算法和改進(jìn)后的算法進(jìn)行仿真后可以看到原算法路徑彎折度較大，而改進(jìn)后算法規(guī)劃的路徑彎折度大大降低，經(jīng)過平滑處理后的算法規(guī)劃的路徑使小車在行進(jìn)過程中由于轉(zhuǎn)彎所帶來的加減速和能耗將會(huì)大大降低，可以有效提高小車的運(yùn)行效率。

4 結(jié)語

本文以全方位無人車定位與路徑規(guī)劃系統(tǒng)為研究對(duì)象，并構(gòu)建了小車的運(yùn)動(dòng)學(xué)模型。小車的定位階段利用ORB特征處理的圖片流進(jìn)行定位，本文針對(duì)傳統(tǒng)FLANN算法匹配錯(cuò)誤率和匹配速度進(jìn)行改進(jìn)，利用雙向求交集的思想對(duì)其進(jìn)行算法優(yōu)化，經(jīng)過實(shí)際運(yùn)行程序和原算法比較，本文改進(jìn)算法匹配準(zhǔn)確率和匹配速度均有所提升。而在路徑規(guī)劃算法中對(duì)傳統(tǒng)的RRT算法以彎折角度為目標(biāo)進(jìn)行優(yōu)化改進(jìn)，通過對(duì)比優(yōu)化算法和原算法，發(fā)現(xiàn)改進(jìn)算法使得路徑距離減少同時(shí)增加了路徑順滑度，增加小車運(yùn)行的平穩(wěn)性，極大地提升了小車的運(yùn)行效率。