基于視覺傳感的全方位車的定位和路徑規(guī)劃研究

王立銀，趙鐵軍

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

0 引言

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

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

1 全方位車的運動學模型的建立及其整體結構

1.1 全方位車的整體結構

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

1.2 全方位車的運動模型

以現(xiàn)實中的某一點作為原點，構建世界坐標系。假設兩車輪始終保持平行，以小車底面中心點為小車參考點，同時假設小車和地面做無相對滑動摩擦的運動，則小車t時刻在世界坐標系中的位姿的計算公式為

式中：x、y為小車在世界坐標系中的坐標，θ為小車前進方向與X軸的夾角。

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

圖1 小車運動模型

車輪的路徑計算公式為

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

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

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

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

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

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

將式（5）展開得到：

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

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

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

2.1 小車的激光里程計

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

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

2.2 小車的視覺里程計

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

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

圖2 算法流程圖

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

圖3 模擬小車拍攝圖

圖4 模擬小車拍攝圖

圖5 匹配結果圖

圖6 算法程序運行圖

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

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

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

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

圖7 平滑處理RRT示意圖

平滑處理改進的RRT算法基本步驟如下：

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

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

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

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

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

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

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

圖9 原RRT算法示意圖

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

4 結語

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