基于激光雷達的叉車型自動導(dǎo)引小車定位研究

錢東海，陳 成，孫林林，趙 偉，劉 洋

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

隨著科學(xué)技術(shù)的不斷發(fā)展與提高，機器人在許多領(lǐng)域都開始被廣泛使用，移動機器人的定位是作為執(zhí)行任務(wù)的最基本要求。即時定位與建圖（simultaneous localization and mapping，SLAM）問題可以描述為機器人在未知環(huán)境中從一個未知位置開始移動，在移動過程中根據(jù)位置估計和地圖進行自身定位，同時在自身定位的基礎(chǔ)上建造增量式地圖，實現(xiàn)機器人的自主定位和導(dǎo)航，可以使用激光雷達或者相機等傳感器實現(xiàn)。激光雷達具備便利、快速以及準(zhǔn)確等優(yōu)勢被廣泛的作為定位傳感器使用。目前激光SLAM 主要基于圖優(yōu)化的方法進行求解，激光SLAM 主要分為前端和后端兩個部分，前端掃描匹配主要是針對幀與幀之間的激光點云數(shù)據(jù)進行處理，求解兩幀相對位姿；后端則利用到掃描匹配來進行回環(huán)檢測，在檢測到回環(huán)時則進行全局的圖優(yōu)化，提高AGV 在整個路徑中的定位精度。

1 掃描匹配

“掃描匹配” 這一概念主要出現(xiàn)在機器人學(xué)相關(guān)文獻中，主要通過將激光掃描點云配準(zhǔn)到同一坐標(biāo)系下而得到相對位姿變化[1]。激光掃描匹配處理的是移動載體運動時雷達的掃描數(shù)據(jù)，點云較稀疏，且誤差和噪聲均較大，大多要求實時處理，力求在精度和效率之間尋求平衡。目前前端掃描匹配的方法主要分為3 類：①基于點的掃描匹配；②基于特征的掃描匹配；③基于數(shù)學(xué)特性的掃描匹配。基于點的掃描匹配是目前主流的方法，主要有以下幾種方法。基于最近點迭代算法[2]（interactive closest points，ICP）進行逐點匹配，其缺點是需要在每次的迭代中進行大量的點對應(yīng)搜索；極坐標(biāo)掃描匹配[3]（polar scan matching，PSM）利用激光掃描的自然極坐標(biāo)來估計它們之間的匹配，避免了對應(yīng)搜索。但是掃描必須經(jīng)過預(yù)處理才能在極性掃描匹配器中使用；實時相關(guān)掃描匹配[4]方法采用窮舉抽樣的方法進行掃描匹配，通過設(shè)計的幾個優(yōu)化，該方法能夠?qū)崟r應(yīng)用；基于正態(tài)分布變換[5]（normal distribution transform，NDT）的掃描匹配，該方法將單次掃描中的離散2D 點變換為定義在2D 平面上的分段連續(xù)且可微的概率密度，概率密度由一組容易計算的正態(tài)分布構(gòu)成，另一幀掃描與NDT 的匹配就定義為最大化其掃描點配準(zhǔn)后在此密度上的得分，并進行優(yōu)化求解；基于優(yōu)化的掃描匹配[6]，通過障礙物概率的方式構(gòu)建誤差函數(shù)使用優(yōu)化方法對最佳位姿進行求解，其主要采用的是高斯牛頓法。本文主要研究基于點的掃描匹配。在基于優(yōu)化的掃描匹配的基礎(chǔ)上，對地圖插值時使用雙三次插值，能夠獲取更優(yōu)的插值精度，優(yōu)化方法使用列文伯格-馬夸爾特法，能有效避免高斯牛頓法的缺陷。

2 叉車型AGV 激光雷達定位中的位姿變換

本文是以叉車型AGV 為研究對象，在AGV 上安裝激光雷達實現(xiàn)導(dǎo)航定位。激光雷達傳出的點云數(shù)據(jù)需要進行處理后才可以使用，同時為了實現(xiàn)AGV 的定位要給出激光雷達與其的位姿變換關(guān)系。主要包含3 個坐標(biāo)系： 全局地圖坐標(biāo)系、AGV 坐標(biāo)系、激光雷達坐標(biāo)系。

2.1 AGV 運動學(xué)模型

本文激光雷達主要是安裝在叉車型自動導(dǎo)引小車上，該叉車型AGV 為單舵輪驅(qū)動，通過控制舵輪實現(xiàn)AGV 的轉(zhuǎn)向功能以及驅(qū)動功能。AGV 主要的運動學(xué)模型如圖1所示。

圖1 AGV 運動學(xué)模型Fig.1 AGV kinematics model

選取圖中從動輪的中心點a（xA，yA）為AGV 位姿的參考點，即AGV 在全局地圖坐標(biāo)系下的位置；（xf，yf）為舵輪在全局地圖坐標(biāo)系下坐標(biāo)；L 為車體軸距；θ 為車體方位角；φ 為舵輪轉(zhuǎn)向角；V 為舵輪運行線速度；R 為車體最小轉(zhuǎn)彎半徑。

由圖可知AGV 角速度與線速度之間的關(guān)系，同時也可以得出AGV 舵輪中心點與從動輪中心點之間的位置關(guān)系。當(dāng)AGV 從k 時刻運行到k+1 時刻，AGV 在全局地圖坐標(biāo)系下的坐標(biāo)如下：

式中：xk′，yk′為AGV 在k 時刻時舵輪在全局坐標(biāo)中的位置；φk，φk+1分別為AGV 在k，k+1 時刻的舵輪角度；xk+1，yk+1為k+1 時刻AGV 在全局坐標(biāo)系下的位置；θk，θk+1分別為AGV 在k，k+1 時刻的車體角度；t 為兩點之間的運動時間；ω 為舵輪角速度。

由于激光雷達是安裝在AGV 上，因此通過激光雷達實現(xiàn)AGV 的定位時，需要用到激光雷達與AGV車體之間的相對位姿，該值通過測量即可得到。為便于描述，將上述AGV 在全局地圖坐標(biāo)系下k+1時刻的車體位姿表示，以表示激光雷達在全局地圖坐標(biāo)系下的位姿。為了計算激光雷達在全局坐標(biāo)系下的位姿，需要先將激光雷達變換到AGV 坐標(biāo)系下，在從AGV 坐標(biāo)系變換到全局坐標(biāo)系下，則其關(guān)系如下：

在AGV 運行過程中，根據(jù)舵輪在k 時刻角速度ω、轉(zhuǎn)角φk以及車體的方位角θk，可以推算出AGV在k+1 時刻的位姿，利用激光雷達與AGV 車體之間的相對位姿變換矩陣，可計算出激光雷達的位姿，在點云匹配中可將該位姿作為激光雷達初始位姿進行迭代優(yōu)化。

2.2 激光雷達測量數(shù)據(jù)

對于二維激光雷達通常采用三角測距原理或者飛行時間進行測距，每次測距時，激光雷達通過發(fā)射器發(fā)射紅外激光信號，激光信號射到障礙物體后產(chǎn)生的反射光被接收器接收，然后經(jīng)過內(nèi)部處理器計算得到障礙物體到激光雷達的距離以及當(dāng)前夾角。在電機的驅(qū)動下，實現(xiàn)360°測距，獲得點云數(shù)據(jù)，包括角度θ=｛θi=（i-1）×360/n，i=1…n｝；距離d=｛di，i=1，…，n｝，其中n 為激光雷達每一圈掃描點個數(shù)；θi與di分別為第i 個掃描點的角度與距離。

對于激光雷達點云數(shù)據(jù)中第i 個掃描點，在激光雷達坐標(biāo)系下，則有：

式中：（xi，yi）和（di，θi）分表示第i 個掃描點在激光雷達笛卡爾坐標(biāo)系和極坐標(biāo)系下的坐標(biāo)值。激光點云數(shù)據(jù)（xi，yi）在全局地圖坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換關(guān)系通過下式計算得到：

式中：（xi，yi）為第i 個數(shù)據(jù)點在激光雷達坐標(biāo)系下的坐標(biāo)值；（xi′，yi′）為第i 個數(shù)據(jù)點在全局地圖坐標(biāo)系下的坐標(biāo)值，激光雷達位姿如式（8）所示。

式中：xL，yL表示激光雷達在全局地圖坐標(biāo)系下的坐標(biāo)值；θL表示激光雷達在全局地圖坐標(biāo)系下航向角。

3 柵格地圖創(chuàng)建及柵格障礙物概率求取

本文進行點云匹配的策略是利用激光雷達在初始位姿處的激光點云構(gòu)造柵格地圖，并且在AGV運動過程中采集新的激光點云數(shù)據(jù)不斷地更新柵格地圖。柵格地圖的創(chuàng)建與基于柵格地圖的AGV導(dǎo)航是同步進行的。本文所要解決的是已知前后兩幀激光點云數(shù)據(jù)，求得后一幀點云數(shù)據(jù)AGV 所在位姿相對于前一幀點云數(shù)據(jù)AGV 所在位姿的位姿變換。

地圖的構(gòu)建方法有很多種，通常會根據(jù)實際情況以及需要來決定使用何種類型的地圖，本文選擇使用柵格地圖來作為掃描匹配的參照。柵格地圖是將二維地圖用一系列的網(wǎng)格單元來表示[7]，對于其中的每個網(wǎng)格，用“0”和“1”分別表示無障礙物和有障礙物兩種狀態(tài)。

上述柵格地圖是離散分布，這在一定程度上限制了可達到的精度，本文使用柵格障礙物概率對柵格中各點是否存在障礙物進行描述，它是柵格中各點位置的連續(xù)函數(shù)，并且掃描匹配時需要求其偏導(dǎo)數(shù)。為使得障礙物概率函數(shù)連續(xù)且可導(dǎo)，本文選擇雙三次插值來求取每一個柵格的障礙物概率函數(shù)。

雙三次插值是對插值點周邊最近的16 個采樣點進行加權(quán)平均。假設(shè)插值點位于Q0（x，y），其中x，y 為任一小數(shù)；i，j 表示其整數(shù)部分；u，v 表示其小數(shù)部分。該方法選取以點（i-1，j-1）到（i+2，j+2）構(gòu)成的包含16 個采樣點數(shù)據(jù)的矩形網(wǎng)格。

本文使用ROBERT G.KEYS[8]提出的基于BiCubic 基函數(shù)的雙三次插值法，將其用于創(chuàng)建激光點云柵格地圖中各柵格的障礙物概率，該方法利用三次多項式求取原函數(shù)的最佳逼近函數(shù)，BiCubic 基函數(shù)形式如下：

式中：a 取-0.5。當(dāng)a=-0.5 時能產(chǎn)生相對于原始函數(shù)的采樣間隔的三階收斂；若a 為其他值時只能產(chǎn)生相對于原始函數(shù)的一階收斂。

在激光點云柵格地圖中，對于一給定的柵格，求取其柵格障礙物概率，需用到其鄰近的16 個柵格，x 方向分別以i-1，i，i+1，i+2；y 方向分別以j-1，j，j+1，j+2 為下標(biāo)表示16 個鄰近的柵格。令函數(shù)f表示點對應(yīng)位置的柵格障礙物概率，可以分別計算出函數(shù)f（Q0）和的值通過式（10）與式（11），式（12）。

其中A，B，C 分別為

分別求X，Y 軸方向的偏導(dǎo)：

式中：A′，C′分別為

4 激光點云匹配的優(yōu)化求解

激光點云匹配是將兩幀激光點云彼此對齊，本文以前一幀激光點云數(shù)據(jù)構(gòu)造一個柵格地圖，然后根據(jù)式（5）求取第二幀激光點云所在時刻的理論位姿，并作為第二幀精確位姿迭代求解的初始位姿。

式中：函數(shù)Ti（ξL）表示將激光雷達坐標(biāo)系下的點轉(zhuǎn)換到全局地圖坐標(biāo)系下。

式（13）為一個非線性最小二乘問題，可將其轉(zhuǎn)換成一系列的線性最小二乘問題，通過迭代計算來求解[9]。最常用的算法為高斯牛頓法[10]，該方法用JTJ 作為牛頓法中二階Hessian 矩陣的近似，從而省略了計算Hessian 矩陣的過程。高斯牛頓法原則上要求近似Hessian 的矩陣JTJ 是可逆的，但實際中可能為奇異矩陣或者病態(tài)的，導(dǎo)致算法不收斂。列文伯格-馬夸爾特算法可以避免高斯牛頓法出現(xiàn)的病態(tài)與非奇異問題，提供更加穩(wěn)定的增量，該算法主要是在高斯牛頓法的基礎(chǔ)上加上信賴區(qū)域以改進其問題。本文選擇列文伯格-馬夸爾特算法進行優(yōu)化求解。

從AGV 運動學(xué)方程可以獲得位姿估計的初始值，然后通過優(yōu)化目標(biāo)方程去估計增量。

對上式進行一階泰勒線性展開[5]，并將展開后的方程對ΔξL求導(dǎo)并令其等于0，可得到下式：

式中：H，J 為

式中：I 為單位矩陣；μ 為一個正實數(shù)。當(dāng)μ=0 時，該算法退化為高斯牛頓法；當(dāng)μ 很大時，該算法退化成了最速下降法。μ 的值會根據(jù)優(yōu)化進行的狀態(tài)隨時調(diào)整，這個參數(shù)即控制著優(yōu)化前進的方向與步長。主要通過下述策略來對μ 值大小進行調(diào)整，需要引入2 個額外的參數(shù)：ρ，ν，其中ρ 由下式計算得出；ν的初始值給定為2。

其中ψ 為：

在每輪迭代過程中ρ 代表每次前進近似函數(shù)的變化量與F（ξL）變化量的差異。如果ρ>0，就意味著此次迭代前進的方向是正確的，繼續(xù)進行下一輪迭代；反之，則迭代前進的方向不對，就需要利用另外一個參數(shù)υ 來調(diào)整，調(diào)整的策略是μ＝μ×ν。持續(xù)此迭代過程直至滿足終止條件，即可獲得前后兩幀的相對位姿。

5 實驗與分析

為驗證本文所提出方法的準(zhǔn)確性，需要通過激光雷達采集真實的數(shù)據(jù)進行實驗。由于AGV 主要的運行環(huán)境為室內(nèi)場景，因此選用2D 激光雷達。本文采用德國SICK 公司的NAV350 激光雷達為室內(nèi)型2D 激光雷達傳感器，包括反光板模式和環(huán)境輪廓檢測模式，在環(huán)境輪廓檢測模式下，能夠輸出周圍輪廓的點云信息。該雷達測量距離為70 m，每幀數(shù)據(jù)有1440 個點。激光雷達安裝在AGV 上進行工作，并且為了獲得準(zhǔn)確數(shù)據(jù)，在墻上安裝反光板，該型號激光雷達可根據(jù)反光板計算出真實位姿，實驗設(shè)備如圖2所示。

圖2 叉車型AGV 實物圖Fig.2 Physical picture of forklift AGV

在反光板模式下，激光雷達可通過對反光板測量計算得到位姿，因此以反光板計算得到的位姿作為基準(zhǔn)，分析本文算法求解位姿的誤差。圖3 給出了相鄰兩幀激光點云數(shù)據(jù)圖，本文算法主要就是在環(huán)境輪廓檢測模式下，根據(jù)兩幀點云數(shù)據(jù)計算出兩幀之間的相對位姿。

圖3 相鄰兩幀激光點云分布圖Fig.3 Distribution of laser point cloud of two adjacent frames

在反光板模式下，測出的位置為x=0.3432 m；y=0.2034 m；角度的弧度值為0.1854。使用本文算法計算出兩幀之間的相對位姿為x=0.3435 m，y=0.2161 m；角度的弧度值為0.1841。因此在x 軸方向的誤差為0.0003 m；y 軸方向上的誤差為0.013 m，角度弧度值的誤差為0.0013。通過上述實驗驗證了本文算法的有效性與穩(wěn)定性。

6 結(jié)語

本文以叉車型AGV 為研究對象，基于激光雷達研究前后兩幀激光點云匹配問題，從而求解兩幀點云所對應(yīng)的AGV 位姿變換關(guān)系，最終應(yīng)用于AGV的定位與導(dǎo)航。算法對前后兩幀激光點云數(shù)據(jù)進行處理，利用第一幀點云數(shù)據(jù)構(gòu)造出一個柵格地圖，使第二幀點云數(shù)據(jù)與前一幀構(gòu)造的柵格地圖對齊，以此來構(gòu)建誤差方程，并采用非線性最小二乘法求解誤差函數(shù)的最小值，從而求得前后兩幀的相對位姿。實驗結(jié)果表明該算法能有效地計算出相鄰兩幀點云數(shù)據(jù)的相對位姿，因此解決了SLAM 前端點云匹配計算位姿的問題，同時SLAM 后端也可利用到點云匹配來進行回環(huán)檢測，在檢測到回環(huán)時則進行全局的圖優(yōu)化，提高AGV 在整個路徑中的定位精度。