薛春嶺,張 峰,劉煉雄
(火箭軍工程大學(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ù)。
為保證箭體晃動(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
結(jié)合圖1,建立如圖2所示的雙目視覺測(cè)量原理示意圖,所需的像機(jī)坐標(biāo)系為CXYZ(包括C1XYZ和C2XYZ),世界坐標(biāo)系為WXYZ和箭體坐標(biāo)系為MXYZ?;谝陨夏P停瑢?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)值。
為有效求解初始箭體坐標(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)。
在求解了標(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è)。
由于位姿變化是影響貯存狀態(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
在坐標(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
為進(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ù)的有效性和可靠性。
針對(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è)需求。