基于多傳感器融合的叉車型AGV定位技術研究

錢東海，左萬權，趙 偉，徐慧慧

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

0 引言

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

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

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

1 叉車型AGV運動學模型

本文研究的叉車型AGV屬于單舵輪AGV。單舵輪AGV結構簡單、轉彎半徑較小，由一個舵輪和兩個從動輪構成。單舵輪AGV自由度的個數(x,y,θ)大于控制輸入量的個數。因此，單舵輪AGV屬于欠驅動系統。

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

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

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

②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舵輪的轉向角。

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

(1)

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

2 正態分布變換算法

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

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

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

(2)

式中：q為網格中參考幀點云的均值;Σ為網格中參考幀點云的協方差。均值與協方差的計算方法如下。

(3)

式中：xi為網格中參考幀點云的笛卡爾坐標;n為網格中參考幀點云中點的個數。

②根據參考幀與當前幀坐標系之間的位姿關系，對當前幀點云進行齊次坐標變換，將其轉化到參考幀坐標系中。

(4)

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

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

(5)

NDT算法流程如圖2所示。

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

3 叉車型AGV全局位姿計算

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

圖3 參考幀與當前幀關系圖Fig.3 Relationship of reference frame and current frame

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

(6)

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

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

(7)

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

4 改進的正態分布變換算法

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

4.1 點云數據預處理

激光雷達掃描到的點云數據主要用于兩個方面：定位與環境地圖構建。

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

4.2 NDT掃描匹配協方差計算

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

(8)

(9)

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

PN=H-1

(10)

4.3 NDT-UKF算法

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

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

NDT-UKF算法步驟如下。

(11)

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

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

(12)

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

(13)

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

(14)

Fw計算方法如下：

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

(15)

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

(16)

⑦根據步驟⑥得到的預測的觀測值，通過加權得到系統預測的觀測值及對應的協方差矩陣。

(17)

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

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

Kk+1=PXZPZZ-1

(18)

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

(19)

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

5 仿真及結果

為了驗證NDT-UKF算法的定位精度，本文利用Matlab R2019b軟件對提出的算法進行仿真分析。Matlab navigation toolbox具有強大的機器人仿真功能，能夠對不同類型的機器人進行運動學仿真、路徑規劃仿真、激光雷達傳感器仿真等。仿真過程中，AGV運行環境地圖及實際軌跡如圖4所示。

圖4 AGV環境地圖及實際軌跡Fig.4 AGV environment map and actual trajectory

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

首先，根據叉車型AGV的運動學模型對AGV位姿進行預測。然后，通過對激光雷達仿真，獲取掃描到的周圍輪廓的點云數據，并采用NDT算法對點云之間的相對位姿關系及對應的協方差矩陣進行求解。最后，采用無跡卡爾曼濾波對預測位姿及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 結論

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