CN108871323A  A kind of highprecision navigation method of the low cost inertial sensor under motordriven environment  Google Patents
A kind of highprecision navigation method of the low cost inertial sensor under motordriven 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 noninertial navigation instruments
Abstract
The embodiment of the invention discloses a kind of highprecision navigation method of inexpensive inertial sensor under motordriven environment.The method includes：Obtain inertial sensor IMU output signal；Lowpass 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 motordriven 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 abovementioned 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 highprecision of the noise jamming under motordriven 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 highprecision of inexpensive inertial sensor under motordriven 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 motordriven environment, improve navigation algorithm
Precision, while reducing calculation amount.
The embodiment of the invention provides a kind of highprecision navigation method of inexpensive inertial sensor under motordriven 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 lowpass 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 3axis 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 nonstationary 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 m_{1},m_{2},m_{3},m_{4}；
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 nonflat
Steady motion state.
Preferably, it obtains when first three axis angular rate is respectively w_{N}, w_{E}, w_{D}；
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 nonstationary 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 nonstationary 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},ω_{by}As system mode, note
MakeWith horizontal acceleration f_{x},f_{y}As state quantity measurement, it is denoted as Z=[f_{y} f_{x}]^{T}；
The gyroscopic drift ω_{bx},ω_{by}With horizontal acceleration f_{x},f_{y}Based 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 righthand rule, and origin refers in carrier center of gravity, xaxis
To carrier direction of advance, yaxis is directed toward on the right side of carrier by origin, and zaxis direction is determined according to xy axis by righthand 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 onestep 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 onestep prediction value of system mode vector, vectorFor the estimated value of last moment,
Matrix Φ is the discrete form of matrix F, Φ_{k/k1}For Matrix of shifting of a step, matrix P_{k/k1}For the onestep prediction side of state vector
Poor matrix, matrix P_{k/k1}For the last moment variance matrix of state vector, matrix Q_{k}For system noise variance matrix.
Preferably, the specific steps updated that measure include：
Wherein, vectorFor the current maximum likelihood estimate of system mode vector, matrix K_{k}For gain matrix, matrix H_{k}For amount
Survey matrix, matrix R_{k}To 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 V_{k1},Last moment acceleration is denoted as A_{k1},The position of last moment is denoted as P_{k1},Resolving time is Δ t；
The then speed V at current time_{k}, position P_{k}Calculation be：
V_{k}=V_{k1}+A_{k1}×Δt
P_{k}=P_{k1}+V_{k}×Δt。
The embodiment of the invention provides a kind of highprecision navigation method of inexpensive inertial sensor under motordriven 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 highprecision navigation of the inexpensive inertial sensor of the offer of the embodiment of the present invention one under motordriven environment
The flow chart of method；
Fig. 2 is a kind of position speed based on inexpensive inertial sensor under motordriven 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 highprecision 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 motordriven
The flow chart of highprecision 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 lowpass 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 3axis 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 nonstationary 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 V_{k1},Last moment acceleration is denoted as A_{k1},The position of last moment is denoted as P_{k1},Resolving time is Δ t；
The then speed V at current time_{k}, position P_{k}Calculation be：
V_{k}=V_{k1}+A_{k1}×Δt
P_{k}=P_{k1}+V_{k}×Δt。
The embodiment of the present invention one provides a kind of highprecision navigation method of inexpensive inertial sensor under motordriven 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 motordriven environment provided by Embodiment 2 of the present invention
Spend the flow chart of update method.The present embodiment is optimized based on abovedescribed 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 m_{1},m_{2},m_{3},m_{4}；
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 nonflat
Steady motion state.
It obtains when first three axis angular rate is respectively w_{N}, w_{E}, w_{D}；
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 nonstationary 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 nonstationary 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},ω_{by}As system mode, it is denoted asWith horizontal acceleration f_{x},f_{y}As state quantity measurement, it is denoted as Z=[f_{y} f_{x}]^{T}；
The gyroscopic drift ω_{bx},ω_{by}With horizontal acceleration f_{x},f_{y}Based 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 righthand rule, and origin refers in carrier center of gravity, xaxis
To carrier direction of advance, yaxis is directed toward on the right side of carrier by origin, and zaxis direction is determined according to xy axis by righthand 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 onestep 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 nonstationary 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 onestep prediction value of system mode vector, vectorFor the estimated value of last moment,
Matrix Φ is the discrete form of matrix F, Φ_{k/k1}For Matrix of shifting of a step, matrix P_{k/k1}For the onestep prediction side of state vector
Poor matrix, matrix P_{k/k1}For the last moment variance matrix of state vector, matrix Q_{k}For system noise variance matrix.
It is described measure correction specific steps include：
Wherein, vectorFor the current maximum likelihood estimate of system mode vector, matrix K_{k}For gain matrix, matrix H_{k}For amount
Survey matrix, matrix R_{k}To 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 V_{k1},Last moment acceleration is denoted as A_{k1},The position of last moment is denoted as P_{k1},Resolving time is Δ t；
The then speed V at current time_{k}, position P_{k}Calculation be：
V_{k}=V_{k1}+A_{k1}×Δt
P_{k}=P_{k1}+V_{k}×Δt。
Second embodiment of the present invention provides a kind of position and speeds based on inexpensive inertial sensor under motordriven 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 f_{x},f_{y},f_{z}Respectively 3axis acceleration value；
Step 2：Calculate the 3axis acceleration under the low maneuvering condition of carrier；The specific steps are：
Have in the state that carrier is low motordriven
Wherein, gravity vector g_{n}=[0 0 g]^{T}, g is local gravitational acceleration, f_{1}Accelerate for three axis under low maneuvering condition
The vector acceleration of degree meter output, is denoted as f_{1}=[f_{x1},f_{y1},f_{z1}]^{T}；
Desired acceleration f can be calculated by formula (2)_{x1},f_{y1},f_{z1}：
Step 3：Defining an amount of deflection is eps, is calculated separatelyf_{x1}f_{x}、f_{y1}f_{y}、f_{z1}f_{z}, if on
Any result is stated greater than amount of deflection eps, then carrier is in acceleration mode；Otherwise, carrier is in nonaccelerating state.
When carrier is in nonaccelerating 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 onestep prediction value of system mode vector, vectorFor the estimated value of last moment,
Matrix Φ is the discrete form of matrix F, Φ_{k/k1}For Matrix of shifting of a step, matrix P_{k/k1}For the onestep prediction side of state vector
Poor matrix, matrix P_{k/k1}For the last moment variance matrix of state vector, matrix Q_{k}For 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 K_{k}For gain matrix, matrix H_{k}For amount
Survey matrix, matrix R_{k}To 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 abovementioned 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 highprecision navigation method of low cost inertial sensor under motordriven 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 lowpass 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 3axis 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 nonstationary 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 m_{1},m_{2},m_{3},m_{4}；
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 nonstationary 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 w_{N}, w_{E}, w_{D}；
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 nonstationary 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 nonstationary 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},ω_{by}As system mode, it is denoted asWith horizontal acceleration f_{x},f_{y}As measurement shape
State is denoted as Z=[f_{y} f_{x}]^{T}；
The gyroscopic drift ω_{bx},ω_{by}With horizontal acceleration f_{x},f_{y}Based 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 righthand rule, and in carrier center of gravity, xaxis is directed toward to be carried origin
Body direction of advance, yaxis are directed toward on the right side of carrier by origin, and zaxis direction is determined according to xy axis by righthand 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 onestep 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 onestep prediction value of system mode vector, vectorFor the estimated value of last moment, matrix
Φ is the discrete form of matrix F, Φ_{k/k1}For Matrix of shifting of a step, matrix P_{k/k1}For the onestep prediction variance square of state vector
Battle array, matrix P_{k/k1}For the last moment variance matrix of state vector, matrix Q_{k}For 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 K_{k}For gain matrix, matrix H_{k}To measure square
Battle array, matrix R_{k}To 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 V_{k1},Last moment acceleration is denoted as A_{k1},The position of last moment is denoted as P_{k1},Resolving time is Δ t；
The then speed V at current time_{k}, position P_{k}Calculation be：
V_{k}=V_{k1}+A_{k1}×Δt
P_{k}=P_{k1}+V_{k}×Δt。
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201810380903.5A CN108871323B (en)  20180425  20180425  Highprecision navigation method of lowcost inertial sensor in locomotive environment 
Applications Claiming Priority (1)
Application Number  Priority Date  Filing Date  Title 

CN201810380903.5A CN108871323B (en)  20180425  20180425  Highprecision navigation method of lowcost inertial sensor in locomotive environment 
Publications (2)
Publication Number  Publication Date 

CN108871323A true CN108871323A (en)  20181123 
CN108871323B CN108871323B (en)  20210824 
Family
ID=64327314
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201810380903.5A Active CN108871323B (en)  20180425  20180425  Highprecision navigation method of lowcost inertial sensor in locomotive environment 
Country Status (1)
Country  Link 

CN (1)  CN108871323B (en) 
Cited By (5)
Publication number  Priority date  Publication date  Assignee  Title 

CN111238530A (en) *  20191127  20200605  南京航空航天大学  Initial alignment method for air moving base of strapdown inertial navigation system 
CN111307114A (en) *  20191129  20200619  哈尔滨工程大学  Water surface ship horizontal attitude measurement method based on motion reference unit 
CN111878064A (en) *  20200511  20201103  中国科学院地质与地球物理研究所  Attitude measurement method 
CN113442171B (en) *  20210701  20220624  南京蔚蓝智能科技有限公司  Robot dynamic stability discrimination method and dynamic selfadaptive attitude control method 
CN111878064B (en) *  20200511  20240405  中国科学院地质与地球物理研究所  Gesture measurement method 
Citations (4)
Publication number  Priority date  Publication date  Assignee  Title 

US20090089001A1 (en) *  20070814  20090402  American Gnc Corporation  Selfcalibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS) 
CN102128624A (en) *  20101228  20110720  浙江大学  High dynamic strapdown inertial navigation parallel computing device 
CN102519458A (en) *  20111216  20120627  浙江大学  Rowing motion compensation method for fiber optic gyroscope trapdown inertial navigation 
CN104655136A (en) *  20150217  20150527  西安航天精密机电研究所  Multiconcavepoint FIR filtering method applicable to laser strapdown inertial navigation system 

2018
 20180425 CN CN201810380903.5A patent/CN108871323B/en active Active
Patent Citations (4)
Publication number  Priority date  Publication date  Assignee  Title 

US20090089001A1 (en) *  20070814  20090402  American Gnc Corporation  Selfcalibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS) 
CN102128624A (en) *  20101228  20110720  浙江大学  High dynamic strapdown inertial navigation parallel computing device 
CN102519458A (en) *  20111216  20120627  浙江大学  Rowing motion compensation method for fiber optic gyroscope trapdown inertial navigation 
CN104655136A (en) *  20150217  20150527  西安航天精密机电研究所  Multiconcavepoint FIR filtering method applicable to laser strapdown inertial navigation system 
NonPatent Citations (3)
Title 

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

CN111238530A (en) *  20191127  20200605  南京航空航天大学  Initial alignment method for air moving base of strapdown inertial navigation system 
CN111307114A (en) *  20191129  20200619  哈尔滨工程大学  Water surface ship horizontal attitude measurement method based on motion reference unit 
CN111307114B (en) *  20191129  20220927  哈尔滨工程大学  Water surface ship horizontal attitude measurement method based on motion reference unit 
CN111878064A (en) *  20200511  20201103  中国科学院地质与地球物理研究所  Attitude measurement method 
CN111878064B (en) *  20200511  20240405  中国科学院地质与地球物理研究所  Gesture measurement method 
CN113442171B (en) *  20210701  20220624  南京蔚蓝智能科技有限公司  Robot dynamic stability discrimination method and dynamic selfadaptive attitude control method 
Also Published As
Publication number  Publication date 

CN108871323B (en)  20210824 
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 highprecision navigation method of the low cost inertial sensor under motordriven 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.  Inmotion alignment for lowcost 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 singleantenna GPS and IMU under largemobility condition  
CN112902956A (en)  Course initial value acquisition method for handheld GNSS/MEMSINS 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 doubleshaft stable platform  
JP4527171B2 (en)  Vehicle attitude angle measurement method using single GPS and inertial data (acceleration, angular velocity)  
JP3425689B2 (en)  Inertial device  
CN113447018B (en)  Realtime 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)  Highlyaccurate 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 