王 雪，李登峰，黃杉杉，龔慶紅

(長(zhǎng)安大學(xué)嵌入式系統(tǒng)應(yīng)用研究所，陜西 西安 710061)

0 引言

為了降低成本，當(dāng)前市場(chǎng)上絕大多數(shù)智能小車都使用一些低成本的移動(dòng)底盤和激光雷達(dá)作為智能小車的主要傳感器。成本降低通常會(huì)降低智能小車?yán)锩鎮(zhèn)鞲衅鞯募庸ぞ冉档?，從而?dǎo)致里程計(jì)信息和激光雷達(dá)所掃描出來(lái)的數(shù)據(jù)信息的準(zhǔn)確性降低[1]。而傳感器輪式里程計(jì)的精度越差，智能小車運(yùn)動(dòng)過(guò)程中產(chǎn)生的累計(jì)誤差也就越大，使智能小車所構(gòu)建的環(huán)境地圖效果也就越差。低成本的激光雷達(dá)一般采用單線雷達(dá)[2]，掃描頻率較低。當(dāng)掃描頻率較低時(shí)，假如小車快速運(yùn)動(dòng)，激光雷達(dá)所掃描出來(lái)的位姿信息就會(huì)發(fā)生嚴(yán)重的畸變。為了解決上述問題，本文介紹了輪式里程計(jì)輔助方法的原理。但是由于輪式里程計(jì)輔助方法是位置線性假設(shè)，位置線性假設(shè)沒有誤差線性假設(shè)合理。因此，本文提出了輪式里程計(jì)和迭代最近點(diǎn)(iterative closest point，ICP)方法的融合。

1 傳感器輪式里程計(jì)輔助算法

去除激光雷達(dá)運(yùn)動(dòng)畸變的方法有ICP、速度估計(jì)迭代最近點(diǎn)(velocity estimation iterative closest point,VICP)、里程計(jì)的輔助方法。但是ICP和VICP法存在一些問題：一是當(dāng)激光雷達(dá)掃描頻率是低幀率時(shí)，對(duì)小車勻速運(yùn)動(dòng)的假設(shè)不成立；二是數(shù)據(jù)預(yù)處理和狀態(tài)估計(jì)過(guò)程耦合。因此，提出了傳感器的輔助方法，包括里程計(jì)輔助方法和慣性測(cè)量組合(inertial measuring unit,IMU)輔助方法。此方法的優(yōu)點(diǎn)是可以比較準(zhǔn)確地估計(jì)局部位姿，還可以跟狀態(tài)估計(jì)解耦。但I(xiàn)MU輔助方法測(cè)量線性加速度的精度較差，并且二次積分在局部的精度也很差。相比較于IMU輔助方法，輪式里程計(jì)輔助方法局部位置測(cè)量和角度測(cè)量的精度較高，速度更新更快[3]。

輪式里程計(jì)輔助方法的原理：運(yùn)用分段線性插值函數(shù)估計(jì)每個(gè)激光點(diǎn)對(duì)應(yīng)的位姿，將得到的位姿進(jìn)行坐標(biāo)轉(zhuǎn)換，最后轉(zhuǎn)換為激光數(shù)據(jù)并發(fā)布。

已知當(dāng)前幀激光起始時(shí)間結(jié)束時(shí)間為ts、te，兩個(gè)激光束之間的時(shí)間間隔為Δt。輪式里程計(jì)輔助算法步驟如下。

①利用拉格朗日插值法得到一條直線L1，而l

(1)

(2)

(3)

②用分段線性插值函數(shù)對(duì)二次曲線進(jìn)行近似。

首先按照分割時(shí)間分段，然后對(duì)每個(gè)時(shí)間分段里面的激光點(diǎn)進(jìn)行線性插值[4]，從而得出該分段中每個(gè)激光點(diǎn)在激光雷達(dá)坐標(biāo)系下對(duì)應(yīng)的里程計(jì)的位姿。

在ts、te時(shí)間段內(nèi)，一共取k個(gè)位姿{(lán)Ps,Ps+1,...,Ps+k-2,Pe}。k個(gè)位姿通過(guò)線性插值獲取。在這k個(gè)位姿之間進(jìn)行拉格朗日線性插值。

設(shè)Ps、Ps+1之間有N個(gè)位姿{(lán)Ps,Ps+1,Ps+2,...,Ps+N-2,Ps+1}，則：

(4)

③坐標(biāo)系的轉(zhuǎn)換。

根據(jù)步驟②，可以求出每個(gè)激光點(diǎn)對(duì)應(yīng)的位姿Pi。設(shè)x′i為轉(zhuǎn)換之后的坐標(biāo)、xi為轉(zhuǎn)換之前的坐標(biāo)，則：

(5)

(6)

(7)

2 輪式里程計(jì)方法和ICP方法的融合

上述輪式里程計(jì)方法采用的是位置線性假設(shè)，位置誤差的線性假設(shè)比位置假設(shè)更加合理。因此，本文提出了融合的方法，即將輪式里程計(jì)方法和ICP方法進(jìn)行融合。

融合算法的步驟如下。

①用輪式里程計(jì)輔助方法去除運(yùn)動(dòng)信息中絕大部分的運(yùn)動(dòng)畸變。

②用ICP方法進(jìn)行匹配，把匹配的結(jié)果作為正確值，將得到的里程計(jì)誤差值假設(shè)是線性分布的。

③把匹配得到的誤差值平均到每一個(gè)激光點(diǎn)上，重新進(jìn)行激光點(diǎn)的修正。

④進(jìn)行ICP匹配方法的迭代，直到收斂為止。

ICP方法原理如下。

給定兩個(gè)點(diǎn)云集合X,P，X為參考幀，P為當(dāng)前幀的點(diǎn)云[5]。

(8)

式中：xi和pi為點(diǎn)云的坐標(biāo)；Nx和Np為點(diǎn)云的數(shù)量[6]。

(9)

求解三維旋轉(zhuǎn)矩陣R和平移向量t，使得誤差E(R,t)達(dá)最小。

分別計(jì)算兩組點(diǎn)云的質(zhì)心：

(10)

對(duì)兩組點(diǎn)云去質(zhì)心：

(11)

要使E(R,t)取最小值，t=μx-R×μp，則：

(12)

(13)

3 試驗(yàn)結(jié)果與分析

3.1 激光雷達(dá)性能參數(shù)

試驗(yàn)使用的激光雷達(dá)選用FLASH LIDIR F4型號(hào)，其性能參數(shù)如表1所示。

表1 FLASH LIDIR F4激光雷達(dá)性能參數(shù)

3.2 激光雷達(dá)運(yùn)動(dòng)畸變?nèi)コ囼?yàn)

為了驗(yàn)證激光雷達(dá)運(yùn)動(dòng)畸變?nèi)コ惴▽?duì)同步定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)系統(tǒng)在實(shí)際環(huán)境中的影響[8]，在機(jī)器人系統(tǒng)(robot operating system,ROS)平臺(tái)中通過(guò)其可視化界面(ROS visualization,RVIZ)觀察兩種算法所建的環(huán)境地圖。智能小車移動(dòng)的線速度設(shè)置為0.7 m/s。

未去除畸變的建圖效果如圖1所示。

圖1 未去除畸變的建圖效果

試驗(yàn)結(jié)果如圖2所示。

圖2 試驗(yàn)結(jié)果

從圖1和圖2可以明顯看出，此時(shí)激光雷達(dá)所掃描得到的點(diǎn)云與原電梯間的地圖情形已經(jīng)偏移了一定的角度，說(shuō)明此時(shí)激光雷達(dá)所掃描出來(lái)的數(shù)據(jù)產(chǎn)生了較大的畸變誤差。圖2(a)是使用輪式里程計(jì)輔助方法所構(gòu)建的圖，圖2(b)是使用輪式里程計(jì)和ICP方法融合的算法所構(gòu)建的圖。比較圖2(a)與圖2(b)可以看出，雖然兩種方法都可以有效去除激光雷達(dá)運(yùn)動(dòng)所產(chǎn)生的畸變，但是圖2(b)的地圖精度比圖2(a)的精度高。

綜上所述，當(dāng)智能小車在運(yùn)動(dòng)速率比較快的情況下，激光雷達(dá)所掃描出來(lái)的信息會(huì)發(fā)生畸變[9]。通過(guò)比較兩種算法的建圖效果，本文設(shè)計(jì)的激光雷達(dá)運(yùn)動(dòng)畸變?nèi)コ惴ㄌ岣吡薙LAM系統(tǒng)構(gòu)建環(huán)境地圖的精度。

4 結(jié)論

針對(duì)激光雷達(dá)加速運(yùn)動(dòng)時(shí)產(chǎn)生的畸變[10]，本文分析了激光雷達(dá)運(yùn)動(dòng)產(chǎn)生畸變的原因，并闡述了輪式里程計(jì)輔助方法去除激光雷達(dá)運(yùn)動(dòng)畸變的原理；分析了輪式里程計(jì)輔助方法的不足，針對(duì)位置線性假設(shè)沒有誤差線性假設(shè)合理的問題，最后提出了輪式里程計(jì)和ICP方法的融合的方法。在智能小車作室內(nèi)移動(dòng)時(shí)，分別使用兩種算法進(jìn)行構(gòu)建環(huán)境地圖。試驗(yàn)結(jié)果表明，輪式里程計(jì)和ICP方法的融合方法比采用里程計(jì)輔助方法所構(gòu)建的環(huán)境地圖精度高。