舱段自动装配位姿求解方法研究

金贺荣 刘 达 于 斌 范秀斌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年生。北京星航机电装备有限公司研发中心高级工程师、博士。