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

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

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

1 環(huán)境建模

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

1.1 坐標系統(tǒng)

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

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

圖2 激光雷達極坐標系

式中：r為反射點與激光雷達中心之間的水平距離；α為信息點在激光雷達極坐標系中的角度。激光雷達采集到的信息點在雷達坐標系中的精確角度計算如式（2）所示：

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

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

1.2 反光柱系統(tǒng)

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

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

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

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

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

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

2 定位系統(tǒng)

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

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

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

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

2.2 特征分割算法

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

特征分割算法輸入為點云數(shù)據(jù)集合R'和距離閾值，輸出為屬于不同反光柱的信息點簇。步驟如下：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為激光雷達，PA為激光雷達到反光柱的最大距離（激光雷達最大探測距離為6.5 m）。PA與圓O 相切，OC、OA為反光柱半徑（反光柱直徑為9 cm）。PC為在滿足上述情況時激光雷達到反光柱的最小距離。閾值φ計算公式為

圖4 閾值計算示意圖

2.3 特征匹配算法

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

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

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

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

3.1 智能叉車平臺搭建

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

圖5 叉車實物圖

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

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

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

表1 特征分割與特征匹配算法準確率

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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