CN110220512A - 一种全站仪组合惯性测量单元的动态定位*** - Google Patents

Abstract

本发明公开了一种全站仪组合惯性测量单元的动态定位***，包括惯性测量单元、全站仪、时间同步器、里程计以及数据处理单元；工作时，全站仪本体实时跟踪反射棱镜，经过时间同步器进行时间同步后给出棱镜所在位置的观测信息，观测信息是指斜距、航向、俯仰角；惯性测量单元测量的惯性数据以及里程计的增量信息经过时间同步器进行时间同步后，与全站仪的观测信息处于统一时间基准，交由数据处理单元中的卡尔曼滤波器统一进行数据解算，得出定位信息。本发明面向港口、码头、机场跑道等特定使用环境，发明了一种高精度自动跟踪全站仪/惯性测量单元组合测量***，实现了在复杂电磁环境下能够达到10厘米的导航定位精度。

Description

一种全站仪组合惯性测量单元的动态定位***

技术领域

本发明涉及定位领域，尤其涉及一种自动跟踪全站仪同惯性测量单元进行组合，从而实现局部区域内车载动态条件下的高精度定位***。

背景技术

目前常用的动态高精度定位方法采用卫星定位和惯性定位组合的方式，实际应用中,如军事领域接收机常常面临高动态、弱信号、强干扰或信号遮挡等复杂多变的环境。同时，复杂电磁环境下，GPS/北斗等卫星导航定位***极易受到压制和欺骗干扰(参见《卫星通信干扰技术的研究》、《卫星导航接收机抗欺骗干扰方法研究》)，这时接收机会失锁转而捕获欺骗信号或者跟踪中受到欺骗信号的影响增大从而整个***受到影响，进而导致卫星/惯性组合导航***定位误差逐渐变大，最终导致***不可用。

导航定位技术已经成为现今众多自动化***中必不可少的设备，定位精度和***可靠性都直接影响着所在***的工作能力和应用潜力。GNSS及其惯性组合导航***已得到广泛的应用，然而卫星导航方式过于依赖卫星***的信息，在特种环境下(如城市高层楼群区、隧道、地下空间、室内、林冠下等卫星信号弱/无地区，大坝、桥梁等构筑物，灾变环境和卫星导航定位设备及人员不可到达地区，无法设置标志点地区)难以适用。

发明内容

基于上述技术问题，本专利发明了一种能工作于特种环境中，无需卫星信息的局部全自主综合定位导航***。其通过引入激光跟踪定位/惯性组合导航理念，使***在无卫星导航定位信号条件下依然能够实现高精度的定位、定姿。

该***引入激光跟踪棱镜获得的实时目标位置信息，基于传统的惯性误差模型，分析了***组成中存在的各种误差项，引入***时间延迟作为新的状态估计参数，建立了简化的，适用于局部坐标系运动的激光跟踪和惯性测量单元组合测量***的误差传递方程和观测方程，最终采用扩展卡尔曼滤波器进行自动跟踪全站仪激光测距测角信息与惯性测量数据的融合处理，从而得到局部坐标系下的高精度的载***置姿态信息。

本发明面向港口、码头、机场跑道等特定使用环境，发明了一种高精度自动跟踪全站仪/惯性测量单元组合测量***，实现了在复杂电磁环境下能够达到10厘米的导航定位精度。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是全站仪组合惯性测量单元的动态定位***的原理示意图；

图2是图1中全站仪组合惯性测量单元的动态定位***的信息流向图；

图3是棱镜和GPS接收机相位中心的平面距离变化图；

图4是时滞修正后棱镜和GPS接收机相位中心的平面距离变化图。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。

参考图1，全站仪组合惯性测量单元的动态定位***包括惯性测量单元、全站仪、时间同步器、里程计、数据处理单元五部分，数据处理单元可通过计算机实现。

根据各部分的部署关系，全站仪和惯性测量单元组合导航***可分为观测站和流动站两部分。其中，全站仪、时间同步器以及计算机做为观测站部分固定放置于已知控制点，惯性测量单元、全反射棱镜以及里程计部署在移动载体上，用以测量移动载体的位置、角速度及加速度。

***实际工作时，全站仪实时跟踪反射棱镜，经时间同步器同步后给出棱镜所在位置的斜距、航向、俯仰角。同时，惯性数据(角速度、加速度)以及里程计的增量信息经过时间同步后，与全站仪观测信息处于统一时间基准，交由卡尔曼滤波器统一进行数据解算，得到定位信息。

在时间同步器以及计算机在进行数据处理时，还对数据进行误差补偿。

实现高精度的组合导航定位，除了要研究合理的滤波器结构外，更重要的是要了解各传感器的误差模型以及各误差源的相互作用关系，对于***级的误差，应从***的角度予以标定及补偿，减小对传感器部件的精度指标要求。下述将详细说明各种误差的模型，在各模型的基础上，施加一个等大的方向补偿，从而实现误差的消除。

a)惯性测量单元误差传递方程

惯性测量单元属于捷联惯导***，其误差方程表达形式如下所述：

其中，δV_N、δV_E分别为惯性测量单元在北向和东向上的速度误差；ΨR_N,ΨR_E,ΨR_D分别为惯性测量单元的航向角误差、俯仰角误差以及横滚角误差；Ω为地球自转角速度，Lc为惯性测量单元所处位置纬度，分别为等效北向和等效东向加速度计误差，ε_N、ε_E、ε_D分别为等效北向、等效东向、等效天向陀螺常值漂移；且，

ε_N＝C₁₁ε_X+C₁₂ε_Y+C₁₃ε_Z；

ε_E＝C₂₁ε_X+C₂₂ε_Y+C₂₃ε_Z；

ε_D＝C₃₁ε_X+C₃₂ε_Y+C₃₁₃ε_Z；

其中，ε_x，ε_y，ε_z代表惯性测量单元在导航计算坐标系下的陀螺漂移，及为惯性测量单元在导航计算坐标系下的加速度计零偏，C代表由于导航计算坐标系与实际导航坐标系之间的姿态误差偏差而引起的交叉耦合分量。

对于本***惯性测量单元而言，加速度计零偏为100μg±10％，陀螺随机漂移ε_x，ε_y，ε_z为0.1°/h±10％。

b)全站仪动态定位精度

在静态下，全站仪的测距测角的精度相当精确，且检测手段相当成熟。但对其动态性能的检测还处于空白状态。全站仪的动态测量性能的成因较为复杂。除了正常的大气影响改正，地球曲率改正，棱镜常数，倾斜误差外，动态测量的误差还包括时间延迟(时滞)，测角及测距误差、测量噪声和随机的跳变。

由于没有可以剔除粗差的多余观测条件，只有通过数学处理的方法将粗差予以剔除。在观测数据满足精度要求的前提下，为了得到基于离散点的空间运动轨迹，可以采用最小二乘拟合和三次样条插值的方法对数据进行处理。由于动静态的时滞产生原因不同，需分开处理。静态的时滞较稳定，一般采用最小二乘估计出时延；动态时只能估计出平均的时延值。

参考图3，其为本发明的棱镜和GPS接收机相位中心的平面距离变化图。对于高精度动态测量而言，全站仪的误差模型为：

x_i＝x₀-[L(1+k)+ΔL]cos(α+Δα)sin(β+Δβ)

y_i＝y₀+[L(1+k)+ΔL]cos(α+Δα)cos(β+Δβ)

z_i＝z₀+[L(1+k)+ΔL]sin(α+Δα)

Δα＝Δα₁+Δα₂

Δβ＝Δβ₁+Δβ₂

其中，x₀及y₀为全站仪坐标，x_i、y_i及z_i为观测i时刻棱镜坐标，k为测距标度系数，ΔL为时间延迟导致的测距动态误差，Δα为俯仰角测量误差，其由Δα₁、Δα₂两部分组成，Δα₁为光电码盘测角误差，其在动静态测量模式均存在，Δα₂为时间延迟导致的俯仰角动态测量误差，Δβ为航向角测量误差，其由Δβ₁、Δβ₂两部分组成，Δβ₁为光电码盘测角误差，其在动静态测量模式均存在，Δβ₂为时间延迟导致的航向角动态测量误差。

c)里程计误差

捷联惯导***通过里程计提供里程增量，实现基于航迹推算的导航定位解算，运动的初时位置为(x₀、y₀、z₀)，通过不断地采集里程计的输出，假设Δl_i为里程计测量的里程增量，为第i次解算出的航向角，θ_i为对应的俯仰角，则利用下面的公式可推算出i时刻的即时位置为(x_i、y_i、z_i)。

里程计测量的是轮胎相对地面的速度、位置变化情况，通常其系数相对固定，使用前均已标定，但轮胎胎压及路面摩擦系数因素都会导致里程计系数出现微小变化。考虑到高精度导航***需要，同时惯性测量单元在短时间内的位置精度较高，因此，可以使用惯性测量单元位置信息对轮胎胎压及路面摩擦系数进行校准，通常使用带遗忘因子的递推最小二乘法保证里程计系数的快速收敛，在精确已知两控制点坐标情况下，可以通过控制点或外界辅助信息进行里程计系数的实时标定。

d)***误差

***的时间同步误差由三部分组成，全站仪采样时延、惯性测量单元采样时延(含里程采样误差)、以及全站仪时间基准同惯性测量单元时间基准间的时间同步误差。其中，全站仪是时间同步误差的主要误差源，其最大可达0.1～0.2秒，剩余两项误差均为微秒级，可忽略不计。

全站仪动态测量过程中的时滞主要包括两部分：一是从全站仪开始测量到输出测量结果的时间；二是从测量数据传入计算机到计算机收到第一个字符这段延迟。计算机提供的动态标准位置的时间是以动态测量数据字符串中最后一个字符传入计算机为标准的，因此动态标准位置的时间延迟了，其相应的位置测量结果比实际的测量结果提前。如果将动态标准位置按照一定的时间差进行回推，其测量误差将会明显减小。当回推的时间和测量时滞一致时，测量误差平均值将达到最小。由于车辆行驶和转动的快慢对全站仪自动跟踪的性能存在一定影响，因此在不同的情况下，其测量时滞略有不同。全站仪动态跟踪的测量时滞在不同动态条件下约为100ms-110ms。时滞改正后，动态测量结果有明显的改善。

全站仪时延模型为τ＝τ_b+τ_r+τ_ω，

τ_b可认为固定时间延迟，且满足

τ_r为慢变漂移，可用一阶马尔可夫过程描述，

τ_ω为快变漂移，满足条件E[τ_ω(t)τ_ω(τ)]＝qδ(t-τ)

t表示全站仪采集时间序列，上标·表示导数，ω_r表示随机白噪声，E表示期望，q为固定值，且q∈[50，100]ms，δ表示单位脉冲函数。

实际使用时，τ_b、τ_ω对时延模型起主导作用，τ_b为100ms加减10％，q的数值一般为50～100ms。

图4的数据中，存在着明显时滞现象。静止状态的水平直线段和动态的波动段段存在0.2m的跳变。经过时滞修正后，误差明显改小。

杆臂效应是由于惯性测量单元安装位置与载体摇摆中心不重合而产生的一种干扰加速度分量。当载体处于摇摆或振动状态而惯性测量单元不在摇摆中心时，加速度计会敏感到离心加速度和切向加速度，从而引起加速度计的测量误差。对于以加速度计和陀螺仪的输出信号作为观测量的捷联惯性导航***，其初始对准会存在较大的原理性误差，因此必须予以消除或补偿。使用激光测距(或)同惯性测量单元进行组合导航前，必须将激光测距中心(或反射棱镜中心)同惯性测量单元量测中心进行归一化处理。

本发明采用标准卡尔曼滤波算法进行滤波，得到定位信息。

如式所述，卡尔曼滤波算法的状态向量为：

X_INS＝[δL,δλ,δh,δv_E,δv_N,δv_U,φ_E,φ_N,φ_U,τ]…(4),

其中，δL,δλ,δh分别为惯性测量单元在导航坐标系的经度，纬度，高程误差；δV_E、δV_N、δV_U分别为惯性测量单元在导航坐标系下的东向，北向，天向误差；Φ_E、Φ_N、Φ_U分别为惯性测量单元在导航坐标系下的俯仰，横滚，航向角误差；τ为全站仪对棱镜观测过程中引起的不确定性时间延迟。

卡尔曼滤波算法的量测方程为：

H_TS＝[I_10×10,0_10×10],

式中，Z_TS、H_TS分别为全站仪组合惯性测量单元***的观测方程和增益方程，L_INS、L_TS分别为观测时刻的惯性测量单元和全站仪各自推算的经度值，λ_INS、λ_TS分别为观测时刻的惯性测量单元和全站仪各自推算的纬度值，h_INS、h_TS分别为观测时刻的惯性测量单元和全站仪各自推算的高程值。

对于标准Kalman滤波算法，其根据下述五个公式进行状态的迭代更新，从而得到定位信息。

状态预测方程：X(k|k-1)＝A X(k-1|k-1)………..(1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果。

预测均方误差方程：P(k|k-1)＝A P(k-1|k-1)A’+Q………(2)

式(2)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A’表示A的转置矩阵，Q是***过程的协方差。

卡尔曼增益矩阵：Kg(k)＝P(k|k-1)H’/(H P(k|k-1)H’+R)………(3)

式(3)中，Kg为卡尔曼增益,H是测量***的参数，R为观测值噪声驱动阵。

状态估计方程：X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1))………(4)

式(4)中，X(k|k)是当前状态最优解，Z(k)为当前时刻观测值。

均方误差估计方程：P(k|k)＝(I-Kg(k)H)P(k|k-1)………(5)

式(5)中，P(k|k)是X(k|k)对应的协方差，I为单位阵。

本专利发明了一种新型高精度自动跟踪全站仪/惯性测量单元组合测量***,实现在无GNSS信号的环境下的精确位置、姿态测量。

通过全站仪和惯性测量单元数据的融合处理，提高了激光动态跟踪测量***的稳定性和捷联惯性实时姿态测量的精度，拓展了动态跟踪测量***的应用领域。实验结果表明,该套***可达厘米级的动态测量精度。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (9)

1.一种全站仪组合惯性测量单元的动态定位***，其特征在于，包括惯性测量单元、全站仪、时间同步器、里程计以及数据处理单元，全站仪由全站仪本体以及全反射棱镜组成；全站仪本体、时间同步器以及数据处理单元作为观测站部分固定放置于已知控制点，惯性测量单元、全反射棱镜以及里程计作为移动站部分部署在移动载体上；工作时，全站仪本体实时跟踪反射棱镜，经过时间同步器进行时间同步后给出棱镜所在位置的观测信息，观测信息是指斜距、航向、俯仰角；惯性测量单元测量的惯性数据以及里程计的增量信息经过时间同步器进行时间同步后，与全站仪的观测信息处于统一时间基准，交由数据处理单元中的卡尔曼滤波器统一进行数据解算，得出定位信息。

2.根据权利要求1所述的全站仪组合惯性测量单元的动态定位***，其特征在于，所述惯性数据由角速度数据和加速度数据组成。

3.根据权利要求1所述的全站仪组合惯性测量单元的动态定位***，其特征在于，所述经过时间同步器进行时间同步是指根据下述全站仪时延模型进行一个同样大小的反向补偿：

全站仪时延模型τ为：τ＝τ_b+τ_r+τ_ω；

其中，τ_b为全站仪的固定时间延迟，且满足τ_r为慢变漂移，且满足τ_ω为快变漂移，满足条件E[τ_ω(t)τ_ω(τ)]＝qδ(t-τ)；t表示全站仪采集时间序列，上标·表示导数，ω_r表示随机白噪声，E表示期望，q为固定值，且q∈[50，100]ms，δ表示单位脉冲函数。

4.根据权利要求1所述的全站仪组合惯性测量单元的动态定位***，其特征在于，所述数据处理单元还用于对惯性测量单元的捷联惯导误差进行补偿，补偿的方式是根据下述捷联惯导***的误差方程给出一个同样大小的反向补偿：

其中，δV_N、δV_E分别为惯性测量单元在北向和东向上的速度误差；Ψ_N,Ψ_E,Ψ_D分别为惯性测量单元的航向角误差、俯仰角误差以及横滚角误差；Ω为地球自转角速度，Lc为惯性测量单元所处位置纬度，分别为等效北向和等效东向加速度计误差，ε_N、ε_E、ε_D分别为等效北向、等效东向、等效天向陀螺常值漂移；且，

ε_N＝C₁₁ε_X+C₁₂ε_Y+C₁₃ε_Z；

ε_E＝C₂₁ε_X+C₂₂ε_Y+C₂₃ε_Z；

ε_D＝C₃₁ε_X+C₃₂ε_Y+C₃₁₃ε_Z；

其中，ε_x，ε_y，ε_z代表惯性测量单元在导航计算坐标系下的陀螺漂移，及为惯性测量单元在导航计算坐标系下的加速度计零偏，C代表由于导航计算坐标系与实际导航坐标系之间的姿态误差偏差而引起的交叉耦合分量。

5.根据权利要求1所述的全站仪组合惯性测量单元的动态定位***，其特征在于，所述数据处理单元还用于对全站仪的误差进行补偿，补偿的方式是根据下述误差模型给出一个同样大小的反向补偿：

x_i＝x₀-[L(1+k)+ΔL]cos(α+Δα)sin(β+Δβ)

y_i＝y₀+[L(1+k)+ΔL]cos(α+Δα)cos(β+Δβ)

z_i＝z₀+[L(1+k)+ΔL]sin(α+Δα)

Δα＝Δα₁+Δα₂

Δβ＝Δβ₁+Δβ₂

其中，x₀及y₀为全站仪坐标，x_i、y_i及z_i为观测i时刻棱镜坐标，k为测距标度系数，△L为时间延迟导致的测距动态误差，△α为俯仰角测量误差，其由△α₁、△α₂两部分组成，△α₁为光电码盘测角误差，其在动静态测量模式均存在，△α₂为时间延迟导致的俯仰角动态测量误差，△β为航向角测量误差，其由△β₁、△β₂两部分组成，△β₁为光电码盘测角误差，其在动静态测量模式均存在，△β₂为时间延迟导致的航向角动态测量误差。

6.根据权利要求1所述的全站仪组合惯性测量单元的动态定位***，其特征在于，惯性测量单元通过里程计提供里程增量，实现基于航迹推算的导航定位解算，运动的初时位置为(x₀、y₀、z₀)，通过不断地采集里程计的输出，假设△l_i为里程计测量的里程增量，为第i次解算出的航向角，θ_i为对应的俯仰角，则利用下面的公式推算出k时刻的即时位置为(x_k、y_k、z_k)：

里程计测量的是轮胎相对地面的速度、位置变化情况，其系数相对固定，使用前均已标定；且使用惯性测量单元的位置信息对里程计对轮胎胎压及路面摩擦系数进行校准。

7.据权利要求6述的全站仪组合惯性测量单元的动态定位***，其特征在于，使用惯性测量单元的位置信息对里程计对轮胎胎压及路面摩擦系数进行校准包括：使用带遗忘因子的递推最小二乘法保证里程计系数的快速收敛，在已知两控制点坐标情况下，通过控制点或外界辅助信息进行里程计系数的实时标定。

8.据权利要求1述的全站仪组合惯性测量单元的动态定位***，其特征在于，使全站仪同惯性测量单元进行组合定位前，将反射棱镜中心同惯性测量单元量测中心进行归一化处理。

9.据权利要求1述的全站仪组合惯性测量单元的动态定位***，其特征在于，交由数据处理单元中的卡尔曼滤波器统一进行数据解算，得出定位信息的过程中：

动态定位***的状态向量为X_INS：

X_INS＝[δL,δλ,δh,δv_E,δv_N,δv_U,φ_E,φ_N,φ_U,τ]·；

其中，δL,δλ,δh分别为惯性测量单元在导航坐标系的经度误差、纬度误差、高程误差；δV_E、δV_N、δV_U分别为惯性测量单元在导航坐标系下的东向误差、北向误差、天向误差；Φ_E、Φ_N、Φ_U分别为惯性测量单元在导航坐标系下的俯仰误差、横滚误差、航向角误差；τ为全站仪对棱镜观测过程中引起的不确定性时间延迟；

量测方程为：

H_TS＝[I_10×10,0_10×10]；

式中，Z_TS、H_TS分别为全站仪组合惯性测量单元***的观测方程和增益方程，L_INS、L_TS分别为观测时刻的惯性测量单元和全站仪各自推算的经度值，λ_INS、λ_TS分别为观测时刻的惯性测量单元和全站仪各自推算的纬度值，h_INS、h_TS分别为观测时刻的惯性测量单元和全站仪各自推算的高程值。