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
- current
- state
- vector
- 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
- 238000000034 method Methods 0.000 title claims abstract description 44
- 238000001914 filtration Methods 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 91
- 239000013598 vector Substances 0.000 claims description 59
- 238000005259 measurement Methods 0.000 claims description 41
- 230000001133 acceleration Effects 0.000 claims description 29
- 238000012937 correction Methods 0.000 claims description 29
- 238000004364 calculation method Methods 0.000 claims description 16
- 238000012546 transfer Methods 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 4
- 238000007781 pre-processing Methods 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 241001643392 Cyclea Species 0.000 claims 1
- 230000008569 process Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 230000003137 locomotive effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 239000006096 absorbing agent Substances 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 230000035939 shock Effects 0.000 description 1
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 embodiment of the invention relates to the field of navigation, in particular to a high-precision navigation method of a low-cost inertial sensor in a locomotive environment.
Background
The inertial sensor IMU is arranged on the carrier platform and can generate a large amount of noise under the influence of platform vibration and shaking, and the generated noise can be reflected in data of the sensor, so that the performance of the inertial sensor IMU is reduced, and the high-precision navigation requirement cannot be met. Currently, there are two general ways to overcome the above mentioned noise effects: one is a software optimization mode, and the other is a carrier platform optimization mode. For most of low-cost inertial sensors, due to the fact that a processing unit is poor in computing capacity and cannot execute a complex algorithm, a carrier platform optimization mode is mostly adopted. However, the diversity of the platform and the equipment, and the need to face various complex motion environments, all bring challenges to the optimization of the carrier platform, often need to design complex shock absorbers, and have poor universality and expansibility. The complexity of platform design, and the simplicity and poor robustness of the algorithm level, the influence of these comprehensive factors causes the low-cost inertial navigation scheme to generate larger errors in a short time. For the above defects in the prior art, no complete solution exists at present, and a high-precision and low-computation-quantity navigation method for a low-cost inertial sensor, which can overcome noise interference in a maneuvering environment, is urgently needed.
Disclosure of Invention
In view of this, the embodiment of the present invention provides a high-precision navigation method for a low-cost inertial sensor in a mobile environment, so as to solve the problems of high complexity and poor universality of an optimized carrier platform and large calculation amount and poor portability of a software optimization algorithm in the prior art.
The embodiment of the invention provides a high-precision navigation method of a low-cost inertial sensor in a mechanical environment, which comprises the following steps:
the method comprises the following steps: acquiring signals, namely acquiring output signals of an inertial sensor, wherein the output signals comprise acceleration and angular velocity;
step two: data preprocessing, namely performing low-pass filtering on the output signal by adopting fast Fourier transform;
step three: the signal compensation is carried out, the current signal is compensated based on the output signal of the inertial sensor in the previous period, and the signal compensation comprises cone compensation and rotary paddle compensation;
step four: acquiring the rotating speed of a carrier motor and the angular speed of a gyroscope, determining the current motion state of a carrier, and judging whether to perform attitude correction according to the motion state;
step five: and resolving the current speed and position of the carrier.
Preferably, in the third step, the rotation vector after compensation can be obtained through conical compensation, the rotation vector can be converted to obtain attitude angles of the carrier, which are respectively a roll angle, a pitch angle and a heading angle, and are respectively denoted as Φ, θ and ψ, and the specific calculation method is as follows:
where β (T) is the compensated rotation vector, Δ α is the current angular increment sample, Δ α' is the angular increment sample of the previous period, ω (T) is the angular velocity, and T is the preset period.
Preferably, in the third step, the compensated speed increment dv (t) can be obtained through the rotating paddling compensation, and the triaxial acceleration can be obtained through the dv (t) by solving, and the specific calculation method is as follows:
where Δ v is the current velocity increment sample, Δ v' is the velocity increment sample of the previous cycle, where Δ r is the velocity rotation error,is the speed rowing error.
Preferably, when the motion state of the carrier is a steady motion state, firstly performing attitude correction, and then calculating the speed and the position of the carrier; and when the motion state of the carrier is a non-steady motion state, the attitude correction is not carried out, and the speed and the position of the carrier are directly calculated.
Preferably, when the carrier includes 4 driving motors, the acquiring of the rotation speed of the carrier motor specifically includes: obtaining the rotating speeds of 4 driving motors of the current carrier, wherein the rotating speeds are m respectively1,m2,m3,m4；
Setting the minimum stable value of the motor as m _ eps;
and if the difference between the rotating speeds of the two motors in front and the two motors in back is greater than the minimum stable value of the motors, or the difference between the rotating speeds of the two motors on the left side and the rotating speeds of the two motors on the right side is greater than the minimum stable value of the motors, determining that the current carrier motion state is a non-steady motion state.
Preferably, the current triaxial angular velocities are obtained as w respectivelyN，wE，wD；
The minimum stable value of the angular velocity is w _ eps;
and if the absolute value of the angular velocity of the current three axes is larger than the minimum stable value of the angular velocity, determining that the current carrier motion state is a non-stationary motion state.
Preferably, the determining the current motion state of the carrier further includes: acquiring a current misalignment angle;
setting a threshold value of the misalignment angle to att _ eps;
and if the current misalignment angle is larger than the threshold value, determining that the current motion state of the carrier is a non-stationary motion state.
Preferably, a current misalignment angle is determined using kalman filtering based on the misalignment angle; the Kalman filtering based on the misalignment angle adopts an east horizontal misalignment angleNorth horizontal misalignment angleAnd gyro drift omegabx,ωbyAs the system state, it is recorded asAt a horizontal acceleration fx,fyAs the measurement state, it is denoted as Z ═ fyfx]T；
The gyro drift ωbx,ωbyAnd horizontal acceleration fx,fyBased on a carrier coordinate system, three axes of the carrier coordinate system are expressed as xyz, the carrier coordinate system is fixedly connected with a carrier, the coordinate system accords with a right-hand rule, an origin is at the center of gravity of the carrier, an x axis points to the advancing direction of the carrier, a y axis points to the right side of the carrier from the origin, and a z axis direction is determined by the right-hand rule according to an xy axis; the east horizontal misalignment angleNorth horizontal misalignment angleNavigating the coordinate system based on the NED;
let the state equation of the filter system be:
whereinIs the derivative of the system state, f (x) is a function of the system state, w is the system noise, Z is the measurement state of the system, h (x) is a function of the measurement state of the system, v is the measurement noise; recording the misalignment angle asGyro drift is omegab＝[ωbxωby0]TThe carrier attitude at the present time is att ═ phi θ ψ]Where φ represents roll angle, θ represents pitch angle, ψ represents heading angle;
from the misalignment angle equation, the above equation can be expressed as
Wherein,for the transformation matrix from the carrier coordinate system to the navigation coordinate system, it is noted as
Simplifying the transformation matrix to the form
Solving Jacobian matrixes for F (x) and H (x) to obtain matrixes F and H, and obtaining a linearized state equation:
wherein, the vector X is the last system state vectorIs a one-step predicted value of system state vector, matrix F is system transfer matrix, vectors W and V are system noise vectorsIs a systematic measurement vector, and the matrix H is a measurement matrix, wherein
Preferably, the kalman filtering based on the misalignment angle includes time updating and measurement correcting, and the performing the attitude correction is performing the measurement correcting to update a gain matrix and a measurement noise sequence variance matrix of the kalman filtering to eliminate the accumulated error.
Preferably, the time updating specifically comprises:
wherein the vectorFor one-step prediction of the system state vector, vectorFor the last time estimate, the matrix Φ is a discretized version of the matrix F, Φk/k-1For one-step transfer of the matrix, matrix Pk/k-1Predicting a variance matrix, matrix P, for a state vector in one stepk/k-1Is the last moment variance matrix, matrix Q, of the state vectorkIs a system noise variance matrix.
Preferably, the specific steps of the measurement update include:
wherein the vectorFor the current optimal estimate of the system state vector, the matrix KkFor the gain matrix, matrix HkFor measuring the matrix, the matrix RkFor measuring the variance matrix of the noise sequence, the matrix I is an identity matrix.
Preferably, the specific step of calculating the current speed and position of the loaded body comprises:
the velocity at the previous moment is recorded as Vk-1，The acceleration at the last moment is recorded as Ak-1，The position at the previous moment is recorded as Pk-1，Resolving time is delta t;
the velocity V at the present momentkPosition PkThe calculation method is as follows:
Vk＝Vk-1+Ak-1×Δt
Pk＝Pk-1+Vk×Δt。
the embodiment of the invention provides a high-precision navigation method of a low-cost inertial sensor in a mobile environment, which considers three factors of carrier motion, gyroscope angular velocity and misalignment angle to determine whether to perform attitude correction, can effectively reduce accumulated errors, improve navigation accuracy and reduce algorithm complexity; moreover, Kalman filtering based on the misalignment angle is adopted, only the biaxial misalignment angle and biaxial gyro offset are used as state quantities, horizontal acceleration is used as quantity measurement, and the calculation amount is greatly reduced on the premise of ensuring the resolving precision.
Drawings
FIG. 1 is a flow chart of a method for high-precision navigation of a low-cost inertial sensor in a mobile environment according to an embodiment of the present invention;
FIG. 2 is a flow chart of a position and velocity updating method based on a low-cost inertial sensor in a mobile environment according to a second embodiment of the present invention;
fig. 3 is a flowchart of a kalman filter attitude correction method based on a misalignment angle according to a third embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, embodiments of the present invention are described in further detail below with reference to the accompanying drawings. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting of the invention.
It should be further noted that, for the convenience of description, only some but not all of the relevant aspects of the present invention are shown in the drawings. Before discussing exemplary embodiments in more detail, it should be noted that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although a flowchart may describe the operations (or steps) as a sequential process, many of the operations can be performed in parallel, concurrently or simultaneously. In addition, the order of the operations may be re-arranged. The process may be terminated when its operations are completed, but may have additional steps not included in the figure. The processes may correspond to methods, functions, procedures, subroutines, and the like.
Example one
The embodiment of the invention can be particularly applied to products needing high-precision navigation, such as unmanned aerial vehicle positioning, automatic driving, digital cities, robot navigation and the like. Fig. 1 is a flowchart of a high-precision navigation method of a low-cost inertial sensor in a mobile environment according to an embodiment of the present invention. The method of the embodiment specifically includes:
110. the method comprises the following steps: and acquiring signals, namely acquiring output signals of the inertial sensor, including acceleration and angular velocity.
120. And (4) data preprocessing, namely performing low-pass filtering on the output signal by adopting fast Fourier transform.
130. And signal compensation, wherein the current signal is compensated based on the output signal of the inertial sensor in the last period, and the signal compensation comprises cone compensation and rotary paddle compensation.
In this embodiment, a rotation vector after compensation can be obtained through cone compensation, the rotation vector is converted to obtain attitude angles of the carrier, which are respectively a roll angle, a pitch angle and a heading angle, and are respectively denoted as Φ, θ and ψ, and the specific calculation method is as follows:
β (T) is the compensated rotation vector, Δ α is the current angular increment sample, Δ α' is the angular increment sample of the previous period, ω (T) is the angular velocity, and T is the preset period.
The compensated speed increment dv (T) can be obtained through the rotary oar-rowing compensation, the triaxial acceleration can be obtained through the dv (T) by solving, and the specific calculation mode is as follows:
where Δ v is the current velocity increment sample, Δ v' is the velocity increment sample of the previous cycle, where Δ r is the velocity rotation error,is the speed rowing error.
140. Determining the motion state of the carrier, acquiring the rotating speed of a carrier motor and the angular speed of a gyroscope, determining the current motion state of the carrier, and judging whether to perform attitude correction according to the motion state.
150. And resolving the current speed and position of the carrier.
In this embodiment, when the motion state of the carrier is a steady motion state, the attitude correction is performed first, and then the speed and the position of the carrier are calculated; and when the motion state of the carrier is a non-steady motion state, the attitude correction is not carried out, and the speed and the position of the carrier are directly calculated.
The specific steps of solving the current speed and position of the carrier comprise:
the velocity at the previous moment is recorded as Vk-1，The acceleration at the last moment is recorded as Ak-1，The position at the previous moment is recorded as Pk-1，Resolving time is delta t;
the velocity V at the present momentkPosition PkThe calculation method is as follows:
Vk＝Vk-1+Ak-1×Δt
Pk＝Pk-1+Vk×Δt。
the embodiment of the invention provides a high-precision navigation method of a low-cost inertial sensor in a mobile environment, which is characterized in that the calculation complexity can be reduced and the precision can be improved by adaptively improving cone compensation and rotary paddle compensation, not performing multi-period compensation processing on signals, only using the signal quantity of the previous period and combining the current signal for compensation; in addition, the motion state is judged based on the rotating speed of the carrier motor and the angular speed of the gyroscope, and whether attitude correction is carried out or not is determined according to the current motion state, so that the calculation amount is further reduced, and the navigation precision is improved.
Example two
Fig. 2 is a flowchart of a position and velocity updating method in a mobile environment based on a low-cost inertial sensor according to a second embodiment of the present invention. The present embodiment is optimized based on the above embodiments, and the method of the present embodiment specifically includes:
210. and acquiring information and determining the current motion state of the carrier.
In the embodiment, the rotation speed of the carrier motor is acquired, and the three-axis angular velocity and the misalignment angle of the gyroscope are acquired.
220. And judging whether the carrier is in a stable state or not.
In this embodiment, when the carrier includes 4 driving motors, the obtaining of the rotation speed of the carrier motor specifically includes: obtaining the rotating speeds of 4 driving motors of the current carrier, wherein the rotating speeds are m respectively1,m2,m3,m4；
Setting the minimum stable value of the motor as m _ eps;
and if the difference between the rotating speeds of the two motors in front and the two motors in back is greater than the minimum stable value of the motors, or the difference between the rotating speeds of the two motors on the left side and the rotating speeds of the two motors on the right side is greater than the minimum stable value of the motors, determining that the current carrier motion state is a non-steady motion state.
Acquiring current triaxial angular velocities of w respectivelyN，wE，wD；
The minimum stable value of the angular velocity is w _ eps;
and if the absolute value of the angular velocity of the current three axes is larger than the minimum stable value of the angular velocity, determining that the current carrier motion state is a non-stationary motion state.
The determining the current motion state of the carrier further comprises: acquiring a current misalignment angle;
setting a threshold value of the misalignment angle to att _ eps;
and if the current misalignment angle is larger than the threshold value, determining that the current motion state of the carrier is a non-stationary motion state.
Determining a current misalignment angle by adopting Kalman filtering based on the misalignment angle; the Kalman filtering based on the misalignment angle adopts an east horizontal misalignment angleNorth horizontal misalignment angleAnd gyro drift omegabx,ωbyAs the system state, it is recorded asAt a horizontal acceleration fx,fyAs the measurement state, it is denoted as Z ═ fyfx]T；
The gyro drift ωbx,ωbyAnd horizontal acceleration fx,fyBased on a carrier coordinate system, three axes of the carrier coordinate system are expressed as xyz, the carrier coordinate system is fixedly connected with a carrier, the coordinate system accords with a right-hand rule, an origin is at the center of gravity of the carrier, an x axis points to the advancing direction of the carrier, a y axis points to the right side of the carrier from the origin, and a z axis direction is determined by the right-hand rule according to an xy axis; the east horizontal misalignment angleNorth horizontal misalignment angleNavigating the coordinate system based on the NED;
let the state equation of the filter system be:
whereinIs the derivative of the system state, f (x) is a function of the system state, w is the system noise, Z is the measurement state of the system, h (x) is a function of the measurement state of the system, v is the measurement noise; recording the misalignment angle asGyro drift is omegab＝[ωbxωby0]TThe carrier attitude at the present time is att ═ phi θ ψ]Where φ represents roll angle, θ represents pitch angle, ψ represents heading angle;
from the misalignment angle equation, the above equation can be expressed as
Wherein,for the transformation matrix from the carrier coordinate system to the navigation coordinate system, it is noted as
Simplifying the transformation matrix to the form
Solving Jacobian matrixes for F (x) and H (x) to obtain matrixes F and H, and obtaining a linearized state equation:
wherein, the vector X is the last system state vectorIs a one-step predicted value of system state vector, matrix F is system transfer matrix, vectors W and V are system noise vectorsIs a systematic measurement vector, and the matrix H is a measurement matrix, wherein
230. And (6) posture correction.
In this embodiment, when the motion state of the carrier is a steady motion state, the attitude correction is performed first, and then the speed and the position of the carrier are calculated; and when the motion state of the carrier is a non-steady motion state, the attitude correction is not carried out, and the speed and the position of the carrier are directly calculated.
The Kalman filtering based on the misalignment angle comprises time updating and measurement correction, wherein the attitude correction is measurement correction, so that a gain matrix and a measurement noise sequence variance matrix of the Kalman filtering are updated, and accumulated errors are eliminated.
The time updating comprises the following specific steps:
wherein the vectorFor one-step prediction of the system state vector, vectorFor the last time estimate, the matrix Φ is a discretized version of the matrix F, Φk/k-1For one-step transfer of the matrix, matrix Pk/k-1Predicting a variance matrix, matrix P, for a state vector in one stepk/k-1Is the last moment variance matrix, matrix Q, of the state vectorkIs a system noise variance matrix.
The specific steps of the measurement correction comprise:
wherein the vectorFor the current optimal estimate of the system state vector, the matrix KkFor the gain matrix, matrix HkFor measuring the matrix, the matrix RkFor measuring the variance matrix of the noise sequence, the matrix I is an identity matrix.
240. The velocity position is resolved.
In this embodiment, the specific step of calculating the current speed and position of the loaded object includes:
the velocity at the previous moment is recorded as Vk-1，The acceleration at the last moment is recorded as Ak-1，The position at the previous moment is recorded as Pk-1，Resolving time is delta t;
the velocity V at the present momentkPosition PkThe calculation method is as follows:
Vk＝Vk-1+Ak-1×Δt
Pk＝Pk-1+Vk×Δt。
the embodiment II of the invention provides a position and speed updating method based on a low-cost inertial sensor in a maneuvering environment, which comprehensively considers three factors of the rotating speed of a carrier motor, the angular speed of a gyroscope and a misalignment angle to determine whether to perform attitude correction, can effectively reduce accumulated errors, improve the navigation accuracy and reduce the algorithm complexity; moreover, Kalman filtering based on the misalignment angle is adopted, only the biaxial misalignment angle and biaxial gyro offset are used as state quantities, horizontal acceleration is used as quantity measurement, and the calculation amount is greatly reduced on the premise of ensuring the resolving precision.
EXAMPLE III
Fig. 3 is a flowchart of a kalman filter attitude correction method based on a misalignment angle according to a third embodiment of the present invention. The method of the embodiment specifically includes:
310. measurements of a three-axis accelerometer of an inertial sensor are acquired.
320. It is determined whether the carrier is in an accelerated state.
In this embodiment, the step of determining whether the carrier is in the acceleration state includes:
step 1: calculating a vector sum of three-axis accelerometers of the inertial sensorIs marked asWherein f isx,fy,fzRespectively, three-axis acceleration values;
step 2: calculating the triaxial acceleration of the carrier in a low maneuvering state; the method comprises the following specific steps:
in the state of low mobility of the carrier, there are
Wherein the gravity vector is gn＝[0 0 g]TG is the local gravitational acceleration, f1The acceleration vector output by the triaxial accelerometer in the low maneuvering state is recorded as f1＝[fx1,fy1,fz1]T；
From equation (2), the ideal acceleration f can be calculatedx1,fy1,fz1：
And step 3: defining a deviation value as eps, respectively calculating|fx1-fx|、|fy1-fy|、|fz1-fzIf any result is greater than the deviation value eps, the carrier is in an acceleration state; otherwise, the carrier is in a non-accelerated state.
When the carrier is in non-acceleration state, the time update and measurement correction are carried out, and when the carrier is in acceleration state, only the time update is carried out
330. Time updating is performed.
In this embodiment, the time updating specifically includes:
wherein the vectorFor one-step prediction of the system state vector, vectorFor the last time estimate, the matrix Φ is a discretized version of the matrix F, Φk/k-1For one-step transfer of the matrix, matrix Pk/k-1Predicting a variance matrix, matrix P, for a state vector in one stepk/k-1Is the last moment variance matrix, matrix Q, of the state vectorkIs a system noise variance matrix.
340. Time updating and measurement correction are performed.
In this embodiment, the measurement correction specifically includes:
wherein the vectorFor the current optimal estimate of the system state vector, the matrix KkFor the gain matrix, matrix HkFor measuring the matrix, the matrix RkFor measuring the variance matrix of the noise sequence, the matrix I is an identity matrix.
350. And determining the carrier attitude.
In this embodiment, determining the attitude of the carrier comprises determining a misalignment angle of the carrier.
It can be understood by those skilled in the art that the condition for performing measurement calibration defined in this embodiment may be combined with the condition for performing measurement calibration defined in the first embodiment or the second embodiment, and the measurement calibration may be performed only when either condition is satisfied, or may be performed when both conditions are satisfied. Numerous variations, changes and substitutions will be apparent to those skilled in the art without departing from the scope of the invention.
The third embodiment of the invention provides a Kalman filtering attitude updating method based on the misalignment angle, Kalman filtering based on the misalignment angle is adopted, only a biaxial misalignment angle and biaxial gyro offset are used as state quantities, horizontal acceleration is used as measurement quantity, and the calculation quantity is greatly reduced on the premise of ensuring the resolving precision; and the parameters are used for measuring the state quantity and quantity, time updating or measurement correction can be selected adaptively, namely, the current acceleration vector sum and the difference between the accelerometer numerical value and the ideal value are used for carrying out adaptive Kalman filtering, and the calculation quantity is further reduced.
It should be noted that the foregoing is only a preferred embodiment of the invention and the technical principles employed. It will be understood by those skilled in the art that the present invention is not limited to the particular embodiments described herein, but is capable of various obvious changes, rearrangements and substitutions as will now become apparent to those skilled in the art without departing from the scope of the invention. Therefore, although the present invention has been described in greater detail by the above embodiments, the present invention is not limited to the above embodiments, and may include other equivalent embodiments without departing from the spirit of the present invention, and the scope of the present invention is determined by the scope of the appended claims.
Claims (12)
1. A high-precision navigation method of a low-cost inertial sensor in a mechanical environment is characterized by comprising the following steps:
the method comprises the following steps: acquiring signals, namely acquiring output signals of an inertial sensor, wherein the output signals comprise acceleration and angular velocity;
step two: data preprocessing, namely performing low-pass filtering on the output signal by adopting fast Fourier transform;
step three: the signal compensation is carried out, the current signal is compensated based on the output signal of the inertial sensor in the previous period, and the signal compensation comprises cone compensation and rotary paddle compensation;
step four: acquiring the rotating speed of a carrier motor and the angular speed of a gyroscope, determining the current motion state of a carrier, and judging whether to perform attitude correction according to the motion state;
step five: and resolving the current speed and position of the carrier.
2. The method as claimed in claim 1, wherein in the third step, the compensated rotation vector is obtained by cone compensation, and the rotation vector is converted to obtain attitude angles, respectively roll angle, pitch angle and heading angle, of the carrier, which are respectively denoted as Φ, θ and ψ, and is calculated by:
where β (T) is the compensated rotation vector, Δ α is the current angular increment sample, Δ α' is the angular increment sample of the previous period, ω (T) is the angular velocity, and T is the preset period.
3. The method according to claim 2, wherein in the third step, the compensated speed increment dv (t) can be obtained by the rotating oar-rowing compensation, and the triaxial acceleration can be obtained by solving the dv (t) by the following specific calculation method:
where Δ v is the current speed increment sample and Δ v' is the speed increment of the previous cycleA quantity sample, where ar is the speed rotation error,is the speed rowing error.
4. The method according to claim 1, characterized in that when the motion state of the carrier is a steady motion state, firstly performing attitude correction, and then calculating the speed and the position of the carrier; and when the motion state of the carrier is a non-steady motion state, the attitude correction is not carried out, and the speed and the position of the carrier are directly calculated.
5. The method according to claim 4, wherein when the carrier includes 4 driving motors, the obtaining of the carrier motor rotation speed specifically includes: obtaining the rotating speeds of 4 driving motors of the current carrier, wherein the rotating speeds are m respectively1,m2,m3,m4；
Setting the minimum stable value of the motor as m _ eps;
and if the difference between the rotating speeds of the two motors in front and the two motors in back is greater than the minimum stable value of the motors, or the difference between the rotating speeds of the two motors on the left side and the rotating speeds of the two motors on the right side is greater than the minimum stable value of the motors, determining that the current carrier motion state is a non-steady motion state.
6. The method of claim 4, wherein the current triaxial angular velocities are obtained as w respectivelyN，wE，wD；
The minimum stable value of the angular velocity is w _ eps;
and if the absolute value of the angular velocity of the current three axes is larger than the minimum stable value of the angular velocity, determining that the current carrier motion state is a non-stationary motion state.
7. The method of claim 1, wherein determining the current motion state of the carrier further comprises: acquiring a current misalignment angle;
setting a threshold value of the misalignment angle to att _ eps;
and if the current misalignment angle is larger than the threshold value, determining that the current motion state of the carrier is a non-stationary motion state.
8. The method of claim 7, wherein a current misalignment angle is determined using kalman filtering based on the misalignment angle; the Kalman filtering based on the misalignment angle adopts an east horizontal misalignment angleNorth horizontal misalignment angleAnd gyro drift omegabx,ωbyAs the system state, it is recorded asAt a horizontal acceleration fx,fyAs the measurement state, it is denoted as Z ═ fyfx]T；
The gyro drift ωbx,ωbyAnd horizontal acceleration fx,fyBased on a carrier coordinate system, three axes of the carrier coordinate system are expressed as xyz, the carrier coordinate system is fixedly connected with a carrier, the coordinate system accords with a right-hand rule, an origin is at the center of gravity of the carrier, an x axis points to the advancing direction of the carrier, a y axis points to the right side of the carrier from the origin, and a z axis direction is determined by the right-hand rule according to an xy axis; the east horizontal misalignment angleNorth horizontal misalignment angleNavigating the coordinate system based on the NED;
let the state equation of the filter system be:
whereinIs the derivative of the system state, f (x) is a function of the system state, w is the system noise, Z is the measurement state of the system, h (x) is a function of the measurement state of the system, v is the measurement noise; recording the misalignment angle asGyro drift is omegab＝[ωbxωby0]TThe carrier attitude at the present time is att ═ phi θ ψ]Where φ represents roll angle, θ represents pitch angle, ψ represents heading angle;
from the misalignment angle equation, the above equation can be expressed as
Wherein,for the transformation matrix from the carrier coordinate system to the navigation coordinate system, it is noted as
Simplifying the transformation matrix to the form
Solving Jacobian matrixes for F (x) and H (x) to obtain matrixes F and H, and obtaining a linearized state equation:
wherein, the vector X is the last system state vectorIs a one-step predicted value of system state vector, matrix F is system transfer matrix, vectors W and V are system noise vectorsIs a systematic measurement vector, and the matrix H is a measurement matrix, wherein
9. The method of claim 8, wherein the kalman filtering based on the misalignment angle comprises a time update and a measurement correction, and the performing the attitude correction is performing the measurement correction to update a gain matrix and a measurement noise sequence variance matrix of the kalman filtering to eliminate the accumulated error.
10. The method according to claim 9, wherein the time updating comprises:
wherein the vectorFor one-step prediction of the system state vector, vectorFor the estimated value of the last time, the matrixPhi is a discretized form of the matrix F, phik/k-1For one-step transfer of the matrix, matrix Pk/k-1Predicting a variance matrix, matrix P, for a state vector in one stepk/k-1Is the last moment variance matrix, matrix Q, of the state vectorkIs a system noise variance matrix.
11. The method of claim 9, wherein the specific steps of the measurement update include:
wherein the vectorFor the current optimal estimate of the system state vector, the matrix KkFor the gain matrix, matrix HkFor measuring the matrix, the matrix RkFor measuring the variance matrix of the noise sequence, the matrix I is an identity matrix.
12. The method according to claim 1, wherein the specific step of calculating the current velocity and position of the loaded body comprises:
the velocity at the previous moment is recorded as Vk-1，The acceleration at the last moment is recorded as Ak-1，The position at the previous moment is recorded as Pk-1，Resolving time is delta t;
the velocity V at the present momentkPosition PkThe calculation method is as follows:
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 |
|---|---|---|
| CN107560613B (en) | Robot indoor track tracking system and method based on nine-axis inertial sensor | |
| CN108225308B (en) | Quaternion-based attitude calculation method for extended Kalman filtering algorithm | |
| CN108871323B (en) | High-precision navigation method of low-cost inertial sensor in locomotive environment | |
| JP6094026B2 (en) | Posture determination method, position calculation method, and posture determination apparatus | |
| CN105203129B (en) | A kind of inertial nevigation apparatus Initial Alignment Method | |
| CN108051866B (en) | Based on strap down inertial navigation/GPS combination subsidiary level angular movement isolation Gravimetric Method | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| CN107289930B (en) | Pure inertial vehicle navigation method based on MEMS inertial measurement unit | |
| JP6922641B2 (en) | Angular velocity derivation device and angular velocity derivation method | |
| CN106814753B (en) | Target position correction method, device and system | |
| CN110954102B (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN108592917B (en) | Kalman filtering attitude estimation method based on misalignment angle | |
| CN108627152B (en) | Navigation method of micro unmanned aerial vehicle based on multi-sensor data fusion | |
| CN108827301A (en) | A kind of improvement error quaternion Kalman filtering robot pose calculation method | |
| JP2012173190A (en) | Positioning system and positioning method | |
| CN106403952A (en) | Method for measuring combined attitudes of Satcom on the move with low cost | |
| JP5419268B2 (en) | 3D attitude estimation device, 3D attitude estimation method, and 3D attitude estimation program | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| JP5164645B2 (en) | Method and apparatus for repetitive calculation control in Kalman filter processing | |
| CN114964222A (en) | Vehicle-mounted IMU attitude initialization method, and mounting angle estimation method and device | |
| CN108955671A (en) | A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle | |
| JP2014240266A (en) | Sensor drift amount estimation device and program | |
| CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN109506674B (en) | Acceleration correction method and device | |
| JP4527171B2 (en) | Vehicle attitude angle measurement method using single GPS and inertial data (acceleration, angular velocity) |
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 |