基于多目紅外視覺的室內(nèi)無人車導(dǎo)航系統(tǒng)研究

周穎文 王 勇，2

（1.江蘇師范大學(xué)電氣工程及自動化學(xué)院，江蘇 徐州 221116；2.徐州市質(zhì)量技術(shù)監(jiān)督綜合檢驗檢測中心，江蘇 徐州 221000）

0 引言

隨著工業(yè)4.0概念和《中國制造2025》戰(zhàn)略文件的提出，智能制造工程領(lǐng)域被列為重點發(fā)展對象。目前，在國內(nèi)智能制造工程領(lǐng)域中，機器人產(chǎn)業(yè)規(guī)模正加速增長[1]，已成為世界最大的機器人市場。其中，室內(nèi)無人車的需求量逐年上升，無人車導(dǎo)航的精度問題成為目前重點研究的對象。高精度導(dǎo)航無人車能夠精確地提供時間和空間信息，從而提高生產(chǎn)生活效率，無人車市場還將不斷擴大，具有廣闊的發(fā)展前景。然而，由于我國機器人行業(yè)起步較晚，高精度導(dǎo)航定位系統(tǒng)發(fā)展較慢，導(dǎo)航定位系統(tǒng)的精度可靠性較低，國外高精度導(dǎo)航定位系統(tǒng)成本較高，因此制約了我國高精度機器人導(dǎo)航系統(tǒng)的發(fā)展。

基于上述研究背景，綜合考慮成本、精度等原因，該文對一種低成本、高精度且實時性較好的多目紅外系統(tǒng)進(jìn)行研究，并使用該系統(tǒng)實現(xiàn)室內(nèi)無人車的高精度導(dǎo)航定位。在該文中，使用一維標(biāo)定桿對多目紅外系統(tǒng)進(jìn)行標(biāo)定，計算多目紅外系統(tǒng)的相關(guān)參數(shù)，并設(shè)計無人車控制系統(tǒng)，完成軌跡規(guī)劃。研究表明，該系統(tǒng)能夠準(zhǔn)確地計算出空間點的位置信息，實現(xiàn)室內(nèi)無人車定位導(dǎo)航。

1 多目紅外系統(tǒng)

該文使用Optitrack公司的Prime13紅外相機進(jìn)行實驗。使用三腳架將多個紅外相機置于高處，為了使多目相機系統(tǒng)的觀測范圍達(dá)到最大，且保證實驗場地中心能被多臺相機同時觀測到，將多臺相機環(huán)形擺放。

將大于3臺相機的系統(tǒng)稱為多目系統(tǒng)，該文實驗中使用相機個數(shù)為8臺。使用多目紅外系統(tǒng)需要首先對其標(biāo)定進(jìn)行研究。標(biāo)定是獲取空間位置信息的重要過程，通過標(biāo)定，可以獲取相機的位姿和內(nèi)部參數(shù)，再結(jié)合相關(guān)視覺理論重建空間三維信息，從而實現(xiàn)空間定位。目前，根據(jù)標(biāo)定物的不同，標(biāo)定方法可以分為3種：三維標(biāo)定塊、二維標(biāo)定平面以及一維標(biāo)定桿[2]。該文使用一維標(biāo)定桿標(biāo)定法對多目相機進(jìn)行標(biāo)定，這種一維標(biāo)定桿加工制造簡單，生產(chǎn)成本較低，相機可以在視場內(nèi)獲得標(biāo)定桿的任意位置數(shù)據(jù)來進(jìn)行參數(shù)計算，數(shù)據(jù)獲取速度較快、效率高，同時，使用這種一維標(biāo)定桿進(jìn)行標(biāo)定，可以大大減少標(biāo)記點在識別和提取過程中產(chǎn)生的誤差。因此，一維標(biāo)定桿標(biāo)定法廣泛應(yīng)用于在較大空間內(nèi)對多目相機的標(biāo)定。該文所使用的一維標(biāo)定桿如圖1所示，由3個特殊材質(zhì)制成的高反光小球A、B和C組成，用來反射紅外相機發(fā)射的紅外光線。每2個小球之間距離不同，但處于同一直線上，AB距離為175 mm，BC距離為325 mm。

圖1 一維標(biāo)定桿示意圖

一維標(biāo)定桿還具有可手持的優(yōu)勢，能夠在短時間內(nèi)在較大場地內(nèi)自由運動，方便相機采集大量數(shù)據(jù)進(jìn)行計算，大大提高了參數(shù)計算的速度和在大空間中進(jìn)行標(biāo)定操作的便捷性。

2 系統(tǒng)標(biāo)定

在一維標(biāo)定桿標(biāo)定算法中，首先需要對相機成像平面圖像進(jìn)行二值化處理，使標(biāo)記點的成像點與周圍壞境的成像形成較大亮度差，以便突出標(biāo)記點在成像圖像中的位置，進(jìn)而提取標(biāo)記點的像素坐標(biāo)。具體操作步驟：將圖像上256個亮度等級進(jìn)行閾值設(shè)置，使像素點灰度值僅存在0和255，此時成像圖像中僅有標(biāo)記點顯示為白色。由于受噪聲和相機鏡頭畸變的影響，在二值化成像平面中顯示的標(biāo)記點的成像點可能是一個不均勻的近圓圖像，因此需要對二值圖像中的標(biāo)記點提取像素邊緣，再將提取后的近圓圖像擬合成圓，最后求解該圓的圓心坐標(biāo)，其圓心坐標(biāo)做為標(biāo)記點的二維像素坐標(biāo)[3]。

一維標(biāo)定桿上的標(biāo)記點在多目相機下投影到成像平面上的示意圖如圖2所示。記Aj、Bj和Cj為標(biāo)記點A、B和C在空間中運動時在不同位置產(chǎn)生的空間點，j=1，2，3，…，N。Oi為第i個相機的光心，i=1，2，3，…，m。標(biāo)記點在相機成像平面中的成像點分別為aij、bij、cij。建立關(guān)于空間點和相機位姿的重投影誤差函數(shù)，對重投影誤差進(jìn)行整體優(yōu)化，相機參數(shù)為優(yōu)化變量。最小化重投影誤差函數(shù)即可得到最優(yōu)的相機參數(shù)解，建立的重投影誤差函數(shù)如公式（1）所示。

圖2 一維標(biāo)定桿多相機成像示意圖

使用稀疏LM（Levenberg-Marquardt）算法[4]對重投影誤差函數(shù)進(jìn)行優(yōu)化求解。為了提高參數(shù)求解的準(zhǔn)確度，根據(jù)標(biāo)定桿一維線性的性質(zhì)，以標(biāo)定桿上的Aj點為原點建立球坐標(biāo)系，根據(jù)球坐標(biāo)與直角坐標(biāo)的轉(zhuǎn)換關(guān)系為x=rsin?cosθ，y=rsin?sinθ，z=rcosθ。其中，r為某一點到球坐標(biāo)系原點的距離，某點到原點的連線與正z軸之間的夾角為?，該連線在xy平面的投影線與正x軸之間的夾角為θ。該文中，Aj為原點，那么Bj的球坐標(biāo)中r值為175，同理Cj的r值為500。由于3個標(biāo)記點都在一條直線上，因此Bj、Cj點球坐標(biāo)的?、θ一致。該空間中Bj、Cj的球坐標(biāo)如公式（2）所示。

將式（2）帶入相機成像模型表達(dá)式[3]，得到aij、bij和cij的球坐標(biāo)表達(dá)式，再根據(jù)公式（1）進(jìn)行參數(shù)求解。

仿真中的相機硬件參數(shù)為焦距5.5 mm、圖像分辨率1280×1024，根據(jù)該參數(shù)，可以計算得到相機實際內(nèi)部參數(shù)值：fx=178.571 pixels，fy=178.571 pixels，u0=640 pixels，v0=512 pixels。根據(jù)系統(tǒng)標(biāo)定過程，采集真實場地下標(biāo)定桿在成像平面上的二維像素坐標(biāo)數(shù)據(jù)，并依次計算2個相機、3個相機以及4個相機系統(tǒng)下的每個相機的內(nèi)參值，結(jié)果見表1。

表1 不同相機數(shù)目組成的系統(tǒng)標(biāo)定結(jié)果

將表1中不同相機數(shù)目下計算出的內(nèi)參均值與實際內(nèi)參值進(jìn)行比較。以不同相機數(shù)目下計算出的fx參數(shù)結(jié)果為例，2臺、3臺和4臺相機的計算結(jié)果的平均值分別為203.201 pixels、227.809 pixels和194.776 pixels，那么4臺相機下的fx計算值與實際內(nèi)參值誤差最小。同理，將其他參數(shù)值進(jìn)行比較，可以發(fā)現(xiàn)，在使用4臺相機進(jìn)行內(nèi)參計算時，誤差均小于2臺相機和3臺相機下的參數(shù)計數(shù)結(jié)果，可以說明4臺相機對抑制噪聲等干擾的效果較好，標(biāo)定結(jié)果更準(zhǔn)確。因此，可以通過增加相機數(shù)目來提高標(biāo)定精度。

3 三維重建

經(jīng)過上述過程可以得到相機的相關(guān)參數(shù)，即得到投影矩陣，再結(jié)合相機成像模型、求解投影矩陣的流程，可以將標(biāo)記點從成像圖像上的二維像素坐標(biāo)恢復(fù)為空間三維坐標(biāo)，該過程為三維重建的過程。根據(jù)相機成像模型，假設(shè)空間某點坐標(biāo)為（X，Y，Z），將該點坐標(biāo)寫為齊次坐標(biāo)[5]形式為（X，Y，Z，1），經(jīng)過參數(shù)估計后，空間某點在一個相機下的投影公式如公式（3）所示。

一般來講，相機數(shù)目越多，根據(jù)投影坐標(biāo)公式得到的約束方程組就越多，從而可以得到更加精準(zhǔn)的空間點坐標(biāo)。為了驗證這一點，該文使用了精度為0.01 mm的云臺，對多目紅外系統(tǒng)進(jìn)行位移定位準(zhǔn)確度測量。測量結(jié)果發(fā)現(xiàn)，2臺相機位移準(zhǔn)確度可達(dá)0.53 mm，3臺相機位移準(zhǔn)確度可達(dá)0.43 mm，6臺相機位移準(zhǔn)確度達(dá)0.26 mm，8臺相機的位移準(zhǔn)確度可達(dá)0.10 mm。可以發(fā)現(xiàn)，相機數(shù)目越多，位移準(zhǔn)確度越好。

4 無人車軌跡控制

該文所搭建的無人車控制系統(tǒng)硬件模塊主要包括電源模塊、控制器模塊、CAN總線接口模塊和電機驅(qū)動模塊。控制器與電機通過CAN總線進(jìn)行通信。其中，電機通過CAN總線向控制器傳輸數(shù)據(jù)時，需要要經(jīng)過編碼器將光信號轉(zhuǎn)換為電信號進(jìn)行傳輸。

使用STM32F103型號芯片作為控制器，在keil環(huán)境下進(jìn)行控制編程，使用PID控制算法實現(xiàn)對基于麥克納姆輪的無人車軌跡控制。根據(jù)麥克納姆輪的運動學(xué)模型[6]，求出無人車4個輪子分別對應(yīng)的旋轉(zhuǎn)速度，作為控制量。4個輪子分別設(shè)置4組PID參數(shù)，以實現(xiàn)對4個麥克納姆輪的PID控制，完成相應(yīng)的運動指令。

在軟件設(shè)計中，整個軟件需要實現(xiàn)小車相關(guān)參數(shù)的測量與計算、CAN總線通信、上位機與小車的串口通信、PID控制以及小車工作狀態(tài)監(jiān)控等多項任務(wù)，這些任務(wù)都是通過軟件端的前后臺操作系統(tǒng)來完成的。系統(tǒng)在運行時，程序會不斷在循環(huán)中調(diào)用相關(guān)函數(shù)來實現(xiàn)模塊功能，通過設(shè)置的相關(guān)中斷優(yōu)先級來運行當(dāng)前要緊急處理的任務(wù)，等中斷任務(wù)處理完成后，再返回到中斷點，繼續(xù)運行該程序，循環(huán)往復(fù)。

在實驗場地中使用八目紅外系統(tǒng)對無人車進(jìn)行運動軌跡捕捉，設(shè)置機器人圓周運動的半徑為1000 mm，速度為0.3 m/s的圓周運動，采集連續(xù)運動3圈的運動軌跡。圓周運動測量結(jié)果和指令軌跡之間的偏差如圖3所示，虛線表示指定軌跡，實線表示測量軌跡。

圖3 圓周運動測量結(jié)果和指令軌跡比較

通過計算測量軌跡與指令軌跡之間的誤差，可以得到最大誤差為237.667 mm。對3圈軌跡中每相鄰2圈的誤差進(jìn)行計算，發(fā)現(xiàn)相鄰兩圈均在水平方向產(chǎn)生最大誤差，分別為90.137 mm、87.776 mm，但是每相鄰兩圈的誤差變化較小。從測量結(jié)果中可以看出，使用PID控制算法控制基于麥克納姆輪的無人車時，無人車在做水平方向運動時易產(chǎn)生較大的累積誤差，該誤差可能和輪子與地面之間的摩擦、傳動機構(gòu)的機械誤差有關(guān)。

5 結(jié)語

該文提出了一套基于視覺的室內(nèi)無人車導(dǎo)航系統(tǒng)，介紹了多目紅外系統(tǒng)的定位流程，并通過對多目紅外視覺系統(tǒng)的標(biāo)定、空間位置進(jìn)行計算，實現(xiàn)了對無人車的定位導(dǎo)航。經(jīng)實驗發(fā)現(xiàn)，使用一維標(biāo)定法對多目紅外系統(tǒng)進(jìn)行標(biāo)定，能夠快速準(zhǔn)確地計算出相機相關(guān)參數(shù)。同時，多目紅外系統(tǒng)可滿足室內(nèi)10 m范圍內(nèi)的定位導(dǎo)航，且相機越多系統(tǒng)精度越高，8臺相機的位移準(zhǔn)確度可達(dá)0.106 mm。

該系統(tǒng)在其他工程領(lǐng)域也有較大的應(yīng)用空間，可應(yīng)用于無人車軌跡精度的測量，通過測量的誤差結(jié)果，可以對無人車進(jìn)行運動誤差補償，以提高無人車的定位導(dǎo)航精度，從而提高無人車的工作效率，也可用于其他機器人產(chǎn)品的運動精度測量，包括無人機等空間運動的機器人產(chǎn)品，能夠為國內(nèi)機器人相關(guān)產(chǎn)品提供一套標(biāo)準(zhǔn)的測試平臺。