馬玉棟，杜福洲，樊洪良，吳 典，陳 婷

（1.北京航空航天大學(xué) 機(jī)械工程及自動(dòng)化學(xué)院，北京 100191；2.滬東中華造船（集團(tuán)）有限公司，上海 200129）

0 引言

隨著航空、航天、船舶等傳統(tǒng)工業(yè)領(lǐng)域在工藝水平上不斷提升，大尺度產(chǎn)品部件吊裝對(duì)接任務(wù)的對(duì)接精度要求逐漸提高，數(shù)字化智能化程度要求也越來(lái)越高[1]?，F(xiàn)階段單一傳感器在大尺度產(chǎn)品部件吊裝對(duì)接過(guò)程難以取得更好的效果，往往需要多傳感器聯(lián)合測(cè)量，通過(guò)對(duì)多組測(cè)量數(shù)據(jù)的調(diào)整、分析和融合，完成對(duì)接任務(wù)。多傳感器集成與數(shù)據(jù)融合技術(shù)已經(jīng)成為測(cè)量輔助裝配領(lǐng)域的重要研究方向，它涉及信號(hào)處理、硬件設(shè)計(jì)、智能算法、機(jī)器視覺(jué)等多個(gè)領(lǐng)域，是新一代智能制造技術(shù)的核心基礎(chǔ)之一。

為實(shí)現(xiàn)大尺度產(chǎn)品部件吊裝對(duì)接實(shí)時(shí)位姿安全監(jiān)控，利用多傳感器數(shù)據(jù)融合技術(shù)，組建了多傳感器聯(lián)合測(cè)量系統(tǒng)-集成測(cè)量單元。在多傳感器系統(tǒng)中，傳感器主要包括單目視覺(jué)測(cè)量系統(tǒng)、九軸MEMS（陀螺儀）以及激光測(cè)距傳感器。工業(yè)相機(jī)是單目視覺(jué)測(cè)量系統(tǒng)中的關(guān)鍵組件，其本質(zhì)的功能就是將光信號(hào)轉(zhuǎn)變成有序的電信號(hào)，利用傳回的圖像來(lái)解算位姿；九軸MEMS集成高精度的角速度計(jì)、加速度計(jì)、磁力計(jì)，求解姿態(tài)；激光測(cè)距傳感器則是利用激光對(duì)目標(biāo)的距離進(jìn)行準(zhǔn)確測(cè)定（又稱(chēng)激光測(cè)距）的傳感器。

多傳感器數(shù)據(jù)融合的關(guān)鍵在于各傳感器之間的所測(cè)數(shù)據(jù)坐標(biāo)系標(biāo)定。起初獨(dú)立傳感器之間并未關(guān)聯(lián)，各個(gè)傳感器所測(cè)數(shù)據(jù)在未標(biāo)定情況下，只在自身定義的坐標(biāo)系下有意義。標(biāo)定的作用就是求得各個(gè)獨(dú)立傳感器所測(cè)數(shù)據(jù)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，只有將多傳感器標(biāo)定于同一個(gè)坐標(biāo)系下，所測(cè)數(shù)據(jù)才能進(jìn)行數(shù)據(jù)融合。早期的Tsai[2～4]等建立手眼約束方程AX=BX來(lái)求解手眼關(guān)系X，確定相機(jī)與機(jī)器人的位姿標(biāo)定關(guān)系。溫卓漫[5]等在基于合作靶標(biāo)的在軌手眼標(biāo)定中提出了一種利用合作靶標(biāo)位姿求解手眼關(guān)系的方法。李醒飛[6]等在多傳感器測(cè)量系統(tǒng)聯(lián)合標(biāo)定中完成了坐標(biāo)測(cè)量機(jī)與單目視覺(jué)系統(tǒng)的坐標(biāo)系標(biāo)定。

本文針對(duì)組建的集成測(cè)量單元，設(shè)計(jì)了利用工業(yè)機(jī)器輔助單目視覺(jué)測(cè)量系統(tǒng)、九軸MEMS以及激光測(cè)距傳感器坐標(biāo)系聯(lián)合標(biāo)定的方法和試驗(yàn)。本方法操作過(guò)程簡(jiǎn)單易實(shí)現(xiàn)，機(jī)器人安裝集成測(cè)量單元運(yùn)動(dòng)任意的幾個(gè)位置（轉(zhuǎn)動(dòng)角度不得超過(guò)90°），即可完成標(biāo)定。

1 監(jiān)測(cè)系統(tǒng)方案設(shè)計(jì)

由單目視覺(jué)、九軸MEMS陀螺以及測(cè)距傳感器組成的集成測(cè)量單元封裝于密閉設(shè)備中，如圖1所示。集成測(cè)量單元還包括電源模塊、數(shù)據(jù)傳輸模塊（RF傳輸）、計(jì)算終端。在大尺度產(chǎn)品部件吊裝對(duì)接過(guò)程中，在工業(yè)相機(jī)視場(chǎng)內(nèi)能捕捉到靶標(biāo)板時(shí)，根據(jù)標(biāo)志點(diǎn)進(jìn)行位姿解算，陀螺與激光測(cè)量數(shù)據(jù)提供校正功能；在靶標(biāo)板不可見(jiàn)時(shí)，僅由陀螺和激光提供4個(gè)自由度的位姿引導(dǎo)，同時(shí)還能監(jiān)測(cè)對(duì)接過(guò)程中部件的速度、加速度等運(yùn)動(dòng)信息。通過(guò)聯(lián)合標(biāo)定參數(shù)與傳感器所測(cè)數(shù)據(jù)，計(jì)算終端進(jìn)行多傳感器數(shù)據(jù)融合后，利用RF無(wú)線(xiàn)模塊將最終融合后的數(shù)據(jù)傳回上位機(jī)進(jìn)行下一步處理。

圖1 多傳感器測(cè)量系統(tǒng)-集成測(cè)量單元

1.1 基于單目視覺(jué)的位姿測(cè)量

單目視覺(jué)利用工業(yè)相機(jī)采集目標(biāo)圖像后，對(duì)觀測(cè)靶標(biāo)進(jìn)行特征識(shí)別處理為特征點(diǎn)，利用多點(diǎn)對(duì)PNP算法計(jì)算相機(jī)外參數(shù)，最終處理為相對(duì)位姿。靶標(biāo)板上標(biāo)志點(diǎn)構(gòu)型方案設(shè)計(jì)多種多樣，只要標(biāo)志點(diǎn)個(gè)數(shù)大于3個(gè)即可構(gòu)建PnP模型，相機(jī)即可解算出標(biāo)志點(diǎn)所在坐標(biāo)系與相機(jī)坐標(biāo)系的相對(duì)位姿，測(cè)量步驟如圖2所示。

單目視覺(jué)位姿測(cè)量主要步驟包括：

1）進(jìn)行相機(jī)內(nèi)參數(shù)的標(biāo)定，采用張正友標(biāo)定法；

2）通過(guò)在世界坐標(biāo)系Ow-xyz指定位置安放多個(gè)（通常放置6個(gè)）靶標(biāo)點(diǎn)，或是任意安放靶標(biāo)點(diǎn)后依靠第三方測(cè)量?jī)x器確定其位置，可以獲得靶標(biāo)點(diǎn)在Ow-xyz下的三維坐標(biāo)；

3）根據(jù)圖像處理方法獲得靶標(biāo)點(diǎn)對(duì)應(yīng)的二維圖像坐標(biāo)，構(gòu)建PnP（Perspective-n-Point）問(wèn)題模型；

4）根據(jù)EPnP（Efficient Perspective-n-Point）算法求解相機(jī)在世界坐標(biāo)系中位姿，即Ai。

1.2 基于九軸MEMS的姿態(tài)測(cè)量

圖2 單目視覺(jué)位姿測(cè)量步驟

九軸MEMS測(cè)量姿態(tài)由電子羅盤(pán)校正，其參考坐標(biāo)系OG-xyz與東北天坐標(biāo)系方向一致；測(cè)量位置通過(guò)加速度對(duì)時(shí)間二次積分獲得[7]，OG-xyz位置由積分起始時(shí)刻傳感器位置決定，即陀螺儀顯示姿態(tài)為陀螺儀本身在東北天坐標(biāo)系姿態(tài)。

1.3 基于激光測(cè)距傳感器的距離測(cè)量

激光測(cè)距傳感器是利用激光對(duì)目標(biāo)的距離進(jìn)行準(zhǔn)確測(cè)定（又稱(chēng)激光測(cè)距）的儀器[8]。激光測(cè)距儀在工作時(shí)向目標(biāo)射出一束很細(xì)的激光，由光電元件接收目標(biāo)反射的激光束，計(jì)時(shí)器測(cè)定激光束從發(fā)射到接收的時(shí)間，計(jì)算出從觀測(cè)者到目標(biāo)的距離。

2 系統(tǒng)聯(lián)合標(biāo)定方法

本文借助工業(yè)機(jī)器人與單目視覺(jué)的手眼標(biāo)定，將多傳感器系統(tǒng)中各傳感器數(shù)據(jù)標(biāo)定于集成測(cè)量單元坐標(biāo)系下。其中單目視覺(jué)構(gòu)建PnP（Perspective-n-Point）問(wèn)題模型求解相機(jī)與靶標(biāo)點(diǎn)的位置與姿態(tài)，九軸MEMS測(cè)量顯示姿態(tài)為陀螺儀本身在東北天坐標(biāo)系姿態(tài)，激光測(cè)距傳感器則是利用激光對(duì)目標(biāo)的距離進(jìn)行準(zhǔn)確測(cè)定（又稱(chēng)激光測(cè)距）的儀器。最終集成測(cè)量單元所需標(biāo)定：1）單目視覺(jué)中相機(jī)自身測(cè)量坐標(biāo)系與集成測(cè)量單元坐標(biāo)系的轉(zhuǎn)換矩陣關(guān)系；2）九軸MEMS自身測(cè)量坐標(biāo)系與集成測(cè)量單元坐標(biāo)系轉(zhuǎn)換矩陣關(guān)系；3）激光測(cè)距傳感器在集成測(cè)量單元坐標(biāo)系下激光發(fā)射點(diǎn)坐標(biāo)以及激光射線(xiàn)方向，即激光射線(xiàn)在集成測(cè)量單元坐標(biāo)系下的空間直線(xiàn)方程。

2.1 單目視覺(jué)與九軸MEMS聯(lián)合標(biāo)定

單目視覺(jué)與九軸MEMS融合測(cè)量模型涉及的各坐標(biāo)系之間關(guān)系如圖3所示。相機(jī)與九軸MEMS固定于集成測(cè)量單元中，測(cè)量單元通過(guò)基準(zhǔn)孔與被測(cè)物固連（轉(zhuǎn)換矩陣TCD已知），則被測(cè)物實(shí)時(shí)位姿可以由集成測(cè)量單元獲得。設(shè)世界坐標(biāo)系為Ow-xyz，相機(jī)測(cè)量靶標(biāo)點(diǎn)在Ow-xyz下坐標(biāo)已知，則由相機(jī)獲得的被測(cè)物在世界坐標(biāo)系下位姿為：

圖3 融合測(cè)量模型示意圖1

考慮集成測(cè)量單元具有多種工作模式，最終測(cè)量結(jié)果如下，其中f(Pa，Pb)表示利用數(shù)據(jù)融合算法。

上述測(cè)量結(jié)果Pa，Pb包含未知參數(shù)X,Y,TGW，需要利用機(jī)器人末端執(zhí)行器作為被測(cè)物進(jìn)行標(biāo)定，如圖3所示。根據(jù)機(jī)器人末端執(zhí)行器與測(cè)量單元轉(zhuǎn)換矩陣TCD已知，將被測(cè)物位姿變動(dòng)D1→D2轉(zhuǎn)換為測(cè)量單元在機(jī)器人基準(zhǔn)坐標(biāo)系OB-xyz下的位姿變動(dòng)C1→C2，再由剛體位姿變換關(guān)系易得：

其中：

由手眼標(biāo)定算法可知，當(dāng)機(jī)器人運(yùn)動(dòng)到k（k＞3）個(gè)不同位置，即式(4)取k-1組不同值時(shí)，對(duì)于X，可以通過(guò)先求解最小二乘擬合問(wèn)題：

得到旋轉(zhuǎn)矩陣RX，其中α，β分別表示A，B對(duì)應(yīng)的旋轉(zhuǎn)向量[9,10]，再通過(guò)求解：

得到平移矩陣TX，從而求解出：

同理可求解Y,TGW。即求得單目視覺(jué)系統(tǒng)和九軸MEMS各自坐標(biāo)系與集成測(cè)量單元坐標(biāo)系的關(guān)系。

2.2 單目視覺(jué)與激光傳感器聯(lián)合標(biāo)定

單目視覺(jué)與激光傳感器融合測(cè)量模型涉及的各坐標(biāo)系之間關(guān)系如圖4所示。相機(jī)與激光測(cè)距傳感器固定于集成測(cè)量單元中，測(cè)量單元通過(guò)基準(zhǔn)孔與被測(cè)物固連（轉(zhuǎn)換矩陣TCD已知），則被測(cè)物實(shí)時(shí)位姿可以由集成測(cè)量單元獲得。設(shè)世界坐標(biāo)系（靶標(biāo)點(diǎn)所在地面）為O3-xyz，相機(jī)測(cè)量靶標(biāo)點(diǎn)在O3-xyz下坐標(biāo)已知，則由相機(jī)獲得的被測(cè)物在世界坐標(biāo)系下位姿為：

圖4 融合測(cè)量模型示意圖2

由單目視覺(jué)與九軸MEMS融合測(cè)量已經(jīng)得到X矩陣，合作靶標(biāo)點(diǎn)坐標(biāo)系O3在集成測(cè)量單元坐標(biāo)系下位姿轉(zhuǎn)換矩陣已知，即Pa為已知條件；合作靶標(biāo)點(diǎn)坐標(biāo)系下取三個(gè)點(diǎn)m,n,p，其坐標(biāo)分別為(1,0,0)(0,1,0)(0,0,1)，由平面方程關(guān)系可知，合作靶標(biāo)點(diǎn)坐標(biāo)系下三點(diǎn)可以確定一個(gè)平面方程；同理集成測(cè)量單元坐標(biāo)系下對(duì)應(yīng)三個(gè)點(diǎn)也可以確定一個(gè)平面方程，集成測(cè)量單元坐標(biāo)系下對(duì)應(yīng)合作靶標(biāo)點(diǎn)坐標(biāo)系三個(gè)點(diǎn)m1,n1,p1坐標(biāo)值如下：

則集成測(cè)量單元坐標(biāo)系下三個(gè)點(diǎn)m1,n1,p1即可確定一個(gè)平面方程：

在集成測(cè)量單元坐標(biāo)系下激光所測(cè)距離，存在激光起點(diǎn)（x,y,z）以及激光終點(diǎn)（x1,y1,z1）則存在公式（其中D為激光測(cè)距示數(shù)）：

激光直線(xiàn)在集成測(cè)量單元坐標(biāo)系下存在空間直線(xiàn)方程：

由式(8)～式(12)五個(gè)式子，通過(guò)至少6組參數(shù)即可消掉多余參數(shù)[11]，解得激光線(xiàn)的空間向量，然后即可求得激光射線(xiàn)在集成測(cè)量單元坐標(biāo)系下的空間向量，即求得我們所需求的激光射線(xiàn)起點(diǎn)在集成測(cè)量單元坐標(biāo)系下坐標(biāo)，以及射線(xiàn)方向。

2.3 總結(jié)

由單目視覺(jué)與九軸MEMS的聯(lián)合標(biāo)定以及單目視覺(jué)與激光測(cè)距傳感器的聯(lián)合標(biāo)定，可以將單目視覺(jué)測(cè)量、九軸MEMS陀螺儀、激光測(cè)距所測(cè)到的姿態(tài)、距離、位置標(biāo)定于集成測(cè)量單元統(tǒng)一坐標(biāo)系中。

3 試驗(yàn)與數(shù)據(jù)分析

3.1 試驗(yàn)環(huán)境搭建

多傳感器測(cè)量系統(tǒng)聯(lián)合標(biāo)定試驗(yàn)由庫(kù)卡機(jī)器人、集成測(cè)量單元以及合作靶標(biāo)點(diǎn)構(gòu)成，如圖6所示。

試驗(yàn)使用庫(kù)卡機(jī)器人的重復(fù)定位精度達(dá)到±0.03mm，集成測(cè)量單元包括單目視覺(jué)測(cè)量系統(tǒng)、九軸MEMS陀螺儀以及激光測(cè)距傳感器等傳感器，合作靶標(biāo)點(diǎn)粘貼與長(zhǎng)200mm，寬130mm的鋁板上，如圖5所示，利用六個(gè)靶標(biāo)點(diǎn)構(gòu)建PnP（Perspective-n-Point）問(wèn)題模型，根據(jù)EPnP（Efficient Perspective-n-Point）算法求解相機(jī)位姿，靶標(biāo)點(diǎn)坐標(biāo)分別為（0，-33，0）（-66，-29，0）（-66，33，0）（0，33，0）（66，33，0）（66，-29，0），單位：mm。

單目視覺(jué)測(cè)量通過(guò)工業(yè)相機(jī)獲得合作靶標(biāo)圖像，進(jìn)行圖像處理獲得所需位姿，九軸MEMS和激光測(cè)距傳感器也通過(guò)相應(yīng)的程序獲得所需數(shù)據(jù)，機(jī)器人位姿則由機(jī)器人PAD獲得所需的機(jī)械臂末端位姿。

圖5 合作靶標(biāo)點(diǎn)

3.2 試驗(yàn)流程

圖6 試驗(yàn)場(chǎng)景

將多傳感器測(cè)量系統(tǒng)的設(shè)備安裝于庫(kù)卡機(jī)器人機(jī)械臂末端，設(shè)備本身已經(jīng)完成多傳感器系統(tǒng)的集成，已具備工業(yè)相機(jī)抓取圖像求解位姿、通過(guò)串口獲取九軸MEMS和激光測(cè)距傳感器數(shù)據(jù)的功能。

試驗(yàn)主要步驟包括：

1）通過(guò)機(jī)器人PAD控制機(jī)器人機(jī)械臂運(yùn)動(dòng)到某位置，記錄機(jī)器人機(jī)械臂末端位姿，待相機(jī)位姿測(cè)量、陀螺儀位姿、激光數(shù)據(jù)穩(wěn)定后，記錄各自數(shù)據(jù)；

2）控制機(jī)械臂繼續(xù)運(yùn)動(dòng)，但運(yùn)動(dòng)轉(zhuǎn)動(dòng)角度不能超過(guò)90°，同步驟1）記錄機(jī)器人機(jī)械臂末端位姿，待相機(jī)位姿測(cè)量、陀螺儀位姿、激光數(shù)據(jù)穩(wěn)定后，記錄各自數(shù)據(jù)；

3）重復(fù)步驟2），記錄9組數(shù)據(jù)，完成試驗(yàn)數(shù)據(jù)記錄。

3.3 試驗(yàn)結(jié)果與數(shù)據(jù)分析

在大尺度產(chǎn)品部件吊裝對(duì)接過(guò)程中，利用集成測(cè)量單元實(shí)時(shí)監(jiān)控位姿，其中單目視覺(jué)系統(tǒng)與九軸MEMS負(fù)責(zé)對(duì)接過(guò)程中的最終位姿調(diào)整，精度要求較高，激光測(cè)距傳感器負(fù)責(zé)吊裝過(guò)程中未達(dá)到相機(jī)測(cè)量范圍時(shí)的引導(dǎo)過(guò)程，精度要求較低。

單目視覺(jué)系統(tǒng)與九軸MEMS聯(lián)合標(biāo)定過(guò)程主要運(yùn)用手眼標(biāo)定原理，手眼標(biāo)定問(wèn)題其實(shí)就是求解AX=XB方程問(wèn)題。其中A為機(jī)器人機(jī)械臂末端坐標(biāo)系在機(jī)器人-單目視覺(jué)系統(tǒng)移動(dòng)前后的轉(zhuǎn)換關(guān)系，B為單目視覺(jué)系統(tǒng)坐標(biāo)系在移動(dòng)前后的相對(duì)關(guān)系。Tsai[2]指出要唯一確定手眼矩陣的各分量，至少需要旋轉(zhuǎn)軸不平行的兩組運(yùn)動(dòng)。由于在觀測(cè)中一般存在噪聲，因此在實(shí)際測(cè)量中一般需要多組運(yùn)動(dòng)來(lái)求解該方程。通過(guò)多次手眼關(guān)系矩陣求解并利用最小二乘法擬合得到最后手眼標(biāo)定結(jié)果來(lái)降低誤差和不確定性。

表1 基于單目視覺(jué)的位姿測(cè)量標(biāo)定精度

表2 基于九軸MEMS的姿態(tài)測(cè)量標(biāo)定精度

本文中采用將獲得的手眼關(guān)系矩陣X代入傳統(tǒng)問(wèn)題方程AX=XB中，選取一組矩陣關(guān)系為代表，通過(guò)比較AX和XB的值來(lái)驗(yàn)證混合標(biāo)定的精度[12]。其中相機(jī)手眼關(guān)系矩陣包含旋轉(zhuǎn)矩陣和平移矩陣，九軸MEMS則只存在旋轉(zhuǎn)矩陣。

由表1可知相機(jī)標(biāo)定到測(cè)量單元坐標(biāo)系下旋轉(zhuǎn)矩陣部分誤差最大為0.006，平移矩陣部分誤差最大為-1.4377；由表2可知九軸MEMS標(biāo)定到集成測(cè)量單元坐標(biāo)系下旋轉(zhuǎn)矩陣部分誤差最大為0.1558。通過(guò)討論驗(yàn)證，單目視覺(jué)與九軸MEMS聯(lián)合標(biāo)定具有足夠的精度與特性，滿(mǎn)足項(xiàng)目任務(wù)要求。

激光射線(xiàn)在集成測(cè)量單元坐標(biāo)系下的標(biāo)定是在相機(jī)標(biāo)定輔助下完成的，通過(guò)數(shù)學(xué)公式計(jì)算得出激光射線(xiàn)在測(cè)量單元坐標(biāo)系下的空間射線(xiàn)方程，再通過(guò)多組直線(xiàn)方程消去多余參數(shù)求解空間射線(xiàn)方程。激光測(cè)距傳感器只在吊裝中間過(guò)程起引導(dǎo)作用，無(wú)具體精度要求，精度要求較低，在相機(jī)標(biāo)定誤差和激光傳感器誤差滿(mǎn)足精度要求的情況下，計(jì)算而得激光射線(xiàn)起點(diǎn)在集成測(cè)量單元坐標(biāo)系下坐標(biāo)，以及射線(xiàn)方向也滿(mǎn)足項(xiàng)目精度要求。

4 結(jié)束語(yǔ)

為實(shí)現(xiàn)大尺度產(chǎn)品部件吊裝對(duì)接實(shí)時(shí)位姿安全監(jiān)控，利用多傳感器數(shù)據(jù)融合技術(shù)，組建了多傳感器聯(lián)合測(cè)量系統(tǒng)-集成測(cè)量單元。本文針對(duì)多傳感器聯(lián)合測(cè)量系統(tǒng)設(shè)計(jì)了單目視覺(jué)系統(tǒng)、九軸MEMS陀螺儀、激光測(cè)距傳感器聯(lián)合標(biāo)定方法。通過(guò)該方法將單目視覺(jué)系統(tǒng)和九軸MEMS陀螺儀標(biāo)定于集成測(cè)量單元坐標(biāo)系中，同時(shí)將激光測(cè)距傳感器的激光射線(xiàn)空間方程標(biāo)定于集成測(cè)量單元坐標(biāo)系中，最終將多傳感器測(cè)量系統(tǒng)歸于統(tǒng)一坐標(biāo)系中。同時(shí)對(duì)所設(shè)計(jì)的標(biāo)定方法與流程做了實(shí)際試驗(yàn)，通過(guò)對(duì)試驗(yàn)數(shù)據(jù)的分析與論證，證明多傳感器測(cè)量系統(tǒng)的聯(lián)合標(biāo)定方法滿(mǎn)足項(xiàng)目任務(wù)的精度與特性要求，達(dá)到了預(yù)期的效果。