基于多傳感融合的吊裝過程監(jiān)測系統(tǒng)聯(lián)合標定方法研究

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

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

0 引言

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

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

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

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

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

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

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

1.1 基于單目視覺的位姿測量

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

單目視覺位姿測量主要步驟包括：

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

2）通過在世界坐標系Ow-xyz指定位置安放多個（通常放置6個）靶標點，或是任意安放靶標點后依靠第三方測量儀器確定其位置，可以獲得靶標點在Ow-xyz下的三維坐標；

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

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

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

圖2 單目視覺位姿測量步驟

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

1.3 基于激光測距傳感器的距離測量

激光測距傳感器是利用激光對目標的距離進行準確測定（又稱激光測距）的儀器[8]。激光測距儀在工作時向目標射出一束很細的激光，由光電元件接收目標反射的激光束，計時器測定激光束從發(fā)射到接收的時間，計算出從觀測者到目標的距離。

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

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

2.1 單目視覺與九軸MEMS聯(lián)合標定

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

圖3 融合測量模型示意圖1

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

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

其中：

由手眼標定算法可知，當(dāng)機器人運動到k（k＞3）個不同位置，即式(4)取k-1組不同值時，對于X，可以通過先求解最小二乘擬合問題：

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

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

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

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

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

圖4 融合測量模型示意圖2

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

則集成測量單元坐標系下三個點m1,n1,p1即可確定一個平面方程：

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

激光直線在集成測量單元坐標系下存在空間直線方程：

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

2.3 總結(jié)

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

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

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

多傳感器測量系統(tǒng)聯(lián)合標定試驗由庫卡機器人、集成測量單元以及合作靶標點構(gòu)成，如圖6所示。

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

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

圖5 合作靶標點

3.2 試驗流程

圖6 試驗場景

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

試驗主要步驟包括：

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

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

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

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

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

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

表1 基于單目視覺的位姿測量標定精度

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

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

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

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

4 結(jié)束語

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