基于激光雷達(dá)和環(huán)境特征的水平運(yùn)輸車輛定位系統(tǒng)

劉春明 王全宇 歐陽(yáng)樂(lè) 黃旭東

隨著人工智能技術(shù)的發(fā)展和應(yīng)用，自動(dòng)化集裝箱碼頭（以下簡(jiǎn)稱“自動(dòng)化碼頭”）建設(shè)蓬勃興起。青島港自動(dòng)化碼頭和上海港洋山深水港區(qū)自動(dòng)化碼頭相繼投產(chǎn)運(yùn)營(yíng)，在港口行業(yè)內(nèi)產(chǎn)生良好的示范效應(yīng)，很多碼頭對(duì)半自動(dòng)化或全自動(dòng)化方案表示出濃厚興趣。作為自動(dòng)化碼頭重要的水平運(yùn)輸設(shè)備，基于磁釘定位系統(tǒng)的自動(dòng)導(dǎo)引車雖然已成功應(yīng)用于自動(dòng)化碼頭生產(chǎn)作業(yè)，但也存在一定局限性：首先，磁釘定位系統(tǒng)的初期投資成本和后期維護(hù)成本較高，很多新建碼頭受制于資金壓力而無(wú)法應(yīng)用該系統(tǒng);其次，磁釘定位系統(tǒng)的施工過(guò)程復(fù)雜，大部分老舊碼頭因場(chǎng)地布局、基礎(chǔ)設(shè)施和作業(yè)模式等難以改變而無(wú)法應(yīng)用該系統(tǒng)。為此，本文提出基于激光雷達(dá)和環(huán)境特征的水平運(yùn)輸車輛定位系統(tǒng)（以下簡(jiǎn)稱“激光雷達(dá)定位系統(tǒng)”），從而以較低的投資和維護(hù)成本、較小的施工難度、較高的精度滿足自動(dòng)化碼頭對(duì)水平運(yùn)輸車輛定位和導(dǎo)航的要求。

1 激光雷達(dá)定位系統(tǒng)基本原理

自動(dòng)化碼頭水平運(yùn)輸車輛的自動(dòng)駕駛系統(tǒng)通過(guò)人工智能算法規(guī)劃車道和路徑，并向制動(dòng)、轉(zhuǎn)向、加速等控制器下達(dá)指令，從而控制車輛開往目的地，實(shí)現(xiàn)車輛自動(dòng)駕駛。在此過(guò)程中，高精度地圖的精確定位、輔助環(huán)境感知、路徑規(guī)劃和決策等功能[1]發(fā)揮著重要作用，其中：精確定位功能是實(shí)現(xiàn)路徑規(guī)劃和決策功能的前提，其通過(guò)對(duì)比車輛傳感器感知的環(huán)境信息與高精度地圖標(biāo)注的環(huán)境信息，得到車輛在地圖上的精確位置;輔助環(huán)境感知功能通過(guò)在高精度地圖上標(biāo)注詳細(xì)的道路信息，輔助汽車在環(huán)境感知的過(guò)程中驗(yàn)證相關(guān)信息;路徑規(guī)劃和決策功能則利用云平臺(tái)采集傳感器感知區(qū)域以外的路況信息，控制車輛提前避讓。

激光雷達(dá)是通過(guò)發(fā)射激光束探測(cè)目標(biāo)位置和速度等特征量的雷達(dá)探測(cè)裝置，其工作原理如下：激光雷達(dá)向目標(biāo)發(fā)射探測(cè)信號(hào)（激光束），并通過(guò)比較目標(biāo)反射回來(lái)的信號(hào)與發(fā)射信號(hào)，獲得目標(biāo)距離、方位、高度、速度、姿態(tài)、形狀等參數(shù)。激光雷達(dá)具有測(cè)距精度和分辨率較高、體積較小、質(zhì)量較輕、隱蔽性較好、抗有源干擾能力較強(qiáng)等優(yōu)點(diǎn)。

本文結(jié)合高精度地圖的功能和激光雷達(dá)的優(yōu)點(diǎn)，提出激光雷達(dá)定位系統(tǒng)，其基本原理如下：首先，在道路上布置一定數(shù)量的標(biāo)志物，并建立這些標(biāo)志物的點(diǎn)云地圖;然后，利用激光雷達(dá)點(diǎn)云數(shù)據(jù)和聚類算法識(shí)別標(biāo)志物，并將識(shí)別出的標(biāo)志物點(diǎn)云數(shù)據(jù)與點(diǎn)云地圖進(jìn)行匹配;最后，將車輛碼盤的里程計(jì)和慣性測(cè)量單元、激光雷達(dá)里程計(jì)、標(biāo)志物點(diǎn)云匹配結(jié)果進(jìn)行融合，輸出車輛位置信息。

2 激光雷達(dá)定位系統(tǒng)構(gòu)成

如圖1所示，激光雷達(dá)定位系統(tǒng)主要由車載多線激光雷達(dá)、車載控制器和環(huán)境特征組成。

（1）車載多線激光雷達(dá) 車載多線激光雷達(dá)每秒發(fā)射上萬(wàn)個(gè)激光點(diǎn)，并接收其反射光，然后采用飛行時(shí)間測(cè)距法計(jì)算多線激光雷達(dá)與探測(cè)點(diǎn)之間的距離，并獲得激光反射強(qiáng)度信息。

（2）車載控制器 車載控制器利用多線激光雷達(dá)收集的目標(biāo)表面大量密集點(diǎn)的三維坐標(biāo)、反射率和紋理等信息，得到被測(cè)目標(biāo)的三維模型以及線、面、體等數(shù)據(jù)，據(jù)此構(gòu)建三維點(diǎn)云地圖并繪制環(huán)境地圖。在車輛行駛過(guò)程中，車載控制器對(duì)激光雷達(dá)獲取的實(shí)時(shí)掃描數(shù)據(jù)與預(yù)存的高精度地圖進(jìn)行連續(xù)匹配，從而達(dá)到環(huán)境感知和車輛定位的目的。由于光速極快，激光飛行時(shí)間極短，測(cè)量設(shè)備必須具備非常高的精度。從效果上來(lái)看，激光雷達(dá)的維度（線束）越多，測(cè)量精度就越高，安全性也就越高。

（3）環(huán)境特征 環(huán)境特征的設(shè)計(jì)選用反光柱。反光柱的特殊材質(zhì)具有高反光率，使激光雷達(dá)能夠在環(huán)境中迅速獲取激光反射強(qiáng)度信息，從而實(shí)現(xiàn)精準(zhǔn)定位。激光反射強(qiáng)度的可控性使其即使在雨、霧、雪等惡劣天氣下也有很強(qiáng)的魯棒性，定位精度達(dá)到。反光柱立桿可以利用碼頭燈桿為載體，其布置效果見(jiàn)圖2。

3 激光雷達(dá)定位系統(tǒng)定位方法

3.1 定位方法架構(gòu)

激光雷達(dá)定位系統(tǒng)定位方法架構(gòu)如圖3所示：首先，在車輛行駛的道路兩旁布置一定數(shù)量的反光柱立桿作為標(biāo)志物，并建立標(biāo)志物點(diǎn)云地圖;然后，利用激光雷達(dá)點(diǎn)云數(shù)據(jù)和聚類算法識(shí)別標(biāo)志物，并將識(shí)別出的標(biāo)志物點(diǎn)云數(shù)據(jù)與點(diǎn)云地圖進(jìn)行匹配;最后，將車輛碼盤的里程計(jì)和慣性測(cè)量單元、激光雷達(dá)里程計(jì)、標(biāo)志物點(diǎn)云匹配結(jié)果進(jìn)行融合，輸出車輛位置信息。

3.2 激光雷達(dá)里程計(jì)的實(shí)現(xiàn)

從長(zhǎng)期來(lái)看，環(huán)境處于動(dòng)態(tài)變化的過(guò)程中;但在很短的時(shí)間（如100 ms）內(nèi)，環(huán)境是固定不變的?；诃h(huán)境的這一特點(diǎn)，可采用迭代最近點(diǎn)（iterative closest point，ICP）算法匹配激光雷達(dá)前后幀數(shù)據(jù)，計(jì)算水平運(yùn)輸車輛在短時(shí)間內(nèi)的位移和旋轉(zhuǎn)情況，從而實(shí)現(xiàn)激光雷達(dá)里程計(jì)的定位功能。

ICP算法的基本原理是：按照一定的約束條件，分別在待匹配的目標(biāo)點(diǎn)云P和源點(diǎn)云Q中，找到最鄰近點(diǎn)（pi，qi），然后計(jì)算最優(yōu)匹配參數(shù)R和t，使得誤差函數(shù)E（R，t）最小。[2]誤差函數(shù)

式中：n為最鄰近點(diǎn)對(duì)的數(shù)量;pi為目標(biāo)點(diǎn)云P中的一點(diǎn);qi為源點(diǎn)云Q中與pi對(duì)應(yīng)的最近點(diǎn);R為旋轉(zhuǎn)矩陣;t為平移向量。ICP算法的計(jì)算步驟如下。

（1）在目標(biāo)點(diǎn)云P中取點(diǎn)集pi∈P。

（2）找出源點(diǎn)云Q中的對(duì)應(yīng)點(diǎn)集qi，qi∈Q，使得最小。

（3）計(jì)算旋轉(zhuǎn)矩陣R和平移向量t，使得誤差函數(shù)E（R，t）最小。

（4）用上一步求得的旋轉(zhuǎn)矩陣R和平移向量t對(duì)pi進(jìn)行旋轉(zhuǎn)和平移變換，得到新的對(duì)應(yīng)點(diǎn)集=Rpi + t，pi∈P。

（5）計(jì)算與對(duì)應(yīng)點(diǎn)集qi的平均距離d，即

（6）如果d小于某一給定的閾值或大于預(yù)設(shè)的最大迭代次數(shù)，則停止迭代計(jì)算;否則，返回第二步，直到滿足收斂條件為止。

（7）采用ICP算法不斷匹配激光雷達(dá)前后幀數(shù)據(jù)，并累計(jì)平移和旋轉(zhuǎn)數(shù)據(jù)，最后輸出激光雷達(dá)里程計(jì)的定位結(jié)果。

3.3 立桿點(diǎn)云匹配

首先，采用全站型電子測(cè)距儀測(cè)量立桿位置，獲取立桿在全局坐標(biāo)系下的坐標(biāo)，并形成全局坐標(biāo)系下的立桿點(diǎn)云格式地圖（見(jiàn)圖4）;然后，通過(guò)篩選強(qiáng)度信息（只有反光柱區(qū)域才能產(chǎn)生密集反光點(diǎn)）并采用點(diǎn)云聚類和識(shí)別算法識(shí)別立桿（見(jiàn)圖5）;最后，采用ICP算法完成立桿點(diǎn)云匹配，從而獲得車輛定位結(jié)果。

3.4 定位結(jié)果融合

激光雷達(dá)定位結(jié)果的融合方法是：通過(guò)對(duì)車輛碼盤的里程計(jì)和慣性測(cè)量單元進(jìn)行預(yù)積分得到里程計(jì)數(shù)據(jù)，再利用擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）算法融合激光雷達(dá)里程計(jì)、立桿點(diǎn)云匹配定位、車輛里程計(jì)，最終得到激光雷達(dá)定位結(jié)果。EKF是標(biāo)準(zhǔn)卡爾曼濾波在非線性情形下的擴(kuò)展形式。作為一種高效率的遞歸濾波器（自回歸濾波器），EKF的基本思想是利用泰勒級(jí)數(shù)展開算法將非線性系統(tǒng)線性化，然后采用卡爾曼濾波框架對(duì)信號(hào)進(jìn)行濾波處理;因此，EKF是一種次優(yōu)濾波，也是目前智能駕駛框架中常見(jiàn)的數(shù)據(jù)融合算法。

4 激光雷達(dá)定位系統(tǒng)測(cè)試

（1）靜態(tài)測(cè)試 在靜態(tài)測(cè)試中，水平運(yùn)輸車輛停在相應(yīng)的坐標(biāo)點(diǎn)處，記錄車輛靜止?fàn)顟B(tài)下激光雷達(dá)定位系統(tǒng)和磁釘定位系統(tǒng)的定位結(jié)果。經(jīng)比較，在車輛靜止?fàn)顟B(tài)下，激光雷達(dá)定位系統(tǒng)與磁釘定位系統(tǒng)的定位誤差不超過(guò)10 cm。

（2）動(dòng)態(tài)測(cè)試 動(dòng)態(tài)測(cè)試分為直行測(cè)試和轉(zhuǎn)彎測(cè)試：在直行測(cè)試中，水平運(yùn)輸車輛分別以2 m/s和的速度在直行車道上行駛，記錄車輛行駛過(guò)程中激光雷達(dá)定位系統(tǒng)和磁釘定位系統(tǒng)的定位結(jié)果;在轉(zhuǎn)彎測(cè)試中，水平運(yùn)輸車輛在直行車道上沿直角轉(zhuǎn)入與當(dāng)前車道垂直的另一條直行車道，記錄車輛行駛過(guò)程中激光雷達(dá)定位系統(tǒng)和磁釘定位系統(tǒng)的定位結(jié)果。經(jīng)比較，在車輛行駛過(guò)程中，激光雷達(dá)定位系統(tǒng)與磁釘定位系統(tǒng)的定位誤差不超過(guò)25 cm。

5 結(jié)束語(yǔ)

測(cè)試結(jié)果顯示：激光雷達(dá)定位系統(tǒng)與磁釘定位系統(tǒng)的靜態(tài)定位誤差不超過(guò)10 cm，動(dòng)態(tài)定位誤差不超過(guò)25 cm，基本能夠滿足自動(dòng)化碼頭對(duì)水平運(yùn)輸車輛定位精度的要求。除了定位精度滿足要求外，激光雷達(dá)定位系統(tǒng)還具有投資和維護(hù)成本較低、施工難度較小、適用范圍較廣等優(yōu)點(diǎn)，可作為磁釘定位系統(tǒng)的替代方案應(yīng)用于新建碼頭和老舊碼頭。

參考文獻(xiàn)：

[1] 宮慧琪，牛芳. 自動(dòng)駕駛關(guān)鍵技術(shù)與產(chǎn)業(yè)發(fā)展態(tài)勢(shì)研究[J]. 信息通信技術(shù)與政策，2018（8）：45-50.

[2] 孫宇，項(xiàng)志宇，劉濟(jì)林. 未知室外環(huán)境下移動(dòng)機(jī)器人的三維場(chǎng)景重建[J]. 浙江大學(xué)學(xué)報(bào)（工學(xué)版），2007，41（12）：1949-1954.

（編輯：張敏 收稿日期：2020-03-09）