CN106323334B - A kind of magnetometer calibration method based on particle group optimizing - Google Patents
A kind of magnetometer calibration method based on particle group optimizing Download PDFInfo
- Publication number
- CN106323334B CN106323334B CN201510359044.8A CN201510359044A CN106323334B CN 106323334 B CN106323334 B CN 106323334B CN 201510359044 A CN201510359044 A CN 201510359044A CN 106323334 B CN106323334 B CN 106323334B
- Authority
- CN
- China
- Prior art keywords
- magnetometer
- indicate
- particle
- acc
- group optimizing
- 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
Links
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
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Measuring Magnetic Variables (AREA)
Abstract
The present invention provides a kind of magnetometer calibration method based on particle group optimizing, including data acquired in the magnetometer in acquisition certain time；It obtains magnetometer and is presently in longitude and latitude and altitude information at position, to calculate the magnetic induction intensity that magnetometer is presently at position；Establish magnetometer calibration model；Construct fitness function；Execute particle swarm optimization algorithm；The corresponding parameter in particle group optimizing global optimum position obtained is substituted into magnetometer calibration model, to calibrate to the data that magnetometer obtains；Orientation estimation is carried out according to data acquired in the magnetometer after gyroscope, accelerometer and calibration.Magnetometer calibration method based on particle group optimizing of the invention can be realized the calibration of a variety of errors by particle swarm optimization algorithm；Without precision instrument, calibration process is simple, and computation complexity is relatively low, and precision is higher；Direction drift greatly reduces, and effectively increases the precision of indoor positioning.
Description
Technical field
The present invention relates to the technical fields of indoor positioning navigation, more particularly to a kind of magnetometer based on particle group optimizing
Calibration method.
Background technique
Indoor positioning technologies based on inertial sensor are the suitable portions that Inertial Measurement Unit (IMU) is mounted on to body
Position carries out the positioning of personnel by dead reckoning.Due to IMU have it is small in size, cheap, easy to carry, be easily integrated, from
The advantages that complete, so that its indoor positioning technologies for being substantially distinguished from other classifications.
Currently, the investment of various research institutions and commercial company in this respect is increasing.But due to sensor itself
Error and the presence of other errors lead to the accumulation of location error, and this error can not be eliminated thoroughly, only can be always
Accumulation, so that positioning accuracy is unable to satisfy application demand.And an important factor for wherein location error generates is to direction of travel
The deviation of deduction.Minor shifts on direction frequently can lead to position and serious offset occur.
Magnetometer calculates the relationship between component by perception earth's magnetic field to calculate direction.Electronic compass is exactly according to this
One principle guides direction.In the indoor locating system based on inertial sensor and magnetometer, magnetometer is introduced to calculate in real time
Direction achievees the purpose that eliminate deflection error accumulation with this.
But indoor environment is different from outdoor environment, by external interference smaller, magnetometer very clean in outdoor electromagnetic environment
The direction of calculating is also more accurate and relatively stable.Indoors, electromagnetic environment is extremely complex, such as building bar construction,
WIFI, household electrical appliance, electric wire etc. device can all generate certain influence to magnetic field.According to different error sources, magnetometer
Error can be divided into soft iron error, hard iron error, non-orthogonal errors, drift, errors of proportional factor etc..These sensors from
Under the interference in body and the external world, very big deviation can occur for the direction that magnetometer calculates, so carrying out before using magnetometer
Calibration is completely necessary.
In the prior art, in order to eliminate magnetometer error, it is generally the case that the simplest method is to eliminate offset calibration
Method.It is maximum to find each axis by enabling XYZ axis place perpendicular to horizontal plane respectively, and around the axis rotating acquisition data for this method
Minimum value averages to calculate offset, is calibrated by subtracting offset.However this method is mainly used for calibration firmly
Iron drift, can not calibrate other errors.In addition, genetic algorithm is also used for the calibration of magnetometer, but genetic algorithm needs
Genetic operator is calculated by cross product and variation, computation complexity is higher.
Summary of the invention
In view of the foregoing deficiencies of prior art, the purpose of the present invention is to provide a kind of magnetic based on particle group optimizing
Power meter calibrating method misses the hard iron error of magnetometer, soft iron error, non-orthogonal errors, scale factor by particle group optimizing
Difference and drift are uniformly calibrated, at the same merge gyroscope, accelerometer calculates navigation attitude in indoor positioning, provide opposite
Accurate navigation attitude reference improves positioning accuracy to reduce deflection error accumulation.
In order to achieve the above objects and other related objects, the present invention provides a kind of magnetometer calibration based on particle group optimizing
Method, comprising the following steps: step S1, in the experimental site far from electromagnetic interference, arbitrarily rotation magnetometer, acquire certain time
Data acquired in interior magnetometer；Step S2, it obtains magnetometer and is presently in longitude and latitude and altitude information at position；Step
S3, the magnetic induction intensity at position is presently according to acquired longitude and latitude and altitude information calculating magnetometer；Step S4, root
Magnetometer calibration model is established according to magnetometer error model；Step S5, fitness function is constructed；Step S6, it is excellent to execute population
Change algorithm；Step S7, the corresponding parameter in particle group optimizing global optimum position obtained is substituted into magnetometer calibration model, with
The data obtained to magnetometer are calibrated；Step S8, the number according to acquired in the magnetometer after gyroscope, accelerometer and calibration
According to progress orientation estimation.
According to the above-mentioned magnetometer calibration method based on particle group optimizing, in which: in the step S3, by longitude and latitude and
Altitude information substitutes into international geomagnetic reference field model the magnetic induction intensity for calculating current position；Used magnetic induction intensity
Calculation formula it is as follows:
Wherein r indicates that the radial distance r=a+h, a that leave the earth's core indicate that earth reference radius, h indicate height above sea level, θ table
Showing geocentric colatitude, φ indicates that east longitude, t indicate the time to be inquired,WithIt is gaussian coefficient,Indicate that n rank m times apply is close
The special Legendre function that associates of partly formatting；L indicates the maximum order of spheric harmonics expansion, and l indicates the order in integral process.
According to the above-mentioned magnetometer calibration method based on particle group optimizing, in which: in the step S4, the mistake of magnetometer
Differential mode type are as follows:
Wherein B indicates the magnetometer measures value under sensor coordinate system；Indicate earth-magnetic field vector；A=CNCS(CSI+
I3×3),CNIndicate non-orthonormal matrix；CSIndicate scale factor matrix；CSIIndicate soft iron matrix；It indicates
Hard iron error under sensor coordinate system；Indicate the drift under sensor coordinate system；wsIndicate white Gaussian noise, I3×3Indicate 3
The unit matrix that row 3 arranges.
Further, the magnetometer calibration method according to above-mentioned based on particle group optimizing, in which: magnetometer calibration model
ForWhereinThe output of magnetometer after indicating calibration.
According to the above-mentioned magnetometer calibration method based on particle group optimizing, in which: in the step S5, the fitness
Function are as follows:
Wherein, B indicates the magnetometer measures value under sensor coordinate system, argminf (T, bs) indicate that function f is made to take minimum
The T of value, bsValue, T=A-1, r0Indicate that magnetometer is presently in the magnetic induction intensity of position；A=CNCS(CSI+I3×3),CNIndicate non-orthonormal matrix；CSIndicate scale factor matrix；CSIIndicate soft iron matrix；I3×3Indicate that 3 rows 3 arrange
Unit matrix；Indicate the hard iron error under sensor coordinate system；Indicate the drift under sensor coordinate system, N indicates magnetic
The sampling number of power meter output.
According to the above-mentioned magnetometer calibration method based on particle group optimizing, in which: the step S6 the following steps are included:
61) particle number m, the number of iterations k, particle dimension n, velocity interval, position range, Studying factors c are initialized1,c2
And stop condition；The stop condition is that the number of iterations reaches preset value or the fitness value of global optimum's particle is less than in advance
If threshold value；
62) position where m particle being randomly generated is set to the current local optimum position of each particle, root
The fitness value that all particles are calculated according to fitness function obtains the smallest particle of fitness value, and set its position as it is global most
Excellent position；
63) according to the renewal equation of speed and position in velocity interval and position range the speed of more new particle and position
It sets；Wherein, the renewal equation of speed and position is respectively as follows:
Wherein, i indicates that i-th of particle, j indicate that the jth of particle ties up variable,Indicate the local optimum position of particle i
The corresponding position of jth dimension variable,Indicate the current position of particle i jth dimension variable,Indicate global optimum position
Jth dimension variable position, vijIndicate the current speed of particle i jth dimension variable；r1And r2Indicate range between (0,1)
Random scale factor；
64) fitness value for recalculating all particles, the fitness value f that each particle is recalculatediWith the particle
The fitness value of local optimum positionCompare, ifSet the current location of particle then as local optimum position
It sets, otherwise keeps local optimum position constant；
65) fitness value of the local optimum position of more all particles chooses the smallest local optimum position of fitness value
It sets, and by its fitness value value fpbestWith the fitness value f of global optimum positiongbestIt is compared, if fpbest<fgbest, then
If fpbestCorresponding position is global optimum position, otherwise keeps current global optimum position constant；
66) it is persistently iterated, the fitness value until reaching default the number of iterations or global optimum position is less than pre-
If threshold value, so that it is determined that global optimum position.
According to the above-mentioned magnetometer calibration method based on particle group optimizing, in which: the step S8 the following steps are included:
81) judge whether Inertial Measurement Unit remains static；
82) when Inertial Measurement Unit remains static, roll angle ψ is calculated using three axis components of accelerometeraccWith bow
Elevation angle thetaacc, the data of gyroscope acquisition are integrated to calculate real-time roll angle ψgyro, pitching angle thetagyroAnd course angle
And it is merged by roll angle and pitch angle of the complementary filter to gyroscope and accelerometer calculating；
83) direction data calculation obtained using magnetometer:
WhereinFor the direction that magnetometer calculates, magx、magy、magzThe respectively magnetic induction intensity of three axis of magnetometer,
ψgyro_accAnd θgyro_accRespectively fused roll angle and pitch angle；
84) direction that the course angle and magnetometer that fusion gyroscope obtains calculate, with the direction after being calibrated.
Magnetometer calibration method according to claim 7 based on particle group optimizing, it is characterised in that: step 81)
In, it calculatesIf normacc=g, then Inertial Measurement Unit remains static；It is no
Then Inertial Measurement Unit is kept in motion, and wherein g is local gravitational acceleration；accx,accy,acczRespectively indicate accelerometer
Measure X acquired in Inertial Measurement Unit, Y, the initial data of Z axis.
Further, the magnetometer calibration method according to above-mentioned based on particle group optimizing, in which: in step 82), use
Following formula merges the roll angle and pitch angle of gyroscope and accelerometer calculating:
ψgyro_acc=a ψgyro+(1-a)ψacc
θgyro_acc=a θgyro+(1-a)θacc
Wherein a is weight coefficient.
Further, the magnetometer calibration method according to above-mentioned based on particle group optimizing, in which: in the step 84),
The fusion of the course angle of gyroscope acquisition and the direction of magnetometer calculating is carried out using following formula:
Wherein b is weight coefficient,The as direction of final output,For gyroscope obtain course angle,For
The direction that magnetometer calculates.
As described above, the magnetometer calibration method of the invention based on particle group optimizing, has the advantages that
(1) by particle swarm optimization algorithm, magnetometer soft iron error, hard iron error, non-orthogonal errors, zero be can be realized
The calibration of a variety of errors such as drift, errors of proportional factor；
(2) it is not necessarily to precision instrument, calibration process is simple, and computation complexity is relatively low, and precision is higher；
(3) direction drift greatly reduces, and effectively increases the precision of indoor positioning.
Detailed description of the invention
Fig. 1 is shown as the flow chart of the magnetometer calibration method of the invention based on particle group optimizing；
Fig. 2 is shown as magnetometer error model and calibrating patterns of the invention；
Fig. 3 is shown as particle swarm optimization algorithm flow chart of the invention；
Fig. 4 is shown as attitude heading reference system algorithm flow chart of the invention.
Specific embodiment
Illustrate embodiments of the present invention below by way of specific specific example, those skilled in the art can be by this specification
Other advantages and efficacy of the present invention can be easily understood for disclosed content.The present invention can also pass through in addition different specific realities
The mode of applying is embodied or practiced, the various details in this specification can also based on different viewpoints and application, without departing from
Various modifications or alterations are carried out under spirit of the invention.
It should be noted that the basic conception that only the invention is illustrated in a schematic way is illustrated provided in the present embodiment,
Then only shown in schema with it is of the invention in related component rather than component count, shape and size when according to actual implementation draw
System, when actual implementation kenel, quantity and the ratio of each component can arbitrarily change for one kind, and its assembly layout kenel can also
It can be increasingly complex.
Magnetometer calibration method based on particle group optimizing of the invention according to magnetometer error type, as hard iron error,
Soft iron error, non-orthogonal errors, errors of proportional factor, drift etc., establish error model, invert to obtain calibrating die to error model
Type；And fitness function is not established with the feature that sensor orientation changes and changes according to magnetic induction intensity, then in above-mentioned base
Magnetometer calibration is carried out using particle swarm optimization algorithm on plinth；Finally gyroscope and accelerometer is combined to merge by complementary filter
The decision in direction is carried out, to obtain the higher attitude heading reference system of precision, to substantially reduce direction drift, improves interior
The precision of positioning.
Referring to Fig.1, the magnetometer calibration method of the invention based on particle group optimizing the following steps are included:
Step S1, in the experimental site far from electromagnetic interference, arbitrarily rotation magnetometer, the magnetometer in certain time is acquired
Acquired data.
Wherein, the experimental site far from electromagnetic interference refers to apart from electronics, electromagnetic equipment and irony object farther out, relatively empty
Spacious place.
Certain time is related with sample rate, without specific range, is generally no less than 16 points.The data of acquisition are more,
Precision is higher, and corresponding computational efficiency is lower.So needing to do a balance between acquisition data volume and computational efficiency.
Step S2, it obtains magnetometer and is presently in longitude and latitude and altitude information at position.
Specifically, it can use GPS receiver to obtain the longitude and latitude and altitude information that magnetometer is presently at position.
Step S3, magnetometer is calculated according to acquired longitude and latitude and altitude information to be presently in the magnetic induction at position strong
Degree.
It specifically, can be by following two method come calculated magnetic induction intensity.
1) longitude and latitude and altitude information are substituted into international geomagnetic reference field (International Geomagnetic
Reference Field, IGRF) magnetic induction intensity of current position can be calculated in model.Used magnetic induction intensity
Calculation formula is as follows:
Wherein r indicates that the radial distance r=a+h, a=6371.2km that leave the earth's core indicate that earth reference radius, h indicate sea
Degree of lifting, θ indicate geocentric colatitude, and φ indicates that east longitude, t indicate the time to be inquired,WithIt is gaussian coefficient,Indicate n
Rank m times Schmidt partly formats the Legendre function that associates；L indicates the maximum order of spheric harmonics expansion, and l indicates integral process
In order.
2) magnetic induction intensity at position is presently in by obtaining magnetometer in line computation in following website:
Http:// www.ngdc.noaa.gov/geomag-web/? model=igrf#igrfwmm
Step S4, calibrating patterns are established according to magnetometer error model.
As shown in Fig. 2, establishing magnetometer calibration model according to the inverse of magnetometer error model.Specifically include following step
It is rapid:
51) in view of hard iron error, soft iron error, errors of proportional factor, drift and non-orthogonal errors the case where, magnetic force
The error model of meter are as follows:
Wherein, B indicates the magnetometer measures value under sensor coordinate system；CNIndicate non-orthonormal matrix；CSIndicate scale factor
Matrix；Indicate the spin matrix from terrestrial coordinate system to sensor coordinate system；Indicate the earth's magnetic field under terrestrial coordinate system
Value；Indicate the hard iron error under sensor coordinate system；Indicate the soft iron error under sensor coordinate system；Indicate sensing
Drift under device coordinate system；wsIndicate white Gaussian noise.
In most cases, soft iron error can be indicated with a linear model:Therefore, magnetic force
Meter error model can be expressed asWherein A=CNCS(CSI+I3×3),CSIIt indicates
Soft iron matrix,Indicate earth-magnetic field vector, I3×3Indicate the unit matrix of 3 rows 3 column, i.e.,
52) it inverts to magnetometer error module, obtaining magnetometer calibration model isWherein
The output of magnetometer after indicating calibration.
Step S5, fitness function is constructed.
Specifically, the principle that will not be changed with the rotation of magnetometer according to magnetic induction intensity modulus value, enables fitness function
Are as follows:
Wherein, argminf (T, bs) indicate the T, b that are minimized function fsValue.Wherein T=A-1, r0Indicate magnetometer
It is presently in the magnetic induction intensity of position；N indicates the sampling number of magnetometer output.
Step S6, particle swarm optimization algorithm is executed.
As shown in figure 3, particle swarm optimization algorithm the following steps are included:
61) particle number m, the number of iterations k, particle dimension n, velocity interval [- V are initializedmax,Vmax], position range,
Practise factor c1,c2And stop condition.
Specifically, stop condition is that the number of iterations reaches the fitness value of preset value or global optimum's particle less than default
Threshold value.
62) position where m particle being randomly generated is set to the current local optimum position of each particle, root
The fitness value that all particles are calculated according to fitness function obtains the smallest particle of fitness value, and set its position as it is global most
Excellent position.
63) according to the renewal equation of speed and position in velocity interval and position range the speed of more new particle and position
It sets.
Wherein, the renewal equation of speed and position is respectively as follows:
Wherein, i indicates i-th of particle, i ∈ [1, m]；J indicates that the jth of particle ties up variable, j ∈ [1, n]；Indicate grain
The corresponding position of jth dimension variable of the local optimum position of sub- i,Indicate the current position of particle i jth dimension variable,Indicate the position of the jth dimension variable of global optimum position, vijIndicate the current speed of particle i jth dimension variable；r1And r2?
Indicate random scale factor of the range between (0,1).
64) fitness value for recalculating all particles, the fitness value f that each particle is recalculatediWith the particle
The fitness value of local optimum positionCompare, ifSet the current location of particle then as local optimum position
It sets, otherwise keeps local optimum position constant.
65) fitness value of the local optimum position of more all particles chooses the smallest local optimum position of fitness value
It sets, and by its fitness value value fpbestWith the fitness value f of global optimum positiongbestIt is compared, if fpbest<fgbest, then
If fpbestCorresponding position is global optimum position, otherwise keeps current global optimum position constant.
66) it is persistently iterated, until reaching stop condition, that is, reaches default the number of iterations or global optimum's particle
Fitness value is less than preset threshold value th, i.e. fgbest< th, so that it is determined that global optimum position.
Step S7, by the parameter T, b of particle group optimizing global optimum position obtainedsMagnetometer calibration model is substituted into,
To be calibrated to the data that magnetometer obtains.
Step S8, the data according to acquired in the magnetometer after gyroscope, accelerometer and calibration carry out orientation estimation.
As shown in figure 4, step S8 specifically includes the following steps:
81) judge whether Inertial Measurement Unit remains static.
Wherein, judgment method are as follows: calculateIf normacc=g, then inertia is surveyed
Amount unit remains static；Otherwise Inertial Measurement Unit is kept in motion, and wherein g is local gravitational acceleration.
Wherein, accx,accy,acczRespectively indicate accelerometer measurement Inertial Measurement Unit acquired in X, Y, Z axis it is original
Data.
82) when Inertial Measurement Unit remains static, roll angle ψ is calculated using three axis components of accelerometeraccWith bow
Elevation angle thetaacc, the data of gyroscope acquisition are integrated to calculate real-time roll angle ψgyro, pitching angle thetagyroAnd course angle
And it is merged by roll angle and pitch angle of the complementary filter to gyroscope and accelerometer calculating.
Since the angle of gyroscope calculating is there are low frequency aberration, accelerating the angle calculated, there are high frequency errors, so passing through
Complementary filter merges the roll angle and pitch angle of gyroscope and accelerometer calculating.It is specific as follows:
ψgyro_acc=a ψgyro+(1-a)ψacc
θgyro_acc=a θgyro+(1-a)θacc
Wherein a is weight coefficient, is set with specific reference to experience.
83) direction data calculation obtained using magnetometer:
WhereinFor the direction that magnetometer calculates, magx、magy、magzThe respectively magnetic induction intensity of three axis of magnetometer.
84) direction that the course angle and magnetometer that fusion gyroscope obtains calculate, with the direction after being calibrated.
Interference due to magnetometer vulnerable to external electromagnetic field, the direction calculated when interfering larger will appear some inclined
Difference.So determining the confidence level in the direction of magnetometer output by the method for disturbance of magnetic field detection.It, can when disturbing larger
Reliability is set as lesser value, when disturbing small, sets biggish value for confidence level.Fusion formula is as follows:
Wherein b is directly proportional to disturbance of magnetic field size, is weight coefficient,The as direction of final output.
It should be noted that the execution sequence of step S1 and step S2-S3 be not it is fixed, can according to the actual situation with
Machine executes.
In conclusion the magnetometer calibration method of the invention based on particle group optimizing passes through particle swarm optimization algorithm, energy
Enough realize the calibration of a variety of errors such as magnetometer soft iron error, hard iron error, non-orthogonal errors, drift, errors of proportional factor；Nothing
Precision instrument is needed, calibration process is simple, and computation complexity is relatively low, and precision is higher；Direction drift greatly reduces, and effectively mentions
The high precision of indoor positioning.So the present invention effectively overcomes various shortcoming in the prior art and has high industrial utilization
Value.
The above-described embodiments merely illustrate the principles and effects of the present invention, and is not intended to limit the present invention.It is any ripe
The personage for knowing this technology all without departing from the spirit and scope of the present invention, carries out modifications and changes to above-described embodiment.Cause
This, institute is complete without departing from the spirit and technical ideas disclosed in the present invention by those of ordinary skill in the art such as
At all equivalent modifications or change, should be covered by the claims of the present invention.
Claims (8)
1. a kind of magnetometer calibration method based on particle group optimizing, it is characterised in that: the following steps are included:
Step S1, in the experimental site far from electromagnetic interference, arbitrarily rotation magnetometer, the magnetometer acquired in certain time is obtained
The data taken；
Step S2, it obtains magnetometer and is presently in longitude and latitude and altitude information at position；
Step S3, magnetometer is calculated according to acquired longitude and latitude and altitude information and is presently in the magnetic induction intensity at position；
Step S4, magnetometer calibration model is established according to magnetometer error model；
Step S5, fitness function is constructed；
Step S6, particle swarm optimization algorithm is executed；
Step S7, the corresponding parameter in particle group optimizing global optimum position obtained is substituted into magnetometer calibration model, with right
The data that magnetometer obtains are calibrated；
Step S8, the data according to acquired in the magnetometer after gyroscope, accelerometer and calibration carry out orientation estimation；
In the step S5, the fitness function are as follows:
Wherein, B indicates the magnetometer measures value under sensor coordinate system, argminf (T, bs) indicate to be minimized function f
T,bsValue, T=A-1, r0Indicate that magnetometer is presently in the magnetic induction intensity of position；A=CNCS(CSI+I3×3),CNIndicate non-orthonormal matrix；CSIndicate scale factor matrix；CSIIndicate soft iron matrix；I3×3Indicate that 3 rows 3 arrange
Unit matrix；Indicate the hard iron error under sensor coordinate system；Indicate the drift under sensor coordinate system, N indicates magnetic
The sampling number of power meter output；
The step S6 the following steps are included:
61) particle number m, the number of iterations k, particle dimension n, velocity interval, position range, Studying factors c are initialized1,c2And
Stop condition；The stop condition is that the number of iterations reaches the fitness value of preset value or global optimum's particle less than preset
Threshold value；
62) position where m particle being randomly generated is set to the current local optimum position of each particle, according to suitable
Response function calculates the fitness value of all particles, obtains the smallest particle of fitness value, and sets its position as global optimum position
It sets；
63) according to the renewal equation of speed and position in velocity interval and position range the speed of more new particle and position；
Wherein, the renewal equation of speed and position is respectively as follows:
Wherein, i indicates that i-th of particle, j indicate that the jth of particle ties up variable,Indicate the jth of the local optimum position of particle i
The corresponding position of variable is tieed up,Indicate the current position of particle i jth dimension variable,Indicate the jth of global optimum position
Tie up the position of variable, vijIndicate the current speed of particle i jth dimension variable；r1And r2Indicate that range is random between (0,1)
Scale factor；
64) fitness value for recalculating all particles, the fitness value f that each particle is recalculatediMost with particle part
The fitness value f of excellent positioni pbestCompare, if fi<fi pbest, then the current location of particle is set as local optimum position, otherwise
Keep local optimum position constant；
65) fitness value of the local optimum position of more all particles chooses the smallest local optimum position of fitness value, and
By its fitness value fpbestWith the fitness value f of global optimum positiongbestIt is compared, if fpbest<fgbest, then f is setpbest
Corresponding position is global optimum position, otherwise keeps current global optimum position constant；
66) it is persistently iterated, the fitness value until reaching default the number of iterations or global optimum position is less than preset
Threshold value, so that it is determined that global optimum position.
2. the magnetometer calibration method according to claim 1 based on particle group optimizing, it is characterised in that: the step S3
In, longitude and latitude and altitude information are substituted into international geomagnetic reference field model to the magnetic induction intensity for calculating current position；Institute
It is as follows using the calculation formula of magnetic induction intensity:
Wherein r indicates that the radial distance r=a+h, a that leave the earth's core indicate that earth reference radius, h indicate height above sea level, and θ indicates ground
Heart colatitude, φ indicate that east longitude, t indicate the time to be inquired,WithIt is gaussian coefficient,Indicate that n' rank m' times apply is close
The special Legendre function that associates of partly formatting；L indicates the maximum order of spheric harmonics expansion, and l indicates the order in integral process.
3. the magnetometer calibration method according to claim 1 based on particle group optimizing, it is characterised in that: the step S4
In, the error model of magnetometer are as follows:
Wherein B indicates the magnetometer measures value under sensor coordinate system；Indicate earth-magnetic field vector；A=CNCS(CSI+I3×3),CNIndicate non-orthonormal matrix；CSIndicate scale factor matrix；CSIIndicate soft iron matrix；Indicate sensor
Hard iron error under coordinate system；Indicate the drift under sensor coordinate system；wsIndicate white Gaussian noise, I3×3Indicate that 3 rows 3 arrange
Unit matrix.
4. the magnetometer calibration method according to claim 3 based on particle group optimizing, it is characterised in that: magnetometer calibration
Model isWhereinThe output of magnetometer after indicating calibration.
5. the magnetometer calibration method according to claim 1 based on particle group optimizing, it is characterised in that: the step S8
The following steps are included:
81) judge whether Inertial Measurement Unit remains static；
82) when Inertial Measurement Unit remains static, roll angle ψ is calculated using three axis components of accelerometeraccAnd pitch angle
θacc, the data of gyroscope acquisition are integrated to calculate real-time roll angle ψgyro, pitching angle thetagyroAnd course angleAnd lead to
Complementary filter is crossed to merge the roll angle and pitch angle of gyroscope and accelerometer calculating；
83) direction data calculation obtained using magnetometer:
WhereinFor the direction that magnetometer calculates, magx、magy、magzThe respectively magnetic induction intensity of three axis of magnetometer,
ψgyro_accAnd θgyro_accRespectively fused roll angle and pitch angle；
84) direction that the course angle and magnetometer that fusion gyroscope obtains calculate, with the direction after being calibrated.
6. the magnetometer calibration method according to claim 5 based on particle group optimizing, it is characterised in that: in step 81),
It calculatesIf normacc=g, then Inertial Measurement Unit remains static；Otherwise it is used to
Property measuring unit be kept in motion, wherein g be local gravitational acceleration；accx,accy,acczRespectively indicate accelerometer measurement
X acquired in Inertial Measurement Unit, Y, the initial data of Z axis.
7. the magnetometer calibration method according to claim 5 based on particle group optimizing, it is characterised in that: in step 82),
It is merged using roll angle and pitch angle of the following formula to gyroscope and accelerometer calculating:
ψgyro_acc=a ψgyro+(1-a)ψacc
θgyro_acc=a θgyro+(1-a)θacc
Wherein a is weight coefficient.
8. the magnetometer calibration method according to claim 5 based on particle group optimizing, it is characterised in that: the step
84) in, the fusion of the course angle of gyroscope acquisition and the direction of magnetometer calculating is carried out using following formula:
Wherein b is weight coefficient,The as direction of final output,For gyroscope obtain course angle,For magnetometer
The direction of calculating.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510359044.8A CN106323334B (en) | 2015-06-25 | 2015-06-25 | A kind of magnetometer calibration method based on particle group optimizing |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510359044.8A CN106323334B (en) | 2015-06-25 | 2015-06-25 | A kind of magnetometer calibration method based on particle group optimizing |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106323334A CN106323334A (en) | 2017-01-11 |
| CN106323334B true CN106323334B (en) | 2019-06-28 |
Family
ID=57729467
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510359044.8A Active CN106323334B (en) | 2015-06-25 | 2015-06-25 | A kind of magnetometer calibration method based on particle group optimizing |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106323334B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115993133B (en) * | 2021-10-20 | 2024-03-15 | 北京三快在线科技有限公司 | Magnetometer calibration method, magnetometer calibration device, magnetometer calibration equipment and storage medium |
Families Citing this family (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107270940B (en) * | 2017-07-11 | 2019-11-15 | 深圳清华大学研究院 | The output bias estimation method of three axis magnetometer |
| FR3069649B1 (en) * | 2017-07-26 | 2021-01-01 | Sysnav | CALIBRATION PROCESS OF A MAGNETOMETER |
| CN107607899B (en) * | 2017-09-28 | 2019-11-19 | 歌尔科技有限公司 | Magnetometer calibration method and apparatus |
| CN110361683B (en) * | 2019-07-17 | 2021-06-18 | 哈尔滨工程大学 | Magnetometer correction method based on dual-objective-function particle group optimization |
| CN111965579B (en) * | 2020-08-12 | 2022-11-11 | 南方电网数字电网研究院有限公司 | Method and device for calibrating precision of micro current sensor, computer equipment and storage medium |
| CN112461224B (en) * | 2020-11-10 | 2021-09-14 | 武汉大学 | Magnetometer calibration method based on known attitude angle |
| CN112880704A (en) * | 2021-01-19 | 2021-06-01 | 中国人民解放军海军工程大学 | Intelligent calibration method for fiber optic gyroscope strapdown inertial navigation system |
| CN112762965B (en) * | 2021-04-08 | 2021-09-07 | 北京三快在线科技有限公司 | Magnetometer calibration method and device |
| WO2023092392A1 (en) * | 2021-11-25 | 2023-06-01 | 上海健康医学院 | Magnetometer sensor experimental positioning device and method |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102288170A (en) * | 2011-07-14 | 2011-12-21 | 浙江大学 | Correction method of electronic compass in underwater vehicle |
| CN103575293A (en) * | 2012-07-25 | 2014-02-12 | 华为终端有限公司 | Magnetometer direction angle correction method and magnetometer |
| CN103869379A (en) * | 2014-03-24 | 2014-06-18 | 东南大学 | Magnetometer correcting method with optimized and modified BP neural network based on genetic algorithm |
| CN103954303A (en) * | 2014-05-15 | 2014-07-30 | 东南大学 | Method for dynamically calculating and correcting course angle of navigation system of magnetometer |
| CN104165642A (en) * | 2014-08-29 | 2014-11-26 | 东南大学 | Method for directly correcting and compensating course angle of navigation system |
| US8933572B1 (en) * | 2013-09-04 | 2015-01-13 | King Fahd University Of Petroleum And Minerals | Adaptive superconductive magnetic energy storage (SMES) control method and system |
-
2015
- 2015-06-25 CN CN201510359044.8A patent/CN106323334B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102288170A (en) * | 2011-07-14 | 2011-12-21 | 浙江大学 | Correction method of electronic compass in underwater vehicle |
| CN103575293A (en) * | 2012-07-25 | 2014-02-12 | 华为终端有限公司 | Magnetometer direction angle correction method and magnetometer |
| US8933572B1 (en) * | 2013-09-04 | 2015-01-13 | King Fahd University Of Petroleum And Minerals | Adaptive superconductive magnetic energy storage (SMES) control method and system |
| CN103869379A (en) * | 2014-03-24 | 2014-06-18 | 东南大学 | Magnetometer correcting method with optimized and modified BP neural network based on genetic algorithm |
| CN103954303A (en) * | 2014-05-15 | 2014-07-30 | 东南大学 | Method for dynamically calculating and correcting course angle of navigation system of magnetometer |
| CN104165642A (en) * | 2014-08-29 | 2014-11-26 | 东南大学 | Method for directly correcting and compensating course angle of navigation system |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115993133B (en) * | 2021-10-20 | 2024-03-15 | 北京三快在线科技有限公司 | Magnetometer calibration method, magnetometer calibration device, magnetometer calibration equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106323334A (en) | 2017-01-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106323334B (en) | A kind of magnetometer calibration method based on particle group optimizing | |
| CN104897154B (en) | Indoor earth-magnetism navigation method and device based on mobile phone | |
| CN106979780B (en) | A kind of unmanned vehicle real-time attitude measurement method | |
| CN103941309B (en) | Geomagnetic sensor calibrator (-ter) unit and its method | |
| CN110007354B (en) | Device and method for measuring flight parameters of semi-aviation transient electromagnetic receiving coil of unmanned aerial vehicle | |
| CN108458714B (en) | Euler angle solving method without gravity acceleration in attitude detection system | |
| CN104535062A (en) | Movable type location method based on magnetic gradient tensor and geomagnetic vector measurement | |
| CN104459828B (en) | Based on the non-aligned bearing calibration of earth magnetism vector system around method of principal axes | |
| CN107421523A (en) | Azimuth calibration method, apparatus, storage medium and computer equipment | |
| CN104613936B (en) | A kind of tilt measuring method for surveying and drawing all-in-one | |
| CN104316037B (en) | A kind of bearing calibration of electronic compass and device | |
| CN106441372B (en) | A kind of quiet pedestal coarse alignment method based on polarization with gravitation information | |
| CN106767671B (en) | Geologic structure face occurrence calculation method based on three-dimensional electronic compass | |
| CN106643792A (en) | Inertial measurement unit and geomagnetic sensor integrated calibration apparatus and calibration method | |
| CN103808349B (en) | The error calibration method of vector sensor and device | |
| CN106842080B (en) | A kind of magnetic field measuring device posture swing interference minimizing technology | |
| CN105021189A (en) | Method for calibrating postures of mobile equipment by comprehensively utilizing accelerometer and gyroscope | |
| CN109725360A (en) | One-point positioning method based on magnetic gradient tensor invariant | |
| CN107883951A (en) | A kind of computational methods and terminal of underwater robot 3 d pose | |
| CN107390155A (en) | A kind of Magnetic Sensor calibrating installation and method | |
| JPWO2013125242A1 (en) | Offset estimation apparatus, offset estimation method, offset estimation program, and information processing apparatus | |
| CN111121758B (en) | Rapid modeling and credible positioning method for indoor magnetic map | |
| CN109725361A (en) | A kind of locating magnetic objects method based on magnetic gradient tensor invariant | |
| CN110736484B (en) | Background magnetic field calibration method based on fusion of gyroscope and magnetic sensor | |
| Skog et al. | Magnetic odometry-a model-based approach using a sensor array |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |