錢東海，左萬權(quán)，趙 偉，徐慧慧

(上海大學機電工程與自動化學院，上海 200444)

0 引言

自動導引小車[1](automated guided vehicle，AGV)是現(xiàn)代制造企業(yè)物流系統(tǒng)中的重要設備。激光自然導航采用激光雷達掃描環(huán)境輪廓，并通過同步定位與建圖(simultaneous localization and mapping，SLAM)技術(shù)實現(xiàn)AGV的定位與環(huán)境地圖創(chuàng)建[2]。采用激光SLAM技術(shù)對AGV進行定位時，需要對激光雷達采集到的點云進行配準，得到點云之間的相對位姿。針對點云配準，有以下方法。

迭代最近點(iterative closest point，ICP)算法[3]根據(jù)兩幀點云之間的對應點對關(guān)系，求解旋轉(zhuǎn)矩陣與平移矩陣；Andrea Censi在ICP算法的基礎上進行改進，提出PL-ICP算法[4]；Biber提出正態(tài)分布變換(normal distributions transform，NDT)算法[5]，求解點云之間的相對位姿；Jinliang Li[6]采用擴展卡爾曼濾波對機器人的編碼器預測位姿及激光點云匹配位姿進行融合。

本文以叉車型AGV為研究背景，對AGV運動學進行分析，建立系統(tǒng)的狀態(tài)方程，并利用無跡卡爾曼濾波算法對預測位姿及NDT激光掃描匹配位姿進行融合，提高AGV的定位精度。

1 叉車型AGV運動學模型

本文研究的叉車型AGV屬于單舵輪AGV。單舵輪AGV結(jié)構(gòu)簡單、轉(zhuǎn)彎半徑較小，由一個舵輪和兩個從動輪構(gòu)成。單舵輪AGV自由度的個數(shù)(x,y,θ)大于控制輸入量的個數(shù)。因此，單舵輪AGV屬于欠驅(qū)動系統(tǒng)。

AGV舵輪含有驅(qū)動電機與轉(zhuǎn)向電機。其中：驅(qū)動電機驅(qū)動AGV前進；轉(zhuǎn)向電機實現(xiàn)AGV轉(zhuǎn)向，并且驅(qū)動電機與轉(zhuǎn)向電機分別裝有編碼器。從動輪起支撐的作用，用于提高AGV的承載能力。

定義AGV兩個從動輪連線的中心O點作為AGV位姿的參考點，AGV運動的方向為AGV局部坐標系X軸的正方向，垂直于AGV運動方向為AGV局部坐標系Y軸的正方向。為了對單舵輪AGV進行運動學建模，對單舵輪AGV提出如下合理的假設。

①AGV始終在水平面上運動，且不會發(fā)生滑動。

②AGV舵輪與從動輪是剛性的。

AGV參考點在k時刻的位姿為(xk,yk,θk)，(k+1)時刻的位姿為(xk+1,yk+1,θk+1)。AGV相鄰時刻運動模型如圖1所示。

圖1 AGV相鄰時刻運動模型Fig.1 AGV motion model at adjacent moments

圖1中：vk、vk+1分別為k時刻、(k+1)時刻AGV舵輪的線速度；δk、δk+1分別為k時刻、(k+1)時刻AGV舵輪的轉(zhuǎn)向角。

根據(jù)k時刻AGV的位姿，可以推算出(k+1)時刻AGV的位姿。單舵輪AGV系統(tǒng)狀態(tài)方程如下。

(1)

式中：Xk為k時刻AGV的位姿；Xk+1為(k+1)時刻AGV的位姿；ηk=(dk,γk)T為AGV在采樣時間內(nèi)移動的距離及車體方向角變化量的測量值；wk=(rk,σk)T為測量值噪聲。

2 正態(tài)分布變換算法

Biber于2003年提出NDT算法用于2D激光點云配準。NDT算法進行點云配準時，無需尋找兩幀點云之間的對應點對。因此，NDT算法比ICP算法的效率更高。

AGV運行時，激光雷達在兩個相鄰時刻采集到兩幀點云數(shù)據(jù)，第一幀為參考幀點云，第二幀為當前幀點云。NDT算法步驟如下。

①將2D空間劃分為一定分辨率的網(wǎng)格，根據(jù)參考幀點云的笛卡爾坐標，計算每個網(wǎng)格的概率密度函數(shù)。

(2)

式中：q為網(wǎng)格中參考幀點云的均值;Σ為網(wǎng)格中參考幀點云的協(xié)方差。均值與協(xié)方差的計算方法如下。

(3)

式中：xi為網(wǎng)格中參考幀點云的笛卡爾坐標;n為網(wǎng)格中參考幀點云中點的個數(shù)。

②根據(jù)參考幀與當前幀坐標系之間的位姿關(guān)系，對當前幀點云進行齊次坐標變換，將其轉(zhuǎn)化到參考幀坐標系中。

(4)

式中：(x,y)為當前幀點云笛卡爾坐標;(tx,ty,θ)為參考幀與當前幀坐標系之間的相對位姿。

③根據(jù)概率密度函數(shù)和當前幀點云的笛卡爾坐標，創(chuàng)建目標函數(shù)，并采用牛頓法求解目標函數(shù)值。當目標函數(shù)達到最小值時，當前幀點云在參考幀坐標系中的概率密度最大。目標函數(shù)如下。

(5)

NDT算法流程如圖2所示。

圖2 NDT算法流程圖Fig.2 Flowchart of NDT algorithm

3 叉車型AGV全局位姿計算

根據(jù)NDT算法得到AGV運動過程中參考幀與當前幀的相對位姿關(guān)系后，進一步根據(jù)參考幀對應的AGV全局位姿，可以計算當前幀對應的AGV全局位姿。參考幀與當前幀關(guān)系如圖3所示。

圖3 參考幀與當前幀關(guān)系圖Fig.3 Relationship of reference frame and current frame

參考幀對應的AGV全局位姿為(x0,y0,θ0)，當前幀對應的AGV全局位姿為(x1,y1,θ1)。由圖3可知，當前幀對應的AGV全局位姿為:

(6)

式中：(Δx,Δy,Δθ)為參考幀與當前幀的相對位姿。

根據(jù)式(6)可以計算得到NDT激光掃描匹配位姿。AGV系統(tǒng)觀測方程如下：

(7)

式中：vk=(vx,vy,vθ)T為觀測噪聲。

4 改進的正態(tài)分布變換算法

本文提出一種改進的正態(tài)分布變換-無跡卡爾曼濾波(normal distribution transformation-unscented Kalman filter,NDT-UKF)算法。該算法利用無跡卡爾曼濾波對編碼器預測位姿及NDT激光掃描匹配位姿進行融合，以提高AGV的定位精度。

4.1 點云數(shù)據(jù)預處理

激光雷達掃描到的點云數(shù)據(jù)主要用于兩個方面：定位與環(huán)境地圖構(gòu)建。

AGV運動時，激光雷達旋轉(zhuǎn)激光頭對周圍環(huán)境輪廓不斷進行掃描，并返回掃描到的點云數(shù)據(jù)。由于激光雷達自身原因和環(huán)境因素的干擾，會產(chǎn)生一些離群點與噪聲點。為了提高AGV定位的實時性與準確性，需要對激光雷達返回的點云數(shù)據(jù)進行離群點移除、濾波[7]等處理，將預處理后的點云數(shù)據(jù)用于AGV的定位與環(huán)境地圖構(gòu)建。

4.2 NDT掃描匹配協(xié)方差計算

利用無跡卡爾曼濾波對編碼器預測位姿及NDT激光掃描匹配位姿進行融合時，需要計算激光掃描匹配位姿對應的協(xié)方差矩陣PN、PN與求解目標函數(shù)海賽矩陣的逆矩陣類似。海賽矩陣H各元素的求解方法如下：

(8)

(9)

根據(jù)式(8)求得海賽矩陣，進一步可以求得NDT激光掃描匹配位姿對應的協(xié)方差矩陣為：

PN=H-1

(10)

4.3 NDT-UKF算法

為了提高AGV的定位精度，本文采用無跡卡爾曼濾波(unscented Kalman filter,UKF)對編碼器預測位姿及NDT激光掃描匹配位姿進行融合。無跡卡爾曼濾波以卡爾曼濾波為基礎，是一種用于非線性濾波的方法。與擴展卡爾曼濾波的不同點在于，它并不是對系統(tǒng)的非線性方程在估計點處進行線性化處理，而是利用無跡變換(unscented transform，UT)在估計點附近確定采樣點，然后利用采樣點進行非線性映射，近似得到狀態(tài)的概率密度函數(shù)。這種近似實質(zhì)上是一種統(tǒng)計近似而并非系統(tǒng)非線性方程的解。采樣點的個數(shù)通常取(2n+1)個。其中，n為系統(tǒng)狀態(tài)方程中狀態(tài)變量的個數(shù)[8-9]。

假設AGV在k時刻的位姿為Xk|k，對應的協(xié)方差矩陣為Pk|k，測量噪聲為高斯白噪聲，對應的協(xié)方差矩陣為Qk。

NDT-UKF算法步驟如下。

(11)

式中：λ為一個縮放比例參數(shù)，用來降低總的預測誤差。

②計算7個Sigma點對應的權(quán)重值ω(i)。

(12)

③將步驟①得到的Sigma點代入AGV運動學方程，計算每個Sigma點的k+1時刻的位姿。

(13)

④根據(jù)步驟③及式(12)，求得(k+1)時刻AGV加權(quán)后的預測位姿及對應的協(xié)方差矩陣。

(14)

Fw計算方法如下：

⑤根據(jù)(k+1)時刻AGV的預測位姿，再次使用UT變換，產(chǎn)生新的Sigma點。

(15)

⑥將步驟⑤得到的Sigma點代入觀測方程，得到預測的觀測值。

(16)

⑦根據(jù)步驟⑥得到的預測的觀測值，通過加權(quán)得到系統(tǒng)預測的觀測值及對應的協(xié)方差矩陣。

(17)

式中：Rk+1為觀測噪聲對應的協(xié)方差矩陣，由式(10)得到。

⑧計算卡爾曼增益Kk+1。

Kk+1=PXZPZZ-1

(18)

⑨對(k+1)時刻AGV位姿與位姿對應的協(xié)方差矩陣進行更新。

(19)

式中：Zk+1為NDT算法計算得到的AGV全局位姿，由式(5)及式(6)計算得到。

5 仿真及結(jié)果

為了驗證NDT-UKF算法的定位精度，本文利用Matlab R2019b軟件對提出的算法進行仿真分析。Matlab navigation toolbox具有強大的機器人仿真功能，能夠?qū)Σ煌愋偷臋C器人進行運動學仿真、路徑規(guī)劃仿真、激光雷達傳感器仿真等。仿真過程中，AGV運行環(huán)境地圖及實際軌跡如圖4所示。

圖4 AGV環(huán)境地圖及實際軌跡Fig.4 AGV environment map and actual trajectory

圖4中：AGV運行環(huán)境為邊長25 m的正方形區(qū)域，黑色區(qū)域為障礙物，白色區(qū)域為AGV可通行區(qū)域，黑色曲線為AGV實際運行軌跡。

首先，根據(jù)叉車型AGV的運動學模型對AGV位姿進行預測。然后，通過對激光雷達仿真，獲取掃描到的周圍輪廓的點云數(shù)據(jù)，并采用NDT算法對點云之間的相對位姿關(guān)系及對應的協(xié)方差矩陣進行求解。最后，采用無跡卡爾曼濾波對預測位姿及NDT算法求得的位姿進行融合。為了觀察NDT-UKF估計軌跡與AGV實際軌跡的誤差，選取NDT-UKF估計軌跡的一部分。AGV運行軌跡如圖5所示。

圖5 AGV運行軌跡Fig.5 The trajectory of the AGV

為了比較不同算法的定位精度，分別使用NDT-UKF算法、NDT-EKF算法、NDT算法對AGV位姿進行估計，并對估計位姿誤差求均值。不同算法定位誤差如表1所示。從表1可以看出，本文提出的NDT-UKF算法的定位精度更高。

表1 不同算法定位誤差Tab.1 Positioning errors of different algorithms

6 結(jié)論

本文以叉車型AGV為背景，研究激光自然導航AGV的定位問題。首先，本文對叉車型AGV的運動學進行分析，建立叉車型AGV系統(tǒng)的狀態(tài)方程。然后，提出NDT-UKF算法。該算法利用無跡卡爾曼濾波對編碼器預測位姿及NDT激光掃描匹配位姿進行融合。最后，通過Matlab R2019b軟件對提出的NDT-UKF算法進行仿真。仿真結(jié)果表明，本文提出的NDT-UKF算法的定位精度更高。進一步工作包括對AGV環(huán)境地圖創(chuàng)建進行研究等。