訾安琪，柴曉冬，鄭樹彬，李立明，張　磊

(上海工程技術(shù)大學(xué) 城市軌道交通學(xué)院，上?！?01620)

?

基于SLAM的連續(xù)運(yùn)動(dòng)測(cè)量坐標(biāo)轉(zhuǎn)換算法研究

訾安琪，柴曉冬，鄭樹彬，李立明，張磊

(上海工程技術(shù)大學(xué) 城市軌道交通學(xué)院，上海201620)

近年來，視覺導(dǎo)航開始成為人們的研究熱點(diǎn);在軌道檢測(cè)中，現(xiàn)有運(yùn)動(dòng)狀態(tài)下的測(cè)量方法所采用的坐標(biāo)系都是建立在測(cè)量系統(tǒng)自身上的一種連續(xù)動(dòng)態(tài)變化的坐標(biāo)系，不能與靜態(tài)基準(zhǔn)點(diǎn)建立明確的解析聯(lián)系;針對(duì)這一問題，結(jié)合同步定位與地圖創(chuàng)建的思想，提出了一種新的檢測(cè)算法;基于SLAM原理，通過雙目視覺系統(tǒng)的坐標(biāo)轉(zhuǎn)換方法，建立動(dòng)態(tài)坐標(biāo)系與靜態(tài)基準(zhǔn)點(diǎn)之間的關(guān)系，求得旋轉(zhuǎn)平移矩陣，將后面N個(gè)位置的坐標(biāo)均轉(zhuǎn)換到初始坐標(biāo)系下，實(shí)現(xiàn)連續(xù)運(yùn)動(dòng)測(cè)量;通過實(shí)驗(yàn)，設(shè)計(jì)運(yùn)動(dòng)路線，驗(yàn)證坐標(biāo)轉(zhuǎn)換算法，并得到了建立在世界坐標(biāo)系下的空間線形曲線;實(shí)驗(yàn)結(jié)果表明：轉(zhuǎn)換后的坐標(biāo)，x、y、z三個(gè)方向的最大誤差不超過0.734 mm。

同步定位與地圖創(chuàng)建；坐標(biāo)轉(zhuǎn)換；連續(xù)測(cè)量；世界坐標(biāo)系

0　引言

軌道是機(jī)車車輛運(yùn)行的基礎(chǔ)，直接影響列車行駛安全以及舒適性。由于行車安全的要求，高速鐵路、城市軌道線路必須具有高精度的幾何線形參數(shù)，做到高平順性。目前，高速無砟軌道結(jié)構(gòu)中人們?nèi)越柚緝x、GPS精測(cè)網(wǎng)進(jìn)行非連續(xù)測(cè)量，這種在靜止?fàn)顟B(tài)下才能進(jìn)行測(cè)量的方法效率較低，無法較好地保證長(zhǎng)距離的軌道線路的高平順性要求。而現(xiàn)有的運(yùn)動(dòng)狀態(tài)下的測(cè)量方法所采用的坐標(biāo)系都是建立在測(cè)量系統(tǒng)自身上的一種連續(xù)動(dòng)態(tài)變化的坐標(biāo)系，這種動(dòng)態(tài)坐標(biāo)系還不能與靜態(tài)基準(zhǔn)點(diǎn)建立明確的解析聯(lián)系，也就無法解決現(xiàn)實(shí)生活中的一些問題[1-2]。

近年來，視覺的方法越來越多的應(yīng)用到軌道檢測(cè)、機(jī)器人等領(lǐng)域。視覺導(dǎo)航更是開始成為人們的研究熱點(diǎn)，它具有獲取信息完整、探索范圍比較大等優(yōu)點(diǎn)，在機(jī)器人導(dǎo)航領(lǐng)域有著重要的研究意義。視覺導(dǎo)航系統(tǒng)中，目前應(yīng)用最多的是在移動(dòng)機(jī)器人上面安裝一個(gè)或若干個(gè)視覺傳感器，通過控制移動(dòng)機(jī)器人，拍攝周圍環(huán)境的局部圖形，然后通過特征提取、圖像匹配、環(huán)境建模等[3]，創(chuàng)建地圖實(shí)現(xiàn)定位，即利用了SLAM的基本原理。本文將SLAM原理與連續(xù)運(yùn)動(dòng)測(cè)量結(jié)合起來，在SLAM原理下，通過雙目視覺系統(tǒng)的坐標(biāo)變換方法，將動(dòng)態(tài)測(cè)量值轉(zhuǎn)換到世界坐標(biāo)系下，實(shí)現(xiàn)非接觸連續(xù)運(yùn)動(dòng)的測(cè)量。

1　SLAM原理

在SLAM研究中，有3個(gè)關(guān)鍵性的問題，即：1)Where am I？2)Where am I going？3)How should I get there？要求機(jī)器人在一個(gè)完全未知的環(huán)境中從一個(gè)未知的位置出發(fā)，利用傳感器對(duì)環(huán)境的觀測(cè)遞增地建立環(huán)境的導(dǎo)航地圖，同時(shí)根據(jù)已建立的地圖同步確定自己的位置[4]。其基本原理圖如圖1所示。

圖1　SLAM原理圖

本文中將兩個(gè)CCD相機(jī)安裝在控制平臺(tái)的固定位置，通過設(shè)計(jì)好的路徑連續(xù)地運(yùn)動(dòng)獲取物體幾何信息，進(jìn)行坐標(biāo)轉(zhuǎn)換，獲得空間線形曲線，從而實(shí)現(xiàn)全局定位與地圖創(chuàng)建。

2　立體視覺基本原理

1)立體視覺介紹：

立體視覺是指從多幅拍攝的圖像中獲取目標(biāo)物體三維幾何信息的一種方法，它的目的在于重構(gòu)場(chǎng)景的三維幾何信息。立體視覺的研究主要包括圖像獲取、攝像機(jī)模型、特征提取、圖像匹配和深度計(jì)算等部分。

雙目立體視覺的方法，即從所拍攝的兩幅圖中獲取物體三維幾何結(jié)構(gòu)信息，從而恢復(fù)出空間線形。雙目視覺在生活中并不少見，其中最完美的雙目視覺結(jié)構(gòu)就是人的雙眼。當(dāng)用雙眼觀察物體時(shí)，會(huì)有遠(yuǎn)近的感覺，而使用兩個(gè)相機(jī)實(shí)現(xiàn)立體視覺系統(tǒng)正是參照此原理，通過建立所拍攝的兩幅圖像之間的位置關(guān)系，根據(jù)對(duì)應(yīng)點(diǎn)的視差恢復(fù)三維信息，計(jì)算深度[5]。

2)空間點(diǎn)重建：

對(duì)于一個(gè)三維多面體，如果知道了它的各個(gè)頂點(diǎn)的三維坐標(biāo)與相鄰關(guān)系，那么這個(gè)多面體的形狀和位置也就是唯一確定的。利用立體視覺的方法獲取物體的三維點(diǎn)坐標(biāo)，也是立體視覺中最基本的。假設(shè)，在圖2中空間中任意點(diǎn)X在兩個(gè)相機(jī)上的投影點(diǎn)分別為XL和XR，兩相機(jī)的投影矩陣分別為M1與M2，于是有

其中，(u1,v1,1)與(u2,v2,1)分別為點(diǎn)X在兩個(gè)相機(jī)中對(duì)應(yīng)點(diǎn)XL和XR的齊次坐標(biāo)；(X,Y,Z,1)為點(diǎn)X在世界坐標(biāo)系下的齊次坐標(biāo)。聯(lián)立上式，消去可得：

由解析幾何的知識(shí)可知，直線OLXL與直線ORXR一定相交，即上述方程組一定有解，而且解是唯一的[6-7]。由此就可以確定空間中點(diǎn)的三維信息，即實(shí)現(xiàn)自定位。

3　坐標(biāo)轉(zhuǎn)換算法原理

連續(xù)運(yùn)動(dòng)測(cè)量基本原理框圖如圖2所示。

圖2　測(cè)量原理框圖

以左攝像機(jī)為例，初始化位置記為P1，將標(biāo)定板固定，并在標(biāo)定板上建立絕對(duì)世界坐標(biāo)系，則標(biāo)定板和攝像機(jī)之間的旋轉(zhuǎn)矩陣和平移向量[R0;t0]可以求得。

1)前后位置關(guān)系：

記前后位置公共特征點(diǎn)在位置P1的坐標(biāo)為(x1i,y1i,z1i)，在位置P2下的坐標(biāo)為(x2i,y2i,z2i)，則P1和P2之間的位置關(guān)系可以表示為：

記

A=

構(gòu)造偽逆矩陣(ATA)-1ATA·AX=b，則最小二乘解為X=(ATA)-1ATb。

即可求得，位置P1和位置P2的之間的[R1;t1]。

以此類推，可以求得連續(xù)運(yùn)動(dòng)狀態(tài)下所有位置之間的R、t。

2)遞推關(guān)系：

通過連續(xù)測(cè)量，可以獲得每個(gè)位置上數(shù)據(jù)點(diǎn)的坐標(biāo)，而又求得了前后位置之間的關(guān)系。這樣就可以將所有位置上的坐標(biāo)轉(zhuǎn)換到初始位置P1的世界坐標(biāo)系下，從而反映出整個(gè)變化的情況。最后一個(gè)位置上面的點(diǎn)在世界坐標(biāo)系下的坐標(biāo)可以通過前面位置的旋轉(zhuǎn)和平移遞推公式求得。即可以通過位置1在相機(jī)坐標(biāo)系下的坐標(biāo)和相機(jī)與世界坐標(biāo)系之間的旋轉(zhuǎn)、平移矩陣獲得位置1上的點(diǎn)在世界坐標(biāo)系下的坐標(biāo)。即：

(1)

其中:(x1,y1,z1,1)為P1位置的相機(jī)所拍攝的坐標(biāo)，(X0,Y0,Z0,1)為其在絕對(duì)世界坐標(biāo)上點(diǎn)的坐標(biāo)。

同理，P1和P2之間的關(guān)系為:

(2)

由此遞推可得最后一個(gè)位置上面的點(diǎn)在世界坐標(biāo)系下的坐標(biāo)：

(3)

4.4算法驗(yàn)證與分析

實(shí)驗(yàn)采用如圖3所示的控制平臺(tái)進(jìn)行驗(yàn)證。

圖3　實(shí)驗(yàn)平臺(tái)示意圖

如上圖所示，整個(gè)實(shí)驗(yàn)平臺(tái)由三部分組成：NI工控機(jī)、三維移動(dòng)平臺(tái)、檢測(cè)平臺(tái)。實(shí)驗(yàn)平臺(tái)中X向位于最底端，可以左右移動(dòng)；Y向支座設(shè)置在X向底座上，可以前后移動(dòng)；而Z向支架位于Y向支架的上表面，可以上下移動(dòng)。3個(gè)方向移動(dòng)的最小單位均為0.05mm，重復(fù)精度為0.02～0.04mm。在檢測(cè)平臺(tái)上固定著兩個(gè)相同型號(hào)的CCD相機(jī)；相機(jī)正前方放置與之垂直的定標(biāo)板[8]。

具體實(shí)驗(yàn)步驟如下：

1)為了得到高精度的旋轉(zhuǎn)矩陣，在左右相機(jī)都能獲取完整標(biāo)定板信息的前提下，盡量多的改變標(biāo)定板的姿態(tài)，同步采集圖像20次進(jìn)行定標(biāo)，獲得相機(jī)的M矩陣；

2)在初始位置建立世界坐標(biāo)系，確定相機(jī)坐標(biāo)系與世界坐標(biāo)系之間的位置關(guān)系；

3)按照設(shè)計(jì)的路線移動(dòng)控制平臺(tái)，拍攝圖像，提取當(dāng)前

圖4　實(shí)驗(yàn)平臺(tái)　　　 圖5　CCD相機(jī)標(biāo)定界面

位置與上一位置的公共特征點(diǎn)，確定前后位置的旋轉(zhuǎn)矩陣和平移矩陣，對(duì)特征點(diǎn)進(jìn)行匹配；

4)繼續(xù)移動(dòng)，重復(fù)步驟3)，獲取所有點(diǎn)的三維坐標(biāo)；

5)將后面所有位置特征點(diǎn)的坐標(biāo)轉(zhuǎn)換到初始位置的攝像機(jī)坐標(biāo)系下，然后再轉(zhuǎn)到世界坐標(biāo)系下，從而獲得空間線形曲線。

圖6　前后位置特征點(diǎn)提取對(duì)比圖

表1所示為移動(dòng)到位置1、位置2和位置3時(shí)所取點(diǎn)轉(zhuǎn)換到初始位置的坐標(biāo)，由轉(zhuǎn)換后點(diǎn)的坐標(biāo)可知，三軸的最大誤差不超過0.734 mm。

坐標(biāo)本實(shí)驗(yàn)中得到的空間線形曲線如圖7所示，x軸向絕對(duì)差為7.104 mm，y軸向絕對(duì)差為105.549 mm，z軸向絕對(duì)差

圖7　獲得的空間線形曲線

點(diǎn)號(hào)12345678坐標(biāo)99.50233-41.62987822.21321é?êêù?úú89.67947-41.49898819.79879é?êêù?úú79.76407-41.29574819.58482é?êêù?úú69.83310-41.26779817.15907é?êêù?úú69.92934-41.31517817.49254é?êêù?úú60.08264-41.29006816.10782é?êêù?úú59.79792-41.36494816.37154é?êêù?úú49.86938-41.29062816.20929é?êêù?úú39.92954-41.22923815.25984é?êêù?úú30.08574-41.04571812.30183é?êêù?úú點(diǎn)號(hào)789101112坐標(biāo)40.40898-41.05182814.52661é?êêù?úú30.05706-41.12708812.14222é?êêù?úú20.12307-41.16644812.3183410.35778-40.83537807.82512é?êêù?úú10.01477-40.69145807.26611é?êêù?úú0.59748-40.97581809.36656é?êêù?úú0.61204-40.90625808.92763é?êêù?úú-9.90029-41.09266809.71264é?êêù?úú

為5.217 mm。而實(shí)際設(shè)計(jì)的路線：x軸的絕對(duì)位移為6 mm；誤差為1.104 mm；y軸的絕對(duì)位移為105 mm，誤差為0.549 mm；z軸的絕對(duì)位移為5 mm，誤差為0.217 mm。

5　結(jié)語

本文基于SLAM原理，對(duì)連續(xù)的運(yùn)動(dòng)軌跡進(jìn)行測(cè)量，并通過雙目視覺系統(tǒng)的坐標(biāo)變換方法，將動(dòng)態(tài)測(cè)量值轉(zhuǎn)換到世界坐標(biāo)系下，從而將動(dòng)態(tài)坐標(biāo)系與靜態(tài)基準(zhǔn)點(diǎn)建立明確的解析聯(lián)系。實(shí)驗(yàn)證明，本文的方法可以進(jìn)行連續(xù)長(zhǎng)距離的非接觸快速檢測(cè)，具有轉(zhuǎn)換精度高、重復(fù)性好等特點(diǎn)，可實(shí)現(xiàn)連續(xù)運(yùn)動(dòng)的測(cè)量，并能夠有效地檢測(cè)出空間線形曲線。

[1]趙國(guó)堂. 高速鐵路軌道幾何狀態(tài)測(cè)量與軌道精調(diào)[R]. 北京:京滬高速鐵路股份有限公司, 2010.

[2]Shen S L, Wub H N, Cui Y J, et al. Long-term settlement behaviour of metro tunnels in the soft deposits of Shanghai[J]. Tunnelling and Underground Space Technology, 2014, 40:309-323.

[3] 韓銳. 未知環(huán)境下基于SLAM的移動(dòng)機(jī)器人導(dǎo)航算法研究[D]. 武漢：武漢理工大學(xué),2006.

[4] 鄭宏. 移動(dòng)機(jī)器人導(dǎo)航和SLAM系統(tǒng)研究[D]. 上海：上海交通大學(xué),2007.

[5] 吳福朝. 計(jì)算機(jī)視覺中的數(shù)學(xué)方法[M].北京：科學(xué)出版社,2008.

[6] Zhang Z Y, Membe S. A flexible new technique for camera calibration[J]. IEEE Transaction on Pattern Analysis and Machine Intelligence, 2000,22(11):1330-1334.

[7] 馬頌德, 張正友. 計(jì)算機(jī)視覺[M]. 北京:科學(xué)出版社, 1997.

[8] 羅偉. 多傳感信息融合的軌道長(zhǎng)波不平順檢測(cè)實(shí)驗(yàn)平臺(tái)研究[D]. 上海工程技術(shù)大學(xué), 2015.

Continuous Movement Measuring Coordinate Transformation Algorithm Based on SLAM

Zi Anqi，Chai Xiaodong，Zheng Shubin，Li Liming，Zhang Lei

(College of Urban Rail Transportation, Shanghai University of Engineering Science,Shanghai201620, China)

In recent years, the visual navigation began to become the focus. In rail detection, the existing sports under the state of measuring methods adopted by the coordinate system are based on the measurement system itself a continuous dynamic changes of the coordinate system, cannot establish clear resolution contact with static reference point. In order to solve this problem, a new detection algorithm is proposed combining with simultaneous localization and mapping. In this paper, a relationship between a dynamic coordinate system and the static reference point was built to realize continuous motion measurement, which was based on the principle of simultaneous localization and mapping (SLAM) and the coordinate transformation method of binocular vision system. We designed a movement course in the experiment to verify the coordinate transformation algorithm, and got a space linear curve based on the world coordinate system. Experimental result shows that after the transformed coordinates, x, y, z three directions of the maximum error is less than 0.734 mm.

simultaneous localization and mapping；coordinate transformation；continuous motion measurement；world coordinate system

2015-08-05；

2015-09-16。

國(guó)家自然科學(xué)基金資助(51478258)；上海市科委重點(diǎn)支撐項(xiàng)目資助(13510501300)；上海市研究生教育創(chuàng)新計(jì)劃學(xué)位點(diǎn)引導(dǎo)布局與建設(shè)培育項(xiàng)目(13sc002)。

訾安琪(1990-)，女，山東鄒城人，碩士研究生，主要從事智能檢測(cè)與機(jī)器視覺方向的研究。

柴曉冬(1962-)，男，安徽人，教授，碩士研究生導(dǎo)師，主要從事智能信息處理、光信息處理方向的研究。

1671-4598(2016)01-0289-03

10.16526/j.cnki.11-4762/tp.2016.01.080

TB114.3

A