Milemeter-assisted coarse alignment position compensation method during travel and computer equipment

Abstract

The invention provides an odometer-assisted coarse alignment position compensation method during travel and computer equipment, and the method utilizes the coarse alignment characteristic during travel based on an odometer and the dead reckoning characteristic of the odometer. The speedometer increment under the navigation coordinate system after navigation conversion is converted into the mileage increment under the initial solidification coordinate system through coordinate conversion, the mileage increment under the solidification coordinate system is accumulated in real time in the navigation process, and the mileage increment accumulated amount under the solidification coordinate system is converted into the navigation coordinate system in real time after coarse alignment convergence; the position of the vehicle can be compensated in real time in the coarse alignment process of the vehicle during traveling, and the higher the convergence precision of the coarse alignment is, the higher the positioning precision in the alignment process of the vehicle during traveling is.

Description

Technical Field

[0001] The invention belongs to the technical field of inertial navigation, and relates to an odometer-assisted on-the-go rough alignment position compensation method and a computer device. Background Art

[0002] Initial alignment is a very important area in the direction of inertial navigation. The accuracy of initial alignment will directly determine the accuracy of subsequent navigation. Initial alignment is divided into coarse alignment and fine alignment. The purpose of coarse alignment is to obtain a more accurate attitude in a shorter time and shorten the time required for fine alignment. The commonly used initial alignment method requires the inertial navigation system to remain stationary for a period of time, which greatly affects the maneuverability and reaction time of the vehicle. Therefore, using on-the-go alignment to achieve alignment while driving is very effective in improving the maneuverability of the vehicle.

[0003] On-the-go alignment cannot be accomplished using only inertial navigation, and requires external equipment to provide the carrier's motion information to achieve it. Due to the autonomy and concealment of the on-board wheel odometer, it fits well with the characteristics of inertial navigation that does not radiate signals. On-the-go alignment based on odometer assistance can achieve alignment in a moving state without any external signal input, and is an important branch of the alignment technology field.

[0004] The purpose of on-the-go alignment is to provide an accurate attitude for subsequent navigation. Its fundamental purpose is navigation positioning. However, most of the current on-the-go alignment methods focus on attitude accuracy, ignoring the position accuracy during the on-the-go alignment process. Especially during the on-the-go alignment process, navigation solution before the attitude is determined will cause a very large position error. If the vehicle position is not corrected after the alignment is completed, it will affect the positioning results in the subsequent vehicle navigation process. Summary of the invention

[0005] The present invention aims to solve at least one of the technical problems existing in the prior art or related art.

[0006] To this end, the present invention provides an odometer-assisted on-the-go rough alignment position compensation method and computer equipment, which solves the problem of positioning accuracy divergence during the on-the-go alignment process.

[0007] The technical solution of the present invention is as follows:

[0008] According to one aspect, an odometer-assisted on-the-go rough alignment position compensation method is provided, the compensation method comprising:

[0009] Step 1: Calculate the attitude change of the inertial navigation system under the load system and the effect of the Earth's rotation on posture

[0010] Step 2: Use the inertial navigation and odometer data to perform on-the-go alignment. After starting the on-the-go alignment, calculate the time from time 0 to t in real time. k Inertial navigation attitude change at the moment And the odometer output increment dS tk Mileage increment projected back to the solidification coordinate system And the mileage increment Real-time accumulation Where b(0) is the inertial navigation carrier coordinate system at time 0, which is called the solidified coordinate system;

[0011] Step 3: Get the attitude output of the alignment during travel at time 0 according to And the one obtained in step 1 and Get real-time attitude output during coarse alignment;

[0012] Step 4: According to the initial position p(0) of the vehicle, the result obtained in step 2 and the output characteristic of the on-the-go coarse alignment obtained in step 3, a position compensation equation in the on-the-go alignment process is obtained, as shown in the following equation, and position compensation in the on-the-go coarse alignment process is completed based on the compensation equation;

[0013]

[0014] Among them, p(t k ) is t k The vehicle position output at all times;

[0015]

[0016] R Mh =R e (1-2f+3fsin 2 L)

[0017] R Nh =R e (1+fsin 2 L)

[0018] Among them, p(t k ) is t k The vehicle position output at each moment, R e is the semi-major axis of the Earth, f is the flattening of the Earth, and L is the local latitude.

[0019] Furthermore, the attitude change of the inertial navigation under the load system is calculated by the following formula:

[0020]

[0021] in, is the angular velocity output by the gyroscope, where for The antisymmetric matrix of .

[0022] Furthermore, the effect of the earth's rotation on the attitude is calculated by the following formula:

[0023]

[0024] in, The angular velocity of the Earth's rotation And the angular velocity of the change from the navigation coordinate system to the geographic coordinate system constitute, for The antisymmetric matrix of .

[0025] Furthermore, in step 3, the real-time attitude output during rough alignment Obtained by the following formula:

[0026]

[0027] According to another aspect, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above method when executing the computer program.

[0028] The above technical solution utilizes the odometer-based on-the-go coarse alignment characteristics and the odometer dead reckoning characteristics to convert the odometer increments in the navigation coordinate system after the navigation back to the mileage increments in the initial solidified coordinate system through coordinate transformation, and accumulates the mileage increments in the solidified coordinate system in real time during the navigation process. After the coarse alignment converges, the accumulated mileage increments in the solidified coordinate system are converted to the navigation coordinate system in real time. The position of the vehicle can be compensated in real time during the coarse alignment process. The higher the coarse alignment convergence accuracy, the higher the positioning accuracy of the vehicle during the alignment process. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The included drawings are used to provide a further understanding of the embodiments of the present invention, which constitute a part of the specification, are used to illustrate the embodiments of the present invention, and together with the text description, explain the principles of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0030] Figure 1 It is a principle block diagram of the present invention. DETAILED DESCRIPTION

[0031] It should be noted that, in the absence of conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0032] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0033] Unless otherwise specifically stated, the relative arrangement of the parts and steps described in these embodiments, numerical expressions and numerical values ​​do not limit the scope of the present invention. At the same time, it should be understood that, for ease of description, the sizes of the various parts shown in the accompanying drawings are not drawn according to the actual proportional relationship. The technology, method and equipment known to ordinary technicians in the relevant field may not be discussed in detail, but in appropriate cases, the technology, method and equipment should be regarded as a part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters represent similar items in the following drawings, so once a certain item is defined in an accompanying drawing, it does not need to be further discussed in subsequent drawings.

[0034] like Figure 1 As shown, in one embodiment of the present invention, an odometer-assisted on-the-go rough alignment position compensation method is provided, and the compensation method includes:

[0035] Step 1: Calculate the attitude change of the inertial navigation system under the load system and the effect of the Earth's rotation on posture

[0036] Step 2: Use the inertial navigation and odometer data to perform on-the-go alignment. After starting the on-the-go alignment, calculate the time from time 0 to t in real time. k Inertial navigation attitude change at the moment And the odometer output increment dS tkMileage increment projected back to the solidification coordinate system And the mileage increment Real-time accumulation Where b(0) is the inertial navigation carrier coordinate system at time 0, which is called the solidified coordinate system;

[0037] Step 3: Get the attitude output of the alignment during travel at time 0 according to And the one obtained in step 1 and Get real-time attitude output during coarse alignment;

[0038] Step 4: According to the initial position p(0) of the vehicle, the result obtained in step 2 and the output characteristic of the on-the-go coarse alignment obtained in step 3, a position compensation equation in the on-the-go alignment process is obtained, as shown in the following equation, and position compensation in the on-the-go coarse alignment process is completed based on the compensation equation;

[0039]

[0040] Among them, p(t k ) is t k The vehicle position output at any time,

[0041] R Mh =R e (1-2f+3fsin 2 L)

[0042] R Nh =R e (1+fsin 2 L)

[0043] Among them, p(t k ) is t k The vehicle position output at each moment, R e is the semi-major axis of the Earth, f is the flattening of the Earth, and L is the local latitude.

[0044] That is, in step 1 of this embodiment, the initial position p(0) of the vehicle is input, and sensor data including gyroscope data is acquired in real time. Accelerometer data f b And the odometer data dS, and calculate the attitude change of the inertial navigation under the load system and the influence of the earth's rotation on the attitude:

[0045] in They are and The antisymmetric matrix of . is the angular velocity output by the gyroscope, The angular velocity of the Earth's rotation And the angular velocity of the change from the navigation coordinate system to the geographic coordinate system Composition, under vehicle conditions Much smaller than so Can be approximately equal to That is, this embodiment takes equal

[0046] In step 3 of this embodiment, the real-time posture output during rough alignment is Obtained by the following formula:

[0047]

[0048] It can be seen that the embodiment of the present invention utilizes the odometer-based on-the-go coarse alignment characteristics and the odometer dead reckoning characteristics to convert the odometer increments in the navigation coordinate system after the navigation back to the mileage increments in the initial solidified coordinate system through coordinate transformation, and accumulates the mileage increments in the solidified coordinate system in real time during the navigation process. After the coarse alignment converges, the accumulated mileage increments in the solidified coordinate system are converted in real time to the navigation coordinate system, so that the position of the vehicle can be compensated in real time during the coarse alignment process. The higher the coarse alignment convergence accuracy, the higher the positioning accuracy of the vehicle during the alignment process.

[0049] According to another embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the method of the above embodiment is implemented when the processor executes the computer program.

[0050] In order to have a further understanding of the compensation method of the embodiment of the present invention, a specific embodiment is described in detail below:

[0051] This embodiment provides an odometer-assisted on-the-go rough alignment position compensation method, which specifically includes:

[0052] The first step is to input the initial latitude and longitude of the vehicle p(0) and obtain gyroscope data in real time. Accelerometer data f b And the odometer data dS, and calculate the attitude change of the inertial navigation under the load system and the change of the attitude due to the rotation of the earth: This facilitates subsequent on-the-go alignment calculations;

[0053] In the second step, since the frequency of the inertial navigation data and the odometer data is consistent, the time from time 0 to t is calculated in real time after the alignment is started. k Inertial navigation attitude change at the moment And the odometer output increment dS tk Mileage increment projected back to time 0 And accumulate mileage increments in real time

[0054] The third step is to use the inertial navigation data and odometer data to perform real-time rough alignment while moving and calculate the attitude matrix at time 0. The specific calculation process is as follows:

[0055] 1) Decompose the specific force equation under n system:

[0056] Where V is the speed of the vehicle in the geographic system, is the attitude matrix of the inertial device at time t, g n is the Earth's gravitational acceleration.

[0057] 2) By transforming the basic formula, This breaks down into:

[0058]

[0059] 3) Combine the equations in 1) and 2) and multiply both left terms by right terms. get:

[0060]

[0061] 4) Integrate both sides to get:

[0062] in

[0063]

[0064] neglect Equally small quantities, and after quadratic fitting discretization, we get:

[0065]

[0066] Where T is the sampling interval, which is usually 0.1 second or 1 second. k =kT, k = 0, 1, 2…N-1. Δθ 1 , Δθ 2 , ΔV 1 and ΔV 2 t k-1 to k The angle and velocity increments of the gyro and accelerometer outputs during the sampling interval. This can be obtained by averaging the odometer output over the sampling interval.

[0067] 5) Using the QUEST algorithm, we can solve the problem of The optimal solution of , that is, the optimal alignment result of coarse alignment.

[0068] 6) Output the posture matrix of the alignment during the movement as follows:

[0069] The fourth step is to solve the equation based on dead reckoning positioning:

[0070]

[0071] Where p(t) is the vehicle position output at time t, p(0) is the vehicle position initially bound at time 0, t k The posture matrix of the carrier at each moment, For a short period of time [t k-1 t k ]The distance increment of the on-board odometer.

[0072] The conventional method is to use the real-time posture output of the on-the-go alignment in step 6) To calculate the position p(t). However, when the attitude is not converged, the attitude matrix with large error is used. Real-time dead reckoning will cause a large error in the position p(t);

[0073] Combined with the rough alignment output characteristics during travel in step 5), Decompose into Because in a short time and For a more accurate value, the alignment result After convergence, dead reckoning positioning solution can be performed to improve the positioning accuracy during the on-the-go alignment process;

[0074] The dead reckoning positioning solution equation is transformed into

[0075]

[0076] because This value changes little during the coarse alignment time and can be regarded as a constant. is the attitude matrix of the carrier at the initial moment, so It can also be regarded as a constant value. Taking out the summation part, we can get

[0077]

[0078] According to the principle of on-the-go alignment, the attitude matrix at time 0 can be output in real time during the on-the-go alignment process. If the mileage increment is accumulated in real time during the alignment process The above formula can be used to complete the position compensation during the rough alignment process.

[0079] In summary, the solution proposed in the embodiment of the present invention uses the differential satellite receiver positioning results to replace the traditional CPIII control point + total station optical measurement method, which significantly improves the measurement speed. And the inertial / odometer combined trajectory after correcting the system error falls within the error band of the satellite measurement result, while retaining the track smoothness characteristics, thereby achieving high-precision absolute measurement based on differential satellite receiver trajectory constraints.

[0080] Features described and / or illustrated above for one embodiment may be used in the same or similar manner in one or more other embodiments, and / or combined with or used in place of features in other embodiments.

[0081] It should be emphasized that the term "include / comprises" when used herein refers to the presence of features, integers, steps or components, but does not exclude the presence or addition of one or more other features, integers, steps, components or combinations thereof.

[0082] The above method of the present invention can be implemented by hardware, or by hardware combined with software. The present invention relates to such a computer-readable program, which, when executed by a logic component, enables the logic component to implement the above-mentioned device or component, or enables the logic component to implement the above-mentioned various methods or steps. The present invention also relates to a storage medium for storing the above program, such as a hard disk, a magnetic disk, an optical disk, a DVD, a flash memory, etc.

[0083] The many features and advantages of these embodiments are apparent from this detailed description, and thus the appended claims are intended to cover all such features and advantages of these embodiments that fall within their true spirit and scope. Furthermore, since numerous modifications and changes will readily occur to those skilled in the art, it is not intended that the embodiments of the invention be limited to the exact construction and operation illustrated and described, but rather all suitable modifications and equivalents falling within the scope thereof are intended to be covered.

[0084] Parts of the present invention that are not described in detail are well known to those skilled in the art.

Claims

1. An odometer-assisted on-the-go rough alignment position compensation method, characterized in that: The compensation method includes: Step 1: Calculate the attitude change of the inertial navigation system under the load system and the effect of the Earth's rotation on posture Step 2: Use the inertial navigation and odometer data to perform on-the-go alignment. After starting the on-the-go alignment, calculate the time from time 0 to t in real time. k Inertial navigation attitude change at the moment And the odometer output increment dS tk Mileage increment projected back to the solidified coordinate system And the mileage increment Real-time accumulation Where b(0) is the inertial navigation carrier coordinate system at time 0, which is called the solidified coordinate system; Step 3: Get the attitude output of the alignment during travel at time 0 according to And the one obtained in step 1 and Get real-time attitude output during coarse alignment; Step 4: According to the initial position p(0) of the vehicle, the result obtained in step 2 and the output characteristic of the on-the-go coarse alignment obtained in step 3, a position compensation equation in the on-the-go alignment process is obtained, as shown in the following equation, and position compensation in the on-the-go coarse alignment process is completed based on the compensation equation; Among them, p(t k ) is t k The vehicle position output at each moment, M pv The formula is: R Mh =R e (1-2f+3fsin 2 L) R Nh =R e (1+fsin 2 L) Among them, p(t k ) is t k The vehicle position output at each moment, R e is the semi-major axis of the Earth, f is the flattening of the Earth, and L is the local latitude.

2. The odometer-assisted on-the-go rough alignment position compensation method according to claim 1, characterized in that: The attitude change of the inertial navigation system under load is calculated by the following formula: in, is the angular velocity output by the gyroscope, where for The antisymmetric matrix of .

3. The odometer-assisted on-the-go rough alignment position compensation method according to claim 2, characterized in that: The effect of the Earth's rotation on the attitude is calculated by the following formula in, The angular velocity of the Earth's rotation And the angular velocity of the change from the navigation coordinate system to the geographic coordinate system constitute, for The antisymmetric matrix of .

4. The odometer-assisted on-the-go rough alignment position compensation method according to claims 1-3, characterized in that: In step 3, the real-time attitude output during rough alignment Obtained by the following formula:

5. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the method according to claims 1 to 4 is implemented.

Images