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

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

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

1 掃描匹配

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

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

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

2.1 AGV 運(yùn)動(dòng)學(xué)模型

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

圖1 AGV 運(yùn)動(dòng)學(xué)模型Fig.1 AGV kinematics model

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中A，B，C 分別為

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

式中：A′，C′分別為

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

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

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

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

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

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

式中：H，J 為

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

其中ψ 為：

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

5 實(shí)驗(yàn)與分析

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

圖2 叉車型AGV 實(shí)物圖Fig.2 Physical picture of forklift AGV

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

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

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

6 結(jié)語

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