Trajectory prediction method and system during GPS failure and vehicle

Abstract

The invention provides a trajectory prediction method during GPS failure, and the method specifically comprises the following steps: entering a trajectory prediction mode when a current GPS signal is invalid, exiting the trajectory prediction mode when the current GPS signal is valid, and predicting the position of a vehicle at the next moment based on the current course angle and running speed of the vehicle in the trajectory prediction mode, and the position at the next moment is used as the current position, and the vehicle position at the next moment is estimated. When the fact that the position information collected by the GPS navigation system is invalid is detected, the MPU enters a track estimation mode, the driving distance is determined based on the driving speed of the vehicle, then the driving distance is converted into the longitude and latitude of the position of the vehicle based on the course angle, and on the premise that no hardware equipment is additionally arranged, prediction of the track of the vehicle under the condition that the GPS signal is lost is achieved. The method is especially suitable for the situation that the GPS signal temporarily loses efficacy when the vehicle passes through the tunnel.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory prediction and provides a trajectory prediction method, system and vehicle when GPS fails. Background Art

[0002] GPS (Global Positioning System) is a satellite positioning system that determines the location of a device by receiving satellite signals and is widely used in vehicle navigation.

[0003] In special circumstances, such as when a vehicle passes through tall buildings, tunnels, or areas covered by dense trees, the GPS signal may be interfered with, resulting in the loss of positioning signals. At this time, the vehicle may deviate from its course due to positioning failure.

[0004] Therefore, how to realize vehicle positioning when GPS signal is lost and provide continuous vehicle position information to the navigation system is a technical problem that needs to be solved urgently. Summary of the Invention

[0005] In view of this, the present application provides a trajectory prediction method when GPS fails, aiming to improve the above-mentioned problem.

[0006] Specifically, the following technical solutions are included:

[0007] On the one hand, an embodiment of the present application provides a method for trajectory prediction when GPS fails, and the method is specifically as follows:

[0008] When the current GPS signal is invalid, the system enters the trajectory prediction mode. When the current GPS signal is valid, the system exits the trajectory prediction mode. In the trajectory prediction mode, the system estimates the vehicle's position at the next moment based on the vehicle's current heading angle and driving speed, and uses the position at the next moment as the current position to estimate the vehicle's position at the next moment.

[0009] In some embodiments of the present invention, the method for estimating the position of a vehicle in trajectory estimation mode is as follows:

[0010] Calculate the vehicle's position P t-2 To position P t-1 The heading angle h;

[0011] Read the vehicle's current speed v t , calculate the vehicle's travel distance D based on the sampling time T, and estimate the vehicle's current position P by the vehicle's travel distance D at the current heading angle h t .

[0012] In some embodiments of the present invention, from position P t-2 To position P t-1The calculation method of the heading angle h is as follows:

[0013] (21) will be from position P t-2 To position P t-1 The driving section is projected into the computer coordinate system as follows:

[0014] y=sin(ΔLon)*cos(Lat t-1 *π / 180);

[0015] x=cos(Lat t-2 *π / 2)*sin(Lat t-1 *π / 2)-sin(Lat t-2 *π / 2)*cos(Lat t-1 *π / 2)*cos(ΔLon);

[0016] (22) Calculate position P t-2 To position P t-1 The heading angle h is calculated as follows:

[0017] h=atan2(y,x)*180 / π;

[0018] Among them, atan2(y,x) represents the arc tangent of the point (y,x) in the computer coordinate system, ΔLon=(Lon t-1 -Lon t-2 )*π / 180,(Lat t-1 ,Lon t-1 ) represents the position P t-1 Longitude and latitude, (Lat t-2 ,Lon t-2 ) represents the position P t-2 longitude and latitude.

[0019] In some embodiments of the present invention, the vehicle's current position P t The calculation formula is as follows:

[0020] Position P t Longitude Lat t The calculation formula is: Lat t =Lat t-1 +(D / R e )*cos(h*π / 180);

[0021] Position P t Dimension Lon t The calculation formula is: Lon t =Lon t-1 +(D / R e )*sin(h*π / 180).

[0022] Among them, (Lat t-1 ,Lon t-1 ) represents the vehicle position P at the previous moment t-1 Longitude and latitude, (Lat t ,Lon t ) represents the current vehicle position P t Longitude and latitude, R e Represents the radius of the Earth.

[0023] In some embodiments of the present invention, when it is detected that the vehicle's current speed signal is lost, based on the position P t-2 To position P t-1 The distance D1 is used to calculate the current speed of the vehicle.

[0024] In some embodiments of the present invention, the vehicle is moving from position P t-2 To position P t-1 The calculation method of the driving distance D1 is as follows:

[0025] (11) Calculate the vehicle position P t-2 To position P t-1 The longitude difference ΔLat and latitude difference ΔLon;

[0026] (12) Calculate the position P by the longitude difference ΔLat and the latitude difference ΔLon t-2 To position P t-1 The corresponding radian change is calculated from the position P t-2 To position P t-1 The driving distance D1.

[0027] In some embodiments of the present invention, the calculation formula of the travel distance D1 is as follows:

[0028] D1=R e *Δc;

[0029] Δc=2*atan2(a 1 / 2 ,(1-a) 1 / 2 ),

[0030] Among them, R e is the radius of the Earth, Δc is the position P t-2 To position P t-1 The corresponding radian change, a is the intermediate parameter, a=(sin(ΔLat / 2)) 2 +cos(Lat t-2 *π / 180)*cos(Lat t-1 *π / 180)*(sin(ΔLon / 2)) 2 .

[0031] On the other hand, an embodiment of the present application provides a trajectory prediction system when GPS fails, the system comprising:

[0032] Vehicle speed sensor, MCU connected to the vehicle speed sensor; GPS navigation system, MPU connected to the GPS navigation system, the MPU and MCU are connected via a t-box;

[0033] The vehicle speed sensor collects the vehicle's current speed and sends it to the MCU; the GPS navigation system periodically collects the vehicle's position signal and sends it to the MPU. When the MPU detects that the position information collected by the GPS navigation system is invalid, it predicts the vehicle's trajectory based on the trajectory prediction method when GPS fails.

[0034] On the other hand, an embodiment of the present application provides a vehicle, on which the above-mentioned trajectory prediction system for when GPS fails is integrated.

[0035] The present invention determines the travel distance based on the vehicle's speed, and then converts the travel distance into the longitude and latitude of the vehicle's position based on the heading angle. Without adding additional hardware equipment, the present invention can predict the vehicle trajectory when the GPS signal is lost. It is particularly suitable for the situation where the GPS signal temporarily fails when the vehicle passes through a tunnel. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0037] Figure 1 A flowchart of a method for trajectory prediction when GPS fails provided by an embodiment of the present invention;

[0038] Figure 2 A schematic diagram of the structure of a trajectory prediction system when GPS fails provided by an embodiment of the present invention;

[0039] The above drawings illustrate specific embodiments of the present application, which will be described in more detail below. These drawings and the textual description are not intended to limit the scope of the present application in any way, but rather to illustrate the concepts of the present application to those skilled in the art by reference to specific embodiments. DETAILED DESCRIPTION

[0040] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are part of the embodiments of this application, not all of them. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0041] Unless otherwise defined, all technical terms used in the embodiments of the present application have the same meanings as commonly understood by those skilled in the art.

[0042] Figure 1 This is a flow chart of a method for trajectory prediction when GPS fails provided by an embodiment of the present invention. The method is specifically as follows:

[0043] When the current GPS signal is invalid, the system enters the trajectory prediction mode. When the current GPS signal is valid, the system exits the trajectory prediction mode. In the trajectory prediction mode, the system estimates the vehicle's position at the next moment based on the vehicle's current heading angle and driving speed, and uses the position at the next moment as the current position to estimate the vehicle's position at the next moment.

[0044] When the GPS signal is valid, the vehicle position collected by the GPS navigation system is used as the vehicle's current position. When the GPS signal is invalid, the vehicle position estimated in the trajectory estimation mode is used as the vehicle's current position. The vehicle position estimation method in the trajectory estimation mode is as follows:

[0045] Calculate the vehicle's position P t-2 To position P t-1 The driving distance d and heading angle h, read the current speed v of the vehicle t , based on the sampling time T, calculate the distance D of the vehicle at the current heading angle h, and estimate the vehicle's current position P by the distance D at the current heading angle h t .

[0046] In the embodiment of the present invention, from position P t-2 To position P t-1 The calculation method of the heading angle h is as follows:

[0047] (21) will be from position P t-2 To position P t-1 The driving section is projected into the computer coordinate system as follows:

[0048] y=sin(ΔLon)*cos(Lat t-1 *π / 180);

[0049] x=cos(Lat t-2 *π / 2)*sin(Latt-1 *π / 2)-sin(Lat t-2 *π / 2)*cos(Lat t-1 *π / 2)*cos(ΔLon);

[0050] Then position P t-2 To position P t-1 The calculation formula of the heading angle h is as follows:

[0051] h=atan2(y,x)*180 / π;

[0052] Among them, atan2(y,x) represents the arc tangent of the point (y,x) in the computer coordinate system, ΔLon=(Lon t-1 -Lon t-2 )*π / 180,(Lat t-1 ,Lon t-1 ) represents the position P t-1 Longitude and latitude, (Lat t-2 ,Lon t-2 ) represents the position P t-2 longitude and latitude.

[0053] In this embodiment of the present invention, the vehicle's current position P t The estimation process is as follows:

[0054] Position P t Longitude Lat t The calculation formula is: Lat t =Lat t-1 +(D / R e )*cos(h*π / 180);

[0055] Position P t Dimension Lon t The calculation formula is: Lon t =Lon t-1 +(D / R e )*sin(h*π / 180).

[0056] In the embodiment of the present invention, when the current speed of the vehicle is lost, the position P is calculated. t-2 To position P t-1 The current speed of the vehicle is calculated by the distance D1. The current speed of the vehicle is the ratio of the distance D1 to the time T. The position P t-2 With position P t-1 The sampling time difference between the two is the sampling time T.

[0057] In the embodiment of the present invention, the vehicle starts from position P t-2 To position Pt-1 The calculation method of the driving distance D1 is as follows:

[0058] (11) Calculate the vehicle position P t-2 To position P t-1 The longitude difference ΔLat and latitude difference ΔLon are calculated as follows:

[0059] ΔLat=(Lat t-1 -Lat t-2 )*π / 180;

[0060] )Lon=(Lon t-1 -Lon t-2 )*π / 180;

[0061] Among them, (Lat t-1 ,Lon t-1 ) represents the position P t-1 Longitude and latitude, (Lat t-2 ,Lon t-2 ) represents the position P t-2 longitude and latitude.

[0062] (12) Calculate the position P by the longitude difference ΔLat and the latitude difference ΔLon t-2 To position P t-1 The corresponding radian change is then calculated from position P t-2 To position P t-1 The driving distance D.

[0063] Let a = (sin(ΔLat / 2)) 2 +cos(Lat t-2 *π / 180)*cos(Lat t-1 *π / 180)*(sin(ΔLon / 2)) 2 ;

[0064] Radian change Δc=2*atan2(a 1 / 2 ,(1-a) 1 / 2 ), where atan2(a 1 / 2 ,(1-a) 1 / 2 ) represents the midpoint of the computer coordinate system (a 1 / 2 ,(1-a) 1 / 2 )’s inverse tangent;

[0065] Then D1=R e *Δc, where R e is the radius of the Earth.

[0066] The present invention determines the travel distance based on the vehicle's speed, and then converts the travel distance into the longitude and latitude of the vehicle's position based on the heading angle. Without adding additional hardware equipment, the present invention can predict the vehicle trajectory when the GPS signal is lost. It is particularly suitable for the situation where the GPS signal temporarily fails when the vehicle passes through a tunnel.

[0067] Figure 2 This is a schematic diagram of the structure of a trajectory prediction system for GPS failure provided by an embodiment of the present invention. For ease of explanation, only the parts related to the embodiment of the present invention are shown. The system includes:

[0068] Vehicle speed sensor, MCU connected to the vehicle speed sensor; GPS navigation system, MPU connected to the GPS navigation system, the MPU and MCU are connected via a t-box;

[0069] The vehicle speed sensor collects the current speed of the vehicle and sends it to the MCU; the GPS navigation system periodically collects the vehicle's position signal, and the sampling period is time T, and sends it to the MPU. When the MPU detects that the position information collected by the GPS navigation system is invalid, it predicts the vehicle's trajectory based on the trajectory prediction method when GPS fails.

[0070] When it detects that the location information collected by the GPS navigation system is invalid, the MPU enters the trajectory estimation mode, determines the travel distance based on the vehicle's speed, and then converts the travel distance into the latitude and longitude of the vehicle's position based on the heading angle. Without adding additional hardware equipment, it can predict the vehicle trajectory when the GPS signal is lost. It is especially suitable for situations where the GPS signal temporarily fails when the vehicle passes through a tunnel.

[0071] The present invention also provides a vehicle that is integrated with the above-mentioned trajectory prediction system in the event of GPS failure. When the vehicle detects that the location information collected by the GPS navigation system is invalid, the MPU enters a trajectory prediction mode, determines the travel distance based on the vehicle's speed, and then converts the travel distance into the longitude and latitude of the vehicle's position based on the heading angle. Without adding additional hardware equipment, the vehicle trajectory can be predicted in the event of GPS signal loss. This is particularly suitable for situations where the GPS signal temporarily fails when the vehicle passes through a tunnel.

[0072] Those skilled in the art will readily appreciate other embodiments of the present invention after considering the specification and practicing the present invention disclosed herein. This application is intended to cover any variations, uses, or adaptations of the present invention that follow the general principles of the present invention and include common knowledge or customary techniques in the art not disclosed herein. The description and examples are to be considered as exemplary only.

[0073] It should be understood that the present application is not limited to the exact structures described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present application is limited only by the appended claims.

Claims

1. A trajectory prediction method when GPS fails, characterized in that: The method is specifically as follows: When the current GPS signal is invalid, the system enters the trajectory prediction mode. When the current GPS signal is valid, the system exits the trajectory prediction mode. In the trajectory prediction mode, the system estimates the vehicle's position at the next moment based on the vehicle's current heading angle and driving speed, and uses the position at the next moment as the current position to estimate the vehicle's position at the next moment.

2. The trajectory prediction method when GPS fails according to claim 1, characterized in that: The vehicle position estimation method in trajectory estimation mode is as follows: Calculate the vehicle's position P t-2 To position P t-1 The heading angle h; Read the vehicle's current speed v t , calculate the vehicle's travel distance D based on the sampling time T, and estimate the vehicle's current position P by the vehicle's travel distance D at the current heading angle h t .

3. The trajectory prediction method when GPS fails as claimed in claim 2, characterized in that: From position P t-2 To position P t-1 The calculation method of the heading angle h is as follows: (21) will be from position P t-2 To position P t-1 The driving section is projected into the computer coordinate system as follows: y=sin(ΔLon)*cos(Lat t-1 *π / 180); x=cos(Lat t-2 *π / 2*sin(Lat t-1 *π / 2)-sin(Lat t-2 *π / 2*cos(Lat t-1 *π / 2)*cos(ΔLon); (22) Calculate position P t-2 To position P t-1 The heading angle h is calculated as follows: h=atan2(y,x)*180 / π; Among them, atan2(y,x) represents the arc tangent of the point (y,x) in the computer coordinate system, ΔLon=(Lon t-1 -Lon t-2 )*π / 180,(Lat t-1 ,Lon t-1 ) represents the position P t-1 Longitude and latitude, (Lat t-2 ,Lon t-2 ) represents the position P t-2 longitude and latitude.

4. The trajectory prediction method when GPS fails as claimed in claim 2, characterized in that: The vehicle's current position P t The calculation formula is as follows: Years t =Years t-1 +(D / R e )*cos(h*π / 180); Lon t =Lon t-1 +(D / R e )*sin(h*π / 180); Among them, (Lat t-1 ,Lon t-1 ) represents the vehicle position P at the last moment t-1 Longitude and latitude, (Lat t ,Lon t ) represents the current vehicle position P t Longitude and latitude, R e Represents the radius of the Earth.

5. The trajectory prediction method when GPS fails as claimed in claim 1, characterized in that: When the vehicle's current speed signal is detected to be lost, based on the position P t-2 To position P t-1 The distance D1 is used to calculate the current speed of the vehicle.

6. The trajectory prediction method when GPS fails as claimed in claim 5, characterized in that: The vehicle starts from position P t-2 To position P t-1 The calculation method of the driving distance D1 is as follows: (11) Calculate the vehicle position P t-2 To position P t-1 The longitude difference ΔLat and latitude difference ΔLon; (12) Calculate the position P by the longitude difference ΔLat and the latitude difference ΔLon t-2 To position P t-1 The corresponding radian change is calculated from the position P t-2 To position P t-1 The driving distance D1.

7. The trajectory prediction method when GPS fails as claimed in claim 6, characterized in that: The calculation formula for the driving distance D1 is as follows: D1=R e *Δc; Δc=2*atan2(a 1 / 2 ,(1-a) 1 / 2 ); Among them, R e is the radius of the Earth, Δc is the position P t-2 To position P t-1 The corresponding radian change, a is the intermediate parameter, a=(sin(ΔLat / 2)) 2 +cos(Lat t-2 *π / 180)*cos(Lat t-1 *π / 180)*(sin(ΔLon / 2)) 2 .

8. A trajectory prediction system when GPS fails, characterized in that: The system comprises: Vehicle speed sensor, MCU connected to the vehicle speed sensor; GPS navigation system, MPU connected to the GPS navigation system, the MPU and MCU are connected via a t-box; The vehicle speed sensor collects the current speed of the vehicle and sends it to the MCU; the GPS navigation system periodically collects the vehicle's position signal and sends it to the MPU. When the MPU detects that the position information collected by the GPS navigation system is invalid, it predicts the vehicle's trajectory based on the trajectory prediction method when GPS fails as described in any one of claims 1 to 7.

9. A vehicle, characterized in that the trajectory prediction system for GPS failure according to claim 8 is integrated into the vehicle.

Images