A combined navigation method of geomagnetic-assisted inertial navigation and laser velocimeter

Abstract

The present invention discloses a geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method, comprising the following steps: obtaining reference positioning information of a carrier at an indicated track point based on an INS / LDV combination; measuring the measured geomagnetic value of each indicated track point based on a geomagnetic sensor; extracting geomagnetic contour lines consisting of points corresponding to each reference positioning information in a geomagnetic reference map; searching for the nearest reference point of each indicated track point in the geomagnetic contour lines to obtain matching positioning information for each indicated track point; determining whether the matching positioning information of each indicated track point meets the accuracy assessment requirements, and returning the matching positioning information of each indicated track point to the INS / LDV combination if the accuracy assessment requirements are met, and correcting the accumulated error of the INS / LDV through filtering. The present invention is applied to the field of navigation, and uses geomagnetic matching to assist the INS / LDV combination in navigation and positioning, thereby further improving the positioning accuracy of the carrier and achieving high-precision and high-reliability carrier positioning.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, in particular to a geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method. Background Art

[0002] Missile launchers are military weapons that launch missiles. Rapidly and accurately determining the geographic coordinates of the launch site is crucial, placing high demands on the accuracy of positioning and orientation technology. Traditional vehicle-mounted missiles utilize a tethered launch system, launched from a pre-set position. This system suffers from poor maneuverability and flexibility, failing to fully utilize the advantages of vehicle-mounted systems. However, tethered launch technology can shorten launch preparation time, improve missile rapid launch capabilities, and enhance battlefield survivability. Current technology primarily relies on inertial navigation combined with other positioning technologies, such as inertial navigation / satellite navigation. A similar approach is disclosed in the High-Precision Train Positioning Terminal based on Beidou, UWB, and inertial navigation (Application No. CN201922479723.5). However, satellite signals in this method are susceptible to external interference, and the reliability of positioning information cannot be guaranteed. Summary of the Invention

[0003] In response to the above-mentioned deficiencies in the prior art, the present invention provides a geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method, which uses geomagnetic matching to assist the INS / LDV combination for navigation and positioning, further improving the positioning accuracy of the carrier and achieving high-precision and highly reliable carrier positioning.

[0004] To achieve the above object, the present invention provides a geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method, comprising the following steps:

[0005] Step 1: Based on the INS / LDV combination, the information of the vehicle's indicated trajectory points over a period of time is obtained, and the reference positioning information of the vehicle at each of the indicated trajectory points is obtained through Kalman filter navigation solution;

[0006] Step 2: Using a geomagnetic sensor to measure the measured geomagnetic value of each of the indicated track points in real time during the operation of the carrier, to form a measured geomagnetic sequence for geomagnetic matching;

[0007] Step 3, matching a geomagnetic reference map according to the reference positioning information, and extracting geomagnetic contour lines consisting of points corresponding to the reference positioning information in the geomagnetic reference map;

[0008] Step 4: Based on the measured geomagnetic sequence, searching for the nearest reference point of each indicated trajectory point in the geomagnetic contour line to obtain a matching geomagnetic sequence corresponding to each indicated trajectory point, and converting the matching geomagnetic sequence into matching positioning information of each indicated trajectory point based on the geomagnetic reference map;

[0009] Step 5: Determine whether the matching positioning information of each of the indicated trajectory points meets the accuracy assessment requirements. If the accuracy assessment requirements are met, the matching positioning information of each of the indicated trajectory points is returned to the INS / LDV combination. After correcting the cumulative error of the INS / LDV through filtering, repeat steps 1 to 5.

[0010] Compared with the prior art, the present invention has the following beneficial technical effects:

[0011] The present invention proposes a geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method. Based on the inertial navigation system / laser velocimeter combined navigation system, a geomagnetic sensor is introduced into the combined system to form a geomagnetic-assisted inertial navigation system / laser velocimeter combined navigation system, further improving the train positioning accuracy and reliability and achieving high-precision and high-reliability carrier positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the structures shown in these drawings without paying any creative work.

[0013] Figure 1 The figure is a flow chart of a combined navigation method of geomagnetic-assisted inertial navigation and laser velocimeter in an embodiment of the present invention.

[0014] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0015] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0016] In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the fact that ordinary technicians in this field can implement it. When the combination of technical solutions is mutually contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0017] This embodiment discloses a geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method. Based on the INS / LDV combined navigation system composed of an inertial navigation system / laser velocimeter, a geomagnetic sensor is introduced into the combined system to form a geomagnetic-assisted inertial navigation system / laser velocimeter combined navigation system, further improving the train positioning accuracy and reliability, and achieving high-precision and highly reliable carrier positioning.

[0018] refer to Figure 1 The geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method in this embodiment specifically includes the following steps 1 to 5.

[0019] Step 1: Using the INS / LDV combination, the vehicle's designated trajectory points are acquired over a period of time. This information is then applied to the Kalman filter navigation solution to obtain the vehicle's reference positioning information at each designated trajectory point. The specific process involves subtracting the velocity output by the INS in the navigation coordinate system from the LDV velocity after error model compensation. This velocity difference is used as the Kalman filter observation. The Kalman filter then outputs and corrects the error state of the vehicle's attitude, velocity, and position, thereby obtaining the reference positioning information for each designated trajectory point.

[0020] In the specific implementation process, the process of INS / LDV combination determining reference positioning information is as follows:

[0021] Step 1.1, INS can obtain the attitude, position, and velocity information of the vehicle during navigation.

[0022] In INS, attitude update refers to the real-time solution of the direction cosine matrix through the output of the sensor. Pose Matrix The differential form is:

[0023]

[0024]

[0025]

[0026] Where, is the skew-symmetric matrix form of the rotation angular velocity of the b system (carrier coordinate system) relative to the n system (navigation coordinate system), is the angular velocity of the gyro output in the b system relative to the i system (inertial coordinate system), R M is the meridian radius of the Earth, R N is the radius of the Earth's circumference, and T is the matrix device;

[0027] INS takes the n system as the navigation coordinate system, and the differential form of the velocity equation is:

[0028]

[0029] Where, is the projection of the carrier's speed in the n system, f b Indicates the specific force value in the b system obtained by adding a meter, g n Indicates the gravitational acceleration of the carrier;

[0030] The carrier position update equation is:

[0031]

[0032] Where λ is the longitude of the carrier, L is the latitude of the carrier, and h is the altitude of the carrier;

[0033] According to the angular velocity information output by the gyroscope and the speed information and position information calculated by the inertial navigation system, the attitude cosine matrix can be updated in real time Thus the posture information of the output carrier is calculated;

[0034] Step 1.2, suppress INS positioning errors;

[0035] In actual situations, due to the presence of factors such as inertial navigation system component errors, initial value errors, and calculation errors, the navigation information output by the system will inevitably contain errors. In order to suppress the divergence of INS positioning errors, the error equation is derived and the error model is established as follows:

[0036] The attitude of the carrier can be represented by the attitude transformation matrix. Since the attitude angle is small, the actual estimated value is and the true value The relationship between can be expressed as:

[0037]

[0038] The attitude error of INS can be expressed as:

[0039]

[0040] The actual speed calculation value is:

[0041]

[0042] Where V c =V n +δV n , g c =g n +δg n ,

[0043] in:

[0044] δK A =diag[δK Ax δK Ay δK Az ]

[0045]

[0046]

[0047] The position error is:

[0048]

[0049]

[0050] Step 1.3, measuring the real-time speed of the carrier during operation by LDV;

[0051] The LDV speed measurement formula is:

[0052]

[0053] Where v is the moving speed of the light source, that is, the linear velocity of the carrier; λ is the laser wavelength; c is the speed of light; θ is the angle between the speed direction and the detection beam direction; f D It is the frequency difference between the frequency of scattered light received by the detector and the frequency of laser emitted by the light source.

[0054] The transformation matrix between the m system and the b system is defined as Then the true speed in the n system is:

[0055] The speedometer scale factor error is defined as δk. Generally, the speedometer scale factor k is equal to 1, and the speedometer output speed is:

[0056] Step 1.4, calculate the LDV speed measurement error;

[0057] The installation error angle is defined as η=[α θ α γ α ψ ] T , where α θ is the pitch installation error angle, α γ is the roll installation error angle, α ψ is the heading installation error angle. In general, the installation is relatively accurate, so the installation error angle is considered to be small. The conversion matrix from the m system to the b system can be expressed as:

[0058] The projection of the tachometer output speed in the n system is:

[0059] Furthermore,

[0060] Furthermore, the speed measurement error of the speed meter in the n system is:

[0061] Step 1.5: Perform Kalman filtering on the INS / LDV combination to obtain the vehicle reference positioning information;

[0062] The state variables of the Kalman filter in the INS / LDV combination system are composed of the state variables of the INS. The state variables of the INS system are selected as follows: the east, north, and sky velocity errors δV E , δV n , δV u , roll angle error, pitch angle error, heading angle error δφ, δθ, δφ, north, east, and sky position errors δλ, δL, δh. The state quantity of the Kalman filter is: X = [δV E δV n δV u δφ δθ δψ δλ δL δh] T

[0063] According to the error equation of INS, we can get:

[0064] System state equation:

[0065]

[0066] Measurement equation:

[0067]

[0068] Furthermore,

[0069]

[0070] Taking the speed difference in the above formula as the measurement, the Kalman filter measurement equation can be obtained as:

[0071]

[0072] Where H is the measurement matrix, v(t) is the measurement noise vector.

[0073] Step 2: Using a geomagnetic sensor, the measured geomagnetic values ​​of each indicated track point are measured in real time during the operation of the carrier to form a measured geomagnetic sequence for geomagnetic matching.

[0074] Step 3: Match the geomagnetic reference map according to the reference positioning information, and extract the geomagnetic contour lines composed of the corresponding points of the reference positioning information in the geomagnetic reference map.

[0075] Step 4: Based on the measured geomagnetic sequence, search for the nearest reference point of each indicated trajectory point in the geomagnetic contour line to obtain the matching geomagnetic sequence corresponding to each indicated trajectory point, and convert the matching geomagnetic sequence into matching positioning information of each indicated trajectory point based on the geomagnetic reference map.

[0076] In the specific implementation process, the nearest reference point of each indicated trajectory point is searched in the geomagnetic contour line, specifically:

[0077] Step 4.1: record the measured geomagnetic values ​​of each indicated track point in the geomagnetic reference map as P1, P2, . . . , P N , and define the set P = {P i :i=1,2,···,N}, where N is the number of indicator trajectory points;

[0078] Step 4.2, record the points P1, P2, ..., P N The nearest reference points on the geomagnetic contour lines are Y1, Y2, ···, Y N , and define the set Y = {Y i :i=1,2,···,N};

[0079] Step 4.3, solve the rigid transformation T to minimize the Euclidean distance between set P and set Y, that is

[0080] Step 4.4, let P = TP and iterate steps 4.2 to 4.4 until the rigid transformation T converges, and output the set P after the rigid transformation T converges. The points P1, P2, . . . , P in the output set P N That is, the matching geomagnetic sequence corresponding to each indicated trajectory point, where the condition for the rigid transformation T to converge is that the difference between the rigid transformation T between the previous and next two iterations is less than the threshold value ε.

[0081] In step 5, it is determined whether the matching positioning information of each indicated trajectory point meets the accuracy assessment requirements. If the accuracy assessment requirements are met, the matching positioning information of each indicated trajectory point is returned to the INS / LDV combination. After correcting the cumulative error of the INS / LDV through filtering, steps 1 to 5 are repeated.

[0082] In this embodiment, the specific process of determining whether the matching positioning information of each indicator trajectory point meets the accuracy assessment requirements is as follows:

[0083] The corresponding points of the reference positioning information of each indicated trajectory point are A1, A2, ···, A N , where N is the number of indicator trajectory points;

[0084] Note that the corresponding points of the matching positioning information of each indicator trajectory point output in step 4 are B1, B2, . . . , B N ;

[0085] Let d i Point A i and point B i The distance between Where, i=1,2,···,N, is the distance average;

[0086] judge If it is true, then the current matching positioning information meets the accuracy evaluation requirements; otherwise, the current matching positioning information does not meet the accuracy evaluation requirements, where σ is the accuracy threshold.

[0087] As an optional implementation method, when the matching positioning information of each indicator trajectory point does not meet the accuracy evaluation requirements, the specific operation process is as follows:

[0088] Step 5.1, set the number of outer loops j = 1;

[0089] Step 5.3, determine j <J max Is it established, where J max The maximum number of outer loops:

[0090] If so, set j = j + 1, update the threshold value to ε = x·ε, and then repeat steps 4.2 to 4.4 and step 5, where x is the gradient coefficient;

[0091] Otherwise, repeat steps 1 to 5.

[0092] In this embodiment, the gradient coefficient takes a value of x=0.5-0.9.

[0093] The above description is only a preferred embodiment of the present invention and does not limit the patent scope of the present invention. All equivalent structural transformations made by using the contents of the present invention description and drawings under the inventive concept of the present invention, or direct / indirect application in other related technical fields are included in the patent protection scope of the present invention.

Claims

1. A geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method, characterized in that: The steps include: Step 1: Based on the INS / LDV combination, the information of the vehicle's indicated trajectory points over a period of time is obtained, and the reference positioning information of the vehicle at each of the indicated trajectory points is obtained through Kalman filter navigation solution; Step 2: Using a geomagnetic sensor to measure the measured geomagnetic value of each of the indicated track points in real time during the operation of the carrier, to form a measured geomagnetic sequence for geomagnetic matching; Step 3, matching a geomagnetic reference map according to the reference positioning information, and extracting geomagnetic contour lines consisting of points corresponding to the reference positioning information in the geomagnetic reference map; Step 4: Based on the measured geomagnetic sequence, searching for the nearest reference point of each indicated trajectory point in the geomagnetic contour line to obtain a matching geomagnetic sequence corresponding to each indicated trajectory point, and converting the matching geomagnetic sequence into matching positioning information of each indicated trajectory point based on the geomagnetic reference map; Step 5: Determine whether the matching positioning information of each of the indicated trajectory points meets the accuracy assessment requirements. If the accuracy assessment requirements are met, the matching positioning information of each of the indicated trajectory points is returned to the INS / LDV combination. After correcting the cumulative error of the INS / LDV through filtering, repeat steps 1 to 5.

2. The geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method according to claim 1, characterized in that: In step 4, the nearest reference point of each indicated trajectory point is searched in the geomagnetic contour line, specifically: Step 4.1: record the measured geomagnetic value of each indicated track point in the geomagnetic reference map as P1, P2, ..., P N , and define the set P = {P i :i=1,2,···,N}, where N is the number of the indication trajectory points; Step 4.2, record the points P1, P2, ..., P N The nearest reference points on the geomagnetic contour lines are Y1, Y2, ···, Y N , and define the set Y = {Y i :i=1,2,···,N}; Step 4.3, solve the rigid transformation T to minimize the Euclidean distance between set P and set Y, that is Step 4.4, let P = TP and iterate steps 4.2 to 4.4 until the rigid transformation T converges, and output the set P after the rigid transformation T converges. The points P1, P2, . . . , P in the output set P N That is, the matching geomagnetic sequence corresponding to each of the indicated trajectory points.

3. The geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method according to claim 2, characterized in that: In step 4.4, the condition for the rigid transformation T to converge is: The difference between the rigid transformation T between the two iterations is less than the threshold value ε.

4. The geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method according to claim 3, characterized in that: In step 5, the specific operation process when the matching positioning information of each of the indicated trajectory points does not meet the accuracy evaluation requirements is as follows: Step 5.1, set the number of outer loops j = 1; Step 5.3, determine j <J max Is it established, where J max The maximum number of outer loops: If so, set j = j + 1, update the threshold value to ε = x·ε, and then repeat steps 4.2 to 4.4 and step 5, where x is the gradient coefficient; Otherwise, repeat steps 1 to 5.

5. The geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method according to claim 4, characterized in that: The gradient coefficient x=0.5-0.

9.

6. The geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method according to any one of claims 1 to 5, characterized in that: In step 5, the specific process of judging whether the matching positioning information of each of the indicated trajectory points meets the accuracy assessment requirements is as follows: The corresponding points of the reference positioning information of each indicated trajectory point are A1, A2, . . . , A N , where N is the number of the indicated trajectory points; Note that the corresponding points of the matching positioning information of each of the indicated trajectory points output in step 4 are B1, B2, . . . , B N ; Let d i Point A i and point B i The distance between Where, i=1,2,···,N, is the distance average; judge If it is true, then the current matching positioning information meets the accuracy evaluation requirements; otherwise, the current matching positioning information does not meet the accuracy evaluation requirements, where σ is the accuracy threshold.

7. The geomagnetic-assisted inertial navigation and laser velocimeter combined navigation method according to any one of claims 1 to 5, characterized in that: In step 1, the process of determining the reference positioning information is as follows: The speed output by the INS in the navigation coordinate system is subtracted from the speed of the LDV after error model compensation, and the speed difference is used as the observation quantity of the Kalman filter. The error state quantity of the attitude, speed, and position of the carrier is then output through the Kalman filter and corrected, thereby obtaining the reference positioning information of each indicated trajectory point.

Images