在实际应用或工程实践中，经常会遇到求解两个或多个运动体间的相对位置、相对姿态（以下简称相对位姿）的问题，尤其是要求获得位于物体内部、无法直接进行测量的几何中心的相对位姿等应用场合。基于上述需求，本文建立了物体几何测量中心随运动体运动的测量模型，并提出了一种适用于物体几何测量中心间相对位姿测量的计算方法。

1 问题描述

如图1所示，在参考单元上建立参考坐标系R（O-XYZ），在运动单元上建立运动坐标系S（o-B1B2B3），并且可以实时获得运动单元表面各点在R系的精确位置坐标，需要计算O-o间的相对位姿。

由于O和o均位于物体内部，传统的方法无法进行直接测量；同时由于位置和姿态的耦合关系，利用运动单元上标志点的空间位置坐标，也无法直接提供所需的姿态信息。在现有条件下，可以准确测得运动单元上的标志点在R系中的实时空间三维位置坐标，同时还有标志点与运动单元内部测量点之间距离的先验信息。如何根据上述数据，快速、准确地获得物体间的相对位姿，是本文所需解决的问题。

2 相对位姿求解

相对位姿的求解包含两方面：一是如何获得待测点在R系中的实时空间三维位置坐标，二是如何获得运动单元相对参考单元的位姿。这里认为运动单元和参考单元均是刚体，在运动过程中本身无形变。针对问题一，由于现有技术手段仅能测得运动单元表面标志点的实时空间三维位置坐标，因此考虑在运动单元表面布设不共面的3个标志点，通过多球面相交测量原理实现对待测点的三维定位，如图2所示，具体计算步骤如下：

2.1 计算标志点与待测点的距离，这里距离是已知的先验知识

在得到待测点位置增量的基础上，即可得到待测点的三维位置坐标，进一步可获得所需的相对位置信息。在上述解算过程中，只要位置增量是在线性化点附近，这种线性化方法就是可行的。若有多余3个标志点的信息，还可利用冗余测量值，通过最小二乘估计技术进一步提高解算精度。

2.4 求解相对姿态

在得到待测点的三维位置坐标基础上，结合在S系中标志点与待测点所构成的位置向量保持不变的特性，利用运动单元待测点与标志点构成的位置向量在R系与S系不同投影之间的转换关系，计算得到运动单元与参考单元间的姿态转换矩阵：

3 算法验证

为了验证所提算法的可行性，构造了如下的实验场景：参考坐标系R与参考单元的体轴坐标系重合，运动单元为一尺寸为300mm×200mm×200mm（长×宽×高）的长方体，待测点位于该长方体的几何中心。初始时刻该待测点在R系中的坐标为[0；1000；0]mm，运动单元上的3个标志点在S系的坐标分别为[0；0；100]mm、[100；0；0]mm、[0；150；100]mm，与待测点的距离分别为100 mm、100 mm、180.28 mm。以上几何位置关系可作为先验知识，为后续计算提供必要的空间约束关系。

设运动单元相对参考单元三个轴向均以1°/s转动，运动单元的待测点相对参考单元三个轴向均以10mm/s平动，待测点初始位置的近似三维坐标相对理想值分别相差30mm、25mm和15mm。在上述假设下，通过转动和平动信息可实时给出运动单元上的3个标志点在R系的位置坐标。利用本文所提的相对位姿计算方法，得到如图3、图4的仿真结果。

4 结论

本文提出一种基于标志点位置信息的相对位姿计算方法，该算法基于运动单元上标志点的动态位置信息、标志点与运动单元待测点的相对距离以及位置向量在不同坐标系中的转换关系，可以实现物体间高精度的相对位姿计算。该算法实现简便，可满足相对位姿测量的要求。

参考文献

[1]王惠南.GPS导航原理与应用[M].科学出版社，2003.

[2]秦永元.惯性导航[M].科学出版社，2006.

作者简介

张仟雨（2000-），女，陕西省西安市人。研究方向为数据处理。

张亚崇（1972-），男，陕西省西安市人。博士。研究方向为导航信息融合。

作者单位

1.西北大学附属中学 陕西省西安市 710075

2.西安飛行自动控制研究所 陕西省西安市 710065