CN110645978A - 一种挖掘机用光纤惯导的高精度定位方法 - Google Patents

Abstract

本发明公开了一种挖掘机用光纤惯导的高精度定位方法，利用挖掘机用惯导输出的角速率及加速度信息，识别挖掘机处于运动状态还是挖掘中。包括如下步骤：(1)坐标系定义；(2)卡尔曼滤波状态变量及滤波模型选定；(3)量测方程建立及误差修正；(4)模拟验证试验。其优点是，能够确保光纤惯导可以实时输出高精度位置，保证按挖掘路线进行前进，位置精度可达到10cm/8h。

Description

一种挖掘机用光纤惯导的高精度定位方法

技术领域

本发明属于一种惯性导航定位方法，具体涉及一种隧道用挖掘机光纤惯 导的高精度定位方法。

背景技术

挖掘机在隧道施工时，高精度定位是重要的，必须确定隧道的起点和终 点，同时可从挖掘机当前位置配合挖掘区域地图信息，可以运用几何运算计 算出挖掘机与挖掘区域的位置关系。在施工过程中必须不断地进行监控以确 保按设计的路线掘进。相当长的一段时间以来，我国隧道挖掘机没有使用测 量系统进行连续导向。由于隧道挖掘机使用环境的限制，不可用DGPS进行 差分定位。因此，用什么系统对挖掘机进行定位成为一个难题。

惯性导航技术是一种自主定位技术，不受使用环境条件限制，但由于惯 性器件误差特性，长时间使用会带来较大的定位误差。即使用偏置重复性为 0.01°的光纤陀螺及偏置重复性为50ug的加表，1h位置误差约1nmil。即单 纯的惯性导航技术不能适用隧道应用环境下挖掘机高精度定位需求。

结合隧道挖掘机的工作特点：启动后近似直线前进1m左右开始挖掘作 业，约半小时该地段完成挖掘后，继续前进1m，重复此过程。一般一个完整 的过程会持续8h左右，定位误差要求不超过15cm。通过惯性器件输出识别 挖掘机运动特性，同时结合其运动过程中的速度约束条件，即挖掘机在运动 时只在沿载体系前进方向有速度(X轴)，垂直于该前进速度方向的两个轴向 (Y轴、Z轴)速度为零，利用Y轴、Z轴两个方向的速度作为观测量构建 卡尔曼滤波方程，对惯导误差进行实时闭环修正。判断到挖掘机处于挖掘状 态后，选用X轴、Y轴及Z轴三个方向速度作为观测量构建卡尔曼滤波方程， 对姿态进行实时闭环修正，速度置零，位置采用判断到挖掘时刻前的位置进 行重置，可大幅提高光纤惯导的定位精度，满足隧道用挖掘机高精度定位需 求。

发明内容

本发明的目的是提供一种挖掘机用光纤惯导的高精度定位方法，利用挖 掘机用惯导输出的角速率及加速度信息，识别挖掘机处于运动状态还是挖掘 中。

本发明是这样实现的，一种挖掘机用光纤惯导的高精度定位方法，包 括如下步骤：

(1)坐标系定义；

(2)卡尔曼滤波状态变量及滤波模型选定；

(3)量测方程建立及误差修正；

(4)模拟验证试验。

所述的步骤(1)为n系，“北天东”导航坐标系；b系，“前上右”载 体坐标系。

所述的步骤(2)为共选取15个系统状态：

X＝[δV_n δV_u δV_e δL δh δλ φ_n φ_u φ_e ▽_x ▽_y ▽_z ε_x ε_y ε_z]^T式 中：

δV_n、δV_u、δV_e分别表示光纤惯导系统北向、天向、东向的速度误差； δL、δh、δλ分别表示光纤惯导系统的纬度误差、高度误差、经度误差；φ_n、 φ_u、φ_e分别表示光纤惯导系统导航坐标系内北、天、东三个方向的失准角； ▽_x、▽_y、▽_z：分别表示光纤惯导系统载体坐标系内X、Y、Z三个方向的加 速度计零偏；ε_x、ε_y、ε_z：分别表示光纤惯导系统载体坐标系内X、Y、Z三 个方向的陀螺漂移；

选用经典卡尔曼滤波方程，具体公式如下：

P_k＝[I-K_kH_k]P_k,k-1。

所述的步骤(3)为设惯性信息的采样频率为200Hz，采样到的陀螺仪X轴、Y轴、Z轴输出分别为加速度计X轴、Y轴、Z轴 输出分别为

则可求得1s内合角速度均值

及合加速度均值

采用滑动窗口窗口设为10，即可求取10s内的及

的均值及均方差 值，即

设MoveFlag为是否运动的标志， 置为‘1’时，表示处于运动状态；置为‘0’时表明处于挖掘状态；

式中，ε₁、ε₂、ε₃、ε₄分别角速度均值阈值、角速度方差阈值、加速度均值 阈值、加速度方差阈值，可以根据实验数据求得，

a)挖掘机处于运动状态

取载体系下Y轴、Z轴速度作为量测量，建立量测矩阵，如下：

式中：Z(k)、H(k)分别为量测量及量测矩阵，V_bx、V_by、V_bz分别为载体 系下光纤惯导输出的速度，

为姿态矩阵

利用上述建立的量测量进行卡尔曼滤波，周期为1s，用下式对惯导速 度、位置、姿态进行误差修正，

V_N＝V_N-X(1)

V_U＝V_U-X(2)

V_E＝V_E-X(3)

L＝L-X(3)

h＝h-X(4)

λ＝λ-X(5)

φ＝[X(7) X(8) X(9)]^T

式中，V_N、V_U、V_E为光纤惯导在导航坐标系下速度，L、h、λ为光纤 惯导输出的纬度、高度、经度，完成修正后，将各状态量清零，

b)挖掘机处于挖掘状态

取载体系X轴、Y轴、Z轴速度作为量测量，同时建立量测矩阵，如 下式

判断到挖掘状态后，需要记录此时刻光纤惯导位置输出，后续使用卡 尔曼滤波后，位置输出值需要使用记录时刻的输出值进行重置，同时将速 度置零，姿态则实时修正，修正完成后将各状态量清零。

所述的步骤(4)为验证光纤惯导定位精度采用更高精度的导航系统或 位置参考点进行对标，使用差分GPS精确定标3个位置参考点，精度在2cm 内。

本发明的优点是，能够确保光纤惯导可以实时输出高精度位置，保证按 挖掘路线进行前进，位置精度可达到10cm/8h。

附图说明

图1为路线与各参考点图；

图2为第1条次试验时试验数据图。

具体实施方式

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

一种挖掘机用光纤惯导的高精度定位方法，利用挖掘机用惯导输出的 角速率及加速度信息，识别挖掘机处于运动状态还是挖掘中。当判断到挖 掘机处于运动状态时，利用Y轴、Z轴速度作为观测量构建卡尔曼滤波方 程实时修正惯导速度、位置及姿态误差；处于挖掘状态时，利用X轴、Y 轴及Z轴速度作为观测量构建卡尔曼滤波方程，实时修正姿态误差，速度 置零，并对位置进行重置。位置重置法是本项专利的核心。这样可确保光 纤惯导可以实时输出高精度位置，保证按挖掘路线进行前进，位置精度可 达到10cm/8h。

一种挖掘机用光纤惯导的高精度定位方法，包括如下步骤：

(1)坐标系定义

a)n系，“北天东”导航坐标系；

b)b系，“前上右”载体坐标系；

(2)卡尔曼滤波状态变量及滤波模型选定

共选取15个系统状态：

X＝[δV_n δV_u δV_e δL δh δλ φ_n φ_u φ_e ▽_x ▽_y ▽_z ε_x ε_y ε_z]^T式 中：

δV_n、δV_u、δV_e分别表示光纤惯导系统北向、天向、东向的速度误差；

δL、δh、δλ分别表示光纤惯导系统的纬度误差、高度误差、经度误差；

φ_n、φ_u、φ_e分别表示光纤惯导系统导航坐标系内北、天、东三个方向的 失准角；

▽_x、▽_y、▽_z：分别表示光纤惯导系统载体坐标系内X、Y、Z三个方向 的加速度计零偏；

ε_x、ε_y、ε_z：分别表示光纤惯导系统载体坐标系内X、Y、Z三个方向的 陀螺漂移；

选用经典卡尔曼滤波方程，具体公式如下：

P_k＝[I-K_kH_k]P_k,k-1

(3)量测方程建立及误差修正

根据挖掘机的运动特点，前进1m左右(20s左右)后开始挖掘，挖掘 半小时后继续前进1m，开始挖掘，如此进行重复。可以得知，挖掘机处于 挖掘的时间远大于前进的时间，如何抑制挖掘机在挖掘状态下的位置误差 发散显得尤为重要。如果挖掘机处于挖掘状态后的起始位置是准确的，光 纤惯导就可将位置约束在此精确位置，位置误差不会发散。而挖掘机处于 运动状态后，由于光纤惯导具有短时间高精度定位的优势，并且结合运动 过程中速度约束条件(载体系下垂直于运动方向的另外两轴速度应为0) 可进一步缩小光纤惯导处于运动状态下的误差。因此，我们可以梳理出， 在精确的已知位置输入下，短时间(即运动状态下)可用光纤惯导自身输 出结合速度约束获得处于挖掘状态后挖掘机起始的高精度位置，处于挖掘 状态后，光纤惯导位置输出约束到处于挖掘状态后的起始位置上，可保证 光纤惯导在全程输出高精度的位置参数。

挖掘机没有指示挖掘机处于何种运动状态的指示，因此如何利用光纤 惯导的陀螺仪及加速度计输出判别挖掘机状态是关键点。设惯性信息的采 样频率为200Hz，采样到的陀螺仪X轴、Y轴、Z轴输出分别为

加速度计X轴、Y轴、Z轴输出分别为

则可求得1s 内合角速度均值

及合加速度均值见下式。

采用滑动窗口(窗口设为10)，即可求取10s内的

及

的均值及均 方差值，即

设MoveFlag为是否运动的标 志，置为‘1’时，表示处于运动状态；置为‘0’时表明处于挖掘状态。

式中，ε₁、ε₂、ε₃、ε₄分别角速度均值阈值、角速度方差阈值、加速度 均值阈值、加速度方差阈值，可以根据实验数据求得。

a)挖掘机处于运动状态

取载体系下Y轴、Z轴速度作为量测量，建立量测矩阵，如下：

式中：Z(k)、H(k)分别为量测量及量测矩阵，V_bx、V_by、V_bz分别为载体 系下光纤惯导输出的速度。

为姿态矩阵。

利用上述建立的量测量进行卡尔曼滤波，周期为1s。用下式对惯导速 度、位置、姿态进行误差修正。

V_N＝V_N-X(1)

V_U＝V_U-X(2)

V_E＝V_E-X(3)

L＝L-X(3)

h＝h-X(4)

λ＝λ-X(5)

φ＝[X(7) X(8) X(9)]^T

式中，V_N、V_U、V_E为光纤惯导在导航坐标系下速度，L、h、λ为光纤 惯导输出的纬度、高度、经度。完成修正后，将各状态量清零。

b)挖掘机处于挖掘状态

取载体系X轴、Y轴、Z轴速度作为量测量，同时建立量测矩阵,如下 式

判断到挖掘状态后，需要记录此时刻光纤惯导位置输出。后续使用卡 尔曼滤波后，位置输出值需要使用记录时刻的输出值进行重置。同时将速 度置零，姿态则实时修正。修正完成后将各状态量清零。

(4)模拟验证试验

验证光纤惯导定位精度的一般方法是采用更高精度的导航系统或位置 参考点进行对标。本发明使用差分GPS精确定标3个位置参考点，精度在 2cm内。光纤惯导置于小推车上，使小推车往返于各个位置参考点(在各 点静止时，小推车右后轮中心与参考点近似重合)，用小推车的前进或倒退 模拟挖掘机的运动状态，用小推车的停止模拟挖掘机的挖掘状态。试验共 进行了六个条次，每条次约8h。以第一条次为例，共停靠17点，运动线 路及各停靠点与3个位置参考点关系如下图1所示。6个条次均按照此路 线进行试验。

以第一条次为例，试验运动过程中记录的

见图2，ε₁、ε₂、ε₃、ε₄分别取值为0.005、0.002、0.0014、0.0012，见各图 红色直线部分。

表1六条次试验结果

从表1可以看出，各条次试验误差均小于9cm，达到了10cm/8h位置 误差的要求，满足隧道挖掘机高精度定位要求。

Claims (5)

1.一种挖掘机用光纤惯导的高精度定位方法，其特征在于：包括如下步骤：

(1)坐标系定义；

(2)卡尔曼滤波状态变量及滤波模型选定；

(3)量测方程建立及误差修正；

(4)模拟验证试验。

2.如权利要求1所述的一种挖掘机用光纤惯导的高精度定位方法，其特征在于：所述的步骤(1)为n系，“北天东”导航坐标系；b系，“前上右”载体坐标系。

3.如权利要求1所述的一种挖掘机用光纤惯导的高精度定位方法，其特征在于：所述的步骤(2)为共选取15个系统状态：

式中：

δV_n、δV_u、δV_e分别表示光纤惯导系统北向、天向、东向的速度误差；δL、δh、δλ分别表示光纤惯导系统的纬度误差、高度误差、经度误差；φ_n、φ_u、φ_e分别表示光纤惯导系统导航坐标系内北、天、东三个方向的失准角；分别表示光纤惯导系统载体坐标系内X、Y、Z三个方向的加速度计零偏；ε_x、ε_y、ε_z：分别表示光纤惯导系统载体坐标系内X、Y、Z三个方向的陀螺漂移；

选用经典卡尔曼滤波方程，具体公式如下：

P_k＝[I-K_kH_k]P_k,k-1。

4.如权利要求1所述的一种挖掘机用光纤惯导的高精度定位方法，其特征在于：所述的步骤(3)为设惯性信息的采样频率为200Hz，采样到的陀螺仪X轴、Y轴、Z轴输出分别为

加速度计X轴、Y轴、Z轴输出分别为

则可求得1s内合角速度均值

及合加速度均值

采用滑动窗口窗口设为10，即可求取10s内的

及

的均值及均方差值，即

设MoveFlag为是否运动的标志，置为‘1’时，表示处于运动状态；置为‘0’时表明处于挖掘状态；

式中，ε₁、ε₂、ε₃、ε₄分别角速度均值阈值、角速度方差阈值、加速度均值阈值、加速度方差阈值，可以根据实验数据求得，

a)挖掘机处于运动状态

取载体系下Y轴、Z轴速度作为量测量，建立量测矩阵，如下：

式中：Z(k)、H(k)分别为量测量及量测矩阵，V_bx、V_by、V_bz分别为载体系下光纤惯导输出的速度，

为姿态矩阵

利用上述建立的量测量进行卡尔曼滤波，周期为1s，用下式对惯导速度、位置、姿态进行误差修正，

V_N＝V_N-X(1)

V_U＝V_U-X(2)

V_E＝V_E-X(3)

L＝L-X(3)

h＝h-X(4)

λ＝λ-X(5)

φ＝[X(7) X(8) X(9)]^T

式中，V_N、V_U、V_E为光纤惯导在导航坐标系下速度，L、h、λ为光纤惯导输出的纬度、高度、经度，完成修正后，将各状态量清零，

b)挖掘机处于挖掘状态

取载体系X轴、Y轴、Z轴速度作为量测量，同时建立量测矩阵，如下式

判断到挖掘状态后，需要记录此时刻光纤惯导位置输出，后续使用卡尔曼滤波后，位置输出值需要使用记录时刻的输出值进行重置，同时将速度置零，姿态则实时修正，修正完成后将各状态量清零。

5.如权利要求1所述的一种挖掘机用光纤惯导的高精度定位方法，其特征在于：所述的步骤(4)为验证光纤惯导定位精度采用更高精度的导航系统或位置参考点进行对标，使用差分GPS精确定标3个位置参考点，精度在2cm内。

Images