閆 照，辛大欣，曹鵬祥

（西安工業(yè)大學(xué)電子信息工程學(xué)院，陜西 西安 710021）

0 引 言

近年來(lái)，自動(dòng)導(dǎo)引車(chē)（automatic guided vehicle，AGV）被廣泛應(yīng)用到零部件裝配、倉(cāng)儲(chǔ)物流、碼頭裝卸等需要頻繁運(yùn)送物料的領(lǐng)域［1］。在貨物裝配運(yùn)送過(guò)程中，AGV 與傳送帶、工作臺(tái)等進(jìn)行近距離接駁。為保證裝卸時(shí)貨物不發(fā)生掉落，需盡量減少與工作臺(tái)之間的?？烤嚯x［2］。但因?yàn)椴钏貯GV無(wú)法橫向移動(dòng)和物理尺寸的限制，?？块g隙往往過(guò)大，不能滿(mǎn)足裝卸貨物的精度要求。

針對(duì)這個(gè)問(wèn)題，文獻(xiàn)［3］采用激光反射信標(biāo)加測(cè)距光電的方法進(jìn)行AGV的精確定位，并采用雙三角差分控制算法保證快速精確進(jìn)入接駁位置，但該方法只適用于縱向接駁，具有很大的局限性。文獻(xiàn)［4］提出了通過(guò)雙磁釘絕對(duì)定位結(jié)合基于視覺(jué)的色帶導(dǎo)引定位方法，最終實(shí)現(xiàn)機(jī)器人的絕對(duì)定位。文獻(xiàn)［5］提出應(yīng)用磁條導(dǎo)向，磁卡進(jìn)行定位控制，最終采用專(zhuān)用的滾動(dòng)導(dǎo)向組件完成二次補(bǔ)償，解決了AGV與工作平臺(tái)的精確接駁問(wèn)題。但文獻(xiàn)［4，5］只適用于人工預(yù)設(shè)固定軌跡的情況，人工成本高、魯棒性差。文獻(xiàn)［6］提出了仿側(cè)方位停車(chē)的之字形控制方法，采用視覺(jué)定位結(jié)合重復(fù)原地旋轉(zhuǎn)加直線(xiàn)的橫向接駁軌跡解決了差速AGV無(wú)軌跡橫向接駁的問(wèn)題。由于攝像頭視角限制，調(diào)整次數(shù)過(guò)多，且路徑曲率變化不平滑。雖然，針對(duì)AGV 接駁問(wèn)題，學(xué)者們做了大量的研究，但是如何高效準(zhǔn)確橫向接駁仍然是一個(gè)具有挑戰(zhàn)性和研究?jī)r(jià)值的問(wèn)題。

本文提出一種基于激光雷達(dá)的AGV 精確接駁方法。接駁方法由精確定位和軌跡規(guī)劃兩部分組成。首先，對(duì)雷達(dá)數(shù)據(jù)的線(xiàn)段分割、特征識(shí)別和自定位確定AGV 在接駁過(guò)程中位姿。然后，采用基于最小接駁半徑的不等半徑算法，完成差速AGV近距離橫向接駁軌跡規(guī)劃。本文所提算法，可取代原有的人工裝卸，在快遞物流分揀和零部件裝配等領(lǐng)域的貨物自動(dòng)裝卸中具有廣泛的應(yīng)用前景。

1 橫向接駁模型分析

工業(yè)生產(chǎn)中，AGV因載荷需求一般采用矩形差速六輪結(jié)構(gòu)，其中，包括2 個(gè)主驅(qū)動(dòng)輪，4 個(gè)隨動(dòng)輪［7］。在與工作平臺(tái)橫向接駁過(guò)程中，如圖1（a）所示，AGV 居于兩側(cè)的差速驅(qū)動(dòng)輪與前端輪廓相差了AGV 長(zhǎng)度1／2 的距離。當(dāng)AGV隨驅(qū)動(dòng)輪作圓周轉(zhuǎn)向時(shí)，前端輪廓也在以更大的半徑做圓周運(yùn)動(dòng)，圖1（b）中陰影部分為前端輪廓所經(jīng)過(guò)的區(qū)域?？梢钥吹?，前端輪廓軌跡明顯與運(yùn)行軌跡存在偏差，即轉(zhuǎn)向偏差。

圖1 差速AGV模型分析

AGV的寬度為w，長(zhǎng)度為l，AGV的整體的轉(zhuǎn)向半徑為r，外驅(qū)動(dòng)輪的轉(zhuǎn)向半徑為r+w／2，轉(zhuǎn)向偏差為d，有

由式（1）可知，轉(zhuǎn)向偏差d與差速AGV 自身尺寸和轉(zhuǎn)向半徑有關(guān)。依據(jù)工業(yè)中AGV實(shí)際尺寸，最大轉(zhuǎn)向偏差范圍為0.18 ～0.59 m，而接駁?？块g隙一般要求在0.1 m 以?xún)?nèi)，顯然并不能滿(mǎn)足裝卸貨物時(shí)接駁精度要求，故須精確接駁算法來(lái)完成差速AGV精確接駁。

2 精確接駁算法

2.1 精確定位算法

2.1.1 總體定位算法

本文提出一種基于角點(diǎn)識(shí)別的精確定位算法，通過(guò)在接駁目標(biāo)點(diǎn)前方放置矩形物體產(chǎn)生3個(gè)具有特定關(guān)系的角點(diǎn)特征，作為導(dǎo)引AGV 進(jìn)入接駁位置的參考角點(diǎn)，如圖2所示。

圖2 AGV定位示意

定位算法分為3個(gè)步驟，首先，需要使用激光雷達(dá)掃描獲得的點(diǎn)集，通過(guò)計(jì)算斜率差［8］對(duì)其進(jìn)行初步分割；然后，對(duì)分割后屬于同一直線(xiàn)特征的數(shù)據(jù)直線(xiàn)擬合，根據(jù)相鄰擬合直線(xiàn)間夾角和角點(diǎn)間距離篩選識(shí)別出特定參考角點(diǎn)［9］；最后，根據(jù)直線(xiàn)角度偏差和參考角點(diǎn)坐標(biāo)求取AGV 自身位姿。

2.1.2 線(xiàn)段分割

受文獻(xiàn)［10］啟發(fā)，提出了一種基于計(jì)算斜率差對(duì)雷達(dá)數(shù)據(jù)進(jìn)行分割的方法，如圖3 所示，激光雷達(dá)數(shù)據(jù)是以O(shè)點(diǎn)為原點(diǎn)，逆時(shí)針等間隔依次排列的P1，P2，P3，…，對(duì)應(yīng)的距離為d1，d2，d3，…。根據(jù)相鄰2個(gè)數(shù)據(jù)的距離大小，由距離短的點(diǎn)Pi向距離長(zhǎng)的點(diǎn)Pi+1做垂線(xiàn)交于點(diǎn)P′i，則αi為障礙物與Pi+1P′i的夾角。

圖3 掃描點(diǎn)示意

由幾何關(guān)系可得

在近似時(shí)根據(jù)相鄰兩數(shù)據(jù)距離值選取其中較小值與間隔角度φ的弧長(zhǎng)作為斜率的分母，減小了近似誤差。令ki＝tanαi，則有

斜率差Δk計(jì)算公式為

當(dāng)i點(diǎn)前后相鄰的數(shù)據(jù)點(diǎn)在同一條直線(xiàn)時(shí)，斜率差變化不大；而當(dāng)i為角點(diǎn)時(shí)，該點(diǎn)的斜率差會(huì)因前后數(shù)據(jù)屬于不同的直線(xiàn)而產(chǎn)生較大的波動(dòng)，即當(dāng)｜Δk（i）｜＞Δkth，｜Δk（i）｜＞｜Δk（i-1）｜且｜Δk（i）｜＞｜Δk（i+1）｜時(shí)，則認(rèn)為i點(diǎn)即為分割點(diǎn)。其中，Δkth根據(jù)多次實(shí)驗(yàn)驗(yàn)證后取值為1.0。

對(duì)于圖4（a）所示的激光雷達(dá)原始數(shù)據(jù)，Δk（i）的值如圖4（b）所示?？梢钥吹剑? 個(gè)數(shù)據(jù)點(diǎn)156，159 和168 在對(duì)應(yīng)的Δk（i）有較明顯的峰值，將該3 個(gè)數(shù)據(jù)點(diǎn)作為數(shù)據(jù)分割點(diǎn)，對(duì)雷達(dá)數(shù)據(jù)進(jìn)行分割。

圖4 環(huán)境測(cè)量原始數(shù)據(jù)與雷達(dá)數(shù)據(jù)分布

2.1.3 特征識(shí)別

經(jīng)過(guò)數(shù)據(jù)分割后，采用最小二乘法［11］對(duì)激光雷達(dá)的數(shù)據(jù)擬合，設(shè)直線(xiàn)的方程為

式中αj為第j條擬合直線(xiàn)斜率；bj為第j條擬合直線(xiàn)截距。

將每一個(gè)激光點(diǎn)的極坐標(biāo)系數(shù)據(jù)（di，θi）轉(zhuǎn)換為直角坐標(biāo)系數(shù)據(jù)（xi，yi），求解方法為

式中θi為第i束激光的角度，di為第i束激光測(cè)得的距離。

求解以下最優(yōu)化問(wèn)題

其中，對(duì)αj，bj分別求偏導(dǎo)

解得

擬合后的直線(xiàn)集合表示為

對(duì)相鄰的直線(xiàn)求取直線(xiàn)間夾角

根據(jù)實(shí)際角點(diǎn)特征，通過(guò)90°+θth＞θj＞90°-θth篩除不符合條件的角點(diǎn)特征，將符合條件的角點(diǎn)特征對(duì)應(yīng)的兩條直線(xiàn)聯(lián)立獲得交點(diǎn)坐標(biāo)（xj，yj）作為真實(shí)角點(diǎn)坐標(biāo)，然后通過(guò)角點(diǎn)間的距離進(jìn)一步識(shí)別出特定的3個(gè)角點(diǎn)。

2.1.4 自定位

在AGV初始位置建立全局坐標(biāo)系，此時(shí)激光雷達(dá)局部坐標(biāo)系與全局坐標(biāo)系重合，識(shí)別出的A，B和C3 個(gè)角點(diǎn)坐標(biāo)即為該角點(diǎn)的全局坐標(biāo)。在初始位置AGV 與工作平臺(tái)保持平行，AGV初始位姿（x0，y0，θ0）為（0，0，90°）。

在接駁過(guò)程中，可通過(guò)求取相同直線(xiàn)的角度變化求得AGV當(dāng)前轉(zhuǎn)過(guò)的角度［12］。由式（10）可得，激光雷達(dá)局部坐標(biāo)系下擬合線(xiàn)段AB和BC與X軸的夾角分別為

則k時(shí)刻AGV的姿態(tài)角度差值θΔk為

式中θAB0，θBC0為初始時(shí)刻線(xiàn)段AB和BC的角度；θABk，θBCk為k時(shí)刻線(xiàn)段AB和BC的角度。

角點(diǎn)A在全局坐標(biāo)系下的坐標(biāo)有（xA0，yA0），在k時(shí)刻雷達(dá)坐標(biāo)系下坐標(biāo)是（xAk，yAk）。則由A角點(diǎn)在全局坐標(biāo)系下k時(shí)刻的位移（xA′k，yA′k）為

同理可得到角點(diǎn)B，C對(duì)應(yīng)的位移。由式（13）和式（14）可得，在全局坐標(biāo)系下，k時(shí)刻的AGV坐標(biāo)（xk，yk，θk）為

2.2 AGV接駁軌跡規(guī)劃

為保證AGV可滿(mǎn)足接駁誤差要求實(shí)現(xiàn)橫向接駁且不發(fā)生碰撞，參照文獻(xiàn)［13］思想，本文提出了一種差速矩形AGV的不等半徑接駁算法。

2.2.1 軌跡規(guī)劃算法

針對(duì)AGV轉(zhuǎn)向偏差問(wèn)題，本文提出最小接駁轉(zhuǎn)向半徑概念，即差速矩形AGV以圓弧軌跡橫向靠近工作平臺(tái)時(shí)，接駁轉(zhuǎn)向軌跡半徑必須滿(mǎn)足大于或等于的最小半徑。最小接駁轉(zhuǎn)向半徑Rcmin由式（1）可得

本文路徑規(guī)劃算法將接駁軌跡中的最小轉(zhuǎn)向半徑圓弧軌跡的替換為最小接駁轉(zhuǎn)向半徑圓弧軌跡，然后根據(jù)AGV起始位姿反向求得與最小接駁半徑圓弧相切且滿(mǎn)足約束條件的逼近軌跡圓弧。

改進(jìn)不等轉(zhuǎn)彎半徑平行泊車(chē)的參考路徑如圖5 所示。AGV的初始位姿為S0（x0，y0，θ0），終點(diǎn)位姿為Sd（xd，yd，θd），其中θ0，θd均為π／2。首先以O(shè)1為圓心，R1為轉(zhuǎn)向半徑，轉(zhuǎn)過(guò)α角度到達(dá)點(diǎn)C。隨后再以O(shè)2為圓心，Rcmin（最小接駁轉(zhuǎn)彎半徑）為半徑作轉(zhuǎn)向運(yùn)動(dòng)，再次轉(zhuǎn)過(guò)相同角度最終到達(dá)目的地Sd（xd，yd，θd）。

圖5 軌跡規(guī)劃算法

由幾何分析可得

將自定位中獲取的AGV初始位姿和目標(biāo)最終位姿，以及式（16）代入式（17）即可求得AGV所需轉(zhuǎn)過(guò)的角度α和第一段圓弧半徑R1的值，從而完成橫向接駁路徑規(guī)劃。

2.2.2 約束空間的生成

由于實(shí)際過(guò)程中可能會(huì)發(fā)生與工作平臺(tái)碰撞或者公式無(wú)解的情況，還需要對(duì)整個(gè)軌跡規(guī)劃添加約束［14］。

由于物料運(yùn)送的需要和成本的考慮，僅在AGV前端設(shè)有一個(gè)激光雷達(dá)，掃描范圍為270°，且為提高AGV 在狹窄環(huán)境下的運(yùn)動(dòng)能力，一般與障礙物距離閾值為0.1 m［15］。為滿(mǎn)足定位要求和保證安全，在橫向逼近的過(guò)程中，運(yùn)動(dòng)方式只考慮前進(jìn)和差速轉(zhuǎn)向，AGV 在橫向逼近的過(guò)程中，最小的轉(zhuǎn)彎半徑Rmin應(yīng)為0.85 m。

不等轉(zhuǎn)彎半徑路徑規(guī)劃方法約束條件主要包括2 個(gè)：1）2個(gè)階段的轉(zhuǎn)彎半徑均需大于等于AGV 最小轉(zhuǎn)彎半徑Rmin，即R1＆R2≥Rmin；2）在整個(gè)接駁的過(guò)程中，AGV的前端外頂點(diǎn)P（xp，yp）不能碰到工作臺(tái)，即xp＜xA且yp＜yA。在接駁過(guò)程中，只有同時(shí)滿(mǎn)足以上2 個(gè)條件的軌跡才能成功接駁。

3 仿真結(jié)果分析

3.1 仿真試驗(yàn)方法

研究選用激光雷達(dá)采集模擬環(huán)境數(shù)據(jù)，通過(guò)分析軟件對(duì)原始激光雷達(dá)數(shù)據(jù)進(jìn)行分析。在室內(nèi)中搭建模擬接駁環(huán)境，如圖6 所示，在接駁位置前方放置長(zhǎng)為32 cm、寬為28 cm的矩形物體作為特定角點(diǎn)特征，實(shí)驗(yàn)中各參數(shù)分別為：Δkth＝0. 8，θth＝3°，車(chē)長(zhǎng)1 m，車(chē)寬0. 7 m，車(chē)輪半徑0.1 m，工作臺(tái)坐標(biāo)范圍為（2 ～3，1）、（3，7.5 ～1），AGV 橫向接駁的?？块g隙為0.05 m。

圖6 模擬接駁環(huán)境

3.2 精確定位結(jié)果分析

如圖7（a）所示，可以看出，在非角點(diǎn)處兩算法斜率差相同，而在A，B，C對(duì)應(yīng)的168，159，156 等處斜率變化劇烈的角點(diǎn)附近，本文算法較文獻(xiàn)［10］算法斜率差更大，角點(diǎn)特征更易識(shí)別分割。

圖7 精確定位結(jié)果

特別注意的是，155，156 點(diǎn)的｜Δk（i）｜均大于Δkth，且｜Δk（155）-Δk（154）｜＞｜Δk（156）-Δk（154）｜，故真實(shí)的角點(diǎn)應(yīng)當(dāng)在155，156點(diǎn)之間，且離兩點(diǎn)均相對(duì)較遠(yuǎn)，此時(shí)本文算法斜率差之差更加明顯。與文獻(xiàn)［10］算法相比，本文算法保持了計(jì)算簡(jiǎn)單、計(jì)算量小的優(yōu)點(diǎn)，更增加了抗干擾能力，更易分割。

對(duì)分割后的數(shù)據(jù)進(jìn)行特征識(shí)別，結(jié)果如圖7（b）所示。在AGV接駁過(guò)程中的選中4 個(gè)不同位置進(jìn)行A，B，C3 個(gè)角點(diǎn)特征的識(shí)別。為驗(yàn)證定位精度，本文通過(guò)計(jì)算3 個(gè)角點(diǎn)間距離與真實(shí)值的誤差來(lái)評(píng)價(jià)定位效果。

如表1所示，AB距離的偏差范圍為［-0.008，0.009］，BC距離的偏差范圍為［-0.004，0.004］，AC距離的偏差范圍為［-0.003，0.005］，偏差范圍和標(biāo)準(zhǔn)差都為最大的AB距離均小于0.01 m。

根據(jù)角點(diǎn)特征求取全局坐標(biāo)系下激光雷達(dá)在4個(gè)不同位置位姿與實(shí)際位姿對(duì)比，定位誤差如表2所示，本文算法平均定位誤差在0.0072 m左右，最大定位誤差為0.009 4 m，也可以保證在0.01 m范圍內(nèi)。由此可見(jiàn)，本文提出的基于角點(diǎn)特征精確定位算法誤差可以滿(mǎn)足AGV 在接駁過(guò)程的定位精度要求。

表2 本文算法定位誤差

3.3 軌跡規(guī)劃結(jié)果分析

如圖8所示，當(dāng)使用最小轉(zhuǎn)向半徑作為最后階段的轉(zhuǎn)向半徑時(shí)，因轉(zhuǎn)向偏差大于?？块g隙要求，導(dǎo)致在按照精度要求接駁時(shí)，AGV 前端輪廓與工作平臺(tái)發(fā)生碰撞，如圖8（a），（c）所示，文獻(xiàn)［13］算法無(wú)法完成橫向接駁；而本文算法規(guī)劃路徑根據(jù)自身物理尺寸和要求?？块g隙計(jì)算得最小接駁半徑，作為最后階段的轉(zhuǎn)向半徑，可根據(jù)AGV 當(dāng)前位姿規(guī)劃一條轉(zhuǎn)向偏差小于?？块g隙要求的路徑，如圖8（b），（d）所示，本文算法可成功完成橫向接駁。

圖8 算法規(guī)劃軌跡對(duì)比

4 結(jié) 論

本文介紹了一種基于激光雷達(dá)的差速AGV 定位接駁方法。通過(guò)實(shí)驗(yàn)驗(yàn)證了接駁算法中提出的數(shù)據(jù)分割算法在特征提取上的準(zhǔn)確性和優(yōu)越性，使得角點(diǎn)和AGV自身定位誤差均小于0.01 m，能夠滿(mǎn)足定位精度要求。再根據(jù)仿真對(duì)比，本文算法規(guī)劃出的接駁軌跡能夠?qū)崿F(xiàn)差速AGV的近距離橫向接駁，驗(yàn)證了本文所提出算法的可行性。