CN109974697B - 一种基于惯性***的高精度测绘方法 - Google Patents

一种基于惯性***的高精度测绘方法 Download PDF

Info

Publication number
CN109974697B
CN109974697B CN201910215758.XA CN201910215758A CN109974697B CN 109974697 B CN109974697 B CN 109974697B CN 201910215758 A CN201910215758 A CN 201910215758A CN 109974697 B CN109974697 B CN 109974697B
Authority
CN
China
Prior art keywords
speed
filtering
unit
odometer
calculated
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.)
Active
Application number
CN201910215758.XA
Other languages
English (en)
Other versions
CN109974697A (zh
Inventor
侯斌
屈建龙
赵云旭
王得山
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
707th Research Institute of CSIC
Original Assignee
707th Research Institute of CSIC
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by 707th Research Institute of CSIC filed Critical 707th Research Institute of CSIC
Priority to CN201910215758.XA priority Critical patent/CN109974697B/zh
Publication of CN109974697A publication Critical patent/CN109974697A/zh
Application granted granted Critical
Publication of CN109974697B publication Critical patent/CN109974697B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于惯性***的高精度测绘方法，以三轴陀螺仪和三轴加速度计组成捷联惯性导航***，进行惯导解算；利用里程计输出的位置增量进行航位推算，输出姿态航向、速度和位置信息；建立惯导捷联解算和里程计航位推算误差的数学模型，采用卡尔曼滤波方法对里程计和惯导误差进行估计和测量；利用管道外预先测量指定等间距点磁标点的位置信息进行数据校正，并利用RTS滤波技术进行回溯滤波，使每个采样点都为卡尔曼滤波最优平滑点。本发明利用后处理采集的惯性器件和里程计数据，结合外部精准的磁标点信号，通过组合导航算法，给出特定测量点的位置信息和整条被测管道的路径的位置轨迹，满足石油管道、隧道、铁路等领域的高精度测绘的要求。

Description

一种基于惯性***的高精度测绘方法
技术领域
本发明属于测绘技术后处理技术领域，涉及利用惯性元件对石油惯导、隧道等环境进行位置的精确测绘技术，尤其是一种基于惯性***的高精度测绘方法。
背景技术
在石油管道、隧道、铁路等使用环境中，基于安全原因，例如石油管道需要定期对管道完整性进行检测，并定位可能的安全隐患点或者需要测量公路隧道和铁轨在整个路径下形变情况，此时北斗或GPS由于精度和可见度的问题，并不能满足厘米级的定位精度。
发明内容
本发明的目的在于克服现有技术的不足之处，提供一种基于惯性***的高精度测绘方法，解决现有GPS或北斗在信号不可见的情况下无法进行测绘和精度低的问题。
本发明解决技术问题所采用的技术方案是：
一种基于惯性***的高精度测绘方法，步骤如下：
(1)以三轴陀螺仪和三轴加速度计组成捷联惯性导航***，进行惯导解算；
(2)利用里程计输出的位置增量进行航位推算，输出姿态航向、速度和位置信息；
(3)建立惯导捷联解算和里程计航位推算误差的数学模型，采用卡尔曼滤波方法对里程计和惯导误差进行估计和测量；
(4)利用管道外预先测量指定等间距点磁标点的位置信息进行数据校正，并利用RTS滤波技术进行回溯滤波，使每个采样点都为卡尔曼滤波最优平滑点。
而且，步骤(1)所述的惯导解算流程为：
记初始姿态角对应的方向余弦矩阵
Figure BDA0002002027160000011
对应的姿态四元数记为
Figure BDA0002002027160000012
将
Figure BDA0002002027160000013
转成姿态四元数
Figure BDA0002002027160000014
形式：
Figure BDA0002002027160000021
k＝0，1，2，…时，利用四元数更新算法计算tk+1时刻的姿态四元数
Figure BDA0002002027160000022
Figure BDA0002002027160000023
其中
Figure BDA0002002027160000024
Figure BDA0002002027160000025
Δθ＝[Δθx Δθy Δθz]T为(tk，tk+1]采样周期内的陀螺输出角增量，单位：rad；
Figure BDA0002002027160000026
为(tk，tk+1]更新周期内n系相对于i系转过的角度
Figure BDA0002002027160000027
Figure BDA0002002027160000028
为e系相对于i系的自转角速率在n系的投影，
Figure BDA0002002027160000029
为n系相对于e系的角速率在n系上的投影，由以下两式计算：
Figure BDA00020020271600000210
Figure BDA00020020271600000211
式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果；最后进行四元数归一化处理
Figure BDA00020020271600000212
得到更新后的姿态四元数
Figure BDA00020020271600000213
加速度计输出的速度增量Δv经由坐标变换得地理坐标系上的速度分量
Figure BDA00020020271600000214
其中，Δv＝[Δvx Δvy Δvz]T为加速度计输出的速度增量；
地理坐标系上的加速度分量为
fn＝Δvn/h
其中，记fn＝[fE fN fU]T；
速度更新初始时刻，vn(0)＝[0 0 0]T；
k＝0，1，2，…时，在一个速度更新周期(tk，tk+1]内，速度更新为
Figure BDA0002002027160000031
其中，vE、vN、vU表示***k时刻东向、北向和天向速度，h表示***采样时间Δvn为***加速度矢量，δA为有害加速度，计算如下：
Figure BDA0002002027160000032
初始时刻，L(0)、λ(0)和S(0)为装订的纬度、经度和高程；
k＝0，1，2，…时，在一个速度更新周期(tk，tk+1]内，位置更新为
Figure BDA0002002027160000033
其中，vE、vN、vU表示***k时刻东向、北向和天向速度，RN为沿卯酉圈的曲率半径，RE为沿子午圈的曲率半径，S表示***的高度，单位为米。
而且，步骤(2)所述的航位推算方程为：
Figure BDA0002002027160000034
Figure BDA0002002027160000035
其中
Figure BDA0002002027160000041
代表里程计测量的***东向、北向和天向的速度，hD为里程计航位推算的高度，TD为***解算周期，
Figure BDA0002002027160000042
为里程计输出的***位置增量。
而且，步骤(3)所述的滤波方法包括如下步骤：
1)Kalman滤波一步预测；
分为①状态转移阵Φk,k-1的计算，②输入噪声方差阵
Figure BDA0002002027160000043
的计算，③状态预测
Figure BDA0002002027160000044
与误差方差预测Pk,k-1的计算三个阶段：
①状态转移阵Φk,k-1的计算
记(tk-1，tk]为一个预测周期，h＝tk－tk-1，预测周期h一般较短，状态转移阵计算如下
Figure BDA0002002027160000045
Figure BDA0002002027160000046
其中
Figure BDA0002002027160000047
Figure BDA0002002027160000048
Figure BDA0002002027160000049
Figure BDA0002002027160000051
Figure BDA0002002027160000052
Figure BDA0002002027160000053
Figure BDA0002002027160000054
Figure BDA0002002027160000055
Figure BDA0002002027160000056
矩阵中各项元素的变量均可由比力坐标变换、速度更新、位置更新计算而得，其中，
Yawd为里程计航向角安装误差，单位：rad；
dSm为里程计输出的位置增量，有dSm＝KodΔN，单位:m；
ΔN表示单位时间内里程计脉冲增量，单位：pluse；
Kod为里程计的标度因数，单位：pluse/m；
dSn为里程计输出的位置增量在n系的投影，单位：m；
L为纬度，单位：rad；；
vE为东向速度，单位：m/s；
vN为北向速度，单位：m/s；
vU为垂向速度，单位：m/s；
RE为子午面垂直的法线平面的曲率半径，单位：m；
RN为子午面上的曲率半径，单位：m；
fE为地理坐标系东向加速度，单位：m/s2；
fN为地理坐标系北向加速度，单位：m/s2；
fU为地理坐标系垂向加速度，单位：m/s2；
ωie为地球自转角速度，ωie＝7.292115×10-5rad/s；
②输入噪声方差阵
Figure BDA0002002027160000061
的计算
连续***的***噪声即三个陀螺和三个加速度计向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为
Qq＝G(t)Q(t)GT(t))
其中，Q(t)为常量，G(t)为噪声输入矩阵，重写如下：
Q＝diag[(0.1°/h)2 (0.1°/h)2 (0.1°/h)2 (0.05mg)2 (0.05mg)2 (0.05mg)2]
Figure BDA0002002027160000062
得到连续***的输入噪声方差Qq后计算Kalman滤波的输入噪声方差
Figure BDA0002002027160000069
如下：
Figure BDA0002002027160000063
③状态预测
Figure BDA0002002027160000064
与误差方差预测Pk,k-1的计算
初始时刻当k＝0时，对
Figure BDA0002002027160000065
和P0进行初始化；
k＝0，1，2，…时，递推计算
Figure BDA0002002027160000066
Figure BDA0002002027160000067
当滤波更新周期未到时，不进行滤波更新，继续进行预测，令
Figure BDA0002002027160000068
Pk＝Pk,k-1
当滤波更新周期到时，
Figure BDA0002002027160000071
Pk按滤波更新计算；
2)Kalman滤波更新；
3)量测计算；
按下式计算量测值：
Figure BDA0002002027160000072
下标s表示捷联惯导***解算输出，下标r表示参考基准输出，
vsE、vsN、vsU为解算东向速度、北向速度、垂向速度，单位：m/s；
Ls、λs为解算纬度、经度，单位：rad；
Ss为解算高度，单位：m；
vrE、vrN、vrU为参考速度，精对准状态下为0；
Lr、λr，Sr为等待命令状态装订纬度、经度，单位rad；
4)滤波增益计算；
按下式计算滤波增益Kk：
Figure BDA0002002027160000073
其中，
Pk,k-1为误差方差预测计算得；
Hk＝[06×3 I6×6 06×10]为***状态观测方程
Figure BDA0002002027160000077
为***外参考的速度和位置噪声协方差，
5)状态估计更新；
按下式计算状态估计
Figure BDA0002002027160000074
Figure BDA0002002027160000075
其中，
Figure BDA0002002027160000076
为状态预测计算得，
6)误差方差更新；
按下式计算误差协方差阵Pk：
Figure BDA0002002027160000081
7)Kalman滤波修正，
Kalman滤波对失准角进行估计，并实时修正姿态阵
Figure BDA0002002027160000082
后得到比较精确的姿态阵，
利用Kalman滤波技术估计的失准角φE、φN、φU，修正角即为
Figure BDA0002002027160000083
采用四元数法修正为
Figure BDA0002002027160000084
其中
Figure BDA0002002027160000085
Figure BDA0002002027160000086
速度位置的修正则采用速度、位置滤波输出值替代即可，其中φE、φN、φU为kalman滤波后的结果，对应***的东向、北向和天向姿态角误差，
Figure BDA0002002027160000087
为修正后***方向余弦矩阵，
Figure BDA0002002027160000088
修正前的***方向余弦矩阵，φc×为φc的数学上的叉乘矩阵。
而且，步骤7所述的Kalman滤波修正包括姿态四元数修正、速度修正、位置修正、加速度计零偏修正、陀螺漂移修正；
所述的姿态四元数修正：
tk时刻，利用Kalman滤波估计得到失准角φE、φN、φU，对应
Figure BDA0002002027160000089
的第1、2、3个元素，修正角记为φc＝[φE φN φU]T，采用四元数法修正tk时刻的姿态四元数
Figure BDA00020020271600000810
Figure BDA00020020271600000811
其中，
Figure BDA00020020271600000812
所述的速度修正：
tk时刻，利用Kalman滤波估计得到速度误差量δvE、δvN、δvU，对应
Figure BDA00020020271600000813
的第4、5、6个元素，修正tk时刻的解算速度：
Figure BDA0002002027160000091
所述的位置修正：
tk时刻，利用Kalman滤波估计得到位置误差量δL、δλ、δS，对应
Figure BDA0002002027160000092
的第7、8、9个元素，修正tk时刻的解算位置：
Figure BDA0002002027160000093
所述的陀螺漂移修正：
利用Kalman滤波估计得到陀螺漂移
Figure BDA0002002027160000094
对应
Figure BDA0002002027160000095
的第10、11、12个元素，修正陀螺零偏误差：
Figure BDA0002002027160000096
并将新的陀螺零偏误差保存到存储器中，留待下次开机使用，
所述的加速度计零偏修正：
利用Kalman滤波估计得到加速度计零偏
Figure BDA0002002027160000097
对应
Figure BDA0002002027160000098
的第13、14、15个元素，修正加速度计零位：
Figure BDA0002002027160000099
并将新的加速度计零位误差保存到存储器中，留待下次开机使用，其中Kai(i＝x,y,z)表示的装订的***三个方向上的标度因数，Nai0(i＝x,y,z)表示装订的***三个方向上的加速度计零位，
在组合导航过程中，对***姿态角误差、里程计误差和陀螺、加速度计零偏进行估计补偿，同时保存滤波过程中的中间变量，包括Xk，Pk，Xk,k-1，Pk,k-1，Φk,k-1为下一步RTS滤波作数据存储，Xk，Pk，Xk,k-1，Pk,k-1分别表示***k时刻***状态量和协方差以及k-1时刻预测k时刻***状态量和协方差。
而且，步骤(4)所述的RTS滤波公式如下：
Figure BDA0002002027160000101
Figure BDA0002002027160000102
Figure BDA0002002027160000103
其中
Figure BDA0002002027160000104
Pf,k，
Figure BDA0002002027160000105
Pf,k+1,k分别为上一部分存储的Xk，Pk，Xk,k-1，Pk,k-1，
Figure BDA0002002027160000106
Ps,k，
Figure BDA0002002027160000107
Ps,k+1分别表示经过RTS平滑后***k时刻***的状态量和协方差以及k+1时刻预测k时刻RTS平滑后***状态量和协方差，Ks,k为平滑***增益矩阵。
本发明的优点和积极效果是：
本发明利用后处理采集的惯性器件和里程计数据，结合外部精准的磁标点信号，通过组合导航算法，给出特定测量点的位置信息和整条被测管道的路径的位置轨迹，满足石油管道、隧道、铁路等领域的高精度测绘的要求。
具体实施方式
下面通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。
以石油管道测绘过程实施为例，测绘***主要包括三个光纤陀螺和三个加速度计组成的惯性导航***和里程计测速装置，在管道中采用里程计来测量***在管道中运动的里程增量，同时在管道外每间隔1km～2km选取1个参考点利用高精度GPS测量该点的精确位置信息作为标校位置点，同时为了检验测绘***的精度，测绘***搭载惯性导航***沿着管道前行，经过参考位置点时会与惯导***进行同步录数，同时储存导航***陀螺和加速计的输出的数据、里程计数据和参考位置点的数据和时戳。待测绘***在管道中完成整个测绘过程后，将存储在***的数据读取到PC端，运行测绘后处理程序输出高精度测绘结果。在管道外每间隔1km～2km选取1个参考点作为参考位置点，用来检验该测绘方法的精度，具体检验方法为：将后处理的输出的位置精度与参考位置点比较，两者之差可认为测绘***的测量误差。
后处理程序是基于前面所描述的高精度处理方法步骤编写的，包括以下步骤：
①以三轴陀螺仪和三轴加速度计组成捷联惯性导航***，进行惯导解算；
②利用里程计输出的位置增量进行航位推算，输出姿态航向、速度和位置信息；
③建立惯导捷联解算和里程计航位推算误差的数学模型，采用卡尔曼滤波方法对里程计
和惯导误差进行估计和测量；
④利用管道外预先测量指定等间距点磁标点的位置信息进行数据校正，并利用RTS滤波
技术进行回溯滤波，使每个采样点都为卡尔曼滤波最优平滑点。
以某一次石油管道测绘实验为例，经过高精度测绘后处理方法得到的结果与参考的标准店比较的精度结果如下：
项目 测量误差(均值) 测量误差(方差)
高度精度 0.5m 1.14m
水平定位精度(东向、北向) 3.8m 2.36m
以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的普通技术人员来说，在不脱离发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (5)

1.一种基于惯性***的高精度测绘方法，其特征在于：步骤如下：
(1)以三轴陀螺仪和三轴加速度计组成捷联惯性导航***，进行惯导解算；
(2)利用里程计输出的位置增量进行航位推算，输出姿态航向、速度和位置信息；
(3)建立惯导捷联解算和里程计航位推算误差的数学模型，采用卡尔曼滤波方法对里程计和惯导误差进行估计和测量；
(4)利用管道外预先测量指定等间距点磁标点的位置信息进行数据校正，并利用RTS滤波技术进行回溯滤波，使每个采样点都为卡尔曼滤波最优平滑点；
其中，步骤(3)所述的滤波方法包括如下步骤：
1)Kalman滤波一步预测；
分为①状态转移阵Φk,k-1的计算，②输入噪声方差阵
Figure FDA0003644556100000011
的计算，③状态预测
Figure FDA0003644556100000012
与误差方差预测Pk,k-1的计算三个阶段：
①状态转移阵Φk,k-1的计算
记(tk-1，tk]为一个预测周期，h＝tk－tk-1，预测周期h较短，状态转移阵计算如下
Figure FDA0003644556100000013
Figure FDA0003644556100000014
其中
Figure FDA0003644556100000015
Figure FDA0003644556100000021
Figure FDA0003644556100000022
Figure FDA0003644556100000023
Figure FDA0003644556100000024
Figure FDA0003644556100000025
Figure FDA0003644556100000026
Figure FDA0003644556100000027
Figure FDA0003644556100000028
矩阵中各项元素的变量均可由比力坐标变换、速度更新、位置更新计算而得，其中，
Yawd为里程计航向角安装误差，单位：rad；
dSm为里程计输出的位置增量，有dSm＝KodΔN，单位:m；
ΔN表示单位时间内里程计脉冲增量，单位：pluse；
Kod为里程计的标度因数，单位：pluse/m；
dSn为里程计输出的位置增量在n系的投影，单位：m；
L为纬度，单位：rad；
vE为东向速度，单位：m/s；
vN为北向速度，单位：m/s；
vU为垂向速度，单位：m/s；
RE为子午面垂直的法线平面的曲率半径，单位：m；
RN为子午面上的曲率半径，单位：m；
fE为地理坐标系东向加速度，单位：m/s2；
fN为地理坐标系北向加速度，单位：m/s2；
fU为地理坐标系垂向加速度，单位：m/s2；
ωie为地球自转角速度，ωie＝7.292115×10-5rad/s；
②输入噪声方差阵
Figure FDA0003644556100000031
的计算
连续***的***噪声即三个陀螺和三个加速度计向量W(t)的协方差阵为Q(t)，则输入噪声的方差阵为
Qq＝G(t)Q(t)GT(t))
其中，Q(t)为常量，G(t)为噪声输入矩阵，重写如下：
Figure FDA0003644556100000032
Figure FDA0003644556100000033
得到连续***的输入噪声方差Qq后计算Kalman滤波的输入噪声方差
Figure FDA0003644556100000041
如下：
Figure FDA0003644556100000042
③状态预测
Figure FDA0003644556100000043
与误差方差预测Pk,k-1的计算
初始时刻当k＝0时，对
Figure FDA0003644556100000044
和P0进行初始化；
k＝0，1，2，…时，递推计算
Figure FDA0003644556100000045
Figure FDA0003644556100000046
当滤波更新周期未到时，不进行滤波更新，继续进行预测，令
Figure FDA0003644556100000047
Pk＝Pk,k-1
当滤波更新周期到时，
Figure FDA0003644556100000048
Pk按滤波更新计算；
2)Kalman滤波更新；
3)量测计算；
按下式计算量测值：
Figure FDA0003644556100000049
下标s表示捷联惯导***解算输出，下标r表示参考基准输出，
vsE、vsN、vsU为解算东向速度、北向速度、垂向速度，单位：m/s；
Ls、λs为解算纬度、经度，单位：rad；
Ss为解算高度，单位：m；
vrE、vrN、vrU为参考速度，精对准状态下为0；
Lr、λr，Sr为等待命令状态装订纬度、经度、高度 ，单位rad；
4)滤波增益计算；
按下式计算滤波增益Kk：
Figure FDA0003644556100000051
其中，
Pk,k-1为误差方差预测计算得；
Hk＝[06×3 I6×6 06×10]为***状态观测方程
Figure FDA0003644556100000052
为***外参考的速度和位置噪声协方差，
5)状态估计更新；
按下式计算状态估计
Figure FDA0003644556100000053
Figure FDA0003644556100000054
其中，
Figure FDA0003644556100000055
为状态预测计算得，
6)误差方差更新；
按下式计算误差协方差阵Pk：
Figure FDA0003644556100000056
7)Kalman滤波修正，
Kalman滤波对失准角进行估计，并实时修正姿态阵
Figure FDA0003644556100000057
后得到比较精确的姿态阵，
利用Kalman滤波技术估计的失准角φE、φN、φU，修正角即为
Figure FDA0003644556100000058
采用四元数法修正为
Figure FDA0003644556100000059
其中
Figure FDA00036445561000000510
Figure FDA0003644556100000061
速度位置的修正则采用速度、位置滤波输出值替代即可，其中φE、φN、φU为kalman滤波后的结果，对应***的东向、北向和天向姿态角误差，
Figure FDA0003644556100000062
为修正后***方向余弦矩阵，
Figure FDA0003644556100000063
为修正前的***方向余弦矩阵，φc×为φc的数学上的叉乘矩阵。
2.根据权利要求1所述的基于惯性***的高精度测绘方法，其特征在于：步骤(1)所述的惯导解算流程为：
记初始姿态角对应的方向余弦矩阵
Figure FDA0003644556100000064
对应的姿态四元数记为
Figure FDA0003644556100000065
将
Figure FDA0003644556100000066
转成姿态四元数
Figure FDA0003644556100000067
形式：
Figure FDA0003644556100000068
k＝0，1，2，…时，利用四元数更新算法计算tk+1时刻的姿态四元数
Figure FDA0003644556100000069
Figure DEST_PATH_IMAGE002
其中
Figure FDA00036445561000000611
Figure FDA00036445561000000612
Δθ＝[Δθx Δθy Δθz]T为(tk，tk+1]采样周期内的陀螺输出角增量，单位：rad；
Figure FDA00036445561000000613
为(tk，tk+1]更新周期内n系相对于i系转过的角度
Figure FDA00036445561000000614
Figure FDA0003644556100000071
为e系相对于i系的自转角速率在n系的投影，
Figure FDA0003644556100000072
为n系相对于e系的角速率在n系上的投影，由以下两式计算：
Figure FDA0003644556100000073
Figure FDA0003644556100000074
式中的速度、纬度和曲率半径均是上一导航解算更新周期的结果；最后进行四元数归一化处理
Figure FDA0003644556100000075
得到更新后的姿态四元数
Figure FDA0003644556100000076
加速度计输出的速度增量Δv经由坐标变换得地理坐标系上的速度分量
Figure FDA0003644556100000077
其中，Δv＝[Δvx Δvy Δvz]T为加速度计输出的速度增量；
地理坐标系上的加速度分量为
fn＝Δvn/h
其中，记fn＝[fE fN fU]T；
速度更新初始时刻，vn(0)＝[0 0 0]T；
k＝0，1，2，…时，在一个速度更新周期(tk，tk+1]内，速度更新为
Figure FDA0003644556100000078
其中，vE、vN、vU表示***k时刻东向、北向和天向速度，h表示***采样时间，Δvn为***加速度矢量，δA为有害加速度，计算如下：
Figure FDA0003644556100000079
初始时刻，L(0)、λ(0)和S(0)为装订的纬度、经度和高程；
k＝0，1，2，…时，在一个速度更新周期(tk，tk+1]内，位置更新为
Figure FDA0003644556100000081
其中，vE、vN、vU表示***k时刻东向、北向和天向速度，RN为沿卯酉圈的曲率半径，RE为沿子午圈的曲率半径，S表示***的高度，单位为米。
3.根据权利要求1所述的基于惯性***的高精度测绘方法，其特征在于：步骤(2)所述的航位推算方程为：
Figure FDA0003644556100000082
Figure FDA0003644556100000083
其中
Figure FDA0003644556100000084
代表里程计测量的***东向、北向和天向的速度，hD为里程计航位推算的高度，TD为***解算周期，
Figure FDA0003644556100000085
为里程计输出的***位置增量。
4.根据权利要求1所述的基于惯性***的高精度测绘方法，其特征在于：步骤7) 所述的Kalman滤波修正包括姿态四元数修正、速度修正、位置修正、加速度计零偏修正、陀螺漂移修正；
所述的姿态四元数修正：
tk时刻，利用Kalman滤波估计得到失准角φE、φN、φU，对应
Figure FDA0003644556100000086
的第1、2、3 个元素，修正角记为φc＝[φE φN φU]T，采用四元数法修正tk时刻的姿态四元数
Figure FDA0003644556100000091
Figure FDA0003644556100000092
其中，
Figure FDA0003644556100000093
所述的速度修正：
tk时刻，利用Kalman滤波估计得到速度误差量δvE、δvN、δvU，对应
Figure FDA0003644556100000094
的第4、5、6个元素，修正tk时刻的解算速度：
Figure FDA0003644556100000095
所述的位置修正：
tk时刻，利用Kalman滤波估计得到位置误差量δL、δλ、δS，对应
Figure FDA0003644556100000096
的第7、8、9个元素，修正tk时刻的解算位置：
Figure FDA0003644556100000097
所述的陀螺漂移修正：
利用Kalman滤波估计得到陀螺漂移
Figure FDA0003644556100000098
对应
Figure FDA0003644556100000099
的第10、11、12个元素，修正陀螺零偏误差：
Figure FDA00036445561000000910
并将新的陀螺零偏误差保存到存储器中，留待下次开机使用，
所述的加速度计零偏修正：
利用Kalman滤波估计得到加速度计零偏
Figure FDA00036445561000000911
对应
Figure FDA00036445561000000912
的第13、14、15个元素，修正加速度计零位：
Figure FDA0003644556100000101
并将新的加速度计零位误差保存到存储器中，留待下次开机使用，其中Kai(i＝x,y,z)表示的装订的***三个方向上的标度因数，Nai0(i＝x,y,z)表示装订的***三个方向上的加速度计零位，
在组合导航过程中，对***姿态角误差、里程计误差和陀螺、加速度计零偏进行估计补偿，同时保存滤波过程中的中间变量，包括Xk，Pk，Xk,k-1，Pk,k-1，Φk,k-1为下一步RTS滤波作数据存储，Xk，Pk，Xk,k-1，Pk,k-1分别表示***k时刻***状态量和协方差以及k-1时刻预测k时刻***状态量和协方差。
5.根据权利要求1所述的基于惯性***的高精度测绘方法，其特征在于：步骤(4)所述的RTS滤波公式如下：
Figure FDA0003644556100000102
Figure FDA0003644556100000103
Figure FDA0003644556100000104
其中
Figure FDA0003644556100000105
Pf,k，
Figure FDA0003644556100000106
Pf,k+1,k分别为上一部分存储的Xk，Pk，Xk,k-1，Pk,k-1，
Figure FDA0003644556100000107
Ps,k，
Figure FDA0003644556100000108
Ps,k+1分别表示经过RTS平滑后***k时刻***的状态量和协方差以及k+1时刻预测k时刻RTS平滑后***状态量和协方差，Ks,k为平滑***增益矩阵。
CN201910215758.XA 2019-03-21 2019-03-21 一种基于惯性***的高精度测绘方法 Active CN109974697B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910215758.XA CN109974697B (zh) 2019-03-21 2019-03-21 一种基于惯性***的高精度测绘方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910215758.XA CN109974697B (zh) 2019-03-21 2019-03-21 一种基于惯性***的高精度测绘方法

Publications (2)

Publication Number Publication Date
CN109974697A CN109974697A (zh) 2019-07-05
CN109974697B true CN109974697B (zh) 2022-07-26

Family

ID=67079763

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910215758.XA Active CN109974697B (zh) 2019-03-21 2019-03-21 一种基于惯性***的高精度测绘方法

Country Status (1)

Country Link
CN (1) CN109974697B (zh)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110296701B (zh) * 2019-07-09 2022-12-13 哈尔滨工程大学 惯性与卫星组合导航***渐变型故障回溯容错方法
CN110763872A (zh) * 2019-11-21 2020-02-07 中国船舶重工集团公司第七0七研究所 一种多普勒测速仪多参数在线标定方法
CN111156994B (zh) * 2019-12-31 2023-10-27 上海星思半导体有限责任公司 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN111207744B (zh) * 2020-01-15 2023-03-21 哈尔滨工程大学 一种基于厚尾鲁棒滤波的管线地理位置信息测量方法
CN111256727B (zh) * 2020-02-19 2022-03-08 广州蓝胖子机器人有限公司 一种基于Augmented EKF的提高里程计精度的方法
CN111169201B (zh) * 2020-03-04 2024-03-26 黑龙江大学 练字监测器及监测方法
CN112066983B (zh) * 2020-09-08 2022-09-23 中国人民解放军国防科技大学 惯性/里程计组合导航滤波方法、电子设备及存储介质
CN112284412B (zh) * 2020-09-09 2022-11-11 上海航天控制技术研究所 一种避免欧拉转换奇异导致精度下降的地面静态对准方法
CN112284415B (zh) * 2020-10-19 2023-01-03 株洲菲斯罗克光电科技股份有限公司 里程计标度误差标定方法、***及计算机存储介质
CN113267156B (zh) * 2021-04-13 2022-10-14 深圳大学 一种利用惯导测量水泥地坪平整度的方法及测量***
CN113252048B (zh) * 2021-05-12 2023-02-28 高新兴物联科技股份有限公司 一种导航定位方法、导航定位***及计算机可读存储介质
CN113483755B (zh) * 2021-07-09 2023-11-07 北京易航远智科技有限公司 一种基于非全局一致地图的多传感器组合定位方法及***
CN113670297A (zh) * 2021-08-23 2021-11-19 上海宇航***工程研究所 一种基于mems和轮式里程计的离线定位方法
CN114608570B (zh) * 2022-02-25 2023-06-30 电子科技大学 一种多模式自切换的管线仪自适应精密定位方法
CN115127547B (zh) * 2022-06-27 2024-04-19 长安大学 一种基于捷联惯导***和图像定位的隧道检测车定位方法
CN117214933B (zh) * 2023-11-07 2024-02-06 中国船舶集团有限公司第七〇七研究所 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103727938A (zh) * 2013-10-28 2014-04-16 北京自动化控制设备研究所 一种管道测绘用惯导里程计组合导航方法
CN103743414A (zh) * 2014-01-02 2014-04-23 东南大学 一种里程计辅助车载捷联惯导***行进间初始对准方法
CN106507913B (zh) * 2010-09-25 2014-10-22 北京自动化控制设备研究所 用于管道测绘的组合定位方法
CN104949687A (zh) * 2014-03-31 2015-09-30 北京自动化控制设备研究所 一种长时间导航***全参数精度评估方法
CN105318876A (zh) * 2014-07-09 2016-02-10 北京自动化控制设备研究所 一种惯性里程计组合高精度姿态测量方法
CN108088443A (zh) * 2016-11-23 2018-05-29 北京自动化控制设备研究所 一种定位定向设备速度补偿方法
CN108106635A (zh) * 2017-12-15 2018-06-01 中国船舶重工集团公司第七0七研究所 惯性卫导组合导航***的长航时抗干扰姿态航向校准方法
CN108180925A (zh) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN108303079A (zh) * 2017-12-21 2018-07-20 中国船舶重工集团公司第七0七研究所 一种水下usbl反向应用的数据平滑方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10533856B2 (en) * 2017-04-05 2020-01-14 Novatel Inc. Navigation system utilizing yaw rate constraint during inertial dead reckoning

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106507913B (zh) * 2010-09-25 2014-10-22 北京自动化控制设备研究所 用于管道测绘的组合定位方法
CN103727938A (zh) * 2013-10-28 2014-04-16 北京自动化控制设备研究所 一种管道测绘用惯导里程计组合导航方法
CN103743414A (zh) * 2014-01-02 2014-04-23 东南大学 一种里程计辅助车载捷联惯导***行进间初始对准方法
CN104949687A (zh) * 2014-03-31 2015-09-30 北京自动化控制设备研究所 一种长时间导航***全参数精度评估方法
CN105318876A (zh) * 2014-07-09 2016-02-10 北京自动化控制设备研究所 一种惯性里程计组合高精度姿态测量方法
CN108088443A (zh) * 2016-11-23 2018-05-29 北京自动化控制设备研究所 一种定位定向设备速度补偿方法
CN108106635A (zh) * 2017-12-15 2018-06-01 中国船舶重工集团公司第七0七研究所 惯性卫导组合导航***的长航时抗干扰姿态航向校准方法
CN108180925A (zh) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN108303079A (zh) * 2017-12-21 2018-07-20 中国船舶重工集团公司第七0七研究所 一种水下usbl反向应用的数据平滑方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Dead-reckoning sensor system and tracking algorithm for 3-D pipeline mapping;Dongjun Hyun等;《Mechatronics》;20091130;第213-223页 *
采用MEMS惯导的小口径管道内检测定位方案可行性研究;牛小骥等;《传感技术学报》;20160131;第29卷(第1期);第40-44页 *

Also Published As

Publication number Publication date
CN109974697A (zh) 2019-07-05

Similar Documents

Publication Publication Date Title
CN109974697B (zh) 一种基于惯性***的高精度测绘方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN102169184B (zh) 组合导航***中测量双天线gps安装失准角的方法和装置
CN103900565B (zh) 一种基于差分gps的惯导***姿态获取方法
CN100516775C (zh) 一种捷联惯性导航***初始姿态确定方法
CN106595652B (zh) 车辆运动学约束辅助的回溯式行进间对准方法
CN103245359B (zh) 一种惯性导航***中惯性传感器固定误差实时标定方法
CN110631574B (zh) 一种惯性/里程计/rtk多信息融合方法
CN111207744B (zh) 一种基于厚尾鲁棒滤波的管线地理位置信息测量方法
CN105091907B (zh) Sins/dvl组合中dvl方位安装误差估计方法
CN107270893A (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
Wu Versatile land navigation using inertial sensors and odometry: Self-calibration, in-motion alignment and positioning
CN109870173A (zh) 一种基于校验点的海底管道惯性导航***的轨迹修正方法
CN103727938B (zh) 一种管道测绘用惯导里程计组合导航方法
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN103364817B (zh) 一种基于r-t-s平滑的pos***双捷联解算后处理方法
CN112284415B (zh) 里程计标度误差标定方法、***及计算机存储介质
Santana et al. Estimation of trajectories of pipeline PIGs using inertial measurements and non linear sensor fusion
CN107677292B (zh) 基于重力场模型的垂线偏差补偿方法
CN114739425A (zh) 基于rtk-gnss及全站仪的采煤机定位标定***及应用方法
CN109470276B (zh) 基于零速修正的里程计标定方法与装置
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN109084755B (zh) 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN107764268B (zh) 一种机载分布式pos传递对准的方法和装置

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
GR01 Patent grant
GR01 Patent grant