基于多傳感器融合的叉車(chē)型AGV定位技術(shù)研究

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

(上海大學(xué)機(jī)電工程與自動(dòng)化學(xué)院，上海 200444)

0 引言

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

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

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

1 叉車(chē)型AGV運(yùn)動(dòng)學(xué)模型

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

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

定義AGV兩個(gè)從動(dòng)輪連線的中心O點(diǎn)作為AGV位姿的參考點(diǎn)，AGV運(yùn)動(dòng)的方向?yàn)锳GV局部坐標(biāo)系X軸的正方向，垂直于AGV運(yùn)動(dòng)方向?yàn)锳GV局部坐標(biāo)系Y軸的正方向。為了對(duì)單舵輪AGV進(jìn)行運(yùn)動(dòng)學(xué)建模，對(duì)單舵輪AGV提出如下合理的假設(shè)。

①AGV始終在水平面上運(yùn)動(dòng)，且不會(huì)發(fā)生滑動(dòng)。

②AGV舵輪與從動(dòng)輪是剛性的。

AGV參考點(diǎn)在k時(shí)刻的位姿為(xk,yk,θk)，(k+1)時(shí)刻的位姿為(xk+1,yk+1,θk+1)。AGV相鄰時(shí)刻運(yùn)動(dòng)模型如圖1所示。

圖1 AGV相鄰時(shí)刻運(yùn)動(dòng)模型Fig.1 AGV motion model at adjacent moments

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

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

(1)

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

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

Biber于2003年提出NDT算法用于2D激光點(diǎn)云配準(zhǔn)。NDT算法進(jìn)行點(diǎn)云配準(zhǔn)時(shí)，無(wú)需尋找兩幀點(diǎn)云之間的對(duì)應(yīng)點(diǎn)對(duì)。因此，NDT算法比ICP算法的效率更高。

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

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

(2)

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

(3)

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

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

(4)

式中：(x,y)為當(dāng)前幀點(diǎn)云笛卡爾坐標(biāo);(tx,ty,θ)為參考幀與當(dāng)前幀坐標(biāo)系之間的相對(duì)位姿。

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

(5)

NDT算法流程如圖2所示。

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

3 叉車(chē)型AGV全局位姿計(jì)算

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

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

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

(6)

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

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

(7)

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

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

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

4.1 點(diǎn)云數(shù)據(jù)預(yù)處理

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

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

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

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

(8)

(9)

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

PN=H-1

(10)

4.3 NDT-UKF算法

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

假設(shè)AGV在k時(shí)刻的位姿為Xk|k，對(duì)應(yīng)的協(xié)方差矩陣為Pk|k，測(cè)量噪聲為高斯白噪聲，對(duì)應(yīng)的協(xié)方差矩陣為Qk。

NDT-UKF算法步驟如下。

(11)

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

②計(jì)算7個(gè)Sigma點(diǎn)對(duì)應(yīng)的權(quán)重值ω(i)。

(12)

③將步驟①得到的Sigma點(diǎn)代入AGV運(yùn)動(dòng)學(xué)方程，計(jì)算每個(gè)Sigma點(diǎn)的k+1時(shí)刻的位姿。

(13)

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

(14)

Fw計(jì)算方法如下：

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

(15)

⑥將步驟⑤得到的Sigma點(diǎn)代入觀測(cè)方程，得到預(yù)測(cè)的觀測(cè)值。

(16)

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

(17)

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

⑧計(jì)算卡爾曼增益Kk+1。

Kk+1=PXZPZZ-1

(18)

⑨對(duì)(k+1)時(shí)刻AGV位姿與位姿對(duì)應(yīng)的協(xié)方差矩陣進(jìn)行更新。

(19)

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

5 仿真及結(jié)果

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

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

圖4中：AGV運(yùn)行環(huán)境為邊長(zhǎng)25 m的正方形區(qū)域，黑色區(qū)域?yàn)檎系K物，白色區(qū)域?yàn)锳GV可通行區(qū)域，黑色曲線為AGV實(shí)際運(yùn)行軌跡。

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

圖5 AGV運(yùn)行軌跡Fig.5 The trajectory of the AGV

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

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

6 結(jié)論

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