基于多目視覺的工程車輛運動學參數(shù)校準系統(tǒng)

朱本龍 李祥坤 黃 歡

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

0 引言

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

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

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

1 參數(shù)校準系統(tǒng)設計

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

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

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

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

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

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

在二值化灰度圖像之后，對標記點邊緣進行提取。

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

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

圖1 一維標定桿多相機成像示意圖

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

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

將公式（3）帶入投影坐標公式（4）。

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

對Aj、Bj和Cj這3個點的重投影進行整體誤差最小化求解，最小化重投影誤差表達式如公式（5）所示。

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

1.3 機械臂末端位置定位

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

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

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

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

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

1.4 誤差參數(shù)辨識

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

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

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

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

測得機械臂工作空間內(nèi)n個點的坐標后，可以將公式（11）轉換成公式（12）。

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

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

2 試驗與結果分析

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

2.1 相機陣列規(guī)劃

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

圖2 相機在鏟斗末端運動平面上的投影圖

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

2.2 鏟斗位置測量

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

2.3 運動學參數(shù)校準

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

表1 無人挖掘機模型機械臂標定前、后控制效果對比

3 結論

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