艙段自動裝配位姿求解方法研究

金賀榮 劉 達 于 斌 范秀斌1.燕山大學河北省并聯(lián)機器人與機電系統(tǒng)實驗室，秦皇島，0660042.燕山大學先進鍛壓成型技術(shù)與科學教育部重點實驗室，秦皇島，066004.北京星航機電裝備有限公司，北京，100074

艙段自動裝配位姿求解方法研究

金賀榮1,2劉 達1,2于 斌1,2范秀斌3
1.燕山大學河北省并聯(lián)機器人與機電系統(tǒng)實驗室，秦皇島，0660042.燕山大學先進鍛壓成型技術(shù)與科學教育部重點實驗室，秦皇島，0660043.北京星航機電裝備有限公司，北京，100074

在航天器自動化總裝過程中需首先確定航天器艙段在裝配空間內(nèi)的準確位姿，為實現(xiàn)艙段位姿的快速精準求解，以圓柱體類航天器艙段結(jié)構(gòu)為例，提出了一種矢量位姿求解法。通過在艙段的圓柱面上合理布設(shè)特征點，利用阻尼最小二乘法求解艙段連體坐標系原點在裝配空間內(nèi)的位置矢量，并結(jié)合空間圓柱面方程求解艙段連體坐標系的旋轉(zhuǎn)矩陣，進而求得艙段的6個位姿參數(shù)。對矢量位姿求解法進行數(shù)值仿真，仿真結(jié)果驗證了該方法的有效性和準確性。

艙段裝配；矢量位姿；阻尼最小二乘法；圓柱體

0 引言

在衛(wèi)星、導彈、火箭等航天器的自動化總裝過程中，需先確定艙段在裝配空間內(nèi)的準確位姿，才能實現(xiàn)艙段的精準調(diào)姿[1-2]，而位姿求解方法是有效識別位姿的基礎(chǔ)。

現(xiàn)有的剛體位姿求解方法多是通過視覺測量、激光跟蹤等空間測量技術(shù)，測量剛體特征點在參考坐標系和剛體連體坐標系內(nèi)的坐標值，將剛體的位姿求解問題轉(zhuǎn)變?yōu)閮蓚€坐標系之間的點匹配問題。求解點匹配問題的常用算法有四組元法、奇異值分解法、正交矩陣法等[3-5]。張志勇等[6]提出了一種基于視覺成像的快速收斂的位姿測量算法，通過改進的正交迭代算法來求解剛體的位姿參數(shù)；張斌等[7]建立了基于鞍點規(guī)劃理論的機翼水平位姿評估方法，并運用奇異值分解和單純形法求解位姿；黃鵬等[8]提出了一種加權(quán)點匹配計算方法，提高剛體位姿求解中坐標轉(zhuǎn)換的精度。采用點匹配法求解剛體位姿具有較高的精度，但是點匹配方法中的連體坐標系和參考坐標系均為笛卡兒坐標系，應用在圓柱體結(jié)構(gòu)的位姿解算時，圓柱面上特征點在連體坐標系內(nèi)的坐標值不易測量，需對特征點在連體坐標系內(nèi)的坐標值進行標定。航天器是由若干艙段組成的，現(xiàn)有位姿識別方法需要標定一些不易測量的關(guān)鍵特征[9]，或需要對特征點進行實時檢測[10]，這直接影響航天器總裝的效率。

本文以圓柱體結(jié)構(gòu)類艙段為例，提出矢量位姿求解法，通過雙目立體視覺測量特征點在連體坐標系內(nèi)的初始坐標值，并確定特征點到艙段對接面的距離，求解艙段在參考坐標系內(nèi)的位置矢量和旋轉(zhuǎn)矩陣，得到艙段的位姿參數(shù)，并通過數(shù)值仿真驗證該方法的有效性與準確性。

1 艙段位姿標識

艙段在裝配空間內(nèi)的位姿可以用與艙段固定的連體坐標系O1X1Y1Z1在參考坐標系OXYZ內(nèi)的位姿參數(shù)來表示，連體坐標系和參考坐標系都是為表示艙段的位姿狀態(tài)而定義的空間坐標系。本文結(jié)合航天器艙段的結(jié)構(gòu)特點，將連體坐標系定義在艙段對接面上，原點O1為對接面的圓心，X1軸過艙段定位銷孔的圓心O2，Z1軸與艙段軸線重合，如圖1所示。

圖1 坐標系示意圖Fig.1 Schematic diagram of coordinate system

對接艙段在參考坐標系內(nèi)的位姿用矢量N表示為

N=(XO1，YO1，ZO1，α，β，γ)T

(1)

其中，(XO1，YO1，ZO1)是O1點在OXYZ內(nèi)的位置矢量，α、β、γ為O1X1Y1Z1相對于OXYZ的姿態(tài)角，即偏轉(zhuǎn)角、俯仰角、橫滾角，如圖2所示。

圖2 艙段姿態(tài)角Fig.2 Attitude angle of cabin

2 矢量位姿求解法

要求得艙段的位姿矢量N，需在艙體的圓柱面上設(shè)置如圖3所示的10個特征點。圖3中特征點a1～a5共線，過特征點a1～a5的直線與艙段軸線平行且與X1軸相交，特征點a6～a10的位置可在測量范圍內(nèi)任意選取。

圖3 特征點示意圖Fig.3 Schematic diagram of characteristic points

艙段圓柱面上的特征點ai在坐標系O1X1Y1Z1內(nèi)的坐標值為Ji=[xiyizi]T，在坐標系OXYZ內(nèi)的坐標值為Pi=[XiYiZi]T，則兩者之間的坐標變換關(guān)系可描述為

Pi=RJi+T

(2)

(3)

RX=[nxnynz]TRY=[oxoyoz]T
RZ=[axayaz]T

式中，i=1,2,…,10；R為O1X1Y1Z1相對于OXYZ的旋轉(zhuǎn)矩陣;T為O1點在OXYZ內(nèi)的位置矢量;RX、RY、RZ分別為O1X1Y1Z1的X1、Y1、Z1軸在OXYZ內(nèi)的方向向量。

對比激光跟蹤技術(shù)，雙目立體視覺測量方法具有效率高、精度匹配、系統(tǒng)簡單、成本低等優(yōu)點，適合于制造現(xiàn)場的在線、非接觸產(chǎn)品檢測和質(zhì)量控制，且一次可測量多個目標[11]，由于本文提出的特征點數(shù)量較多但均不屬于復雜關(guān)鍵特征，所以選取雙目立體視覺測量方法。通過雙目視覺測量技術(shù)測得ai在OXYZ內(nèi)的坐標值Pi，并標定ai到對接面的距離li，則艙段的位置矢量T可構(gòu)建如下最小二乘表達式：

(4)

式中，lR為艙體半徑。

采用阻尼最小二乘法對式(4)進行求解，計算f(Pi)的雅可比矩陣：

(5)

阻尼最小二乘法迭代計算公式為

Pk+1=Pk-ΔPk

(6)

ΔPk=(A(Pk)TA(Pk))-1A(Pk)Tf(Pk)

(A(Pk)TA(Pk)+ukI)ΔPk=A(Pk)Tf(Pk)

(7)

式中，uk為阻尼因子，uk>0；k為迭代次數(shù);I為單位矩陣。

A(Pk)TA(Pk)+ukI是正定矩陣，對其進行喬列斯基分解：

(8)

式中，Lp為下三角矩陣。

式(8)可化為兩個三角形方程組：

(9)

式中，Yp為過渡矩陣。

聯(lián)立式(8)、式(9)可求得迭代過程中的ΔPk。由式(6)～式(9)經(jīng)過迭代計算得到XO1、YO1、ZO1的值，得到艙段的位置矢量T。

由空間圓柱面方程[12]可知，艙體圓柱面在坐標系OXYZ內(nèi)的方程可通過向量形式表示為

|(Pi-T)×RZ|=lR

(10)

同樣采用阻尼最小二乘法求解RZ，構(gòu)造的最小二乘表達式為

(11)

經(jīng)迭代計算便可得到一組RZ，由于迭代初值選取的不同，求得的RZ方向可能與實際相反，則通過下式進行判定：

(12)

式中，σi為第i個特征點與O1點的連線與圓柱軸線的夾角。

當cosσi>0時，RZ為Z1軸在OXYZ內(nèi)的單位方向向量；當cosσi<0時，-RZ為Z1軸在OXYZ內(nèi)的單位方向向量。

(13)

X1軸在OXYZ內(nèi)的單位方向向量為

(14)

艙段連體坐標系O1X1Y1Z1的Y1軸在OXYZ內(nèi)的方向向量為

RY=RZ×RX

(15)

得到O1X1Y1Z1轉(zhuǎn)換到OXYZ的旋轉(zhuǎn)矩陣R：

R=[RXRYRZ]

(16)

O1X1Y1Z1旋轉(zhuǎn)變換到OXYZ過程為先繞Y軸旋轉(zhuǎn)α的角度，再繞X軸旋轉(zhuǎn)β的角度，最后繞Z軸旋轉(zhuǎn)γ的角度，旋轉(zhuǎn)過程如圖4所示。

圖4 坐標系旋轉(zhuǎn)變換Fig.4 Rotation transformation of coordinate system

旋轉(zhuǎn)矩陣R可表示為

(17)

式中，cα、sα分別是cosα、sinα的簡寫，其他類同。

由式(16)和式(17)可得

(18)

3 數(shù)值仿真

本文通過構(gòu)造一組仿真算例，對比通過矢量位姿求解法求得的艙段位姿與艙段實際位姿來驗證矢量位姿求解法的正確性，通過UG8.0建立如圖5所示的仿真模型。

圖5 仿真模型Fig.5 Simulation model

艙體半徑685 mm，在參考坐標系內(nèi)的實際位姿為N=(420 mm，680 mm，2250 mm，-13.62°，21.72°，-21.52°)。

在艙體圓柱面上按照前文所述的方式標記10個特征點，并提取特征點在參考坐標系內(nèi)的坐標值及特征點到對接面的距離，如表1所示。

通過MATLAB編寫仿真程序，將表1數(shù)據(jù)代入位姿算法中求解圓柱體位姿，表1中特征點的坐標值為準確值，為驗證測量誤差對該位姿算法的影響，對特征點在參考坐標系下的坐標值賦予隨機誤差，誤差的幅值分別為0.2 mm、0.4 mm、0.6 mm、0.8 mm、1 mm，每一種情況進行5次仿真計算，每一次給特征點坐標值賦予不同的隨機誤差，取5次結(jié)果的平均值作為位姿參數(shù)，得到的結(jié)果如表2所示，所得到的艙段位姿與實際位姿的誤差如表3所示。

表1 特征點坐標值及其到對接面距離Tab.1 Coordinate values of characteristic points and the distances to the docking surface mm

表2 位姿參數(shù)
Tab.2 Pose parameters

坐標誤差幅值(mm)位姿參數(shù)X(mm)Y(mm)Z(mm)α(°)β(°)γ(°)0420.0680.02250.0-13.6221.72-21.520.2420.0680.02250.0-13.6221.73-21.520.4420.0680.02250.1-13.6221.72-21.520.6420.0680.02250.0-13.6121.73-21.510.8419.9680.02250.1-13.6321.72-21.541.0420.3680.12250.3-13.5921.74-21.55

表3 位姿誤差
Tab.3 Pose errors

坐標誤差幅值(mm)位姿誤差Δα(°)Δβ(°)Δγ(°)Δl(mm)000000.20.00060.0041-0.00140.03680.4-0.0007-0.0027-0.00620.05850.60.00650.00270.00160.05360.8-0.0104-0.0073-0.02040.14541.0-0.03540.0123-0.03440.4439

注：Δl為O1點坐標計算值與實際值之間的偏差。

從數(shù)值仿真結(jié)果可以看出，本文提出的艙段位姿解算方法是有效的，在特征點測量坐標值無誤差的情況下，計算得到的艙體位姿與艙體的實際位姿完全相符。在特征點的坐標值存在測量誤差的情況下，本方法仍具有較高的精度，并且可通過多次測量，利用對計算結(jié)果求均值的方法來減小測量誤差對位姿計算精度的影響。

4 結(jié)論

(1)本文提出矢量位姿求解法，特征點選取簡單，且只需在艙段初始靜態(tài)下一次測量所選取的特征點，保證了測量效率與精度，用以準確得到對接艙段的空間位姿，并通過仿真對其進行了驗證。

(2)仿真試驗結(jié)果表明，矢量位姿求解法在特征點的坐標值存在測量誤差的情況下，仍具有較高的求解精度。

(3)在應用矢量位姿求解法求解艙段位姿的基礎(chǔ)上，采用多次測量計算求均值的方法，可顯著提高艙段位姿的計算精度。

(4)在本文研究的基礎(chǔ)上可采用三個轉(zhuǎn)動自由度相對獨立的調(diào)姿機構(gòu)，即求得旋轉(zhuǎn)矩陣后解出三個姿態(tài)角，利用高精度伺服電機驅(qū)動艙段分別完成偏轉(zhuǎn)、俯仰、橫滾，對艙段進行快速準確調(diào)姿。

[1] 林雪竹,李麗娟,曹國華,等.大部件對接中iGPS高精度位姿測量優(yōu)化設(shè)計[J].航空學報,2015,36(4):1299-1311. LIN Xuezhu, LI Lijuan, CAO Guohua, et al .Optimal Design Based on iGPS High-precision Posture Measurement for Large Size Component Joining[J]. Acta Aeronauticaet Astronautica Sinica,2015,36(4):1299-1311.

[2] 朱永國,黃翔,宋利康, 等. 基于理想驅(qū)動力的中機身調(diào)姿多項式軌跡規(guī)劃[J].計算機集成制造統(tǒng),2015,21(7):1790-1796. ZHU Yongguo, HUANG Xiang, SONG Likang, et al. Polynomial Trajectory Planning Method Based on Ideal Drive Forces for Aircraft Fuselage Pose Adjustment[J].Computer Integrated Manufacturing Systems,2015,21(7):1790-1796.

[3] ROUHANI M, SAPPA A D. Implicit Polynomial Representation through a Fast Fitting Error Estimation[J].IEEE Transactions on Image Processing,2012,21(4):2089-2098.

[4] RAHAYEM M R, KJELLANDER J A P. Quadric Segmentation and Fitting of Data Captured by a Laser Profile Scanner Mounted on an Industrial Robot[J]. International Journal of Advanced Manufacturing Technology,2011,52(1/4):155-169.

[5] 化春鍵,陳瑩.高精度尺度不變特征點匹配方法及其應用[J].中國機械工程,2014,25(12):1655-1658. HUA Chunjian, CHEN Ying. Precise Scale Invariant Feature Matching and Its Application[J].China Mechanical Engineering,2014,25(12):1655-1658.

[6] 張志勇,張靖,朱大勇.一種基于視覺成像的快速收斂的位姿測量算法及實驗研究[J].航空學報,2007,28(4):943-947. ZHANG Zhiyong, ZHANG Jing, ZHU Dayong.A Fast Convergent Pose Estimation Algorithm and Experiments Based on Vision Images[J].Acta Aeronauticaet Astronautica Sinica,2007,28(4):943-947.

[7] 張斌,姚寶國,柯映林.基于鞍點規(guī)劃理論的機翼水平位 姿評估方法[J].浙江大學學報(工學版),2009,43(10):1761-1765. ZHANG Bin, YAO Baoguo, KE Yinglin.Saddle Point Programming Based Level-posture Evaluation Method for Aircraft Wings[J].Journal of Zhejiang University (Engineering Science),2009,43(10):1761-1765.

[8] 黃鵬,王青,李江雄,等.激光跟蹤儀三維坐標轉(zhuǎn)換綜合優(yōu)化方法[J].計算機集成制造系統(tǒng),2015,21(11):2912-2920. HUANG Peng, WANG Qing, LI Jiangxiong, et al. Comprehensive Optimization for Three-dimensional Coordinate Transformation of Laser Tracker[J].Computer Integrated Manufacturing Systems,2015,21(11):2912-2920.

[9] 劉繼紅,龐英仲,鄒成.基于關(guān)鍵特征的飛機大部件對接位姿調(diào)整技術(shù)[J].計算機集成制造系統(tǒng),2013,19(5):1009-1014. LIU Jihong, PANG Yingzhong, ZOU Cheng. Adjusting Position-orientation of Large Components Based on Key Features[J].Computer Integrated Manufacturing Systems,2013,19(5):1009-1014.

[10] 屈穩(wěn)太,應蓓華,董鳳蓮.大剛體姿態(tài)計算及多軸同步協(xié)調(diào)控制技術(shù)[J].中國機械工程,2014,25(22):3050-3055. QU Wentai, YING Beihua, DONG Fenglian. Calculation of Large Body Pose and Muti-axis Synchronous Coordinating Control[J].China Mechanical Engineering,2014,25(22):3050-3055.

[11] 全燕鳴,黎淑梅,麥青群.基于雙目視覺的工件尺寸在機三維測量[J].光學精密工程,2013,21(4):1054-1061. QUAN Yanming, LI Shumei, MAI Qingqun. On-machine 3D Measurement of Workpiece Dimens-ions Based on Binocular Vision[J].Optics and Precision Engineering,2013,21(4):1054-1061.

[12] 董明望,吳林,戴明輝,等.圓柱凸輪側(cè)向傳動機構(gòu)圓柱凸輪廓面研究[J].中國機械工程,2014,25(3):337-340. DONG Mingwang, WU Lin, DAI Minghui, et al. Research on Cylindrical Cam Lateral Transmission Mechanism Profile [J]. China Mechanical Engineering,2014,25(3):337-340.

(編輯 袁興玲)

Study on Posture Solution Method of Automatic Assembly of Cabins

JIN Herong1,2LIU Da1,2YU Bin1,2FAN Xiubin3

1.Parallel Robot and Mechatronic System Laboratory of Hebei Province,
Yanshan University, Qinhuangdao,Hebei，066004 2.Key Laboratory of Advanced Forging & Stamping Technology and Science of Ministry of National Education,Yanshan University, Qinhuangdao,Hebei，066004 3.Beijing Xinghang Mechanical-Electrical Equipment Co.，Ltd.，Beijing，100074

The accurate posture of spacecraft cabins in the assembly spaces should be determined in the spacecraft assembly production. Taking cylinder class spacecraft cabin structure as an example, a vector posture solving method was proposed to realize the accurate solution of cabin’s postures. By laying reasonable feature points on the cylindrical surfaces of cabins, the position vector of origin of body-fixed coordinate system of cabins in the assembly space was solved using the damped least square method. The rotation matrix of body-fixed coordinate system of cabins was solved combined with the space cylindrical surface equations, and then the six pose parameters of cabin were obtained. The numerical simulation was conducted on the cabin using the proposed method and the simulation results show its validity and accuracy.

cabin assembly; vector posture; damped least square method; cylinder

2016-03-02

國家自然科學基金資助項目(51275437)

TP242

10.3969/j.issn.1004-132X.2017.01.015

金賀榮，男，1975年生。燕山大學機械工程學院副教授。主要研究方向為工程機械高機動性與柔性智能裝配。發(fā)表論文40余篇。E-mail:ysujhr@ysu.edu.cn。劉 達，男，1992年生。燕山大學機械工程學院碩士研究生。于 斌，男，1990年生。燕山大學機械工程學院碩士研究生。范秀斌，男，1990年生。北京星航機電裝備有限公司研發(fā)中心高級工程師、博士。