CN111832690B - Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm - Google Patents

Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm Download PDF

Info

Publication number
CN111832690B
CN111832690B CN202010544718.2A CN202010544718A CN111832690B CN 111832690 B CN111832690 B CN 111832690B CN 202010544718 A CN202010544718 A CN 202010544718A CN 111832690 B CN111832690 B CN 111832690B
Authority
CN
China
Prior art keywords
inertial navigation
navigation system
calibrated
particle
data
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
CN202010544718.2A
Other languages
Chinese (zh)
Other versions
CN111832690A (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.)
Naval University of Engineering PLA
Original Assignee
Naval University of Engineering PLA
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 Naval University of Engineering PLA filed Critical Naval University of Engineering PLA
Priority to CN202010544718.2A priority Critical patent/CN111832690B/en
Publication of CN111832690A publication Critical patent/CN111832690A/en
Application granted granted Critical
Publication of CN111832690B publication Critical patent/CN111832690B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • General Health & Medical Sciences (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • Biophysics (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Computational Linguistics (AREA)
  • Biomedical Technology (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The invention relates to a method for calculating a gyro measurement value of an inertial navigation system based on a particle swarm optimization algorithm, which is suitable for improving the measurement accuracy of the gyro measurement value of the inertial navigation system to be calibrated in the motion process and comprises the following steps: acquiring a gyro measurement value of the inertial navigation system to be calibrated at each correction time point; constructing a particle population according to the gyro constant drift amount, and randomly initializing particles in the variation range of the gyro constant drift amount; resolving the speed data and the position data of each particle to obtain an optimal particle; and obtaining the gyro constant drift amount of the inertial navigation system to be calibrated by using the position data of the current group of optimal particles, and then obtaining a calibrated gyro measurement value. The method adopts a particle swarm optimization-based algorithm, does not need a prior error model of an inertial device, specific maneuver, external information assistance and external information meeting Gaussian hypothesis conditions; the pure inertial navigation resolving error is minimized, and the navigation precision of the inertial navigation system to be calibrated in the pure inertial navigation working mode is improved.

Description

Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm
Technical Field
The invention relates to the technical field of inertial navigation measurement value calculation, in particular to a gyroscope measurement value calculation method of an inertial navigation system based on a particle swarm optimization algorithm.
Background
The strapdown inertial navigation system has the advantages of high autonomy, independence on external information, no information transmission to the outside and the like, can effectively enhance the autonomy and the concealment of the underwater vehicle, and the working state of the strapdown inertial navigation system which does not depend on the external information is called a pure inertial navigation state (or called a pure inertial navigation state). If the underwater vehicle is in an extremely severe environment such as a strong interference environment, the availability of information provided by the external navigation sensor will be deteriorated or even failed, and in such a situation, the pure inertial navigation becomes the only means for the underwater navigation of the vehicle.
In the pure inertial navigation stage, due to the influence of errors of measuring unit devices, oscillating type and accumulation type system errors exist in the strap-down inertial navigation system in the resolving process, and the strap-down inertial navigation system is very unfavorable for an aircraft in an underwater environment for a long time. Therefore, accurately estimating and deducting the device error of the measurement unit from the measurement value is an important way for realizing long-period and high-precision navigation of the strapdown inertial navigation system.
The most widely used technical scheme at present is to adopt a kalman filtering algorithm, namely a KF algorithm, to estimate the device error of the inertial measurement unit. Although KF algorithms are currently considered to be based on l 2 The optimal estimation method of the norm still has certain limitations, and particularly, a KF algorithm can realize effective estimation of the device error of the inertial measurement unit and needs to meet the following conditions
1. And accurately obtaining a prior error model of the inertial device.
2. A specific maneuver is required.
The above constraint conditions are relatively harsh relative to the actual working environment of the underwater vehicle, so that the adaptability of the underwater vehicle to a complex environment and the rapid response capability of the underwater vehicle are limited.
Disclosure of Invention
Aiming at the problems, the invention provides a gyro measurement value calculation method of an inertial navigation system based on a particle swarm optimization algorithm, so that the process of obtaining the measurement value in the inertial navigation process is free from the prior error model and the specific maneuver of the inertial device to be obtained; meanwhile, the navigation accuracy of the inertial navigation system to be calibrated in a pure inertial navigation working mode is improved by improving the measurement accuracy of the inertial navigation system to be calibrated.
In order to solve the problems, the technical scheme provided by the invention is as follows:
the method for calculating the gyro measurement value of the inertial navigation system based on the particle swarm optimization algorithm is suitable for improving the measurement accuracy of the gyro measurement value of the inertial navigation system to be calibrated in the motion process, and comprises the following steps of:
s100, acquiring a measured value of the inertial navigation system to be calibrated at each correction time point in a time period from the moment when the inertial navigation system to be calibrated starts to autonomously propel to the moment when the inertial navigation system to be calibrated reaches an artificially preset target position; the correction time point corresponds to the acquisition frequency from the starting time point of the inertial navigation system to be calibrated; the measurement value of the inertial navigation system to be calibrated comprises current speed measurement data and current position measurement data;
s200, constructing a particle population according to the gyro constant drift amount, and randomly initializing particles in the variation range of the gyro constant drift amount; the particles contain velocity data and position data;
s300, resolving the speed data and the position data of each particle to obtain the optimal particle, and specifically comprises the following steps:
s310, iteratively calculating the speed data and the position data of each particle until the maximum value of the iteration times is reached, and then outputting the speed data and the position data of each particle after iteration; the iteration times are constant and are preset manually; if the speed data or the position data obtained by one iteration calculation exceeds the variation range of the gyro constant drift amount in the iteration process, outputting the speed data and the position data obtained by the last iteration calculation of the iteration calculation, and then outputting the speed data and the position data of each particle after the iteration;
s320, performing speed calculation and position calculation on the inertial navigation system to be calibrated by using the speed data and the position data of each particle after iteration to obtain the east-direction speed, the north-direction speed, the latitude and the longitude output by the inertial navigation system to be calibrated at the moment t, then calculating the value of a cost function of the inertial navigation system to be calibrated through the cost function of the inertial navigation system to be calibrated, and calibrating the particle with the minimum cost function value as the current group optimal particle of the iteration; the cost function is calculated as follows:
Figure BDA0002540200070000031
wherein, t 0 Calculating starting time of the inertial navigation system to be calibrated; t is t e The resolving end time of the inertial navigation system to be calibrated is obtained;
Figure BDA0002540200070000032
resolving an error for the east speed of the inertial navigation system to be calibrated at the time t;
Figure BDA0002540200070000033
resolving an error for the north speed of the inertial navigation system to be calibrated at the time t; e.g. of a cylinder Lat (t) resolving errors of the latitude of the inertial navigation system to be calibrated at the moment t; e.g. of the type Lon (t) resolving an error of the longitude of the inertial navigation system to be calibrated at the moment t; rho 1 、ρ 2 、ρ 3 、ρ 4 The weight coefficient is preset manually;
s400, acquiring a gyro constant drift amount of the inertial navigation system to be calibrated by using the position data of the current group optimal particles; and then deducting the gyro constant drift amount of the inertial navigation system to be calibrated from the gyro measured value of the inertial navigation system to be calibrated to obtain a calibrated gyro measured value.
Preferably, the step S100 of obtaining the measurement value of the inertial navigation system to be calibrated includes the following steps:
s110, measuring current angular acceleration measurement data by a gyroscope loaded on the inertial navigation system to be calibrated, and measuring current linear acceleration measurement data by a linear accelerometer;
s120, resolving current angular acceleration measurement data and current linear acceleration measurement data in real time by adopting a mechanical programming equation to obtain current speed measurement data and current position measurement data of the inertial navigation system to be calibrated; and packaging the current speed measurement data and the current position measurement data, and outputting the data serving as the measurement value of the inertial navigation system to be calibrated.
Preferably, the speed data comprises a current speed and an updated speed;
the position data comprises a current position, an updated position, a current individual optimal position and a current group optimal position;
the iterative calculation of the velocity data of each particle in S310 is calculated as follows:
Figure BDA0002540200070000034
wherein the content of the first and second substances,
Figure BDA0002540200070000041
is the velocity of the particle after update;
Figure BDA0002540200070000042
is the current position of the particle;
Figure BDA0002540200070000043
is the current velocity of the particle;
Figure BDA0002540200070000044
the position of the optimal particle for the current individual;
Figure BDA0002540200070000045
the position of the optimal particle for the current population; r is 1 And r 2 Is a random number between (0, 1); c. C 1 The value range of the learning factor for representing the learning ability of the particle to the particle is (0, 2); c. C 2 The value range of the learning factor for representing the learning ability of the particle to other particles is (0, 2); c. C 1 And c 2 Random within a value range; omega is an inertia weight constant; d is the dimension of the search space, d =1,2, \ 8230, K is the number of the dimension of the search space; i =1,2, \ 8230, N, N is the population size; m is the iteration number of the current population;
the iterative calculation of the position data for each particle is calculated as follows:
Figure BDA0002540200070000046
wherein the content of the first and second substances,
Figure BDA0002540200070000047
is the updated position of the particle.
Preferably, the calibrated gyro measurement value in S400 is calculated according to the following formula:
Figure BDA0002540200070000048
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0002540200070000049
the measured value of the gyroscope of the inertial navigation system to be calibrated is obtained;
Figure BDA00025402000700000410
position data of the current population optimal particles output when iteration is terminated;
Figure BDA00025402000700000411
and the measured value is the calibrated gyro value.
Compared with the prior art, the invention has the following advantages:
1. the invention adopts the particle swarm optimization algorithm to intelligently search the gyro constant drift amount, so that a prior error model of an inertial device is not needed, and specific maneuvering is not needed.
2. According to the method, the gyroscope constant drift amount is intelligently searched based on the particle swarm optimization algorithm, so that the pure inertial navigation resolving error is minimized, the measurement accuracy of the inertial navigation system to be calibrated is improved, and the navigation accuracy of the inertial navigation system to be calibrated in a pure inertial navigation working mode is improved.
Drawings
FIG. 1 is a flow chart of steps of an embodiment of the present invention.
FIG. 2 is a flow chart of the steps for obtaining optimal particles in an embodiment of the present invention.
Detailed Description
The present invention is further illustrated by the following examples, which are intended to be purely exemplary and are not intended to limit the scope of the invention, as various equivalent modifications of the invention will occur to those skilled in the art upon reading the present disclosure and fall within the scope of the appended claims.
As shown in fig. 1, a method for calculating a gyro measurement value of an inertial navigation system based on a particle swarm is suitable for improving the measurement accuracy of the gyro measurement value of the inertial navigation system to be calibrated in a motion process, and includes the following steps:
s100, collecting a measured value of the inertial navigation system to be calibrated once at each correction time point by using a gyroscope and an accelerometer which are collected by a FOSN optical fiber strapdown inertial navigation system arranged on a double-shaft turntable within a time period from the moment when the inertial navigation system to be calibrated starts to be propelled to the moment when the inertial navigation system to be calibrated reaches a target position preset manually, wherein the total time is 5 hours; the correction time point corresponds to the acquisition frequency from the starting time point of the inertial navigation system to be calibrated; setting the acquisition frequency to be 10Hz; the measured value of the inertial navigation system to be calibrated comprises current speed measured data and current position measured data; the performance index of the ray IMU used in this example is shown in table 1:
table 1 performance index data table of light IMU of this embodiment
Figure BDA0002540200070000051
In this embodiment, for the purpose of simplifying the explanatory text, the particle swarm optimization algorithm is hereinafter abbreviated as "PSO algorithm", and the solution mode using the PSO algorithm is abbreviated as "PSO-solution"; simplifying the pure inertial navigation solution mode by using the inertial navigation simulation original data as 'IN-solution'; the Kalman filtering algorithm is abbreviated as a KF algorithm, and the calculation mode adopting the KF algorithm is abbreviated as KF-calculation.
In this embodiment, the inertial navigation system to be calibrated is in a static base condition, and its attitude angle is: the pitch angle is 0 degree, the roll angle is 0 degree, and the course angle is 36.8 degrees; the current position measurement data is: longitude 114.2429 deg., latitude 30.58 deg..
The method for obtaining the measured value of the inertial navigation system to be calibrated of the calibration inertial navigation system specifically comprises the following steps:
and S110, measuring current angular acceleration measurement data by a gyroscope loaded on the inertial navigation system to be calibrated, and measuring current linear acceleration measurement data by a linear accelerometer.
S120, solving current angular acceleration measurement data and current linear acceleration measurement data in real time by adopting a mechanical programming equation to obtain current speed measurement data and current position measurement data of the inertial navigation system to be calibrated; and packaging the current speed measurement data and the current position measurement data, and outputting the data serving as the measurement value of the inertial navigation system to be calibrated.
S200, constructing a particle population according to the gyro constant drift amount, and randomly initializing particles in the variation range of the gyro constant drift amount; the particles contain velocity data and position data; the speed data comprises a current speed and an updated speed; the position data comprises a current position, an updated position, a current individual optimal position and a current group optimal position; in this embodiment, the minimum speed of the inertial navigation system to be calibrated is set to be v min =0.1/100, the maximum speed of the inertial navigation system to be calibrated is v max =0.1/100; the variation range of the gyro constant drift amount is
Figure BDA0002540200070000061
S300, resolving the speed data and the position data of each particle to obtain an optimal particle, as shown in fig. 2, specifically including the following steps:
s310, iteratively calculating the speed data and the position data of each particle until the maximum value of the iteration times is reached, and then outputting the speed data and the position data of each particle after iteration; the iteration times are constant and are preset manually; if the speed data or the position data obtained by one iteration calculation exceeds the variation range of the gyro constant drift amount in the iteration process, outputting the speed data and the position data obtained by the last iteration calculation of the iteration calculation, and then outputting the speed data and the position data of each particle after the iteration.
Wherein the iterative calculation of the velocity data for each particle is calculated according to equation (1):
Figure BDA0002540200070000062
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0002540200070000071
is the velocity of the particle after update;
Figure BDA0002540200070000072
is the current position of the particle;
Figure BDA0002540200070000073
is the current velocity of the particle;
Figure BDA0002540200070000074
the position of the optimal particle for the current individual;
Figure BDA0002540200070000075
the position of the optimal particle for the current population; r is 1 And r 2 Is a random number between (0, 1); c. C 1 The value range of the learning factor for representing the learning ability of the particle to the particle is (0, 2); c. C 2 To characterize the learning factor of the particle's ability to learn from other particles, the value range is (0, 2), c in this embodiment 1 ＝1.0，c 2 =1.0; omega is an inertia weight constant; d is the dimension of the search space, d =1,2, \ 8230, K is the number of the dimension of the search space; i =1,2, \8230, N, N is the population size, N =30; m is the number of iterations of the current population, and m =50.
The iterative calculation of the position data for each particle is calculated as equation (2):
Figure BDA0002540200070000076
wherein the content of the first and second substances,
Figure BDA0002540200070000077
is the updated position of the particle.
S320, performing speed calculation and position calculation on the inertial navigation system to be calibrated by using the speed data and the position data of each particle after iteration to obtain the east-direction speed, the north-direction speed, the latitude and the longitude output by the inertial navigation system to be calibrated at the time t, then calculating the value of the cost function of the inertial navigation system to be calibrated through the cost function of the inertial navigation system to be calibrated, and calibrating the particle with the minimum cost function value as the optimal particle of the current group of the iteration; the cost function is calculated according to equation (3):
Figure BDA0002540200070000078
wherein, t 0 Calculating starting time of the inertial navigation system to be calibrated; t is t e The resolving end time of the inertial navigation system to be calibrated is obtained;
Figure BDA0002540200070000079
calculating an error for the east speed of the inertial navigation system to be calibrated at the time t according to the formula (4):
Figure BDA00025402000700000710
wherein, V E,SINS (t) the east-direction speed output by the inertial navigation system to be calibrated at the moment t; v E,refer And (t) is the east reference speed output by the external navigation system at the time t.
Figure BDA00025402000700000711
Calculating an error for the north direction speed of the inertial navigation system to be calibrated at the time t according to the formula (5):
Figure BDA00025402000700000712
wherein, V N,SINS (t) the north velocity output by the inertial navigation system to be calibrated at the moment t; v N,refer And (t) is the northbound reference speed output by the external navigation system at the time t.
e Lat (t) calculating a latitude resolving error of the inertial navigation system to be calibrated at the moment t according to the formula (6):
e Lat (t)＝P Lat,SINS (t)-P Lat,refer (t) (6)
wherein, P Lat,SINS (t) the latitude output by the inertial navigation system to be calibrated at the moment t; p is Lat,refer And (t) is the reference latitude output by the external navigation system at the time t.
e Lon (t) calculating the longitude resolving error of the inertial navigation system to be calibrated at the moment t according to the formula (7):
e Lon (t)＝P Lon,SINS (t)-P Lon,refer (t) (7)
wherein, P Lon,SINS (t) longitude output by the inertial navigation system to be calibrated at the moment t; p Lon,refer And (t) is the reference longitude output by the external navigation system at the time t.
ρ 1 、ρ 2 、ρ 3 、ρ 4 The weight coefficient is preset manually; in this embodiment, ρ is set 1 ＝1.0，ρ 2 ＝1.0，ρ 3 ＝1.0，ρ 4 ＝15。
In this step, if the inertial navigation system to be calibrated is in a static condition, calculating an adaptive value function value of each particle according to the cost function, and calibrating the particle with the largest adaptive value function value as the optimal particle of the current group of the iteration; the fitness function is calculated as equation (8):
F＝1/(J+e′) (8)
wherein e' is an infinitesimal quantity; in this embodiment, setting e' =10 -10
When the inertial navigation system to be calibrated is in a static condition, the reason for adopting the adaptive value function to screen the optimal particles of the current population is as follows: the cost function is determined by the sum of the velocity error and the position error at each time instant. And because the gyro constant drift can cause oscillation type and accumulation type navigation resolving errors, the influence rule of the gyro constant drift on the navigation resolving errors can be more directly reflected by adopting the cost function expression, so that the gyro constant drift can be accurately searched and estimated according to the adaptive value calculating result.
In order to eliminate the influence of random noise of the device on the experimental result, the measurement is repeated for 50 times, and then the average value of the constant drift of the gyroscope is obtained
Figure BDA0002540200070000091
(the unit is in °/h, the same applies below).
S400, acquiring a gyro constant drift amount of the inertial navigation system to be calibrated by using the position data of the current group optimal particles; and then, carrying out error calibration on the gyro measurement value of the inertial navigation system to be calibrated by utilizing the gyro constant drift amount, and carrying out pure inertial navigation resolving on the calibrated gyro measurement value to obtain the calibrated gyro measurement value.
Wherein the calibrated gyro measurement value is calculated according to the formula (9):
Figure BDA0002540200070000092
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0002540200070000093
the measured value of the gyroscope of the inertial navigation system to be calibrated is obtained;
Figure BDA0002540200070000094
position data of the current population optimal particles output when iteration is terminated;
Figure BDA0002540200070000095
and the measured value is the calibrated gyro value.
IN order to compare the accuracy advantages of the measured value of the gyroscope of the inertial navigation system to be calibrated obtained by PSO-calculation compared with the prior art, the IN-calculation and the KF-calculation are respectively adopted again for the inertial navigation system to be calibrated under the condition that other parameters are completely the same, and the following results are obtained: the longitude error and the constant latitude drift amount obtained by KF-resolving are respectively 0.9720 'and 5.2560'; the longitude error and the constant latitude drift obtained by IN-solution are 7.0781 'and 1.8965', respectively. The longitude error and the constant latitude drift obtained by PSO-solution are respectively 0.0139 'and 0.0502'. It can be clearly seen that the PSO algorithm can intelligently and accurately search and estimate the constant drift of the gyroscope and compare the advantage of the constant drift of the gyroscope estimated by the KF algorithm under the condition of a static base, and the feasibility and the effectiveness of the navigation error suppression method in the inertial navigation system based on the PSO algorithm optimization are verified.
The PSO-solution versus KF-solution and IN-solution specific error calibration pairs are shown IN Table 2:
table 2 table comparing errors of the present invention with those of the prior art
Figure BDA0002540200070000096
Figure BDA0002540200070000101
As can be seen from Table 2, the PSO-solution has a higher pure inertial navigation accuracy than the KF-solution and the IN-solution, which also indicates that the PSO algorithm can search for and estimate a more accurate gyro constant drift than the KF algorithm under the static base condition.
The above embodiments are only used for illustrating the design idea and features of the present invention, and the purpose of the present invention is to enable those skilled in the art to understand the content of the present invention and implement it accordingly, and the protection scope of the present invention is not limited to the above embodiments. Therefore, all equivalent changes and modifications made in accordance with the principles and concepts disclosed herein are intended to be included within the scope of the present invention.

Claims (4)

1. A method for calculating a gyro measurement value of an inertial navigation system based on a particle swarm optimization algorithm is suitable for improving the measurement accuracy of the gyro measurement value of the inertial navigation system to be calibrated in a motion process, and is characterized in that: it comprises the following steps:
s100, acquiring a measured value of the inertial navigation system to be calibrated at each correction time point in a time period from the moment when the inertial navigation system to be calibrated starts to autonomously propel to the moment when the inertial navigation system to be calibrated reaches a target position preset manually; the correction time point corresponds to the acquisition frequency from the starting time point of the inertial navigation system to be calibrated; the measurement value of the inertial navigation system to be calibrated comprises current speed measurement data and current position measurement data;
s200, constructing a particle population according to the gyro constant drift amount, and randomly initializing particles in the variation range of the gyro constant drift amount; the particles contain velocity data and position data;
s300, resolving the speed data and the position data of each particle to obtain the optimal particle, and specifically comprises the following steps:
s310, iteratively calculating the speed data and the position data of each particle until the maximum value of the iteration times is reached, and then outputting the speed data and the position data of each particle after iteration; the iteration times are constant and are preset manually; if the speed data or the position data obtained by one iteration calculation exceeds the variation range of the gyro constant drift amount in the iteration process, outputting the speed data and the position data obtained by the last iteration calculation of the iteration calculation, and then outputting the speed data and the position data of each particle after the iteration;
s320, performing speed calculation and position calculation on the inertial navigation system to be calibrated by using the speed data and the position data of each particle after iteration to obtain the east-direction speed, the north-direction speed, the latitude and the longitude output by the inertial navigation system to be calibrated at the time t, then calculating the value of the cost function of the inertial navigation system to be calibrated through the cost function of the inertial navigation system to be calibrated, and calibrating the particle with the minimum cost function value as the optimal particle of the current group of the iteration; the cost function is calculated as follows:
Figure FDA0002540200060000011
wherein, t 0 Calculating starting time of the inertial navigation system to be calibrated; t is t e The resolving end time of the inertial navigation system to be calibrated is obtained;
Figure FDA0002540200060000021
resolving an error for the east speed of the inertial navigation system to be calibrated at the time t;
Figure FDA0002540200060000022
resolving an error for the north speed of the inertial navigation system to be calibrated at the time t; e.g. of the type Lat (t) resolving errors of the latitude of the inertial navigation system to be calibrated at the moment t; e.g. of the type Lon (t) resolving an error of the longitude of the inertial navigation system to be calibrated at the moment t; ρ is a unit of a gradient 1 、ρ 2 、ρ 3 、ρ 4 The weight coefficient is preset manually;
s400, acquiring a gyro constant drift amount of the inertial navigation system to be calibrated by using the position data of the current group optimal particles; and then deducting the gyro constant drift amount of the inertial navigation system to be calibrated from the gyro measured value of the inertial navigation system to be calibrated to obtain a calibrated gyro measured value.
2. The method for calculating the gyro measurement value of the inertial navigation system based on the particle swarm optimization algorithm according to claim 1, wherein the method comprises the following steps: the step S100 of obtaining the measured value of the inertial navigation system to be calibrated comprises the following steps:
s110, measuring current angular acceleration measurement data by a gyroscope loaded on the inertial navigation system to be calibrated, and measuring current linear acceleration measurement data by a linear accelerometer;
s120, resolving current angular acceleration measurement data and current linear acceleration measurement data in real time by adopting a mechanical programming equation to obtain current speed measurement data and current position measurement data of the inertial navigation system to be calibrated; and packaging the current speed measurement data and the current position measurement data, and outputting the data serving as the measurement value of the inertial navigation system to be calibrated.
3. The method for calculating the gyro measurement value of the inertial navigation system based on the particle swarm optimization algorithm according to claim 2, wherein the method comprises the following steps: the speed data comprises a current speed and an updated speed;
the position data comprises a current position, an updated position, a current individual optimal position and a current group optimal position;
the iterative calculation of the velocity data of each particle in S310 is calculated as follows:
Figure FDA0002540200060000023
wherein the content of the first and second substances,
Figure FDA0002540200060000024
is the velocity of the particle after update;
Figure FDA0002540200060000025
is the current position of the particle;
Figure FDA0002540200060000026
is the current velocity of the particle;
Figure FDA0002540200060000027
the position of the optimal particle for the current individual;
Figure FDA0002540200060000028
the position of the optimal particle for the current population; r is a radical of hydrogen 1 And r 2 Is a random number between (0, 1); c. C 1 The value range of the learning factor for representing the learning ability of the particle to the particle is (0, 2); c. C 2 The value range of the learning factor for representing the learning ability of the particle to other particles is (0, 2); c. C 1 And c 2 Random within a value range; omega is an inertia weight constant; d is the dimension of the search space, d =1,2, \ 8230, K is the number of the dimension of the search space; i =1,2, \ 8230, N, N is the population size; m is the iteration number of the current population;
the iterative calculation of the position data for each particle is calculated as follows:
Figure FDA0002540200060000031
wherein the content of the first and second substances,
Figure FDA0002540200060000032
is the updated position of the particle.
4. The method for calculating the gyro measurement value of the inertial navigation system based on the particle swarm optimization algorithm according to claim 1, wherein the method comprises the following steps: in S400, the calibrated gyro measurement value is calculated according to the following formula:
Figure FDA0002540200060000033
wherein the content of the first and second substances,
Figure FDA0002540200060000034
the measured value of the gyroscope of the inertial navigation system to be calibrated is obtained;
Figure FDA0002540200060000035
position data of the current population optimal particles output when iteration is terminated;
Figure FDA0002540200060000036
and the measured value is the calibrated gyro measured value.
CN202010544718.2A 2020-06-15 2020-06-15 Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm Active CN111832690B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010544718.2A CN111832690B (en) 2020-06-15 2020-06-15 Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010544718.2A CN111832690B (en) 2020-06-15 2020-06-15 Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm

Publications (2)

Publication Number Publication Date
CN111832690A CN111832690A (en) 2020-10-27
CN111832690B true CN111832690B (en) 2022-10-11

Family

ID=72897647

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010544718.2A Active CN111832690B (en) 2020-06-15 2020-06-15 Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm

Country Status (1)

Country Link
CN (1) CN111832690B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112880704A (en) * 2021-01-19 2021-06-01 中国人民解放军海军工程大学 Intelligent calibration method for fiber optic gyroscope strapdown inertial navigation system
CN113686338A (en) * 2021-08-12 2021-11-23 上海哈航惯性科技有限公司 Route optimization method based on inertial navigation equipment
CN114485641B (en) * 2022-01-24 2024-03-26 武汉梦芯科技有限公司 Attitude calculation method and device based on inertial navigation device navigation azimuth fusion

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106990426B (en) * 2017-03-16 2020-04-10 北京无线电计量测试研究所 Navigation method and navigation device
CN110031882B (en) * 2018-08-02 2023-05-30 哈尔滨工程大学 External measurement information compensation method based on SINS/DVL integrated navigation system
CN110780326A (en) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 Vehicle-mounted integrated navigation system and positioning method

Also Published As

Publication number Publication date
CN111832690A (en) 2020-10-27

Similar Documents

Publication Publication Date Title
CN111832690B (en) Gyro measurement value calculation method of inertial navigation system based on particle swarm optimization algorithm
CN106990426B (en) Navigation method and navigation device
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
KR101988786B1 (en) Initial alignment of inertial navigation devices
US8645063B2 (en) Method and system for initial quaternion and attitude estimation
CN104344837B (en) Speed observation-based redundant inertial navigation system accelerometer system level calibration method
CN104344836B (en) Posture observation-based redundant inertial navigation system fiber-optic gyroscope system level calibration method
EP1941236B1 (en) Systems and methods for reducing vibration-induced errors in inertial sensors
EP2187170A2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
CN110715659A (en) Zero-speed detection method, pedestrian inertial navigation method, device and storage medium
KR101739390B1 (en) Method for improving the accuracy of self-alignment about the inertial navigation system through gravitational error compensation
CN110887480B (en) Flight attitude estimation method and system based on MEMS sensor
CN104019828A (en) On-line calibration method for lever arm effect error of inertial navigation system in high dynamic environment
CN112146655B (en) Elastic model design method for BeiDou/SINS tight integrated navigation system
CN109945859B (en) Kinematics constraint strapdown inertial navigation method of self-adaptive H-infinity filtering
EP3527948B1 (en) Air data aided inertial measurement unit
CN112197765B (en) Method for realizing fine navigation of underwater robot
CN109612460B (en) Plumb line deviation measuring method based on static correction
de La Parra et al. Low cost navigation system for UAV's
CN110873563B (en) Cloud deck attitude estimation method and device
CN116007620A (en) Combined navigation filtering method, system, electronic equipment and storage medium
CN108627152A (en) A kind of air navigation aid of the miniature drone based on Fusion
JP2021518529A (en) How to calibrate the gyrometer installed in the vehicle
CN113566850B (en) Method and device for calibrating installation angle of inertial measurement unit and computer equipment
CN111982126B (en) Design method of full-source BeiDou/SINS elastic state observer model

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