基于非線性優(yōu)化的激光雷達(dá)在線標(biāo)定算法

楊 超，李天劍，胡 歡，黃 民

(北京信息科技大學(xué) 機(jī)電學(xué)院，北京 100192)

0 引 言

近些年來(lái)，基于激光雷達(dá)定位的自動(dòng)引導(dǎo)車(automated guided vehicle，AGV)逐漸運(yùn)用于各個(gè)不同領(lǐng)域。通過(guò)激光雷達(dá)作為主傳感器進(jìn)行精確位姿估計(jì)的前提是預(yù)先知道傳感器的外參配置。對(duì)于一個(gè)機(jī)器人系統(tǒng)而言，標(biāo)定外參通常是機(jī)器人工作前的首要步驟[1]。

在機(jī)器人領(lǐng)域，關(guān)于外參標(biāo)定的研究一直是熱點(diǎn)所在，尤其是即時(shí)定位與建圖技術(shù)(simultaneous localization and mapping，SLAM)技術(shù)發(fā)展火熱以后。關(guān)于激光雷達(dá)的外參標(biāo)定研究較多的包括相機(jī)與激光雷達(dá)的聯(lián)合標(biāo)定等，黎云飛等提出的基于fmincon法的單線激光與單目相機(jī)外參標(biāo)定法[2]，降低了標(biāo)定計(jì)算量。姚文濤等提出了一種自適應(yīng)攝像機(jī)與激光雷達(dá)聯(lián)合標(biāo)定算法[3]，簡(jiǎn)化了標(biāo)定過(guò)程。韓棟斌等提出了基于多點(diǎn)匹配的三維激光雷達(dá)外參標(biāo)定方法[4]，減小了標(biāo)定初值對(duì)標(biāo)定結(jié)果的影響。Zhe Gao等提出了利用簡(jiǎn)單的平面測(cè)量方法在硬件要求較低情況下對(duì)2D激光雷達(dá)的標(biāo)定方法[5]。祝飛等通過(guò)融合多種棋盤格約束的面陣相機(jī)與線激光約束，減少了激光噪點(diǎn)對(duì)標(biāo)定結(jié)果的影響，提升了標(biāo)定精度[6]。Miguel Oliveira等利用ROS框架提出了一種多模型的外參標(biāo)定算法，同時(shí)標(biāo)定多個(gè)外參并簡(jiǎn)化了標(biāo)定流程[7]。在利用里程計(jì)的激光標(biāo)定方面，Andrea Censi曾對(duì)此問(wèn)題展開(kāi)研究，提出了一種里程計(jì)與激光雷達(dá)的聯(lián)合標(biāo)定方案，該方案解決了對(duì)機(jī)器人內(nèi)參與外參的同時(shí)標(biāo)定，該方案在外參解決上采用了閉式求解。目前，對(duì)于7×24 h工作的移動(dòng)機(jī)器人而言，其激光外參需要進(jìn)行實(shí)時(shí)在線標(biāo)定，以保持其精度的準(zhǔn)確性，上述標(biāo)定方案通常都需要輔助以特定標(biāo)定板等工具進(jìn)行標(biāo)定，同時(shí)受限于環(huán)境(對(duì)標(biāo)定環(huán)境有嚴(yán)格要求)，基本都是離線標(biāo)定方案，不利于機(jī)器人進(jìn)行在線標(biāo)定，另外目前也存在標(biāo)定精度不夠等問(wèn)題。

針對(duì)上述問(wèn)題，基于里程計(jì)的激光標(biāo)定算法，本文提出了一種利用非線性優(yōu)化的求解方式，該方法將標(biāo)定問(wèn)題建模為觀測(cè)與預(yù)測(cè)之間的非線性模型，能夠利用非線性最小二乘方案進(jìn)行求解，通過(guò)誤差最小化，可以得到較為精確的標(biāo)定值。

1 模型構(gòu)建

使用2D激光雷達(dá)的移動(dòng)機(jī)器人主要考慮其在2D平面中的運(yùn)動(dòng)，忽略機(jī)器人的形態(tài)大小，單純以點(diǎn)來(lái)描述機(jī)器人，那么在二維平面中描述機(jī)器人的位姿需要3個(gè)自由度，設(shè)位姿為p，則

p=(x,y,θ)

其中，x,y代表機(jī)器人在對(duì)應(yīng)坐標(biāo)系下的位置，θ描述機(jī)器人的姿態(tài)，即其繞垂于x,y平面的旋轉(zhuǎn)角，稱之為偏航角。

機(jī)器人的姿態(tài)觀測(cè)可通過(guò)傳感器獲得，本文以傳感器為2D激光雷達(dá)以及碼盤為主的差速運(yùn)動(dòng)機(jī)器人為研究對(duì)象，通過(guò)輪式碼盤可得到機(jī)器人的里程計(jì)數(shù)據(jù)，即機(jī)器人本體的位姿變換。而2D激光雷達(dá)亦可通過(guò)幀間匹配得到雷達(dá)在世界坐標(biāo)系下的位姿變換。激光雷達(dá)與機(jī)器人本體間存在靜態(tài)的位姿變換，即所要求解的標(biāo)定外參。根據(jù)二者之間的關(guān)系得到非線性數(shù)學(xué)模型，通過(guò)對(duì)非線性問(wèn)題求解，即可得到激光雷達(dá)外參。

1.1 非線性優(yōu)化模型構(gòu)建

圖1描述了機(jī)器人在一定間隔時(shí)間t內(nèi)機(jī)器人各坐標(biāo)系之間的運(yùn)動(dòng)關(guān)系，假設(shè)機(jī)器人從t1時(shí)刻運(yùn)動(dòng)到t2時(shí)刻，圖中O1代表t1時(shí)刻的碼盤航跡推算得到的世界坐標(biāo)系下里程計(jì)位姿，即機(jī)器人車體位姿，O2為t2時(shí)刻對(duì)應(yīng)的世界坐標(biāo)系下里程計(jì)位姿，To代表里程計(jì)航跡推算所得機(jī)器人在t1到t2時(shí)刻對(duì)應(yīng)坐標(biāo)變換。S1表示t1時(shí)刻激光雷達(dá)觀測(cè)所得激光雷達(dá)在世界坐標(biāo)系下位姿，S2表示t2時(shí)刻激光雷達(dá)觀測(cè)得到激光雷達(dá)在世界坐標(biāo)系下位姿，Ts代表激光雷達(dá)觀測(cè)所得雷達(dá)在t1到t2時(shí)刻之間的坐標(biāo)變換。Tx表示激光雷達(dá)在機(jī)器人坐標(biāo)系下的靜態(tài)坐標(biāo)變換，即所求外參對(duì)應(yīng)的坐標(biāo)變換，該值為靜態(tài)值，不會(huì)隨機(jī)器人運(yùn)動(dòng)而發(fā)生變化。

圖1 機(jī)器人坐標(biāo)系間運(yùn)動(dòng)關(guān)系

根據(jù)上述對(duì)應(yīng)關(guān)系，目前已知在不同時(shí)刻下激光雷達(dá)及里程計(jì)在世界坐標(biāo)系下的對(duì)應(yīng)位姿，而根據(jù)歐式變換，可進(jìn)一步獲得在t2時(shí)刻機(jī)器人在t1時(shí)刻里程計(jì)坐標(biāo)系下的坐標(biāo)變換，首先將t1時(shí)刻世界坐標(biāo)系下里程計(jì)位姿轉(zhuǎn)換為歐式變換矩陣

(1)

其中，Oθ1，Ox1，Oy1分別為機(jī)器人里程計(jì)在t1時(shí)刻對(duì)應(yīng)的位置參數(shù)與位姿參數(shù)。To1代表從世界坐標(biāo)系到t1時(shí)刻機(jī)器人坐標(biāo)系的坐標(biāo)變換。

將t2時(shí)刻世界坐標(biāo)系下里程計(jì)位姿轉(zhuǎn)換為歐式變換矩陣

(2)

To2代表從世界坐標(biāo)系在t2時(shí)刻機(jī)器人坐標(biāo)系的坐標(biāo)變換。

根據(jù)歐式群變換理論，可獲得t2時(shí)刻機(jī)器人到t1時(shí)刻機(jī)器人的坐標(biāo)變換為

To=To1To2

(3)

同理，可獲得t2時(shí)刻激光雷達(dá)在t1時(shí)刻對(duì)應(yīng)激光雷達(dá)坐標(biāo)系下坐標(biāo)變換為

Ts=Ts1Ts2

(4)

根據(jù)式(3)、式(4)求解得到坐標(biāo)變換To、Ts，以及激光雷達(dá)到機(jī)器人坐標(biāo)系下的坐標(biāo)變換Tx，可得到t2時(shí)刻激光雷達(dá)到t1時(shí)刻里程計(jì)坐標(biāo)系下的坐標(biāo)變換，該坐標(biāo)變換可通過(guò)兩種方式得到

T1-2=TxTs

(5)

T1-2=ToTx

(6)

將這兩個(gè)等式連列，消去T1-2，則得到下面方程

TxTs=ToTx

(7)

將式(7)稍作化簡(jiǎn)，則得

(8)

式(8)即所求數(shù)學(xué)模型，該數(shù)學(xué)模型為非線性化數(shù)學(xué)模型。

1.2 里程計(jì)位姿求解

差速運(yùn)動(dòng)機(jī)器人的里程計(jì)運(yùn)動(dòng)學(xué)模型依賴固連于連桿兩端的雙電機(jī)通過(guò)速度差分求得，如圖2所示，其對(duì)應(yīng)機(jī)器人速度及角速度為

圖2 差速運(yùn)動(dòng)學(xué)模型

(9)

(10)

其中，vL與vR分別為左右輪輪速，d為輪間距。

假設(shè)機(jī)器人在t1時(shí)刻位姿為p(x，y，θ), 在理想情況下，通過(guò)航機(jī)推算，t2時(shí)刻機(jī)器人的位姿為

(11)

而實(shí)際運(yùn)用中，機(jī)器人的運(yùn)動(dòng)收到噪聲的影響，真實(shí)的速度與測(cè)量得到的速度存在一定誤差。將這種誤差描述為以0為中心的有限方差的隨機(jī)變量[8]。實(shí)際的速度及角速度表達(dá)如下

(12)

其中，εα1v2+α2w2,εα3v2+α4w2為方差為α1v2+α2w2，α3v2+α4w2， 均值為0的誤差變量，α1～α4為機(jī)器人特定的誤差參數(shù)。

因此，對(duì)應(yīng)的運(yùn)動(dòng)學(xué)航跡推算模型為

(13)

1.3 激光雷達(dá)位姿求解

激光雷達(dá)的位姿求解有多種方式，SLAM技術(shù)中大量涉及關(guān)于激光傳感器的定位問(wèn)題，主流的激光雷達(dá)位姿求解主要通過(guò)幀間匹配的方式，即通過(guò)匹配相鄰兩幀激光或關(guān)鍵激光幀的重合部分，求解得到激光雷達(dá)的位姿變換。而匹配方式也包含多種方法，包括原始的迭代鄰近點(diǎn)匹配[9]，融合速度估計(jì)的VICP[10]匹配以及點(diǎn)線匹配等。目前，在匹配方式中，點(diǎn)線匹配方式效果較好，本文選用點(diǎn)線匹配來(lái)進(jìn)行雷達(dá)位姿求解。

點(diǎn)線匹配算法又稱點(diǎn)線迭代臨近點(diǎn)匹配，由于兩幀激光在掃描過(guò)程中，激光點(diǎn)并不會(huì)完全進(jìn)行點(diǎn)到點(diǎn)重合，該算法將原始的點(diǎn)到點(diǎn)匹配優(yōu)化為點(diǎn)到線直接的匹配，用點(diǎn)去匹配最鄰近的直線段，提升了激光幀間的精度[11]，如圖3所示。其算法流程如下：

圖3 點(diǎn)線匹配

(1)列出兩幀激光之間的變換關(guān)系，將當(dāng)前激光幀中激光點(diǎn)用pi表示，將參考幀中激光點(diǎn)用piw表示，則激光點(diǎn)之間的變換關(guān)系為

(14)

其中，Δx,Δy表示激光點(diǎn)從pi到piw的位置變換，θk表示激光點(diǎn)從pi到piw的位姿變換。

(2)計(jì)算激光幀從當(dāng)前變換到參考幀坐標(biāo)系后，激光點(diǎn)pi到臨近兩個(gè)激光點(diǎn)所組成的線段之間的最近距離；

(3)通過(guò)最小二乘，將此問(wèn)題表達(dá)為非線性優(yōu)化問(wèn)題，通過(guò)最小化殘差，求解得到兩幀激光之間的坐標(biāo)變換，其優(yōu)化模型為

(15)

其中，i∈{1,2,…,n}，n為當(dāng)前激光幀與參考激光幀所匹配得到的激光點(diǎn)數(shù)量，pj1i為參考激光幀中距離點(diǎn)piw最近的激光點(diǎn)，niT為過(guò)激光點(diǎn)piw且垂直于線段pj1ipj2i的單位法向量。

2 外參求解

2.1 實(shí)際考慮

根據(jù)以上所述，已經(jīng)求得激光雷達(dá)位姿變換Ts，里程計(jì)位姿變換To，可將二者帶入式(8)中，由于該式為非線性數(shù)學(xué)模型，無(wú)法通過(guò)線性方式之間求解。設(shè)所解Tx如下

由于Tx為坐標(biāo)變換，則x7=x8=0，x9=1，x1=x5，x2=-x4則Tx的形式變換為

且存在約束x12+x22=1， 代入式(8)后可發(fā)現(xiàn)該方程存在解析解，可通過(guò)拉格朗日乘子將該問(wèn)題轉(zhuǎn)換為閉式解形式。但這種解析解僅適用于理想情況，在實(shí)際情況中，里程計(jì)中速度與角速度的獲取本身帶有高斯誤差，而在實(shí)際場(chǎng)景中，機(jī)器人要面臨不同的路面情況，其左右輪會(huì)存在不同程度打滑，這都是不可忽略的誤差。其次，激光雷達(dá)的波束模型同樣帶有噪聲，實(shí)際中的一面直墻在激光雷達(dá)的觀測(cè)中不盡然是一條直線，如圖4所示，同時(shí)在幀間匹配過(guò)程中，存在誤匹配的情況，這些都會(huì)造成激光雷達(dá)位姿求解的誤差。這些誤差最終都會(huì)造成解析解的標(biāo)定精度，Andrea Censi曾將此閉式解問(wèn)題通過(guò)反復(fù)迭代標(biāo)定來(lái)提高標(biāo)定精度。

圖4 激光噪點(diǎn)

本文提供另一種求解方式，直接將此問(wèn)題作為優(yōu)化問(wèn)題進(jìn)行求解。

2.2 非線性求解

考慮將問(wèn)題轉(zhuǎn)化為優(yōu)化問(wèn)題，將式(8)轉(zhuǎn)化為如下數(shù)學(xué)模型

eT=Ts-T′o

(16)

e=z′-o′x

(17)

其中，e為位姿殘差，z為激光雷達(dá)在世界坐標(biāo)系中位姿，o′x為里程計(jì)位姿通過(guò)外參求得激光雷達(dá)在世界坐標(biāo)系中位姿，該問(wèn)題可通過(guò)非線性最小二乘進(jìn)行求解。

目標(biāo)為最小化預(yù)測(cè)和觀測(cè)的差，將里程計(jì)得到坐標(biāo)變換轉(zhuǎn)換得到的激光坐標(biāo)變換作為預(yù)測(cè)值，將激光幀間匹配得到的坐標(biāo)變換作為觀測(cè)值，則

ei(x)=zi-fi(x)

(18)

zi為ti時(shí)刻激光觀測(cè)位姿，fi(x) 為ti時(shí)刻里程計(jì)預(yù)測(cè)位姿，ei(x) 為ti時(shí)刻位姿殘差。假設(shè)誤差服從高斯分布，設(shè)其對(duì)應(yīng)的信息矩陣為Σi， 因此該觀測(cè)值誤差的平方定義為

Ei(x)=ei(x)TΣiei(x)

(19)

因此，非線性最小二乘的目標(biāo)函數(shù)為

(20)

對(duì)誤差函數(shù)進(jìn)行線性化

ei(x+Δx)=ei(x)+Ji(x)Δx

其中，雅克比矩陣Ji為映射函數(shù)對(duì)狀態(tài)向量x的導(dǎo)數(shù)，其值為

因此，目標(biāo)函數(shù)轉(zhuǎn)化為

(21)

其中， F(x+Δx) 為關(guān)于變量Δx的二次函數(shù)，其極值可通過(guò)令其關(guān)于Δx的導(dǎo)數(shù)為0求解得到

(22)

令x=x+Δx, 然后不斷迭代，直到收斂。

2.3 其它考慮

(1)由于激光雷達(dá)幀間匹配對(duì)初值要求較高，在初值較為不準(zhǔn)確的前提下有可能在解算時(shí)陷入局部最優(yōu)值，因此將里程計(jì)得到位姿變換作為初值進(jìn)行幀間匹配。

(2)里程計(jì)采樣過(guò)程中，輪子可能存在打滑，造成里程計(jì)數(shù)據(jù)的不準(zhǔn)確，因此在優(yōu)化之前，首先進(jìn)行濾波，去除一定百分比的較大誤差。

(3)為進(jìn)一步提高精度，采用二次優(yōu)化的方式，即一次優(yōu)化完畢后，將第一次優(yōu)化所得外參作為初值帶入進(jìn)行二次優(yōu)化，得到較為精確的外參。

(4)算法實(shí)現(xiàn)以ROS[12]系統(tǒng)為框架，ROS(robot operating system)為開(kāi)源機(jī)器人操作系統(tǒng)，其利用節(jié)點(diǎn)/話題通信方式以及功能包開(kāi)發(fā)方式加快了機(jī)器人軟件的開(kāi)發(fā)進(jìn)程，其中，ROS中便捷的TF轉(zhuǎn)換與監(jiān)聽(tīng)功能能夠幫助進(jìn)行里程計(jì)及激光雷達(dá)數(shù)據(jù)同步。

整體算法流程如下：

(1)訂閱激光雷達(dá)數(shù)據(jù)/scan；

(2)通過(guò)訂閱/tf話題得到每幀激光雷達(dá)對(duì)應(yīng)時(shí)間里程計(jì)位姿；

(3)通過(guò)兩幀里程計(jì)位姿得到相鄰里程計(jì)位姿差 d_point_odom；

(4)判斷d_point_odom運(yùn)動(dòng)距離間隔與角度間隔是否足夠大，若不滿足返回第(1)步；

(5)通過(guò)給定外參初值，將d_point_odom轉(zhuǎn)換為激光坐標(biāo)系下位姿tmpPose，并作為初值傳遞給幀間匹配PL-ICP；

(6)通過(guò)幀間匹配，得到相鄰激光幀位姿差d_point_scan；

(7)通過(guò)角度偏差進(jìn)行濾波；

(8)構(gòu)建非線性優(yōu)化模型目標(biāo)函數(shù)，添加殘差；

(9)達(dá)到預(yù)定優(yōu)化數(shù)量后，進(jìn)行迭代優(yōu)化求解，得到一次標(biāo)定參數(shù)；

(10)將所得標(biāo)定參數(shù)作為新一次優(yōu)化初值進(jìn)行二次優(yōu)化，返回第(1)步；

(11)達(dá)到優(yōu)化數(shù)量后，通過(guò)距離偏差進(jìn)行濾波，迭代求解后，得到最終外參。

3 實(shí) 驗(yàn)

本文使用搭載雙激光雷達(dá)的差速運(yùn)動(dòng)機(jī)器人為實(shí)驗(yàn)對(duì)象，其雷達(dá)型號(hào)為SICK TIM561。

(1)機(jī)器人選擇：本文使用機(jī)器人為中型AGV，該機(jī)器人其左后方與右后方分別放置一枚SICK TIM561激光雷達(dá)(圖5)。同時(shí)安裝有雙電機(jī)及輪式編碼器，為差速運(yùn)動(dòng)機(jī)器人。

圖5 機(jī)器人及激光雷達(dá)

(2)環(huán)境選擇：考慮到幀間匹配的準(zhǔn)確性，為保證標(biāo)定準(zhǔn)確，標(biāo)定環(huán)境傾向于選擇結(jié)構(gòu)化環(huán)境，本文使用木板搭建方形結(jié)構(gòu)環(huán)境(圖6)，這種環(huán)境能夠保證幀間匹配(點(diǎn)線匹配)的準(zhǔn)確性，環(huán)境大小為(3m×3m)。

圖6 標(biāo)定環(huán)境

(3)激光雷達(dá)：本文選用激光雷達(dá)型號(hào)為SICK TIM561，激光雷達(dá)掃描頻率為15 Hz角度分辨率為0.33度，掃描范圍為0.05 m～10 m，掃描角度范圍為270°，該激光雷達(dá)可在一幀激光內(nèi)得到721束激光束，影響該激光的掃描精度因素包括掃描距離，由于激光本身在觀察過(guò)程中存在噪聲，當(dāng)激光距離障礙物過(guò)近或過(guò)遠(yuǎn)時(shí)，其噪聲相對(duì)較大，在實(shí)驗(yàn)中，需首先對(duì)激光進(jìn)行預(yù)處理濾波，排除較近與較遠(yuǎn)距離數(shù)據(jù)。激光在機(jī)器人運(yùn)行過(guò)程中會(huì)存在畸變問(wèn)題，尤其是激光幀率較小(5 hz ～10 hz)，其畸變隨機(jī)器人運(yùn)行速度提高而增大，在標(biāo)定過(guò)程中，應(yīng)慢速運(yùn)行。另外，在掃描反射比低于10%(深色物體)時(shí)，因光本身的性質(zhì)，激光的掃描范圍會(huì)相應(yīng)減小，最遠(yuǎn)距離為8 m。

(4)數(shù)據(jù)提取：在進(jìn)行優(yōu)化前，需提取優(yōu)化數(shù)據(jù)，包括激光幀間匹配數(shù)據(jù)以及里程計(jì)的變換位姿。由于輪子打滑等因素的影響，過(guò)短時(shí)間內(nèi)，里程計(jì)的誤差將會(huì)對(duì)里程計(jì)位姿造成顯著影響。因此，在數(shù)據(jù)提取時(shí)，應(yīng)盡量選擇稍長(zhǎng)時(shí)間段內(nèi)的數(shù)據(jù)。

(5)軌跡選擇：激光外參標(biāo)定包括兩個(gè)部分：位置參數(shù)與姿態(tài)參數(shù)，單純的平移變換無(wú)法標(biāo)定位置參數(shù)，單純的旋轉(zhuǎn)變換無(wú)法標(biāo)定姿態(tài)參數(shù)。機(jī)器人在運(yùn)動(dòng)過(guò)程中需包含平移與旋轉(zhuǎn)軌跡。

3.1 外參標(biāo)定波動(dòng)值

在激光雷達(dá)外參真值無(wú)法實(shí)際獲得的情況下，通過(guò)多組實(shí)驗(yàn)的外參波動(dòng)大小驗(yàn)證算法有效性。通過(guò)在所搭建結(jié)構(gòu)化環(huán)境中，進(jìn)行多次實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果外參見(jiàn)表1。

表1 機(jī)器人左右激光外參標(biāo)定值

從標(biāo)定結(jié)果可以看出，左激光外參在x方向波動(dòng)值在1 mm 以內(nèi)，y方向波動(dòng)值在2 mm以內(nèi)，theta值波動(dòng)范圍在0.01弧度以內(nèi)，右激光x方向波動(dòng)值在2 mm以內(nèi)，y方向波動(dòng)值在2 mm以內(nèi)，theta值波動(dòng)范圍在0.01弧度以內(nèi)。

3.2 雙激光雷達(dá)驗(yàn)證

SICK TIM561激光雷達(dá)的掃描角度范圍為270°，在rviz中觀察標(biāo)定運(yùn)動(dòng)狀態(tài)下左右激光的點(diǎn)云匹配程度如圖7、圖8所示。

圖7 左右激光點(diǎn)云匹配(1)

圖8 左右激光匹配(2)

圖中黑色激光點(diǎn)云為左激光捕獲點(diǎn)云，白色為右激光捕獲點(diǎn)云。左圖為標(biāo)定前，即初值分別為(-0.335，0.36，2.36)(-0.335，-0.36，2.36)，左右激光幀捕獲點(diǎn)云無(wú)法嚴(yán)格重合，標(biāo)定后，取標(biāo)定參數(shù)為(-0.3375，0.350，2.34)(-0.330，-0.360，-2.37)，其左右激光點(diǎn)云重合程度優(yōu)于標(biāo)定前。

3.3 對(duì)比實(shí)驗(yàn)

與閉式求解方式精度對(duì)比：錄制相同數(shù)據(jù)包，分別通過(guò)兩種算法對(duì)同一數(shù)據(jù)包進(jìn)行處理，處理結(jié)果見(jiàn)表2。

表2 算法對(duì)比

在相同數(shù)據(jù)包下，閉式求解x方向波動(dòng)為6 mm，y方向波動(dòng)為6 mm，theta值波動(dòng)為0.02弧度，與本文算法相比，本文算法精度優(yōu)于閉式求解方式。

3.4 實(shí)際項(xiàng)目場(chǎng)景應(yīng)用

本文算法適用于不同場(chǎng)景，圖為在實(shí)際工廠中進(jìn)行在線標(biāo)定后標(biāo)定效果，非結(jié)構(gòu)化環(huán)境下標(biāo)定效果如圖9、圖10 所示。

圖9 實(shí)際工廠環(huán)境標(biāo)定效果(1)

圖10 實(shí)際工廠環(huán)境標(biāo)定效果(2)

在非結(jié)構(gòu)化環(huán)境中，激光噪點(diǎn)較多，激光標(biāo)定精度不如結(jié)構(gòu)化環(huán)境，但標(biāo)定前后左右激光仍可達(dá)到較好的匹配程度，算法可有效運(yùn)用于非結(jié)構(gòu)化環(huán)境。

3.5 激光外參標(biāo)定在SLAM中應(yīng)用

SLAM技術(shù)在定位過(guò)程中，對(duì)傳感器外參精度有較高要求，不精確的激光外參會(huì)對(duì)機(jī)器人定位效果產(chǎn)生較大影響，進(jìn)而影響建圖效果，圖11為使用本文算法標(biāo)定前后，利用圖優(yōu)化SLAM算法進(jìn)行建圖效果。

圖11 標(biāo)定前后SLAM效果

從圖11可看到，標(biāo)定前，SLAM建圖存在重影問(wèn)題，在A、B、C這3處位置存在不同程度重影，標(biāo)定后，重影問(wèn)題明顯減小。

4 結(jié)束語(yǔ)

針對(duì)以輪式里程計(jì)以及激光雷達(dá)為主要傳感器的差速機(jī)器人的激光標(biāo)定問(wèn)題，提出了一種基于優(yōu)化方式的標(biāo)定算法，算法將標(biāo)定問(wèn)題轉(zhuǎn)換為預(yù)測(cè)與觀測(cè)之間的非線性優(yōu)化模型，標(biāo)定過(guò)程中，對(duì)標(biāo)定軌跡有一定要求，需包含平移與旋轉(zhuǎn)，就標(biāo)定環(huán)境而言，不僅適用于結(jié)構(gòu)化環(huán)境，同樣適用于非結(jié)構(gòu)化環(huán)境，結(jié)構(gòu)化環(huán)境中的標(biāo)定結(jié)果優(yōu)于非結(jié)構(gòu)化環(huán)境。通過(guò)實(shí)際實(shí)驗(yàn)驗(yàn)證，通過(guò)雙激光雷達(dá)點(diǎn)云匹配，標(biāo)定后點(diǎn)云重合程度明顯優(yōu)于標(biāo)定前，在結(jié)構(gòu)化環(huán)境中，該算法的標(biāo)定波動(dòng)值在x、y方向誤差保持在2 mm以內(nèi)，角度誤差保持在0.01弧度以內(nèi)，優(yōu)于閉式求解方式。在實(shí)際工廠環(huán)境標(biāo)定實(shí)驗(yàn)中，該算法標(biāo)定結(jié)果能夠有效提高點(diǎn)云匹配精度，在SLAM應(yīng)用方面，標(biāo)定后激光建圖精度明顯優(yōu)于標(biāo)定前。