Strapdown inertial navigation system inner lever arm calibration method and device based on least square

Abstract

The invention provides a method and a device for calibrating an inner lever arm of a strapdown inertial navigation system based on least square, and solves the technical problem that the existing inner lever arm calibration technology is too complicated. The method comprises the following steps: forming an inner lever arm model in an inertial navigation plane according to a relative error of an accelerometer and a rotating shaft; determining a speed error model formed by the inner lever arm according to the inner lever arm model; forming an observation equation of the inner lever arm according to the speed error model; and using least square estimation to calibrate an inner lever arm vector. High-precision inner lever arm calibration can be realized without setting initial parameters, and the method is suitable for engineering application.

Description

Technical Field

[0001] This invention relates to the field of strapdown inertial navigation technology, specifically to a method and apparatus for calibrating the internal arms of a strapdown inertial navigation system based on least squares. Background Technology

[0002] In existing technologies, the core measurement components of a strapdown inertial navigation system typically consist of three gyroscopes and three accelerometers. The angular velocity and linear acceleration in three-dimensional space measured by orthogonally mounted gyroscopes and accelerometers are used to calculate the velocity and position of the system's measurement center. Ideally, the measurement center of a strapdown inertial navigation system is the convergence of the three accelerometer centers at a single point. However, due to the physical dimensions of the accelerometers, the measurement centers of the three accelerometers cannot converge at a single point, thus forming an inner arm. When angular velocity or angular acceleration is present, it causes a centripetal acceleration proportional to the square of the angular velocity and a tangential acceleration proportional to the angular acceleration in the accelerometer output, resulting in an inner arm error that affects the measurement accuracy of the strapdown inertial navigation system.

[0003] To improve the measurement accuracy of strapdown inertial navigation systems, the inner arm needs to be calibrated. Traditional calibration methods typically extend the inner arm vector into the system's state variables based on the existing system error model, and then use Kalman filtering to estimate the inner arm vector, thus achieving the calibration purpose. This method requires setting complex initial filter parameters, and because the inner arm vector is coupled with other system errors, the estimation result is correlated with the filter parameter settings, significantly increasing the complexity and accuracy of the inner arm calibration. Summary of the Invention

[0004] In view of the above problems, embodiments of the present invention provide a method and apparatus for calibrating the inner arm of a strapdown inertial navigation system based on least squares, thereby solving the technical problem that the existing inner arm calibration technology is too complex.

[0005] The least squares-based stick arm calibration method for strapdown inertial navigation systems according to embodiments of the present invention includes:

[0006] The inner arm model in the inertial navigation plane is formed based on the relative error between the accelerometer and the rotation axis;

[0007] Determine the velocity error model for the formation of the inner arm based on the inner arm model;

[0008] The observation equations for the inner arm are derived based on the velocity error model;

[0009] The inner arm vector is calibrated using least squares estimation.

[0010] In one embodiment of the present invention, the inner rod arm model forming the inertial navigation plane includes:

[0011] The inner rod vector of the accelerometer in the measurement plane is formed by projecting the measurement center of the accelerometer onto the rotation axis.

[0012] In one embodiment of the present invention, the formation of the inner rod arm vector of the accelerometer in the measurement plane includes:

[0013] Draw a perpendicular line O-AX from the measurement center of accelerometer AX to the rotation axis. The projections of line segment O-AX onto the OXY plane of the carrier coordinate system are AXx and AXy, which are the two inner arms of acceleration AX. Draw a perpendicular line O-AY from the measurement center of accelerometer AY to the rotation axis. The projections of line segment O-AY onto the OXY plane of the carrier coordinate system are AYx and AYy, which are the two inner arms of acceleration AY.

[0014] In one embodiment of the present invention, the formation of the velocity error model includes:

[0015] Using the planar velocity information of the strapdown inertial navigation system as a measurement, the instantaneous velocity is formed based on the velocity error of the inner arm under angular velocity input;

[0016] Instantaneous velocity is generated based on the velocity error of the inner arm under angular acceleration input.

[0017] A velocity error model caused by the inner arm is generated based on the velocity error under the input of angular velocity and angular acceleration.

[0018] In one embodiment of the present invention, the step of forming the observation equation for the inner arm based on the velocity error model includes:

[0019] The velocity error determined by the angular velocity factor and angular acceleration factor in the velocity error model is used to form the observation matrix;

[0020] The observation equation is formed based on the observation matrix.

[0021] In one embodiment of the present invention, the observation matrix is:

[0022] In the formula, w(i) is the angular velocity of the orientation rotation mechanism at time i. Let be the angular acceleration of the azimuth rotation mechanism at time i; h(i) be the navigation azimuth angle at time i; Ts be the sampling interval; k = 1, 2, ..., n;

[0023] The observation equation is:

[0024] y k =H k x (7)

[0025] In the formula, For observation vector data; Let be the vector of the inner arm.

[0026] In one embodiment of the present invention, the step of calibrating the inner arm vector by processing the observation data using least squares estimation based on the observation equation includes:

[0027] The corresponding inner arm vector is estimated using linear least squares estimation based on the observed data.

[0028] In one embodiment of the present invention, the calibration method includes:

[0029] - Perform two-position initial alignment or rotation modulation initial alignment to obtain high-precision attitude and orientation information;

[0030] - The orientation shifting mechanism returns to zero;

[0031] - Set the velocity vector to zero and begin pure inertial navigation calculations. The orientation shifting mechanism will then perform the following calibration path control process according to the calibration path arrangement rules:

[0032] Control the orientation shifting mechanism to rotate counterclockwise 3 times;

[0033] Control the orientation shifting mechanism to rotate clockwise 3 times;

[0034] The orientation shifting mechanism returns to zero, and the pure inertial navigation calculation ends;

[0035] - Using the gyroscope data, azimuth data, and velocity data obtained during the path control calibration process, the observation matrix H from time 1 to the final n-th time is obtained. k (k = 1, 2, ..., n) and observation vector y k (k = 1, 2, ..., n);

[0036] - Concatenate the obtained observation matrix and observation vector to obtain the joint observation equation:

[0037] y = Hx (8)

[0038] In the formula,

[0039] Based on the observation equation, the inner arm vector x = [AXx AYy AXy AYx] is obtained using the least squares estimation method. T The estimated value

[0040] An embodiment of the present invention provides a least-squares-based stick arm calibration device for a strapdown inertial navigation system, comprising:

[0041] The memory is used to store the program code in the process of the above-mentioned least squares-based strapdown inertial navigation system internal arm calibration method.

[0042] A processor for executing the program code.

[0043] The least-squares-based stick arm calibration device for a strapdown inertial navigation system according to an embodiment of the present invention includes:

[0044] The inner arm mapping module is used to generate an inner arm model in the inertial navigation plane based on the relative error between the accelerometer and the rotation axis.

[0045] The lever arm error mapping module is used to determine the velocity error model of the inner lever arm based on the inner lever arm model.

[0046] The observation establishment module is used to generate the observation equations for the inner arm based on the velocity error model;

[0047] The observation calibration module is used to estimate the inner arm vector using least squares.

[0048] The least squares-based strapdown inertial navigation system internal arm calibration method and apparatus of this invention can achieve high-precision internal arm calibration without setting initial parameters, making it suitable for engineering applications. Attached Figure Description

[0049] Figure 1 The diagram shown is a schematic flowchart of an embodiment of the present invention for a least-squares-based stick arm calibration method in a strapdown inertial navigation system.

[0050] Figure 2 The diagram shown is a schematic representation of the formation of the inner arm in a strapdown inertial navigation system inner arm calibration method based on least squares according to an embodiment of the present invention.

[0051] Figure 3 The diagram shows a calibration process of an inner arm calibration method for a strapdown inertial navigation system based on least squares according to an embodiment of the present invention.

[0052] Figure 4 The diagram shown is a schematic diagram of the internal arm calibration path arrangement during the calibration process of an internal arm calibration method for a strapdown inertial navigation system based on least squares according to an embodiment of the present invention.

[0053] Figure 5 The diagram shown is a schematic representation of the calibration device architecture of a strapdown inertial navigation system based on least squares calibration method for internal lever arm calibration according to an embodiment of the present invention. Detailed Implementation

[0054] To make the objectives, technical solutions, and advantages of this invention clearer and more understandable, the invention will be further described below in conjunction with the accompanying drawings and specific embodiments. Obviously, the described embodiments are merely some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0055] An embodiment of the present invention provides a least-squares-based method for calibrating the internal lever arm of a strapdown inertial navigation system, as follows: Figure 1 As shown. In Figure 1 In this embodiment, the following are included:

[0056] Step 100: Based on the relative error between the accelerometer and the rotation axis, form the inner rod arm model in the inertial navigation plane.

[0057] Those skilled in the art will understand that the formation of the inner arm error is generally related to the motion state of the rotation axis. By measuring the measurement vector components of each accelerometer in the measurement plane relative to the rotation axis, a mapping model of the formation of the accelerometer inner arm during the axis's motion can be formed.

[0058] Step 200: Determine the velocity error model for the formation of the inner arm based on the inner arm model.

[0059] A quantitative model for the velocity error of the inner arm in the measurement plane is formed based on the influence of the angular velocity and angular acceleration of the rotation axis on the inner arm.

[0060] Step 300: Formulate the observation equations for the inner arm based on the velocity error model.

[0061] Based on the velocity error model, an observation matrix is ​​constructed during the data acquisition process of inertial navigation observations affected by the inner arm error. This matrix describes the underlying rules by which the true values ​​of each accelerometer measurement center in the measurement plane are transformed into observed values ​​under the influence of error. An observation equation incorporating velocity error is then constructed using the observation matrix.

[0062] Step 400: Use least squares estimation to calibrate the inner arm vector.

[0063] Those skilled in the art will understand that the least squares estimation method is used to estimate the true state of system or sensor parameters from observation data with errors. Least squares estimation is performed using the transpose of the observation matrix to generate estimates of the lever vectors within the measurement centers of each accelerometer in the measurement plane. These estimates are then used to overcome the inherent biases of the accelerometers.

[0064] The least squares-based strapdown inertial navigation system internal arm calibration method of this invention can achieve high-precision internal arm calibration without setting initial parameters, making it suitable for engineering applications.

[0065] like Figure 1 As shown, in one embodiment of the present invention, step 100 includes:

[0066] Step 110: Form the inner rod vector of the accelerometer in the measurement plane based on the projection of the accelerometer's measurement center and rotation axis.

[0067] In one embodiment of the present invention, the rotation axis of the orientation rotation mechanism of the strapdown inertial navigation system is used as the rotation reference, and the measurement center of the accelerometer is projected onto the rotation axis. The two projection line segments in the rotation axis plane of the carrier coordinate system are the two inner rod arm vectors of the accelerometer.

[0068] In one embodiment of the present invention, the inner arm of the strapdown inertial navigation system based on least squares calibration method is as follows: Figure 2 As shown. In Figure 2 In the figure, a perpendicular line O-AX is drawn from the measurement center of accelerometer AX to the rotation axis. The projections of line segment O-AX onto the OXY plane of the carrier coordinate system are AXx and AXy, which are the two inner arms of acceleration AX. Similarly, a perpendicular line O-AY is drawn from the measurement center of accelerometer AY to the rotation axis. The projections of line segment O-AY onto the OXY plane of the carrier coordinate system are AYx and AYy, which are the two inner arms of acceleration AY.

[0069] like Figure 1 As shown, in one embodiment of the present invention, step 200 includes:

[0070] Step 210: Using the planar velocity information of the strapdown inertial navigation system as a measurement, the instantaneous velocity is formed based on the velocity error of the inner arm under the angular velocity input.

[0071] In one embodiment of the present invention, the eastward horizontal velocity information V of the strapdown inertial navigation system is used. E and northward horizontal velocity information V N As a measurement, the velocity error of the inner arm under angular velocity input can be obtained as follows:

[0072]

[0073] In the formula, w(i) is the angular velocity of the azimuth rotation mechanism at time i; h(i) is the navigation azimuth angle at time i; Ts is the sampling interval; k = 1, 2, ..., n; V E (n) and V N (n) represents the eastward velocity and the northward velocity at time n, respectively.

[0074] Step 220: Develop instantaneous velocity based on the velocity error of the inner arm under angular acceleration input.

[0075] In one embodiment of the present invention, the eastward horizontal velocity information V of the strapdown inertial navigation system is used.E and northward horizontal velocity information V N As a measurement, the velocity error of the inner arm under angular acceleration input can be obtained as follows:

[0076]

[0077] In the formula, Let be the angular acceleration of the orientation rotation mechanism at time i.

[0078] Step 230: Generate a velocity error model caused by the inner arm based on the velocity error under the input of angular velocity and angular acceleration.

[0079] In one embodiment of the present invention, the velocity error model is as follows:

[0080]

[0081]

[0082] like Figure 1 As shown, in one embodiment of the present invention, step 300 includes:

[0083] Step 310: Combine the velocity error determined by the angular velocity factor and angular acceleration factor in the velocity error model to form the observation matrix.

[0084] In one embodiment of the present invention, an observation matrix formed according to formulas (5) and (6) is as follows:

[0085]

[0086] Step 320: Form the observation equation based on the observation matrix.

[0087] An observation equation reflecting the relationship between the inner arm and the observation data is constructed using the observation matrix, as follows:

[0088] y k =H k x(7)

[0089] In the formula, For observation vector data; Let be the vector of the inner arm.

[0090] like Figure 1 As shown, in one embodiment of the present invention, step 400 includes:

[0091] Step 410: Estimate the corresponding inner arm vector using the observation data based on the linear least squares estimation.

[0092] In one embodiment of the present invention, the linear least squares estimation formula is as follows:

[0093]

[0094] Where y represents the observed data, H is the observation matrix, and HT is the transpose of the observation matrix. This is the vector data for the inner arm.

[0095] The calibration process of the least squares-based strapdown inertial navigation system internal arm calibration method described in the above embodiments is as follows: Figure 3 As shown. In Figure 3 In the calibration process, the following steps are taken:

[0096] (1) Power on the strapdown inertial navigation system;

[0097] (2) Warm up for ten minutes, or you can set the corresponding warm-up time according to the start time of the strapdown inertial navigation system;

[0098] (3) Perform initial alignment at two positions or rotational modulation initial alignment to obtain high-precision attitude and orientation information;

[0099] (4) The orientation shifting mechanism returns to zero;

[0100] (5) Set the velocity vector to zero and begin pure inertial navigation calculations, following the calibration path arrangement rules (e.g., ...). Figure 4 As shown, the orientation shifting mechanism is calibrated and path controlled as follows:

[0101] Control the orientation shifting mechanism to rotate counterclockwise 3 times;

[0102] Control the orientation shifting mechanism to rotate clockwise 3 times;

[0103] The orientation shifting mechanism returns to zero, and the pure inertial navigation calculation ends;

[0104] (6) Using the gyroscope data, azimuth data, and velocity data obtained in step (5), obtain the observation matrix H from time 1 to the final n time. k (k = 1, 2, ..., n) and observation vector y k (k = 1, 2, ..., n);

[0105] (7) Concatenate the observation matrix and observation vector obtained in step (6) to obtain the joint observation equation:

[0106] y = Hx(8)

[0107] In the formula,

[0108] (8) Using equation (9), the inner arm vector x = [AXx AYy AXy AYx] is obtained by the least squares estimation method. T The estimated value

[0109] The least-squares-based strut inertial navigation system (SINS) internal arm calibration method of this invention forms a calibration path through the internal orientation shifting mechanism of the SINS, fully excites the internal arm error, and eliminates the influence of other system errors on the internal arm calibration accuracy. This results in accurate internal arm vector calibration.

[0110] An embodiment of the present invention provides a least-squares-based stick arm calibration device for a strapdown inertial navigation system, comprising:

[0111] The memory is used to store the program code in the process of the least squares-based strapdown inertial navigation system internal arm calibration method in the above embodiments;

[0112] The processor is used to execute the program code in the process of the least squares-based strapdown inertial navigation system arm calibration method described in the above embodiments.

[0113] The processor can be a DSP (Digital Signal Processor), an FPGA (Field-Programmable Gate Array), an MCU (Microcontroller Unit) system board, a SoC (System on a Chip) system board, or a PLC (Programmable Logic Controller) minimum system including I / O.

[0114] An embodiment of the present invention is a least-squares-based stick arm calibration device for a strapdown inertial navigation system, such as... Figure 5 As shown. In Figure 5 In this embodiment, the following are included:

[0115] The inner arm mapping module 10 is used to form an inner arm model in the inertial navigation plane based on the relative error between the accelerometer and the rotation axis.

[0116] The lever arm error mapping module 20 is used to determine the velocity error model of the inner lever arm based on the inner lever arm model.

[0117] The observation establishment module 30 is used to generate the observation equations for the inner arm based on the velocity error model;

[0118] The observation calibration module 40 is used to estimate the inner arm vector of the calibration using least squares.

[0119] like Figure 5 As shown, in one embodiment of the present invention, the inner arm mapping module 10 includes:

[0120] The vector mapping unit 11 is used to form the inner rod vector of the accelerometer in the measurement plane based on the projection of the accelerometer's measurement center and rotation axis.

[0121] like Figure 5 As shown, in one embodiment of the present invention, the arm error mapping module 20 includes:

[0122] The angular velocity error mapping unit 21 is used to take the planar velocity information of the strapdown inertial navigation system as a quantity and form an instantaneous velocity based on the velocity error of the inner arm under the angular velocity input.

[0123] Angular acceleration error mapping unit 22 is used to generate instantaneous velocity based on the velocity error of the inner arm under angular acceleration input;

[0124] The velocity error model construction unit 23 is used to form a velocity error model caused by the inner arm based on the velocity error under the input of angular velocity and angular acceleration.

[0125] like Figure 5 As shown, in one embodiment of the present invention, the observation establishment module 30 includes:

[0126] The observation matrix forming unit 31 is used to form an observation matrix by combining the velocity error determined by the angular velocity factor and the angular acceleration factor in the velocity error model.

[0127] The observation equation forming unit 32 is used to form observation equations based on the observation matrix.

[0128] like Figure 5 As shown, in one embodiment of the present invention, the observation calibration module 40 includes:

[0129] The inner arm estimation unit 41 is used to estimate the corresponding inner arm vector based on the linear least squares estimation using the observation data.

[0130] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A least squares based method for calibrating the lever arms in a strapdown inertial navigation system, characterized in that, The method comprises: forming an inner gimbal arm model in an inertial navigation plane according to relative errors between the accelerometers and the rotation axis; determining a velocity error model formed by the inner gimbal arm according to the inner gimbal arm model; forming an observation equation of the inner gimbal arm according to the velocity error model; calibrating the inner gimbal arm vector by using least square estimation.

2. The least-squares based method of calibrating the lever arms in a strapdown inertial navigation system as in claim 1, wherein, The forming of the inner gimbal arm model in the inertial navigation plane comprises: forming an inner gimbal arm vector of the accelerometer in a measurement plane according to a projection of the measurement center of the accelerometer to the rotation axis.

3. The least-squares based strapdown inertial navigation system inner lever arm calibration method of claim 2, wherein, The forming of the inner gimbal arm vector of the accelerometer in the measurement plane comprises: forming a perpendicular line O-AX of the measurement center of the accelerometer AX to the rotation axis, and projections AXx and AXy of the line segment O-AX in a carrier coordinate system OXY plane as two inner gimbal arms of the acceleration AX; forming a perpendicular line O-AY of the measurement center of the accelerometer AY to the rotation axis, and projections AYx and AYy of the line segment O-AY in the carrier coordinate system OXY plane as two inner gimbal arms of the acceleration AY.

4. The least-squares based method of calibrating the lever arms in a strapdown inertial navigation system as in claim 1, wherein, The forming of the velocity error model comprises: forming an instantaneous velocity according to velocity errors of the inner gimbal arm under angular velocity input, with plane velocity information of the strapdown inertial navigation system as a measurement; forming an instantaneous velocity according to velocity errors of the inner gimbal arm under angular acceleration input. forming a velocity error model caused by the inner gimbal arm according to velocity errors under angular velocity and angular acceleration input.

5. The least-squares based method of calibrating the lever arms in a strapdown inertial navigation system as in claim 1, wherein, The forming of the observation equation of the inner gimbal arm according to the velocity error model comprises: forming an observation matrix according to velocity errors determined by angular velocity factors and angular acceleration factors in the velocity error model; forming an observation equation according to the observation matrix.

6. The least-squares based method of calibrating the lever arms in a strapdown inertial navigation system as in claim 5, wherein, The observation matrix is: wherein, in the formula, w(i) is the angular velocity of the azimuth rotation mechanism at the i-th moment; is the angular acceleration of the azimuth rotation mechanism at the i-th moment; h(i) is the navigation azimuth at the i-th moment; Ts is the sampling interval time; k = 1, 2, …, n; The observation equation is: y k = H k x(7) wherein is the observation vector data; is the inner link arm vector.

7. The least-squares based method of calibrating the lever arms in a strapdown inertial navigation system as in claim 1, wherein, The calibrating of the inner gimbal arm vector by using least square estimation comprises: estimating corresponding inner gimbal arm vectors by observation data according to linear least square estimation.

8. The least-squares based method of calibrating the lever arms in a strapdown inertial navigation system as in claim 1, wherein, The calibrating method comprises: performing two-position initial alignment or rotation modulation initial alignment to obtain high-precision attitude and azimuth information; returning the azimuth displacement mechanism to zero position; zeroing the velocity vector, starting pure inertial navigation solution, and performing azimuth displacement mechanism calibration path control process according to calibration path arrangement rules: controlling the azimuth displacement mechanism to rotate counterclockwise for 3 turns; controlling the azimuth displacement mechanism to rotate clockwise for 3 turns; returning the azimuth displacement mechanism to zero position, and ending the pure inertial navigation solution; - obtaining an observation matrix H from time 1 to final time n using the gyro data, azimuth data and speed data obtained by the calibration path control process k (k = 1, 2,..., n) and observation vector y k (k = 1, 2,..., n); concatenating the obtained observation matrix and observation vector to obtain a joint observation equation: y = Hx (8) In the formulae, According to the observation equation, the estimated value of the inner bar arm vector x = [AXx AYy AXy AYx] is obtained using the least square estimation method T ​ 9. A least squares based strapdown inertial navigation system inner lever arm calibration apparatus, characterized by, The method comprises: a memory for storing program codes in a least square based strapdown inertial navigation system inner gimbal arm calibration method processing process according to any one of claims 1 to 8; a processor for executing the program codes.

10. A least squares based strapdown inertial navigation system inner gimbal calibration apparatus, characterized by, The method comprises: an inner gimbal arm mapping module for forming an inner gimbal arm model in an inertial navigation plane according to relative errors between the accelerometers and the rotation axis; a gimbal arm error mapping module for determining a velocity error model formed by the inner gimbal arm according to the inner gimbal arm model; an observation establishing module for forming an observation equation of the inner gimbal arm according to the velocity error model; an observation calibration module for calibrating the inner gimbal arm vector by using least square estimation.

Images