Description of strapdown inertial navigation
1. Four concepts: "geographic" coordinate system, "machine body" coordinate system, conversion formulas between them, and coefficients used in conversion formulas.
Geographical coordinate system: east, north, sky, hereinafter referred to as geography. In this coordinate system, the gravity is always (0,0,1g), and the geomagnetism is always (0,1,x) (the verticality of the geomagnetism does not care) two three-dimensional vectors.
Body coordinate system: hereinafter referred to as the body, on which there are gyro, accumulator, electronic compass sensors, three three-dimensional vectors.
Conversion formula: Hereinafter referred to as formula, the formula is the expression method to describe the attitude of the airframe. Generally, it is a formula for converting from geography to the airframe based on geography. There are quaternion, Euler angle, and direction cosine matrix.
The coefficient of the conversion formula: hereinafter referred to as the coefficient, q0123 of quaternion, ROLL/PITCH/YAW of Euler angle, 9 numbers of cosine matrix. The coefficient is the specific value describing the expression method of the body's posture.
Attitude is actually a combination of formula + coefficient. It is usually expressed by the easy-to-understand formula "Euler angle", and the coefficient is roll xx degrees, pitch xx degrees and heading xx degrees.
2. Five data sources: gravity, geomagnetism, gyroscope, plus meter, electronic compass, the first two are from geography, and the last three are from the body.
3. Gyro vector: Based on the body, it is also integrated on the body. Because there is no reference data source geographically, it is very independent. It is directly integrated on the old coefficient of the formula to get the new coefficient.
The strapdown inertial navigation algorithm in the narrow sense refers to this gyro integral formula, which is also divided into Euler angle, direction cosine matrix, and quaternion. Their integral algorithm includes incremental method and numerical integration method (X-order Runge-Library Tower) wait
4. Adding vector and gravity vector: Adding is based on the body, and gravity is based on geography. The gravity vector (0,0,1g) is converted to the body with a formula, and the error is calculated from the adding vector of the body. In theory, there should be no error. Thinking about this error backwards, it is actually the coefficient error of the conversion formula. So this error can be used to correct the coefficients of the formula (roll, pitch), which is the attitude.
5. Electronic compass vector and geomagnetic vector: Same as above, except that the geographic vertical vector is cut off because it is useless. Only the vectors on the geographic level are left. The error can be used to correct the coefficient (heading) of the formula.
6. In this way, the coefficients are constantly updated by the gyro integral, and constantly updated by the error, and the attitude represented by it and the formula are also constantly updated.
If you use integration and correction, and finally use Euler angle output to control the PID (because the angle is more intuitive), then there needs to be a conversion from quaternion coefficient to Euler angle coefficient. There are conversion algorithms among the three commonly used formulas.
Let’s make a more straightforward example: the
body is like a ship, the geography is the map, the attitude is the heading (the position of the bow on the map), gravity and geomagnetism are the lighthouses on the map, and the gyro/integral formula is the helmsman. And the electronic compass is a watchman.
The helmsman is responsible for estimating and stabilizing the course. He believes that if the ship is heading north, it will always head north. He thinks it will head east after turning a 90-degree turn.
Of course, if the helmsman is very powerful, it may be estimated accurately and lasts a long time. But if you only trust the helmsman, you will definitely get lost, so there are usually maps and watchmen to observe the error.
According to the lighthouse position on the map and the current heading of the ship, the observer calculated that the lighthouse should theoretically be in the X position of the ship. However, seeing that the actual lighthouse is in the Y position of the ship, the current course of the ship must be deviated, and the deviation is ERR=XY.
If the helmsman receives the ERR report from the watchman and thinks it is reliable, he will listen to 90%*ERR. If the weather is bad and the map error is large, he will listen to 10%*ERR, and correct the estimated heading based on this. .