薛春嶺，張 峰，劉煉雄

（火箭軍工程大學(xué)，陜西 西安 710025）

引言

對(duì)長(zhǎng)時(shí)間處于發(fā)射臺(tái)上的液體運(yùn)載火箭、支撐箭體的支架、發(fā)射臺(tái)鋼絲軸承、液壓系統(tǒng)等出現(xiàn)塑性變形或故障，可能會(huì)引起火箭箭體晃動(dòng)或傾斜[1]。當(dāng)晃動(dòng)或傾斜幅度超出一定范圍會(huì)使箭體元件受損，不斷地累積就會(huì)造成箭體傾斜或倒塌等嚴(yán)重后果。因此，必須實(shí)時(shí)監(jiān)測(cè)火箭箭體的晃動(dòng)量。通常的方法是，在設(shè)備內(nèi)部安裝陀螺儀傳感器、3 軸加速度傳感器或壓力傳感器進(jìn)行結(jié)構(gòu)件晃動(dòng)監(jiān)測(cè)，并以晃動(dòng)量監(jiān)測(cè)值來(lái)消除誤差影響[2]。

設(shè)備晃動(dòng)可能給一些核心元件帶來(lái)潛在危險(xiǎn)，例如當(dāng)航天設(shè)備中的燃料存儲(chǔ)箱發(fā)生晃動(dòng)時(shí)，會(huì)導(dǎo)致燃料泄漏，甚至?xí)蛉剂闲孤┒l(fā)生爆炸。對(duì)此，張小虎和LIU 等人以火箭的靶場(chǎng)實(shí)驗(yàn)測(cè)量為背景，利用攝影測(cè)量技術(shù)的非接觸特點(diǎn)，通過獲得人工標(biāo)識(shí)點(diǎn)坐標(biāo)及實(shí)時(shí)位姿變換參數(shù)的方法，對(duì)火箭整體實(shí)時(shí)晃動(dòng)進(jìn)行在線監(jiān)測(cè)，同時(shí)利用獲取的晃動(dòng)數(shù)值減小測(cè)量誤差[3-4]。這些研究方式盡管能夠?qū)崿F(xiàn)對(duì)箭體晃動(dòng)實(shí)時(shí)監(jiān)測(cè)，但由于所采用的技術(shù)手段仍存在特征點(diǎn)提取、坐標(biāo)值求解精度不高及姿態(tài)參數(shù)解算效率較低等不足，一定程度上影響了晃動(dòng)測(cè)量的精度。

在實(shí)際的位姿參數(shù)求解過程中，求取位姿變換參數(shù)需通過多個(gè)點(diǎn)的約束進(jìn)行有效求解。正如文獻(xiàn)[5]中指出，求解體目標(biāo)的三維位置姿態(tài)參數(shù)的問題可轉(zhuǎn)化為利用多個(gè)標(biāo)識(shí)點(diǎn)求參數(shù)的PNP 問題，因而為了求有效解，需對(duì)PNP 問題分析討論。依據(jù)有關(guān)求解PNP 問題及雙目交會(huì)測(cè)量的相關(guān)研究[6-12]，在求解位姿變換參數(shù)的問題上，為獲取線性唯一解可由P6P 問題簡(jiǎn)化為P3P 問題，利用3 個(gè)以上標(biāo)識(shí)點(diǎn)即可完成對(duì)相關(guān)參數(shù)的直接線性求解?；诖?，該文采用雙目視覺測(cè)量方法構(gòu)建一種任意2 個(gè)標(biāo)識(shí)點(diǎn)間空間距離相等的約束關(guān)系，對(duì)所建立的標(biāo)識(shí)點(diǎn)坐標(biāo)系位姿變換參數(shù)進(jìn)行了高精度求解，結(jié)合箭體坐標(biāo)系與標(biāo)識(shí)點(diǎn)坐標(biāo)系間的幾何約束，依據(jù)空間物體位姿轉(zhuǎn)換參數(shù)獲取了結(jié)構(gòu)件晃動(dòng)的在線監(jiān)測(cè)數(shù)據(jù)。

1 火箭箭體晃動(dòng)在線監(jiān)測(cè)方法

為保證箭體晃動(dòng)測(cè)量結(jié)果的可靠性和準(zhǔn)確性，本文利用2 臺(tái)單目像機(jī)組成雙目視覺測(cè)量系統(tǒng)和3 個(gè)編碼標(biāo)識(shí)點(diǎn)實(shí)現(xiàn)對(duì)標(biāo)識(shí)點(diǎn)坐標(biāo)的在線測(cè)量，并依據(jù)標(biāo)識(shí)點(diǎn)的坐標(biāo)變化求解相應(yīng)位姿變換的參數(shù)，進(jìn)而間接表征結(jié)構(gòu)件發(fā)生的晃動(dòng)特征量。圖1為晃動(dòng)測(cè)量示意圖。

圖1 晃動(dòng)測(cè)量示意圖Fig.1 Schematic diagram of sloshing measurement

1.1 標(biāo)識(shí)點(diǎn)空間坐標(biāo)的實(shí)時(shí)測(cè)量

結(jié)合圖1，建立如圖2所示的雙目視覺測(cè)量原理示意圖，所需的像機(jī)坐標(biāo)系為CXYZ（包括C1XYZ和C2XYZ），世界坐標(biāo)系為WXYZ和箭體坐標(biāo)系為MXYZ?；谝陨夏Ｐ停瑢?duì)箭體結(jié)構(gòu)的晃動(dòng)量監(jiān)測(cè)即可轉(zhuǎn)換為初始箭體坐標(biāo)系M0XYZ與實(shí)時(shí)箭體坐標(biāo)系MtXYZ間的位姿參數(shù)求解。

圖2 雙目視覺測(cè)量原理示意圖Fig.2 Schematic diagram of binocular vision measurement

用Xm、Xn表示空間某點(diǎn)在m、n坐標(biāo)系下對(duì)應(yīng)的坐標(biāo)向量，則2 個(gè)坐標(biāo)系之間轉(zhuǎn)換關(guān)系為

式中：Rmn、Tmn分別表示旋轉(zhuǎn)矩陣和平移向量。

結(jié)合圖2，可得如下轉(zhuǎn)換關(guān)系。

像機(jī)坐標(biāo)系CXYZ與世界坐標(biāo)系WXYZ轉(zhuǎn)換關(guān)系為

世界坐標(biāo)系WXYZ與箭體坐標(biāo)系MXYZ轉(zhuǎn)換關(guān)系為

像機(jī)坐標(biāo)系CXYZ與箭體坐標(biāo)系MXYZ轉(zhuǎn)換關(guān)系為

同時(shí)，通過(2)式和(3)式可得像機(jī)坐標(biāo)系CXYZ和箭體坐標(biāo)系MXYZ轉(zhuǎn)換關(guān)系為

由于(4)式和(5)式是等價(jià)的，比較得到

由于雙目像機(jī)固定，因而像機(jī)坐標(biāo)系與世界坐標(biāo)系相對(duì)位置不變，即RCW和TCW不隨時(shí)間改變；但箭體坐標(biāo)系與世界坐標(biāo)系將隨著箭體晃動(dòng)發(fā)生相對(duì)改變，即RWM和TWM與時(shí)間有關(guān)。故像機(jī)、初始箭體坐標(biāo)系間的位姿變換參數(shù)RCM0、TCM0可表示為

當(dāng)標(biāo)識(shí)點(diǎn)P的世界坐標(biāo)(XW,YW,ZW)隨著箭體晃動(dòng)發(fā)生變化時(shí)，其在初始箭體坐標(biāo)系下的坐標(biāo)(XM0,t,YM0,t,ZM0,t)也必將發(fā)生變化。雙目視覺測(cè)量系統(tǒng)能夠同時(shí)獲取2 組關(guān)于標(biāo)識(shí)點(diǎn)坐標(biāo)的方程，克服了單目像機(jī)無(wú)法求解3 個(gè)未知數(shù)的缺點(diǎn)，因而可有效完成對(duì)初始箭體坐標(biāo)的實(shí)時(shí)測(cè)量。當(dāng)獲取了標(biāo)識(shí)點(diǎn)P在2 臺(tái)測(cè)量像機(jī)中對(duì)應(yīng)像點(diǎn)p01(x1,y1)和p02(x2,y2)時(shí)，依據(jù)共線方程可得到如下關(guān)系式：

(9)式中包含4 個(gè)方程和3 個(gè)未知數(shù)(XM0,t,YM0,t,ZM0,t)，故根據(jù)事先標(biāo)定好的像機(jī)參數(shù)K1、K2、RC1M0、TC1M0、RC2M0和TC2M0即可對(duì)標(biāo)識(shí)點(diǎn)的空間坐標(biāo)(XM0,t,YM0,t,ZM0,t)進(jìn)行實(shí)時(shí)測(cè)量。由于上述線性求解結(jié)果易受噪聲和初始誤差的影響，因此在獲取標(biāo)識(shí)點(diǎn)坐標(biāo)的初始值后，需對(duì)其進(jìn)行誤差修正。采用文獻(xiàn)[13]的約束條件，以修正后標(biāo)識(shí)點(diǎn)坐標(biāo)與實(shí)際標(biāo)識(shí)點(diǎn)坐標(biāo)的距離平方和最小為目標(biāo)函數(shù)，優(yōu)化求 解出精度可靠的坐標(biāo)值。

1.2 箭體坐標(biāo)系位姿變換參數(shù)實(shí)時(shí)測(cè)量

為有效求解初始箭體坐標(biāo)系M0XYZ與實(shí)時(shí)箭體坐標(biāo)系MtXYZ間的位姿變換參數(shù)RM0,t、TM0,t，可將其直接轉(zhuǎn)換為求解不同時(shí)刻下標(biāo)識(shí)點(diǎn)坐標(biāo)在初始箭體坐標(biāo)下發(fā)生的位姿變換：

在實(shí)時(shí)獲取坐標(biāo)值(XM0,t,YM0,t,ZM0,t)、(XM0,YM0,ZM0)后，可依據(jù)(10)式求解不同時(shí)刻箭體坐標(biāo)系的位姿變換參數(shù)RM0,t、TM0,t，進(jìn)而求得箭體晃動(dòng)量。

由于直接求解的參數(shù)結(jié)果容易受到擾動(dòng)因素的干擾，因此在求解過程中對(duì)標(biāo)識(shí)點(diǎn)設(shè)置了任意2 個(gè)標(biāo)識(shí)點(diǎn)間空間距離相等的約束條件。在獲得線性求解的參數(shù)初值后，按照約束條件對(duì)位姿變換參數(shù)進(jìn)行非線性優(yōu)化，以保證參數(shù)精度的可靠性和穩(wěn)定性。其目標(biāo)優(yōu)化函數(shù)為

式中：Pi、Pj為像機(jī)實(shí)時(shí)測(cè)量得到的標(biāo)識(shí)點(diǎn)坐標(biāo)；為利用初值坐標(biāo)和求解出的位姿變換參數(shù)進(jìn)行求解的標(biāo)識(shí)點(diǎn)坐標(biāo)。

2 位姿變換參數(shù)求解流程

在求解了標(biāo)識(shí)點(diǎn)初始箭體坐標(biāo)系中的實(shí)時(shí)坐標(biāo)后，可由其坐標(biāo)變化求出相應(yīng)的位姿變換參數(shù)，具體求解流程如下。

Step 1:建立箭體坐標(biāo)系、世界坐標(biāo)系和2 個(gè)單目像機(jī)坐標(biāo)系，并對(duì)雙目像機(jī)內(nèi)外參數(shù)進(jìn)行初始標(biāo)定；

Step 2:利用全站儀等設(shè)備對(duì)編碼標(biāo)識(shí)點(diǎn)進(jìn)行位姿測(cè)量，記為初始坐標(biāo)(XM0,YM0,ZM0)；

Step 3:利用雙目像機(jī)獲取標(biāo)識(shí)點(diǎn)的實(shí)時(shí)圖像，結(jié)合相應(yīng)畸變校正法[14]對(duì)圖像畸變進(jìn)行在線校正，并運(yùn)用(9)式對(duì)標(biāo)識(shí)點(diǎn)進(jìn)行實(shí)時(shí)測(cè)量，記為實(shí)時(shí)坐標(biāo)(XM0,t,YM0,t,ZM0,t)；

Step 4:根據(jù)步驟2、3 獲得的標(biāo)識(shí)點(diǎn)初始坐標(biāo)值和實(shí)時(shí)坐標(biāo)值，求位姿變換參數(shù)初始值。

Step 5:利用非線性優(yōu)化方法求位姿變換參數(shù)RM0,t、TM0,t，據(jù)此求解箭體晃動(dòng)值，完成對(duì)箭體晃動(dòng) 量的在線監(jiān)測(cè)。

3 實(shí)驗(yàn)驗(yàn)證

由于位姿變化是影響貯存狀態(tài)箭體晃動(dòng)的主要因素，而本文在簡(jiǎn)化其他影響因素的基礎(chǔ)上，對(duì)箭體晃動(dòng)的測(cè)試實(shí)驗(yàn)利用3 軸旋轉(zhuǎn)臺(tái)驗(yàn)證了位姿變換的影響。實(shí)驗(yàn)設(shè)備主要包括3 軸旋轉(zhuǎn)臺(tái)、微視MVC14KSAC-GE6 像機(jī)、C3516-M 鏡頭、自研箭體模擬件和測(cè)量?jī)x，相關(guān)設(shè)備和模擬件如圖3所示。

圖3 測(cè)量實(shí)驗(yàn)中相關(guān)設(shè)備和模擬件Fig.3 Relevant equipment and simulators in measurement experiment

在OpenCV2.4.9 環(huán)境下，像機(jī)采樣頻率遠(yuǎn)高于箭體晃動(dòng)頻率，并對(duì)雙目像機(jī)坐標(biāo)系與世界坐標(biāo)系位置進(jìn)行標(biāo)定。相關(guān)實(shí)驗(yàn)數(shù)據(jù)的對(duì)比分析結(jié)果和參數(shù)標(biāo)定如圖4所示。

圖4 雙目像機(jī)標(biāo)定實(shí)驗(yàn)Fig.4 Calibration experiment of binocular camera

3.1 坐標(biāo)測(cè)量精度驗(yàn)證實(shí)驗(yàn)

在坐標(biāo)測(cè)量精度驗(yàn)證實(shí)驗(yàn)中，分別將3 個(gè)標(biāo)識(shí)點(diǎn)等間距粘貼于標(biāo)識(shí)點(diǎn)坐標(biāo)系（與箭體坐標(biāo)系重合）的3 個(gè)坐標(biāo)軸上。首先，在離線情況下完成對(duì)2 臺(tái)像機(jī)的高精度標(biāo)定，并獲取其內(nèi)外參數(shù)，具體標(biāo)定實(shí)驗(yàn)見圖4所示；然后，利用雙目交會(huì)原理從不同位置對(duì)標(biāo)識(shí)點(diǎn)坐標(biāo)進(jìn)行測(cè)量，并將其與標(biāo)準(zhǔn)坐標(biāo)值（由全站儀等輔助設(shè)備測(cè)量得到的高精度坐標(biāo)）進(jìn)行多組對(duì)比實(shí)驗(yàn)。在標(biāo)準(zhǔn)坐標(biāo)下，標(biāo)識(shí)點(diǎn)1 坐標(biāo)為（100.00,0.00,0.00）、點(diǎn)2 坐標(biāo)為（0.00,100.00,0.00）、點(diǎn)3 坐標(biāo)為（0.00,0.00,100.00），單位為mm，如圖5所示。

圖5 標(biāo)識(shí)點(diǎn)位置Fig.5 Location of marked points

為方便實(shí)驗(yàn)分析處理，在獲取了標(biāo)識(shí)點(diǎn)空間坐標(biāo)后，將其空間坐標(biāo)轉(zhuǎn)換至標(biāo)識(shí)點(diǎn)坐標(biāo)系中，其中每組實(shí)驗(yàn)所獲取的標(biāo)識(shí)點(diǎn)坐標(biāo)如表1所示，進(jìn)行數(shù)據(jù)處理后的測(cè)量偏差如圖6所示。由圖6 可知，在10 次不同測(cè)量實(shí)驗(yàn)中，單個(gè)標(biāo)識(shí)點(diǎn)測(cè)量誤差最大值不超0.026 mm、坐標(biāo)總偏差不超過0.079 mm，偏差平均值為0.063 mm。從實(shí)驗(yàn)結(jié)果可以看出，文中利用的雙目視覺測(cè)量方法求解的標(biāo)識(shí)點(diǎn)坐標(biāo)精度較高，能夠滿足實(shí)際測(cè)量和后續(xù)位姿參數(shù)解算的精度要求。

表1 標(biāo)識(shí)點(diǎn)坐標(biāo)測(cè)量結(jié)果Table 1 Coordinates measurement results of marked points 單位為mm

圖6 標(biāo)識(shí)點(diǎn)坐標(biāo)測(cè)量誤差Fig.6 Coordinates measurement error of marked points

3.2 位姿變換精度驗(yàn)證實(shí)驗(yàn)

為進(jìn)一步驗(yàn)證所提方法求解位姿變換參數(shù)的精度，在前面得到標(biāo)識(shí)點(diǎn)坐標(biāo)的基礎(chǔ)上，利用3 軸旋轉(zhuǎn)臺(tái)，設(shè)計(jì)了分步位姿變換的實(shí)驗(yàn)方案。首先，利用位姿測(cè)量方法對(duì)標(biāo)識(shí)點(diǎn)坐標(biāo)系、世界坐標(biāo)系和旋轉(zhuǎn)臺(tái)坐標(biāo)系之間的位姿參數(shù)進(jìn)行標(biāo)定；然后，通過控制旋轉(zhuǎn)臺(tái)的實(shí)時(shí)輸出量，對(duì)標(biāo)識(shí)點(diǎn)坐標(biāo)系的實(shí)時(shí)位姿參數(shù)（3 個(gè)姿態(tài)角Ax、Ay、Az和3 個(gè)平移量Tx、Ty、Tz）進(jìn)行轉(zhuǎn)換求解，以獲取實(shí)驗(yàn)測(cè)量的理論參考數(shù)據(jù)；同時(shí)，利用雙目視覺測(cè)量系統(tǒng)對(duì)標(biāo)識(shí)點(diǎn)坐標(biāo)進(jìn)行實(shí)時(shí)測(cè)量，并解算各時(shí)刻間的位姿變換參數(shù)；最后，通過標(biāo)準(zhǔn)參考數(shù)據(jù)（控制旋轉(zhuǎn)臺(tái)輸出的角度α0、β0、γ0及利用幾何變換求解的平移量Tx0、Ty0、Tz0）與測(cè)量數(shù)據(jù)的對(duì)比來(lái)驗(yàn)證方法的有效性。

圖7 為實(shí)驗(yàn)所測(cè)量的旋轉(zhuǎn)角參數(shù)和平移參數(shù)，藍(lán)線為測(cè)量值，紅線為真實(shí)值，滾動(dòng)角、俯仰角和偏航角依次按照線性增至10°，加載區(qū)間分別為0～50 s、50 s～100 s 和100 s～150 s。數(shù)據(jù)分析結(jié)果如表2所示。由表2 可以看出，利用本文方法所得標(biāo)識(shí)點(diǎn)坐標(biāo)系的實(shí)時(shí)位姿參數(shù)(3 個(gè)姿態(tài)角和3 個(gè)平移量)，Y軸俯仰角誤差較大，最大誤差為1.27°；Z軸平移量誤差較大，最大誤差為8.14 mm。

圖7 位姿變換參數(shù)測(cè)量精度實(shí)驗(yàn)Fig.7 Measurement accuracy experiment of pose transformation parameters

表2 位姿參數(shù)解算誤差分析Table 2 Error analysis of pose parameter calculation

由表2 可知，測(cè)量結(jié)果的平移量平均誤差不超過6.93 mm、旋轉(zhuǎn)角度平均誤差不超過0.54°，與文獻(xiàn)[15]～[16]中方法的求解精度相當(dāng)，有效證明了本文方法求解位姿變換參數(shù)的有效性和可靠性。

4 結(jié)論

針對(duì)火箭整體結(jié)構(gòu)早期晃動(dòng)量的在線監(jiān)測(cè)，提出了一種基于雙目像機(jī)實(shí)時(shí)位姿參數(shù)測(cè)量的方法。在求解高精度實(shí)時(shí)坐標(biāo)中，引入了任意2 個(gè)標(biāo)識(shí)點(diǎn)間空間距離相等的約束條件，進(jìn)一步優(yōu)化求解了箭體坐標(biāo)系下的實(shí)時(shí)坐標(biāo)值，保證了參數(shù)精度的可靠性和穩(wěn)定性。根據(jù)不同時(shí)刻間箭體坐標(biāo)系的位姿變換參數(shù)，實(shí)現(xiàn)了對(duì)箭體結(jié)構(gòu)件晃動(dòng)量的在線監(jiān)測(cè)。從實(shí)驗(yàn)驗(yàn)證結(jié)果看，標(biāo)識(shí)點(diǎn)實(shí)時(shí)坐標(biāo)的求解精度滿足了火箭箭體結(jié)構(gòu)件晃動(dòng)量的監(jiān)測(cè)需求。