基于多傳感器的AGV定位誤差校正方法研究

曲立國， 鄧亞頌

(安徽師范大學(xué) 物理與電子信息學(xué)院， 安徽 蕪湖 241002)

0 引 言

自動(dòng)導(dǎo)引運(yùn)輸車(Automated Guided Vehicle， AGV)是現(xiàn)代物流系統(tǒng)的重要組成部分， 隨著物流行業(yè)的發(fā)展與科技技術(shù)水平的提升， 國內(nèi)AGV技術(shù)發(fā)展迅速， AGV需求量居國際前列． AGV之所以能夠?qū)崿F(xiàn)無人駕駛， 導(dǎo)航和導(dǎo)引對(duì)其起到了關(guān)鍵的作用， 而AGV能否穩(wěn)定精確地完成任務(wù)， 定位的準(zhǔn)確性將會(huì)直接影響AGV導(dǎo)航的精度． 通常， 對(duì)于已知環(huán)境的定位方法主要分為相對(duì)定位法和絕對(duì)定位法． 相對(duì)定位法是指通常使用航位推算和慣性導(dǎo)航完成AGV姿態(tài)計(jì)算的導(dǎo)航方法， 但是這種方法的估計(jì)誤差將會(huì)被累積． 隨著導(dǎo)航技術(shù)的發(fā)展， 為了提高定位的準(zhǔn)確性， 許多研究人員試圖考慮將兩種方法結(jié)合起來解決一些問題[1]．

輪式AGV定位的最簡單方法就是根據(jù)車輪的旋轉(zhuǎn)角度得到AGV與道路的相對(duì)位置， 這一方法的弊病在于AGV在平坦的地面移動(dòng)時(shí)， 輪子打滑是主要的方向誤差來源． Cho B S等人利用慣性坐標(biāo)系下線加速度傳感器和角加速度傳感器來進(jìn)行導(dǎo)航位姿推算， 研究了接觸面上的滑動(dòng)誤差的補(bǔ)償?shù)葐栴}[2-5]．

在近幾年的研究中， 地標(biāo)被廣泛地應(yīng)用于室內(nèi)移動(dòng)機(jī)器人定位， 地標(biāo)被分為自然地標(biāo)和人造地標(biāo)， 人造地標(biāo)相比于自然地標(biāo)是它們不需要改變室內(nèi)環(huán)境． Okuyama K等人將QR碼作為人工地標(biāo)引入到機(jī)器人的定位與導(dǎo)航上， 通過垂直于人工地標(biāo)的攝像機(jī)檢測(cè)到人工地標(biāo)， 機(jī)器人讀取QR碼里設(shè)置的定位數(shù)據(jù)而得到確切位置， 進(jìn)而校準(zhǔn)機(jī)器人的姿態(tài)[6-8]．

張濤等人將視覺信標(biāo)和編碼器數(shù)據(jù)進(jìn)行融合來達(dá)到室內(nèi)定位的目的， 使用Kalman濾波器融合編碼器和視覺定位信息補(bǔ)充單個(gè)傳感器定位的缺陷， 有效地提高了定位的精度[9-12]．

基于QR碼AGV定位系統(tǒng)如圖 1 所示， QR碼成網(wǎng)格狀分布在操作空間中． 例如： 需要AGV從被標(biāo)號(hào)為(0，0)的工作點(diǎn)行走至(2，2)的工作點(diǎn)， 在AGV的路徑規(guī)劃中選擇(0，0)——(1，1)——(2，2)顯然為最佳路徑． 在此過程中需要AGV在(0,0)點(diǎn)首先要使小車方向調(diào)整為45°， 再從(0,0)——(1,1)——(2,2)． 系統(tǒng)中有兩個(gè)關(guān)鍵問題需要解決： 第一是AGV如何在QR碼處確定前進(jìn)的絕對(duì)角度， 保證正確到達(dá)下一個(gè)節(jié)點(diǎn)； 第二是離開QR碼后， 在兩個(gè)QR碼之間如何進(jìn)行局部的位置姿態(tài)調(diào)整， 保證行進(jìn)方向的正確性， 以及如何補(bǔ)償車輪打滑的問題.

針對(duì)此問題， 本文在分析已有研究成果的基礎(chǔ)上， 重新設(shè)計(jì)編碼器在AGV上的安裝方法， 提出一種S形曲線修正算法， 解決兩QR碼間的AGV導(dǎo)航和車輪打滑問題． 其次， 實(shí)驗(yàn)中每經(jīng)過一個(gè)QR碼， 即通過QR碼內(nèi)嵌入標(biāo)簽號(hào)和位置信息， 相機(jī)在識(shí)別到QR碼后， 提取QR碼特征點(diǎn)在網(wǎng)絡(luò)中的位置， 并且計(jì)算當(dāng)前QR碼與AGV之間的位置偏差和姿態(tài)， 根據(jù)QR碼與AGV的位置關(guān)系對(duì)AGV的位置和姿態(tài)進(jìn)行調(diào)整， 確定AGV的絕對(duì)行駛角度．

圖 1 QR碼分布圖Fig.1 Distribution diagram of QR code

1 基于編碼器的自定位校正系統(tǒng)

1.1 AGV的結(jié)構(gòu)設(shè)計(jì)

AGV從結(jié)構(gòu)上主要包含輪式、 足式、 履帶式、 軀干式四個(gè)大類[13]． 本文AGV設(shè)計(jì)是為滿足工業(yè)生產(chǎn)使用， ， 四輪結(jié)構(gòu)在穩(wěn)定性方面要強(qiáng)于三輪結(jié)構(gòu)， 所以采用四輪式AGV結(jié)構(gòu)． 普通輪式AGV轉(zhuǎn)向裝置的結(jié)構(gòu)一般分為兩種方式： 一種是使用舵機(jī)轉(zhuǎn)向， 前輪是自由輪， 后輪是驅(qū)動(dòng)輪， 使用一個(gè)電機(jī)進(jìn)行驅(qū)動(dòng)， 轉(zhuǎn)向使用舵機(jī)控制轉(zhuǎn)向輪(前輪)實(shí)現(xiàn)； 另一種是采用差動(dòng)控制轉(zhuǎn)向， 與舵機(jī)轉(zhuǎn)向相同的是， 后輪是驅(qū)動(dòng)輪， 其左、 右輪使用獨(dú)立的電機(jī)驅(qū)動(dòng)， 前輪為自由輪， 轉(zhuǎn)向通過控制左右驅(qū)動(dòng)輪速度的方式實(shí)現(xiàn)[14]．

綜合考慮到AGV的穩(wěn)定性以及對(duì)工作精確度的要求， 本文中AGV對(duì)以往的輪式AGV構(gòu)造進(jìn)行了改進(jìn)， 采用了中間兩輪做驅(qū)動(dòng)輪， 且分別由兩個(gè)直流電機(jī)進(jìn)行控制， 前后各一個(gè)萬向輪的結(jié)構(gòu)布局， 使得小車可以前后左右自由地移動(dòng)， 大大提高了AGV的可操作性、 穩(wěn)定性以及轉(zhuǎn)向精度．

其次， 輪式AGV在行走中容易出現(xiàn)車輪打滑的情況， 從而記錄不了AGV行走的真實(shí)距離， 本文實(shí)驗(yàn)中AGV通過改裝， 如圖 2 所示， 形成一個(gè)與兩邊驅(qū)動(dòng)輪同軸的編碼器輪， 來測(cè)量AGV行走的真實(shí)距離， 為AGV的定位提供準(zhǔn)確性． 與直流電機(jī)對(duì)應(yīng)的的編碼器通過改裝， 與電機(jī)形成一個(gè)同軸線的轉(zhuǎn)動(dòng)效果， 車輪打滑時(shí)編碼器輪卻不打滑， 且與AGV之間通過減震彈簧連接， 從而不受AGV本身的壓力影響， 使得在不影響AGV轉(zhuǎn)彎的前提下達(dá)到精確測(cè)量AGV真實(shí)位移的效果． 此外相機(jī)安裝在AGV驅(qū)動(dòng)輪連軸的正中心， 因而角度上存在的誤差不會(huì)被疊加到位置誤差中， 進(jìn)一步提高定位精度．

圖 2 車體結(jié)構(gòu)圖Fig.2 Car structure diagram

1.2 S形曲線修正算法

針對(duì)以往研究中輪式AGV在行走的過程中容易出現(xiàn)車輪打滑， 從而偏離原路線行走， 使得無法測(cè)量真實(shí)位移的問題， 本文基于對(duì)AGV編碼器的特殊設(shè)計(jì)提出了一種S形路徑誤差補(bǔ)償?shù)姆椒ǎ?來修正由車輪打滑帶來的誤差， 使得車頭保持直線向前行走.

如圖 3 所示， AGV在行進(jìn)過程中左右編碼器記錄的路程分別為SL1和SR1，θn為下一時(shí)刻AGV的航偏角與水平軸的夾角(θn為陀螺儀提供的相對(duì)角度)， Δθn為AGV前后兩個(gè)時(shí)刻位姿的變化量． 可分以下兩種情況討論：

圖 3 AGV偏移示意圖Fig.3 Diagram of AGV migration

1) AGV車輪不打滑時(shí)：SL1=SR1.

由于采樣的周期性， 所以位置的推算為

(1)

2) AGV車輪出現(xiàn)打滑時(shí)：SL1≠SR1．

若AGV發(fā)生偏移， 當(dāng)SL1>SR1時(shí)， 如圖 4 第一部分所示， 則

SL1-SR1=ΔS>0.(2)

根據(jù)圖示幾何關(guān)系， 為了保證AGV最后能回到原軌跡并且保持正方向不變， 直線向前行走， 按照本文提出的算法， AGV需要進(jìn)行如圖 4 第二部分所示的調(diào)整， 使左右兩輪行進(jìn)路程滿足如下關(guān)系

SL2-SR2=-2ΔS.(3)

圖 4 “S”形曲線修正路徑圖Fig.4 “S” curve correction path map

為了AGV最后能達(dá)到上述目的， 接下來還需要做如圖 4 第三部分所示的調(diào)整， 使左右兩輪行進(jìn)路程滿足如下關(guān)系

SL3-SR3=ΔS.(4)

根據(jù)以上算法推算在AGV發(fā)生偏移時(shí)， 進(jìn)行以上兩次算法調(diào)整則可以滿足回到原軌跡并且保持正方向不變直線向前行走的目的， 若發(fā)生SL1

如圖 3 中所示， 即為某一時(shí)刻車輪出現(xiàn)了打滑的情況， 則此時(shí)刻AGV的位置推算為

(5)

下面對(duì)如圖4第二部分進(jìn)行調(diào)整， 調(diào)整過程中位置推算公式為

(6)

為了保證AGV最后能回到原軌跡并且保持正方向不變直線向前行走， 接下來還需要做如圖 4 第三部分調(diào)整， 調(diào)整過程中位置推算公式為

(7)

2 AGV位姿校正系統(tǒng)

2.1 QR碼的分布及地圖的創(chuàng)建

本實(shí)驗(yàn)?zāi)M物流倉環(huán)境， QR碼被粘貼在地板上， 以網(wǎng)格模式規(guī)則分布， 工業(yè)相機(jī)每次僅可以檢測(cè)到一個(gè)QR碼， 此設(shè)計(jì)的好處是每次相機(jī)確定的QR碼位置為絕對(duì)位置．

如圖 1 所示， QR碼由草料二維碼生成器軟件生成． QR碼的標(biāo)簽號(hào)和位置信息被編碼在二維碼中， 例如“(X,Y)=(00000001)”． QR碼的方向如圖 6 中YR方向， 固定為向上的方向?yàn)閅軸方向， 向右為X軸方向． 并且為了QR碼被解碼時(shí)， 它不會(huì)相互混亂， 生成的每個(gè)代碼中的標(biāo)簽號(hào)和位置信息都各不相同．

在地圖中每個(gè)點(diǎn)都為AGV的行進(jìn)路線， AGV的每一次行走相當(dāng)于從地圖上一個(gè)坐標(biāo)點(diǎn)移動(dòng)到另外一個(gè)坐標(biāo)點(diǎn)． 每一個(gè)QR碼是AGV工作中使用的視覺信標(biāo)節(jié)點(diǎn)， 如圖 1 中將20個(gè)信標(biāo)整齊地、 間距地、 同方向地排列在工作地面上， AGV通過安裝于底部且垂直于地面的相機(jī)QR碼進(jìn)行識(shí)別． AGV的定位過程就是需要在每一個(gè)局部空間中實(shí)時(shí)反饋AGV的位置和航向角．

2.2 AGV位姿的校正

針對(duì)目前QR碼在AGV應(yīng)用上的問題， 本文利用局部QR碼定位的準(zhǔn)確性來進(jìn)行AGV位姿的校正， AGV每次在QR碼處確定前進(jìn)的絕對(duì)角度， 保證正確到達(dá)下一個(gè)節(jié)點(diǎn)．

2.2.1 基于QR碼的位姿推算

圖 5 給出了AGV與QR碼之間的位置關(guān)系， 其中Yw-Ow-Xw為世界坐標(biāo)系下的位置坐標(biāo)，Yc-Oc-Xc為AGV的坐標(biāo)，Yr-Or-Xr為QR碼坐標(biāo)． (在本文實(shí)驗(yàn)中默認(rèn)為QR碼坐標(biāo)與世界坐標(biāo)重合， 忽略在粘貼QR碼時(shí)的人為誤差)．

圖 5 AGV與QR碼之間的位置關(guān)系Fig.5 Position relation between AGV and QR code

如圖 6 所示， AGV在工作中漸漸靠近QR碼時(shí)， 相機(jī)能拍攝到圖像信息， 相機(jī)的旋轉(zhuǎn)中心還未與QR碼的旋轉(zhuǎn)中心重合， 而此時(shí)AGV的實(shí)時(shí)位姿推算可由以下算法求得：

設(shè)機(jī)器人坐標(biāo)系下某二維碼坐標(biāo)為[xc,yc]T， 對(duì)應(yīng)的世界坐標(biāo)系下坐標(biāo)為[xw,yw]T． 由公式推導(dǎo)可得

xw=xccosθα-ycsinθα+rx,

yw=xcsinθα+yccosθα+ry.(8)

圖 6 AGV漸近QR碼時(shí)的位置關(guān)系Fig.6 The position relation of AGV asymptotically QR code

轉(zhuǎn)換為矩陣形式有

Xw=H·Xc+r,(9)

其線性關(guān)系可表示為

Xw=[xw,yw,1]T,Xc=[xc,yc,1]T.(10)

結(jié)合式(1)～(3)可得

(11)

即線性關(guān)系式

Xw=Tw,c·Xc,(12)

式中：Tw,c為兩坐標(biāo)系之間相互變換的變換矩陣．

由式(5)也可寫出其反向變換矩陣為

則可用T來表示AGV的位姿， 進(jìn)行此次位姿的調(diào)整， 直到使AGV的旋轉(zhuǎn)中心與QR碼旋轉(zhuǎn)中心重合， 結(jié)束此階段局部的位姿校正．

2.2.2 基于QR碼的偏轉(zhuǎn)角校正

當(dāng)AGV行進(jìn)到QR碼區(qū)域時(shí)， 會(huì)對(duì)相機(jī)中拍攝到的二維碼進(jìn)行解碼， 提取QR碼中編碼的標(biāo)簽號(hào)和位置信息， 與地圖上的信息進(jìn)行比較確定AGV實(shí)時(shí)的位置坐標(biāo)， 進(jìn)行QR碼局部的位姿校正， 是基于QR碼與AGV之間旋轉(zhuǎn)中心重合時(shí)的偏轉(zhuǎn)角進(jìn)行校正．

如圖 7(a) 所示， 即為AGV旋轉(zhuǎn)中心與QR碼旋轉(zhuǎn)中心重合時(shí)的一種情況， 在這種情況下需要基于QR碼圖像對(duì)小車的航偏角進(jìn)行校正， 即對(duì)相機(jī)采集到的QR碼圖像進(jìn)行處理． 為了更快和能夠適應(yīng)不同的光照強(qiáng)度， 首先對(duì)采集到的圖像進(jìn)行了灰度圖像自適應(yīng)閾值分割， 如圖7(b)所示． 再對(duì)邊緣進(jìn)行直線提取求得二維碼正向與AGV正向之間的偏向角， 如圖7(c)所示． 在AGV旋轉(zhuǎn)中心進(jìn)行位置的校正是通過中間兩個(gè)驅(qū)動(dòng)輪差速控制來調(diào)整AGV的姿態(tài)．

圖 7 QR碼旋轉(zhuǎn)中心信息的提取Fig.7 Extraction of information from the center of rotation of QR code

3 實(shí)驗(yàn)結(jié)果分析

如圖 8 所示為本次實(shí)驗(yàn)搭建的載體， 實(shí)驗(yàn)在2.4 m×1.6 m的地板上進(jìn)行， 相機(jī)安裝距離地面約為0.08 m， 地板上以0.8 m×0.8 m的地磚為基準(zhǔn)等間隔鋪設(shè)QR碼， 目的是讓AGV在此環(huán)境中按照命令進(jìn)行自主行進(jìn)．

圖 8 實(shí)驗(yàn)載體AGVFig.8 Experimental carrier AGV

如圖 9 為本文AGV定位軌跡結(jié)果， 圖9中所示AGV行進(jìn)軌跡設(shè)定為一個(gè)“C”字形， 和使用本文提出的S形曲線修正算法與QR碼相結(jié)合的軌跡相比較， 只使用單個(gè)編碼器軌跡存在誤差累積問題． 而使用本文算法的軌跡始終在不停地調(diào)整， 在QR碼處行進(jìn)行進(jìn)角度計(jì)算， 且在未檢測(cè)到QR碼時(shí)行走路徑不停地逼近直線． 將本文定位軌跡與文獻(xiàn)[15]的定位軌跡相比， 系統(tǒng)的航向誤差不會(huì)出現(xiàn)如圖 10 中航向角度誤差隨時(shí)間累積和轉(zhuǎn)彎時(shí)定位出現(xiàn)偏差現(xiàn)象． 結(jié)果表明本算法可以用于室內(nèi)AGV的路徑控制中．

如圖 11 所示為使用本文算法的AGV在X和Y軸方向上的位置偏差分布圖， 由圖可知當(dāng)AGV上的視覺傳感器采集到QR碼時(shí)， 則利用第2節(jié)方法對(duì)AGV進(jìn)行位置與姿態(tài)的校正． 當(dāng)沒有檢測(cè)到QR碼時(shí)， 則利用第1節(jié)方法對(duì)AGV位置進(jìn)行估計(jì)并且利用S形曲線修正算法進(jìn)行打滑誤差的補(bǔ)償， 從而達(dá)到降低定位、 導(dǎo)航誤差的目的．

圖 9 本文AGV定位軌跡圖Fig.9 AGV location trace in this paper

圖 10 文獻(xiàn)[15]定位軌跡圖Fig.10 Location trace in document [15]

圖 11 X, Y軸方向位置偏差Fig.11 Position deviation of X, Y axis orientation

4 結(jié) 論

本文從AGV在應(yīng)用中的兩個(gè)關(guān)鍵問題入手， 介紹了一種基于編碼器的S形路徑修正算法， 并且設(shè)計(jì)了新的編碼器安裝方法， 使編碼器輪與AGV車輪形成一個(gè)隨動(dòng)但相互不受力的效果． 其次將QR碼應(yīng)用到視覺定位中， 根據(jù)QR碼與AGV的位置關(guān)系對(duì)AGV的位置和姿態(tài)進(jìn)行調(diào)整以及進(jìn)行角度的變換． 將QR碼與編碼器間隔使用， 補(bǔ)償了編碼器大角度范圍變化下積累的誤差． 實(shí)驗(yàn)結(jié)果表明此方法具有很好的實(shí)用性與穩(wěn)定性， 定位精度也能夠滿足室內(nèi)AGV的需求． 另外， 在實(shí)驗(yàn)中發(fā)現(xiàn)相機(jī)安裝距地面的距離、 QR碼大小和相機(jī)像素將會(huì)影響相機(jī)的識(shí)別精度， 值設(shè)置大于QR碼寬度的一半可能會(huì)造成累計(jì)誤差超過相機(jī)視覺范圍偏離QR碼， 造成誤差的積累．