基于特征分割與匹配的智能叉車定位算法

丁松，周奎，李凱，王鑫，蔡營，梁黃黃，付勇智

（1.湖北汽車工業(yè)學(xué)院，湖北 十堰442002;2.東風(fēng)汽車集團(tuán)股份有限公司 技術(shù)中心，湖北 武漢430000）

定位技術(shù)是智能叉車自主導(dǎo)航功能中關(guān)鍵的技術(shù)之一。劉洋提出了位姿估算矯正法，實(shí)現(xiàn)了基于激光的AGV 定位［1］；肖載鴻使用二維碼作為視覺路標(biāo)，利用二維碼的定位信息對AGV 進(jìn)行位姿校正［2］；楊玉明基于慣性導(dǎo)航原理提出適用于全向AGV 的定位方法，僅使用慣性測量單元和編碼器對AGV 進(jìn)行定位［3］；魏焱提出了在單目視覺條件下實(shí)現(xiàn)對移動機(jī)器人進(jìn)行定位的方法［4］；李飛等人提出了基于兩級WiFi 指紋的定位算法［5］；黃百川等人充分利用了低功耗藍(lán)牙（BLE）的3 個獨(dú)立通道并提出了一種不確定的加權(quán)三邊測量方法從而確定用戶最佳位置［6］；廖凌俊提出了一種基于動態(tài)測距方程的ADS-TWR算法，修正了由運(yùn)動產(chǎn)生的測距偏差，分析了影響動態(tài)測距的因素，最后結(jié)合擴(kuò)展卡爾曼濾波給出了的目標(biāo)動態(tài)定位算法［7］，但是智能叉車的計(jì)算資源無法達(dá)到要求；張濤提出一種基于視覺信標(biāo)和里程計(jì)融合的室內(nèi)定位方法［8］，但叉車由于成本原因未安裝里程計(jì)。由于期望盡可能減少環(huán)境和叉車本身的改造難度和成本，故倉庫中不會再額外安裝WiFi、藍(lán)牙等；且為了降低成本，叉車使用了算力較弱的低成本處理器，也不會增設(shè)其他傳感器。在倉庫光照條件較弱，僅依靠叉車原有的單線激光雷達(dá)和環(huán)境中部署的若干反光柱進(jìn)行定位的情況下，上述算法均不能很好適用。文中基于現(xiàn)有倉庫和叉車固有的硬件條件，設(shè)計(jì)了基于單線激光雷達(dá)的特征分割與特征匹配的智能叉車定位算法，并應(yīng)用于智能叉車，效果良好。

1 環(huán)境建模

環(huán)境建模主要是建立合理的坐標(biāo)系系統(tǒng)（世界坐標(biāo)系、雷達(dá)坐標(biāo)系），確立坐標(biāo)轉(zhuǎn)換關(guān)系及航向角計(jì)算公式，建立反光柱系統(tǒng)與特征地圖。在整個定位過程中始終將智能叉車視為質(zhì)點(diǎn)。

1.1 坐標(biāo)系統(tǒng)

如圖1所示，在環(huán)境中選定合適的位置作為世界坐標(biāo)系原點(diǎn)并建立世界坐標(biāo)系xoy，x'o'y'為以激x'光雷達(dá)為原點(diǎn)建立的雷達(dá)坐標(biāo)系。環(huán)境中布置的反光柱的坐標(biāo)、確定智能叉車的航向角與位置均基于世界坐標(biāo)系。圖1 中（x,y）為智能叉車在世界坐標(biāo)中的坐標(biāo)位置，θ為叉車的航向角，（x's,y's）為智能叉車在雷達(dá)坐標(biāo)系下的坐標(biāo)，α為反光柱與激光雷達(dá)掃描平面的x'軸之間的夾角。激光雷達(dá)信息點(diǎn)用極坐標(biāo)形式表示，坐標(biāo)系如圖2所示。極坐標(biāo)（r,α）為激光雷達(dá)的信息點(diǎn)，與雷達(dá)二維直角坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換關(guān)系為

圖1 智能叉車坐標(biāo)系統(tǒng)

圖2 激光雷達(dá)極坐標(biāo)系

式中：r為反射點(diǎn)與激光雷達(dá)中心之間的水平距離；α為信息點(diǎn)在激光雷達(dá)極坐標(biāo)系中的角度。激光雷達(dá)采集到的信息點(diǎn)在雷達(dá)坐標(biāo)系中的精確角度計(jì)算如式（2）所示：

式中：λ為激光雷達(dá)的某個信息點(diǎn)在雷達(dá)坐標(biāo)系中的精確角度;θf為接收到的數(shù)據(jù)包中第1 個信息點(diǎn)的絕對角度（在雷達(dá)坐標(biāo)系中的精確角度）；i為該信息點(diǎn)在數(shù)據(jù)包中的索引；n為激光雷達(dá)的采樣率。智能叉車航向角的計(jì)算公式為

式中：θ為叉車航向角；（xa,ya）和（xb,yb）為反光柱a、b在世界坐標(biāo)系中的坐標(biāo)；θa和θb為反光柱a、b在雷達(dá)坐標(biāo)系中的角度；Ra和Rb為反光柱a、b與激光雷達(dá)之間的距離。智能叉車世界坐標(biāo)的計(jì)算公式為

1.2 反光柱系統(tǒng)

為了提高激光雷達(dá)接收反光柱信息的質(zhì)量，反光柱的擺放應(yīng)遵循以下規(guī)則：1）在AGV 行駛路徑上每個位置至少能接收到3個反光柱；2）叉車行駛路徑兩側(cè)的反光柱應(yīng)該呈不對稱安裝；3）在有玻璃或者有較強(qiáng)反光效果的位置應(yīng)該安裝反光柱以提高定位精度；4）在叉車的卸貨站點(diǎn)處應(yīng)該多安裝幾個反光柱來提高定位精度。

精確測量安裝的每個反光柱的世界坐標(biāo)，在應(yīng)用環(huán)境中一共安裝了n個反光柱，第i個反光柱的世界坐標(biāo)用（xi,yi）來表示，n個反光柱的世界坐標(biāo)構(gòu)成反光柱世界坐標(biāo)向量R：

第i個反光柱與第j個反光柱之間的絕對距離為

每個反光柱跟其他反光柱的絕對距離構(gòu)成該反光柱的距離向量M，第i個反光柱的距離向量為

在智能叉車行走區(qū)域內(nèi)精準(zhǔn)布置的所有反光柱在世界坐標(biāo)系下的距離向量構(gòu)成了環(huán)境特征地圖，用n×（n-1）二維矩陣G表示環(huán)境特征地圖：

除此之外還需外存儲與G中的距離向量Mi一一對應(yīng)的R。根據(jù)式（5）～（8）得到當(dāng)前探測到的所有反光柱的距離向量M'i。此時所有反光柱的距離向量構(gòu)成當(dāng)前探測到的反光柱距離矩陣G'。

2 定位系統(tǒng)

文中設(shè)計(jì)了基于特征分割與特征匹配的定位算法并將其應(yīng)用于智能叉車的定位系統(tǒng)，定位系統(tǒng)主要分為數(shù)據(jù)輸入、反光柱信息點(diǎn)處理、叉車位姿計(jì)算等幾個模塊。

2.1 定位系統(tǒng)框架

文中選用基于單線激光雷達(dá)的定位技術(shù)，雖然難度較高，但定位精度高、穩(wěn)定性好。定位系統(tǒng)框架如圖3 所示。基于TCP/IP 協(xié)議接收激光雷達(dá)的數(shù)據(jù)包，篩選出數(shù)據(jù)包中的反光柱信息點(diǎn)，用特征分割算法對信息點(diǎn)進(jìn)行聚類處理，再使用三點(diǎn)定位算法［9］處理聚類結(jié)果，計(jì)算出每個反光柱在雷達(dá)坐標(biāo)系中的坐標(biāo)。根據(jù)計(jì)算得到的雷達(dá)坐標(biāo)系坐標(biāo)計(jì)算反光柱距離矩陣，利用特征匹配算法匹配出若干組反光柱的世界坐標(biāo)與雷達(dá)坐標(biāo)的映射關(guān)系，最后計(jì)算叉車位姿。

圖3 定位系統(tǒng)框架圖

2.2 特征分割算法

對篩選出來的數(shù)據(jù)進(jìn)行特征分割，即將屬于不同反光柱的反射點(diǎn)分割開，將屬于同一個反光柱的反射點(diǎn)聚集在一起。由于激光雷達(dá)掃描到的信息點(diǎn)有如下特點(diǎn)：同一反光柱的信息點(diǎn)兩兩之間總是相鄰，且相互之間的差異不大。若信息點(diǎn)與激光雷達(dá)中心點(diǎn)的距離的差值小于設(shè)定的閾值時，則屬于同一個反光柱。

特征分割算法輸入為點(diǎn)云數(shù)據(jù)集合R'和距離閾值，輸出為屬于不同反光柱的信息點(diǎn)簇。步驟如下：1）取數(shù)據(jù)集合R'的第i個元素和第j個元素（j=i+1）；2）判斷第i和第j個元素差值的絕對值是否小于閾值；3）若是小于閾值則存儲第j個元素，繼續(xù)取第（j+1）個元素，跳回到2），否則存下此時將j賦值給h；4）判斷h與j的差值是否大于2，若不是則繼續(xù)取第（j+1）個元素，跳回到2）；5）若差值大于2，則1個反光柱元素分割完整，令i=h-1，跳回1）；6）數(shù)據(jù)集合R'遍歷完成，則結(jié)束。

如圖4 所示，三角形P為激光雷達(dá)，PA為激光雷達(dá)到反光柱的最大距離（激光雷達(dá)最大探測距離為6.5 m）。PA與圓O 相切，OC、OA為反光柱半徑（反光柱直徑為9 cm）。PC為在滿足上述情況時激光雷達(dá)到反光柱的最小距離。閾值φ計(jì)算公式為

圖4 閾值計(jì)算示意圖

2.3 特征匹配算法

以反光柱之間的距離作為特征，對G與G'進(jìn)行匹配，獲取若干組反光柱在世界坐標(biāo)系中與雷達(dá)坐標(biāo)系中的坐標(biāo)映射關(guān)系。

定位系統(tǒng)預(yù)存R和G。在最理想情況下，第i個反光柱與其他所有反光柱的距離信息的全集為Mi，進(jìn)而形成G。把Mj的全體二元及以上子集作為第i個反光柱的冪集，記為P（Mi）。某時刻掃描到的反光柱中的第j個反光柱的距離向量為Mj。匹配原則如下：若Mj∩P（Mi）=Mj，則雷達(dá)坐標(biāo)系中的第j個反光柱與世界坐標(biāo)系中的第i個反光柱匹配成功，輸出該坐標(biāo)映射關(guān)系；若Mj∩P（Mi）≠M(fèi)j，則雷達(dá)坐標(biāo)系中第j個反光柱與世界坐標(biāo)系中的第i個反光柱匹配失敗，雷達(dá)坐標(biāo)系中的第（j+1）個反光柱繼續(xù)與世界坐標(biāo)系中的其他反光柱匹配。直至與其他所有反光柱匹配失敗，則第j個反光柱匹配失敗，取下一個反光柱進(jìn)行匹配。

特征匹配算法輸入為R、G和G'，輸出為反光柱世界坐標(biāo)與雷達(dá)坐標(biāo)映射關(guān)系。步驟如下：1）取G'第i行并判斷是否為G第i行的子集（i=0,1,2，…），若是則輸出反光柱世界坐標(biāo)與雷達(dá)坐標(biāo)系坐標(biāo)映射關(guān)系，i加1；2）若不是，則繼續(xù)與環(huán)境地圖矩陣的下一行比較，直至遍歷完環(huán)境矩陣所有行，無法與環(huán)境矩陣匹配則匹配失?。?）取G'下一行元素并跳回到1）；4）遍歷完G'所有行，結(jié)束。

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

3.1 智能叉車平臺搭建

選取德國倍加福OMD10M-R2000-B23 的單線激光雷達(dá)，掃描范圍為-180°～180°，搭載激光雷達(dá)的叉車實(shí)物如圖5 所示。由于倉庫面積較小且環(huán)境簡單，構(gòu)建的反光柱系統(tǒng)復(fù)雜度較低。

圖5 叉車實(shí)物圖

3.2 測試及結(jié)果分析

反光柱世界坐標(biāo)為手工測量，存在一定的但在可控范圍內(nèi)的誤差。在設(shè)置了反光柱系統(tǒng)的應(yīng)用環(huán)境中手動駕駛智能叉車，激光雷達(dá)對環(huán)境中的反光柱進(jìn)行掃描，實(shí)時獲取實(shí)際數(shù)據(jù)，分別記錄存儲靜態(tài)時的數(shù)據(jù)和動態(tài)行走時的數(shù)據(jù)。

以實(shí)際獲得的數(shù)據(jù)作為實(shí)驗(yàn)數(shù)據(jù)，利用MATLAB 對特征分割算法及特征匹配算法進(jìn)行離線測試，測試次數(shù)設(shè)為1000，將離線測試的定位結(jié)果與預(yù)設(shè)的參考點(diǎn)/參考軌跡比較得到算法準(zhǔn)確率見表1，路面不平整車體發(fā)生傾斜時，算法準(zhǔn)確率下降。

表1 特征分割與特征匹配算法準(zhǔn)確率

靜態(tài)時，將智能叉車在反光柱系統(tǒng)區(qū)域的若干位置靜止停放，進(jìn)行定位測試，將其結(jié)果與手工測量的叉車?yán)硐胱鴺?biāo)進(jìn)行比較。動態(tài)時，在反光柱系統(tǒng)中按預(yù)定軌跡移動叉車的同時進(jìn)行定位測試，將定位系統(tǒng)記錄的叉車移動過程中的一系列位置坐標(biāo)與叉車預(yù)定軌跡比較。定位橫、縱誤差的計(jì)算如式（10）～（11）所示：

式中：Δx和Δy為定位橫、縱軸誤差；Δs為距離偏差；xt和yt為實(shí)際計(jì)算的橫縱坐標(biāo)；X和Y為橫縱坐標(biāo)的理論值。靜態(tài)定位坐標(biāo)誤差如圖6所示，具體數(shù)據(jù)如表2 所示。方差較小，定位穩(wěn)定性較好，且在過程中沒有出現(xiàn)定位失敗的情況。

圖6 靜態(tài)定位誤差曲線圖

表2 靜態(tài)定位誤差

智能叉車進(jìn)行了速度分別為5 km·h?1和10 km·h?1的動態(tài)定位測試，如圖7 所示，定位坐標(biāo)曲線與理想坐標(biāo)基本重合，誤差較小。動態(tài)定位的誤差如圖8所示，具體數(shù)據(jù)如表3所示。經(jīng)過統(tǒng)計(jì)，叉車速度分別為5 km·h?1和10 km·h?1時的定位距離偏差的標(biāo)準(zhǔn)差分別為1.19 和2.05，定位誤差波動較小，叉車具有較好的定位性能。

表3 動態(tài)定位誤差

圖7 動態(tài)定位軌跡曲線圖

圖8 動態(tài)定位誤差曲線圖

由于存在隨機(jī)誤差且未考慮傳輸時延和計(jì)算時延，動態(tài)定位時的誤差較靜態(tài)有所增大，但均在可接受范圍內(nèi)，且定位誤差波動較小。綜上，定位系統(tǒng)的定位精度和穩(wěn)定性較高。該定位也為后續(xù)的導(dǎo)航提供的技術(shù)基礎(chǔ)的合理性。

文中系統(tǒng)應(yīng)用環(huán)境為湖北十堰某公司倉庫，場景相對簡單且單一，因此并未進(jìn)行多項(xiàng)不同條件下的測試，如不同光照、不同地面平整程度下的測試等。而叉車以5 km·h?1速度進(jìn)行測試已能夠滿足在倉庫室內(nèi)場景的應(yīng)用需求，能保證較高的定位精度和穩(wěn)定性。

4 結(jié)論

對智能叉車的運(yùn)行環(huán)境進(jìn)行建模，并搭建了智能叉車定位系統(tǒng)，對該定位系統(tǒng)進(jìn)行實(shí)車測試，結(jié)果表明在精確度和穩(wěn)定性等方面均滿足實(shí)車運(yùn)行的要求。系統(tǒng)針對某工廠應(yīng)用環(huán)境和其所提供的叉車進(jìn)行開發(fā)，不具備環(huán)境和設(shè)備條件設(shè)計(jì)實(shí)驗(yàn)與其他定位方法進(jìn)行對比，但經(jīng)過實(shí)車測試，可滿足后續(xù)的應(yīng)用需求。文中所采用的定位方法需要在應(yīng)用環(huán)境中安裝反光柱，存在一定的局限性，后續(xù)考慮采用如SLAM等無需信標(biāo)的定位方法。