[发明专利]一种组合式空间位姿精密动态测量装置及方法

权利要求书

1.一种组合式空间位姿精密动态测量装置，其特征在于，该系统包 括惯性测量单元、全站仪测量单元、数据采集处理单元以及计算机；

惯性测量单元和全站仪测量单元分别与数据采集处理单元相连接， 惯性测量单元输出目标固连体角速度和加速度信息在其敏感轴上的输出 值，通过惯性测量单元平台坐标系与目标固连体坐标系的相应坐标变换 得到目标固连体坐标系内的加速度和角速度值；全站仪测量单元输出对 应目标固连体上靶标的位置信息；确定惯性测量单元平台坐标系与目标 固连体坐标系之间的变换关系；全站仪组间的位置关系，定义全局坐标 系；测量得到全站仪组时滞参数；

数据采集处理单元分别与惯性测量单元和全站仪测量单元相连接， 数据采集处理单元接收、存储并输出目标固连体角速度和加速度信息和 目标固连体上靶标的位置信息；

计算机与数据采集处理单元相连接，用于将角速度和加速度信息和 靶标的位置信息进行数据融合处理，所述数据融合处理，采用时间回溯 算法修正消除全站仪测量单元实时动态跟踪测量的时滞，融合处理惯性 测量单元和全站仪测量单元的测量数据，在计算机记录惯性测量单元测 量数据序列中寻找起算时间点，更新时间回溯时刻并初始化解算参数； 作为参数输入下一步的解算程序中；计算机单元根据惯性测量解算原理 以回溯的时刻为数据处理的时间起算点，全站仪测量单元更新的数据为 初始参数，并计算出目标固连体在全局坐标系的位置和姿态。

2.根据权利要求1所述的空间位姿精密动态测量装置，其特征在于， 所述惯性测量单元是用于目标固连体全位姿测量所需的按平台式或捷联 式工作方式工作的惯性仪表组。

3.根据权利要求1所述的空间位姿精密动态测量装置，其特征在于， 所述全站仪测量单元包括：全站仪组和靶标组，每个全站仪与对应的靶 标非接触连接，靶标组接收并反射所述全站仪组发射的信号。

4.一种组合式空间位姿精密动态测量的方法，其特征在于，包括以 下步骤：

步骤S1：设备启动并进行初始化，使设备进入稳定运行阶段；

步骤S2：自检和标定：确定各靶标在目标固连体坐标系内坐标；惯 性测量单元平台坐标系与目标固连体坐标系之间的变换关系；全站仪组 间的位置关系，定义全局坐标系；测量得到全站仪组时滞参数；

步骤S3：惯性测量单元测出目标固联体加速度、角速度在其敏感轴 上的输出值，通过惯性测量单元平台坐标系与目标固连体坐标系的相应 坐标变换得到目标固连体坐标系内的加速度和角速度值，发送给数据采 集处理单元；

步骤S4：各全站仪测量计算出对应靶标在全站仪坐标系内的坐标信 息，通过坐标转换得到目标固连体上各靶标在全局坐标系下的坐标值， 发送给数据采集处理单元；

步骤S5：数据采集处理单元接收、存储惯性测量单元发送来的目标 固连体的加速度和角速度值和全站仪测量单元发送来的各靶标在全局坐 标系下的坐标值，记录数据的同时记录数据到达时刻，并根据计算机单 元需求转发相应的数据；

步骤S6：判断全站仪数据是否更新到达，如果全站仪数据到达后， 则执行步骤S7，如果全站仪数据没有到达，则执行步骤S8；

步骤S7：根据全站仪测量数据解算目标固联体位置和姿态；根据全 站仪时滞参数，在计算机记录的惯性测量单元测量的数据序列中寻找起 算时间点；更新时间回溯时刻并初始化解算参数；作为参数输入下一步 的解算程序中；

步骤S8：计算机单元根据惯性测量解算原理，以步骤S7回溯的时 刻为数据处理的时间起算点，全站仪测量单元更新的数据为初始参数，解 算出目标固联体位置和姿态；

步骤S9：KALMAN滤波器：分别将步骤S2、步骤S7、步骤S8各 步计算出的参数数据，和步骤S3、步骤S4所测原始数据，作为卡尔曼 滤波算法的系统信息和测量信息，通过对误差进行最优估计修正，从而 计算出目标固连体坐标系相对于全局坐标系的位置和姿态信息，输出最 优位姿值；

步骤S10：判断是否收到测量结束指令，如无收到，继续执行步骤6 进行测量。