工程科学与技术   2021, Vol. 53 Issue (3): 150-158

Pose Measurement Method and Experimental Study in the Docking of Cabins by Parallel Mechanism
LI Shiqi, CHEN Dong, WANG Junfeng
School of Mechanical Sci. & Eng., Huazhong Univ. of Sci. & Technol., Wuhan 430074, China
Abstract: Aiming at overcoming the shortcomings in the current series docking technology and realizing automatic cabin docking, a cabin docking system was developed and a pose measurement and conversion method was put forward to provide reliable motion parameters for the parallel mechanism to achieve the docking task. Firstly, the coordinate system relation between the movable cabin and the laser tracker was matched through measuring relevant features. The coordinate system of the movable cabin was taken as the intermediate coordinate system, and equations of the relative pose relation between the movable cabin and the mobile platform as well as that between the movable cabin and the fixed cabin were constructed respectively. Because the analytical solutions of the relative pose equations which contain measurement errors, were difficult to obtain, the relative pose equations were constructed into custom scalar functions. Thus the problem of relative pose calculation was transformed into the optimization of each scalar function and the simplex method was used to solve each scalar function to gain the relative pose parameters. The position transfer conversion method was used to calculate the target pose of mobile platform. Finally, a simulation platform for cabin docking test was built, and the docking was successfully realized by using the above pose calculation results. In order to evaluate the docking effect, three groups of feature points on the end faces of the two cabins were measured after docking. The mean absolute deviation of the coordinate of the feature points set in the x-direction was 2.673 mm, which was consistent with the result of the chamfering of pins being slightly inserted into the pin holes. Meanwhile, the mean absolute deviation of the coordinates of the feature points set in the y and z directions were 0.120, 0.163 mm respectively, which is consistent with the result that the end faces of two cabins are facing directly and the holes are basically aligned with the pins, and to some extent, showed the validity of the proposed pose measurement method and the correctness of the calculated results.
Key words: parallel mechanism    assembly of cabins    relative pose    pose measurement method

1 舱段对接的位姿测量问题

 图1 舱段对接中的相对位姿关系 Fig. 1 Relative pose relations in cabin docking

2 舱段对接系统的结构组成

 图2 舱段对接系统的结构组成 Fig. 2 Composition of cabin docking system

1）位姿测量子系统采用的测量设备为FARO–Vantage激光跟踪仪及直径为2.2225 cm（0.875英寸）的光学靶球，配套的测量数据处理软件为CAM2 measure10。FARO–Vantage最新标定的测距精度为（8.0±0.4） μm/m，测角精度为（10.0±2.5） μm/m。

2）对接执行子系统采用的对接设备为6–UPU并联机构，主要由动平台、静平台及通过虎克铰分别连接动、静平台的6个折返式电动缸组成。定义6个上铰点连线的几何中心为动平台 $\left\{ M \right\}$ 的原点，沿导轨运动方向为x轴正向，竖直向上为 ${\textit{z}}$ 轴正向，由右手定则确定y轴方向。从零位起，末端（动平台）在xy ${\textit{z}}$ 轴方向上各具有±100 mm的行程，在绕xy ${\textit{z}}$ 轴方向上各具有±10°的转动范围。标定后，并联机构的平移重复定位误差小于0.1 mm，转动重复定位误差小于0.05°，满足舱段对接的精度要求。

3）运动控制子系统的硬件包括控制柜和计算机等；软件集成了位姿拟合、调姿规划、运动控制等核心算法。主要处理位姿测量子系统发送的测量数据，并向对接执行子系统发送调姿规划及运动控制等指令。

3 相对位姿关系的测量及构造

${\textit{z}}$ yx方式描述欧拉角 ${\left[ {\alpha ,\beta ,\gamma } \right]^{\rm{T}}}$ 的旋转次序，旋转矩阵 ${{R}}$ 可记为：

 $\begin{array}{l} {{R}} = {{R}}\left( {\alpha ,\beta ,\gamma } \right) = {{{R}}_{\textit{z}}}\left( \alpha \right){{{R}}_{{y}}}\left( \beta \right){{{R}}_{{x}}}\left( \gamma \right) = \\ \left[\!\!\!\! {\begin{array}{*{20}{c}} {\cos\; \alpha \cos\; \beta }&{\cos\; \alpha \sin\; \beta \sin\; \gamma - \sin\; \alpha \cos\; \gamma }&{\cos\; \alpha \sin\; \beta \cos\; \gamma + \sin\; \alpha \sin\; \gamma } \\ {\sin\; \alpha \cos\; \beta }&{\sin\; \alpha \sin\; \beta \sin\; \gamma + \cos\; \alpha \cos\; \gamma }&{\sin\; \alpha \sin\; \beta \cos\; \gamma - \cos\; \alpha \sin\; \gamma } \\ { - \sin\; \beta }&{\cos\; \beta \sin\; \gamma }&{\cos\; \beta \cos\; \gamma } \end{array}}\!\!\!\! \right] \end{array}$ (1)

 $\left\{\!\!\!\! {\begin{array}{*{20}{l}} {\alpha = \arctan \left( {{{{R}}_{21}}/{{{R}}_{11}}} \right)}, \\ {\beta = \arctan \left( { - {{{R}}_{31}}/\sqrt {{{R}}_{11}^2 + {{R}}_{21}^2} } \right)}, \\ {\gamma = \arctan \left( {{{{R}}_{32}}/{{{R}}_{33}}} \right)} \end{array} } \right.$ (2)

3.1 移动舱段与激光跟踪仪坐标关系匹配

3.2 移动舱段与动平台的相对位姿测量

3.2.1 $\left\{ D \right\}$ $\left\{ M \right\}$ 的相对姿态测量

 图3 移动舱段坐标系下的参考标记点K Fig. 3 Reference marker K in the coordinate system of movable platform

 图4 沿x轴平移后的K点坐标变化 Fig. 4 Coordinate of point K changes after translation along the x axis

 $_D^M{{R}}(a) \cdot {[{d_{{x}}},0,0]^{\rm{T}}} = {}^D{K_{{x}}^{'}}$ (3)

 图5 沿y轴平移后K点坐标变化 Fig. 5 Coordinate of point K changes after translation along the y axis

$\left\{ M \right\}$ K点坐标变化亦可等价为 $\left\{ M \right\}$ 下的坐标点 ${[0,{d_{{y}}},0]^{\rm{T}}}$ 按某个欧拉角b的旋转变换。记并联机构沿y轴运动后由激光跟踪仪测得的K点坐标为 ${}^D{K'_{{y}}}$ ，则：

 $_D^M{{R}}(b) \cdot {[0,{d_{{y}}},0]^{\rm{T}}} = {}^D{K'_{{y}}}$ (4)

 图6 沿 $\textit{z}$ 轴平移后K点坐标变化示意图 Fig. 6 Coordinate of point K changed after translation along the $\textit{z}$ axis

 $_D^M{{R}}(c) \cdot {[0,0,{d_{\textit{z}}}]^{\rm{T}}} = {}^D{K'_\textit{z}}$ (5)
3.2.2 $\left\{ D \right\}$ $\left\{ M \right\}$ 的相对位置测量

 图7 按照已知欧拉角旋转后k点坐标变化 Fig. 7 Change of coordinates of point k after rotation according to the known Euler angle

 $\left\| {{}^M{{{k}}'_i} - {}^M{{{k}}_i}} \right\| = \left\| {{}^D{{{k}}'_i} - {}^D{{{k}}_i}} \right\|, {i \ge 3}$ (6)
3.3 固定舱段与移动舱段的相对位姿测量

 图8 对接端面上3组特征点位置分布 Fig. 8 Location distribution of three groups of feature points on the end face

 $_F^D{{R}} \cdot {}^D{p_i} + _D^M{{T}} = {}^D{t_i},i = 1,2,3$ (7)
4 相对位姿关系的等价变换及求解

4.1 移动舱段与动平台相对位姿等价变换

 ${\;\;\;\;\;\;\;\;\;{A}} = {\left[\!\!\!\! {\begin{array}{*{20}{c}} {{}^D{K_{{x}}^\prime} } \\ {{}^D{K_{{y}}^\prime} } \\ {{}^D{K_{\textit{z}}^\prime} } \end{array}} \!\!\!\!\right]^{\rm{T}}} - _D^M\!{{R}} \cdot \left[\!\!\!\! {\begin{array}{*{20}{c}} {{d_{{x}}}}&0&0 \\ 0&{{d_{{y}}}}&0 \\ 0&0&{{d_{\textit{z}}}} \end{array}} \!\!\!\!\right]$ (8)

 $\left\{\!\!\!\!\begin{array}{l} {d_{{x}}} = \left\| {{}^D{K^\prime_{{x}}} - {}^D{K_{{x}}}} \right\|, \\ {d_{{y}}} = \left\| {{}^D{K^\prime_{{y}}} - {}^D{K_{{y}}}} \right\|, \\ {d_{\textit{z}}} = \left\| {{}^D{K^\prime_{\textit{z}}} - {}^D{K_{\textit{z}}}} \right\| \end{array}\right.$ (9)

 ${\;\;\;\;\;\;\;\;\;\;\;\;f_1} = \sum {a_{ij}^2} ,i = 1,2,3,j = 1,2,3$ (10)

 ${\;\;\;\;\;\;\;\;\;f_2} = \left\| {{}^M{{{k}}'_i} - {}^M{{{k}}_i}} \right\| - \left\| {{}^D{{{k}}'_i} - {}^D{{{k}}_i}} \right\|, {i \ge 3}$ (11)
4.2 舱段之间的相对位姿等价变换

 ${\;\;\;\;\;\;\;\;\;\;\;\;\left\{\!\!\!\!\begin{array}{l} _F^D{{R}} \cdot ({}^D{p_2} - {}^D{p_1}) = {}^D{t_2} - {}^D{t_1}, \\ _F^D{{R}} \cdot ({}^D{p_3} - {}^D{p_2}) = {}^D{t_3} - {}^D{t_2} \end{array} \right.}$ (12)

 ${\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\left\{\!\!\!\!\begin{array}{l} {f_3} = \left\| {{F_1}} \right\| + \left\| {{F_2}} \right\|, \\ {F_1} = _F^D{{R}} \cdot ({}^D{p_2} - {}^D{p_1}) - ({}^D{t_2} - {}^D{t_1}), \\ {F_2} = _F^D{{R}} \cdot ({}^D{p_3} - {}^D{p_2}) - ({}^D{t_3} - {}^D{t_2}) \end{array}\right.}$ (13)
4.3 相对位姿参数优化的初值确定及步骤

1）在利用单纯形法求解式（10）和（11）的标量函数 ${f_1}$ ${f_2}$ 极小值时，需要用到 $\left\{ D \right\}$ $\left\{ M \right\}$ 的相对位姿初值。

2）在求解 $\left\{ F \right\}$ $\left\{ D \right\}$ 的相对位姿时，将相对姿态角的迭代初值取 ${\left[ {0,0,0} \right]^{\rm{T}}}$ 。然后，用单纯型法求得使式（11）中标量函数取极小值时的最佳姿态旋转矩阵。

a）外缩。如果 $f({x}_{{\rm{r}}})< f({x}_{n{+1}})$ ，计算 ${x_{{\rm{co}}}} \!=\! \bar x \!+\! \left( {{x_{\rm{r}}} - \bar x} \right)/2$ ，并计算函数值 $f\left( {{x_{{\rm{co}}}}} \right)$ 。如果 $f({x}_{\rm{co}})< f({x}_{\rm{r}})$ ，接受 ${x_{{\rm{co}}}}$ 并终止迭代；否则，转到步骤5。

b）内缩。如果 $f({x}_{\rm{r}})\!\ge \! f({x}_{{n+1}})$ ，计算 ${x_{{\rm{ci}}}} \!=\! \bar x + \left( {{x_{{{n + 1}}}} \!-\! \bar x} \right)/2$ ，并计算函数值 $f\left( {{x_{{\rm{ci}}}}} \right)$ 。如果 $f({x}_{\rm{ci}})< f({x}_{n+1})$ ，接受 ${x_{{\rm{ci}}}}$ 并终止迭代；否则，转到步骤5。

 $\left\{\!\!\!\!\begin{array}{l} {}_F^D{\overline{{ R}}} \cdot {}^D{p_i} + {}_F^D{{{T}}_1} = {}^D{t_1}, \\ {}_F^D{\overline{{ R}}} \cdot {}^D{p_i} + {}_F^D{{{T}}_2} = {}^D{t_2}, \\ {}_F^D{\overline{{ R}}} \cdot {}^D{p_i} + {}_F^D{{{T}}_3} = {}^D{t_3} \end{array}\right.$ (14)

$\left\{ F \right\}$ 相对于 $\left\{ D \right\}$ 的平均最佳位置矢量：

 ${}_F^D{\overline{{T}}} = \frac{1}{3}\left( {{}_F^D{{{T}}_1} + {}_F^D{{{T}}_2} + {}_F^D{{{T}}_3}} \right)$ (15)
5 并联机构目标位姿的换算方法

 图9 舱段对接中各坐标系传递换算关系 Fig. 9 Transfer conversion relation of each coordinate system in cabin docking problem

$\left\{ F \right\}$ 下任意一参考点p的坐标为：

 ${}^F\!\!{{p}} = {\left[ {{}^F\!\!{p_x},{}^F\!\!{p_y},{}^F\!\!{p_{\textit{z}}}} \right]^{\rm{T}}}$ (16)

 ${}^D\!\!{{p}} = {}_F^D{{R}} \cdot {}^F\!\!{{p}} + {}_F^D{{T}}$ (17)

 ${}^M\!\!{{p}} = {}_D^M{{R}} \cdot {}^D\!\!{{p}} + {}_D^M{{T}}$ (18)

 ${}_F^{M'}\!\!{{R}} = {}_D^M{{R}},{}_F^{M'}\!\!{{T}} = {}_D^M{{T}}$ (19)

 ${}^{M'}{{p}} = {}_F^{M'}\!\!{{R}} \cdot {}^F\!\!{{p}} + {}_F^{M'}{{T}}$ (20)

 ${}^{M'}\!\!{{p}} = {}_D^M{{R}} \cdot {}^F\!\!{{p}} + {}_D^M{{T}}$ (21)

$\left\{ {M'} \right\}$ 相对 $\left\{ M \right\}$ 的位姿为 $\left[ {{}_{M'}^M{{R}},{}_{M'}^M{{T}}} \right]$ ，则参考点变换到 $\left\{ M \right\}$ 下的坐标为：

 ${}^M\!\!{{p}} = {}_{M'}^M\!{{R}} \cdot {}^{M'}\!\!{{p}} + {}_{M'}^M{{T}}$ (22)

 $\qquad\qquad\qquad{}^M{{p}} = {}_D^M{{R}} \cdot \left( {{}_F^D{{R}} \cdot {}^F\!\!{{p}} + {}_F^D{{T}}} \right) + {}_D^M{{T}}$ (23)

 $\qquad\qquad\qquad{}^M{{p}} = {}_{M'}^M{{R}} \cdot \left( {{}_D^M{{R}} \cdot {}^F\!\!{{p}} + {}_D^M{{T}}} \right) + {}_{M'}^M{{T}}$ (24)

 ${\;\;\;\;\;\;\;\;\;\;\;\;\;\left\{\!\!\!\!\begin{array}{l} {}_{M'}^M{{R}} = {}_D^M{{R}} \cdot {}_F^D{{R}} \cdot {}_D^M{{{R}}^{ - 1}}, \\ {}_{M'}^M{{T}} = {}_D^M{{R}} \cdot {}_F^D{{T}} + {}_D^M{{T}} - {}_{M'}^M{{R}} \cdot {}_D^M{{T}} \end{array} \right. \!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!}$ (25)

6 位姿测量及换算方法的试验验证

 图10 舱段对接模拟试验实物场景 Fig. 10 Physical scene of cabin docking simulation

 \begin{aligned}[b] {}_D^M{{T}} &= {\left[ {{\rm{1\;236}}{\rm{.956\;9, - 0}}{\rm{.001\;5,781}}{\rm{.906\;6}}} \right]^{\rm{T}}},\\ {}_D^M{{R}} &= \left[\!\!\!\! {\begin{array}{*{20}{c}} {{\rm{0}}{\rm{.999\;98}}}&{0.000\;31}&{ - 0.006\;99} \\ { - 0.000\;28}&{{\rm{0}}{\rm{.999\;99}}}&{{\rm{0}}{\rm{.004\;40}}} \\ {{\rm{0}}{\rm{.006\;99}}}&{ - 0.004\;40}&{{\rm{0}}{\rm{.999\;97}}} \end{array}} \!\!\!\!\right]{\text{。}} \end{aligned}

 \begin{aligned}[b] {}_F^D{{T}} &= {\left[ {{\rm{68}}{\rm{.741\;6, - 12}}{\rm{.184\;1, - 73}}{\rm{.627\;6}}} \right]^{\rm{T}}},\\ {}_F^D{{R}} &= \left[\!\!\!\! {\begin{array}{*{20}{c}} {{\rm{0}}{\rm{.998\;4}}}&{{\rm{ - 0}}{\rm{.042\;4}}}&{{\rm{ - 0}}{\rm{.038\;6}}} \\ {{\rm{0}}{\rm{.042\;5}}}&{{\rm{0}}{\rm{.999\;1}}}&{{\rm{0}}{\rm{.003\;1}}} \\ {{\rm{0}}{\rm{.038\;5}}}&{{\rm{ - 0}}{\rm{.004\;7}}}&{{\rm{0}}{\rm{.999\;2}}} \end{array}} \!\!\!\!\right]{\text{。}} \end{aligned}

 ${}_{M'}^{M}\!{{R}} = \left[\!\!\!\! {\begin{array}{*{20}{c}} {{\rm{0}}{\rm{.998\;4}}}&{{\rm{ - 0}}{\rm{.042\;5}}}&{{\rm{ - 0}}{\rm{.038\;4}}} \\ {{\rm{0}}{\rm{.042\;6}}}&{{\rm{0}}{\rm{.999\;1}}}&{{\rm{0}}{\rm{.003\;4}}} \\ {{\rm{0}}{\rm{.038\;3}}}&{{\rm{ - 0}}{\rm{.005\;0}}}&{{\rm{0}}{\rm{.999\;3}}} \end{array}} \!\!\!\!\right]{\text{。}}$

 图11 舱段对接前与对接后的场景 Fig. 11 Scene before and after cabin docking

 图12 舱段对接前的3组特征点绝对坐标偏差 Fig. 12 Absolute coordinate deviation s of three groups feature points before cabin docking

 图13 舱段对接后的三组特征点绝对坐标偏差 Fig. 13 Absolute coordinate deviation s of three groups feature points after cabin docking

7 结　论

 [1] Mei Z,Maropoulos P G. Review of the application of flexible,measurement-assisted assembly technology in aircraft manufacturing[J]. Proceedings of the Institution of Mechanical Engineers(Part B:Journal of Engineering Manufacture), 2014, 228(10): 1185-1197. DOI:10.1177/0954405413517387 [2] Guo Zhimin,Jiang Junxia,Ke Yinglin. Posture alignment for large aircraft parts based on three POGO sticks distributed support[J]. Acta Aeronauticaet Astronautica Sinica, 2009, 30(7): 1319-1324. [郭志敏,蒋君侠,柯映林. 基于POGO柱三点支撑的飞机大部件调姿方法[J]. 航空学报, 2009, 30(7): 1319-1324. DOI:10.3321/j.issn:1000-6893.2009.07.023] [3] Zhu Y,Huang X,Wei F,et al. Trajectory planning algorithm based on quaternion for 6–DOF aircraft wing automatic position and pose adjustment method[J]. Chinese Journal of Aeronautics, 2010, 23(6): 707-714. DOI:10.1016/S1000-9361(09)60273-9 [4] Dasgupta B,Mruthyunjaya T S. The Stewart platform manipulator:A review[J]. Mechanism and Machine Theory, 2000, 35(1): 15-40. DOI:10.1016/S0094-114X(99)00006-3 [5] Jayaweera N,Webb P,Johnson C. Measurement assisted robotic assembly of fabricated aero-engine components[J]. Assembly Automation, 2010, 30(1): 56-65. DOI:10.1108/01445151011016073 [6] Zou Jihua,Zhou Wanyong,Han Xianguo. Normal adjusting algorithm of a 3–RPS parallel mechanism in airplane assembly[J]. China Mechanical Engineering, 2011, 22(5): 557-560. [邹冀华,周万勇,韩先国. 飞机装配中基于3–RPS的并联机构法向调整算法[J]. 中国机械工程, 2011, 22(5): 557-560.] [7] Jin Herong,Liu Da,Yu Bin,et al. Study on posture solution method of automatic assembly of cabins[J]. China Mechanical Engineering, 2017, 28(1): 88-92. [金贺荣,刘达,于斌,等. 舱段自动装配位姿求解方法研究[J]. 中国机械工程, 2017, 28(1): 88-92. DOI:10.3969/j.issn.1004-132X.2017.01.015] [8] Jin Herong,Liu Da. Two-point positioning and pose adjustment method for automatic assembly of barrel-type cabin[J]. China Mechanical Engineering, 2018, 29(12): 1467-1474. [金贺荣,刘达. 面向筒类舱段自动装配的两点定位调姿方法[J]. 中国机械工程, 2018, 29(12): 1467-1474. DOI:10.3969/j.issn.1004-132X.2018.12.013] [9] Liu H,Zhu W,Ke Y. Pose alignment of aircraft structures with distance sensors and CCD cameras[J]. Robotics and Computer-Integrated Manufacturing, 2017, 48: 30-38. DOI:10.1016/j.rcim.2017.02.003 [10] Chen Guanyu,Cheng Qunlin,Zhang Jieyu,et al. Multi-sensor measurement based position and pose adjustment method for automatic docking of spacecraft cabins[J]. Journal of Beijing University of Aeronautics and Astronautics, 2019, 45(6): 1232-1239. [陈冠宇,成群林,张解语,等. 基于多传感器测量的航天器舱段自动对接位姿调整方法[J]. 北京航空航天大学学报, 2019, 45(6): 1232-1239. DOI:10.13700/j.bh.1001-5965.2018.0593] [11] Wen K,Du F,Zhang X. Algorithm and experiments of six-dimensional force/torque dynamic measurements based on a Stewart platform[J]. Chinese Journal of Aeronautics, 2016, 29(6): 1840-1851. DOI:10.1016/j.cja.2016.10.015 [12] Wen Jingjing,Liu Chengwu,Wu Bin,et al. Study on auto-docking method of missile cabins based on stewart platform[J]. Journal of Projectiles,Rockets,Missiles and Guidance, 2017, 37(5): 47-51. [温晶晶,刘承骛,吴斌,等. 基于Stewart平台的导弹舱段自动对接方法研究[J]. 弹箭与制导学报, 2017, 37(5): 47-51.]