SINS / ODO / GNSS annealing satellite selection tight combination vehicle-mounted system based on speed correction and method thereof

Abstract

The invention discloses an SINS / ODO / GNSS annealing satellite selection tight combination vehicle-mounted system based on speed correction and a method thereof, and relates to the technical field of train navigation and positioning. By establishing a speed optimization algorithm based on four-wheel ABS sliding mode filtering, the speed error of the vehicle during large-amplitude turning is reduced, a satellite selection algorithm of a multi-stage annealing algorithm is selected, the optimal satellite combination is selected, the algorithm operation efficiency is improved, and meanwhile the accuracy of satellite calculation is guaranteed. An algorithm based on extended Kalman filter (EKF) is used, multi-source data are fused, the positioning precision, the system robustness and the dynamic tracking capability are improved, and especially under the condition of GNSS signal loss or interference, error accumulation and position drift can be effectively reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of train navigation and positioning, and in particular to a SINS / ODO / GNSS annealing star selection tight combination vehicle-mounted system based on speed correction and a method thereof. Background Art

[0002] With the development of intelligent automobile systems, the demand for real-time and accurate vehicle positioning has become more urgent. Due to the complex and changeable driving environment of vehicles, the current mainstream approach is to combine the Global Navigation Satellite System (GNSS) with the Strapdown Inertial Navigation System (SINS), and use algorithms such as Kalman filtering to perform optimal estimation, thereby giving full play to the complementary advantages of the two. SINS can provide high autonomy and real-time navigation capabilities, while GNSS is used to control error accumulation. Although the SINS / GNSS combination can solve the problem of vehicle positioning in general scenarios, GNSS is susceptible to environmental interference in complex environments (urban canyons), resulting in reduced positioning accuracy. In special environments (tunnels), the GNSS signal is completely lost and cannot maintain good accuracy in long tunnels. Therefore, the vehicle navigation system usually also uses a combination of SINS and odometer (Odometer, ODO). When GNSS signals are affected, ODO can provide accurate speed information and achieve high-precision positioning in complex or extreme environments. ODO mainly infers vehicle speed by detecting the vehicle's drive shaft speed. In extreme road conditions (such as slippery roads, gravel roads, and large turns), errors will be generated due to differences in tire-road friction, which will affect the positioning results. At the same time, when using satellite observations and ephemeris data to solve position and speed in the tight combination algorithm, there is redundancy in the number of satellites, which affects the running speed of the algorithm.

[0003] Therefore, we proposed a SINS / ODO / GNSS annealing satellite selection tight integration vehicle-mounted system and method based on velocity correction. Summary of the invention

[0004] The purpose of the present invention is to solve the above-mentioned problems and to provide a SINS / ODO / GNSS annealing star selection tight combination vehicle-mounted system and method based on velocity correction.

[0005] To achieve the above object, the technical solution adopted by the present invention is:

[0006] According to one aspect of the present invention, a SINS / ODO / GNSS annealing satellite selection tight combination vehicle-mounted system based on velocity correction and a method thereof are provided. Compared with the prior art, the present invention has the following beneficial effects:

[0007] The process of a SINS / ODO / GNSS annealing star selection tight combination vehicle-mounted system based on speed correction proposed in the present invention is an efficient filtering solution, which effectively improves the positioning accuracy of vehicles on complex roads by combining multiple sensors.

[0008] The sensor used in the SINS / ODO / GNSS annealing star selection compact combination vehicle-mounted system based on speed correction proposed by the present invention has the characteristic of being easy to install.

[0009] The invention proposes a SINS / ODO / GNSS annealing satellite selection tight combination vehicle-mounted system based on velocity correction. Two sets of velocity observation systems are used in implementation to reduce the positioning error of the system when the satellite signal is poor.

[0010] The invention proposes a SINS / ODO / GNSS annealing star selection tight combination vehicle-mounted system based on speed correction, which can correct the overall speed and heading through four-wheel sensors by means of sliding film filtering.

[0011] The present invention proposes a SINS / ODO / GNSS annealing satellite selection tight combination vehicle-mounted system based on velocity correction, which can quickly and accurately find the optimal satellite combination in the satellite selection stage, thereby improving the hardware computing efficiency and reducing the GNSS positioning error. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] Figure 1 It is a schematic diagram of the structure of the combined system of the present invention;

[0013] Figure 2 It is a schematic diagram of the installation position of the sensor of the present invention;

[0014] Figure 3 A schematic diagram of a vehicle wheel speed sensing system of the present invention;

[0015] Figure 4 It is a schematic diagram of the EKF tight combination algorithm based on INS / GNSS / ODO of the present invention;

[0016] Figure 5 The flow chart of speed correction based on sliding film filtering of the present invention;

[0017] Figure 6 This is a flow chart of the multi-stage annealing star selection algorithm of the present invention. DETAILED DESCRIPTION

[0018] In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the present invention is further explained below in conjunction with specific implementation methods.

[0019] Benru Figure 1-6As shown, the present invention provides a SINS / ODO / GNSS annealing satellite selection compact vehicle-mounted system based on speed correction and its method, the system includes: a multi-star multi-frequency external measurement antenna module, a GNSS receiver module, a SSINS inertial navigation module, and an ODO speed detection module based on an ABS wheel speed sensor. The system structure diagram is shown in Figure 1 shown.

[0020] The multi-satellite multi-frequency external measurement antenna module adopts a multi-feed point design with high unit gain and strong satellite search signal, which can effectively reduce the GNSS signal measurement error and make the entire combined vehicle-mounted system have a certain degree of anti-interference.

[0021] The combined vehicle system includes the installation and calibration of various sensors. Sensor installation locations such as Figure 2 As shown, the odometer is installed on the vehicle's drive shaft, the vehicle's own ABS wheel speed sensor is used, the front wheel gyroscope is installed on the front suspension system, the inertial sensor is fixed to the vehicle body, and the satellite antenna is installed on the top of the vehicle.

[0022] The system collects sensor data through various communications. The collected sensor data include: satellite observation data, front wheel inertial navigation heading, odometer pulse, three-axis accelerometer, three-axis gyroscope data and vehicle four-wheel speed.

[0023] In the system, the strapdown inertial navigation method is used to perform inertial navigation solution based on the original measurement values ​​of the gyroscope and accelerometer collected to obtain the carrier attitude, speed and position information. The speed, position and attitude errors are obtained according to the solution formula.

[0024] Based on the ephemeris data and observation data collected by the satellite receiver, a satellite navigation error model is established by considering error forms such as satellite ephemeris error, atmospheric delay error, and receiver signal tracking error.

[0025] The front wheel is a steering wheel. The heading angle measured by the front wheel gyroscope is the front wheel rotation angle. The front wheel direction forms an angle with the vehicle body direction. Figure 3 The relationship is used to establish the vehicle motion estimation equation. The front wheel rotation angle is generally greater than the vehicle heading angle, and this angle is set as the sub-filter threshold. The vehicle speed errors at all levels are corrected by designing the sliding surface and the sliding film control rate.

[0026] The system establishes a measurement model through the speed, position, and attitude error data of the strapdown inertial navigation method.

[0027] A multi-stage annealing algorithm is established to screen satellite observation data, and the satellite geometric distribution is used as the objective function to select the appropriate satellite combination, reduce the complexity of the combined positioning algorithm, and optimize the positioning accuracy.

[0028] According to the above state equation and measurement equation, the vehicle posture is calculated, and the vehicle posture and speed information are used to perform navigation solution through EKF.

[0029] The implementation method of the SINS / ODO / GNSS annealing satellite selection tight combination vehicle-mounted system based on velocity correction is as follows:

[0030] Step 1: Collect real-time data, including three-axis gyroscope data, three-axis accelerometer data, four-wheel ABS wheel speed, steering system heading angle, GNSS navigation receiver observation data and ephemeris data.

[0031] Step 2: Obtain the three-axis velocity, position, and attitude errors based on the strapdown inertial navigation system solution formula.

[0032] The attitude errors in the east, north and sky directions are , then the attitude error equation is expressed as:

[0033] (1)

[0034] in is the northeast sky velocity error, then the velocity error equation is:

[0035] (2)

[0036] in is the angular velocity of the Earth's rotation, represents the radius of the equatorial plane of the earth ellipsoid; are latitude, longitude and altitude respectively; Indicates the three-axis error of the gyroscope; represents the three-axis accelerometer error. Then the position error equation is:

[0037] (3)

[0038] The gyroscope error equation is

[0039] (4)

[0040] in represents a random constant, is a first-order Markov process, is white noise, Indicates the relevant time.

[0041] Assuming that the three-axis errors are the same, the acceleration error model is expressed as:

[0042] (5)

[0043] Step 3: Establish a satellite navigation error model based on satellite ephemeris error, atmospheric delay error, receiver signal tracking error, noise and other error forms;

[0044] The error model of GNSS receiver clock error and clock drift is expressed as;

[0045] (6)

[0046] in , are the receiver clock error and clock drift error.

[0047] Step 4: Establish the vehicle motion estimation equation by analyzing the vehicle's four-wheel ABS motion equation.

[0048] Step 4.1: The trajectory of the vehicle when turning can be regarded as an arc with O as the center and radius R. Suppose the change in the vehicle's heading angle is The vehicle travels a distance of The wheel speed meter scale factor is corrected to make the vehicle travel at a constant speed on a flat and straight road, and the wheel speed scale factor is corrected by the speed obtained by the GNSS receiver.

[0049] After correcting the proportional factor, the rear wheel speed is converted into the driving distance, and the distances of the left and right rear wheels are expressed as:

[0050] (7)

[0051] (8)

[0052] in The left and right wheelbase of the vehicle.

[0053] According to the vehicle front wheel rotation angle collected in step 1 Available

[0054] (9)

[0055] in The front and rear wheelbase of the vehicle.

[0056] according to Figure 3 The geometric relationship shown in the figure, the distances between the left and right front wheels are expressed as:

[0057] (10)

[0058] (11)

[0059] Step 4.2: The front wheel turning angle is generally greater than the vehicle heading angle, and this angle is set as the sub-filter threshold. The vehicle speed errors at all levels are corrected by designing the sliding surface and the sliding film control rate.

[0060] First, establish the synovial observer system model, set the system variables , then the state space model of the speed observation is expressed as:

[0061] (12)

[0062] A is the system state matrix, and D is the disturbance caused by tire slip or idling. The observation model of the speed observation system can be expressed as:

[0063] (13)

[0064] C is the observation transfer matrix, which is calculated using the observation formula in step 4.1.

[0065] Design the sliding surface as an observation error function

[0066] (14)

[0067] The sliding surface of the system is ensured to approach zero by switching the sliding film control rate, thus achieving accurate estimation of the state.

[0068] (15)

[0069] Where K is the control gain and sign is the sign function.

[0070] Step 5: Establish the state transfer matrix through the error model system described in steps 3 and 4. Assume that the state quantity is The first three state quantities are three-axis attitude errors, followed by northeast sky velocity error and longitude, latitude and altitude errors, and finally gyroscope three-axis drift, acceleration three-axis zero bias, receiver clock error and clock drift error.

[0071] According to formula 2 in step 2, formulas 3 and 4 in step 3.

[0072] The error state equation is expressed as

[0073] (16)

[0074] in are attitude error, three-axis velocity error and position error, It is a 9×9 strapdown inertial navigation system error matrix. is the remaining state quantity, The specific form is as follows:

[0075] (17)

[0076] Assuming that all errors are random white noise, then

[0077] (18)

[0078] is the gyroscope random white noise, is the random white noise of the accelerometer, is the random white noise of GNSS clock error and clock drift.

[0079] Step 6: Establish a measurement model using the observation data obtained in steps 1 and 5. The measurement values ​​are set as pseudorange, pseudorange rate, and ODO output speed.

[0080] Step 6.1: The satellite signal received by the GNSS receiver is transmitted at the speed of light. The relative distance can be obtained through the GNSS receiver and the satellite launch time. That is, the pseudo-range observation equation can be expressed as:

[0081] (19)

[0082] is the geometric distance from the receiver to the satellite, is the observed pseudorange, is the receiver clock error. Then the measurement equation of the receiver clock error after correction is as follows:

[0083] (20)

[0084] is the position coordinate of satellite n, the actual position of the receiver True position with WGS-84 rectangular coordinate system The conversion relationship can be expressed as:

[0085] (twenty one)

[0086] in, is the radius of curvature of the local Mauci circle, and e is the eccentricity of the earth. Substituting the above formula into the geometric distance formula, we can get:

[0087] (twenty two)

[0088] In the formula, the real position It can be expressed as:

[0089] (twenty three)

[0090] That is the position error state quantity. According to formula 18, the pseudo-range nonlinear measurement equation of the compact combination system can be obtained:

[0091] (twenty four)

[0092] Step 6.1: Derivative the above equation with respect to time, we get

[0093] (25)

[0094] in, is the unknown receiver clock drift, is the clock drift of satellite n. The geometric distance can be determined according to the above formula The relationship between the rate of change and speed.

[0095] (26)

[0096] Combining Equation 24 and Equation 25, we can obtain the line-of-sight unit vector between the receiver and the satellite.

[0097] (27)

[0098] Its geographic coordinate system The relationship is:

[0099] (28)

[0100] in is the transfer matrix from the navigation coordinate system to the Earth-centered Earth-fixed coordinate system; is the speed output by SINS. Substituting equations and into equation can obtain the final SINS / GNSS tightly combined pseudorange rate measurement equation.

[0101] (29)

[0102] are clock drift and clock drift noise.

[0103] Step 7: Use the satellite geometric dilution of precision factor GDOP as the objective function, expressed as

[0104] (30)

[0105] where H is the satellite geometry matrix.

[0106] An annealing control formula is established using exponential cooling to achieve the transition from global search to local search. The formula is as follows:

[0107] (31)

[0108] in, is the current temperature, Set to 0.9.

[0109] Introduce 6 random satellites, calculate their GDOP values, and set them as the initial value of the objective function. When the temperature is greater than the threshold, use the method of generating a fast domain solution. At this time, the domain solution is generated by replacing three satellites. When the annealing temperature is less than the threshold, it can be considered that the current combination is gradually becoming stable. By replacing a single satellite, the precision domain solution is generated and the GDOP value is calculated.

[0110] Based on GDOP and the current temperature, the Metropolis criterion is used to determine whether the current solution is accepted. The acceptance probability is:

[0111] (32)

[0112] is the current optimal solution, is the GDOP value of the current latest solution.

[0113] Repeat the steps of generating domain solutions and judging whether to accept them until the temperature drops to the minimum value, and the optimal solution is considered to have been found.

[0114] Since the satellite motion changes less than that of the ground carrier, after locking the optimal solution, the optimal combination can maintain high-precision positioning for a certain period of time. Therefore, after setting it for 1 hour, the temperature is increased again, and the iteration is repeated by generating a precision domain solution to avoid a decrease in positioning accuracy due to satellite movement or environmental changes.

[0115] Step 8: Use SINS / GNSS and ODO data to perform navigation solution through EKF. The solution process is as follows: Figure 5 As shown in the figure, when the heading is less than the threshold, the satellite pseudo-range rate is used as the velocity error observation value. When the heading is greater than a certain level or the quality of satellite selection is too low, the ODO velocity is used as the measurement to participate in the operation. The linearization of the state space and measurement equations can be completed by performing Taylor series expansion of the nonlinear system state space equation and ignoring high-order small terms, and filtering can be performed.

[0116] Step 8.1 Time Update

[0117] (33)

[0118] (34)

[0119] Step 8.2 Status Update

[0120] (35)

[0121] (36)

[0122] (37)

[0123] (38)

[0124] Step 9: After outputting the final estimated value, substitute the optimal estimated value into the strapdown inertial navigation system to calculate the specific posture and position of the vehicle, and achieve high-precision positioning of the vehicle in complex environments.

[0125] like Figure 1 As shown in the figure, the fast initial alignment system for train navigation based on dual antenna assistance studied in this invention combines different sensor data to propose a tight combination positioning algorithm for dual observation system. The input information includes satellite observation data, front wheel inertial navigation heading, odometer pulse, three-axis accelerometer, three-axis gyroscope data and vehicle four-wheel speed. Based on the EKF algorithm, the system combines INS sensor, GNSS observation value and ODO speed after sliding film filtering for positioning, and outputs the optimal estimate of position and speed.

[0126] The present invention studies a dual-antenna-assisted train navigation rapid initial alignment system, in which the GNSS receiver, SINS inertial measurement unit, and ODO module work simultaneously, and the sensor installation positions are as follows: Figure 2 As shown, the odometer is installed on the vehicle's drive shaft, the vehicle's own ABS wheel speed sensor is used, the front wheel gyroscope is installed on the front suspension system, the inertial sensor is fixed to the vehicle body, and the satellite antenna is installed on the top of the vehicle.

[0127] like Figure 3 As shown, the ODO system based on the sliding film filter observer studied in the present invention intervenes when the turning radius of the carrier is large. When turning, the driving trajectory of the vehicle can be regarded as an arc. By detecting the four-wheel ABS wheel speed sensors, the relationship between each tire and the vehicle can be calculated. At the same time, due to the relationship between the left and right wheel speeds at the time of turning, the vehicle heading can be roughly calculated.

[0128] like Figure 4 As shown, the fusion process of the EKF used in the present invention is as follows: after initialization, after the time update stage, it is determined whether the satellite meets the minimum number of positioning in the satellite selection stage, or whether the heading angle increment is large, that is, it is considered that the satellite speed is no longer reliable at this time, and the observation information obtained by ODO is used to participate in the measurement update, so as to obtain the optimal estimate and output it.

[0129] like Figure 5As shown, the present invention uses four-wheel ABS wheel speed sensors and wheel headings to calculate the carrier speed, thereby obtaining the carrier east and north speed errors. The algorithm first collects corresponding sensor data, obtains an estimated value through a system state model, introduces the observed data into a sliding film observer to adjust the error, corrects the error through state feedback, and outputs a converged state estimate.

[0130] like Figure 6 As shown in the figure, the multi-stage annealing satellite selection algorithm proposed in the present invention first randomly obtains a satellite combination, uses the GDOP value as the objective function to find the initial value, and when the temperature is high, uses a fast domain solution method to add a large amplitude disturbance to determine whether the new solution after the disturbance is accepted. When the new solution is accepted, the annealing temperature is lowered and the process is repeated. When the temperature drops below the threshold, a low disturbance is used to generate an accurate domain solution to ensure the stability of the existing satellite combination.

[0131] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0132] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A vehicle-mounted positioning method based on velocity correction using SINS / ODO / GNSS annealing and satellite selection is characterized by: The following steps are involved: Step 1: Collect real-time data, including three-axis gyroscope data, three-axis accelerometer data, four-wheel ABS wheel speed, steering system heading angle, GNSS navigation receiver observation data and ephemeris data; Step 2: Obtain the three-axis speed, position, and attitude errors according to the strapdown inertial navigation system solution formula; Step 3: Establish a satellite navigation error model based on satellite ephemeris error, atmospheric delay error, receiver signal tracking error, noise and other error forms; Step 4: Establish the vehicle motion estimation equation by analyzing the vehicle's four-wheel ABS motion equation; Step 4.1: The trajectory of the vehicle when turning can be regarded as an arc with O as the center and radius R. Suppose the change in the vehicle's heading angle is The vehicle travels a distance of ; Correct the wheel speed meter scale factor to make the vehicle travel at a constant speed on a flat and straight road. Correct the wheel speed scale factor using the speed obtained by the GNSS receiver. Step 4.2: The front wheel turning angle is generally greater than the vehicle heading angle. This angle is set as the sub-filter threshold. The vehicle speed errors at all levels are corrected by designing the sliding surface and the sliding film control rate. Step 5: Establish a state transfer matrix through the error model system described in steps 3 and 4; let the state quantity be The first three state quantities are the three-axis attitude error, followed by the northeast sky velocity error and the longitude, latitude and altitude errors, and finally the gyroscope three-axis drift, acceleration three-axis zero bias, receiver clock error and clock drift error; Step 6: Establish a measurement model through the observation data obtained in steps 1 and 5, and set the measurement values ​​as pseudorange, pseudorange rate and ODO output speed; Step 7: Use the satellite geometric dilution of precision GDOP as the objective function. An exponential cooling method is used to establish an annealing control formula to achieve the transition from global search to local search. Introduce 6 random satellites, calculate their GDOP values, and set them as the initial value of the objective function. When the temperature is greater than the threshold, use the method of generating a fast domain solution. At this time, the domain solution is generated by replacing three satellites. When the annealing temperature is less than the threshold, it can be considered that the current combination is gradually becoming stable. Generate a precision domain solution by replacing a single satellite and calculate the GDOP value. Based on GDOP and current temperature, the Metropolis criterion is used to decide whether the current solution is accepted. Repeat the steps of generating domain solutions and judging whether to accept them until the temperature drops to the minimum value, and the optimal solution is considered to have been found; After locking the optimal solution, the optimal combination can maintain high-precision positioning for a certain period of time, and then the temperature can be raised again, and the iteration can be repeated by generating a precision domain solution to avoid the decrease in positioning accuracy due to satellite movement or environmental changes; Step 8: Use SINS / GNSS and ODO data to perform navigation solution through EKF. When the heading is less than the threshold, use the satellite pseudo-range rate as the velocity error observation value. When the heading is greater than a certain level or the quality of satellite selection is too low, use the ODO velocity as the measurement to participate in the calculation. Perform Taylor series expansion on the state space equation of the nonlinear system and ignore the high-order small terms to complete the linearization of the state space and measurement equations. Step 9: After outputting the final estimated value, substitute the optimal estimated value into the strapdown inertial navigation system to calculate the specific posture and position of the vehicle, and achieve high-precision positioning of the vehicle in complex environments.

2. The SINS / ODO / GNSS annealing star selection tight combination vehicle-mounted positioning method based on velocity correction according to claim 1 is characterized in that: The system for executing the method of claim 1 comprises a multi-satellite multi-frequency external measurement antenna module, a GNSS receiver module, a SSINS inertial navigation module, and an ODO speed detection module based on an ABS wheel speed sensor.

Images