A strapdown compass rough alignment method based on DVL assistance in uniform speed straight flight

Abstract

The invention discloses a rough alignment method of a dynamic compass loop based on DVL assistance, and relates to the field of navigation control. The method is used for a marine carrier equipped with a strapdown optical gyrocompass system or an inertial navigation system and a DVL. The method comprises establishing an eastward horizontal alignment control loop, a northward horizontal alignment control loop and an azimuth alignment control loop based on the inertial navigation system error equation, and performing error compensation and alignment on at least one of the three control loops by using the DVL to collect the speed of the carrier in real time. The invention can ensure that the ship can still maintain a high dynamic alignment accuracy when the satellite navigation signal is interfered and shielded. The speed information provided by the DVL is used as an auxiliary to realize the rough alignment of the dynamic compass loop under the condition of uniform speed and straight navigation, so as to ensure that the azimuth misalignment angle is kept small when entering the fine alignment, and the fine alignment Klarman filter equation is kept linear, so as to improve the accuracy of the initial alignment, so that the inertial navigation device can provide navigation information with guaranteed accuracy when entering the navigation.

Description

Technical Field

[0001] The invention belongs to the field of inertial navigation, and in particular relates to a DVL-assisted strapdown compass loop coarse alignment method under uniform speed direct navigation. Background Art

[0002] The shipborne inertial navigation system refers to an autonomous position calculation system that uses inertial devices (gyroscopes, accelerometers), angle sensors, reference directions and initial positions to determine the ship's heading, position and speed.

[0003] In order to improve the maneuverability of the ship, the inertial navigation equipment usually needs to complete the initial alignment during the ship's navigation. In the rough alignment stage, the ship is required to maintain a uniform and straight state to avoid additional errors introduced by accelerated maneuvers and achieve rapid alignment of the inertial navigation equipment. In the navigation state, an external reference device is required to provide speed information to achieve dynamic compass loop alignment, compensate for the latitude error caused by linear motion and In general, the speed provided by the satellite navigation equipment is used as a reference, but the satellite navigation information is sometimes interfered with, and there is no method to correct the alignment of the inertial navigation equipment under such interference. Summary of the invention

[0004] Purpose of the Invention

[0005] At present, in the navigation process of ships, too much reliance is placed on the speed assistance provided by satellite navigation for dynamic compass loop alignment. However, satellite navigation signals are easily damaged and interfered with. When the satellite navigation speed cannot be provided normally, the accuracy of the coarse alignment of the compass loop of the ship's inertial navigation equipment will be affected, and ultimately the performance of the fine alignment in the initial alignment will be reduced. The navigation performance of the inertial navigation equipment cannot meet the requirements of high-precision navigation.

[0006] After extensive research, the applicant found that although DVL can only provide one-dimensional velocity information, if it is fully utilized and converted, it can achieve compass loop alignment of the inertial navigation device under dynamic conditions when satellite guidance information is not available, thereby ensuring the rapid startup performance of the inertial navigation device.

[0007] Principle of the invention:

[0008] The invention establishes an eastward horizontal alignment control loop, a northward horizontal alignment control loop and an azimuth alignment control loop based on an inertial navigation system error equation; on the one hand, a damping link is added to each control loop to calculate the correction angular rate of the alignment loop in real time, and the gyro angular rate is corrected in the attitude matrix calculation process, and then the non-divergent pitch angle, roll angle and heading angle are output; the dynamic compass loop is coarsely aligned under a uniform speed navigation state, and the speed provided by the DVL is used as a reference to compensate for the angular velocity of the ship's motion, the angular velocity of the earth's rotation, the motion acceleration of the ship, and harmful acceleration (including the Coriolis acceleration and the centripetal acceleration), so as to improve the accuracy of the coarse alignment of the compass.

[0009] A dynamic compass loop alignment method based on DVL assistance is used for a marine carrier equipped with a strapdown optical gyrocompass system or an inertial navigation system and a Doppler log (DVL). The method comprises establishing an eastward horizontal alignment control loop, a northward horizontal alignment control loop and an azimuth alignment control loop based on an inertial navigation system error equation, and is characterized in that, for at least one of the three control loops, especially the eastward and northward directions, error compensation is performed according to the following steps:

[0010] Step (1), when the carrier is in a uniform speed navigation state, using a Doppler speed meter to collect the speed of the carrier in the carrier coordinate system in real time

[0011] Step (2): Determine the attitude matrix between the navigation coordinate system and the carrier coordinate system And use the transformation matrix to calculate the speed of the carrier in the navigation coordinate system

[0012] Step (3), calculating the earth's rotation angular velocity at the current position of the carrier based on the following formula: Angular velocity of the navigation system relative to the Earth system and harmful acceleration

[0013]

[0014]

[0015]

[0016]

[0017] Among them, L dvl The ship's latitude is updated in real time based on the speed in the navigation system converted from DVL measurements, R e represents the radius of the Earth, It represents the north velocity in the navigation system converted from DVL measurement. represents the eastward velocity in the navigation system converted from DVL measurement, Represents the posture matrix The true speed under the converted ship navigation system; |

[0018] Step (4): calculate the angular velocity of the earth's rotation The three-axis components of the navigation system and the angular velocity of the navigation system relative to the earth system and harmful acceleration The attitude update of the strapdown compass alignment method under uniform speed navigation and the acceleration update control loop in the navigation coordinate system are introduced.

[0019] Furthermore, the attitude matrix between the navigation coordinate system and the carrier coordinate system at the next moment is iteratively updated based on the attitude angle output by the strapdown inertial device at the previous moment

[0020] Furthermore, the method comprises

[0021] Based on formula (1.11), the latitude of the carrier can be updated:

[0022]

[0023] In the formula, R is the north velocity in the navigation system converted from DVL measurement. e represents the radius of the earth, L0 represents the local latitude when the carrier starts to align, and L dvl The ship's latitude is updated in real time based on the speed in the navigation system converted from DVL measurements.

[0024] Further, the method includes converting the speed information measured by the DVL into the navigation system based on the following formula:

[0025]

[0026] in, It represents the speed of the ship under load measured by DVL. It represents the speed in the ship navigation system after the attitude matrix conversion.

[0027] The method also includes initializing the empirical control parameter K E1 , K E2 , K E3 , Substitute the initialized parameters into formulas 1.5a-1.5d to obtain the velocity error at the current time k The speed of updating the current moment The current position error δp Ek and the corrected angular rate ω at the current moment cNk ,

[0028]

[0029]

[0030]

[0031]

[0032] Furthermore, the carrier includes a ship and a submersible.

[0033] Beneficial Effects

[0034] The present invention can maintain a high dynamic alignment accuracy when the satellite navigation signal is interfered and shielded. The speed information provided by the DVL is used as an auxiliary to achieve the dynamic compass rough alignment under the condition of uniform speed and straight sailing, ensure that the azimuth misalignment angle is kept small when entering the fine alignment, keep the linearity of the fine alignment Karaman filter equation, improve the accuracy of the initial alignment, and enable the inertial navigation device to provide accurate navigation information when entering the navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a schematic flow chart of the strapdown compass rough alignment method under DVL-assisted uniform speed straight navigation of the present invention;

[0036] Figure 2 A schematic block diagram of the compass horizontal alignment easting loop;

[0037] Figure 3 Schematic diagram of the dynamic east compass alignment loop under uniform speed sailing conditions;

[0038] Figure 4 It is a schematic diagram of the strapdown compass alignment circuit in the uniform speed straight navigation state of the present invention. DETAILED DESCRIPTION

[0039] like Figure 1 As shown, the specific method of the present invention comprises the following steps:

[0040] (1) Establish the alignment control loop based on the inertial navigation system error equation

[0041] Taking the horizontal alignment of the east-bound loop as an example, the principle and steps of horizontal alignment are explained. Figure 2 shown.

[0042] Figure 2The flowchart of the horizontal eastward loop for alignment is shown in Figure 1. i represents the geocentric inertial coordinate system, in which the X axis is located in the equatorial plane and points to the vernal equinox, the Z axis points to the North Pole along the Earth's rotation axis, the Y axis conforms to the right-hand rule and is perpendicular to the plane formed by XZ, and the origin of the coordinates is located at the Earth's center of mass; e represents the Earth coordinate system, in which the X axis points to the intersection of the Earth's equatorial plane and the Greenwich meridian, the Z axis points to the North Pole along the Earth's rotation axis, the Y axis conforms to the right-hand rule and is perpendicular to the plane formed by XZ, and the origin of the coordinates is located at the Earth's center of mass; n represents the navigation coordinate system, referred to as the navigation system, the three axes of the navigation coordinate system point to the geographic east, north and celestial direction of the carrier's position respectively, and the origin of the coordinates is located at the center of mass of the carrier; b represents the carrier coordinate system, referred to as the carrier system, the three axes of which are consistent with the right front and upper direction of the carrier, in which the X axis points to the right side of the carrier, the Y axis points forward along the longitudinal direction of the carrier, the Z axis is perpendicular to the carrier and is vertically upward, and the origin of the coordinates is located at the center of mass of the carrier. and f b is the output of the gyroscope and accelerometer under load, and the two are input into the control loop as measurement values. c =[ω cE ω cN ω cU ] T represents the calculated easting, northing and celestial correction angular rates; is the component of the Earth's rotation angular rate in the navigation system, ω ie is 7.292×10 -5 rad / s, L and R e ( Figure 2 (in the middle) are the local geographic latitude and the radius of the earth respectively. is the rotation matrix between the carrier system and the navigation system, represents the output of the accelerometer in the navigation system, Indicates the eastward velocity error in the navigation system. δp E , K E1 , K E2 and K E3 are the variables and adjustment parameters in the eastward control loop respectively. θ represents the non-divergent pitch angle output after the eastward horizontal alignment loop is stabilized.

[0043] The eastward horizontal alignment control loop is based on the inertial navigation system error equation. Before the damping loop is added, the closed-loop characteristic equation of the eastward horizontal loop is:

[0044]

[0045] In formula (1.1), ω s Indicates the Schuler oscillation angular frequency. At this time, the system is unstable and has the Schuler periodic oscillation characteristics. In order to stabilize the output of the horizontal loop and no longer oscillate, the control loop must be damped.

[0046] (2) Add a damping element to the alignment loop

[0047] First, add a negative feedback damping link K at 1 / s in the control loop. E1 , the characteristic equation of the control system becomes:

[0048]

[0049] At this time, by properly configuring the parameter K E1 The system can be damped and stabilized and converge to the equilibrium position. However, the convergence speed of the system is too slow. By connecting the feedforward link K in parallel at the proportional coefficient E2 , which can shorten the oscillation period of the eastward horizontal alignment loop to its original times, improving the alignment speed. In addition, in order to eliminate the steady-state error caused by gyro drift, an integral link K is added at the proportional coefficient E3 / s, using the energy storage characteristics of the integral link, the alignment steady-state error of the horizontal loop is further reduced. Therefore, by adding K E1 , K E2 and K E3 The specific control process of the three control links and three control loops is shown in the following equation 1.3ac. Figure 2 The block diagram of the third-order horizontally aligned eastward loop is shown.

[0050] based on Figure 2 The transmission relationship between variables can be obtained by the horizontal control law of the east-bound loop:

[0051]

[0052]

[0053]

[0054] The update cycle and program solution cycle of the strapdown compass system sensors (gyroscopes and accelerometers, etc.) is T, which is generally aligned with the time parameter T in the loop. d It is much longer than the program calculation period T, so the first-order difference can be directly used to approximate the differential. The control law in formula (1.3) can be discretized according to the update period T as follows:

[0055]

[0056]

[0057]

[0058] In formula (1.4), k = 1, 2, 3..., which represents the sensor sampling sequence. According to formula (1.4), the north correction angular rate ω of the east horizontal alignment loop can be calculated in real time cN , in the attitude matrix The gyro angular rate is corrected during the calculation process, and then a non-divergent pitch angle θ can be output.

[0059] The above formula is only applicable to the rough alignment of the compass loop when the ship is moored. In the dynamic situation, the speed and position provided by the satellite navigation are generally used directly to compensate for the error. In special cases where the satellite navigation is not available, the present invention uses the hull speed provided by the DVL to derive the speed and latitude of the northeast sky navigation coordinate system and then perform the rough alignment of the dynamic compass loop.

[0060] However, DVL can only provide velocity information and cannot provide direct application to the three-dimensional navigation control loop.

[0061] Therefore, the present invention transforms the speed information provided by the DVL through a clever design so that the compass loop can be aligned. No similar reports have been found so far.

[0062] (3) Using the speed provided by DVL as a reference for error correction

[0063] Introducing the reference speed provided by DVL Later, the inventor changed the strapdown compass alignment algorithm in the moored state from Figure 2 Expand to Figure 3 The alignment is carried out under the condition that the ship is sailing at a constant speed.

[0064] Referring to formula (1.4), the control law discretization formula of the dynamic east compass alignment loop is obtained:

[0065]

[0066]

[0067]

[0068]

[0069] from Figure 3 From equation (1.5), it can be seen that the input for real-time calculation of the corrected angular rate is mainly the measured output value f of the accelerometer. b , the measured output value of the gyroscope External reference speed (Measured by DVL). This embodiment takes the east-direction horizontal loop as an example for description. For other directions, the components of the corresponding directions can be directly obtained by substituting them. The components of the acceleration measured by the accelerometer in the navigation coordinate system are directly used. Component on the east axis The accelerometer directly measures the acceleration f under the load system. b To obtain the acceleration in the navigation coordinate system, it is necessary to use the rotation matrix between the carrier system and the navigation system to convert Iterative Calculation It is necessary to calculate the angular velocity of the carrier coordinate system relative to the navigation coordinate system Angular velocity measured directly by gyroscope Deduction cN (ω cNk is ω at time k cN , k represents the sensor sampling sequence), and get The upper right subscript indicates the coordinate system in which the quantity is expressed, and the lower right subscript indicates the rotation of the latter coordinate relative to the previous coordinate, for example, It represents the angular velocity of the carrier coordinate system relative to the geocentric inertial coordinate system in the carrier coordinate system; It represents the angular velocity of the earth coordinate system relative to the geocentric inertial coordinate system in the carrier coordinate system, ω ie It is usually called the angular velocity of the Earth's rotation; It represents the rotational angular velocity of the navigation coordinate system relative to the earth coordinate system in the carrier coordinate system. Available through

[0070] Calculated, among which,

[0071] is the component of the Earth's rotation angular rate in the navigation system;

[0072] Available through Calculated, where In this formula, L and R e are the local geographic latitude and the radius of the earth, Indicates the northward velocity of the carrier in the navigation coordinate system, represents the eastward speed of the carrier in the navigation coordinate system, Related to the carrier movement speed, using the input reference speed Calculate and then Finally, use the updated It can be updated iteratively At the same time, the velocity error is determined by the inertial navigation eastward velocity and reference speed Subtract and get.

[0073] When the ship has a moving speed, the measured value of the ship-borne accelerometer in the navigation system during navigation is as follows:

[0074]

[0075] Where f n It represents the measurement value of the accelerometer in the navigation system. represents the acceleration of the ship motion in the navigation system, represents the Coriolis acceleration and centripetal acceleration caused by motion, g n represents the gravitational acceleration vector in the navigation system, ▽ n is the equivalent accelerometer bias in the navigation system.

[0076] It can be seen from formula (1.6) that when the ship has a moving speed, the output of the accelerometer also contains other acceleration information, which is harmful information in the strapdown compass alignment and will cause additional errors in the compass output. Therefore, in the navigation state, the output of the accelerometer and gyroscope cannot be directly used to calculate the attitude correction angular rate. The error caused by the speed needs to be analyzed and compensated. as well as The update of realizes the compensation of the above errors.

[0077] Strapdown compass alignment under navigation requires additional calculation and compensation in the following four aspects, including the angular velocity of the ship. Earth's rotation rate The ship's acceleration Harmful acceleration B n (including Coriolis acceleration and centripetal acceleration). When the strapdown compass is aligned on a stationary base or moored, these variables can be ignored and not considered. Except for the above four aspects, the calculation process is the same as that of the strapdown compass moored or stationary base.

[0078] In the state of uniform straight flight, the acceleration of motion Theoretically, it is only affected by waves, and its error is relatively small, so it is treated as ordinary noise. Therefore, the errors in the other three navigation items mentioned above are mainly calculated and compensated. The specific formula is:

[0079]

[0080]

[0081]

[0082] From the above equations (1.7)-(1.9), and B n The calculation process requires the ship's speed and position to be known in advance. However, the classic compass alignment method does not output the ship's speed and position information when moored or under a static base, so auxiliary speed measurement or positioning sensors are required during navigation to realize the calculation of the above three compensation values.

[0083] Therefore, when the initial position of the ship is known, based on the speed information obtained by the ship-borne DVL measurement, after the rough alignment is completed, the error terms caused by the speed can be calculated and compensated according to the following formulas during the alignment:

[0084] (I) Speed ​​calculation under navigation system

[0085] The speed information measured by DVL is the speed v of the ship under load. b However, the navigation solution of the strapdown compass is performed under the navigation system, and the calculation of the compensation terms in equations (1.7)-(1.9) also involves the speed under the ship's navigation system. Therefore, it is necessary to convert the speed information measured by the DVL to the navigation system. The conversion relationship can be obtained through the attitude matrix between the navigation system and the carrier system. Implementation. The details are as follows:

[0086]

[0087] In the formula It represents the speed of the ship under load measured by DVL. It represents the speed of the ship's navigation system after the attitude matrix conversion. Before the strapdown compass alignment, since the rough alignment has been completed, the initial attitude matrix can be calculated from the rough alignment attitude angle. When aligning during strapdown compass navigation, the attitude matrix at each moment It can be calculated from the attitude angle output by the strapdown compass at the previous moment.

[0088] (II) Latitude calculation

[0089] Generally, after rough alignment, the attitude angle is basically a small angle, so the error of the ship speed information in the DVL navigation system after attitude conversion is also small. Therefore, based on formula (1.11), the latitude of the ship can be updated:

[0090]

[0091] In the formula, R is the north velocity in the navigation system measured by DVL. e Indicates the radius of the earth. L0 indicates the local latitude when the ship starts to align, which can be provided by external information. dvl The latitude of the ship is updated in real time based on the speed in the navigation system measured by the DVL.

[0092] (III) Compensation value calculation for uniform speed navigation

[0093] Substituting the latitude information of the ship under navigation status and the speed information under the navigation system calculated by equations (1.10) and (1.11) into equations (1.6)-(1.9), the following formula is obtained:

[0094]

[0095]

[0096]

[0097]

[0098] The corresponding local angular velocity of the Earth's rotation can be calculated The three-axis components of the navigation system and the angular velocity of the navigation system relative to the earth system and harmful acceleration The calculated error term is then introduced into the attitude update of the strapdown compass alignment method under uniform navigation and the acceleration of the navigation system for compensation.

[0099] After adding the error compensation link caused by speed, the strapdown compass alignment method based on DVL assistance under the state of uniform speed and straight flight is as follows: Figure 4 As shown, Figure 4 This is the alignment method of the strapdown compass under uniform straight-line navigation. At this time, the ship's motion acceleration Approximately 0.

[0100] The input is: the measured value f of the accelerometer in the navigation system n , the speed of the ship under load measured by DVL as well as is the output of the gyroscope under the load system, and the output is the pitch angle θ. The two boxes at the bottom of the loop represent the compensation process for the error, and the specific calculation formula has been detailed in the above text.

[0101] like Figure 4 As shown, at the beginning of control, the empirical control parameter K is initialized E1 , K E2 , K E3 , Substitute the initialized parameters into formulas 1.5a-1.5d to obtain the velocity error at the current time k The speed of updating the current moment The current position error δp Ek and the corrected angular rate ω at the current moment cNk , substituting the corrected angular rate at the current moment into the formula right Update, then update Substitute into formula 1.12-1.15, and for f n Update, then enter formula 1.5 to complete the closed-loop update. After reaching a certain accuracy or time, based on Determine the pitch angle. If θ represents the pitch angle, ψ represents the heading angle, and γ represents the roll angle, then It can be expressed by the following tense.

[0102]

[0103] The pitch angle can be determined by the element in the third row and second column of the above formula, that is, the arc sine of the element can be calculated.

[0104] The parts not mentioned in the control loop are prior art and will not be described in detail here.

Claims

1. A dynamic compass loop alignment method based on DVL assistance, the method is used for a marine carrier equipped with a strapdown optical gyrocompass system or an inertial navigation system and a Doppler log (DVL), the method comprising establishing an eastward horizontal alignment control loop, a northward horizontal alignment control loop and an azimuth alignment control loop based on an inertial navigation system error equation, and is characterized in that: For at least one of the three control loops, perform error compensation according to the following steps: Step (1), when the carrier is in a uniform speed navigation state, using a Doppler speed meter to collect the speed of the carrier in the carrier coordinate system in real time Step (2): Determine the attitude matrix between the navigation coordinate system and the carrier coordinate system And use the transformation matrix to calculate the speed of the carrier in the navigation coordinate system Step (3), calculating the angular velocity of the earth's rotation at the current position of the carrier based on the following formula: Angular velocity of the navigation system relative to the Earth system and harmful acceleration Among them, L dvl The ship's latitude is updated in real time based on the speed in the navigation system converted from DVL measurements, R e represents the radius of the Earth, It represents the north velocity in the navigation system converted from DVL measurement. represents the eastward velocity in the navigation system converted from DVL measurement, Represents the posture matrix The true speed of the converted ship navigation system, represents the output of the accelerometer in the navigation system, represents the acceleration of the ship motion in the navigation system, ω ie is 7.292×10 - 5 rad / s; Step (4): calculate the angular velocity of the earth's rotation The three-axis components of the navigation system and the angular velocity of the navigation system relative to the earth system and harmful acceleration Introduced into the attitude update control loop of the strapdown compass alignment method under uniform speed navigation, The method also includes initializing the empirical control parameter K E1 , K E2 , K E3 , Substitute the initialized parameters into formulas 1.5a-1.5d to obtain the velocity error at the current time k The speed of updating the current moment The current position error δp Ek and the corrected angular rate ω at the current moment cNk , Substitute the corrected angular rate at the current moment into the formula right Update, then update Substitute into formula 1.12-1.15, and for f n Update, then enter formula 1.5a-1.5d to complete the closed-loop update. After reaching a certain accuracy or time, based on Determine the pitch angle, where if θ represents the pitch angle, ψ represents the heading angle, and γ represents the roll angle, then It can be expressed by the following formula:

2. The DVL-assisted dynamic compass loop alignment method according to claim 1, characterized in that: Iteratively update the attitude matrix between the navigation coordinate system and the carrier coordinate system at the next moment based on the attitude angle output by the strapdown inertial device at the previous moment 3. The DVL-assisted dynamic compass loop alignment method according to claim 1, characterized in that: The method comprises Based on formula (1.11), the latitude of the carrier can be updated: In the formula, R is the north velocity in the navigation system converted from DVL measurement. e represents the radius of the earth, L0 represents the local latitude when the carrier starts to align, and L dvl The ship's latitude is updated in real time based on the speed in the navigation system converted from DVL measurements.

4. The DVL-assisted dynamic compass loop alignment method according to claim 1, characterized in that: The method includes converting the speed information measured by the DVL into the navigation system based on the following formula: in, It represents the speed of the ship under load measured by DVL. It represents the speed in the ship navigation system after the attitude matrix conversion.

5. The DVL-assisted dynamic compass loop alignment method according to claim 1, characterized in that: The carrier includes a ship and a submarine.

Images