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 PDFInfo
- 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
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
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
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 ωbx,ωbyAs system mode, note
MakeWith horizontal acceleration fx,fyAs state quantity measurement, it is denoted as Z=[fy fx]T；
The gyroscopic drift ωbx,ωbyWith 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 ωbx,ωbyAs system mode, it is denoted asWith horizontal acceleration fx,fyAs state quantity measurement, it is denoted as Z=[fy fx]T；
The gyroscopic drift ωbx,ωbyWith 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 ωbx,ωbyAs system mode, it is denoted asWith horizontal acceleration fx,fyAs measurement shape
State is denoted as Z=[fy fx]T；
The gyroscopic drift ωbx,ωbyWith 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。
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 (4)
| 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 |
Citations (4)
| 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 |
-
2018
- 2018-04-25 CN CN201810380903.5A patent/CN108871323B/en active Active
Patent Citations (4)
| 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)
| Title |
|---|
| 尤太华等: "机抖式激光惯组高动态下导航误差分析", 《航天控制》 * |
| 张泽等: "新的捷联惯性导航划桨误差补偿算法", 《吉林大学学报（工学版）》 * |
| 秦永元等: "基于对偶法捷联惯导圆锥与划桨算法一般结果", 《中国惯性技术学报》 * |
Cited By (6)
| 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 |
|---|---|---|
| CN108871323A (en) | A kind of high-precision navigation method of the low cost inertial sensor under motor-driven environment | |
| JP4876204B2 (en) | Small attitude sensor | |
| CN110887481B (en) | Carrier dynamic attitude estimation method based on MEMS inertial sensor | |
| CN106767797B (en) | inertial/GPS combined navigation method based on dual quaternion | |
| CN109682377A (en) | A kind of Attitude estimation method based on the decline of dynamic step length gradient | |
| JP2012173190A (en) | Positioning system and positioning method | |
| 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 | |
| CN110873563B (en) | Cloud deck attitude estimation method and device | |
| 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 | |
| CN116429095A (en) | Method for measuring muzzle vibration during advancing based on combination of main inertial navigation and sub inertial navigation | |
| Guan et al. | Sensor fusion of gyroscope and accelerometer for low-cost attitude determination system | |
| 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 |
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 |