明瑞浩，蘇慶雙，徐兆江，顧亞飛

（1.江蘇金陵智造研究院有限公司，江蘇 南京 210000；2.上海飛機(jī)制造有限公司，上海 200000）

目前，工業(yè)園區(qū)包含種類(lèi)繁多的人工駕駛?cè)加蛙?chē)或電動(dòng)車(chē)用于物料運(yùn)輸、巡邏檢查、人員接駁等任務(wù)，存在運(yùn)行效率低、人力資源消耗大以及運(yùn)行成本高等問(wèn)題。無(wú)人車(chē)的應(yīng)用將從根本上解決上述問(wèn)題，從而為企業(yè)降低運(yùn)營(yíng)成本、提升自動(dòng)化水平以及智能化生產(chǎn)助力賦能。

自動(dòng)駕駛首先要解決的就是“我在哪里”的問(wèn)題。因此定位問(wèn)題是自動(dòng)駕駛技術(shù)的核心問(wèn)題，不準(zhǔn)確的定位將會(huì)造成決策控制的重大失誤，從而導(dǎo)致重大的安全事故，所以高精度定位技術(shù)是實(shí)現(xiàn)自動(dòng)駕駛至關(guān)重要的環(huán)節(jié)[1]。

針對(duì)工業(yè)園區(qū)環(huán)境，本文提出了一種基于RTK（Real-Time Kinematic）/全站儀的三維激光 SLAM 建圖優(yōu)化技術(shù)，可以實(shí)現(xiàn)園區(qū)環(huán)境的高精度建圖。同時(shí)驗(yàn)證了一種基于RTK/激光雷達(dá)/IMU/里程計(jì)的多傳感器融合定位技術(shù)，可以達(dá)到厘米級(jí)的定位精度。

1 基于RTK/全站儀的三維激光點(diǎn)云地圖構(gòu)建

1.1 激光 SLAM 建圖

本文采用的激光SLAM 建圖方法的系統(tǒng)組成如圖1 所示。該系統(tǒng)主要由點(diǎn)云分割、特征提取、激光里程計(jì)、激光建圖和位姿轉(zhuǎn)換五個(gè)模塊組成。系統(tǒng)接收來(lái)自激光雷達(dá)的點(diǎn)云輸入，首先將其映射到距離圖像上進(jìn)行點(diǎn)云分割，然后進(jìn)入特征提取模塊，激光里程計(jì)模塊利用提取的特征找出相鄰掃描間的變換關(guān)系。激光建圖模塊對(duì)這些特征做進(jìn)一步處理，并將其注冊(cè)到全局點(diǎn)云地圖中。最后，位姿變換模塊融合激光里程計(jì)和激光建圖模塊的位姿估計(jì)結(jié)果，輸入到后端優(yōu)化模塊中，經(jīng)過(guò)后端優(yōu)化后輸出最終的6 自由度位姿估計(jì)。

圖1 三維激光雷達(dá)SLAM 算法流程圖

1.2 基于RTK/全站儀的地圖優(yōu)化

為了確保先驗(yàn)地圖的精確性，本文采用基于RTK/全站儀的地圖優(yōu)化技術(shù)對(duì)所建地圖進(jìn)行優(yōu)化，即在建圖過(guò)程中通過(guò)融合RTK/全站儀的全局無(wú)漂移位置信息消除SLAM 的累計(jì)誤差，完成環(huán)境精確地圖的構(gòu)建。具體地，是在室外GPS 信號(hào)良好區(qū)域，利用RTK 數(shù)據(jù)進(jìn)行輔助定位并優(yōu)化地圖，在室外無(wú)衛(wèi)星信號(hào)以及室內(nèi)采用全站儀進(jìn)行輔助定位并優(yōu)化地圖。

在后端優(yōu)化部分采用因子圖優(yōu)化[2]的方法對(duì)多源信息進(jìn)行融合以獲取更為精確的位姿信息，采用的因子圖結(jié)構(gòu)示意如圖2 所示。

圖2 地圖優(yōu)化因子圖

在因子圖中，通過(guò)節(jié)點(diǎn)表示每個(gè)待優(yōu)化的變量，圖中節(jié)點(diǎn)Pi（i=0，1，2，3）為6 自由度位姿估計(jì)。通過(guò)邊表示傳感器的量測(cè)信息，傳感器的量測(cè)對(duì)節(jié)點(diǎn)構(gòu)成了約束關(guān)系。圖中Ri（i=1，2）為通過(guò)RTK/全站儀獲取的全局三維位置約束；里程計(jì)為前端激光里程計(jì)獲取的6自由度位姿對(duì)相鄰位姿節(jié)點(diǎn)的約束；閉環(huán)約束為通過(guò)回環(huán)檢測(cè)方法對(duì)同一位置不同時(shí)刻的位姿節(jié)點(diǎn)的額外約束。

2 基于RTK/激光雷達(dá)/IMU/里程計(jì)的多源信息融合定位

2.1 基于先驗(yàn)地圖的點(diǎn)云匹配定位

本文采用基于幾何特征的全局描述子定位方法，它可以快速和準(zhǔn)確地定位至正確的全局位置，其初始定位流程如圖3 所示。

圖3 初始全局定位的算法流程圖

首先從掃描點(diǎn)云中提取特征圖，再?gòu)奶卣鲌D中提取描述符向量，基于該描述符向量使用KD 樹(shù)搜索找到最接近的候選幀，利用距離公式（1）和閾值從候選幀中選出最大可能的相似場(chǎng)景，完成全局定位。

式中，Iq和Ic為任意兩個(gè)描述符分別為兩個(gè)描述符的第j 列的列向量。

在確定初始估計(jì)位姿后，采用基于特征點(diǎn)的ICP點(diǎn)云配準(zhǔn)方法[3]對(duì)位姿進(jìn)行求解，即通過(guò)提取當(dāng)前幀點(diǎn)云的特征與其周?chē)c(diǎn)云特征地圖進(jìn)行匹配，得到當(dāng)前的精確位姿。然而如果將當(dāng)前幀的點(diǎn)云匹配至整個(gè)先驗(yàn)地圖，則會(huì)由于存在大量的冗余點(diǎn)云而導(dǎo)致匹配速度大大降低，無(wú)法滿(mǎn)足定位實(shí)時(shí)性的需求。對(duì)于全局地圖的存儲(chǔ)采用了基于特征圖[4]的方式，因此在定位過(guò)程中，只需要提取當(dāng)前幀點(diǎn)云的特征與其周?chē)欢ǚ秶鷥?nèi)的點(diǎn)云特征地圖進(jìn)行匹配，即可獲取較為精確的定位信息，這樣大大減小了計(jì)算量，提高了算法的實(shí)時(shí)性。

2.2 多傳感器融合定位

在實(shí)際應(yīng)用中，基于先驗(yàn)地圖的點(diǎn)云匹配定位在地圖變化較小的場(chǎng)景中，定位精度較高，但在面向復(fù)雜多變的環(huán)境時(shí)，則存在定位誤差波動(dòng)大、定位不可靠的問(wèn)題。為了保證無(wú)人車(chē)在工業(yè)園區(qū)中安全可靠地運(yùn)行，本文采用多種導(dǎo)航傳感器融合定位的方法。由于使用的傳感器信息大多是非線性的，因此本文采用基于擴(kuò)展卡爾曼濾波（EKF）框架[5]，建立狀態(tài)方程和量測(cè)方程，實(shí)現(xiàn)對(duì)RTK、激光雷達(dá)、慣性傳感器（IMU）和里程計(jì)的信息融合。

EKF 首先將非線性函數(shù)進(jìn)行泰勒展開(kāi)并進(jìn)行一階線性截?cái)?，把非線性問(wèn)題轉(zhuǎn)化為線性問(wèn)題。線性化處理得到一階線性化的狀態(tài)方程和量測(cè)方程后，再用卡爾曼濾波進(jìn)行計(jì)算?？柭鼮V波是一種最優(yōu)估計(jì)[6]，在濾波初始估計(jì)的基礎(chǔ)上，通過(guò)遞推計(jì)算更新?tīng)顟B(tài)估計(jì)，且要求滿(mǎn)足估計(jì)誤差的方差最小，其狀態(tài)方程和量測(cè)方程為：

式中，Xk為k 時(shí)刻要被估計(jì)的狀態(tài)向量；Zk為k 時(shí)刻的量測(cè)向量；Φk，k-1為 k-1 時(shí)刻到 k 時(shí)刻的系統(tǒng)轉(zhuǎn)移矩陣，值與狀態(tài)量的模型有關(guān)；Wk-1為k-1 時(shí)刻的系統(tǒng)噪聲；Γk-1為系統(tǒng)的噪聲矩陣；Hk為k 時(shí)刻的量測(cè)矩陣；Vk為k 時(shí)刻的量測(cè)噪聲。

本文選取慣導(dǎo)平臺(tái)誤差角、三軸速度誤差、三軸位置誤差、陀螺零偏、陀螺一階馬爾可夫過(guò)程、加速度計(jì)一階馬爾可夫過(guò)程構(gòu)成狀態(tài)量：

系統(tǒng)的量測(cè)方程如下：

（1）RTK 量測(cè)方程

PS，VS，ASh是捷聯(lián)解算得到的位置（經(jīng)緯高）、速度和航向信息，PG，VG，AGh為導(dǎo)航儀接收到的經(jīng)過(guò)基準(zhǔn)站補(bǔ)償后輸出的位置、速度和航向信息，二者作差可得RTK 量測(cè)方程為：

（2）激光雷達(dá)量測(cè)方程

PL，AL為三維激光點(diǎn)云與先驗(yàn)地圖匹配輸出的位置（經(jīng)緯高）、姿態(tài)信息，將其與捷聯(lián)解算得到的位置、姿態(tài)作差，可得激光雷達(dá)量測(cè)方程為：

（3）里程計(jì)量測(cè)方程

利用車(chē)載里程計(jì)以及一段滑窗內(nèi)的慣性傳感器數(shù)據(jù)進(jìn)行零速檢測(cè)，若檢測(cè)到當(dāng)前處于靜止?fàn)顟B(tài)，則以三軸零速度作為量測(cè)數(shù)據(jù)，對(duì)系統(tǒng)的狀態(tài)量進(jìn)行校正[7]。

最后利用上述狀態(tài)量和量程方程，通過(guò)濾波器估計(jì)得到的誤差量對(duì)系統(tǒng)進(jìn)行反饋校正，從而提高RTK/激光雷達(dá)/IMU/里程計(jì)組合導(dǎo)航系統(tǒng)的精度和魯棒性。

3 試驗(yàn)

本文針對(duì)上述建圖與定位方法分別設(shè)計(jì)了驗(yàn)證試驗(yàn)。試驗(yàn)平臺(tái)為針對(duì)工業(yè)園區(qū)巡檢任務(wù)設(shè)計(jì)開(kāi)發(fā)的無(wú)人巡檢車(chē)（車(chē)體尺寸：2.6*1.4*1.7m，最大行駛速度：25km/h），激光雷達(dá)與組合導(dǎo)航儀天線安裝于車(chē)頂，確保其信號(hào)或視野不受遮擋，組合導(dǎo)航儀安裝在車(chē)體內(nèi)部的后軸中心位置。試驗(yàn)場(chǎng)景為某工業(yè)園區(qū)，其具有占地面積大、環(huán)境信息豐富、基礎(chǔ)設(shè)施完善等特點(diǎn)，屬于典型的工業(yè)園區(qū)環(huán)境。

3.1 地圖構(gòu)建試驗(yàn)

本文開(kāi)展了兩組建圖試驗(yàn)，一組試驗(yàn)采用激光SLAM 建圖，另一組是在第一組的基礎(chǔ)上，基于RTK/全站儀數(shù)據(jù)完成地圖優(yōu)化。

在試驗(yàn)中，原本是直行的道路，而激光SLAM 方法由于里程計(jì)累計(jì)誤差造成了較大的地圖偏差，但在增加了RTK/全站儀的全局?jǐn)?shù)據(jù)后，地圖得到了有效的糾正。

3.2 定位試驗(yàn)

在定位試驗(yàn)中，選擇全站儀測(cè)量數(shù)據(jù)作為定位真值，通過(guò)全站儀跟蹤設(shè)置在試驗(yàn)車(chē)頂部的測(cè)量棱鏡，實(shí)時(shí)測(cè)量車(chē)輛位置信息，然后與定位算法輸出的定位數(shù)據(jù)進(jìn)行對(duì)比，計(jì)算得到定位算法的定位誤差。

試驗(yàn)結(jié)果表明，本文提出的多傳感器融合定位算法具備厘米級(jí)定位的能力。

4 結(jié)論

本文提出的基于RTK/全站儀的三維激光SLAM建圖優(yōu)化技術(shù)，利用RTK/全站儀的全局無(wú)漂移的位置信息解決了激光SLAM 建圖中的累計(jì)誤差問(wèn)題，為構(gòu)建精確環(huán)境地圖提供了有利保證。另外，本文提出的基于擴(kuò)展卡爾曼濾波的多傳感器融合定位技術(shù)解決了在實(shí)際應(yīng)用中，基于先驗(yàn)地圖的點(diǎn)云匹配定位方法在面向復(fù)雜多變環(huán)境時(shí)的定位誤差波動(dòng)大、定位不可靠的問(wèn)題。試驗(yàn)結(jié)果表明，本文提出的建圖與定位技術(shù)有效地避免了建圖偏差和定位不穩(wěn)定的問(wèn)題，同時(shí)又具有較高的定位精度和良好的環(huán)境適應(yīng)性，可以滿(mǎn)足工業(yè)園區(qū)無(wú)人車(chē)自動(dòng)駕駛的建圖與定位要求。后續(xù)，上述方法將在本公司針對(duì)工業(yè)園區(qū)開(kāi)發(fā)的無(wú)人物流車(chē)、巡檢車(chē)、清掃車(chē)等車(chē)型上進(jìn)一步推廣應(yīng)用。