CN108871323A - A kind of high-precision navigation method of the low cost inertial sensor under motor-driven environment - Google Patents

A kind of high-precision navigation method of the low cost inertial sensor under motor-driven environment Download PDF

Info

Publication number
CN108871323A
CN108871323A CN201810380903.5A CN201810380903A CN108871323A CN 108871323 A CN108871323 A CN 108871323A CN 201810380903 A CN201810380903 A CN 201810380903A CN 108871323 A CN108871323 A CN 108871323A
Authority
CN
China
Prior art keywords
carrier
matrix
speed
vector
misalignment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201810380903.5A
Other languages
Chinese (zh)
Other versions
CN108871323B (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.)
Allwinner Technology Co Ltd
Original Assignee
Allwinner Technology Co Ltd
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 Allwinner Technology Co Ltd filed Critical Allwinner Technology Co Ltd
Priority to CN201810380903.5A priority Critical patent/CN108871323B/en
Publication of CN108871323A publication Critical patent/CN108871323A/en
Application granted granted Critical
Publication of CN108871323B publication Critical patent/CN108871323B/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
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Abstract

The embodiment of the invention discloses a kind of high-precision navigation method of inexpensive inertial sensor under motor-driven environment.The method includes:Obtain inertial sensor IMU output signal;Low-pass filtering is carried out to inertial sensor IMU output signal using Fast Fourier Transform;Coning compensation and rotation paddle compensation are carried out in conjunction with the semaphore of upper period IMU;Carrier movement state is judged based on carrier motor revolving speed and gyroscope angular speed, it is determined whether carry out attitude updating;Finally, the resolving for carrying out speed and position, determines the current speed of carrier and position.The technical solution of the embodiment of the present invention fully considers carrier movement state, and carrier can be overcome to shake and shake the influence caused by IMU, so that attitude algorithm is more accurate, strong robustness, and versatility with higher and scalability.

Description

A kind of high-precision navigation method of the low cost inertial sensor under motor-driven environment
Technical field
The present embodiments relate to a kind of height of navigation field more particularly to inexpensive inertial sensor under motor-driven environment Precision air navigation aid.
Background technique
Inertial sensor IMU is arranged on carrier platform, and being shaken and shaking by platform is influenced, and can be generated a large amount of The noise of noise, generation can be reflected in the data of sensor, caused the performance of inertial sensor IMU to decline, be not able to satisfy height The navigation needs of precision.Currently, overcome the mode of above-mentioned influence of noise it is usual there are two types of:One is the mode of software optimization, one Kind is the mode to carrier platform optimization.For most of inexpensive inertial sensors, it is limited by processing unit operation energy Power is poor, can not execute complicated algorithm, to mostly use the mode to carrier platform optimization.But the multiplicity of platform, equipment Property, and the movement environment in face of Various Complex is needed, challenge all is brought to optimization carrier platform, it is complicated to generally require design Shock absorbing apparatus, and versatility and scalability are all poor.The complexity of Platform Designing, in addition simple, the robust of algorithm level Property it is poor, the influences of these composite factors is so that inexpensive inertial navigation scheme generates biggish error in a short time.For existing The disadvantages mentioned above of technology, there is presently no a kind of perfect solutions, are badly in need of a kind of for inexpensive inertial sensor, energy Enough overcome the high-precision of the noise jamming under motor-driven environment, the air navigation aid of low operand.
Summary of the invention
In view of this, the embodiment of the invention provides a kind of high-precision of inexpensive inertial sensor under motor-driven environment to lead Boat method, with solve in the prior art optimization carrier platform complexity is high, poor universality and software optimization algorithm are computationally intensive, The problem of transplantability difference, the method for the invention can overcome the influence for shaking, shaking under motor-driven environment, improve navigation algorithm Precision, while reducing calculation amount.
The embodiment of the invention provides a kind of high-precision navigation method of inexpensive inertial sensor under motor-driven environment, institutes The method of stating comprises the steps of:
Step 1:Signal acquisition obtains the output signal of inertial sensor, including acceleration and angular speed;
Step 2:Data prediction carries out low-pass filtering to the output signal using Fast Fourier Transform;
Step 3:Signal compensation, the output signal based on upper period inertial sensor compensate current demand signal, institute Stating signal compensation includes coning compensation and rotation paddle compensation;
Step 4:Carrier motor revolving speed and gyroscope angular speed are obtained, carrier current motion state is determined, according to the fortune Dynamic state judges whether to attitude updating;
Step 5:Resolve the current speed of the carrier and position.
Preferably, in the step 3, the rotating vector after overcompensation, the rotation can get by coning compensation The attitude angle of carrier, respectively roll angle, pitch angle and course angle can be obtained after vector is converted, be denoted as φ, θ, ψ respectively, specifically Calculation is:
Wherein β (T) is the rotating vector after overcompensation, and Δ α is that current angle increment samples, and Δ α ' is previous cycle Angle increment sampling, ω (t) be angular speed, T is predetermined period.
Preferably, it in the step 3, can be obtained by rotation paddle compensation by compensated speed increment dv (T), it can solve to obtain 3-axis acceleration by the dv (T), specific calculation is:
Wherein, Δ v is that current speed increment samples, and Δ v' is that the speed increment of previous cycle samples, and wherein Δ r is speed Rotation error is spent,For speed sculling error.
Preferably, when the motion state of the carrier is smooth motion state, then attitude updating is first carried out, then resolve institute State speed and the position of carrier;When the motion state of the carrier is non-stationary motion state, then without attitude updating, directly Connect the speed for resolving the carrier and position.
Preferably, when the carrier includes 4 driving motors, the acquisition carrier motor revolving speed is specifically included:It obtains The revolving speed of 4 driving motors of present carrier, respectively m1,m2,m3,m4
The minimum stationary value that motor is arranged is m_eps;
If two, front motor speed is greater than minimum stationary value or the left side two of motor with two motor speed difference of rear A motor speed is greater than the minimum stationary value of motor with two motor speed difference of right side, it is determined that present carrier motion state is non-flat Steady motion state.
Preferably, it obtains when first three axis angular rate is respectively wN, wE, wD
The minimum stationary value of angular speed is w_eps;
If when the absolute value of the angular speed of first three axis is greater than the minimum stationary value of angular speed, it is determined that present carrier movement State is non-stationary motion state.
Preferably, the determining carrier current motion state further includes:Obtain current misalignment;
The threshold value that misalignment is arranged is att_eps;
If current misalignment is greater than the threshold value, it is determined that carrier current motion state is non-stationary motion state.
Preferably, current misalignment is determined using the Kalman filtering based on misalignment;The karr based on misalignment Graceful filtering uses the horizontal misalignment of east orientationThe horizontal misalignment of north orientationWith gyroscopic drift ωbxbyAs system mode, note MakeWith horizontal acceleration fx,fyAs state quantity measurement, it is denoted as Z=[fy fx]T
The gyroscopic drift ωbxbyWith horizontal acceleration fx,fyBased on carrier coordinate system, three axis of carrier coordinate system It is expressed as xyz, the carrier coordinate system is connected firmly with carrier, and coordinate system meets right-hand rule, and origin refers in carrier center of gravity, x-axis To carrier direction of advance, y-axis is directed toward on the right side of carrier by origin, and z-axis direction is determined according to xy axis by right-hand rule;, the east orientation Horizontal misalignmentThe horizontal misalignment of north orientationBased on NED navigational coordinate system;
If the state equation of filter system is:
WhereinFor the derivative of system mode, f (x) is the function of system mode, and w is system noise, and Z is the measurement of system State, h (x) are the state quantity measurement function of system, and v is to measure noise;Remember that misalignment isGyroscopic drift For ωb=[ωbx ωby 0]T, the attitude of carrier at current time is att=[φ θ ψ], and wherein φ represents roll angle, and θ is represented Pitch angle, ψ represent course angle;
According to misalignment angle equation, above formula is represented by
Wherein,It is carrier coordinate system to the transition matrix of navigational coordinate system, is denoted as
Simplifying transition matrix is following form
To f (x) and h (x), it solves Jacobian matrix and obtains matrix F and H, the state equation after being linearized:
Wherein, vector X is last moment system mode vector, vectorFor the one-step prediction value of system mode vector, square Battle array F is system transfer matrix, and vector W, V are system noise vector, vectorFor system measurements vector, matrix H is measurement matrix, Wherein
Preferably, the Kalman filtering based on misalignment includes the time updating and measuring correction, the carry out posture Correction is to carry out measurement correction, to update the gain matrix of Kalman filtering and measure noise sequence variance matrix, eliminates accumulation Error.
Preferably, the specific steps of the time update include:
Wherein, vectorFor the one-step prediction value of system mode vector, vectorFor the estimated value of last moment, Matrix Φ is the discrete form of matrix F, Φk/k-1For Matrix of shifting of a step, matrix Pk/k-1For the one-step prediction side of state vector Poor matrix, matrix Pk/k-1For the last moment variance matrix of state vector, matrix QkFor system noise variance matrix.
Preferably, the specific steps updated that measure include:
Wherein, vectorFor the current maximum likelihood estimate of system mode vector, matrix KkFor gain matrix, matrix HkFor amount Survey matrix, matrix RkTo measure noise sequence variance matrix, matrix I is unit matrix.
Preferably, the specific steps for resolving institute's carrier current speed and position include:
The speed of last moment is denoted as Vk-1,Last moment acceleration is denoted as Ak-1,The position of last moment is denoted as Pk-1,Resolving time is Δ t;
The then speed V at current timek, position PkCalculation be:
Vk=Vk-1+Ak-1×Δt
Pk=Pk-1+Vk×Δt。
The embodiment of the invention provides a kind of high-precision navigation method of inexpensive inertial sensor under motor-driven environment, examine Consider carrier movement, gyroscope angular speed and misalignment three aspect factor, it is determined whether carry out attitude updating, can be effectively reduced Accumulated error improves the accuracy of navigation, reduces the complexity of algorithm;Also, the Kalman filtering based on misalignment is used, only Use twin shaft misalignment and twin shaft gyro to bias as quantity of state, uses horizontal acceleration as measurement, guaranteeing to resolve essence Under the premise of degree, operand is significantly reduced.
Detailed description of the invention
Fig. 1 is a kind of high-precision navigation of the inexpensive inertial sensor of the offer of the embodiment of the present invention one under motor-driven environment The flow chart of method;
Fig. 2 is a kind of position speed based on inexpensive inertial sensor under motor-driven environment provided by Embodiment 2 of the present invention Spend the flow chart of update method;
Fig. 3 is a kind of process for Kalman filtering attitude updating method based on misalignment that the embodiment of the present invention three provides Figure.
Specific embodiment
To make the objectives, technical solutions, and advantages of the present invention clearer, with reference to the accompanying drawing to of the invention specific real Example is applied to be described in further detail.It is understood that specific embodiment described herein is used only for explaining the present invention, Rather than limitation of the invention.
It also should be noted that only the parts related to the present invention are shown for ease of description, in attached drawing rather than Full content.It should be mentioned that some exemplary embodiments are described before exemplary embodiment is discussed in greater detail At the processing or method described as flow chart.Although operations (or step) are described as the processing of sequence by flow chart, It is that many of these operations can be implemented concurrently, concomitantly or simultaneously.In addition, the sequence of operations can be by again It arranges.The processing can be terminated when its operations are completed, it is also possible to have the additional step being not included in attached drawing. The processing can correspond to method, function, regulation, subroutine, subprogram etc..
Embodiment one
The embodiment of the present invention one can specifically be applied in the product for needing high-precision navigation, if unmanned plane positions, be driven automatically It sails, digital city, robot navigation etc..Fig. 1 is a kind of inexpensive inertial sensor that the embodiment of the present invention one provides motor-driven The flow chart of high-precision navigation method under environment.The method of the present embodiment specifically includes:
110, step 1:Signal acquisition obtains the output signal of inertial sensor, including acceleration and angular speed.
120, data prediction carries out low-pass filtering to the output signal using Fast Fourier Transform.
130, signal compensation, the output signal based on upper period inertial sensor compensates current demand signal, described Signal compensation includes coning compensation and rotation paddle compensation.
In the present embodiment, the rotating vector after overcompensation, the rotating vector warp can get by coning compensation The attitude angle of carrier, respectively roll angle, pitch angle and course angle can be obtained after conversion, be denoted as φ, θ, ψ respectively, specific calculating side Formula is:
Wherein, β (T) is the rotating vector after overcompensation, and Δ α is that current angle increment samples, and Δ α ' is the last week The angle increment of phase samples, and ω (t) is angular speed, and T is predetermined period.
It can be obtained by rotation paddle compensation by compensated speed increment dv (T), it can be in the hope of by the dv (T) Solution obtains 3-axis acceleration, and specific calculation is:
Wherein, Δ v is that current speed increment samples, and Δ v' is that the speed increment of previous cycle samples, and wherein Δ r is speed Rotation error is spent,For speed sculling error.
140, it determines carrier movement state, obtains carrier motor revolving speed and gyroscope angular speed, determine carrier current kinetic State judges whether to attitude updating according to the motion state.
150, the carrier current speed and position are resolved.
In the present embodiment, when the motion state of the carrier is smooth motion state, then attitude updating is first carried out, then Resolve speed and the position of the carrier;When the motion state of the carrier is non-stationary motion state, then without posture It corrects, the speed of carrier described in directly calculation and position.
The specific steps for resolving the carrier current speed and position include:
The speed of last moment is denoted as Vk-1,Last moment acceleration is denoted as Ak-1,The position of last moment is denoted as Pk-1,Resolving time is Δ t;
The then speed V at current timek, position PkCalculation be:
Vk=Vk-1+Ak-1×Δt
Pk=Pk-1+Vk×Δt。
The embodiment of the present invention one provides a kind of high-precision navigation method of inexpensive inertial sensor under motor-driven environment, By adaptively being improved coning compensation and rotation paddle compensation, the compensation deals of multicycle are not carried out to signal, only It compensated with the semaphore in a upper period and in conjunction with current demand signal, and can reduce computational complexity, while improving precision;Separately Outside, motion state judgement is carried out based on carrier motor revolving speed, gyroscope angular speed, determines whether to carry out according to current motion state Attitude updating creates a further reduction operand, improves the precision of navigation.
Embodiment two
Fig. 2 is a kind of position speed based on inexpensive inertial sensor under motor-driven environment provided by Embodiment 2 of the present invention Spend the flow chart of update method.The present embodiment is optimized based on above-described embodiment, and the method for the present embodiment specifically includes:
210, information is obtained, determines carrier current motion state.
In the present embodiment, carrier motor revolving speed is obtained, three axis angular rate of gyroscope and misalignment are obtained.
220, judge whether carrier is in steady state.
In the present embodiment, when the carrier includes 4 driving motors, the acquisition carrier motor revolving speed is specific to wrap It includes:Obtain the revolving speed of 4 driving motors of present carrier, respectively m1,m2,m3,m4
The minimum stationary value that motor is arranged is m_eps;
If two, front motor speed is greater than minimum stationary value or the left side two of motor with two motor speed difference of rear A motor speed is greater than the minimum stationary value of motor with two motor speed difference of right side, it is determined that present carrier motion state is non-flat Steady motion state.
It obtains when first three axis angular rate is respectively wN, wE, wD
The minimum stationary value of angular speed is w_eps;
If when the absolute value of the angular speed of first three axis is greater than the minimum stationary value of angular speed, it is determined that present carrier movement State is non-stationary motion state.
The determining carrier current motion state further includes:Obtain current misalignment;
The threshold value that misalignment is arranged is att_eps;
If current misalignment is greater than the threshold value, it is determined that carrier current motion state is non-stationary motion state.
Current misalignment is determined using the Kalman filtering based on misalignment;The Kalman filtering based on misalignment is adopted With the horizontal misalignment of east orientationThe horizontal misalignment of north orientationWith gyroscopic drift ωbxbyAs system mode, it is denoted asWith horizontal acceleration fx,fyAs state quantity measurement, it is denoted as Z=[fy fx]T
The gyroscopic drift ωbxbyWith horizontal acceleration fx,fyBased on carrier coordinate system, three axis of carrier coordinate system It is expressed as xyz, the carrier coordinate system is connected firmly with carrier, and coordinate system meets right-hand rule, and origin refers in carrier center of gravity, x-axis To carrier direction of advance, y-axis is directed toward on the right side of carrier by origin, and z-axis direction is determined according to xy axis by right-hand rule;, the east orientation Horizontal misalignmentThe horizontal misalignment of north orientationBased on NED navigational coordinate system;
If the state equation of filter system is:
WhereinFor the derivative of system mode, f (x) is the function of system mode, and w is system noise, and Z is the measurement of system State, h (x) are the state quantity measurement function of system, and v is to measure noise;Remember that misalignment isGyroscopic drift For ωb=[ωbx ωby 0]T, the attitude of carrier at current time is att=[φ θ ψ], and wherein φ represents roll angle, and θ is represented Pitch angle, ψ represent course angle;
According to misalignment angle equation, above formula is represented by
Wherein,It is carrier coordinate system to the transition matrix of navigational coordinate system, is denoted as
Simplifying transition matrix is following form
To f (x) and h (x), it solves Jacobian matrix and obtains matrix F and H, the state equation after being linearized:
Wherein, vector X is last moment system mode vector, vectorFor the one-step prediction value of system mode vector, square Battle array F is system transfer matrix, and vector W, V are system noise vector, vectorFor system measurements vector, matrix H is measurement matrix, Wherein
230, attitude updating.
In the present embodiment, when the motion state of the carrier is smooth motion state, then attitude updating is first carried out, then Resolve speed and the position of the carrier;When the motion state of the carrier is non-stationary motion state, then without posture It corrects, the speed of carrier described in directly calculation and position.
Wherein, the Kalman filtering based on misalignment includes the time updating and measuring correction, and the progress attitude updating is To carry out measurement correction, to update the gain matrix of Kalman filtering and measure noise sequence variance matrix, accumulated error is eliminated.
The specific steps that time updates include:
Wherein, vectorFor the one-step prediction value of system mode vector, vectorFor the estimated value of last moment, Matrix Φ is the discrete form of matrix F, Φk/k-1For Matrix of shifting of a step, matrix Pk/k-1For the one-step prediction side of state vector Poor matrix, matrix Pk/k-1For the last moment variance matrix of state vector, matrix QkFor system noise variance matrix.
It is described measure correction specific steps include:
Wherein, vectorFor the current maximum likelihood estimate of system mode vector, matrix KkFor gain matrix, matrix HkFor amount Survey matrix, matrix RkTo measure noise sequence variance matrix, matrix I is unit matrix.
240, computing speed position.
In the present embodiment, the specific steps for resolving institute's carrier current speed and position include:
The speed of last moment is denoted as Vk-1,Last moment acceleration is denoted as Ak-1,The position of last moment is denoted as Pk-1,Resolving time is Δ t;
The then speed V at current timek, position PkCalculation be:
Vk=Vk-1+Ak-1×Δt
Pk=Pk-1+Vk×Δt。
Second embodiment of the present invention provides a kind of position and speeds based on inexpensive inertial sensor under motor-driven environment more New method comprehensively considers carrier motor revolving speed, gyroscope angular speed and misalignment three aspect factor, it is determined whether carries out posture school Just, it can be effectively reduced accumulated error, improve the accuracy of navigation, reduce the complexity of algorithm;Also, using based on misalignment The Kalman filtering at angle is used only twin shaft misalignment and the biasing of twin shaft gyro as quantity of state, uses horizontal acceleration as amount Measurement, under the premise of guaranteeing calculation accuracy, significantly reduces operand.
Embodiment three
Fig. 3 is a kind of process for Kalman filtering attitude updating method based on misalignment that the embodiment of the present invention three provides Figure.The method of the present embodiment specifically includes:
310, the measured value of the three axis accelerometer of inertial sensor is obtained.
320, judge whether carrier is in acceleration mode.
In the present embodiment, judge that the step of whether carrier is in acceleration mode is:
Step 1:Calculate the vector sum of the three axis accelerometer of the inertial sensorIt is denoted as Wherein fx,fy,fzRespectively 3-axis acceleration value;
Step 2:Calculate the 3-axis acceleration under the low maneuvering condition of carrier;The specific steps are:
Have in the state that carrier is low motor-driven
Wherein, gravity vector gn=[0 0 g]T, g is local gravitational acceleration, f1Accelerate for three axis under low maneuvering condition The vector acceleration of degree meter output, is denoted as f1=[fx1,fy1,fz1]T
Desired acceleration f can be calculated by formula (2)x1,fy1,fz1
Step 3:Defining an amount of deflection is eps, is calculated separately|fx1-fx|、|fy1-fy|、|fz1-fz|, if on Any result is stated greater than amount of deflection eps, then carrier is in acceleration mode;Otherwise, carrier is in non-accelerating state.
When carrier is in non-accelerating state, carries out time update and measure amendment, when carrier is in acceleration mode, only Carry out time update
330, time update is carried out.
In the present embodiment, the specific steps of the time update include:
Wherein, vectorFor the one-step prediction value of system mode vector, vectorFor the estimated value of last moment, Matrix Φ is the discrete form of matrix F, Φk/k-1For Matrix of shifting of a step, matrix Pk/k-1For the one-step prediction side of state vector Poor matrix, matrix Pk/k-1For the last moment variance matrix of state vector, matrix QkFor system noise variance matrix.
340, it carries out time update and measures amendment.
In the present embodiment, the modified specific steps of measurement include:
Wherein, vectorFor the current maximum likelihood estimate of system mode vector, matrix KkFor gain matrix, matrix HkFor amount Survey matrix, matrix RkTo measure noise sequence variance matrix, matrix I is unit matrix.
350, attitude of carrier is determined.
In the present embodiment, the posture for determining carrier includes the misalignment of determining carrier.
It will be understood by those skilled in the art that the condition for measure correction limited in the present embodiment can be with embodiment One or embodiment two in limit carry out measure correction condition be combined with each other, both can only meet either condition and be measured Correction, the condition that can also meet the two simultaneously just carry out measurement correction.It is able to carry out for a person skilled in the art various Apparent variation is readjusted and is substituted without departing from protection scope of the present invention.
The embodiment of the present invention three provides a kind of Kalman filtering posture renewal method based on misalignment, using based on mistake Twin shaft misalignment and the biasing of twin shaft gyro is used only as quantity of state in the Kalman filtering at quasi- angle, use horizontal acceleration as Measurement significantly reduces operand under the premise of guaranteeing calculation accuracy;Also, using above-mentioned parameter as quantity of state And measurement, time update can be adaptive selected or measure amendment, that is, use current acceleration vector sum and acceleration The difference of count value and ideal value carries out adaptive Kalman filter, further reduced operand.
It should also be noted that the above is only a better embodiment of the present invention and the applied technical principle.Those skilled in the art It will be appreciated that the invention is not limited to the specific embodiments described herein, it is able to carry out for a person skilled in the art various obvious Variation, readjustment and substitution without departing from protection scope of the present invention.Therefore, although by above embodiments to this hair It is bright to be described in further detail, but the present invention is not limited to the above embodiments only, in the feelings for not departing from present inventive concept It can also include more other equivalent embodiments under condition, and the scope of the invention is determined by the scope of the appended claims.

Claims (12)

1. a kind of high-precision navigation method of low cost inertial sensor under motor-driven environment, which is characterized in that the method packet Containing following steps:
Step 1:Signal acquisition obtains the output signal of inertial sensor, including acceleration and angular speed;
Step 2:Data prediction carries out low-pass filtering to the output signal using Fast Fourier Transform;
Step 3:Signal compensation, the output signal based on upper period inertial sensor compensate current demand signal, the letter Number compensation include coning compensation and rotation paddle compensation;
Step 4:Carrier motor revolving speed and gyroscope angular speed are obtained, carrier current motion state is determined, according to the movement shape State judges whether to attitude updating;
Step 5:Resolve the current speed of the carrier and position.
2. the method according to claim 1, wherein can get and pass through by coning compensation in the step 3 Rotating vector after compensation, can obtain the attitude angle of carrier after the rotating vector is converted, respectively roll angle, pitch angle and Course angle, is denoted as φ, θ, ψ respectively, and specific calculation is:
Wherein β (T) is the rotating vector after overcompensation, and Δ α is that current angle increment samples, and Δ α ' is the angle of previous cycle Increment sampling, ω (t) are angular speed, and T is predetermined period.
3. according to the method described in claim 2, it is characterized in that, can be obtained in the step 3 by rotation paddle compensation It obtains and passes through compensated speed increment dv (T), can solve to obtain 3-axis acceleration, specific calculation by the dv (T) For:
Wherein, Δ v is that current speed increment samples, and Δ v' is that the speed increment of previous cycle samples, and wherein Δ r is speed rotation Turn error,For speed sculling error.
4. the method according to claim 1, wherein when the motion state of the carrier is smooth motion state When, then attitude updating is first carried out, then resolve speed and the position of the carrier;When the motion state of the carrier is non-stationary fortune When dynamic state, then without attitude updating, the speed of carrier described in directly calculation and position.
5. according to the method described in claim 4, it is characterized in that, when the carrier include 4 driving motors when, the acquisition Carrier motor revolving speed, specifically includes:Obtain the revolving speed of 4 driving motors of present carrier, respectively m1,m2,m3,m4
The minimum stationary value that motor is arranged is m_eps;
If two, front motor speed is greater than the minimum stationary value or two, left side electricity of motor with two motor speed difference of rear Machine revolving speed is greater than the minimum stationary value of motor with two motor speed difference of right side, it is determined that present carrier motion state is non-stationary fortune Dynamic state.
6. according to the method described in claim 4, it is characterized in that, obtaining when first three axis angular rate is respectively wN, wE, wD
The minimum stationary value of angular speed is w_eps;
If when the absolute value of the angular speed of first three axis is greater than the minimum stationary value of angular speed, it is determined that present carrier motion state For non-stationary motion state.
7. the method according to claim 1, wherein the determining carrier current motion state, further includes:It obtains Current misalignment;
The threshold value that misalignment is arranged is att_eps;
If current misalignment is greater than the threshold value, it is determined that carrier current motion state is non-stationary motion state.
8. the method according to the description of claim 7 is characterized in that working as slip using the Kalman filtering determination based on misalignment Quasi- angle;The Kalman filtering based on misalignment uses the horizontal misalignment of east orientationThe horizontal misalignment of north orientationIt is floated with gyro Move ωbxbyAs system mode, it is denoted asWith horizontal acceleration fx,fyAs measurement shape State is denoted as Z=[fy fx]T
The gyroscopic drift ωbxbyWith horizontal acceleration fx,fyBased on carrier coordinate system, three axis of carrier coordinate system is indicated For xyz, the carrier coordinate system is connected firmly with carrier, and coordinate system meets right-hand rule, and in carrier center of gravity, x-axis is directed toward to be carried origin Body direction of advance, y-axis are directed toward on the right side of carrier by origin, and z-axis direction is determined according to xy axis by right-hand rule;, the east orientation level MisalignmentThe horizontal misalignment of north orientationBased on NED navigational coordinate system;
If the state equation of filter system is:
WhereinFor the derivative of system mode, f (x) is the function of system mode, and w is system noise, and Z is the state quantity measurement of system, H (x) is the state quantity measurement function of system, and v is to measure noise;Remember that misalignment isGyroscopic drift is ωb= [ωbx ωby 0]T, the attitude of carrier at current time is att=[φ θ ψ], and wherein φ represents roll angle, and θ represents pitch angle, ψ Represent course angle;
According to misalignment angle equation, above formula is represented by
Wherein,It is carrier coordinate system to the transition matrix of navigational coordinate system, is denoted as
Simplifying transition matrix is following form
To f (x) and h (x), it solves Jacobian matrix and obtains matrix F and H, the state equation after being linearized:
Wherein, vector X is last moment system mode vector, vectorFor the one-step prediction value of system mode vector, matrix F is System transfer matrix, vector W, V are system noise vector, vectorFor system measurements vector, matrix H is measurement matrix, wherein
9. according to the method described in claim 8, it is characterized in that, the Kalman filtering based on misalignment include the time more New and measurement correction, the progress attitude updating is to carry out measurement correction, to update the gain matrix and amount of Kalman filtering Noise sequence variance matrix are surveyed, accumulated error is eliminated.
10. according to the method described in claim 9, it is characterized in that, the specific steps of time update include:
Wherein, vectorFor the one-step prediction value of system mode vector, vectorFor the estimated value of last moment, matrix Φ is the discrete form of matrix F, Φk/k-1For Matrix of shifting of a step, matrix Pk/k-1For the one-step prediction variance square of state vector Battle array, matrix Pk/k-1For the last moment variance matrix of state vector, matrix QkFor system noise variance matrix.
11. according to the method described in claim 9, it is characterized in that, the specific steps updated that measure include:
Wherein, vectorFor the current maximum likelihood estimate of system mode vector, matrix KkFor gain matrix, matrix HkTo measure square Battle array, matrix RkTo measure noise sequence variance matrix, matrix I is unit matrix.
12. the method according to claim 1, wherein the tool for resolving institute's carrier current speed and position Body step includes:
The speed of last moment is denoted as Vk-1,Last moment acceleration is denoted as Ak-1,The position of last moment is denoted as Pk-1,Resolving time is Δ t;
The then speed V at current timek, position PkCalculation be:
Vk=Vk-1+Ak-1×Δt
Pk=Pk-1+Vk×Δt。
CN201810380903.5A 2018-04-25 2018-04-25 High-precision navigation method of low-cost inertial sensor in locomotive environment Active CN108871323B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810380903.5A CN108871323B (en) 2018-04-25 2018-04-25 High-precision navigation method of low-cost inertial sensor in locomotive environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810380903.5A CN108871323B (en) 2018-04-25 2018-04-25 High-precision navigation method of low-cost inertial sensor in locomotive environment

Publications (2)

Publication Number Publication Date
CN108871323A true CN108871323A (en) 2018-11-23
CN108871323B CN108871323B (en) 2021-08-24

Family

ID=64327314

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810380903.5A Active CN108871323B (en) 2018-04-25 2018-04-25 High-precision navigation method of low-cost inertial sensor in locomotive environment

Country Status (1)

Country Link
CN (1) CN108871323B (en)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111238530A (en) * 2019-11-27 2020-06-05 南京航空航天大学 Initial alignment method for air moving base of strapdown inertial navigation system
CN111307114A (en) * 2019-11-29 2020-06-19 哈尔滨工程大学 Water surface ship horizontal attitude measurement method based on motion reference unit
CN111878064A (en) * 2020-05-11 2020-11-03 中国科学院地质与地球物理研究所 Attitude measurement method
CN113442171B (en) * 2021-07-01 2022-06-24 南京蔚蓝智能科技有限公司 Robot dynamic stability discrimination method and dynamic self-adaptive attitude control method
CN111878064B (en) * 2020-05-11 2024-04-05 中国科学院地质与地球物理研究所 Gesture measurement method

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090089001A1 (en) * 2007-08-14 2009-04-02 American Gnc Corporation Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)
CN102128624A (en) * 2010-12-28 2011-07-20 浙江大学 High dynamic strapdown inertial navigation parallel computing device
CN102519458A (en) * 2011-12-16 2012-06-27 浙江大学 Rowing motion compensation method for fiber optic gyroscope trapdown inertial navigation
CN104655136A (en) * 2015-02-17 2015-05-27 西安航天精密机电研究所 Multi-concave-point FIR filtering method applicable to laser strap-down inertial navigation system

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090089001A1 (en) * 2007-08-14 2009-04-02 American Gnc Corporation Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)
CN102128624A (en) * 2010-12-28 2011-07-20 浙江大学 High dynamic strapdown inertial navigation parallel computing device
CN102519458A (en) * 2011-12-16 2012-06-27 浙江大学 Rowing motion compensation method for fiber optic gyroscope trapdown inertial navigation
CN104655136A (en) * 2015-02-17 2015-05-27 西安航天精密机电研究所 Multi-concave-point FIR filtering method applicable to laser strap-down inertial navigation system

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
尤太华等: "机抖式激光惯组高动态下导航误差分析", 《航天控制》 *
张泽等: "新的捷联惯性导航划桨误差补偿算法", 《吉林大学学报(工学版)》 *
秦永元等: "基于对偶法捷联惯导圆锥与划桨算法一般结果", 《中国惯性技术学报》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111238530A (en) * 2019-11-27 2020-06-05 南京航空航天大学 Initial alignment method for air moving base of strapdown inertial navigation system
CN111307114A (en) * 2019-11-29 2020-06-19 哈尔滨工程大学 Water surface ship horizontal attitude measurement method based on motion reference unit
CN111307114B (en) * 2019-11-29 2022-09-27 哈尔滨工程大学 Water surface ship horizontal attitude measurement method based on motion reference unit
CN111878064A (en) * 2020-05-11 2020-11-03 中国科学院地质与地球物理研究所 Attitude measurement method
CN111878064B (en) * 2020-05-11 2024-04-05 中国科学院地质与地球物理研究所 Gesture measurement method
CN113442171B (en) * 2021-07-01 2022-06-24 南京蔚蓝智能科技有限公司 Robot dynamic stability discrimination method and dynamic self-adaptive attitude control method

Also Published As

Publication number Publication date
CN108871323B (en) 2021-08-24

Similar Documents

Publication Publication Date Title
JP4876204B2 (en) Small attitude sensor
CN106767797B (en) inertial/GPS combined navigation method based on dual quaternion
CN110887481B (en) Carrier dynamic attitude estimation method based on MEMS inertial sensor
CN109682377A (en) A kind of Attitude estimation method based on the decline of dynamic step length gradient
JP2012173190A (en) Positioning system and positioning method
CN108871323A (en) A kind of high-precision navigation method of the low cost inertial sensor under motor-driven environment
US20220252399A1 (en) Composite sensor and angular velocity correction method
CN108592917A (en) A kind of Kalman filtering Attitude estimation method based on misalignment
CN108627152A (en) A kind of air navigation aid of the miniature drone based on Fusion
Cui et al. In-motion alignment for low-cost SINS/GPS under random misalignment angles
JP5419268B2 (en) 3D attitude estimation device, 3D attitude estimation method, and 3D attitude estimation program
JPH10185612A (en) Method and device for compensating sculling in strap down type inertia navigation system, and memory for strap down type inertia navigation system
CN110793515A (en) Unmanned aerial vehicle attitude estimation method based on single-antenna GPS and IMU under large-mobility condition
CN112902956A (en) Course initial value acquisition method for handheld GNSS/MEMS-INS receiver, electronic equipment and storage medium
CN110873563B (en) Cloud deck attitude estimation method and device
CN113108781B (en) Improved coarse alignment method applied to unmanned ship during advancing
CN107063181A (en) The measuring method and device of the level inclination of Multifunctional adjustment table under complex environment
CN114111771A (en) Dynamic attitude measurement method of double-shaft stable platform
JP4527171B2 (en) Vehicle attitude angle measurement method using single GPS and inertial data (acceleration, angular velocity)
JP3425689B2 (en) Inertial device
CN113447018B (en) Real-time attitude estimation method of underwater inertial navigation system
US6085149A (en) Integrated inertial/VMS navigation solution
JP3375720B2 (en) Ship inertial navigation system
JP2756554B2 (en) Inertial device
JP2007075967A (en) Highly-accurate attitude detection device of moving body

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