[go: up one dir, main page]

CN114184209A - Inertial Error Suppression Method for Low-speed Inspection Platform System - Google Patents

Inertial Error Suppression Method for Low-speed Inspection Platform System Download PDF

Info

Publication number
CN114184209A
CN114184209A CN202111268698.1A CN202111268698A CN114184209A CN 114184209 A CN114184209 A CN 114184209A CN 202111268698 A CN202111268698 A CN 202111268698A CN 114184209 A CN114184209 A CN 114184209A
Authority
CN
China
Prior art keywords
odometer
error
inertial
coordinate system
carrier coordinate
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.)
Granted
Application number
CN202111268698.1A
Other languages
Chinese (zh)
Other versions
CN114184209B (en
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.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
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 Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN202111268698.1A priority Critical patent/CN114184209B/en
Publication of CN114184209A publication Critical patent/CN114184209A/en
Application granted granted Critical
Publication of CN114184209B publication Critical patent/CN114184209B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

The invention provides an inertial error suppression method for a low-speed detection platform system, which comprises the following steps: acquiring the mileage output by the odometer under a carrier coordinate system; acquiring the position increment of the odometer under a carrier coordinate system; acquiring an observation matrix according to system observation measurement; acquiring a system state transition matrix; estimating a state variable through a Kalman filtering algorithm, and correcting the installation error of the odometer based on the estimated state variable; and acquiring a lateral accumulated position error caused by the drift of the gyroscope after the error of the odometer is corrected, acquiring a drift equivalent angular velocity of the gyroscope based on the lateral accumulated position error caused by the drift of the gyroscope, and finishing the correction of the inertial error of the track detection platform system based on the drift equivalent angular velocity of the gyroscope. By applying the technical scheme of the invention, the technical problem that the course error of the inertial navigation system cannot be deeply corrected by directly using the station positioning result as the observed quantity in the prior art is solved.

Description

用于低速检测平台系统的惯性误差抑制方法Inertial Error Suppression Method for Low-speed Inspection Platform System

技术领域technical field

本发明涉及惯性轨道检测技术领域，尤其涉及一种用于低速检测平台系统的惯性误差抑制方法。The invention relates to the technical field of inertial track detection, in particular to an inertial error suppression method for a low-speed detection platform system.

背景技术Background technique

在轨道精调过程中，利用全站仪和CPIII点进行精确定位仍是目前绝对测量的主要手段。利用全站仪可以获得单点亚毫米级精度位置信息，实际测量时一般每60m～120m设一个站点，站点之间的测量精度则通过惯性组合导航系统来实现。差分卫星接收机的精度容易受到多种因素干扰，这些干扰产生的误差量级与低速检测平台工作距离相比比例较大，多数情况下无法忽略，因此低速检测平台一般采用惯性/里程计组合测量全站仪站点间的轨道参数，并利用站点处的高精度定位信息修正惯性/里程计组合的累计误差。但由于站点的稀疏性，无法直接利用站点定位结果作为观测量对惯导系统的航向误差进行更深入的修正，导致修正后的测量轨迹会出现驻波状轨迹误差，站点输出误差为0。In the process of orbit fine-tuning, the use of total station and CPIII point for precise positioning is still the main means of absolute measurement at present. The position information of a single point with sub-millimeter precision can be obtained by using a total station. In actual measurement, a station is generally set every 60m to 120m, and the measurement accuracy between stations is achieved by an inertial integrated navigation system. The accuracy of the differential satellite receiver is easily interfered by various factors. The magnitude of the error generated by these interferences is relatively large compared with the working distance of the low-speed detection platform, which cannot be ignored in most cases. Therefore, the low-speed detection platform generally adopts the combined inertial/odometer measurement. The orbital parameters between the total stations and the high-precision positioning information at the stations are used to correct the accumulated error of the inertial/odometer combination. However, due to the sparseness of the station, it is impossible to directly use the station positioning result as the observation value to further correct the heading error of the inertial navigation system, resulting in a standing wave-like trajectory error in the corrected measurement trajectory, and the station output error is 0.

发明内容SUMMARY OF THE INVENTION

本发明提供了一种用于低速检测平台系统的惯性误差抑制方法，能够解决现有技术中无法直接利用站点定位结果作为观测量对惯导系统的航向误差进行更深入的修正的技术问题。The invention provides an inertial error suppression method for a low-speed detection platform system, which can solve the technical problem in the prior art that the station positioning result cannot be directly used as an observation quantity to further correct the heading error of the inertial navigation system.

本发明提供了一种用于低速检测平台系统的惯性误差抑制方法，惯性误差抑制方法包括：获取里程计在载体坐标系下输出的里程；基于里程计在载体坐标系下输出的里程计算获取里程计在载体坐标系下的位置增量；将惯性导航系统在载体坐标系下的位置增量与里程计在载体坐标系下的位置增量的差值作为系统观测量，根据系统观测量获取观测矩阵；获取系统状态转移矩阵；基于系统观测矩阵和系统状态转移矩阵，通过卡尔曼滤波算法，对状态变量进行估计，基于估计后的状态变量对里程计安装误差进行修正；基于修正后的里程计安装误差，获取修正完里程计误差后由于陀螺漂移引起的侧向累计位置误差，基于由于陀螺漂移引起的侧向累计位置误差获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。The invention provides an inertial error suppression method for a low-speed detection platform system. The inertial error suppression method includes: obtaining the mileage output by the odometer in the carrier coordinate system; calculating and obtaining the mileage based on the mileage output by the odometer in the carrier coordinate system Calculate the position increment in the carrier coordinate system; take the difference between the position increment of the inertial navigation system in the carrier coordinate system and the position increment of the odometer in the carrier coordinate system as the system observation amount, and obtain the observation according to the system observation amount Matrix; obtain the system state transition matrix; based on the system observation matrix and the system state transition matrix, the state variables are estimated by the Kalman filter algorithm, and the odometer installation error is corrected based on the estimated state variables; based on the corrected odometer Installation error, obtain the lateral cumulative position error caused by gyro drift after correcting the odometer error, obtain the equivalent angular velocity of gyro drift based on the lateral cumulative position error caused by gyro drift, and complete the track detection platform based on the equivalent angular velocity of gyro drift Inertial error correction of the system.

进一步地，在完成对轨道检测平台系统的惯性误差修正之后，惯性误差抑制方法还包括：将修正后的轨道检测平台系统的惯性误差与设定惯性误差精度阈值范围进行比较，当修正后的轨道检测平台系统的惯性误差超出设定惯性误差精度阈值范围时，重复上述步骤，直至修正后的轨道检测平台系统的惯性误差处于设定惯性误差精度阈值范围内。Further, after completing the inertia error correction of the orbit detection platform system, the inertia error suppression method further includes: comparing the inertia error of the corrected orbit detection platform system with the set inertia error accuracy threshold range, when the corrected orbit When the inertial error of the detection platform system exceeds the set inertial error precision threshold range, repeat the above steps until the corrected inertial error of the orbit detection platform system is within the set inertial error precision threshold range.

进一步地，里程计在载体坐标系下输出的里程可根据

Figure BDA0003327401220000021
来获取，其中，
Figure BDA0003327401220000022
为里程计在k时刻在载体坐标系下输出的里程，
Figure BDA0003327401220000023
为里程计与惯性导航系统之间的安装关系矩阵，KD为里程计刻度系数，
Figure BDA0003327401220000024
为里程计在里程计坐标系下的脉冲数矢量形式，Nk为里程计在第k个采样周期内输出的脉冲数。Further, the mileage output by the odometer in the carrier coordinate system can be based on
Figure BDA0003327401220000021
to obtain, of which,
Figure BDA0003327401220000022
is the mileage output by the odometer in the carrier coordinate system at time k,
Figure BDA0003327401220000023
is the installation relationship matrix between the odometer and the inertial navigation system, K D is the odometer scale coefficient,
Figure BDA0003327401220000024
is the pulse number vector form of the odometer in the odometer coordinate system, and N k is the number of pulses output by the odometer in the kth sampling period.

进一步地，里程计在载体坐标系下的位置增量可根据

Figure BDA0003327401220000031
来获取，其中，
Figure BDA0003327401220000032
为里程计在载体坐标系下的位置增量，δαθ为俯仰角误差，δαψ为航向角误差，δKD为里程计刻度系数误差，
Figure BDA0003327401220000033
为里程计在k时刻在载体坐标系下沿x轴输出的里程，
Figure BDA0003327401220000034
为里程计在k时刻在载体坐标系下沿y轴输出的里程，
Figure BDA0003327401220000035
为里程计在k时刻在载体坐标系下沿z轴输出的里程，X为状态变量。Further, the position increment of the odometer in the carrier coordinate system can be determined according to
Figure BDA0003327401220000031
to obtain, of which,
Figure BDA0003327401220000032
is the position increment of the odometer in the carrier coordinate system, δα θ is the pitch angle error, δα ψ is the heading angle error, δK D is the odometer scale coefficient error,
Figure BDA0003327401220000033
is the mileage output by the odometer along the x-axis in the carrier coordinate system at time k,
Figure BDA0003327401220000034
is the mileage output by the odometer along the y-axis in the carrier coordinate system at time k,
Figure BDA0003327401220000035
is the mileage output by the odometer along the z-axis in the carrier coordinate system at time k, and X is the state variable.

进一步地，系统观测量可根据

Figure BDA0003327401220000036
获取，其中，
Figure BDA0003327401220000037
为惯性导航系统在载体坐标系下的位置增量，Hk为观测矩阵，
Figure BDA0003327401220000038
Further, the system observations can be based on
Figure BDA0003327401220000036
get, where,
Figure BDA0003327401220000037
is the position increment of the inertial navigation system in the carrier coordinate system, H k is the observation matrix,
Figure BDA0003327401220000038

进一步地，惯性导航系统在载体坐标系下的位置增量

Figure BDA00033274012200000313
可根据
Figure BDA0003327401220000039
来获取，其中，
Figure BDA00033274012200000310
为惯性导航系统在导航坐标系下的位置增量，
Figure BDA00033274012200000311
为k时刻惯性导航系统在导航坐标系下的速度，
Figure BDA00033274012200000312
为k+1时刻惯性导航系统在导航坐标系下的速度，Ts为计算周期。Further, the position increment of the inertial navigation system in the carrier coordinate system
Figure BDA00033274012200000313
according to
Figure BDA0003327401220000039
to obtain, of which,
Figure BDA00033274012200000310
is the position increment of the inertial navigation system in the navigation coordinate system,
Figure BDA00033274012200000311
is the speed of the inertial navigation system in the navigation coordinate system at time k,
Figure BDA00033274012200000312
is the speed of the inertial navigation system in the navigation coordinate system at time k+1, and T s is the calculation period.

进一步地，基于估计后的状态变量对里程计安装误差进行修正具体包括：基于估计后的状态变量对惯性导航系统与里程计之间的安装关系矩阵以及里程计刻度系数进行修正以完成对里程计安装误差进行修正。Further, correcting the installation error of the odometer based on the estimated state variable specifically includes: correcting the installation relationship matrix between the inertial navigation system and the odometer and the odometer scale coefficient based on the estimated state variable to complete the correction of the odometer. Installation errors are corrected.

进一步地，惯性导航系统与里程计之间的安装关系矩阵可根据

Figure BDA0003327401220000041
进行修正，里程计刻度系数可根据KD,k+1＝(1+δKD,k)KD,k进行修正，其中，
Figure BDA0003327401220000042
为k+1时刻里程计与惯性导航系统之间的安装关系矩阵，
Figure BDA0003327401220000043
为k时刻里程计与惯性导航系统之间的安装关系矩阵，KD,k+1为k+1时刻里程计刻度系数，KD,k为k时刻里程计刻度系数。Further, the installation relationship matrix between the inertial navigation system and the odometer can be based on
Figure BDA0003327401220000041
Correction, the odometer scale coefficient can be corrected according to K D,k+1 =(1+δK D,k )K D,k , where,
Figure BDA0003327401220000042
is the installation relationship matrix between the odometer and the inertial navigation system at time k+1,
Figure BDA0003327401220000043
is the installation relationship matrix between the odometer and the inertial navigation system at time k, K D,k+1 is the calibration coefficient of the odometer at time k+1, and K D,k is the calibration coefficient of the odometer at time k.

进一步地，陀螺漂移等效角速度可根据

Figure BDA0003327401220000044
来获取，其中，Δx(t)为由于陀螺漂移引起的侧向累计位置误差，ω为陀螺漂移等效角速率，t为从上一个站点出发后经过的时间，v为当前数据段的平均推行速度，τ为任一时刻。Further, the equivalent angular velocity of gyro drift can be calculated according to
Figure BDA0003327401220000044
to obtain, where Δx(t) is the lateral cumulative position error due to gyro drift, ω is the equivalent angular rate of gyro drift, t is the elapsed time from the previous station, and v is the average push of the current data segment speed, τ is any moment.

应用本发明的技术方案，提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法通过获取观测矩阵和系统状态转移矩阵，基于卡尔曼滤波算法，对状态变量进行估计，并根据估计后的状态变量对里程计安装误差进行修正；在对里程计安装误差进行修正后，可通过站点处位置误差反向推导航向误差，即获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。此种方式能够显著降低全站仪定点修正引起的驻波状误差，相比传统惯性/里程计组合导航算法对惯导系统的误差的抑制效果更加显著。By applying the technical solution of the present invention, an inertial error suppression method for a low-speed detection platform system is provided. The inertial error suppression method obtains an observation matrix and a system state transition matrix, and estimates the state variables based on a Kalman filter algorithm. And the odometer installation error is corrected according to the estimated state variables; after the odometer installation error is corrected, the navigation error can be reversely pushed through the position error at the site, that is, the equivalent angular velocity of the gyro drift is obtained, based on the gyro drift, etc. The effective angular velocity is used to complete the inertia error correction of the orbit detection platform system. This method can significantly reduce the standing wave-like error caused by the fixed-point correction of the total station, and is more effective in suppressing the error of the inertial navigation system than the traditional inertial/odometer integrated navigation algorithm.

附图说明Description of drawings

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。The accompanying drawings, which are included to provide a further understanding of the embodiments of the invention, constitute a part of the specification, are used to illustrate the embodiments of the invention, and together with the description, serve to explain the principles of the invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and for those of ordinary skill in the art, other drawings can also be obtained from these drawings without creative effort.

图1示出了根据本发明的具体实施例提供的两条曲线的航向差值变化的示意图。FIG. 1 shows a schematic diagram of changes in heading difference between two curves provided according to a specific embodiment of the present invention.

具体实施方式Detailed ways

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。It should be noted that the embodiments in the present application and the features of the embodiments may be combined with each other in the case of no conflict. The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, but not all of the embodiments. The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the invention, its application, or uses. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。It should be noted that the terminology used herein is for the purpose of describing specific embodiments only, and is not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly dictates otherwise, the singular is intended to include the plural as well, furthermore, it is to be understood that when the terms "comprising" and/or "including" are used in this specification, it indicates that There are features, steps, operations, devices, components, and/or combinations thereof.

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。The relative arrangement of the components and steps, the numerical expressions and numerical values set forth in these embodiments do not limit the scope of the invention unless specifically stated otherwise. Meanwhile, it should be understood that, for the convenience of description, the dimensions of various parts shown in the accompanying drawings are not drawn in an actual proportional relationship. Techniques, methods, and devices known to those of ordinary skill in the relevant art may not be discussed in detail, but where appropriate, such techniques, methods, and devices should be considered part of the authorized description. In all examples shown and discussed herein, any specific value should be construed as illustrative only and not as limiting. Accordingly, other examples of exemplary embodiments may have different values. It should be noted that like numerals and letters refer to like items in the following figures, so once an item is defined in one figure, it does not require further discussion in subsequent figures.

根据本发明的具体实施例，提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法包括：获取里程计在载体坐标系下输出的里程；基于里程计在载体坐标系下输出的里程计算获取里程计在载体坐标系下的位置增量；将惯性导航系统在载体坐标系下的位置增量与里程计在载体坐标系下的位置增量的差值作为系统观测量，根据系统观测量获取观测矩阵；获取系统状态转移矩阵；基于系统观测矩阵和系统状态转移矩阵，通过卡尔曼滤波算法，对状态变量进行估计，基于估计后的状态变量对里程计安装误差进行修正；基于修正后的里程计安装误差，获取修正完里程计误差后由于陀螺漂移引起的侧向累计位置误差，基于由于陀螺漂移引起的侧向累计位置误差获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。According to a specific embodiment of the present invention, an inertial error suppression method for a low-speed detection platform system is provided. The inertial error suppression method includes: obtaining the mileage output by the odometer in the carrier coordinate system; The odometer output from the next output is calculated to obtain the position increment of the odometer in the carrier coordinate system; the difference between the position increment of the inertial navigation system in the carrier coordinate system and the position increment of the odometer in the carrier coordinate system is used as the system observation value , obtain the observation matrix according to the system observation quantity; obtain the system state transition matrix; based on the system observation matrix and the system state transition matrix, the state variables are estimated by the Kalman filter algorithm, and the odometer installation error is corrected based on the estimated state variables ;Based on the corrected odometer installation error, obtain the lateral cumulative position error caused by gyro drift after correcting the odometer error, obtain the equivalent angular velocity of gyro drift based on the lateral cumulative position error caused by gyro drift, based on gyro drift, etc. The effective angular velocity is used to complete the inertia error correction of the orbit detection platform system.

应用此种配置方式，提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法通过获取观测矩阵和系统状态转移矩阵，基于卡尔曼滤波算法，对状态变量进行估计，并根据估计后的状态变量对里程计安装误差进行修正；在对里程计安装误差进行修正后，可通过站点处位置误差反向推导航向误差，即获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。此种方式能够显著降低全站仪定点修正引起的驻波状误差，相比传统惯性/里程计组合导航算法对惯导系统的误差的抑制效果更加显著。其中，在本发明中，低速检测平台一般是指速度小于或等于2m/s的检测平台。By applying this configuration, an inertial error suppression method for a low-speed detection platform system is provided. The inertial error suppression method obtains the observation matrix and the system state transition matrix, and estimates the state variables based on the Kalman filter algorithm. The odometer installation error is corrected according to the estimated state variables; after the odometer installation error is corrected, the navigation and heading error can be reversely pushed through the position error at the site, that is, the equivalent angular velocity of gyro drift can be obtained. The angular velocity completes the inertia error correction for the orbit detection platform system. This method can significantly reduce the standing wave-like error caused by the fixed-point correction of the total station, which is more effective in suppressing the error of the inertial navigation system than the traditional inertial/odometer integrated navigation algorithm. Wherein, in the present invention, the low-speed detection platform generally refers to a detection platform with a speed less than or equal to 2m/s.

在本发明中，为了进一步地提高估计精度，在完成对轨道检测平台系统的惯性误差修正之后，惯性误差抑制方法还包括：将修正后的轨道检测平台系统的惯性误差与设定惯性误差精度阈值范围进行比较，当修正后的轨道检测平台系统的惯性误差超出设定惯性误差精度阈值范围时，重复上述步骤，直至修正后的轨道检测平台系统的惯性误差处于设定惯性误差精度阈值范围内。在此种配置方式下，通过对每次修正后的惯性误差进行评估，当不满足所设定的精度要求时，可以进行迭代计算的方式，进一步地提高估计精度。In the present invention, in order to further improve the estimation accuracy, after the inertia error correction of the orbit detection platform system is completed, the inertia error suppression method further includes: comparing the corrected inertia error of the orbit detection platform system with the set inertia error accuracy threshold When the inertial error of the corrected orbit detection platform system exceeds the set inertial error precision threshold range, repeat the above steps until the corrected inertial error of the orbit detection platform system is within the set inertial error precision threshold range. In this configuration, by evaluating the inertial error after each correction, when the set accuracy requirement is not met, an iterative calculation method can be performed to further improve the estimation accuracy.

在本发明中，为了实现用于轨道检测平台系统的惯性误差抑制，首先需要获取里程计在载体坐标系下输出的里程。作为本发明的一个具体实施例，低速轨道检测系统包括惯性导航系统和里程计，通过夹紧装置使得低速轨道检测系统与轨道紧密接触，因此侧向速度和垂向速度始终为0，前向速度通过采集里程计输出可以获得。里程计在载体坐标系下输出的里程可根据

Figure BDA0003327401220000071
来获取，其中，
Figure BDA0003327401220000072
为里程计在k时刻在载体坐标系下输出的里程，
Figure BDA0003327401220000073
为里程计与惯性导航系统之间的安装关系矩阵，KD为里程计刻度系数，
Figure BDA0003327401220000074
为里程计在里程计坐标系下的脉冲数矢量形式，Nk为里程计在第k个采样周期内输出的脉冲数。In the present invention, in order to realize the inertia error suppression for the track detection platform system, it is first necessary to obtain the mileage output by the odometer in the carrier coordinate system. As a specific embodiment of the present invention, the low-speed track detection system includes an inertial navigation system and an odometer, and the low-speed track detection system is in close contact with the track through a clamping device, so the lateral speed and vertical speed are always 0, and the forward speed is always 0. Obtained by collecting the odometer output. The mileage output by the odometer in the carrier coordinate system can be calculated according to
Figure BDA0003327401220000071
to obtain, of which,
Figure BDA0003327401220000072
is the mileage output by the odometer in the carrier coordinate system at time k,
Figure BDA0003327401220000073
is the installation relationship matrix between the odometer and the inertial navigation system, K D is the odometer scale coefficient,
Figure BDA0003327401220000074
is the pulse number vector form of the odometer in the odometer coordinate system, and N k is the number of pulses output by the odometer in the kth sampling period.

进一步地，里程计与惯性导航系统之间的安装关系矩阵

Figure BDA0003327401220000075
可以表示为欧拉角的形式
Figure BDA0003327401220000076
然后基于里程计在载体坐标系下输出的里程计算获取里程计在载体坐标系下的位置增量。令δKD表示里程计刻度系数误差，则对
Figure BDA0003327401220000077
进行整理，可以表示为误差向量的形式，即里程计在载体坐标系下的位置增量为
Figure BDA0003327401220000081
其中，
Figure BDA0003327401220000082
为里程计在载体坐标系下的位置增量，δαθ为俯仰角误差，δαψ为航向角误差，δKD为里程计刻度系数误差，
Figure BDA0003327401220000083
为里程计在k时刻在载体坐标系下沿x轴输出的里程，
Figure BDA0003327401220000084
为里程计在k时刻在载体坐标系下沿y轴输出的里程，
Figure BDA0003327401220000085
为里程计在k时刻在载体坐标系下沿z轴输出的里程，X为状态变量。Further, the installation relationship matrix between the odometer and the inertial navigation system
Figure BDA0003327401220000075
can be expressed in the form of Euler angles
Figure BDA0003327401220000076
Then, based on the mileage output by the odometer in the carrier coordinate system, the position increment of the odometer in the carrier coordinate system is obtained. Let δK D denote the odometer scale coefficient error, then
Figure BDA0003327401220000077
After finishing, it can be expressed in the form of an error vector, that is, the position increment of the odometer in the carrier coordinate system is
Figure BDA0003327401220000081
in,
Figure BDA0003327401220000082
is the position increment of the odometer in the carrier coordinate system, δα θ is the pitch angle error, δα ψ is the heading angle error, δK D is the odometer scale coefficient error,
Figure BDA0003327401220000083
is the mileage output by the odometer along the x-axis in the carrier coordinate system at time k,
Figure BDA0003327401220000084
is the mileage output by the odometer along the y-axis in the carrier coordinate system at time k,
Figure BDA0003327401220000085
is the mileage output by the odometer along the z-axis in the carrier coordinate system at time k, and X is the state variable.

进一步地，在获取了里程计在载体坐标系下的位置增量之后，系统观测量采用单位时间内的位置增量进行计算，可以充分利用惯导系统短时精度较高的特点，使得对安装误差和刻度系数误差的估计更加准确。在k时刻内，惯性导航系统的位置增量为

Figure BDA0003327401220000086
其中，
Figure BDA0003327401220000087
为惯性导航系统在导航坐标系下的位置增量，
Figure BDA0003327401220000088
为k时刻惯性导航系统在导航坐标系下的速度，
Figure BDA0003327401220000089
为k-1时刻惯性导航系统在导航坐标系下的速度，Ts为计算周期。将惯性导航系统在导航坐标系下的位置增量转换到载体坐标系b系下，可得
Figure BDA00033274012200000810
Further, after obtaining the position increment of the odometer in the carrier coordinate system, the system observation value is calculated by the position increment per unit time, which can make full use of the high short-term accuracy of the inertial navigation system, so that the installation Estimates of errors and scale factor errors are more accurate. At time k, the position increment of the inertial navigation system is
Figure BDA0003327401220000086
in,
Figure BDA0003327401220000087
is the position increment of the inertial navigation system in the navigation coordinate system,
Figure BDA0003327401220000088
is the speed of the inertial navigation system in the navigation coordinate system at time k,
Figure BDA0003327401220000089
is the speed of the inertial navigation system in the navigation coordinate system at time k-1, and T s is the calculation period. Convert the position increment of the inertial navigation system in the navigation coordinate system to the carrier coordinate system b, we can get
Figure BDA00033274012200000810

选择惯性导航系统在载体坐标系下的位置增量与所述里程计在载体坐标系下的位置增量的差值作为系统观测量Zk：

Figure BDA00033274012200000811
The difference between the position increment of the inertial navigation system in the carrier coordinate system and the position increment of the odometer in the carrier coordinate system is selected as the system observation value Z k :
Figure BDA00033274012200000811

根据系统观测量可得观测矩阵Hk，

Figure BDA00033274012200000812
其中，
Figure BDA00033274012200000813
为惯性导航系统在载体坐标系下的位置增量。According to the systematic observations, the observation matrix H k can be obtained,
Figure BDA00033274012200000812
in,
Figure BDA00033274012200000813
It is the position increment of the inertial navigation system in the carrier coordinate system.

进一步地，在获取了系统观测矩阵之后，考虑到误差向量短时变化不大，因此系统状态转移矩阵可以近似为单位阵，即Fk＝I。Further, after the system observation matrix is obtained, considering that the error vector does not change much in a short time, the system state transition matrix can be approximated as an identity matrix, that is, F k =I.

基于系统观测矩阵和系统状态转移矩阵，通过卡尔曼滤波算法，对状态变量Xk＝[δKD,k δαθ,k δαψ,k]T进行估计，基于估计后的状态变量X对里程计安装误差进行修正。Based on the system observation matrix and the system state transition matrix, the state variable X k =[δK D,k δα θ,k δα ψ,k ] T is estimated through the Kalman filter algorithm, and the odometer is estimated based on the estimated state variable X Installation errors are corrected.

具体地，

Figure BDA0003327401220000091
specifically,
Figure BDA0003327401220000091

其中，Xk,k-1为一步预测状态，Kk为滤波增益矩阵，Xk为k时刻的状态变量，Xk-1为k-1时刻的状态变量，Pk,k-1为一步预测均方差误差阵，Pk为k时刻的均方差误差阵，Pk-1为k-1时刻的均方差误差阵，Qk为系统噪声矩阵，Rk为测量噪声矩阵。Among them, X k, k-1 is the one-step prediction state, K k is the filter gain matrix, X k is the state variable at time k, X k-1 is the state variable at time k-1, and P k, k-1 is one step. Prediction mean square error matrix, P k is the mean square error error matrix at time k, P k-1 is the mean square error error matrix at time k-1, Q k is the system noise matrix, and R k is the measurement noise matrix.

在本发明中，基于估计后的状态变量对里程计安装误差进行修正具体包括：基于估计后的状态变量对惯性导航系统与里程计之间的安装关系矩阵以及里程计刻度系数进行修正以完成对里程计安装误差进行修正。其中，惯性导航系统与里程计之间的安装关系矩阵可根据

Figure BDA0003327401220000092
进行修正，里程计刻度系数可根据KD,k+1＝(1+δKD,k)KD,k进行修正，其中，
Figure BDA0003327401220000093
为k+1时刻里程计与惯性导航系统之间的安装关系矩阵，
Figure BDA0003327401220000094
为k时刻里程计与惯性导航系统之间的安装关系矩阵，KD,k+1为k+1时刻里程计刻度系数，KD,k为k时刻里程计刻度系数。In the present invention, correcting the installation error of the odometer based on the estimated state variable specifically includes: correcting the installation relationship matrix between the inertial navigation system and the odometer and the odometer scale coefficient based on the estimated state variable to complete the correction of the odometer. The odometer installation error is corrected. Among them, the installation relationship matrix between the inertial navigation system and the odometer can be based on
Figure BDA0003327401220000092
Correction, the odometer scale coefficient can be corrected according to K D,k+1 =(1+δK D,k )K D,k , where,
Figure BDA0003327401220000093
is the installation relationship matrix between the odometer and the inertial navigation system at time k+1,
Figure BDA0003327401220000094
is the installation relationship matrix between the odometer and the inertial navigation system at time k, K D,k+1 is the calibration coefficient of the odometer at time k+1, and K D,k is the calibration coefficient of the odometer at time k.

修正里程计安装误差后，每次到达全站仪设站地点时的测量位置误差，则基本上是由测量过程中的航向陀螺漂移导致的，如图1所示，误差增长曲线是一条曲线，不能通过修正航向误差消除。此外由于测量过程中噪声的影响，对比两条曲线(第一曲线为真值曲线，第二曲线为包含误差的测量值曲线)的航向差值变化，会引入更大的航向误差，因此需要利用位置误差对航向陀螺漂移误差进行反向推导。After correcting the installation error of the odometer, the measurement position error when reaching the site of the total station is basically caused by the drift of the heading gyro during the measurement process. As shown in Figure 1, the error growth curve is a curve, It cannot be eliminated by correcting the heading error. In addition, due to the influence of noise in the measurement process, comparing the change of the heading difference between the two curves (the first curve is the true value curve, and the second curve is the measured value curve containing the error) will introduce a larger heading error, so it is necessary to use The position error reverses the heading gyro drift error.

在本发明中，根据误差产生原理，航向陀螺漂移与形式里程之间的关系可以表示为

Figure BDA0003327401220000101
其中，ΔX为修正完里程计误差后，由于陀螺漂移引起的侧向累计位置误差，ω为陀螺漂移等效角速率，t为从上一个站点出发后经过的时间，v为当前数据段的平均推行速度，Δt为系统采样周期，将
Figure BDA0003327401220000102
的离散化形式转换为连续时间函数形式可得
Figure BDA0003327401220000103
其中，Δx(t)为由于陀螺漂移引起的侧向累计位置误差，τ为任一时刻。In the present invention, according to the principle of error generation, the relationship between the drift of the heading gyro and the formal mileage can be expressed as
Figure BDA0003327401220000101
Among them, ΔX is the lateral cumulative position error caused by gyro drift after correcting the odometer error, ω is the equivalent angular rate of gyro drift, t is the elapsed time from the previous station, and v is the average of the current data segment Pushing speed, Δt is the sampling period of the system, the
Figure BDA0003327401220000102
Convert the discretized form of , to the continuous-time function form, we can get
Figure BDA0003327401220000103
Among them, Δx(t) is the lateral accumulated position error caused by gyro drift, and τ is any moment.

为了对本发明有进一步地了解，下面结合具体实施例对本发明所提供的用于低速检测平台系统的惯性误差抑制方法进行详细说明。In order to have a further understanding of the present invention, the inertia error suppression method for a low-speed detection platform system provided by the present invention will be described in detail below with reference to specific embodiments.

根据本发明的具体实施例，提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法具体包括如下步骤。According to a specific embodiment of the present invention, an inertial error suppression method for a low-speed detection platform system is provided, and the inertial error suppression method specifically includes the following steps.

获取里程计在载体坐标系下输出的里程；里程计在载体坐标系下输出的里程可根据

Figure BDA0003327401220000104
来获取，其中，
Figure BDA0003327401220000105
为里程计在k时刻在载体坐标系下输出的里程，
Figure BDA0003327401220000106
为里程计与惯性导航系统之间的安装关系矩阵，KD为里程计刻度系数，
Figure BDA0003327401220000107
为里程计在里程计坐标系下的脉冲数矢量形式，Nk为里程计在第k个采样周期内输出的脉冲数。Obtain the mileage output by the odometer in the carrier coordinate system; the mileage output by the odometer in the carrier coordinate system can be
Figure BDA0003327401220000104
to obtain, of which,
Figure BDA0003327401220000105
is the mileage output by the odometer in the carrier coordinate system at time k,
Figure BDA0003327401220000106
is the installation relationship matrix between the odometer and the inertial navigation system, K D is the odometer scale coefficient,
Figure BDA0003327401220000107
is the pulse number vector form of the odometer in the odometer coordinate system, and N k is the number of pulses output by the odometer in the kth sampling period.

里程计与惯性导航系统之间的安装关系矩阵

Figure BDA0003327401220000108
可以表示为欧拉角的形式
Figure BDA0003327401220000111
Installation relationship matrix between odometer and inertial navigation system
Figure BDA0003327401220000108
can be expressed in the form of Euler angles
Figure BDA0003327401220000111

基于里程计在载体坐标系下输出的里程计算获取里程计在载体坐标系下的位置增量；令δKD表示里程计刻度系数误差，则对

Figure BDA0003327401220000112
进行整理，可以表示为误差向量的形式，即里程计在载体坐标系下的位置增量为
Figure BDA0003327401220000113
其中，
Figure BDA0003327401220000114
为里程计在载体坐标系下的位置增量，δαθ为俯仰角误差，δαψ为航向角误差，δKD为里程计刻度系数误差，
Figure BDA0003327401220000115
为里程计在k时刻在载体坐标系下沿x轴输出的里程，
Figure BDA0003327401220000116
为里程计在k时刻在载体坐标系下沿y轴输出的里程，
Figure BDA0003327401220000117
为里程计在k时刻在载体坐标系下沿z轴输出的里程，X为状态变量。Calculate the position increment of the odometer in the carrier coordinate system based on the odometer output by the odometer in the carrier coordinate system; let δK D represent the odometer scale coefficient error, then
Figure BDA0003327401220000112
After finishing, it can be expressed in the form of an error vector, that is, the position increment of the odometer in the carrier coordinate system is
Figure BDA0003327401220000113
in,
Figure BDA0003327401220000114
is the position increment of the odometer in the carrier coordinate system, δα θ is the pitch angle error, δα ψ is the heading angle error, δK D is the odometer scale coefficient error,
Figure BDA0003327401220000115
is the mileage output by the odometer along the x-axis in the carrier coordinate system at time k,
Figure BDA0003327401220000116
is the mileage output by the odometer along the y-axis in the carrier coordinate system at time k,
Figure BDA0003327401220000117
is the mileage output by the odometer along the z-axis in the carrier coordinate system at time k, and X is the state variable.

将惯性导航系统在载体坐标系下的位置增量与里程计在载体坐标系下的位置增量的差值作为系统观测量，根据系统观测量获取观测矩阵；获取系统状态转移矩阵；基于系统观测矩阵和系统状态转移矩阵，通过卡尔曼滤波算法，对状态变量进行估计，基于估计后的状态变量对里程计安装误差进行修正。系统观测量可根据

Figure BDA0003327401220000118
获取，其中，
Figure BDA0003327401220000119
为惯性导航系统在载体坐标系下的位置增量，Hk为观测矩阵，
Figure BDA0003327401220000121
惯性导航系统在载体坐标系下的位置增量
Figure BDA0003327401220000122
可根据
Figure BDA0003327401220000123
来获取，其中，
Figure BDA0003327401220000124
为所述惯性导航系统在导航坐标系下的位置增量，
Figure BDA0003327401220000125
为k时刻惯性导航系统在导航坐标系下的速度，
Figure BDA0003327401220000126
为k-1时刻惯性导航系统在导航坐标系下的速度，Ts为计算周期。The difference between the position increment of the inertial navigation system in the carrier coordinate system and the position increment of the odometer in the carrier coordinate system is taken as the system observation amount, and the observation matrix is obtained according to the system observation amount; the system state transition matrix is obtained; based on the system observation The matrix and the system state transition matrix are used to estimate the state variables through the Kalman filter algorithm, and the odometer installation error is corrected based on the estimated state variables. System observations can be based on
Figure BDA0003327401220000118
get, where,
Figure BDA0003327401220000119
is the position increment of the inertial navigation system in the carrier coordinate system, H k is the observation matrix,
Figure BDA0003327401220000121
The position increment of the inertial navigation system in the carrier coordinate system
Figure BDA0003327401220000122
according to
Figure BDA0003327401220000123
to obtain, of which,
Figure BDA0003327401220000124
is the position increment of the inertial navigation system in the navigation coordinate system,
Figure BDA0003327401220000125
is the speed of the inertial navigation system in the navigation coordinate system at time k,
Figure BDA0003327401220000126
is the speed of the inertial navigation system in the navigation coordinate system at time k-1, and T s is the calculation period.

基于修正后的里程计安装误差，获取修正完里程计误差后由于陀螺漂移引起的侧向累计位置误差，基于由于陀螺漂移引起的侧向累计位置误差获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。Based on the corrected odometer installation error, obtain the lateral cumulative position error caused by gyro drift after correcting the odometer error, obtain the equivalent angular velocity of gyro drift based on the lateral cumulative position error caused by gyro drift, and obtain the equivalent angular velocity based on gyro drift. The angular velocity completes the inertia error correction for the orbit detection platform system.

将修正后的轨道检测平台系统的惯性误差与设定惯性误差精度阈值范围进行比较，当修正后的所述轨道检测平台系统的惯性误差超出设定惯性误差精度阈值范围时，重复上述步骤，直至修正后的所述轨道检测平台系统的惯性误差处于设定惯性误差精度阈值范围内。Compare the inertial error of the corrected orbit detection platform system with the set inertial error precision threshold range. When the corrected inertial error of the orbit detection platform system exceeds the set inertial error precision threshold range, repeat the above steps until The corrected inertial error of the orbit detection platform system is within the set inertial error precision threshold range.

综上所述，本发明提供了一种用于低速检测平台系统的惯性误差抑制方法，该惯性误差抑制方法通过获取观测矩阵和系统状态转移矩阵，基于卡尔曼滤波算法，对状态变量进行估计，并根据估计后的状态变量对里程计安装误差进行修正；在对里程计安装误差进行修正后，可通过站点处位置误差反向推导航向误差，即获取陀螺漂移等效角速度，基于陀螺漂移等效角速度完成对轨道检测平台系统的惯性误差修正。此种方式能够显著降低全站仪定点修正引起的驻波状误差，相比传统惯性/里程计组合导航算法对惯导系统的误差的抑制效果更佳显著。To sum up, the present invention provides an inertial error suppression method for a low-speed detection platform system. The inertial error suppression method obtains the observation matrix and the system state transition matrix, and estimates the state variables based on the Kalman filter algorithm. And the odometer installation error is corrected according to the estimated state variables; after the odometer installation error is corrected, the navigation error can be reversely pushed through the position error at the site, that is, the equivalent angular velocity of the gyro drift is obtained, based on the gyro drift, etc. The effective angular velocity is used to complete the inertia error correction of the orbit detection platform system. This method can significantly reduce the standing wave-like error caused by the fixed-point correction of the total station, and is more effective in suppressing the error of the inertial navigation system than the traditional inertial/odometer integrated navigation algorithm.

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。For ease of description, spatially relative terms, such as "on", "over", "on the surface", "above", etc., may be used herein to describe what is shown in the figures. The spatial positional relationship of one device or feature shown to other devices or features. It should be understood that spatially relative terms are intended to encompass different orientations of the device in use or operation in addition to the orientation depicted in the figures. For example, if the device in the figures is turned over, elements described as "above" or "over" other devices or features would then be oriented "below" or "over" the other devices or features under other devices or constructions". Thus, the exemplary term "above" can encompass both an orientation of "above" and "below." The device may also be otherwise oriented (rotated 90 degrees or at other orientations) and the spatially relative descriptions used herein interpreted accordingly.

此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。In addition, it should be noted that the use of words such as "first" and "second" to define components is only for the convenience of distinguishing corresponding components. Unless otherwise stated, the above words have no special meaning and therefore cannot be understood to limit the scope of protection of the present invention.

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims (9)

1. An inertial error suppression method for a low-speed test platform system, the inertial error suppression method comprising:
acquiring the mileage output by the odometer under a carrier coordinate system;
calculating and acquiring the position increment of the odometer under the carrier coordinate system based on the mileage output by the odometer under the carrier coordinate system;
taking the difference value of the position increment of the inertial navigation system under a carrier coordinate system and the position increment of the odometer under the carrier coordinate system as a system observed quantity, and obtaining an observation matrix according to the system observation measurement;
acquiring a system state transition matrix;
estimating a state variable through a Kalman filtering algorithm based on the system observation matrix and the system state transition matrix, and correcting the installation error of the odometer based on the estimated state variable;
and acquiring a lateral accumulated position error caused by the drift of the gyroscope after the error of the odometer is corrected based on the corrected mounting error of the odometer, acquiring a drift equivalent angular velocity of the gyroscope based on the lateral accumulated position error caused by the drift of the gyroscope, and finishing the correction of the inertial error of the track detection platform system based on the drift equivalent angular velocity of the gyroscope.
2. The inertial error suppression method for low speed detection platform system according to claim 1, wherein after completing the inertial error correction for the orbit detection platform system, the inertial error suppression method further comprises: and comparing the corrected inertia error of the track detection platform system with a set inertia error precision threshold range, and repeating the steps when the corrected inertia error of the track detection platform system exceeds the set inertia error precision threshold range until the corrected inertia error of the track detection platform system is within the set inertia error precision threshold range.
3. The inertial error suppression method for low-speed detection platform system according to claim 1, wherein the mileage outputted by the odometer in the carrier coordinate system is determined according to the mileage
Figure FDA0003327401210000021
To obtain, wherein,
Figure FDA0003327401210000022
the mileage output by the odometer under the carrier coordinate system at the moment k,
Figure FDA0003327401210000023
is an installation relationship matrix between the odometer and the inertial navigation system, KDIn order to obtain the scale factor of the odometer,
Figure FDA0003327401210000024
in the form of a pulse number vector of the odometer in an odometer coordinate system, NkThe number of pulses output by the odometer in the kth sampling period.
4. The method of claim 3The inertial error suppression method for the low-speed detection platform system is characterized in that the position increment of the odometer under the carrier coordinate system can be determined according to the position increment
Figure FDA0003327401210000025
To obtain, wherein,
Figure FDA0003327401210000026
delta alpha being the position increment of the odometer in the carrier coordinate systemθFor pitch angle error, delta alphaψIs the course angle error, δ KDIn order to measure the error of the scale factor of the odometer,
Figure FDA0003327401210000027
the mileage output by the odometer along the x axis under the carrier coordinate system at the moment k,
Figure FDA0003327401210000028
the mileage output by the odometer along the y axis under the carrier coordinate system at the moment k,
Figure FDA0003327401210000029
the mileage output by the odometer along the z axis under the carrier coordinate system at the moment k is shown, and X is a state variable.
5. The inertial error suppression method for a low-speed detection platform system according to claim 4, wherein the system observations are based on
Figure FDA00033274012100000210
Obtaining, wherein,
Figure FDA00033274012100000211
for position increment of inertial navigation system in carrier coordinate system, HkIn order to observe the matrix, the system,
Figure FDA00033274012100000212
6. the inertial error suppression method for low-speed detection platform system according to claim 5, wherein the position increment of the inertial navigation system in the carrier coordinate system
Figure FDA0003327401210000031
Can be based on
Figure FDA0003327401210000032
Figure FDA0003327401210000033
To obtain, wherein,
Figure FDA0003327401210000034
for position increments of the inertial navigation system in a navigation coordinate system,
Figure FDA0003327401210000035
for the velocity of the inertial navigation system at time k in the navigation coordinate system,
Figure FDA0003327401210000036
velocity, T, of the inertial navigation system at the moment k +1 in a navigation coordinate systemsIs a calculation cycle.
7. The inertial error suppression method for low-speed detection platform systems according to any one of claims 1 to 6, wherein correcting the odometer mounting error based on the estimated state variable specifically comprises: and correcting the installation relation matrix between the inertial navigation system and the odometer scale coefficient based on the estimated state variable so as to finish correcting the installation error of the odometer.
8. The inertial error suppression method for low-speed inspection platform system according to claim 7,the mounting relationship matrix between the inertial navigation system and the odometer may be based on
Figure FDA0003327401210000037
Corrected, the scale factor of the odometer can be according to KD,k+1＝(1+δKD,k)KD,kA correction is made, wherein,
Figure FDA0003327401210000038
is an installation relation matrix between the odometer at the moment k +1 and the inertial navigation system,
Figure FDA0003327401210000039
is an installation relation matrix between the milemeter at the time K and the inertial navigation system, KD,k+1Is the scale factor of the odometer at the moment K +1, KD,kAnd the scale factor of the odometer at the moment k.
9. The inertial error suppression method for low-speed detection platform system according to claim 1, wherein the gyro drift equivalent angular velocity is determined according to
Figure FDA00033274012100000310
And acquiring, wherein Δ x (t) is a lateral accumulated position error caused by gyro drift, ω is an equivalent angular rate of gyro drift, t is the time elapsed from the last station, v is the average pushed speed of the current data segment, and τ is any time.
CN202111268698.1A 2021-10-29 2021-10-29 Inertial error suppression method for low-speed detection platform system Active CN114184209B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111268698.1A CN114184209B (en) 2021-10-29 2021-10-29 Inertial error suppression method for low-speed detection platform system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111268698.1A CN114184209B (en) 2021-10-29 2021-10-29 Inertial error suppression method for low-speed detection platform system

Publications (2)

Publication Number Publication Date
CN114184209A true CN114184209A (en) 2022-03-15
CN114184209B CN114184209B (en) 2023-10-13

Family

ID=80601643

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111268698.1A Active CN114184209B (en) 2021-10-29 2021-10-29 Inertial error suppression method for low-speed detection platform system

Country Status (1)

Country Link
CN (1) CN114184209B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115615430A (en) * 2022-12-21 2023-01-17 中国船舶集团有限公司第七〇七研究所 Positioning data correction method and system based on strapdown inertial navigation
CN115900764A (en) * 2022-11-30 2023-04-04 三一机器人科技有限公司 Odometer calibration and measurement method, device and system, and AGV

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1221586A2 (en) * 2001-01-08 2002-07-10 Motorola, Inc. Position and heading error-correction method and apparatus for vehicle navigation systems
US6459990B1 (en) * 1999-09-23 2002-10-01 American Gnc Corporation Self-contained positioning method and system thereof for water and land vehicles
US6477465B1 (en) * 1999-11-29 2002-11-05 American Gnc Corporation Vehicle self-carried positioning method and system thereof
WO2013037034A1 (en) * 2011-09-14 2013-03-21 Trusted Positioning Inc. Method and apparatus for navigation with nonlinear models
CN105318876A (en) * 2014-07-09 2016-02-10 北京自动化控制设备研究所 Inertia and mileometer combination high-precision attitude measurement method
CN108180925A (en) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 A kind of odometer assists vehicle-mounted dynamic alignment method
CN108731670A (en) * 2018-05-18 2018-11-02 南京航空航天大学 Inertia/visual odometry combined navigation locating method based on measurement model optimization
CN109870173A (en) * 2019-04-11 2019-06-11 中国石油化工股份有限公司 A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
WO2019175516A1 (en) * 2018-03-15 2019-09-19 Sysnav Method of calibrating a gyrometer installed in a vehicle
CN111006675A (en) * 2019-12-27 2020-04-14 西安理工大学 Self-calibration method of vehicle-mounted laser inertial navigation system based on high-precision gravity model
CN111380516A (en) * 2020-02-27 2020-07-07 上海交通大学 Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information
CN111811506A (en) * 2020-09-15 2020-10-23 中国人民解放军国防科技大学 Visual/inertial odometry integrated navigation method, electronic device and storage medium
CN112697141A (en) * 2020-12-16 2021-04-23 北京航空航天大学 Inertial navigation/odometer moving base posture and position alignment method based on reverse navigation

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6459990B1 (en) * 1999-09-23 2002-10-01 American Gnc Corporation Self-contained positioning method and system thereof for water and land vehicles
US6477465B1 (en) * 1999-11-29 2002-11-05 American Gnc Corporation Vehicle self-carried positioning method and system thereof
EP1221586A2 (en) * 2001-01-08 2002-07-10 Motorola, Inc. Position and heading error-correction method and apparatus for vehicle navigation systems
WO2013037034A1 (en) * 2011-09-14 2013-03-21 Trusted Positioning Inc. Method and apparatus for navigation with nonlinear models
CN105318876A (en) * 2014-07-09 2016-02-10 北京自动化控制设备研究所 Inertia and mileometer combination high-precision attitude measurement method
CN108180925A (en) * 2017-12-15 2018-06-19 中国船舶重工集团公司第七0七研究所 A kind of odometer assists vehicle-mounted dynamic alignment method
WO2019175516A1 (en) * 2018-03-15 2019-09-19 Sysnav Method of calibrating a gyrometer installed in a vehicle
CN108731670A (en) * 2018-05-18 2018-11-02 南京航空航天大学 Inertia/visual odometry combined navigation locating method based on measurement model optimization
CN109870173A (en) * 2019-04-11 2019-06-11 中国石油化工股份有限公司 A kind of track correct method of the submarine pipeline inertial navigation system based on checkpoint
CN111006675A (en) * 2019-12-27 2020-04-14 西安理工大学 Self-calibration method of vehicle-mounted laser inertial navigation system based on high-precision gravity model
CN111380516A (en) * 2020-02-27 2020-07-07 上海交通大学 Inertial navigation/odometer vehicle combined navigation method and system based on odometer measurement information
CN111811506A (en) * 2020-09-15 2020-10-23 中国人民解放军国防科技大学 Visual/inertial odometry integrated navigation method, electronic device and storage medium
CN112697141A (en) * 2020-12-16 2021-04-23 北京航空航天大学 Inertial navigation/odometer moving base posture and position alignment method based on reverse navigation

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王万征;李洪;郭玉胜;: "一种车载组合导航系统航向修正方法研究", 导航定位与授时, no. 06, pages 32 - 37 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115900764A (en) * 2022-11-30 2023-04-04 三一机器人科技有限公司 Odometer calibration and measurement method, device and system, and AGV
CN115615430A (en) * 2022-12-21 2023-01-17 中国船舶集团有限公司第七〇七研究所 Positioning data correction method and system based on strapdown inertial navigation
CN115615430B (en) * 2022-12-21 2023-03-10 中国船舶集团有限公司第七〇七研究所 Positioning data correction method and system based on strapdown inertial navigation

Also Published As

Publication number Publication date
CN114184209B (en) 2023-10-13

Similar Documents

Publication Publication Date Title
CN110926460B (en) Uwb positioning abnormal value processing method based on IMU
CN107655493B (en) SINS six-position system-level calibration method for fiber-optic gyroscope
CN111044082B (en) A fast on-orbit calibration method of gyro error parameters based on star sensor assistance
CN114018274A (en) Vehicle positioning method and device and electronic equipment
CN104075713B (en) Inertance/astronomy combined navigation method
KR100898169B1 (en) Initial alignment method of inertial navigation system
CN108592945A (en) Online calibration method for errors of inertia/astronomical combination system
CN113008229B (en) Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor
CN106468554B (en) A kind of measuring method of the inertial parameter of contactless rolling satellite
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN109489661B (en) Gyro combination constant drift estimation method during initial orbit entering of satellite
CN114184209A (en) Inertial Error Suppression Method for Low-speed Inspection Platform System
CN110702113B (en) Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor
CN107621266A (en) The space non-cooperative target Relative Navigation of distinguished point based tracking
CN104613966B (en) A kind of cadastration off-line data processing method
CN114111767A (en) Method for optimizing line design line type based on multi-information fusion
CN104819717B (en) A kind of multi-rotor aerocraft attitude detecting method based on MEMS inertial sensor group
CN110207694A (en) A kind of polar region grid inertial navigation/ultra-short baseline Combinated navigation method based on relative position information
CN114201722A (en) Dynamic calculation method based on post-processing vehicle body-bogie installation relation
CN109506674B (en) Acceleration correction method and device
CN112284388B (en) Unmanned aerial vehicle multisource information fusion navigation method
CN115371705B (en) A DVL calibration method based on special orthogonal group and robust invariant extended Kalman filter
CN116337060A (en) Digital road network auxiliary inertial autonomous combined navigation method and system based on index table
CN111076718B (en) Autonomous navigation positioning method for subway train
CN109141414A (en) A kind of identification of farm work vehicle external acceleration and posture synchronous estimation method

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