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

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

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

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

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

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

0 引言

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

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

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

1 艙段位姿標識

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

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

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

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

(1)

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

圖2 艙段姿態角Fig.2 Attitude angle of cabin

2 矢量位姿求解法

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

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

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

Pi=RJi+T

(2)

(3)

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

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

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

(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為迭代次數;I為單位矩陣。

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

(8)

式中，Lp為下三角矩陣。

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

(9)

式中，Yp為過渡矩陣。

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

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

|(Pi-T)×RZ|=lR

(10)

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

(11)

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

(12)

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

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

(13)

X1軸在OXYZ內的單位方向向量為

(14)

艙段連體坐標系O1X1Y1Z1的Y1軸在OXYZ內的方向向量為

RY=RZ×RX

(15)

得到O1X1Y1Z1轉換到OXYZ的旋轉矩陣R：

R=[RXRYRZ]

(16)

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

圖4 坐標系旋轉變換Fig.4 Rotation transformation of coordinate system

旋轉矩陣R可表示為

(17)

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

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

(18)

3 數值仿真

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

圖5 仿真模型Fig.5 Simulation model

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

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

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

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

表2 位姿參數
Tab.2 Pose parameters

坐標誤差幅值(mm)位姿參數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點坐標計算值與實際值之間的偏差。

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

4 結論

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

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

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

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

[1] 林雪竹,李麗娟,曹國華,等.大部件對接中iGPS高精度位姿測量優化設計[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] 朱永國,黃翔,宋利康, 等. 基于理想驅動力的中機身調姿多項式軌跡規劃[J].計算機集成制造統,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] 張斌,姚寶國,柯映林.基于鞍點規劃理論的機翼水平位 姿評估方法[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] 黃鵬,王青,李江雄,等.激光跟蹤儀三維坐標轉換綜合優化方法[J].計算機集成制造系統,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] 劉繼紅,龐英仲,鄒成.基于關鍵特征的飛機大部件對接位姿調整技術[J].計算機集成制造系統,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] 屈穩太,應蓓華,董鳳蓮.大剛體姿態計算及多軸同步協調控制技術[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] 董明望,吳林,戴明輝,等.圓柱凸輪側向傳動機構圓柱凸輪廓面研究[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年生。燕山大學機械工程學院副教授。主要研究方向為工程機械高機動性與柔性智能裝配。發表論文40余篇。E-mail:ysujhr@ysu.edu.cn。劉 達，男，1992年生。燕山大學機械工程學院碩士研究生。于 斌，男，1990年生。燕山大學機械工程學院碩士研究生。范秀斌，男，1990年生。北京星航機電裝備有限公司研發中心高級工程師、博士。