CN101608920A - 一种组合式空间位姿精密动态测量装置及方法 - Google Patents

Abstract

本发明公开组合式空间位姿精密动态测量装置及方法，装置包括惯性测量单元、全站仪测量单元以及分别与数据采集处理单元连接，数据采集处理单元与计算机连接。惯性测量单元测出目标固联体加速度、角速度，全站仪测量计算出目标固连体位姿，通过数据采集处理单元发送给计算机，计算机根据全站仪数据到达时刻、测量时滞和惯性测量解算原理，采用时间回溯算法和KALMAN滤波，融合处理两种数据，消除全站仪测量时滞，解算出最优估计的目标固联体位置和姿态。本发明提高了现有全站仪测量系统的稳定性和实时位姿测量精度，丰富现有动态跟踪测量系统的测量输出信息，满足了大型装置对大尺度空间的高精度、高采样率、长时间运行的实时测量需求。

Description

一种组合式空间位姿精密动态测量装置及方法

技术领域

本发明属于大尺度(超过300米)精密测量技术领域，涉及一种空间位姿精密动态测量技术。

背景技术

精密测量技术和装置是先进制造主要支撑技术之一，体现国家工业与科技水平。国民经济和先进制造的不断发展对精密测量提出新的要求，因此提高精密测量水平对提高我国先进制造能力，促进国民经济发展具有重要意义。针对大尺度(达到300米)空间的高精度、高采样率、长时间运行的实时测量问题，还没有哪一种技术具有绝对的竞争优势。目前，可选技术主要包括：全球定位系统(GPS)、惯性测量技术(IPS)和全站仪测量技术(TS)。综合评价各具优势和不足。

GPS具有全天候、速度快、精度高、点间相互不用通视等优点。也有受美国控制制约、要求视空条件好、周围不能有强功率发射电台等限制。测量速度和精度可达到20Hz，1cm。

60年代以来，随着激光的出现，激光陀螺和光纤陀螺问世，以其优良的性能受到关注，迅速进入惯性测量领域。惯性测量元件的发展，为惯性测量装置和技术的发展奠定了良好的基础。惯性测量单元仅依靠惯性装置本身就能在目标固联体内部独立地完成测量任务，具有高度的自主性，不受测量范围限制，采样率高给出近似连续的测量数据，能提供速度、加速度等丰富的动态测量信息。由于惯性测量单元具有高度自主性，不从外部获得信息进行校准，单独采用惯性单元进行测量时会使得定位定向误差发生累积，其定位误差随测量时间增加而变大，无法进行长时间连续工作。

有着“测量机器人”之称的全站仪测量技术测程大、价格低、环境适应能力强，静态测量精度高(测角精度可达到0.5″，静态测距精度1mm+1ppm)，但动态测量能力较弱。随着全站仪技术的发展，功能的不断完善，全站仪已能进行自动识别、锁定和跟踪。尽管全站仪测量系统实现了动态跟踪测量和智能化，但仍存在时滞和采样率低(5Hz)等问题，尤其存在较大的测量时滞，对动态测量精度造成很大的影响。

发明内容

为了解决现有技术的问题，本发明针对大型装置对大尺度空间的高精度、高采样率、长时间运行的实时测量需求，提出一种组合式空间位姿精密动态测量装置及方法。

为达成所述目的，本发明组合式空间位姿精密动态测量装置，该系统包括惯性测量单元、全站仪测量单元、数据采集处理单元以及计算机；惯性测量单元和全站仪测量单元分别与数据采集处理单元相连接，惯性测量单元输出目标固连体角速度和加速度信息；全站仪测量单元输出对应目标固连体上靶标的位置信息；数据采集处理单元分别与惯性测量单元和全站仪测量单元相连接，数据采集处理单元接收、存储并输出目标固连体角速度和加速度信息和目标固连体上靶标的位置信息；计算机与数据采集处理单元相连接，用于将角速度和加速度信息和靶标的位置信息进行数据融合处理，并输出目标固连体的位置和姿态。

其中惯性测量单元是用于目标固连体全位姿测量所需的按平台式或捷联式工作方式工作的惯性仪表组。

其中全站仪测量单元包括：全站仪群和靶标组，每个全站仪与对应的靶标非接触连接，靶标组接收并反射所述全站仪群发射的信号。

其中计算机的数据融合处理，采用时间回溯算法修正消除全站仪测量单元实时动态跟踪测量的时滞，融合处理惯性测量单元和全站仪测量单元的测量数据，计算出目标固连体在全局坐标系的位置和姿态信息。

为达成所述目的，本发明组合式空间位姿精密动态测量方法，包括如下步骤：

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

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

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

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

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

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

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

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

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

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

其中，所述各靶标在目标固连体坐标系内坐标，是预先标定出靶标与目标固联体之间的几何位置尺寸关系，定义目标固连体坐标系，从而得到各靶标在目标固连体坐标系内坐标；

其中，所述惯性测量单元平台坐标系与目标固连体坐标系之间的变换关系是：预先标定出惯性测量单元所构成的平台坐标系与目标固联体坐标系之间的几何位置和尺寸关系，从而得到平台坐标系与目标固连体坐标系之间的变换关系。

其中，所述全局坐标系是：预先标定出全站仪群之间的位置关系，定义全局坐标系，从而得到各全站仪自身坐标系与全局坐标系之间的变换关系以及目标固连体坐标系相对全局坐标系的初始状态。

其中，所述全站仪组时滞参数是利用目标固连体运行一理论轨迹，同时用全站仪群进行测量，用测量数据与理论数据进行相关处理得到全站仪组时滞参数。

其中，所述数据融合处理，是根据全站仪实时动态跟踪测量时存在的时滞，采用时间回溯算法，当全站仪位姿修正数据到达时，将惯性测量数据回溯到全站仪测量数据的真实时间点，以该时间点作为数据处理起算点，以全站仪到达数据作为起算初始参数，用于消除时滞，从而计算出目标固连体在全局坐标系的位置和姿态信息。

本发明的技术效果或优点：在全站仪测量系统中引入惯性自主测量方法提高测量质量，由陀螺与加速度计实时测量出被测目标的角速度与加速度，再通过积分分别给出姿态和位置信息。由全站仪每隔一段时间给出修正信号，采用时间回溯算法修正时滞。充分发挥全站仪测量和惯性测量各自的优势，相互补充。本发明两种技术融合测量的优势如下：

1、测程达到全站仪测程，达到100米到1000米以上。

2、得益于惯性测量优势提高了采样率，得到近似连续信号。

3、利用惯性测量优势直接提供速度、加速度动态信息。

4、惯性测量单元与全站仪测量单元优势互补，采用时间回溯算法修正全站仪测量单元时滞，使组合测量单元测量精度超过全站仪动态测量精度，达到全站仪静态测量精度。

5、全站仪测量单元的引入，克服了惯性测量单元测量误差随时间累计的弱点，使组合测量误差不随时间增加而累积与系统运行时间长短无关，延长了连续工作时间。

两种测量技术的融合实现了大尺度、高采样率、高精度、可长期运行的精密实时动态测量，为国家重大装备制造领域的精密测量提供了一种低成本的测量手段。

附图说明

图1是本发明装置的结构图

图2是现有技术500米口径球面射电望远镜示意图

图3确认时滞量仿真说明图

图4是本发明时间回溯算法运行示意图

图5是本发明时间回溯法对一组实测数据的处理结果

图6是本发明的所述方法的运行流程图

具体实施方式

下面结合附图详细说明本发明技术方案中所涉及的各个细节问题。应指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

本发明是针对世界上最大的500米口径球面射电望远镜(FAST)中馈源支撑系统的精密测量需求(工作范围300米，采样率10Hz，精度RMS 2mm)提出的。以该关键技术研究为背景，对各种相关测量技术进行了深入研究，提出研制一种组合式空间位姿精密动态测量装置和方法，可实现大尺度、超大尺度空间位姿的高精度实时测量。

FAST是利用贵州喀斯特地区的洼坑作为望远镜台址建造的世界上最大的球面射电望远镜，图2是现有技术500米口径球面射电望远镜示意图。具体工程实现是将500米口径的主反射面分成4600块小球面单元拼合并安装在洼地内，形成初始中性球面。观测时，反射面下方的驱动装置C在计算机控制下使小球面单元块沿径向运动，使对准被观测的天体St1的300米口径内的球面调整成抛物面，随天体St2运动不同时刻对准的区域不同；在反射面周边约600米尺度上建造数个一百多米高的塔B，馈源仓A由过塔钢索在计算机控制下拖动，在焦面上运动；索拖动下的馈源仓需在150米高、206米口径的焦面上运动；实现高精度的指向跟踪，馈源仓的高精度实时测量是关键，定位测量精度达到RMS 2mm，测量频率大于10Hz。这是极具挑战性的难题。为验证FAST各项关键技术的可行性，建造了50米整体缩尺模型。

综合考虑各种技术的应用前景，FAST 50米模型上采用三台徕卡TCRA1101型全站仪，测量馈源仓上三个360度棱镜为靶标，计算得出位置和姿态，由于存在采样率低和时滞等问题，动态测量精度无法满足FAST工作要求。对由三台全站仪组成的位姿动态测量系统性能进行了大量的测试，所有116组动态实验数据表明：

1、全站仪组合位姿动态测量系统存在时滞；2、在确定工况下时滞稳定不变；3、动态测量时时滞是测量误差的主要来源约占80％。FAST在测量范围、测量精度和测量速度的高要求以及大量的实验结果催生了这种“组合式空间位姿精密动态测量装置和方法”的问世。

请参阅图1，本发明的组合式空间位姿精密动态测量装置，包括惯性测量单元1、全站仪测量单元2、数据采集处理单元3以及计算机4；

惯性测量单元1和全站仪测量单元2分别与数据采集处理单元3相连接，惯性测量单元1输出目标固连体角速度和加速度信息；全站仪测量单元2输出对应目标固连体上靶标的位置信息；所述惯性测量单元是用于目标固连体全位姿测量所需的按平台式或捷联式工作方式工作的惯性仪表组。惯性测量单元1：装有三个单自由度陀螺和三个加速度计。陀螺的三个敏感轴与加速度计的三个敏感轴严格保持方向一致，并构成一个直角坐标系，即平台坐标系。在具体实施例中，可选用封装完整的成品惯性仪表组。它与目标固连体通过螺栓刚性连接，并由加工过程精确保证惯性仪表组所建平台坐标系与目标固连体坐标系间的转换精度。惯性仪表组输出目标固联体的加速度和角速度测量值。

全站仪测量单元2包括：全站仪群和靶标组，每个全站仪与对应的靶标非接触连接，靶标组接收并反射所述全站仪群发射的信号。全站仪测量单元2：由三个结构相同的全站仪和对应目标固连体上三个靶标组成。全站仪融光、机、电等先进技术于一身，其整个系统主要部分为电子测距单元或称之为测距仪；电子测角及微处理器单元或称之为电子经纬仪；电子记录单元或称存储单元。全站仪的测量原理和激光跟踪仪一样为极坐标法，需要测量一个斜距和二个角度可得到被测点的三维坐标，但是测距方式、跟踪方式及结构设计不同，决定了跟踪仪测量精度和采样率好于全站仪，而测程则无法与全站仪相比，跟踪仪采用干涉法和绝对测距法，测程几十米，全站仪采用多测尺相位法测距测程达到公里量级。可选用封装完整的徕卡公司的TCA系列全站仪，如TCA2003和TCRA1101等。靶标可选为三个空心角隅棱镜、实心角隅棱镜、或360棱镜等，它们与目标固联体通过螺栓刚性连接，并由加工过程保证反射镜中心点在运动目标固连坐标系内的坐标精度。

数据采集处理单元3分别与惯性测量单元1和全站仪测量单元2相连接，数据采集处理单元3接收、存储并输出目标固连体角速度和加速度信息和目标固连体上靶标的位置信息；计算机4与数据采集处理单元3相连接，用于将角速度和加速度信息和靶标的位置信息进行数据融合处理，并输出目标固连体的位置和姿态。所述计算机4的数据融合处理，是根据全站仪实时动态跟踪测量时存在的时滞，采用时间回溯算法修正消除全站仪测量单元2实时动态跟踪测量的时滞，融合处理惯性测量单元1和全站仪测量单元2的测量数据，计算出目标固连体在全局坐标系的位置和姿态信息。

数据采集处理单元3以及计算机4：计算机4内安装数据采集处理单元3和存储多传感器数据融合算法软件程序，数据采集处理单元3可选用市场成型产品Moxa卡，计算机4选择带有安装Moxa卡插槽的PC机即可。

现将本实施例中实施办法和部分算法公式阐述如下：

确定时滞：确定工况下，驱动目标固联体沿已知轨迹运动，用全站仪测量单元2测量目标固联体位姿，测量结果与理论轨迹进行相关比较可得到具体时滞量Δt。图3中e是理论轨迹，f是全站仪实测轨迹，在时间轴上将全站仪测量数据左移并计算e和f曲线之差的均方根误差，当误差达到最小值时时间轴移动的数据量即时滞量。

惯性测量单元1：惯性测量单元1由三个陀螺构成的陀螺组件及三个加速度计构成的加速度计组件，分别感测对应敏感方位的角速度及加速度，加速度和角速度经过积分等运算，获取目标固联体的位置及旋转角度从而获得目标固联体的位姿信息。

目标固联体的速度取决于它的初速度、加速度和作用时间，即速度是对加速度的积分，可表示为：

$\mathrm{\υ}\left(t_k\right)=\mathrm{\υ}\left(t_0\right)+\underset{0}{\overset{k}{\∫}}a\left(t\right)\mathrm{dt}$

式中速度为υ，时间为t，加速度为a，υ(t₀)为初始时刻目标固联体的运动速度向量。而目标固联体的位置取决于起始位置、速度和运行时间，也就是说位置就等于对速度的积分，可写成

$r\left(t_k\right)=r\left(t_0\right)+\underset{0}{\overset{k}{\∫}}\mathrm{\υ}\left(t\right)\mathrm{dt}$

式中r(t₀)为初始时刻目标固联体的位置向量。

同理，对目标固联体的角度测定也同样如此，角速度变化经积分可得到旋转角度。

$\mathrm{\ψ}\left(t_k\right)=\mathrm{\ψ}\left(t_0\right)+\underset{0}{\overset{k}{\∫}}\mathrm{\ω}\left(t\right)\mathrm{dt}$

目标固联坐标系和全局坐标系之间通过坐标变换矩阵来求得，陀螺仪测量得到目标固联体的姿态角具体算法包括：

目标固联坐标系统三轴定义分别为x^b为运行方向，前向为正，y^b为运行方向的交叉方向，右侧为正，z^b为目标固联体垂直方向，向下为正。加速度计测量得到的沿运目标固联坐标系的比力信息。

全局坐标系统三轴定义分别为xⁿ为北方向(大地北)，yⁿ为东方向，zⁿ为铅垂线方向，向下为正。陀螺仪测量可得到的沿全局坐标系的角加速度。

目标固联坐标系和全局坐标系之间通过坐标变换矩阵来求得。变换矩阵由三个欧拉角组成，航空工程中通常把ψ、θ和φ称为目标固联体姿态的方位Raw、俯仰pitch和侧滚角roll。其中θ俯仰角(pitch)，即目标固联坐标系xb轴与水平线的夹角，头朝上为正。φ为侧滚角(roll)，是目标固联坐标系yb轴与水平线的夹角，右测朝下为正。ψ为偏航角(Raw)，是在水平面内，目标固联坐标系xb轴与北方向之间的夹角，右偏为正。陀螺仪测量得到目标固联体的姿态角(Roll、Pitch、Yaw)。

引入四元数向量： $\stackrel{\‾}{q}=\left[\begin{array}{c}{q}_0\\ \stackrel{\→}{q}\end{array}\right],$ 满足限制条件 ${\stackrel{\‾}{q}}^T\stackrel{\‾}{q}=q_0^2+{\stackrel{\→}{q}}^T\stackrel{\→}{q}=1.$ 其中， $q_0=\cos \frac{\mathrm{\μ}}{2}$ 为标量， $\stackrel{\→}{q}=i{q}_1+j{q}_2+k{q}_3=\stackrel{\→}{\mathrm{\ζ}}\sin \frac{\mathrm{\μ}}{2}$ 为矢量。单位矢量

方向为旋转轴，μ为旋转角。

根据矢量

旋转变换 ${\stackrel{\→}{r}}^n=\stackrel{\‾}{q}\⊗{\stackrel{\→}{r}}^b\⊗{\stackrel{\‾}{q}}^{-1}$ 和四元数相乘法则，姿态矩阵可以用四元数表示为

$\stackrel{b\→n}{C}\left(\stackrel{\‾}{q}\right)=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2(q_1^2+q_2^2)\end{array}\right]$

$\stackrel{n\→b}{C}\left(\stackrel{\‾}{q}\right)=\stackrel{b\→n}{C}\left({\stackrel{\‾}{q}}^{-1}\right)=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2\left(q_2{q}_3{-q}_0{q}_1\right)& 1-2(q_1^2+q_2^2)\end{array}\right]$

四元数向量满足微分方程：

$\stackrel{\·}{\stackrel{\‾}{q}}=\frac{1}{2}\stackrel{\‾}{q}\⊗\stackrel{\‾}{\mathrm{\ω}}$ 即 $\stackrel{\·}{\stackrel{\‾}{q}}=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right]\left(\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right)$

其中，符号

表示四元数乘积。注意，式中， $\stackrel{\‾}{\mathrm{\ω}}=\left[\begin{array}{c}0\\ \stackrel{\→}{\mathrm{\ω}}\end{array}\right],$ 并且， $\stackrel{\→}{\mathrm{\ω}}={\mathrm{\ω}}_{x}i+{\mathrm{\ω}}_{y}j+{\mathrm{\ω}}_{z}k$ 为目标固联坐标系xbybzb相对全局坐标系坐标系xyz的转动角速度，即

$\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]-\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3\end{array}\right]$

式中， $\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$ 为目标固联体的绝对角速度； $\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3\end{array}\right]$ 为北—东—下全局坐标系的牵连角速度：

$\left[\begin{array}{c}{\mathrm{\ω}}_1\\ {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3\end{array}\right]=\left[\begin{array}{c}{\mathrm{\Ω}}_1+\frac{v_E}{R+h}\\ {\mathrm{\Ω}}_2-\frac{v_N}{R+h}\\ {\mathrm{\Ω}}_3-\frac{v_E}{R+h}\tan \mathrm{\λ}\end{array}\right]$

式中，Ω＝15.04107°/h，地球自转角速度；

λ＝目标固联体纬度；

v_E＝东向速度；

v_N＝北向速度；

R＝6378km，地球半径；

h＝目标固联体高度。

在短时间飞行的低速目标固联体上，地球表观运动角速度[ω₁ ω₂ ω₃]^T可忽略不计。因此，[ω_x ω_y ω_z]^T≈[p q r]^T，即，安装在目标固联体上的三只速率陀螺输出可作为相对角速度矢量

的直接测量。

姿态角可由姿态矩阵的元素C_ij计算如下：

方位角 $\mathrm{\ψ}=\arctan \frac{C_{12}}{C_{11}}$

俯仰角θ＝-arcsinC₁₃

测滚角 $\mathrm{\φ}=\arctan \frac{C_{23}}{C_{33}}.$

全站仪测量单元2：将靶标安装在被测目标固联体上，每个靶标由一台全站仪跟踪测量，靶标在全局坐标系下的坐标[X，Y，Z]_i(全局)，靶标在目标固联坐标系中的坐标[x，y，z]_i(目标)已知。运动刚体，动态测量需要确定它在空间的六个自由度，即要确定目标固联坐标系与全局坐标系的坐标转换参数ψ，θ，γ，X₀，Y₀，Z₀。

通过对目标刚体上三个观测点进行观测，三个观测点在刚体本身坐标系的坐标已知。由如下公式解算两坐标系统转换的旋转参数和平移参数ψ，θ，γ，X₀，Y₀，Z₀。

时间回溯：如图4示出消除时滞的时间回溯算法，T为惯性测量单元1数据采样周期，T1＝m*T为全站仪测量采样周期，Δt＝n*T为全站仪测量时滞，i即t₂为当前时刻，j全站仪数据到达点，t₁时间回溯后解算起始时刻，t₀时间回溯后解算起始时刻的前一时刻以计算初速度。全站仪一组数据到达后至下组数据到达前任意时刻t₂，用由全站仪测量单元2数据修正后的惯性测量数据解算出目标固联体在该时刻位姿。具体时间回溯办法为，全站仪数据在j时到达设k＝0，在全站仪下组数据到达前，惯性测量单元1每运行一个周期T，k值加1；在已经记录的惯性测量数据中，回溯惯性测量数据到t₁＝(i-k-n)*T时刻，将该时刻作为数据融合处理起算点，将全站仪最新到达数据作为该起算时刻的位置和姿态初始参数，用该时刻t₁与前一时刻t₀两时刻实测位置信息进行差分计算解算出速度作为该时刻速度初始参数，初始参数完备，可解算位姿，借此方法消除了全站仪测量时滞影响；

如图5示出用时间回溯办法对其中一组全站仪实测数据的处理结果，展示了消除时滞对测量精度的改善，图5a中曲线a是理论轨迹，曲线b是全站仪测量实测曲线，反应了存在时滞和采样率低问题，图5b是将图5a中曲线b按时滞量进行时间移位，使之与曲线a重合；图5c曲线c代表图5a中曲线a与曲线b之差，图5c中曲线d代表图5b中曲线a与曲线b之差，(RMS曲线d-RMS曲线c)/RMS曲线d＝0.802显示出消除时滞影响对动态测量精度的改善效果。

如图6示出算法流程图，计算机采集惯性测量单元1与全站仪测量单元2的测量输出值，经过相应初步的数据变换处理后，送入扩展卡尔曼滤波器。相应的算法流程主要由以下几部分组成：

(1)开始，初始化：设备启动并进行初始化，使设备进入稳定运行阶段；

(2)自检和标定：作为系统参数送入计算机，在循环测量解算过程中保持不变。

a.确定各靶标在目标固连体坐标系内坐标，是预先标定出靶标与目标固联体之间的几何位置尺寸关系，定义目标固连体坐标系，从而得到各靶标在目标固连体坐标系内坐标；

b.惯性测量单元1平台坐标系与目标固连体坐标系之间的变换关系，是预先标定出惯性测量单元1所构成的平台坐标系与目标固联体坐标系之间的几何位置和尺寸关系，从而得到平台坐标系与目标固连体坐标系之间的变换关系；

c.全站仪群间的位置关系，定义全局坐标系；是预先标定出全站仪群之间的位置关系，定义全局坐标系，从而得到各全站仪自身坐标系与全局坐标系之间的变换关系以及目标固连体坐标系相对全局坐标系的初始状态；

d.测量得到全站仪组时滞参数；目标固连体运行一理论轨迹，同时用全站仪群进行测量，用测量数据与理论数据进行相关处理得到全站仪组时滞参数；

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

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

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

(6)判断全站仪数据是否到达更新，如果全站仪数据到达后，则执行步骤(7)，如果全站仪数据没有到达，则执行步骤(8)。

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

(8)计算机4根据惯性测量解算原理，以该时间点为数据处理的时间起算点，全站仪测量单元2更新的数据为初始参数，解算出目标固联体位置和姿态；

(9)KALMAN滤波器：分别将(2)(7)(8)各步计算出的参数和数据，和惯性测量单元1和全站仪测量单元2所测原始数据(3)(4)，作为卡尔曼滤波算法的系统信息和测量信息，通过对误差进行最优估计修正，从而计算出目标固连坐标系相对于全局坐标系的位置和姿态信息，输出最优位姿值；

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

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (10)

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

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

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

计算机与数据采集处理单元相连接，用于将角速度和加速度信息和靶标的位置信息进行数据融合处理，并输出目标固连体的位置和姿态。

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

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

4、根据权利要求1所述的空间位姿精密动态测量装置，其特征在于，所述计算机的数据融合处理，采用时间回溯算法修正消除全站仪测量单元实时动态跟踪测量的时滞，融合处理惯性测量单元和全站仪测量单元的测量数据，计算出目标固连体在全局坐标系的位置和姿态信息。

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

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

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

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

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

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

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

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

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

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

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

6.根据权利要求5所述的动态测量的方法，其特征在于：所述各靶标在目标固连体坐标系内坐标，是预先标定出靶标与目标固联体之间的几何位置尺寸关系，定义目标固连体坐标系，从而得到各靶标在目标固连体坐标系内坐标。

7.根据权利要求5所述的动态测量的方法，其特征在于：所述惯性测量单元平台坐标系与目标固连体坐标系之间的变换关系是：预先标定出惯性测量单元所构成的平台坐标系与目标固联体坐标系之间的几何位置和尺寸关系，从而得到平台坐标系与目标固连体坐标系之间的变换关系。

8.根据权利要求5所述的动态测量的方法，其特征在于：所述全局坐标系是：预先标定出全站仪群之间的位置关系，定义全局坐标系，从而得到各全站仪自身坐标系与全局坐标系之间的变换关系以及目标固连体坐标系相对全局坐标系的初始状态。

9.根据权利要求5所述的动态测量的方法，其特征在于：所述全站仪组时滞参数是利用目标固连体运行一理论轨迹，同时用全站仪群进行测量，用测量数据与理论数据进行相关处理得到全站仪组时滞参数。

10.根据权利要求5所述的动态测量的方法，其特征在于：所述数据融合处理，是根据全站仪实时动态跟踪测量时存在的时滞，采用时间回溯算法，当全站仪位姿修正数据到达时，将惯性测量数据回溯到全站仪测量数据的真实时间点，以该时间点作为数据处理起算点，以全站仪到达数据作为起算初始参数，用于消除时滞，从而计算出目标固连体在全局坐标系的位置和姿态信息。

Images