STM32的單目視覺室內(nèi)平面跟蹤定位

姚萬業(yè)，賈昭鑫

STM32的單目視覺室內(nèi)平面跟蹤定位

姚萬業(yè)，賈昭鑫

（華北電力大學自動化學院，保定071003）

針對室內(nèi)平面移動物體的定位，提出了一種基于STM32的單目視覺室內(nèi)平面跟蹤定位方式。在平面內(nèi)放置標定點，通過標定，求取出圖像坐標系與世界坐標系實際的對應關系，實時追蹤圖像坐標系，依照對應關系推算出實際位置。為了驗證該方法的有效性，采用比對平面移動物體與特定點誤差的方法，同時直觀地觀測對應點之間的距離關系。實驗中測得的移動位置與實際位置相對誤差小于1.5%，對于精度要求一般的室內(nèi)定位需求有著較高的實用性。

單目視覺；STM32；相機標定；平面定位

引 言

單目視覺定位是僅僅用一臺相機完成目標追蹤定位，它具有使用簡單和適用廣泛兩個突出特點，與雙目視覺不同的是，它無需解決立體視覺中兩相機間的最優(yōu)距離和特征點的匹配問題，也不會像全方位視覺傳感器那樣產(chǎn)生很大的畸變。在精度要求不高、場景簡單開闊的平臺，單目視覺追蹤定位具有廣闊的應用市場。尹英杰［1］基于單目視覺測量平面上物體位置的方法，實現(xiàn)了單目視覺測量模型的標定。王鵬［2］使用了4個共面的特征點，利用特征點與攝像機成像中心之間的距離因子建立位姿測量模型，并將位姿測量模型進行線性化處理，最后利用線性方程的基礎解和變量變換中變量之間的相互關系，實現(xiàn)位姿參數(shù)的求解。方帥［3］等提出了一種基于對應幾何關系的非標定的攝像機定位新算法，避開了繁瑣的攝像機標定這一過程，運用攝影幾何關系對非標定單攝像機拍攝的單幅圖像求解出從2D圖像平面到真實的3D平面的非線性單映射關系。顧慶華［4］將OpenCV的視覺庫移植到嵌入式系統(tǒng)中，依靠處理器芯片完成復雜的圖像運算和算法實現(xiàn)，進而實現(xiàn)了實時性的追蹤和定位。

本文提出了一種采用STM32單片機作為控制器，通過Matlab輔助計算相機校正的新方法。STM32通過液晶屏獲取特定顏色的位置進行追蹤，同時獲取坐標用來進行定位，Matlab通過單圖片的標定求出世界坐標系到圖像坐標系轉(zhuǎn)換的必要參數(shù)，將參數(shù)應用到STM32中，解決了單片機無法進行相機標定等復雜運算的問題。最終以一種相對低廉和簡單的方式實現(xiàn)了對平面內(nèi)物體的實時跟蹤和定位。

1　相機標定原理

1.1單目相機模型

單目相機模型中有三種坐標系，分別為：世界坐標系、像平面坐標系、相機坐標系，三種坐標系的關系如圖1所示。相機坐標系即以光軸中心O為原點的坐標系，其z軸滿足右手法則；成像原點Of所代表平面即為像平面坐標系，實際物體坐標系即為世界坐標系。

其中，P在世界坐標系的值為（Xw，Yw，Zw），選取追蹤定位物體就在該平面。被追蹤平面物體已確定，所以高度已知，Zw無需求取。Pc是P在圖像坐標系的投影點，其相機坐標系的值為（xc，yc，zc）。取相機的光軸中心點Oc為坐標系的原點，Zc軸的方向平行于相機光軸，并取相機到景物的方向為正方向，Xc軸方向取圖像坐標沿水平增加的方向，Yc軸方向取圖像坐標沿豎直增加的方向。

1.2平面內(nèi)相機標定

相機的內(nèi)參數(shù)模型為［5］：

式中：kx，ky分別為X軸和Y軸方向的放大系數(shù)；（u0，v0）為光軸中心點的圖像坐標，（xc，yc，zc）為物體在相機坐標系的坐標值。

相機的外參數(shù)模型是世界坐標系在相機坐標中的描述。（xw，yw，zw）為點在世界坐標系中的坐標，（xc，yc，zc）為對應在相機坐標系中的坐標。相機的外參數(shù)矩陣為：

式中：n=［nxnynz］T、o=［oxoyoz］T、a=［axayaz］T，分別是xw，yw，zw軸在相機坐標系中的方向矢量，p=［pxpypz］T是世界坐標系的坐標原點在相機坐標系中的位置矢量。

將式（2）代入式（1），因為已知被追蹤物體的平面坐標，所以高度值zw不予考慮，此處令zw=0，整理后得方程組為：

式中：

由式（4）可知，只要求出m'，便可以確定世界坐標系與圖像坐標系的轉(zhuǎn)換關系。對于世界坐標系中的n個坐標已知點，每個空間點都符合式（4），則可得：

式中：

其中（xwi，ywi，zwi）表示第i個點在世界坐標系中的坐標，（xwi，ywi）即為第i個點在平面內(nèi)的坐標。（ui，vi）為其圖像坐標，利用最小二乘法，可以求解獲得m'：

在得到參數(shù)m'之后，通過式（6）可得出：

已知圖像坐標（ui，vi），通過式（7）可以解方程求出對應的世界坐標系的坐標，得到式（8）。

2　Matlab標定與物體追蹤

Matlab自帶工具箱中提供了camera calibration，該工具箱可用于相機的標定，在實驗中未采用標定工具箱，僅應用了工具箱中的DetectCheckerboardPoints函數(shù)，該函數(shù)用來獲取圖像坐標系中的平面靶標角點的位置坐標。利用該坐標和平面內(nèi)已知點的坐標，通過相關的矩陣計算，最終求出世界坐標與圖像坐標的對應關系。

2.1提取角點

通過DetectCheckerboardPoints函數(shù)，可以很精確地提取角點，角點的精度可以達到0.1像素。提取角點過程中，可以通過設置初始參數(shù)對自動檢測到的角點進行調(diào)整，多次反復進行，可以提高精度，直到對提取的角點位置比較滿意。鏡頭沒有畸變，又因為是單張圖，所以識別過程準確快速。角點提取如圖2所示。

圖2　Matlab角點提取

2.2世界坐標系與圖像坐標系的對應關系

本次測試中，選取識別所有點中的54個識別點，建立一個2n×8的矩陣，編寫簡單函數(shù)，將所識別角點按照公式運算代入矩陣中，成為矩陣A。在世界坐標系中，設角點1所在的位置為世界坐標系即平面的坐標原點，坐標為（0，0），圖像右手方向為x軸正方向，下方為y軸正方向。由于每個小方格的實際尺寸均已知（9 mm），所以可以算出54個角點的實際坐標位置，編寫簡單函數(shù)，建立2n×1矩陣B，通過Matlab的矩陣運算，求出世界坐標與圖像坐標的對應關系m'的值。

2.3圖像獲取與顏色識別跟蹤

硬件設計中的單片機采用STM32F103，攝像頭采用CCD的200萬像素攝像頭OV7725，液晶顯示采用320×240的液晶顯示器。采用ILI9341控制器顏色的追蹤獲得移動物體的像素坐標，然后就可轉(zhuǎn)換為圖像坐標。相機顏色輸出為常見的RGB565格式，RGB色彩通過R、G、B三原色的相互加權產(chǎn)生各種不同顏色，RGB空間模型具有處理簡便、顯示美觀、使用直接等特點，但由于三個分量與亮度均有密切的關系，特別容易受到光照的影響，當光照強度發(fā)生變化的時候，同一顏色的RGB值在顏色空間中的位置會發(fā)生較大的變化，所以本實驗中將RGB轉(zhuǎn)換為由色調(diào)（Hue）、色彩飽和度（Saturation）、亮度（Value）這三個分量來定量描述顏色的HSV值。定義所追蹤物體的HSV值范圍，算法遍歷液晶每個像素，找到匹配的HSV值，以確定追蹤中心。相機實時追蹤如圖3所示。

3　實驗與結(jié)果

實驗中選取凳子平面為標準追蹤平面，標定靶紙尺寸為210 mm×297 mm，方格尺寸為9 mm，相機固定在一定高度，實時獲取圖像，截取一張圖像，應用Matlab進行相機的標定，實驗圖像如圖4所示。

圖3　相機實時追蹤

圖4　實驗測試

選取靶標左上角第一個黑色方塊的右上角為世界坐標系的原點（0，0），選取AC方向為xw的正方向，AB方向為yw的正方向，zw正方向垂直于凳子平面向上。

通過Matlab中Detect Checkerboard Points函數(shù)獲取每一個角點的圖像坐標，因為已經(jīng)定義了世界坐標系的原點，每一個方格的邊長為9 mm，所以確定了世界坐標系中每一個角點的坐標。通過式（6），計算出m'，將該值寫入STM32的程序中的式（8），通過串口打印出實時測得的圖像坐標與世界坐標值，如圖5所示。

圖5　實時測得物體坐標

通過處理和比較數(shù)據(jù)，計算該方法的相對誤差，測量ABCD四個已知角點的位置，圖像坐標與計算出的世界坐標位置如表1所列。

表1　特定位置的圖像坐標及計算出的世界坐標位置

為了更直觀地顯示誤差的大小，測量AB、BD、DC、CA四個邊長的距離，該邊長的實際距離通過已知的標定板可直接獲取，邊長的測量結(jié)果及其誤差如表2所列。

表2　邊長測量結(jié)果及其誤差

通過表2可以得知測得的相對誤差小于1.5%，為了實驗的準確性，在標定板上另外確定了3組除ABCD以外的12個點，進行比對精度測量，測得結(jié)果誤差均小于1.5%，說明了該實驗和測試方法的有效性和實用性。

結(jié) 語

常見的單目識別定位方式在定位特征的檢測與匹配上往往需要處理很多問題，圖像處理需要借助ARM等高端芯片進行處理。提出了基于STM32的依靠Matlab相機標定獲取標定參數(shù)的的特定顏色識別追蹤室內(nèi)平面定位方法，利用特定的顏色識別進行定位特征的檢測與匹配，根據(jù)簡單的對應關系獲取世界坐標。追蹤定位與標定分開進行，整體結(jié)構簡單、造價低廉、使用方便。通過實驗驗證了該方法的有效性，可用于視頻監(jiān)控中的目標追蹤和定位，同時還可以應用于平面物體長度、角度值的測量。

［1］尹英杰，徐德，張正濤，等.基于單目視覺的平面測量［J］.電子測量與儀器學報，2013（4）：347-352.

［2］王鵬，孫長庫，張子淼.單目視覺位姿測量的線性求解［J］.儀器儀表學報，2011（5）：1126-1131.

［3］方帥，曹洋，徐心和.一種非標定攝像機的定位新算法［J］.儀器儀表學報，2005（8）：845-848.

［4］顧慶華.基于嵌入式系統(tǒng)的雙目立體成像的設計與實現(xiàn)［D］.上海：上海交通大學，2013.

［5］徐德，譚民，李原.機器人視覺測量與控制［M］.北京：國防工業(yè)出版社，2011：35-39.

［6］弋英民，劉丁.基于單目視覺的輪式機器人同步定位與地圖構建［J］.儀器儀表學報，2010（1）：117-121.

［7］趙汝進，張啟衡，左顥睿，等.一種基于直線特征的單目視覺位姿測量方法［J］.光電子·激光，2010（6）：894-897.

［8］童張海.單目視覺空間目標位姿測量算法研究［D］.哈爾濱：哈爾濱工業(yè)大學，2012.

［9］尚倩.基于雙目立體視覺的目標識別與定位［D］.北京：北京交通大學，2011.

［10］高如新，王俊孟.基于雙目視覺的目標識別與定位［J］.河南理工大學學報：自然科學版，2014（4）：495-500.

［11］尋言言，薛河儒，姜新華.基于MATLAB的相機標定方法［J］.內(nèi)蒙古農(nóng)業(yè)大學學報：自然科學版，2014（2）：164-168.

［12］張益昕.基于計算機視覺的大尺度三維幾何尺寸測量方法及應用［D］.南京：南京大學，2011.

［13］舒娜.攝像機標定方法的研究［D］.南京：南京理工大學，2014.

［14］謝宇浪.線結(jié)構光視覺測量的標定方法與實驗評價［D］.武漢：湖北工業(yè)大學，2014.

［15］Zhao Jun，Liu Guo- ping.A Novel Localization Method for Indoor Mobile Robot Based on Odometry and Ceiling Visual Features［C］//中國自動化學會控制理論專業(yè)委員會第三十四屆中國控制會議論文集（D卷），2015.

姚萬業(yè)（教授），研究方向為新能源網(wǎng)絡控制系統(tǒng)、圖像識別、DCS分散控制系統(tǒng)。

Monocular Vision Plane Positioning Based on STM32

Yao Wanye，Jia Zhaoxin
（North China Electric Power University，Baoding 071003，China）

Aiming at the problem about the object positioning on the plane，a monocular vision plane positioning method is proposed based on STM32.The corresponding relationship between the image coordinate system and the world coordinate system is obtained through the marker point.By tracking the image coordinate system in real-time，the actual location is obtained according to the corresponding relationship.In order to verify the validity of the method，the distance between the corresponding points is directly observed through contrasting the error between the plane moving object and the specific point.The relative error of the target is less than 1.5%in the experimental result，which verifies the effectiveness of the proposed method.It has a very high practicability for the plane positioning with a low demand of accuracy.

monocular vision；STM32；camera calibration；plane positioning

TP242.6

A

??薛士然

2016-04-11）