CN110631573A - 一种惯性/里程计/全站仪多信息融合方法 - Google Patents

Abstract

本发明公开了一种惯性/里程计/全站仪多信息融合方法，针对惯性/里程计组合导航测量精度会随里程增加而下降的问题，利用全站仪测量控制点对惯性/里程计组合导航结果进行定点约束，提高测量精度，实现大型工程轨迹的高精度、连续、动态测量。

Description

一种惯性/里程计/全站仪多信息融合方法

技术领域

本发明属于一种组合导航***信息融合方法，具体涉及一种针对惯性基准大型工程轨迹精密测量方法。

背景技术

铁路、桥梁、隧道、大坝等大型工程是保障国家安全与民生的重要条件。在自然环境、地质结构稳定性、长期服役等复合因素作用下沉降、线形变化、错位变形的情况普遍存在，一方面对大型工程运营质量产生重大影响，另一方面，如果形变积累达到一定程度，将会导致结构强度突变，破坏工程自身固有的动/静力学稳定性，最终导致灾难性事件的发生。因此必须及时、高效、全面地进行大型工程精密轨迹测量，并以此为基础进行安全性评估和采取相应的灾害预防措施。

传统的大型工程轨迹精密测量方法主要包括检测尺、水准仪、全站仪等方法，监测点数量有限，劳动强度高，作业效率低，只适用于重点区域短距离、小范围的检测，基于惯性基准的大型工程精密轨迹测量是一种自主式测量方法，具有精度高、抗干扰能力强，能够全天候连续提供测量信息等优点，并且能够克服其它测量技术只能对离散的点进行测量、基准点难以确定、不能连续重复测量、测量仪器不易安装等缺点，具有广泛的应用前景。

为提高检测效率和测量精度，减轻测量人员的劳动强度，国内外许多专家学者在惯性基准大型工程轨迹精密测量应用领域开展了深入的研究。俄罗斯INFOTRANS公司通过基于惯性测量的惯性/里程计/GPS/激光测距仪等多信息融合的方法，实现了对铁路线路的精密轨迹测量，300m区间轨迹测量精度优于5mm。美国通用公司(GE)旗下的PII公司针对管道的测绘与形变，研制了一款基于惯性基准的位移测量装置，通过惯性/里程计组合，结合卫星定点修正的方法，实现了管道轨迹的精密测量。国内GJ-6型轨道检测车通过惯性/里程计/激光测距仪多信息融合的方法也实现了对铁路线路轨迹的精密测量，100m区间轨迹测量精度优于4mm。

惯性测量由于其自主、连续、不受干扰等特点，在众多领域都得到了广泛应用。但惯性导航***误差随时间累积，单纯采用惯性测量难以满足大型工程轨迹毫米级测量精度的要求，惯性/里程计组合导航利用里程计高精度测速信息抑制惯导***的误差累积，能够有效提高惯性导航***测量精度。全站仪是一种集光、机、电为一体的高精度测量仪器，将测角、测距、数据处理等诸多功能集于一体，架站完成后，能够对定点目标实现高精度测量。

惯性/里程计组合导航***的测量误差仍然随里程增加而累积，为满足大型工程轨迹毫米级测量精度的需求，本发明提出一种惯性/里程计/全站仪多信息融合方法，利用全站仪的高精度测量信息抑制惯性/里程计组合导航的误差累积，提高惯性基准轨迹测量精度，对于大型工程轨迹精密测量而言具有非常重要的意义。惯性/里程计组合导航利用里程计的高精度测速信息抑制惯性***随时间累积的误差，提高惯性***测量精度。但惯性/里程计组合导航测量精度会随里程增加而下降，仅通过惯性/里程计组合导航难以满足大型工程轨迹毫米级测量精度的需求。

发明内容

本发明的目的是提出的一种惯性/里程计/全站仪多信息融合方法，它可应用于大型工程轨迹精密测量。

本发明是这样实现的，一种惯性/里程计/全站仪多信息融合方法，它包括如下步骤，

步骤1：惯性/里程计组合导航；

步骤2：惯性/里程计/全站仪多信息融合方法。

所述的步骤1包括如下步骤，

1)状态方程

惯性/里程计组合导航Kalman滤波选取21维状态变量：

X＝[δV_NδV_UδV_Eφ_Nφ_Uφ_EδLδhδλ▽_x▽_y▽_zε_xε_yε_zφ_ayφ_azδK_DR_xR_yR_z]^T

式中，δV_N、δV_U、δV_E表示惯导***北向、天向、东向速度误差，φ_N、φ_U、φ_E：表示惯导***北向、天向、东向失准角，δL、δh、δλ表示惯导***纬度、高度、经度误差，▽_x、▽_y、▽_z表示x轴、y轴、z轴加速度计零位误差，ε_x、ε_y、ε_z表示x轴、y轴、z轴陀螺常值漂移，φ_ay、φ_az表示里程计与惯性测量单元之间残余方位和俯仰安装误差角，δK_D为里程计刻度系数误差，R_x、R_y、R_z表示里程计与惯性测量单元之间的杆臂误差，

对应的状态方程表示如下：

式中，w为***噪声，A为***状态矩阵，根据状态变量，参照捷联惯导***误差方程，可以得到***状态矩阵中各元素的值，

2)量测方程

Kalman滤波量测方程形式如下：

Z＝HX+v

式中，Z表示Kalman滤波观测量，H表示***观测矩阵，v为***量测噪声，

惯性/里程计组合导航Kalman滤波采用速度匹配模式，以惯性导航解算得到的速度和里程计测量的速度之差值作为Kalman滤波器的观测量，即观测量：

Z＝V_SINS-V_Odo

式中，V_SINS表示惯性导航解算得到的速度，V_Odo表示里程计输出速度，

根据观测量可以得到量测矩阵H

式中，v_bx表示里程计测量坐标系x轴速度，

分别表示陀螺测得的载体坐标系x轴、y轴、z轴角速率，

3)滤波计算

根据组合导航***状态方程和量测方程，选择合适的初始状态估计值 X₀、初始估计均方误差阵P₀、***噪声初始方差阵Q₀以及量测噪声方差阵R_k，参照Kalman滤波计算公式，可以实现对各项误差的准确估计，

4)误差修正

利用Kalman滤波计算得到的状态估计值，对惯导***位置误差、速度误差以及里程计刻度系数误差进行修正可以得到检测***的位置、速度及姿态信息，实现高精度惯性空间基准建立。

所述的步骤2包括如下步骤，

1)控制点测量

惯性/里程计组合导航测量轨迹与真实轨迹存在一定偏差，为提高测量精度，在测量过程中增加控制点测量，其中，A点、B点、C点为全站仪测量得到的约束控制点，

2)转换角度计算

假设A点测量坐标为(x₁,y₁,z₁)、B点测量坐标为(x₂,y₂,z₂)、C点测量坐标为 (x₃,y₃,z₃)，A1点、B1点、C1点分别为与A点、B点、C点相对应的惯性/里程计组合导航结果，A1点坐标为(x′₁,y′₁,z′₁)、B1点坐标为(x′₂,y′₂,z′₂)、C1点坐标为 (x′₃,y′₃,z′₃)，所有坐标均为转换到同一坐标系后的值，

根据全站仪测量的A点、B点坐标可以得到水平方向参考角度α_x以及高低方向参考角度α_y，

α_x＝atan((x₂-x₁)/(z₂-z₁))

α_y＝atan((y₂-y₁)/(z₂-z₁))

根据惯性/里程计组合导航得到的A1点、B1点坐标可以得到水平角度α′_x以及高低角度α′_y，

α′_x＝atan((x′₂-x′₁)/(z′₂-z′₂))

α′_y＝atan((y′₂-y′₁)/(z′₂-z′₁))

结合参考角度可以得到水平方向转换角度Δα_x以及高低方向转换角度Δα_y，

Δα_x＝α_x-α'_x

Δα_y＝α_y-α'_y

3)转换矩阵计算

转换角度可以得到水平方向转换矩阵C_x以及高低方向转换矩阵C_y，

4)轨迹融合

根据转换矩阵对惯性/里程计组合导航结果以及全站仪测量坐标进行轨迹融合，

X′＝C_x·X

Y′＝C_Y·Y

式中，X′为融合后的水平方向坐标，X为惯性/里程计组合导航得到的水平方向坐标，Y′为融合后的高低方向坐标，Y为惯性/里程计组合导航得到的高低方向坐标，

利用转换矩阵C_x、C_y对A1B1之间的所有惯性/里程计组合测量结果进行约束可以提高整个区间段的轨迹测量精度，B1C1区间段可以采用相同的方法，利用B点、C点的全站仪测量结果进行约束。

本发明的优点是，全站仪虽然难以实现轨迹连续测量，但是可以对离散点实现亚毫米级高精度测量。因此可以在轨迹测量过程中利用全站仪对惯性/ 里程计组合导航结果进行定点约束，能够有效提高测量精度，实现大型工程轨迹的高精度、连续、动态测量。

附图说明

图1为惯性/里程计/全站仪轨迹测量示意图。

图中，1惯性/里程计组合得到的测量轨迹，2大型工程的真实轨迹，3惯性/里程计/全站仪多信息融合得到的轨迹。

具体实施方式

下面结合附图和具体实施例对本发明进行详细介绍：

本发明提出的惯性/里程计/全站仪多信息融合方法首先进行惯性/里程计组合导航Kalman滤波计算，建立里程计测速误差模型，通过Kalman滤波实现对各项误差的有效估计，再利用全站仪的高精度测量信息对惯性/里程计组合导航的测量轨迹进行线形约束，提高测量精度。

具体的计算方法如下：

步骤1：惯性/里程计组合导航

1)状态方程

惯性/里程计组合导航Kalman滤波选取21维状态变量：

X＝[δV_NδV_UδV_Eφ_Nφ_Uφ_EδLδhδλ▽_x▽_y▽_zε_xε_yε_zφ_ayφ_azδK_DR_xR_yR_z]^T

式中，δV_N、δV_U、δV_E表示惯导***北向、天向、东向速度误差，φ_N、φ_U、φ_E：表示惯导***北向、天向、东向失准角，δL、δh、δλ表示惯导***纬度、高度、经度误差，▽_x、▽_y、▽_z表示x轴、y轴、z轴加速度计零位误差，ε_x、ε_y、ε_z表示x轴、y轴、z轴陀螺常值漂移，φ_ay、φ_az表示里程计与惯性测量单元之间残余方位和俯仰安装误差角，δK_D为里程计刻度系数误差，R_x、R_y、R_z表示里程计与惯性测量单元之间的杆臂误差。

对应的状态方程表示如下：

式中，w为***噪声，A为***状态矩阵，根据状态变量，参照捷联惯导***误差方程，可以得到***状态矩阵中各元素的值。

2)量测方程

Kalman滤波量测方程形式如下：

Z＝HX+v

式中，Z表示Kalman滤波观测量，H表示***观测矩阵，v为***量测噪声。

惯性/里程计组合导航Kalman滤波采用速度匹配模式，以惯性导航解算得到的速度和里程计测量的速度之差值作为Kalman滤波器的观测量。即观测量：

Z＝V_SINS-V_Odo

式中，V_SINS表示惯性导航解算得到的速度，V_Odo表示里程计输出速度。

根据观测量可以得到量测矩阵H

式中，v_bx表示里程计测量坐标系x轴速度，

分别表示陀螺测得的载体坐标系x轴、y轴、z轴角速率。

3)滤波计算

根据组合导航***状态方程和量测方程，选择合适的初始状态估计值 X₀、初始估计均方误差阵P₀、***噪声初始方差阵Q₀以及量测噪声方差阵R_k，参照Kalman滤波计算公式，可以实现对各项误差的准确估计。

4)误差修正

利用Kalman滤波计算得到的状态估计值，对惯导***位置误差、速度误差以及里程计刻度系数误差进行修正可以得到检测***的位置、速度及姿态信息，实现高精度惯性空间基准建立。

步骤2：惯性/里程计/全站仪多信息融合方法

1)控制点测量

参照图1，惯性/里程计组合导航测量轨迹与真实轨迹存在一定偏差，为提高测量精度，在测量过程中增加控制点测量，图1中A点、B点、C点为全站仪测量得到的约束控制点。

2)转换角度计算

假设A点测量坐标为(x₁,y₁,z₁)、B点测量坐标为(x₂,y₂,z₂)、C点测量坐标为 (x₃,y₃,z₃)，A1点、B1点、C1点分别为与A点、B点、C点相对应的惯性/里程计组合导航结果，A1点坐标为(x′₁,y′₁,z′₁)、B1点坐标为(x′₂,y′₂,z′₂)、C1点坐标为 (x′₃,y′₃,z′₃)，所有坐标均为转换到同一坐标系后的值。

根据全站仪测量的A点、B点坐标可以得到水平方向参考角度α_x以及高低方向参考角度α_y。

α_x＝atan((x₂-x₁)/(z₂-z₁))

α_y＝atan((y₂-y₁)/(z₂-z₁))

根据惯性/里程计组合导航得到的A1点、B1点坐标可以得到水平角度α′_x以及高低角度α′_y。

α′_x＝atan((x′₂-x′₁)/(z′₂-z′₂))

α′_y＝atan((y′₂-y′₁)/(z′₂-z′₁))

结合参考角度可以得到水平方向转换角度Δα_x以及高低方向转换角度Δα_y。

Δα_x＝α_x-α'_x

Δα_y＝α_y-α'_y

3)转换矩阵计算

转换角度可以得到水平方向转换矩阵C_x以及高低方向转换矩阵C_y。

4)轨迹融合

根据转换矩阵对惯性/里程计组合导航结果以及全站仪测量坐标进行轨迹融合。

X′＝C_x·X

Y′＝C_Y·Y

式中，X′为融合后的水平方向坐标，X为惯性/里程计组合导航得到的水平方向坐标，Y′为融合后的高低方向坐标，Y为惯性/里程计组合导航得到的高低方向坐标。

利用转换矩阵C_x、C_y对A1B1之间的所有惯性/里程计组合测量结果进行约束可以提高整个区间段的轨迹测量精度，B1C1区间段可以采用相同的方法，利用B点、C点的全站仪测量结果进行约束。

Claims (3)

1.一种惯性/里程计/全站仪多信息融合方法，其特征在于：它包括如下步骤，

步骤1：惯性/里程计组合导航；

步骤2：惯性/里程计/全站仪多信息融合方法。

2.如权利要求1所述的一种惯性/里程计/全站仪多信息融合方法，其特征在于：所述的步骤1包括如下步骤，

1)状态方程

惯性/里程计组合导航Kalman滤波选取21维状态变量：

式中，δV_N、δV_U、δV_E表示惯导***北向、天向、东向速度误差，φ_N、φ_U、φ_E：表示惯导***北向、天向、东向失准角，δL、δh、δλ表示惯导***纬度、高度、经度误差，表示x轴、y轴、z轴加速度计零位误差，ε_x、ε_y、ε_z表示x轴、y轴、z轴陀螺常值漂移，φ_ay、φ_az表示里程计与惯性测量单元之间残余方位和俯仰安装误差角，δK_D为里程计刻度系数误差，R_x、R_y、R_z表示里程计与惯性测量单元之间的杆臂误差，

对应的状态方程表示如下：

式中，w为***噪声，A为***状态矩阵，根据状态变量，参照捷联惯导***误差方程，可以得到***状态矩阵中各元素的值，

2)量测方程

Kalman滤波量测方程形式如下：

Z＝HX+v

式中，Z表示Kalman滤波观测量，H表示***观测矩阵，v为***量测噪声，

惯性/里程计组合导航Kalman滤波采用速度匹配模式，以惯性导航解算得到的速度和里程计测量的速度之差值作为Kalman滤波器的观测量，即观测量：

Z＝V_SINS-V_Odo

式中，V_SINS表示惯性导航解算得到的速度，V_Odo表示里程计输出速度，

根据观测量可以得到量测矩阵H

式中，v_bx表示里程计测量坐标系x轴速度，

分别表示陀螺测得的载体坐标系x轴、y轴、z轴角速率，

3)滤波计算

根据组合导航***状态方程和量测方程，选择合适的初始状态估计值X₀、初始估计均方误差阵P₀、***噪声初始方差阵Q₀以及量测噪声方差阵R_k，参照Kalman滤波计算公式，可以实现对各项误差的准确估计，

4)误差修正

利用Kalman滤波计算得到的状态估计值，对惯导***位置误差、速度误差以及里程计刻度系数误差进行修正可以得到检测***的位置、速度及姿态信息，实现高精度惯性空间基准建立。

3.如权利要求1所述的一种惯性/里程计/全站仪多信息融合方法，其特征在于：所述的步骤2包括如下步骤，

1)控制点测量

惯性/里程计组合导航测量轨迹与真实轨迹存在一定偏差，为提高测量精度，在测量过程中增加控制点测量，其中，A点、B点、C点为全站仪测量得到的约束控制点，

2)转换角度计算

假设A点测量坐标为(x₁,y₁,z₁)、B点测量坐标为(x₂,y₂,z₂)、C点测量坐标为(x₃,y₃,z₃)，A1点、B1点、C1点分别为与A点、B点、C点相对应的惯性/里程计组合导航结果，A1点坐标为(x′₁,y′₁,z′₁)、B1点坐标为(x′₂,y′₂,z′₂)、C1点坐标为(x′₃,y′₃,z′₃)，所有坐标均为转换到同一坐标系后的值，

根据全站仪测量的A点、B点坐标可以得到水平方向参考角度α_x以及高低方向参考角度α_y，

α_x＝atan((x₂-x₁)/(z₂-z₁))

α_y＝atan((y₂-y₁)/(z₂-z₁))

根据惯性/里程计组合导航得到的A1点、B1点坐标可以得到水平角度α′_x以及高低角度α′_y，

α′_x＝atan((x′₂-x′₁)/(z′₂-z′₂))

α′_y＝atan((y′₂-y′₁)/(z′₂-z′₁))

结合参考角度可以得到水平方向转换角度Δα_x以及高低方向转换角度Δα_y，

Δα_x＝α_x-α′_x

Δα_y＝α_y-α′_y

3)转换矩阵计算

转换角度可以得到水平方向转换矩阵C_x以及高低方向转换矩阵C_y，

4)轨迹融合

根据转换矩阵对惯性/里程计组合导航结果以及全站仪测量坐标进行轨迹融合，

X'＝C_x·X

Y'＝C_Y·Y

式中，X'为融合后的水平方向坐标，X为惯性/里程计组合导航得到的水平方向坐标，Y'为融合后的高低方向坐标，Y为惯性/里程计组合导航得到的高低方向坐标，

利用转换矩阵C_x、C_y对A1B1之间的所有惯性/里程计组合测量结果进行约束可以提高整个区间段的轨迹测量精度，B1C1区间段可以采用相同的方法，利用B点、C点的全站仪测量结果进行约束。

Images