趙梓喬 肖 力

(華中科技大學(xué)自動(dòng)化學(xué)院 武漢 430074)

?

基于三維激光掃描儀的室內(nèi)移動(dòng)設(shè)備定位與建圖*

趙梓喬 肖 力

(華中科技大學(xué)自動(dòng)化學(xué)院 武漢 430074)

論文采用慣導(dǎo)與里程計(jì)組合輔助點(diǎn)云配準(zhǔn)的方式實(shí)現(xiàn)了室內(nèi)環(huán)境下移動(dòng)設(shè)備的定位與地圖建立，克服了自主研制的三維掃描儀數(shù)據(jù)更新頻率低的缺點(diǎn)。首先，采用慣導(dǎo)與里程計(jì)組合完成移動(dòng)設(shè)備的姿態(tài)測量與航位推算；然后，將航位推算的結(jié)果作為點(diǎn)云配準(zhǔn)的初始值，之后采用基于正態(tài)分布變換的點(diǎn)云配準(zhǔn)算法完成點(diǎn)云的順序配準(zhǔn)；最后，根據(jù)點(diǎn)云配準(zhǔn)結(jié)果修正航位推算的定位結(jié)果，并在此基礎(chǔ)上完成定位與建圖。論文以輪式小車為實(shí)驗(yàn)平臺，搭載自制的三維激光掃描儀，在樓道環(huán)境中驗(yàn)證了文中所述方法的有效性。

點(diǎn)云; 慣導(dǎo); 正態(tài)分布變換; 定位; 建圖

Class Number TN249

1 引言

在未知環(huán)境中定位與環(huán)境地圖的建立是設(shè)備自主導(dǎo)航的基礎(chǔ)，特別是在無GPS信號的室內(nèi)環(huán)境中。定位與地圖信息是設(shè)備運(yùn)動(dòng)與行為決策的基本前提。在室內(nèi)環(huán)境中一般采用基于WLAN[1]、Zigbee[2]以及RSSI[3]等無線電技術(shù)的定位方式，此類方式需要在定位環(huán)境中布置大量無線節(jié)點(diǎn)，受障礙物的影響較大，定位精度無法滿足設(shè)備自主導(dǎo)航的要求。

目前室內(nèi)未知環(huán)境中主要采用機(jī)器視覺和二維或三維點(diǎn)云數(shù)據(jù)獲取移動(dòng)設(shè)備的運(yùn)動(dòng)信息，同時(shí)建立地圖信息。其中基于視覺的同步定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)系統(tǒng)發(fā)展較快，Stephen Se等利用機(jī)器視覺采用尺度不變的地標(biāo)完成了移動(dòng)機(jī)器人的全局定位與建圖，但實(shí)際應(yīng)用中機(jī)器視覺受環(huán)境光線影響較大，系統(tǒng)魯棒性低[5]。Michael Bosse、 Robert Zlot等將型號為Hokuyo UTM-30LX的二維激光掃描儀安裝在彈簧上使其旋轉(zhuǎn)來獲取三維點(diǎn)云信息，并由此完成三維空間的定位與建圖，這種由二維掃描儀獲取三維點(diǎn)云的方式在復(fù)雜環(huán)境中存在積累誤差，不適合大范圍場景的應(yīng)用[6]。Diego Viejo使用型號為LMS-200 Sick的三維激光掃描儀實(shí)現(xiàn)了空間中六維位姿的估計(jì)，但該傳感器成本較高，推廣應(yīng)用較為困難[7]。

三維激光掃描儀受環(huán)境光照影響小，測量精度高，可以直接獲取空間的三維信息。本文中使用的三維激光掃描儀為自主研制的低成本三維掃描儀，但目前掃描儀數(shù)據(jù)更新頻率較低。為克服其數(shù)據(jù)更新頻率低的缺點(diǎn)，本文采用間隔掃描的方式采集點(diǎn)云數(shù)據(jù)，期間通過慣導(dǎo)與里程計(jì)組合進(jìn)行航位推算得到設(shè)備的位姿變化量，并將其作為點(diǎn)云配準(zhǔn)的初始值，之后采用基于正態(tài)分布變換的點(diǎn)云配準(zhǔn)算法[4]完成點(diǎn)云的順序配準(zhǔn)。最后根據(jù)配準(zhǔn)結(jié)果修正航位推算得到的位姿，同時(shí)建立三維點(diǎn)云地圖。實(shí)驗(yàn)中對比了無慣導(dǎo)與里程計(jì)輔助的配準(zhǔn)方式，證明了此方法在實(shí)際應(yīng)用中的可行性與有效性。

2 慣導(dǎo)與里程計(jì)組合定位

2.1 基于kalman濾波器的姿態(tài)測量模型

(1)

(2)

(3)

(4)

采用陀螺儀的測量值進(jìn)行卡爾曼濾波器的狀態(tài)更新，加速度計(jì)與磁力計(jì)的測量值作為觀測值，根據(jù)四元數(shù)微分方程與陀螺儀誤差測量模型建立卡爾曼濾波器的7維狀態(tài)方程[9]如下：

(5)

加速度計(jì)與磁力計(jì)的測量值作為觀測量建立卡爾曼濾波器的觀測方程：

z=H(x,t)+v(t)

(6)

其中觀測矩陣

將系統(tǒng)狀態(tài)方程(5)離散化，同時(shí)將卡爾曼濾波器的觀測方程(6)線性化。根據(jù)卡爾曼濾波器的離散狀態(tài)方程與線性化之后的觀測方程，進(jìn)行卡爾曼濾波器的迭代計(jì)算便可得到濾波之后的姿態(tài)信息。

2.2 姿態(tài)與里程計(jì)組合航位推算

采用光電編碼器作為里程計(jì)傳感器，相對于傳統(tǒng)的里程計(jì)，光電編碼器有更高的測量精度。車頭方向?yàn)檩d體坐標(biāo)系的X軸正方向，光電編碼器的采樣周期為Δt，則在Δt時(shí)間內(nèi)車輛的位移在載體坐標(biāo)系中可以表示為

(7)

其中ΔS=KDnD為載體在X軸方向上位移，nD為光電編碼里程計(jì)的脈沖個(gè)數(shù)，KD為里程計(jì)的刻度系數(shù)。

結(jié)合上一小節(jié)得到的姿態(tài)，載體在導(dǎo)航坐標(biāo)系中的位移可以表示為

(8)

其中ψ與θ分別表示偏航角與俯仰角。根據(jù)式(8)累加各個(gè)采樣周期內(nèi)載體的位移，進(jìn)行航位推算即可得到設(shè)備的實(shí)時(shí)位置。

3 NDT點(diǎn)云配準(zhǔn)與建圖

(9)

NDT配準(zhǔn)的具體步驟如下：

1) 根據(jù)參考點(diǎn)云P各點(diǎn)的位置，將各點(diǎn)存儲到m個(gè)柵格中，柵格表示為B={b1,b2,…,bm}；

2) 計(jì)算各個(gè)柵格中點(diǎn)云的分布函數(shù)；

3) 計(jì)算NDT配準(zhǔn)的目標(biāo)函數(shù)的函數(shù)值，采用牛頓法迭代計(jì)算最優(yōu)的六維位姿向量；

4) 牛頓迭代法收斂或迭代次數(shù)帶到最大迭代次數(shù)時(shí)配準(zhǔn)完成。

室內(nèi)環(huán)境中如樓梯、過道等場景之間相似度高，當(dāng)兩幅點(diǎn)云相差距離較大時(shí)配準(zhǔn)失敗的可能性增加，同時(shí)配準(zhǔn)迭代的次數(shù)增加，耗費(fèi)更多時(shí)間。慣導(dǎo)與里程計(jì)組合的航位推算在短距離范圍內(nèi)的積累誤差較小，將航位推算得到的位姿變換作為NDT配準(zhǔn)的初始值可以減少配準(zhǔn)的迭代次數(shù)，增加配準(zhǔn)成功的可能性,同時(shí)利用點(diǎn)云配準(zhǔn)結(jié)果可以減小航位推算的積累誤差。本文中使用的三維激光掃描儀為實(shí)驗(yàn)室自主研制，360°全景掃描時(shí)的點(diǎn)云數(shù)據(jù)更新頻率在20s左右，慣導(dǎo)與里程計(jì)組合航位推算的更新頻率為20Hz。整個(gè)定位與建圖過程采用停—走—停模式，總的步驟如下：

圖1 NDT配準(zhǔn)的流程圖

1) 在起始位置全景掃描得到一幅點(diǎn)云數(shù)據(jù)作為初始參考點(diǎn)云加入全局點(diǎn)云地圖；

2) 開始移動(dòng)，期間慣導(dǎo)與里程計(jì)組合進(jìn)行航位推算實(shí)時(shí)更新移動(dòng)設(shè)備的位置與姿態(tài)；

3) 停止移動(dòng)，全景掃描得到此處點(diǎn)云數(shù)據(jù)，以航位推算得到的位姿變換作為初始值將點(diǎn)云數(shù)據(jù)與上一幅點(diǎn)云數(shù)據(jù)進(jìn)行NDT配準(zhǔn)，將配準(zhǔn)后的點(diǎn)云加入全局點(diǎn)云地圖；

4) 根據(jù)配準(zhǔn)結(jié)果修正航位推算的位姿，返回步驟2)。

4 實(shí)驗(yàn)結(jié)果與分析

本文中的移動(dòng)設(shè)備為輪式小車，作為里程計(jì)的編碼器與車輪剛性連接，用于獲取三維點(diǎn)云的三維激光掃描儀為實(shí)驗(yàn)室自制，如圖2所示。此外，姿態(tài)測量的慣性傳感器采用ADIS16405，其內(nèi)部集成陀螺儀、加速度計(jì)以及磁力計(jì)，通過SPI接口輸出傳感器的原始測量數(shù)據(jù)。實(shí)驗(yàn)在實(shí)驗(yàn)室外的樓道中進(jìn)行，整個(gè)樓道呈長條狀，總長約75m，每間隔3m～4m進(jìn)行一次全景掃描。

圖2 三維激光掃描儀實(shí)物

樓道環(huán)境單一，直接采用NDT配準(zhǔn)后兩點(diǎn)云之間仍存在較大偏差，如圖3所示，圖中的網(wǎng)格邊長為1m。兩幅點(diǎn)云中點(diǎn)個(gè)數(shù)都在16萬左右，配準(zhǔn)過程總共迭代27次，耗時(shí)6.597s。當(dāng)采用航位推算輔助配準(zhǔn)后，配準(zhǔn)過程總共迭代16次，耗時(shí)3.968s，迭代次數(shù)減少了9次，時(shí)間減少了2.6s，如圖4所示，局部放大部分兩點(diǎn)云之間相差約2cm。圖5中黑色線條為移動(dòng)設(shè)備在樓道點(diǎn)云地圖中的移動(dòng)軌跡。點(diǎn)云配準(zhǔn)完成后可以修正航位推算的積累誤差，如圖5中箭頭所指的地方。圖6為整個(gè)樓道的點(diǎn)云地圖的俯視圖，圖中網(wǎng)格的邊長為2m。

圖3 無航位推算的配準(zhǔn)結(jié)果

圖4 航位推算輔助的配準(zhǔn)結(jié)果

上述算法均在Qt 5.4.2環(huán)境下采用C++編程實(shí)現(xiàn)，點(diǎn)云與行進(jìn)軌跡的顯示采用OpenGL直接繪制。執(zhí)行程序的PC平臺為：Intel(R) Core(TM) i5-4430 CPU @ 3.00GHz 四核，RAM 4.00GB DDRIII。

圖5 設(shè)備在點(diǎn)云地圖中的移動(dòng)軌跡

對比上述結(jié)果，可以發(fā)現(xiàn)慣導(dǎo)與里程計(jì)組合的航位推算不僅可以加快點(diǎn)云配準(zhǔn)的速度，而且提高了點(diǎn)云配準(zhǔn)的成功率。短距離高頻率的航位推算可以很好地彌補(bǔ)實(shí)驗(yàn)室自主研制的三維掃描儀數(shù)據(jù)更新頻率低的缺點(diǎn)，同時(shí)間斷性的點(diǎn)云配準(zhǔn)數(shù)據(jù)可以修正航位推算的積累誤差，兩者之間相輔相成。

圖6 樓道點(diǎn)云地圖

5 結(jié)語

本文介紹了一種基于三維激光掃描儀的定位與點(diǎn)云地圖構(gòu)建系統(tǒng)。首先，建立基于kalman濾波器的姿態(tài)測量模型，由慣導(dǎo)的測量計(jì)算得到的設(shè)備的實(shí)時(shí)姿態(tài)；然后，與編碼器測量得到的里程數(shù)據(jù)組合進(jìn)行航位推算得到移動(dòng)設(shè)備的實(shí)時(shí)位姿；之后采用基于正態(tài)分布變換的點(diǎn)云配準(zhǔn)算法配準(zhǔn)間隔掃描得到的點(diǎn)云數(shù)據(jù)，配準(zhǔn)的初始值由航位推算得到；最后，根據(jù)配準(zhǔn)結(jié)果修正航位推算的定位結(jié)果，并完成點(diǎn)云地圖的構(gòu)建。實(shí)驗(yàn)證明本系統(tǒng)有效地克服了三維掃描儀數(shù)據(jù)更新速度慢的缺點(diǎn)，實(shí)現(xiàn)了高頻航位推算與低頻點(diǎn)云數(shù)據(jù)的結(jié)合。

[1] 郎昕培,許可, 趙明.基于無線局域網(wǎng)的位置定位技術(shù)研究和發(fā)展[J].計(jì)算機(jī)科學(xué),2006(6):21-24. LANG Xinpei, XU Ke, ZHAO Ming. Research &Development of WLAN-based Location Techniques[J]. Computer Science,2006(6):21-24.

[2] Yuh-Ming Cheng.Using ZigBee and Room-Based Location Technology to Constructing an Indoor Location-Based Service Platform[C]// Intelligent Information Hiding and Multimedia Signal Processing, 2009. IIH-MSP’09. Fifth International Conference on.12-14 Sept,2009:803-806.

[3] Junhui Zhao,Yuqiang Zhang,MengjieYe.Research on the Received Signal Strength Indication Location Algorithm for RFID System[C]// Communications and Information Technologies, 2006. ISCIT '06. International Symposium on.Oct.,2006,18:881-885.

[4] Peter Biber and Wolfgang Stra?er. The normal distributions transform: A new approach to laser scan matching[C]// In Proceedings of the IEEE International Conference on Intelligent Robots and Systems (IROS). 27-31 Oct. 2003:2743-2748.

[5] Stephen Se, David G. Lowe, James J. Little. Vision-based global localization and mapping for mobile robots[J].IEEE Transactions on Robotics，2005,21(3)：364-375.

[6] Michael Bosse, Robert Zlot, and Paul Flick. Design of a Spring Mounted 3D Range Sensor with Application to Mobile Mapping[J].IEEE Transactions on Robotics,2012,28(5):1104-1119.

[7] Diego Viejo Miguel Cazorla.A robust and fast method for 6DoF motion estimation from generalized 3D data[J]. Autonomous Robot,2014,36(4):295-308.

[8] J. F. Guerrero-Castellanos,H. Madrigal-Sastre,S. Durand; N. Marchand.Design and implementation of an Attitude and Heading Reference System (AHRS)[C]// 2011 8th International Conference on Electrical Engineering Computing Science and Automatic Control (CCE).26-28 Oct. 2011:1-5.

[9] R. R. Lima, L. A. B. Trres.Performance Evaluation of Attitude Estimation Algorithms in the Design of an AHRS for Fixed Wing UAVs[C]// Robotics Symposium and Latin American Robotics Symposium (SBR-LARS),2012,16-19:255-260.

[10] Besl P J, McKay N D. Method for registration of 3-D shapes[J]. Robotics-DL tentative. International Society for Optics and Photonics,1992:586-606.

[11] Martin Magnusson, Achim Lilienthal,Tom Duckett.Scan registration for autonomous mining vehicles using 3D-NDT[J].Journal of Field Robotics,24(10):803-827.

Localization and Mapping Indoor for Mobile Devices Based on Three-dimensional Scanner

ZHAO Ziqiao XIAO Li

(School of Automation,Huazhong University of Science & Technology, Wuhan 430074)

In this paper, a solution is proposed to locate and map for mobile devices indoor by registration point cloud assisted with INS and odometer, it overcomes the shortcomings of the three-dimensional scanner's low data updating rate. First, the INS and odometer measure are used to accomplish mobile device's attitude and dead reckoning, then normal distribution transform (NDT) algorithm is applied to point cloud registration, as the initial value is the result of dead reckoning. Finally, the point cloud registration correction dead reckoning, and accomplish the localization and mapping. The effectiveness of the method described in the paper is demonstnated in a corridor, which using the wheeled trolley equipped with a three-dimensional laser scanner.

point cloud, INS, normal distribution transform, localization, mapping

2016年5月6日,

2016年6月25日

趙梓喬，男，碩士研究生，研究方向：點(diǎn)云處理，同時(shí)定位與建圖。肖力，男，碩士生導(dǎo)師，研究方向：三維掃描技術(shù)與導(dǎo)航系統(tǒng)。

TN249

10.3969/j.issn.1672-9722.2016.11.032