朱本龍 李祥坤 黃 歡

（1．徐州市檢驗(yàn)檢測中心，江蘇 徐州 221111；2．江蘇師范大學(xué)電氣工程及自動化學(xué)院，江蘇 徐州 221116）

0 引言

工程車輛應(yīng)用廣泛，隨著技術(shù)發(fā)展與應(yīng)用的需要，無人工程車輛技術(shù)已成為未來發(fā)展的趨勢。和傳統(tǒng)工程車輛相比，無人操控對車輛機(jī)械臂控制精度有更高的要求[1]。提高機(jī)械臂末端定位精度的方法有兩種[2]，即使用更好的生產(chǎn)工藝和對機(jī)械臂進(jìn)行運(yùn)動學(xué)參數(shù)標(biāo)定。第一種方法會極大地增加生產(chǎn)成本，所以參數(shù)標(biāo)定是最好的選擇。

進(jìn)行工程車輛運(yùn)動學(xué)參數(shù)標(biāo)定對工程車輛末端執(zhí)行機(jī)構(gòu)的位置測量精度有很高要求。常用測量儀器有拉線傳感器和激光跟蹤儀。激光跟蹤儀設(shè)備昂貴，拉線傳感器操作步驟多，相比之下，基于相機(jī)的視覺測量方案具有成本低、檢測精度高且檢測速度快等優(yōu)點(diǎn)。

該文設(shè)計(jì)了一個(gè)基于多目視覺的工程車輛機(jī)械臂運(yùn)動學(xué)參數(shù)校準(zhǔn)系統(tǒng)。首先根據(jù)相機(jī)視場的范圍對多目視覺系統(tǒng)中相機(jī)的布局進(jìn)行調(diào)整，使該系統(tǒng)能夠適用于不同大小的工程車輛。其次對多目視覺系統(tǒng)的精度進(jìn)行校準(zhǔn)，使用校準(zhǔn)后的系統(tǒng)測量工程車輛機(jī)械臂的末端位置。最后使用最小二乘法計(jì)算機(jī)械臂運(yùn)動學(xué)參數(shù)誤差，并對原參數(shù)進(jìn)行補(bǔ)償，以提高無人工程車輛的控制精度。

1 參數(shù)校準(zhǔn)系統(tǒng)設(shè)計(jì)

1.1 多目視覺系統(tǒng)相機(jī)陣列的規(guī)劃

相機(jī)視場是指相機(jī)的有效觀測范圍，因?yàn)橄鄼C(jī)使用的是透視原理，所以相機(jī)的視場是一個(gè)錐體。多目視覺系統(tǒng)相機(jī)陣列規(guī)劃的目標(biāo)是使被測量的物體至少同時(shí)處于2個(gè)或2個(gè)以上的相機(jī)視場中，這樣才能得到深度信息。對運(yùn)動物體來說，相機(jī)規(guī)劃尤其重要，因?yàn)橐坏┪矬w脫離了多目相機(jī)的視場，就容易出現(xiàn)誤跟蹤。

該文將無人工程車輛機(jī)械臂鏟斗末端的運(yùn)動軌跡限制在一個(gè)平面上。進(jìn)行位置測量時(shí)，為了確保有足夠的測量精度，必須要令2個(gè)或者2個(gè)以上的相機(jī)同時(shí)觀測到鏟斗末端，這樣就一定會有2個(gè)或者2個(gè)以上相機(jī)視場在運(yùn)動平面上的投影相互重疊。雖然每個(gè)相機(jī)視場在運(yùn)動平面上的投影區(qū)域的頂點(diǎn)可以為相機(jī)位姿的函數(shù)，但是卻很難得到多個(gè)相機(jī)視場在運(yùn)動平面上投影的重疊面積的解析形式，也就無法直接對該目標(biāo)進(jìn)行優(yōu)化[3]。因此，該文使用軟件輔助手動調(diào)節(jié)的方式在投影平面上對相機(jī)位姿進(jìn)行布局，具體步驟如下：1）將無人工程車輛機(jī)械臂鏟斗末端的運(yùn)動約束在一個(gè)平面上，并確定其運(yùn)動軌跡。2）根據(jù)投影方程，獲得每個(gè)相機(jī)視場在運(yùn)動平面上的投影。3）手動調(diào)節(jié)相機(jī)位姿，保證待檢測軌跡上的每點(diǎn)至少位于2個(gè)相機(jī)視場的重疊范圍之內(nèi)。4）在保證以上條件的前提下縮短相機(jī)和投影面的距離，并增加相機(jī)視野的重疊程度，進(jìn)而提高測量精度，最終得到各相機(jī)的位姿信息。

1.2 多目視覺系統(tǒng)標(biāo)定

在進(jìn)行多目相機(jī)陣列規(guī)劃和無人工程車輛機(jī)械臂鏟斗末端位置測量時(shí)，要用到相機(jī)的內(nèi)、外參數(shù)，根據(jù)相機(jī)的像素坐標(biāo)可以對相機(jī)的內(nèi)、外參數(shù)進(jìn)行估計(jì)。該文使用一維標(biāo)定桿對多目視覺系統(tǒng)進(jìn)行標(biāo)定[4]。

使用一維標(biāo)定桿法對多目視覺系統(tǒng)標(biāo)定的步驟主要如下：首先，對相機(jī)拍到的一維標(biāo)定桿圖像進(jìn)行二值化處理[5]，以便對標(biāo)定桿上的標(biāo)記點(diǎn)進(jìn)行邊緣提取。因?yàn)榉垂獍悬c(diǎn)可以與周圍環(huán)境產(chǎn)生很大的亮度差，灰度對比很明顯，所以對標(biāo)定桿的灰度圖進(jìn)行二值化操作可以突出反光靶點(diǎn)的亮度。在圖像中，如果第r行c列的像素灰度值小于60，則將灰度值變?yōu)?；如果大于等于60，灰度值變?yōu)?55，二值化設(shè)置如公式（1）所示。

在二值化灰度圖像之后，對標(biāo)記點(diǎn)邊緣進(jìn)行提取。

其次，由于噪聲和畸變的影響，二值化圖像后得到的標(biāo)記點(diǎn)是一個(gè)不均勻的近圓圖像，因此該文通過將近圓擬合成圓并求解圓心的方法來獲取標(biāo)記點(diǎn)的二維位置。利用提取到的標(biāo)記點(diǎn)邊緣，使用最小二乘法求取標(biāo)記點(diǎn)中心的像素坐標(biāo)[6]。

最后，相機(jī)參數(shù)估計(jì)。一維標(biāo)定桿在多目相機(jī)下的投影關(guān)系如圖1所示。3個(gè)共線標(biāo)記點(diǎn)為A、B和C，記Aj、Bj和Cj為標(biāo)記點(diǎn)在空間中的不同位置，j=1，2，3，…，n。Oi為第i個(gè)相機(jī)的光心，i=0，1，2，3，…，m。記L為不同點(diǎn)之間的距離，那么L1=||A-B||，L2=||B-C||，L3=||A-C||。標(biāo)記點(diǎn)在相機(jī)成像平面中的成像點(diǎn)分別為aij、bij和cij。

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

假設(shè)經(jīng)過估計(jì)的重建矩陣H后得到的圖像點(diǎn)稱為重投影點(diǎn)，將該點(diǎn)和觀測點(diǎn)之間的距離最小化，如公式（2）所示。

函數(shù)d（x，y）為x和y的幾何距離函數(shù)，該函數(shù)稱為重投影誤差。根據(jù)Aj、Bj和Cj的共線性質(zhì)，以Aj為中心建立球坐標(biāo)系，用Aj來描述Bj、Cj的位置，那么空間點(diǎn)Bj、Cj與Aj的關(guān)系如公式（3）所示。

將公式（3）帶入投影坐標(biāo)公式（4）。

式中：矩陣K的參數(shù)由相機(jī)內(nèi)部結(jié)構(gòu)參數(shù)決定，稱為內(nèi)部參數(shù)矩陣；矩陣M為相機(jī)相對世界坐標(biāo)的位姿，稱為相機(jī)外部參數(shù)矩陣；H為一個(gè)投影變換矩陣，稱為單應(yīng)性矩陣；XW、YW和ZW是世界坐標(biāo)系3個(gè)方向上的位置；u和v為像素坐標(biāo)系2個(gè)方向上的位置；ZC為相機(jī)坐標(biāo)系中物體在光軸方向上的位置。

對Aj、Bj和Cj這3個(gè)點(diǎn)的重投影進(jìn)行整體誤差最小化求解，最小化重投影誤差表達(dá)式如公式（5）所示。

該過程同時(shí)對相機(jī)內(nèi)、外參數(shù)和標(biāo)定桿上標(biāo)記點(diǎn)的位置進(jìn)行優(yōu)化。需要優(yōu)化的參數(shù)過多，如果直接使用LM算法對其進(jìn)行求解的話，由于其雅可比矩陣具有稀疏性，會消耗大量計(jì)算時(shí)間，因此使用稀疏LM算法進(jìn)行求解。此算法可將參數(shù)按一定分塊結(jié)構(gòu)進(jìn)行求解，能夠有效減少計(jì)算時(shí)間。

1.3 機(jī)械臂末端位置定位

聯(lián)立所有相機(jī)的投影坐標(biāo)公式，可以獲得觀測點(diǎn)的三維信息。該文使用一臺無人挖掘機(jī)模型對該方法進(jìn)行驗(yàn)證。測量前需要先完成無人挖掘機(jī)模型機(jī)械臂的運(yùn)動學(xué)參數(shù)建模。不考慮模型車轉(zhuǎn)臺的水平轉(zhuǎn)動，其機(jī)械臂可以近似看成一個(gè)3自由度連桿機(jī)構(gòu)[7]。該文使用D-H參數(shù)模型對機(jī)械臂進(jìn)行運(yùn)動學(xué)建模，這是一種對連桿和關(guān)節(jié)進(jìn)行建模的通用方法。在機(jī)械臂的底座上建立基坐標(biāo)系，每到一個(gè)關(guān)節(jié)進(jìn)行一次坐標(biāo)系的變換，每次變換都會得到一個(gè)變換關(guān)系矩陣，最終得到執(zhí)行機(jī)構(gòu)坐標(biāo)相對基座標(biāo)的變換關(guān)系。

從坐標(biāo)系i到坐標(biāo)系 1i+ 的D-H參數(shù)坐標(biāo)變換如公式（6）所示。

式中：i為每個(gè)關(guān)節(jié)的標(biāo)號；αi為關(guān)節(jié)扭角；θi為關(guān)節(jié)轉(zhuǎn)角；αi為連桿長度；di為2個(gè)相鄰坐標(biāo)系i和i+1關(guān)于i坐標(biāo)系Z軸上的距離。

將每個(gè)工作裝置的變換矩陣按順序相乘就能得到機(jī)械臂鏟斗末端相對基坐標(biāo)系的坐標(biāo)變換矩陣，表達(dá)式如公式（7）所示。

測量過程如下：在機(jī)械臂鏟斗末端的運(yùn)動平面上選定若干空間坐標(biāo)點(diǎn)作為給定軌跡；使用鏟斗末端相對無人挖掘機(jī)模型基坐標(biāo)系的坐標(biāo)變換求這些點(diǎn)的運(yùn)動學(xué)逆解，求得運(yùn)動平面上這些空間點(diǎn)對應(yīng)的機(jī)械臂關(guān)節(jié)角度；使用這些關(guān)節(jié)角度對機(jī)械臂進(jìn)行控制，最后利用多目視覺得到無人挖掘機(jī)模型鏟斗末端標(biāo)記點(diǎn)的實(shí)際位置數(shù)據(jù)。

1.4 誤差參數(shù)辨識

根據(jù)機(jī)械臂的運(yùn)動學(xué)模型確定機(jī)械臂鏟斗末端的位置，如公式（8）所示。

由于制造工藝和裝配的影響，機(jī)械臂的4個(gè)運(yùn)動學(xué)參數(shù)必然會和理論上的值有一些差距，又因?yàn)闄C(jī)械臂各個(gè)關(guān)節(jié)的轉(zhuǎn)角都有編碼盤進(jìn)行讀取，所以不認(rèn)為關(guān)節(jié)轉(zhuǎn)角存在誤差，對應(yīng)3個(gè)運(yùn)動學(xué)參數(shù)a、d和α存在誤差Δa、Δd和Δα，機(jī)械臂的實(shí)測位置如公式（9）所示。

則機(jī)械臂鏟斗末端的位置誤差為公式（10）所示。

在運(yùn)動學(xué)參數(shù)誤差比較小的情況下，公式（10）可以近似為一個(gè)線性方程，如公式（11）所示。

測得機(jī)械臂工作空間內(nèi)n個(gè)點(diǎn)的坐標(biāo)后，可以將公式（11）轉(zhuǎn)換成公式（12）。

式中：A為關(guān)于無人挖掘機(jī)模型3個(gè)連桿D-H參數(shù)的雅可比矩陣；ΔX為D-H參數(shù)的誤差向量；b為n組數(shù)據(jù)的末端位置偏差向量。

得到D-H參數(shù)的誤差模型后，可以使用最小二乘法求D-H參數(shù)的最佳值，以使機(jī)械臂末端位置誤差最小[8]。具體表達(dá)式如公式（13）所示。

2 試驗(yàn)與結(jié)果分析

試驗(yàn)使用的多目視覺系統(tǒng)包括4個(gè)相機(jī)，相機(jī)型號為Prime13，相機(jī)參數(shù)為焦距5.5 mm，圖像分辨率1280×1024 pixel，水平視場角56°，垂直視場角46°。試驗(yàn)對象為一臺小型無人挖掘機(jī)模型，其初始參數(shù)如下：大臂等效為一個(gè)長58 cm的連桿，小臂等效為長25 cm的連桿，鏟斗等效為長10 cm的連桿。為了提高控制精度，初始參數(shù)需要進(jìn)行校準(zhǔn)。

2.1 相機(jī)陣列規(guī)劃

使用4個(gè)相機(jī)對小型無人挖掘機(jī)模型機(jī)械臂鏟斗末端位置進(jìn)行測量。4個(gè)相機(jī)之間相距1.5 m，各自距地面1.5 m。同時(shí)4個(gè)相機(jī)在一個(gè)和機(jī)械臂鏟斗末端運(yùn)動平面平行的直線上，4個(gè)相機(jī)和運(yùn)動平面之間的距離為3.5 m。此時(shí)4個(gè)相機(jī)視場在運(yùn)動平面上的投影兩兩覆蓋，使多目相機(jī)系統(tǒng)的探測范圍可以完全覆蓋住機(jī)械臂鏟斗末端的運(yùn)動范圍。多目相機(jī)測量系統(tǒng)4個(gè)相機(jī)視場在運(yùn)動平面上的投影如圖2所示，C1區(qū)域表示第1個(gè)相機(jī)視場在運(yùn)動平面上的投影；C1&C2表示第1個(gè)相機(jī)視場在運(yùn)動平面上的投影和第2個(gè)相機(jī)視場在運(yùn)動平面上的投影相重疊的區(qū)域；C2&C3表示第2個(gè)相機(jī)視場在運(yùn)動平面上的投影和第3個(gè)相機(jī)視場在運(yùn)動平面上的投影相重疊的區(qū)域；C3&C4表示第3個(gè)相機(jī)視場在運(yùn)動平面上的投影和第4個(gè)相機(jī)視場在運(yùn)動平面上的投影相重疊的區(qū)域；C4區(qū)域表示第4個(gè)相機(jī)視場在運(yùn)動平面上的投影；虛線為鏟斗末端在運(yùn)動平面上的運(yùn)動軌跡。圖2中兩兩相交的視場覆蓋了長為4.5 m、寬為3.75 m的矩形。

圖2 相機(jī)在鏟斗末端運(yùn)動平面上的投影圖

經(jīng)過標(biāo)定桿校準(zhǔn)，在當(dāng)前多目相機(jī)布局下，多目測量系統(tǒng)的測量平均誤差為0.155 mm。

2.2 鏟斗位置測量

設(shè)置完30個(gè)在機(jī)械臂鏟斗末端運(yùn)動平面上的目標(biāo)空間點(diǎn)后，按照上文測量位置的方法可以測得30個(gè)標(biāo)記點(diǎn)的實(shí)際位置數(shù)據(jù)。標(biāo)記點(diǎn)的位置與鏟斗末端的位置相差一個(gè)坐標(biāo)變換，對標(biāo)記點(diǎn)的位置使用坐標(biāo)變換得到鏟斗末端的實(shí)際位置。

2.3 運(yùn)動學(xué)參數(shù)校準(zhǔn)

將采集到的數(shù)據(jù)使用D-H參數(shù)誤差模型進(jìn)行處理，得到符合預(yù)期的D-H參數(shù)誤差向量，對原D-H參數(shù)進(jìn)行補(bǔ)償，得到校準(zhǔn)后的D-H參數(shù)。在D-H參數(shù)校正后，將上文中運(yùn)動平面上前5個(gè)點(diǎn)的機(jī)械臂關(guān)節(jié)角度數(shù)據(jù)輸入小型無人挖掘機(jī)模型控制器中，再使用多目視覺系統(tǒng)測量末端執(zhí)行機(jī)構(gòu)的位置，得到5個(gè)空間點(diǎn)坐標(biāo)。將其與這5個(gè)點(diǎn)的給定位置做比較可以得到位置上的誤差，取這5個(gè)點(diǎn)誤差的平均值作為機(jī)械臂校準(zhǔn)后的絕對定位誤差，見表1。未標(biāo)定時(shí)末端絕對定位誤差為1 cm～4 cm，標(biāo)定后機(jī)械臂末端絕對定位誤差為0.2 cm～0.4 cm，控制精度大為提升。

表1 無人挖掘機(jī)模型機(jī)械臂標(biāo)定前、后控制效果對比

3 結(jié)論

該文所設(shè)計(jì)的系統(tǒng)可以精確地測得機(jī)械臂末端的位置，使用的基于運(yùn)動學(xué)參數(shù)誤差模型的方法也可以很好地對無人挖掘機(jī)模型的機(jī)械臂進(jìn)行運(yùn)動學(xué)參數(shù)的辨識標(biāo)定。未來希望能夠利用算法自動規(guī)劃相機(jī)位姿，快速又準(zhǔn)確地對多目相機(jī)陣列進(jìn)行位置部署。