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 PDF

Info

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
particle
indicate
calibration
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
Application number
CN201510359044.8A
Other languages
Chinese (zh)
Other versions
CN106323334A (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.)
Shanghai Advanced Research Institute of CAS
Original Assignee
Shanghai Advanced Research Institute of CAS
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 Shanghai Advanced Research Institute of CAS filed Critical Shanghai Advanced Research Institute of CAS
Priority to CN201510359044.8A priority Critical patent/CN106323334B/en
Publication of CN106323334A publication Critical patent/CN106323334A/en
Application granted granted Critical
Publication of CN106323334B publication Critical patent/CN106323334B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

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

A kind of magnetometer calibration method based on particle group optimizing
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.
CN201510359044.8A 2015-06-25 2015-06-25 A kind of magnetometer calibration method based on particle group optimizing Active CN106323334B (en)

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)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
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
CN107607899B (en) * 2017-09-28 2019-11-19 歌尔科技有限公司 Magnetometer calibration method and apparatus
CN110361683A (en) * 2019-07-17 2019-10-22 哈尔滨工程大学 Magnetometer bearing calibration based on two-objective programming particle group optimizing

Citations (6)

* Cited by examiner, † Cited by third party
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

Patent Citations (6)

* Cited by examiner, † Cited by third party
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

Also Published As

Publication number Publication date
CN106323334A (en) 2017-01-11

Similar Documents

Publication Publication Date Title
CN105190237B (en) Moving direction Estimating Confidence Interval
Wang et al. A self-calibration method for nonorthogonal angles between gimbals of rotational inertial navigation system
CN104061934B (en) Pedestrian indoor position tracking method based on inertial sensor
Wu et al. Fast complementary filter for attitude estimation using low-cost MARG sensors
CN104718561B (en) The pick up calibration determined based on end point and location estimation
CN104884902B (en) For three axle magnetometer and the method and apparatus of the data fusion of three axis accelerometer
CN104121905B (en) Course angle obtaining method based on inertial sensor
Ren et al. A multi-position self-calibration method for dual-axis rotational inertial navigation system
CN105066917B (en) A kind of small pipeline GIS-Geographic Information System measuring device and its measurement method
CN101762793B (en) Magnetic data processing device
Sheinker et al. Localization in 3-D using beacons of low frequency magnetic field
CN104215259B (en) A kind of ins error bearing calibration based on earth magnetism modulus gradient and particle filter
Le Grand et al. 3-axis magnetic field mapping and fusion for indoor localization
CN103162689B (en) The assisted location method of auxiliary vehicle positioning system and vehicle
CN107024674B (en) A kind of magnetometer method for quick field calibration based on least square method of recursion
CN102818564B (en) Calibration method of three-dimensional electronic compass
AU2019232853B2 (en) IMU calibration
CN101887068B (en) Calibration compensation method for triaxial vector sensor and biaxial vector sensor
CN103940425B (en) A kind of magnetic inertia combines Strapdown inertial measurement method
CN104755941B (en) Method for making mobile device surface be aligned with the coordinate system of sensor
KR20040013439A (en) Attitude error compensation system of fluxgate and method thereof
US10685083B2 (en) Apparatuses and methods for calibrating magnetometer attitude-independent parameters
KR102029823B1 (en) Geomagnetic sensor calibration apparatus and method thereof
CN105180968B (en) A kind of IMU/ magnetometers installation misalignment filters scaling method online
Liu et al. Novel calibration algorithm for a three-axis strapdown magnetometer

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