喻 俊 樓佩煌 武 星 王龍軍

南京航空航天大學(xué)，南京，210016

0 引言

自動 引 導(dǎo) 車 （automated guided vehicle，AGV）是一種輪式移動機(jī)器人，廣泛用于工廠自動化生產(chǎn)線、倉儲物流、機(jī)場和港口中的物料傳送［1］。視覺導(dǎo)引方式是通過安裝在AGV上的攝像機(jī)采集鋪設(shè)導(dǎo)引路徑圖像，通過計(jì)算機(jī)視覺測量算法實(shí)時測量AGV相對導(dǎo)引路徑的距離偏差和角度偏差。攝像機(jī)標(biāo)定（camera calibration，CC）是計(jì)算機(jī)視覺測量的一個關(guān)鍵問題，精確的視覺標(biāo)定能夠消除視覺測量的系統(tǒng)誤差［2］。

攝像機(jī)標(biāo)定方法主要有傳統(tǒng)標(biāo)定法、主動視覺標(biāo)定法和自標(biāo)定法。傳統(tǒng)的攝像機(jī)標(biāo)定是將精密制作的立體或平面標(biāo)靶放置在場景中，利用幾何成像原理建立場景中世界坐標(biāo)點(diǎn)與圖像中觀測點(diǎn)之間的關(guān)系模型，采用線性、非線性法計(jì)算攝像機(jī)的內(nèi)外部參數(shù)和畸變參數(shù)，具有標(biāo)定精度高的特點(diǎn)［2］?；谥鲃右曈X的標(biāo)定方法需要控制攝像機(jī)做特定運(yùn)動，如相互正交的平動［3］和繞光軸的旋轉(zhuǎn)運(yùn)動［4］等，利用參考點(diǎn)的圖像坐標(biāo)變化實(shí)現(xiàn)攝像機(jī)的內(nèi)部參數(shù)標(biāo)定。該方法對攝像機(jī)運(yùn)動精度要求苛刻，主要用于工業(yè)機(jī)器人手眼標(biāo)定，不適用于攝像機(jī)運(yùn)動自由度受限的場合。

在鏡頭和攝像機(jī)位置均不變的被動視覺導(dǎo)引AGV中，攝像機(jī)內(nèi)外部參數(shù)和畸變參數(shù)均不變［2］。傳統(tǒng)標(biāo)定法雖然可以獲得攝像機(jī)的外部參數(shù)，但是其計(jì)算的外部參數(shù)只是標(biāo)靶坐標(biāo)系與攝像機(jī)坐標(biāo)系之間的關(guān)系。在AGV視覺導(dǎo)航應(yīng)用中，還需要獲得攝像機(jī)與AGV坐標(biāo)系之間的關(guān)系，而不借助其他精密測量設(shè)備則很難將標(biāo)靶相對機(jī)器人坐標(biāo)系精確地放置。根據(jù)現(xiàn)場標(biāo)定的要求，本文提出一種基于靜止和運(yùn)動兩種狀態(tài)的AGV視覺標(biāo)定方法。首先采用靜止?fàn)顟B(tài)下的平面模型標(biāo)定法計(jì)算內(nèi)部參數(shù)、畸變參數(shù)和AGV相對模板的外部參數(shù)，建立對三種圖像失真進(jìn)行圖像校正的聯(lián)合模型，再在運(yùn)動狀態(tài)下標(biāo)定出AGV坐標(biāo)系相對失真校正后圖像坐標(biāo)系的參數(shù)。

1 視覺系統(tǒng)模型

攝像機(jī)前傾安裝方式的AGV［5］視覺系統(tǒng)因其視場較大，精度較低，其標(biāo)定通常采用基于消失點(diǎn)（vanishing point，VP）的方法［6］，這種線性方法只能標(biāo)定出攝像機(jī)安裝的俯角和高度，忽略了其他自由度上與理想姿態(tài)的誤差。本文研究的雙向型AGV結(jié)構(gòu)如圖1所示。AGV的驅(qū)動系統(tǒng)由左右兩個獨(dú)立驅(qū)動的驅(qū)動輪和前后兩個起支撐作用的萬向輪組成。這種機(jī)構(gòu)可以通過調(diào)節(jié)兩個驅(qū)動輪的速度和轉(zhuǎn)向，實(shí)現(xiàn)直線運(yùn)動和任意轉(zhuǎn)彎半徑的圓弧運(yùn)動［7］，兩驅(qū)動輪軸線的中點(diǎn)稱控制中心點(diǎn)。根據(jù)AGV的運(yùn)動學(xué)模型，采用最優(yōu)控制方法，將AGV相對導(dǎo)引路徑的距離偏差和角度偏差同時消除到零［7］。根 據(jù) AGV 的 機(jī)械結(jié)構(gòu)和控制模型，定義AGV坐標(biāo)系OAXAYA的原點(diǎn)OA為控制中心點(diǎn)在地面的投影，XA為AGV直線前進(jìn)方向，YA平行于驅(qū)動輪軸線。攝像機(jī)的理想位姿是使攝像機(jī)位于控制中心點(diǎn)的上方且垂直于地面。圖像行向量、列向量分別與XA、YA平行。攝像機(jī)這種安裝方式的AGV具有測量精度高、實(shí)時性強(qiáng)的特性，且AGV能夠雙向運(yùn)動。

給定一個一般空間平面與圖像平面之間的單應(yīng)矩陣，如果空間平面在世界坐標(biāo)系的坐標(biāo)已知，則該單應(yīng)矩陣可以提供關(guān)于攝像機(jī)內(nèi)部參數(shù)的兩個線性約束。但是僅知道某一個場景中的兩幅圖像間的單應(yīng)矩陣不能對攝像機(jī)內(nèi)部參數(shù)構(gòu)成任何約束［8］。由于AGV只能在地面做二維剛體運(yùn)動，因此無論AGV以何種方式運(yùn)動，地面參考點(diǎn)均在一個平面內(nèi)，不能獲得足夠的約束以求解出所有的內(nèi)部參數(shù)。標(biāo)定模板與圖像平面平行時的攝像機(jī)標(biāo)定方法［9］必須借助輔助測量設(shè)備才能滿足平行條件，不易在現(xiàn)場實(shí)施。

攝像機(jī)實(shí)際位姿相對理想位姿存在三個角度誤差和兩個平移誤差，在肉眼下微調(diào)攝像機(jī)姿態(tài)能夠?qū)⑦@些誤差控制在較小的范圍內(nèi)。根據(jù)相機(jī)成像原理，一個不垂直于主光軸的平面視場在垂直于主光軸的成像平面上的成像會存在傾斜失真；普通鏡頭由于制造工藝的缺陷會形成圖像徑向畸變失真；成像物理平面在行列兩個方向上的單位長度的像素?cái)?shù)不一定相等，會形成圖像比例失真［2］。這三種圖像失真會給基于平面矢量圖的視覺測量引入系統(tǒng)誤差，因此必須建立圖像失真的聯(lián)合模型，對三種圖像失真作圖像校正。AGV坐標(biāo)系的原點(diǎn)OA和XA方向分別作為控制策略的參照基準(zhǔn)，也必須精確標(biāo)定。

圖1 AGV結(jié)構(gòu)模型

2 靜止?fàn)顟B(tài)攝像機(jī)標(biāo)定及圖像校正

2.1 靜止?fàn)顟B(tài)攝像機(jī)標(biāo)定

包含安裝誤差的攝像機(jī)實(shí)際成像模型如圖2所示。設(shè)攝像機(jī)坐標(biāo)系為OcXcYcZc，圖像坐標(biāo)系為（u，v）。根據(jù)理想透視模型有

式中，A為含有4參數(shù)的內(nèi)部參數(shù)矩陣；u0、v0為攝像機(jī)的主點(diǎn)在圖像中的像素坐標(biāo)；kx、ky分別為攝像機(jī)焦距歸一化后，像素相對成像平面物理坐標(biāo)在u、v方向的放大系數(shù)。

圖2 攝像機(jī)實(shí)際成像模型圖

攝像機(jī)坐標(biāo)系OcXcYcZc與世界坐標(biāo)系OwXwYwZw在歐氏空間的坐標(biāo)變換可以通過三維旋轉(zhuǎn)矩陣和平移矩陣表示：

式中，R為3×3坐標(biāo)旋轉(zhuǎn)矩陣；T為3×1坐標(biāo)平移矩陣。

一般情況下，僅僅考慮徑向畸變已經(jīng)足夠描述由于鏡頭引起的非線性畸變，徑向畸變關(guān)于主點(diǎn)是中心對稱的［4］。Zhang等［10］提出的由理想坐標(biāo)參數(shù)化實(shí)際坐標(biāo)的模型與Tasi［11］提出的由實(shí)際坐標(biāo)參數(shù)化理想坐標(biāo)的模型類似，但后者在對畸變圖像校正時，能夠直接使用標(biāo)定出的模型參數(shù)。設(shè)成像平面上的理想物理坐標(biāo)為（Xu，Yu），畸變后的實(shí)際物理坐標(biāo)為（Xd，Yd），采用 Tasi［11］提出的畸變參數(shù)模型，有

式中，k1、k2為徑向畸變參數(shù)。

設(shè)實(shí)際像素點(diǎn)（u，v）經(jīng)圖像徑向畸變校正后的像素點(diǎn)為（ud，vd），攝像機(jī)內(nèi)部參數(shù)模型中，kx、ky分別是像素相對成像平面物理坐標(biāo)在u、v方向的放大系數(shù)，由式（3）可得

要求解的模型參數(shù)包括攝像機(jī)內(nèi)部參數(shù)u0、v0、kx、ky，畸變參數(shù)k1、k2和外部參數(shù)R、T。這是一個非線性最優(yōu)問題，通常采用先不考慮畸變的線性模型，估計(jì)出每個參數(shù)的初值，再用最大似然估計(jì)法計(jì)算非線性模型下參數(shù)的精確值。Zhang等［10］提出的平面模板標(biāo)定法比 Tasi［11］提出的三維模板標(biāo)定法更易在工業(yè)現(xiàn)場實(shí)現(xiàn)，具有較高的柔性。對于平面模板坐標(biāo)系，有Zw＝0，令旋轉(zhuǎn)矩陣 R＝ ［r1r2r3］，平移矩陣T＝［txtytz］T，由式（1）、式（2）可得

式中，H為3×3單應(yīng)矩陣。

將精確打印的有N個方格交點(diǎn)的模板以不同的姿態(tài)拍攝M 幅圖像（其中一幅為模板放置在地面上），這些不同位姿的模板不在同一平面內(nèi)且不是純平動的，平面模板之間的運(yùn)動參數(shù)不需要知道。采用Harris角點(diǎn)檢測算法提取圖像中方格的交點(diǎn)，根據(jù)圖像方格交點(diǎn)與世界坐標(biāo)系中模板真實(shí)坐標(biāo)的一一對應(yīng)關(guān)系，由式（5）消去Zc，每個方格點(diǎn)可以確定兩個約束方程。因此，當(dāng)N≥5時，即可采用最小二乘法計(jì)算出第i（i＝1，2，…，M）幅圖像對應(yīng)的單應(yīng)矩陣Hi。

旋轉(zhuǎn)矩陣R是一個單位正交矩陣，可以建立兩個約束條件［10］：

由式（6）、式（7）可以得到內(nèi)部參數(shù)矩陣的兩個約束方程。當(dāng)M≥3時，即可采用最小二乘法計(jì)算出內(nèi)部參數(shù)矩陣A的4個內(nèi)部參數(shù)的初值。已知內(nèi)部參數(shù)矩陣A和單應(yīng)矩陣Hi，代入式（6）可以得到每幅圖像對應(yīng)的r1i、r2i和Ti。r3i可以由下式得到：

至此，模型參數(shù)中的u0、v0、kx、ky、Ri和Ti的初值均已得出。令畸變參數(shù)k1、k2的初值為0，在非線性模型下，采用最大似然估計(jì)法對模型參數(shù)進(jìn)行優(yōu)化。

對有N個方格交點(diǎn)的M幅圖像，最大似然估計(jì)可以通過最小化距離函數(shù)獲得：

其中，mij（A，k1，k2）是由式（4）獲得的畸變校正后的像素點(diǎn)；m⌒ij（A，Ri，Ti，Mij）是由式（1）、式（2）獲得的理想透視模型下世界坐標(biāo)點(diǎn)Mij對應(yīng)的理想圖像坐標(biāo)點(diǎn)。對式（9），采用基于梯度下降的非線性最小二乘最優(yōu)化算法Levenberg－Marquardt，獲得使函數(shù)值最小的 u0、v0、kx、ky、k1、k2、Ri和Ti的最優(yōu)解。模板采集的次數(shù)越多，計(jì)算結(jié)果越精確。

2.2 圖像失真校正

由視覺成像模型可知，圖像相對地面真實(shí)場景存在三種失真，即徑向畸變引起的畸變失真，kx與ky不相等引起的比例失真，以及攝像機(jī)主光軸不嚴(yán)格垂直于地面引起的傾斜失真。為了方便基于矢量圖的視覺測量算法的實(shí)現(xiàn)，需要事先對這三種圖像失真進(jìn)行校正，使校正后的圖像與地面真實(shí)場景成比例。畸變失真參數(shù)和比例失真參數(shù)通過攝像機(jī)標(biāo)定已經(jīng)確定。

設(shè)在攝像機(jī)標(biāo)定中，標(biāo)定模板放置在地面時，對應(yīng)的外部參數(shù)為Rg和Tg，令Tg＝［tgxtgytgz］T，tgx、tgy、tgz為三個坐標(biāo) 軸 上 的平移分量。Rg是隱式的旋轉(zhuǎn)矩陣，用歐拉角表示時，根據(jù)坐標(biāo)旋轉(zhuǎn)的次序有多種形式［12］。設(shè)模板所在的世界坐標(biāo)系為OwXwYwZw，順序繞Zw軸旋轉(zhuǎn)γ、Yw軸旋轉(zhuǎn)β、Xw軸旋轉(zhuǎn)α后，與攝像機(jī)坐標(biāo)系平行，即

其中，Rx（α）、Ry（β）、Rz（γ）為歐拉角旋轉(zhuǎn)矩陣，它們的逆矩陣分別為自身的轉(zhuǎn)置。由Rg計(jì)算α、β、γ可以參考文獻(xiàn)［12］。

首先由式（4）對圖像作徑向畸變校正，得到像素點(diǎn)（ud，vd）。再作比例失真校正，以攝像機(jī)主點(diǎn)為中心，作以u軸為基準(zhǔn)的比例拉伸，由式（1）可得

最后作傾斜失真校正，如圖2所示，對圖像平面分別以y、x 為轉(zhuǎn)軸，作RTy（β）、RTx（α）的順序旋轉(zhuǎn)變換，令（U，V）為對三種圖像失真校正后的圖像，則

校正后的成像模型可以簡化為

其中，s＝kx／tgz為像素相對標(biāo)定模板的比例因子。式（13）表明，失真校正后的圖像相對地面上的平面模板是一個含有比例因子s的二維旋轉(zhuǎn)平移變換。由于AGV坐標(biāo)平面OAXAYA是作為控制策略的基準(zhǔn)定義的，考慮到機(jī)械安裝誤差等因素，在不借助精密儀器測量的情況下，很難精確地將標(biāo)定模板放置在與AGV坐標(biāo)平面完全一致的位置，圖像與AGV坐標(biāo)平面OAXAYA滿足比例因子同為s的二維旋轉(zhuǎn)平移變換。因此有

式中，ψ、tAx和tAy分別為校正后AGV坐標(biāo)系OAXAYA的旋轉(zhuǎn)角和兩個平移分量。

解出ψ、tAx和tAy即可得到校正后的圖像相對AGV坐標(biāo)系的參數(shù)。

3 運(yùn)動狀態(tài)AGV標(biāo)定

3.1 直線運(yùn)動狀態(tài)下的標(biāo)定

式（15）表明，參考點(diǎn)在圖像序列中的坐標(biāo)呈線性分布，ψ與參考點(diǎn)的初始位置及AGV的速度無關(guān)，標(biāo)定方法具有較好的柔性。采用最小二乘法直線擬合，如圖3所示，擬合直線與圖像坐標(biāo)系U軸的夾角即為ψ，順時針為正。由最小二乘法得ψ的估計(jì)為

式中，F(xiàn)為采集的視頻幀數(shù)。

受圖像隨機(jī)噪聲和可能存在的驅(qū)動輪滑動影響，Ut與Vt并不一定是完全線性相關(guān)的，線性相關(guān)系數(shù)r可以用來分析它們的線性相關(guān)度（理想狀況下｜r｜＝1）：

圖3 直線運(yùn)動狀態(tài)下的標(biāo)定

3.2 自轉(zhuǎn)狀態(tài)下的標(biāo)定

式中，ω為AGV的角速度；φ0、RA分別為參考點(diǎn)的初始位置角和到AGV坐標(biāo)原點(diǎn)的距離。

由式（18）和式（14）可得

式（19）表明，參考點(diǎn)的像素坐標(biāo)在以（tAx，tAy）為圓心、以sRA為半徑的圓上，并且與參考點(diǎn)的初始位置角和AGV的角速度無關(guān)。對圖像序列中的參考點(diǎn)采用最小二乘法圓弧擬合，如圖4所示，擬合圓弧的圓心即為AGV坐標(biāo)系相對圖像坐標(biāo)系的平移向量。

圖4 自轉(zhuǎn)狀態(tài)下的標(biāo)定

圓是一種非線性模型，為了便于計(jì)算，設(shè)圖像坐標(biāo)系下圓的方程為

令d＝－（x2＋y2），e為估計(jì)誤差，則估計(jì)圓弧模型參數(shù)就近似轉(zhuǎn)化為二元線性回歸問題：

d＝a0＋a1x＋a2y＋e

其條件是B的秩R（B）≥3，由于樣本數(shù)量較大，故很容易滿足。圓弧圓心坐標(biāo)為

圓弧半徑為

同樣考慮隨機(jī)噪聲和可能存在的驅(qū)動輪滑動的影響，圓弧擬合均方差為

4 實(shí)驗(yàn)及分析

圖5 標(biāo)定流程圖

本文采用的實(shí)驗(yàn)設(shè)備為維視VS－818HC工業(yè)相機(jī)（3.5～8mm手動變焦鏡頭VS－0358M），標(biāo)定流程如圖5所示。靜止?fàn)顟B(tài)下采集不同姿態(tài)網(wǎng)格模板圖像16幅，圖像分辨率為720×576像素，采用平面模板標(biāo)定法標(biāo)定出攝像機(jī)的內(nèi)部參數(shù)、徑向畸變參數(shù)和每個模板對應(yīng)的外部參數(shù)。由放置在地面的平面模板對應(yīng)的外部參數(shù)計(jì)算得到攝像機(jī)相對地面的兩個傾斜角和物距。通過三種圖像失真校正的聯(lián)合模型對圖像進(jìn)行校正，也確定了圖像相對地面真實(shí)場景的比例因子。靜止?fàn)顟B(tài)各參數(shù)標(biāo)定結(jié)果如表1所示，大部分參數(shù)的標(biāo)定均方差都在0.5%以內(nèi)，ky比kx大近10%，比例失真嚴(yán)重。圖像相對地面場景的比例因子為2.72，視覺測量精度較高。

表1 靜止?fàn)顟B(tài)參數(shù)標(biāo)定

分別采集AGV勻速直線運(yùn)動和原地自轉(zhuǎn)視頻各4次，對視頻圖像先進(jìn)行失真校正，提取參考點(diǎn)在圖像中的坐標(biāo)，再對參考點(diǎn)像素序列分別采用直線和圓弧進(jìn)行擬合，獲得失真校正后圖像坐標(biāo)系相對AGV坐標(biāo)系的旋轉(zhuǎn)和平移參數(shù)。如表2所示，直線運(yùn)動狀態(tài)下參考點(diǎn)的線性相關(guān)性高且非常穩(wěn)定，說明并沒有出現(xiàn)非理想的滑動。如表3所示，自轉(zhuǎn)運(yùn)動狀態(tài)下標(biāo)定出的圓心坐標(biāo)也非常穩(wěn)定，也沒有出現(xiàn)非理想的滑動，換算到實(shí)際物理坐標(biāo)下的均方差小于1mm。

表2 直線運(yùn)動狀態(tài)參數(shù)標(biāo)定

表3 自轉(zhuǎn)狀態(tài)參數(shù)標(biāo)定

5 結(jié)論

本文提出了一種基于靜止和運(yùn)動兩種狀態(tài)的視覺導(dǎo)引AGV視覺標(biāo)定方法，該方法能夠在不借助其他輔助測量設(shè)備的條件下，精確標(biāo)定出攝像機(jī)的內(nèi)部參數(shù)、徑向畸變參數(shù)和相對AGV控制模型坐標(biāo)系的外部參數(shù)。經(jīng)三種圖像失真聯(lián)合模型校正后的圖像能夠應(yīng)用于基于平面矢量圖的視覺測量。本文提出的現(xiàn)場標(biāo)定方法具有柔性好、精度高、實(shí)用性強(qiáng)的特點(diǎn)。

［1］Iris F A.Vis，Survey of Research in the Design and Control of Automated Guided Vehicle Systems［J］.European Journal of Operational Research，2006，170（3）：677－709.

［2］Salvi J，Armangue X，Batlle J.A Comparative Re－view of Camera Calibrating Methods with Accuracy Evaluation［J］.Pattern Recognition，2001，35：1617－1635.

［3］吳福朝，李華，胡占義.基于主動視覺系統(tǒng)的攝像機(jī)自標(biāo)定方法研究［J］.自動化學(xué)報(bào)，2001，27（6）：752－762.

［4］Tordoff B，Murray D W.The Impact of Radial Distortion on the Self－calibration of Rotating Cameras［J］.Computer Vision and Image Understanding，2004，96：17－34.

［5］王榮本，徐友春，李慶東，等.AGVS圖象識別多分支路徑的研究［J］.中國圖象圖形學(xué)報(bào)，2000，8（5）：632－637.

［6］Guillou E，Meneveaux D，Maisel E，et al.Using Vanishing Points for Camera Calibration and Coarse 3D Reconstruction from a Single Image［J］.The Visual Computer，2000，16（7）：396－410.

［7］陳無畏，李碧春，孫海濤，等.基于視覺導(dǎo)航的AGV模糊－最優(yōu)控制研究［J］.中國機(jī)械工程，2006，17（24）：2546－2550.

［8］孫鳳梅，胡占義.平面單應(yīng)矩陣對攝像機(jī)內(nèi)參數(shù)約束的一些性質(zhì)［J］.計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)，2007，19（5）：647－650.

［9］羅晨，朱利民，丁漢.標(biāo)定模板與圖像平面平行時的攝像機(jī)標(biāo)定方法［J］.中國機(jī)械工程，2006，17（16）：1558－1561.

［10］Zhang Zhengyou.Flexible Camera Calibration by Viewing a Plane from Unknown Orientations［C］／／Proceedings of the 1999 7th IEEE Int.Conference on Computer Vision（ICCV’99）.Kerkyra，1999：666－673.

［11］Tsai Y.A Versatile Camera Calibration Technique for High－accuracy 3DMachine Vision Metrology Using Off－the Shelf TV Cameras and Lenses［J］.IEEE Int.J.Robot.Automat.，1987，3：323－344.

［12］Diebel J.Representing Attitude：Euler Angles，Unit Quaternions，and Rotation Vectors［M］.Stanford：Stanford University，2006.