CN110645978A - 一种挖掘机用光纤惯导的高精度定位方法 - Google Patents
一种挖掘机用光纤惯导的高精度定位方法 Download PDFInfo
- Publication number
- CN110645978A CN110645978A CN201810670711.8A CN201810670711A CN110645978A CN 110645978 A CN110645978 A CN 110645978A CN 201810670711 A CN201810670711 A CN 201810670711A CN 110645978 A CN110645978 A CN 110645978A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- excavator
- optical fiber
- fiber inertial
- axis
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
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=[δVn δVu δVe δL δh δλ φn φu φe ▽x ▽y ▽z εx εy εz]T式 中:
δVn、δVu、δVe分别表示光纤惯导***北向、天向、东向的速度误差; δL、δh、δλ分别表示光纤惯导***的纬度误差、高度误差、经度误差;φn、 φu、φe分别表示光纤惯导***导航坐标系内北、天、东三个方向的失准角; ▽x、▽y、▽z:分别表示光纤惯导***载体坐标系内X、Y、Z三个方向的加 速度计零偏;εx、εy、εz:分别表示光纤惯导***载体坐标系内X、Y、Z三 个方向的陀螺漂移;
选用经典卡尔曼滤波方程,具体公式如下:
Pk=[I-KkHk]Pk,k-1。
式中,ε1、ε2、ε3、ε4分别角速度均值阈值、角速度方差阈值、加速度均值 阈值、加速度方差阈值,可以根据实验数据求得,
a)挖掘机处于运动状态
取载体系下Y轴、Z轴速度作为量测量,建立量测矩阵,如下:
式中:Z(k)、H(k)分别为量测量及量测矩阵,Vbx、Vby、Vbz分别为载体 系下光纤惯导输出的速度,
利用上述建立的量测量进行卡尔曼滤波,周期为1s,用下式对惯导速 度、位置、姿态进行误差修正,
VN=VN-X(1)
VU=VU-X(2)
VE=VE-X(3)
L=L-X(3)
h=h-X(4)
λ=λ-X(5)
φ=[X(7) X(8) X(9)]T
式中,VN、VU、VE为光纤惯导在导航坐标系下速度,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=[δVn δVu δVe δL δh δλ φn φu φe ▽x ▽y ▽z εx εy εz]T式 中:
δVn、δVu、δVe分别表示光纤惯导***北向、天向、东向的速度误差;
δL、δh、δλ分别表示光纤惯导***的纬度误差、高度误差、经度误差;
φn、φu、φe分别表示光纤惯导***导航坐标系内北、天、东三个方向的 失准角;
▽x、▽y、▽z:分别表示光纤惯导***载体坐标系内X、Y、Z三个方向 的加速度计零偏;
εx、εy、εz:分别表示光纤惯导***载体坐标系内X、Y、Z三个方向的 陀螺漂移;
选用经典卡尔曼滤波方程,具体公式如下:
Pk=[I-KkHk]Pk,k-1
(3)量测方程建立及误差修正
根据挖掘机的运动特点,前进1m左右(20s左右)后开始挖掘,挖掘 半小时后继续前进1m,开始挖掘,如此进行重复。可以得知,挖掘机处于 挖掘的时间远大于前进的时间,如何抑制挖掘机在挖掘状态下的位置误差 发散显得尤为重要。如果挖掘机处于挖掘状态后的起始位置是准确的,光 纤惯导就可将位置约束在此精确位置,位置误差不会发散。而挖掘机处于 运动状态后,由于光纤惯导具有短时间高精度定位的优势,并且结合运动 过程中速度约束条件(载体系下垂直于运动方向的另外两轴速度应为0) 可进一步缩小光纤惯导处于运动状态下的误差。因此,我们可以梳理出, 在精确的已知位置输入下,短时间(即运动状态下)可用光纤惯导自身输 出结合速度约束获得处于挖掘状态后挖掘机起始的高精度位置,处于挖掘 状态后,光纤惯导位置输出约束到处于挖掘状态后的起始位置上,可保证 光纤惯导在全程输出高精度的位置参数。
挖掘机没有指示挖掘机处于何种运动状态的指示,因此如何利用光纤 惯导的陀螺仪及加速度计输出判别挖掘机状态是关键点。设惯性信息的采 样频率为200Hz,采样到的陀螺仪X轴、Y轴、Z轴输出分别为 加速度计X轴、Y轴、Z轴输出分别为则可求得1s 内合角速度均值及合加速度均值见下式。
式中,ε1、ε2、ε3、ε4分别角速度均值阈值、角速度方差阈值、加速度 均值阈值、加速度方差阈值,可以根据实验数据求得。
a)挖掘机处于运动状态
取载体系下Y轴、Z轴速度作为量测量,建立量测矩阵,如下:
式中:Z(k)、H(k)分别为量测量及量测矩阵,Vbx、Vby、Vbz分别为载体 系下光纤惯导输出的速度。
利用上述建立的量测量进行卡尔曼滤波,周期为1s。用下式对惯导速 度、位置、姿态进行误差修正。
VN=VN-X(1)
VU=VU-X(2)
VE=VE-X(3)
L=L-X(3)
h=h-X(4)
λ=λ-X(5)
φ=[X(7) X(8) X(9)]T
式中,VN、VU、VE为光纤惯导在导航坐标系下速度,L、h、λ为光纤 惯导输出的纬度、高度、经度。完成修正后,将各状态量清零。
b)挖掘机处于挖掘状态
取载体系X轴、Y轴、Z轴速度作为量测量,同时建立量测矩阵,如下 式
判断到挖掘状态后,需要记录此时刻光纤惯导位置输出。后续使用卡 尔曼滤波后,位置输出值需要使用记录时刻的输出值进行重置。同时将速 度置零,姿态则实时修正。修正完成后将各状态量清零。
(4)模拟验证试验
验证光纤惯导定位精度的一般方法是采用更高精度的导航***或位置 参考点进行对标。本发明使用差分GPS精确定标3个位置参考点,精度在 2cm内。光纤惯导置于小推车上,使小推车往返于各个位置参考点(在各 点静止时,小推车右后轮中心与参考点近似重合),用小推车的前进或倒退 模拟挖掘机的运动状态,用小推车的停止模拟挖掘机的挖掘状态。试验共 进行了六个条次,每条次约8h。以第一条次为例,共停靠17点,运动线 路及各停靠点与3个位置参考点关系如下图1所示。6个条次均按照此路 线进行试验。
表1六条次试验结果
从表1可以看出,各条次试验误差均小于9cm,达到了10cm/8h位置 误差的要求,满足隧道挖掘机高精度定位要求。
Claims (5)
1.一种挖掘机用光纤惯导的高精度定位方法,其特征在于:包括如下步骤:
(1)坐标系定义;
(2)卡尔曼滤波状态变量及滤波模型选定;
(3)量测方程建立及误差修正;
(4)模拟验证试验。
2.如权利要求1所述的一种挖掘机用光纤惯导的高精度定位方法,其特征在于:所述的步骤(1)为n系,“北天东”导航坐标系;b系,“前上右”载体坐标系。
4.如权利要求1所述的一种挖掘机用光纤惯导的高精度定位方法,其特征在于:所述的步骤(3)为设惯性信息的采样频率为200Hz,采样到的陀螺仪X轴、Y轴、Z轴输出分别为加速度计X轴、Y轴、Z轴输出分别为则可求得1s内合角速度均值及合加速度均值
式中,ε1、ε2、ε3、ε4分别角速度均值阈值、角速度方差阈值、加速度均值阈值、加速度方差阈值,可以根据实验数据求得,
a)挖掘机处于运动状态
取载体系下Y轴、Z轴速度作为量测量,建立量测矩阵,如下:
式中:Z(k)、H(k)分别为量测量及量测矩阵,Vbx、Vby、Vbz分别为载体系下光纤惯导输出的速度,
利用上述建立的量测量进行卡尔曼滤波,周期为1s,用下式对惯导速度、位置、姿态进行误差修正,
VN=VN-X(1)
VU=VU-X(2)
VE=VE-X(3)
L=L-X(3)
h=h-X(4)
λ=λ-X(5)
φ=[X(7) X(8) X(9)]T
式中,VN、VU、VE为光纤惯导在导航坐标系下速度,L、h、λ为光纤惯导输出的纬度、高度、经度,完成修正后,将各状态量清零,
b)挖掘机处于挖掘状态
取载体系X轴、Y轴、Z轴速度作为量测量,同时建立量测矩阵,如下式
判断到挖掘状态后,需要记录此时刻光纤惯导位置输出,后续使用卡尔曼滤波后,位置输出值需要使用记录时刻的输出值进行重置,同时将速度置零,姿态则实时修正,修正完成后将各状态量清零。
5.如权利要求1所述的一种挖掘机用光纤惯导的高精度定位方法,其特征在于:所述的步骤(4)为验证光纤惯导定位精度采用更高精度的导航***或位置参考点进行对标,使用差分GPS精确定标3个位置参考点,精度在2cm内。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810670711.8A CN110645978A (zh) | 2018-06-26 | 2018-06-26 | 一种挖掘机用光纤惯导的高精度定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810670711.8A CN110645978A (zh) | 2018-06-26 | 2018-06-26 | 一种挖掘机用光纤惯导的高精度定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN110645978A true CN110645978A (zh) | 2020-01-03 |
Family
ID=68988566
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810670711.8A Pending CN110645978A (zh) | 2018-06-26 | 2018-06-26 | 一种挖掘机用光纤惯导的高精度定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110645978A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112797978A (zh) * | 2021-04-14 | 2021-05-14 | 上海力信测量***有限公司 | 一种掘进机的导向方法、***和存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101970763A (zh) * | 2008-01-08 | 2011-02-09 | 易斯麦私人有限公司 | 确定电采掘机铲的空间位姿的实时方法 |
CN104713554A (zh) * | 2015-02-01 | 2015-06-17 | 北京工业大学 | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 |
US20160091317A1 (en) * | 2014-09-29 | 2016-03-31 | Caterpillar Inc. | Navigation system and method for machine |
CN205918424U (zh) * | 2015-08-04 | 2017-02-01 | 联邦科学和工业研究组织 | 采矿机器 |
CN106500727A (zh) * | 2016-10-21 | 2017-03-15 | 北京自动化控制设备研究所 | 一种基于路径匹配的惯导***误差修正方法 |
-
2018
- 2018-06-26 CN CN201810670711.8A patent/CN110645978A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101970763A (zh) * | 2008-01-08 | 2011-02-09 | 易斯麦私人有限公司 | 确定电采掘机铲的空间位姿的实时方法 |
US20160091317A1 (en) * | 2014-09-29 | 2016-03-31 | Caterpillar Inc. | Navigation system and method for machine |
CN104713554A (zh) * | 2015-02-01 | 2015-06-17 | 北京工业大学 | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 |
CN205918424U (zh) * | 2015-08-04 | 2017-02-01 | 联邦科学和工业研究组织 | 采矿机器 |
CN106500727A (zh) * | 2016-10-21 | 2017-03-15 | 北京自动化控制设备研究所 | 一种基于路径匹配的惯导***误差修正方法 |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112797978A (zh) * | 2021-04-14 | 2021-05-14 | 上海力信测量***有限公司 | 一种掘进机的导向方法、***和存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102865881B (zh) | 一种惯性测量单元的快速标定方法 | |
CN102706366B (zh) | 一种基于地球自转角速率约束的sins初始对准方法 | |
CN107655493B (zh) | 一种光纤陀螺sins六位置***级标定方法 | |
CN105091907B (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN106969783B (zh) | 一种基于光纤陀螺惯性导航的单轴旋转快速标定技术 | |
CN100516775C (zh) | 一种捷联惯性导航***初始姿态确定方法 | |
CN104344836B (zh) | 一种基于姿态观测的冗余惯导***光纤陀螺***级标定方法 | |
CN104181574A (zh) | 一种捷联惯导***/全球导航卫星***组合导航滤波***及方法 | |
CN104764463B (zh) | 一种惯性平台调平瞄准误差的自检测方法 | |
CN107479076B (zh) | 一种动基座下联合滤波初始对准方法 | |
CN105571578B (zh) | 一种利用伪观测取代精密转台的原地旋转调制寻北方法 | |
CN111024075B (zh) | 一种结合蓝牙信标和地图的行人导航误差修正滤波方法 | |
CN109870173A (zh) | 一种基于校验点的海底管道惯性导航***的轨迹修正方法 | |
CN105371844A (zh) | 一种基于惯性/天文互助的惯性导航***初始化方法 | |
CN104344837A (zh) | 一种基于速度观测的冗余惯导***加速度计***级标定方法 | |
CN103557864A (zh) | Mems捷联惯导自适应sckf滤波的初始对准方法 | |
CN104977002A (zh) | 基于sins/双od的惯性组合导航***及其导航方法 | |
CN105352502A (zh) | 一种微惯性航姿参考***的姿态获取方法 | |
CN116067394A (zh) | 一种***性调制惯导***误差的方法及终端 | |
CN111735474A (zh) | 一种基于数据回溯的动基座罗经对准方法 | |
CN109959374A (zh) | 一种行人惯性导航全时全程逆向平滑滤波方法 | |
CN110672095A (zh) | 一种基于微惯导的行人室内自主定位算法 | |
CN103674064A (zh) | 捷联惯性导航***的初始标定方法 | |
CN105988129A (zh) | 一种基于标量估计算法的ins/gnss组合导航方法 | |
CN103901459B (zh) | 一种mems/gps组合导航***中量测滞后的滤波方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20200103 |
|
RJ01 | Rejection of invention patent application after publication |