SINS / DVL installation error angle calibration method based on optimization alignment

Abstract

The application discloses a SINS / DVL installation error angle calibration method based on optimization alignment, and the method comprises the following steps: (1) constructing a SINS / DVL combined navigation motion model containing installation error angles; (2) designing an installation error angle parameter optimization target function; and (3) designing a SINS / DVL installation error angle optimization estimation algorithm. The application provides a real-time online installation angle error calibration algorithm which only relies on SINS and DVL output data, and effectively estimates the installation error between the SINS and the DVL in a complex underwater environment without GNSS signals, thereby improving the navigation performance of the SINS / DVL combined navigation.

Description

Technical Field

[0001] This invention belongs to the field of navigation technology, and specifically relates to a method for calibrating the installation error angle of SINS / DVL based on optimized alignment. Background Technology

[0002] An Autonomous Underwater Vehicle (AUV) is an autonomous navigation platform used for underwater monitoring and detection, widely applied in military and civilian navigation fields. Sealing Inertial Navigation (SINS) is an autonomous navigation system based on Newton's law of inertia, widely used in underwater navigation. Because the constant and random drift of SINS are unavoidable and accumulate over time, additional sensors are needed to assist SINS in achieving high-precision underwater navigation. Doppler velocity logs (DVLs) can provide the vehicle's velocity in bottom-tracking and water-tracking modes without errors accumulating over time. In underwater environments where GNSS signals cannot reach, DVLs are sensors that can provide effective auxiliary information. In engineering applications, installation angle error is a major factor affecting the accuracy of SINS / DVL integrated navigation systems. Current technologies for estimating installation angle error during underwater navigation are limited by the need for accurate velocity and position information from third-party sensors, or the need for specific vehicle trajectories and complex motion excitations; otherwise, calibration during underwater navigation cannot be achieved.

[0003] The existing technology is as follows:

[0004] Application No.: CN201910447543.0, Application Title: A Method for Calibrating the Installation Error Angle of USBL Based on Attitude Determination. Ultra-short baseline positioning systems (ULS) play a crucial role in underwater vehicle positioning. The installation error angle of the USBL positioning system significantly impacts its positioning accuracy. Traditional calibration methods have limited accuracy in estimating the installation error angle and are highly dependent on the route. In this application, when the SINS and USBL are fixed together during application, the USBL installation error angle remains constant. The method then utilizes attitude determination to calibrate the USBL installation error angle. First, a vector observation model based on the installation error angle matrix is ​​established. By constructing observation vectors and reference vectors, this method can calibrate the installation error angles of the SINS and USBL in real time. The advantages of this method are: it can calibrate the USBL installation error angle in real time, is simple to operate, and has no specific requirements for the calibration route; the higher the positioning accuracy of the USBL positioning system in the hydrophone receiving array coordinate system, the higher the calibration accuracy of this method.

[0005] The method used GPS position with RTK positioning accuracy to correct the attitude error of SINS. Given the transponder position calculated beforehand via LBL, a vector observation model based on the USBL installation error angle matrix was constructed, and the USBL installation error angle was solved using attitude determination methods. Our approach, however, in complex underwater environments without GNSS signals, relies solely on SINS and DVL sensors. We construct a SINS / DVL integrated navigation motion model that includes the installation error angle, design an objective function to optimize the installation error angle parameters, and combine this with a SINS / DVL installation error angle optimization estimation algorithm to calibrate the installation error angle.

[0006] To address these issues, this application first proposes a real-time online calibration algorithm for installation error angles that relies solely on SINS and DVL output data. Summary of the Invention

[0007] To address the above issues, this application proposes a SINS / DVL installation error angle calibration method based on optimized alignment. The aim is to effectively estimate the installation error between SINS and DVL sensors in complex underwater environments without GNSS signals, relying solely on SINS and DVL sensors, thereby improving the navigation performance of the KF method.

[0008] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0009] This invention provides a method for calibrating SINS / DVL installation error angles based on optimized alignment, comprising the following steps:

[0010] (1) Construct a motion model for SINS / DVL integrated navigation that includes the installation error angle;

[0011] The SINS / DVL integrated navigation motion model mentioned in step (1), which includes the installation error angle, is:

[0012]

[0013] The above equation is derived from the specific force equation, and the specific parameters in the equation have the following meanings:

[0014] η represents the installation angle error from the DVL coordinate system to the SINS coordinate system, f b This represents the specific force information measured by the accelerometer in the b-frame. Let represent the attitude matrix from time t to time 0 in the b system. Let represent the attitude matrix from time t to time 0 in the n-system. This represents the attitude matrix from the n-frame to the b-frame, and the velocity in the DVL coordinate system is... This represents the vector representing the rotational angular velocity of the Earth coordinate system relative to the inertial coordinate system in the vehicle coordinate system. The angular velocity of the carrier, g, is measured by the gyroscope in the carrier coordinate system. n Represents gravity vector information, only Since η is unknown, and all other components are obtained directly or indirectly, after integration and rearrangement, we get:

[0015]

[0016] The corresponding symbols in the above formula are defined as follows:

[0017]

[0018] Further selection of data at equal intervals, abbreviated as:

[0019]

[0020] By understanding the transformation relationship between quaternions and the attitude matrix, and then utilizing the multiplication property of quaternions, we obtain:

[0021]

[0022] (2) An objective function for optimizing the installation error angle parameter was designed;

[0023] The objective function for optimizing the installation error angle parameters constructed in step (2) is:

[0024]

[0025] The parameters in the formula are as follows: x=[qη] T Theoretically, Γ = 0 when q and η are true. 4×1 Meanwhile, L approaches 0, at which point the problem of solving q and η can be transformed into a parameter optimization problem;

[0026] (3) An optimal estimation algorithm for the installation error angle of SINS / DVL was designed;

[0027] The SINS / DVL installation error angle optimization estimation algorithm mentioned in step (3) refers to using the optimization alignment algorithm to solve the parameter optimization problem in step (2).

[0028] As a further improvement of the present invention, the specific process of the SINS / DVL installation error angle calibration method based on optimized alignment described in step (3) is as follows:

[0029] For the current time t M :

[0030] Step 1: Calculate α M ,βM and γ M Value, and then according to and Use them t M-τ α, β, and γ are calculated using time values;

[0031] Step 2: Set k = 0, and use a better estimate (x) k ,u k )start

[0032] Step 3: Calculate the first and second partial derivatives

[0033] Step 4: Solve for the increment (Δx, Δu);

[0034] Step 5: Update (x) k ,u k );

[0035] Step 6: Set k = k + 1, go back to step 3, and continue until the convergence accuracy is reached or the maximum number of iterations is reached;

[0036] Step 7: The initial attitude quaternion q and the installation angle error η were obtained;

[0037] Step 8: Calculate the attitude matrix at the current time using the initial attitude matrix.

[0038] Technical principle:

[0039] To address the problem of real-time estimation of installation errors during underwater navigation, this paper completes the estimation of the installation error angle and underwater navigation tasks in two stages. First, under simple maneuvering conditions, the installation error angle and initial attitude values ​​are estimated using an optimization estimation method. During the estimation process, since the navigation coordinate system cannot be obtained... An approximation method was used because its order of magnitude is small and does not affect the alignment results, thus providing approximate values ​​for the installation error angle and initial attitude. However, the solution for the attitude values ​​relies on chain decomposition. in The speed cannot be directly obtained from the carrier output by the IMU and DVL, and The optimal estimation method cannot provide accurate real-time attitude, velocity, and position information because it includes the constant drift of the gyroscope. Secondly, this paper uses misalignment angle error, velocity error, position error, gyroscope constant drift, accelerometer constant drift, and installation error angle as state variables. The difference between the velocity calculated by SINS and the velocity projected from the DVL output to the navigation coordinate system is used as a measurement to construct the KF method and complete the navigation task.

[0040] Beneficial effects:

[0041] This method is based on an optimized alignment SINS / DVL installation error angle calibration method. It constructs a SINS / DVL integrated navigation motion model that includes the installation error angle, designs an objective function for optimizing the installation error angle parameters, and combines it with a SINS / DVL installation error angle optimization estimation algorithm to calibrate the installation error angle. The advantages of this invention are that it effectively obtains coarse estimates of the installation error angle and initial attitude angle, and when combined with the KF method, it can provide good initial values ​​for iteration to reduce navigation errors and improve navigation accuracy. Attached Figure Description

[0042] Figure 1 This is a flowchart illustrating the overall navigation method of the present invention.

[0043] Figure 2 The motion information used in the simulation test of this invention;

[0044] Figure 3 The OBA method is used in this invention to estimate the installation angle error in simulation testing;

[0045] Figure 4 This refers to the attitude angle estimation error using the OBA method in the simulation test of this invention. Detailed Implementation

[0046] The present invention will be further described below with reference to the accompanying drawings and specific preferred embodiments, but this does not limit the scope of protection of the present invention.

[0047] The following is a detailed description of the specific implementation of the SINS / DVL installation angle error calibration method based on an iterative algorithm proposed in this invention. The overall flowchart is as follows: Figure 1 As shown, it includes the following steps:

[0048] (1) Construct a motion model for SINS / DVL integrated navigation that includes the installation error angle;

[0049] (2) Design the objective function for optimizing the installation error angle parameters;

[0050] (3) Design an optimal estimation algorithm for the installation error angle of SINS / DVL.

[0051] First, construct the SINS / DVL integrated navigation motion model mentioned in step (1), which includes the installation error angle.

[0052] The specific force equation (inertial navigation differential equation) in the b system is:

[0053]

[0054] Where V b(t) Indicates the velocity in the b-frame. f represents the carrier angular velocity measured by the gyroscope in the b-frame. b This represents the specific force information measured by the accelerometer in the b-frame, g. n Represents gravity vector information. Let represent the attitude matrix from the n-frame to the b-frame, and its transformation is . in It can be determined as follows:

[0055]

[0056] in This represents the vector representing the rotational angular velocity of the Earth coordinate system relative to the inertial coordinate system in the vehicle coordinate system. This represents the angular velocity of rotation of the Earth coordinate system relative to the inertial coordinate system in the geographic coordinate system.

[0057] By using the chain decomposition rule of the attitude matrix, the attitude matrix... It can be broken down into:

[0058]

[0059] Where the initial attitude matrix It is a constant value. This represents the attitude matrix from time t to time 0 in the b-system. Let represent the attitude matrix from time t to time 0 in the n-system, where Their differential equations are:

[0060]

[0061] in This represents the projection of the angular velocity of the n-frame relative to the i-frame onto the n-frame. It is caused by two parts: one part is caused by the rotation of the Earth, and the other part is caused by the motion of the carrier.

[0062]

[0063] This represents the angular velocity of the e-frame relative to the i-frame. Represents the angular velocity of the n-frame relative to the e-frame:

[0064]

[0065] L represents the local latitude, ω ie The angular velocity representing the Earth's rotation is 7.2921151467 × 10⁻⁶. -5 rad / s.

[0066]

[0067] VE and V N R represents the eastward and northward velocities relative to the n-system, respectively. E and R N Let be the radius of curvature in the plane normal to the meridian and the radius of curvature in the meridian plane of the reference ellipsoid, respectively. These radii can be determined by the following formula:

[0068]

[0069] R e Let e ​​be the semi-major axis of the reference ellipsoid, and e be the ellipticity of the ellipsoid. According to equations (5)(6)(7)(8), The calculation requires a real-time speed V E V N And latitude L data, but this information cannot be obtained via GNSS underwater, so It is obtained by approximating the values ​​of velocity and position at the initial moment.

[0070] In practical underwater navigation systems using SINS / DVL integrated navigation, due to installation errors, it is difficult to guarantee that the DVL coordinate system and the SINS coordinate system (b-system) will perfectly coincide. Assuming the installation angular error from the DVL coordinate system to the SINS coordinate system is represented by a vector η, then the installation error angular matrix from the DVL coordinate system to the SINS coordinate system can be expressed as: Projecting the velocity output from the DVL coordinate system onto the SINS coordinate system, we can obtain:

[0071]

[0072] Substitute equations (3) and (9) into equation (1):

[0073]

[0074] Multiply both sides of equation (10) by We can obtain:

[0075]

[0076] In the above equation, only Since η is unknown, all other components can be obtained directly or indirectly.

[0077] The observability analysis of the installation angle error is performed below. We note that the velocity error caused by the installation angle error is... The following is an expansion:

[0078]

[0079] In most cases, during AUV navigation, speed is only measured in the forward direction. lateral velocity and vertical velocity They are all very small, because The second column only and and Related, η y It is difficult to calibrate. This also explains η. y The impact on speed error is small, therefore η is ignored. y The estimation does not affect the accuracy of navigation and positioning.

[0080] The SINS / DVL integrated navigation motion model, including the installation error angle, has been determined in step (1). Next, an approximate algorithm will be given in step (2) to calculate the integral involved in step (1) to obtain the objective function for optimizing the installation error angle parameter.

[0081] Integrate both sides of equation (11) in step (1) over the time interval [0, t].

[0082]

[0083] Decompose the terms in formula (13), the first integral term on the left is... It can be represented as

[0084]

[0085] The second integral term on the left side of equation (13) It can be represented as

[0086]

[0087] Note that both equations (14) and (15) have terms, but they take opposite values, so they cancel each other out. According to the integral discretization rule, the integration time period is 0~t[t] k t k+1 The interval can be divided into M segments, based on the update time interval, where t k =kT and T are the uniform durations of the update time intervals, and the rest are... The item can be written as:

[0088]

[0089] The third integral term on the left-hand side of equation (13) Represented as:

[0090]

[0091] Where Δv is the update interval [t]k t k+1 The velocity increment measured by the internal accelerometer, Δθ, is the update interval [t]. k t k+1 The angular increment measured by the internal gyroscope. The fourth integral term on the left side of equation (13). Represented as

[0092]

[0093] The last integral term on the left-hand side of equation (13) is expressed as:

[0094]

[0095] The terms on the right side of equation (13) It can be represented as

[0096]

[0097] Substitute equations (14)-(20) into equation (13):

[0098]

[0099] The above equation can be reformulated as follows:

[0100]

[0101] The definition of a symbol is as follows:

[0102]

[0103] In equation (23), due to the variation in the integration length, the equations for different M values ​​exhibit different error characteristics. Here, we select data lengths at equal intervals, and the time interval can be taken as τ = 1s:

[0104]

[0105] For the sake of brevity, we will rewrite equation (24) as follows:

[0106]

[0107] The coefficient is α = α M -α M-τ ,β=β M -β M-τ ,γ=γ M -γ M-τ

[0108] The SINS / DVL installation error angle optimization estimation algorithm in design step (3) is used to solve the parameter optimization problem in step (2):

[0109] First, using the transformation relationship between quaternions and attitude matrices, and the multiplication property of quaternions, we obtain:

[0110]

[0111] The left side of equation (26) is constructed as the following objective function:

[0112]

[0113] When both the quaternion q and the installation error η are infinitely close to the true value, the left side of equation (26) is infinitely close to the vector 0. 4×1 Γ T Γ is the sum of squares of its elements. T Γ is not less than zero, therefore the function Γ T Γ has an extremum of zero. Consider the constraint q. T When q = 1, the problem of finding the parameters q and η can be transformed into an optimization problem of finding the extrema of a multivariate function with Lagrange constraints.

[0114] Since the accumulation of data from different time intervals is beneficial for identifying unknown parameters, the objective function is further constructed as follows:

[0115]

[0116] In the formula u is the Lagrange multiplier; note that L is a scalar. The iteration method is as follows:

[0117]

[0118] Where (ΔxΔu) satisfies:

[0119]

[0120] The final algorithm flow is as follows:

[0121] For the current time t M :

[0122] Step 1: Calculate α incrementally according to equation (23) M , and γ M The values ​​are then used based on (24) and (25). M-τ α, β, and γ are calculated using time values;

[0123] Step 2: Set k = 0, and use a better estimate (x) k ,u k )start;

[0124] Step 3: Calculate the first and second partial derivatives

[0125] Step 4: Solve for (Δx, Δu) according to the requirements of (30);

[0126] Step 5: Use (29) to update (x) k ,u k );

[0127] Step 6: Set k = k + 1, go back to step 3, and continue until the convergence accuracy is reached or the maximum number of iterations is reached;

[0128] Step 7: The initial attitude quaternion q and the installation angle error η were obtained;

[0129] Step 8: Using the initial attitude matrix, calculate the attitude matrix at the current time according to formula (3).

[0130] This completes the derivation of the SINS / DVL mounting angle error calibration algorithm based on iterative algorithm of this invention.

[0131] The beneficial effects of this invention were verified through the following simulations:

[0132] Simulation test settings:

[0133] Generate a description of the simulated trajectory (simple motion):

[0134] To enable the estimation of the installation error angle between SINS / DVL of the x and z axes under simple maneuvering conditions, a design was developed as follows: Figure 2 The following is a motion model including simple changes in velocity and angle:

[0135] Throughout the motion, the velocities are: uniform, uniformly accelerated, and variable acceleration. The initial velocity in the navigation coordinate system is

[350] m / s, and from 0 to 10 s, the velocity in the direction of the carrier's forward movement is 1 m / s. 2 The vehicle undergoes uniform acceleration for 10 seconds, maintaining uniform motion from 10 to 100 seconds. From 100 to 110 seconds, it undergoes variable acceleration in the forward direction of the vehicle with a rate of -2sin(PI*t / 10), maintaining uniform motion from 110 to 300 seconds. The heading angle is rotated 90°, with an initial attitude angle of [166990]°. This attitude angle is maintained constant from 0 to 120 seconds. From 120 to 140 seconds, the heading angle increases sinusoidally to 180°, while other angles remain unchanged, and this state is maintained until 300 seconds. Additionally, the accelerometer constant drift is [505050]ug, and the gyroscope constant drift is [0.010.010.01]° / h. The installation error angle is set to [0.501.9]°.

[0136] Simulation results based on the Optimized Alignment (OBA) algorithm:

[0137] The OBA algorithm introduced in this article can simultaneously estimate the initial attitude angle and the installation error angle. Regarding the setting of the initial value: the initial value of q is obtained by the OBA algorithm without considering the installation error angle (the eigenvector corresponding to the smallest eigenvalue of q-method), and the initial value of the installation error angle is selected as 1 / 10 of the set value. Figure 3 The installation error angles sita-x and sita-z estimated by the OBA method have mean and variance of [0.455-1.904]° and [0.001325-0.000143]° for the last 100 seconds, respectively. Figure 4 The attitude angle error values ​​estimated by the OBA method have a mean and variance of [0.6535 - 0.2546 0.2538]' (angular minutes) and [0.046936 0.008328 0.048017]' for the last 100 seconds, respectively. The simulation experiment of trajectory one proves that the OBA method can achieve coarse alignment with a certain accuracy and can be used as the initial input value of the kf method.

[0138] Those skilled in the art will understand that embodiments of the present invention can be provided as methods or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0139] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0140] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0141] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0142] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A method for calibrating SINS / DVL installation error angles based on optimized alignment, comprising the following steps, characterized in that: (1) Construct a motion model for SINS / DVL integrated navigation that includes the installation error angle; The SINS / DVL integrated navigation motion model mentioned in step (1), which includes the installation error angle, is: ； The above equation is derived from the specific force equation, and the specific parameters in the equation have the following meanings: This represents the installation angle error from the DVL coordinate system to the SINS coordinate system. This represents the specific force information measured by the accelerometer in the b-frame. Let represent the attitude matrix from time t to time 0 in the b system. Let represent the attitude matrix from time t to time 0 in the n-system. This represents the attitude matrix from the n-frame to the b-frame, and the velocity in the DVL coordinate system is... , This represents the vector representing the rotational angular velocity of the Earth coordinate system relative to the inertial coordinate system in the vehicle coordinate system. This represents the angular velocity of the carrier as measured by the gyroscope in the carrier coordinate system. Represents gravity vector information, only and The unknown components are obtained directly or indirectly. After integration and rearranging, we get: ； The corresponding symbols in the above formula are defined as follows: ； Further selection of data at equal intervals, abbreviated as: ； By understanding the transformation relationship between quaternions and the attitude matrix, and then utilizing the multiplication property of quaternions, we obtain: ； (2) An objective function for optimizing the installation error angle parameter was designed; The objective function for optimizing the installation error angle parameters constructed in step (2) is: ； The parameters in the formula are as follows: , Theoretically when and When taking the truth value ,at the same time Approaching 0, at this point and The problem of solving this problem can be transformed into a parameter optimization problem; (3) An optimal estimation algorithm for the installation error angle of SINS / DVL was designed; The SINS / DVL installation error angle optimization estimation algorithm mentioned in step (3) refers to using the optimization alignment algorithm to solve the parameter optimization problem in step (2).

2. The SINS / DVL installation error angle calibration method based on optimized alignment according to claim 1, characterized in that: The specific process of the SINS / DVL installation error angle calibration method based on optimized alignment in step (3) is as follows: For the current time : Step 1: Calculate , and Value, and then according to and Use them Calculated by time value , and ; Step 2: Set k=0, and use a better estimate. start Step 3: Calculate the first and second partial derivatives , , ; Step 4: Solve for the increment ; Step 5: Update ; Step 6: Set k=k+1, go back to step 3, and continue until the convergence accuracy is reached or the maximum number of iterations is reached; Step 7: The initial pose quaternion was obtained. and installation angle error ; Step 8: Calculate the attitude matrix at the current time using the initial attitude matrix.

Images