An unmanned vehicle tracking method and terminal based on the fusion of UWB and GPS

Abstract

The present invention provides an unmanned vehicle tracking method and terminal based on the fusion of UWB and GPS, including: S1, obtaining real-time positioning information of the UWB tag according to multiple UWB base stations, and the UWB tag is located on the unmanned vehicle; S2, obtaining the GPS positioning information of the unmanned vehicle, obtaining the first driving data of the unmanned vehicle according to the GPS positioning information, and obtaining the second driving data of the unmanned vehicle according to the real-time positioning information, and judging whether the first driving data is consistent with the second driving data, if not, recording the second driving data; S3, obtaining the turning information of the unmanned vehicle according to the target unmanned vehicle path trajectory and the second driving data, and repeatedly executing S1-S3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory; avoiding the single positioning module from being unable to provide accurate positioning information, and improving the accuracy of unmanned tracking driving.

Description

Technical Field

[0001] The present invention relates to the field of unmanned driving technology, and in particular to an unmanned vehicle tracking method and terminal based on the fusion of UWB and GPS. Background Art

[0002] Driverless cars are the main development trend in the automotive field in the future, and car tracking driving is a way for driverless cars to drive automatically. Among them, tracking driving refers to the automatic tracking driving of the car according to a pre-set route. At present, the positioning and navigation technologies used in driverless tracking driving are mostly GPS (Global Positioning System), Beidou, etc., but when these signals are blocked, or when the signal is weak or interfered, accurate GPS positioning and navigation cannot be provided by a single GPS module, resulting in a small area where the driverless tracking driving function is applicable, and the application scenarios of the driverless tracking driving function are relatively limited.

[0003] In addition, the positioning accuracy of GPS is usually around 3 meters, while the ranging accuracy of UWB (Ultra Wideband) positioning technology is 10 centimeters. Compared with the positioning accuracy of the GPS module, UWB has higher ranging accuracy, and the UWB tag module is economical and affordable, and the cost of positioning chips is much lower than that of GPS or Beidou. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide an unmanned vehicle tracking method and terminal based on the fusion of UWB and GPS, so as to avoid the inability of a single positioning module to provide accurate positioning information and improve the accuracy of unmanned tracking driving.

[0005] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0006] An unmanned vehicle tracking method based on the fusion of UWB and GPS, comprising:

[0007] S1. Obtaining real-time positioning information of the UWB tag according to multiple UWB base stations, where the UWB tag is located on the unmanned vehicle;

[0008] S2, obtaining GPS positioning information of the unmanned vehicle, obtaining first driving data of the unmanned vehicle according to the GPS positioning information, and obtaining second driving data of the unmanned vehicle according to the real-time positioning information, and determining whether the first driving data is consistent with the second driving data, and if not, recording the second driving data;

[0009] S3. Obtain the steering information of the unmanned vehicle according to the target unmanned vehicle path trajectory and the second driving data, and repeat S1-S3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory.

[0010] In order to solve the above technical problems, another technical solution adopted by the present invention is:

[0011] An unmanned vehicle tracking terminal based on the fusion of UWB and GPS includes a memory, a processor, and a computer program stored in the memory and running on the processor. When the processor executes the computer program, each step of the unmanned vehicle tracking method based on the fusion of UWB and GPS is implemented.

[0012] The beneficial effects of the present invention are: real-time positioning of unmanned vehicles is performed simultaneously through UWB (Ultra Wideband) positioning technology and GPS positioning technology, and the accuracy of GPS positioning technology is verified based on UWB positioning technology, so that unmanned vehicles can complete preset unmanned vehicle driving and tracking tasks when GPS and other satellite signals are weak or lost, or even without using GPS satellite signals, thereby avoiding the inability of a single positioning module to provide accurate positioning information, thereby improving the general practicality of unmanned tracking driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] Figure 1 A flowchart of a method for tracking an unmanned vehicle based on the fusion of UWB and GPS provided in an embodiment of the present invention;

[0014] Figure 2 A spatial coordinate diagram of an unmanned vehicle tracking method based on the fusion of UWB and GPS provided in an embodiment of the present invention;

[0015] Figure 3 A schematic diagram of the structure of an unmanned vehicle tracking terminal based on the fusion of UWB and GPS provided in an embodiment of the present invention;

[0016] Description of labels:

[0017] 1. An unmanned vehicle tracking terminal based on the fusion of UWB and GPS; 2. Memory; 3. Processor. DETAILED DESCRIPTION

[0018] In order to explain the technical content, achieved objectives and effects of the present invention in detail, the following is an explanation in combination with the implementation modes and the accompanying drawings.

[0019] Please refer to Figure 1 , an unmanned vehicle tracking method based on the fusion of UWB and GPS is provided in an embodiment of the present invention, comprising:

[0020] S1. Obtaining real-time positioning information of the UWB tag according to multiple UWB base stations, where the UWB tag is located on the unmanned vehicle;

[0021] S2, obtaining GPS positioning information of the unmanned vehicle, obtaining first driving data of the unmanned vehicle according to the GPS positioning information, and obtaining second driving data of the unmanned vehicle according to the real-time positioning information, and determining whether the first driving data is consistent with the second driving data, and if not, recording the second driving data;

[0022] S3. Obtain the steering information of the unmanned vehicle according to the target unmanned vehicle path trajectory and the second driving data, and repeat S1-S3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory.

[0023] From the above description, it can be seen that the beneficial effects of the present invention are: real-time positioning of the unmanned vehicle is performed simultaneously through UWB (Ultra Wideband) positioning technology and GPS positioning technology, and the accuracy of GPS positioning technology is verified based on UWB positioning technology, so that the unmanned vehicle can complete the preset unmanned vehicle driving tracking task when the GPS and other satellite signals are weak or lost, or even without using GPS satellite signals, thereby avoiding the inability of a single positioning module to provide accurate positioning information, thereby improving the general practicality of unmanned tracking driving.

[0024] Furthermore, the number of the UWB base stations is at least three; the real-time positioning information includes instant latitude and longitude information;

[0025] The S1 is specifically:

[0026] Determine whether the UWB tag receives ranging signals from the multiple UWB base stations at the same time, and if so, obtain the positioning distance between the UWB tag and each UWB base station according to the ranging signals;

[0027] The real-time longitude and latitude information of the UWB tag is obtained based on the positioning distance.

[0028] From the above description, we can know that based on the ultra-wideband positioning technology, the distance between the moving platforms to be tested is measured, and based on the distance data and site location information, the location estimation of the tag node is finally obtained through the three-sided positioning algorithm; because the satellite signal will be severely interfered indoors, which makes GPS or Beidou unable to send positioning information, the unmanned vehicle uses the ultra-wideband module for ranging in this case, which can obtain centimeter-level precision distance data, avoid the single positioning module from being unable to provide accurate positioning information, and improve the accuracy of positioning information. In addition, the ultra-wideband tag module is much cheaper than the satellite positioning chip, which reduces the cost of the unmanned vehicle.

[0029] Further, the first driving data includes a first moving distance and a first heading angle, and the second driving distance data includes a second moving distance and a second heading angle;

[0030] The S2 is specifically:

[0031] Obtaining GPS positioning information of the unmanned vehicle, wherein the GPS positioning information includes GPS positioning information at a previous moment and a current moment;

[0032] Obtaining a first moving distance and a first heading angle of the unmanned vehicle according to the GPS positioning information at the previous moment and the current moment;

[0033] At the same time, real-time positioning information of the unmanned vehicle is obtained, wherein the real-time positioning information includes real-time positioning information at a previous moment and a current moment;

[0034] Obtaining a second moving distance and a second heading angle of the unmanned vehicle according to the real-time positioning information at the previous moment and the current moment;

[0035] Determine whether the first moving distance and the first heading angle are consistent with the second moving distance and the second heading angle; if not, record the second moving distance and the second heading angle.

[0036] From the above description, it can be seen that ultra-wideband positioning and satellite positioning simultaneously obtain the real-time positioning information of the unmanned vehicle, and the accuracy of satellite positioning is verified based on ultra-wideband positioning, so as to avoid the unmanned vehicle being unable to accurately position when the satellite positioning is interfered with, causing the unmanned vehicle's tracking deviation, thereby improving the accuracy of the unmanned vehicle's tracking.

[0037] Furthermore, the S3 is specifically:

[0038] Acquire a point set of the target unmanned vehicle path trajectory, and determine a target point in the point set according to the real-time positioning information at the current moment and the second heading angle;

[0039] Calculate the target heading angle of the real-time positioning information at the current moment relative to the target point;

[0040] Obtaining steering information of the unmanned vehicle according to the target heading angle and the second heading angle;

[0041] Controlling the unmanned vehicle to turn according to the turning information;

[0042] Repeat S1-S3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory.

[0043] From the above description, it can be seen that the real-time positioning information obtained by combining ultra-wideband positioning technology and satellite positioning technology has high accuracy, thereby ensuring the success rate of subsequent unmanned vehicle tracking tasks.

[0044] Furthermore, the target heading angle of the real-time positioning information at the current moment relative to the target point is calculated as follows:

[0045] Target heading angle Ret 目标 =ar cosθ;

[0046] in, The real-time positioning information at the current moment is (lon 0 ,lat 0 ), the target point is (lon 1 ,lat 1 );Wherein, lon represents the longitude coordinate of the UWB tag, lat represents the latitude coordinate of the UWB tag; Dist represents the distance between the real-time positioning information and the target point, and cosθ represents the cosine value of the target heading angle;

[0047] The step of obtaining the steering information of the unmanned vehicle according to the target heading angle and the second heading angle is specifically:

[0048] If the difference between the target heading angle and the second heading angle is greater than zero, the unmanned vehicle performs a left turn;

[0049] If the difference between the target heading angle and the second heading angle is zero, the unmanned vehicle keeps going straight;

[0050] If the difference between the target heading angle and the second heading angle is less than zero, the unmanned vehicle performs a right turn.

[0051] From the above description, it can be seen that every time the unmanned vehicle obtains real-time positioning information, it means that the unmanned vehicle has moved. Therefore, based on the real-time positioning information of the unmanned vehicle, the next target point to be reached is obtained one by one, and the next heading of the unmanned vehicle can be accurately calculated based on the current position of the unmanned vehicle and the target point to be reached. In addition, the real-time positioning information can be obtained through satellite positioning or ultra-wideband positioning, which is highly accurate and improves the success rate of the unmanned vehicle's tracking mission.

[0052] Furthermore, the real-time longitude and latitude information of the UWB tag obtained based on the positioning distance is specifically:

[0053] Obtaining the site longitude and latitude information of each UWB base station, and calculating the earth radius of the location of the corresponding UWB base station according to the site longitude and latitude information;

[0054] Obtaining the three-dimensional space distance between any two UWB base stations according to the earth radius;

[0055] Calculate the two-dimensional space distance and the relative heading angle between the two UWB base stations according to the three-dimensional space distance, and establish a coordinate system based on the UWB base stations;

[0056] Obtaining the instant tag coordinates of the UWB tag according to the two-dimensional space distance and the positioning distance;

[0057] Substitute the real-time tag coordinates and the relative heading angle into a coordinate rotation formula to obtain the real-time longitude and latitude information of the UWB tag.

[0058] From the above description, it can be seen that the three-dimensional data obtained based on the positioning technology is converted into two-dimensional data after corresponding calculation, and the moving path of the unmanned vehicle is converted from three-dimensional to two-dimensional, which reduces the complexity of the tracking algorithm and increases the data processing speed of the tracking algorithm, thereby improving the efficiency of the unmanned vehicle tracking task.

[0059] Furthermore, the earth radius includes the spherical radius corresponding to the UWB base station and the latitude circle radius of the latitude;

[0060] The calculation of the earth radius corresponding to the location of the UWB base station according to the site longitude and latitude information is specifically:

[0061] The radius of the sphere is

[0062] The radius of the latitude circle is

[0063] Among them, E a is the equatorial radius of the Earth, E b is the earth's polar radius, and the latitude and longitude information of the station are respectively (lon Z0 ,lat Z0 ) and (lon Z1 ,lat Z1 );where lon Z is the longitude coordinate of the UWB base station, lat Z is the latitude coordinate of the UWB base station.

[0064] From the above description, it can be seen that by performing three-dimensional space calculations through the latitude and longitude information of the ultra-wideband base station, the distance between the site and the center of the earth and the distance between the site and the polar axis of the earth are obtained, which facilitates the establishment of a coordinate system with the site, thereby converting the three-dimensional space calculation into a two-dimensional space calculation and simplifying the algorithm.

[0065] Further, the three-dimensional space distance is the arc length between any two UWB base stations;

[0066] The method of obtaining the three-dimensional space distance between any two UWB base stations according to the earth radius is specifically as follows:

[0067]

[0068] Among them, E e is the arc length of the circle of the sphere where any two UWB base stations are located, E f is the arc length of the circle of latitude where any two UWB base stations are located.

[0069] From the above description, it can be seen that the site distance obtained through longitude and latitude information is a three-dimensional space distance, and the line between the two points is an arc. Therefore, the three-dimensional space distance between any two sites is calculated by the circle of the spherical radius and the circle of the latitude circle; the relative relationship between the two sites is calculated based on the three-dimensional space distance.

[0070] Further, the two-dimensional space distance and the relative heading angle between the two UWB base stations are calculated according to the three-dimensional space distance as follows:

[0071] The two-dimensional space distance is

[0072] The relative heading angle is

[0073] From the above description, it can be seen that according to the relative relationship between ultra-wideband base stations, a corresponding coordinate system is established to convert three-dimensional spatial data into two-dimensional spatial data, reducing the complexity of the tracking algorithm. At the same time, a coordinate system is established based on the base station, and the positioning data has high accuracy and strong applicability. As long as ultra-wideband base stations are built around the unmanned vehicle, this application can be used to achieve unmanned vehicle tracking, thereby improving the general practicality of unmanned tracking driving.

[0074] Please refer to Figure 3 Another embodiment of the present invention provides an unmanned vehicle tracking terminal based on the fusion of UWB and GPS, including a memory, a processor, and a computer program stored in the memory and running on the processor. When the processor executes the computer program, it implements the various steps of the unmanned vehicle tracking method based on the fusion of UWB and GPS.

[0075] From the above description, it can be seen that the beneficial effects of the present invention are: real-time positioning of the unmanned vehicle is performed simultaneously through UWB (Ultra Wideband) positioning technology and GPS positioning technology, and the accuracy of GPS positioning technology is verified based on UWB positioning technology, so that the unmanned vehicle can complete the preset unmanned vehicle driving tracking task when the GPS and other satellite signals are weak or lost, or even without using GPS satellite signals, thereby avoiding the inability of a single positioning module to provide accurate positioning information, thereby improving the general practicality of unmanned tracking driving.

[0076] An unmanned vehicle tracking method and terminal based on the fusion of UWB and GPS provided in an embodiment of the present invention can be applied to unmanned vehicle tracking driving, avoiding the inability of a single positioning module to provide accurate positioning information, and improving the accuracy of unmanned tracking driving. The following is an explanation through a specific embodiment:

[0077] Please refer to Figure 1 to Figure 2 , Embodiment 1 of the present invention is:

[0078] An unmanned vehicle tracking method based on the fusion of UWB and GPS, comprising:

[0079] S1. Obtaining real-time positioning information of the UWB tag according to multiple UWB base stations, where the UWB tag is located on an unmanned vehicle.

[0080] The number of the UWB base stations is at least three, and the real-time positioning information includes instant latitude and longitude information.

[0081] The S1 is specifically:

[0082] S11. Determine whether the UWB tag receives ranging signals from the multiple UWB base stations at the same time. If so, obtain the positioning distance between the UWB tag and each UWB base station according to the ranging signals.

[0083] S12, obtaining the real-time longitude and latitude information of the UWB tag based on the positioning distance;

[0084] Specifically, the site longitude and latitude information of each UWB base station is obtained, and the earth radius of the location of the corresponding UWB base station is calculated according to the site longitude and latitude information.

[0085] The earth radius includes the spherical radius corresponding to the UWB base station and the latitude circle radius of the latitude:

[0086] The radius of the sphere is

[0087] The radius of the latitude circle is

[0088] Among them, E a is the equatorial radius of the Earth, E b is the earth's polar radius, and the latitude and longitude information of the station are respectively (lon Z0 ,lat Z0 ) and (lon Z1 ,lat Z1 );where lon Z is the longitude coordinate of the UWB base station, lat Z is the latitude coordinate of the UWB base station.

[0089] Specifically, Ea =6378137 meters, E b =6356752.2142 meters.

[0090] S13. Obtain the three-dimensional space distance between any two UWB base stations according to the earth radius.

[0091] The three-dimensional space distance is the arc length between any two UWB base stations:

[0092] Specifically,

[0093] Among them, E e is the arc length of the circle of the sphere where any two UWB base stations are located, E f is the arc length of the circle of latitude where any two UWB base stations are located.

[0094] S14. Calculate the two-dimensional space distance and the relative heading angle between the two UWB base stations according to the three-dimensional space distance, and establish a coordinate system based on the UWB base stations.

[0095] Specifically, the two-dimensional space distance is

[0096] The relative heading angle is

[0097] S15. Obtaining the real-time tag coordinates of the UWB tag according to the two-dimensional space distance and the positioning distance.

[0098] S16. Substitute the instant tag coordinates and the relative heading angle into a coordinate rotation formula to obtain the instant longitude and latitude information of the UWB tag.

[0099] S2. Obtain the GPS positioning information of the unmanned vehicle, obtain the first driving data of the unmanned vehicle according to the GPS positioning information, and obtain the second driving data of the unmanned vehicle according to the real-time positioning information, and determine whether the first driving data is consistent with the second driving data. If not, record the second driving data.

[0100] The first driving data includes a first moving distance and a first heading angle, and the second driving distance data includes a second moving distance and a second heading angle.

[0101] The S2 is specifically:

[0102] S21, obtaining GPS positioning information of the unmanned vehicle, wherein the GPS positioning information includes GPS positioning information at a previous moment and a current moment;

[0103] S22, obtaining a first moving distance and a first heading angle of the unmanned vehicle according to the GPS positioning information at the previous moment and the current moment;

[0104] S23, simultaneously acquiring real-time positioning information of the unmanned vehicle, wherein the real-time positioning information includes real-time positioning information at a previous moment and a current moment;

[0105] S24, obtaining a second moving distance and a second heading angle of the unmanned vehicle according to the real-time positioning information at the previous moment and the current moment;

[0106] S25. Determine whether the first moving distance and the first heading angle are consistent with the second moving distance and the second heading angle; if not, record the second moving distance and the second heading angle.

[0107] In an optional implementation, the step before S3 includes: controlling the unmanned vehicle to collect trajectory points to form a target unmanned vehicle path trajectory.

[0108] S3. Obtain the steering information of the unmanned vehicle according to the target unmanned vehicle path trajectory and the second driving data, and repeat S1-S3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory.

[0109] The S3 is specifically:

[0110] S31, obtaining a point set of the target unmanned vehicle path trajectory, and determining a target point in the point set according to the real-time positioning information at the current moment and the second heading angle.

[0111] S32, calculating the target heading angle of the real-time positioning information at the current moment relative to the target point.

[0112] Specifically, the target heading angle is Ret 目标 =arcosθ;

[0113] in, The real-time positioning information at the current moment is (lon 0 ,lat 0 ), the target point is (lon 1 ,lat 1 );Wherein, lon represents the longitude coordinate of the UWB tag, lat represents the latitude coordinate of the UWB tag; Dist represents the distance between the real-time positioning information and the target point, and cosθ represents the cosine value of the target heading angle.

[0114] S33. Obtain steering information of the unmanned vehicle according to the target heading angle and the second heading angle.

[0115] Specifically, if the difference between the target heading angle and the second heading angle is greater than zero, the unmanned vehicle performs a left turn;

[0116] If the difference between the target heading angle and the second heading angle is zero, the unmanned vehicle keeps going straight;

[0117] If the difference between the target heading angle and the second heading angle is less than zero, the unmanned vehicle performs a right turn.

[0118] S34: Control the unmanned vehicle to turn according to the turning information.

[0119] S35. Repeat S1-S3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory.

[0120] Please refer to Figure 1 to Figure 2 , Embodiment 2 of the present invention is:

[0121] A tracking method for unmanned vehicles based on the fusion of UWB and GPS is applied in actual scenarios, including:

[0122] D0, respectively record the longitude and latitude information of the three UWB base stations around the unmanned vehicle; among them, the longitude and latitude information of base station 0 is (lon z0 ,lat z0 ), the longitude and latitude information of base station 1 is (lon z1 ,lat z1 ), the latitude and longitude information of base station 2 is (lon z2 ,lat z2 ).

[0123] D1. Determine whether the UWB tag receives ranging signals from three UWB base stations at the same time. If so, obtain the positioning distance between the UWB tag and each UWB base station according to the ranging signal, and obtain the real-time longitude and latitude information of the UWB tag based on the positioning distance; wherein, the positioning distance between the UWB tag and base station 0 is dist_0, and the positioning distance between the UWB tag and base station 1 is dist_1.

[0124] Specifically, refer to Figure 2 , D11, taking base station 0 as the coordinate origin, establish a rectangular coordinate system, and calculate the two-dimensional space distance and relative heading angle between base station 0 and base station 1 according to the site longitude and latitude information of the three UWB base stations:

[0125] (1) Calculate the radius of the sphere where base station 1 is located:

[0126] Calculate the radius of the latitude circle at the latitude of base station 1:

[0127] (2) Calculate the arc length of the circle with the radius of the sphere where base station 0 and base station 1 are located

[0128] Calculate the arc length of the circle of latitude where base station 0 and base station 1 are located

[0129] (3) Calculate the two-dimensional distance between base station 0 and base station 1

[0130] Calculate the relative heading angle from base station 0 to base station 1

[0131] D12. Obtain the instantaneous tag coordinates of the UWB tag according to the two-dimensional space distance and the positioning distance, and substitute the instantaneous tag coordinates and the relative heading angle into the coordinate rotation formula to obtain the instantaneous longitude and latitude information of the UWB tag:

[0132] (1) Calculate the real-time tag coordinates of the UWB tag;

[0133] Based on the triangle sine and cosine theorem cosA = (dist_0 2 +dist_1 2 -Dist 二维 2 );

[0134]

[0135] The vertical coordinate of the instant label coordinate is The horizontal axis is

[0136] (2) Calculate the real-time longitude and latitude information of the UWB tag;

[0137] The instantaneous longitude of the UWB tag is lon tag =xcos(Ret 相对 )+ysin(Ret 相对 );

[0138] The instant latitude of the UWB tag is lat tag =ycos(Ret 相对 )-xsin(Ret 相对 ).

[0139] D2. Obtain the GPS positioning information of the unmanned vehicle, obtain the first driving data of the unmanned vehicle according to the GPS positioning information, and obtain the second driving data of the unmanned vehicle according to the real-time positioning information, and determine whether the first driving data is consistent with the second driving data. If not, record the second driving data. The first driving data includes the first moving distance Dist1 and the first heading angle Ret 1 The second travel distance data includes a second moving distance Dist 2 and the second heading angle Ret 2 .

[0140] D3, according to the target unmanned vehicle path trajectory and the second driving data to obtain the steering information of the unmanned vehicle, repeat the steps D1-D3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory. The real-time positioning information is the real-time longitude and latitude information (lon tag ,lat tag ).

[0141] Specifically, D31, obtain the point set of the target unmanned vehicle path trajectory, according to the real-time positioning information (lon tag ,lat tag ) and the second heading angle Ret 2 Determine the trajectory point 5 meters in front of the unmanned vehicle in the point set as the target point; where the coordinates of the target point are (lon obj ,lat obj ).

[0142] D32, calculating the target heading angle of the real-time positioning information at the current moment relative to the target point:

[0143]

[0144] The target heading angle is Ret 目标 =arcosθ.

[0145] D33, calculate the target heading angle Ret 目标 and the second heading angle Ret 2 The difference is used to obtain the steering information of the unmanned vehicle:

[0146] If Ret 目标 -Ret 2 >0, the unmanned vehicle performs a left turn;

[0147] If Ret 目标 -Ret 2 =0, the unmanned vehicle keeps going straight;

[0148] If Ret 目标 -Ret 2 <0, the unmanned vehicle performs a right turn.

[0149] D34. Control the unmanned vehicle to turn according to the turning information.

[0150] D35. Repeat steps D1-D3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory.

[0151] Please refer to Figure 3 , Embodiment 3 of the present invention is:

[0152] An unmanned vehicle tracking terminal 1 based on the fusion of UWB and GPS includes a memory 2, a processor 3, and a computer program stored in the memory 2 and running on the processor 3. When the processor 3 executes the computer program, each step in Example 1 and Example 2 is implemented.

[0153] In summary, the present invention provides a method and terminal for tracking unmanned vehicles based on the fusion of UWB and GPS. The unmanned vehicles are positioned in real time by using UWB (Ultra Wideband) positioning technology and GPS positioning technology. Since satellite signals are seriously interfered with indoors, GPS or Beidou cannot send positioning information, and the positioning accuracy of a single GPS is generally about 3 meters, while the ranging accuracy of UWB is 10 centimeters. The fusion of GPS and UWB can greatly improve the positioning accuracy of unmanned vehicles. Therefore, in this case, the unmanned vehicle uses an ultra-wideband module for ranging, and can obtain distance data with centimeter-level accuracy, thereby avoiding the inability of a single positioning module to provide accurate positioning information and improving the accuracy of positioning information. The accuracy of GPS positioning technology is verified based on UWB positioning technology, so that the unmanned vehicle can complete the preset unmanned vehicle driving tracking task in the case where the GPS and other satellite signals are weak or lost, or even without using GPS satellite signals, thereby broadening the application scenarios of unmanned tracking driving. In addition, the ultra-wideband tag module is much cheaper than the satellite positioning chip, which reduces the cost of unmanned vehicles.

[0154] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent transformations made using the contents of the present invention's specification and drawings, or directly or indirectly applied in related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. An unmanned vehicle tracking method based on the fusion of UWB and GPS, characterized in that: include: S1. Obtaining real-time positioning information of a UWB tag according to a plurality of UWB base stations, wherein the UWB tag is located on an unmanned vehicle; S2, obtaining GPS positioning information of the unmanned vehicle, obtaining first driving data of the unmanned vehicle according to the GPS positioning information, and obtaining second driving data of the unmanned vehicle according to the real-time positioning information, and determining whether the first driving data is consistent with the second driving data, and if not, recording the second driving data; S3, obtaining the steering information of the unmanned vehicle according to the target unmanned vehicle path trajectory and the second driving data, and repeatedly executing S1-S3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory; The number of the UWB base stations is at least three; the real-time positioning information includes real-time latitude and longitude information; The S1 is specifically: Determine whether the UWB tag receives ranging signals from the multiple UWB base stations at the same time, and if so, obtain the positioning distance between the UWB tag and each UWB base station according to the ranging signals; Obtaining real-time longitude and latitude information of the UWB tag based on the positioning distance; The instant latitude and longitude information of the UWB tag obtained based on the positioning distance is specifically: Obtaining the site longitude and latitude information of each UWB base station, and calculating the earth radius of the location of the corresponding UWB base station according to the site longitude and latitude information; Obtaining the three-dimensional space distance between any two UWB base stations according to the earth radius; Calculate the two-dimensional space distance and the relative heading angle between the two UWB base stations according to the three-dimensional space distance, and establish a coordinate system based on the UWB base stations; Obtaining the instant tag coordinates of the UWB tag according to the two-dimensional space distance and the positioning distance; Substituting the instant tag coordinates and the relative heading angle into a coordinate rotation formula to obtain the instant latitude and longitude information of the UWB tag; The earth radius includes the spherical radius corresponding to the UWB base station and the latitude circle radius of the latitude; The calculation of the earth radius corresponding to the location of the UWB base station according to the site longitude and latitude information is specifically: The radius of the sphere is The radius of the latitude circle is Among them, E a is the equatorial radius of the Earth, E b is the earth's polar radius, and the latitude and longitude information of the station are respectively (lon Z0 ,lat Z0 ) and (lon Z1 ,lat Z1 );where lon Z is the longitude coordinate of the UWB base station, lat Z is the latitude coordinate of the UWB base station; The three-dimensional space distance is the arc length between any two UWB base stations; The method of obtaining the three-dimensional space distance between any two UWB base stations according to the earth radius is specifically as follows: Among them, E e is the arc length of the circle of the sphere where any two UWB base stations are located, E f is the arc length of the circle of latitude where any two UWB base stations are located; The specific method of calculating the two-dimensional space distance and the relative heading angle between the two UWB base stations according to the three-dimensional space distance is as follows: The two-dimensional space distance is The relative heading angle is 2. The unmanned vehicle tracking method based on the fusion of UWB and GPS according to claim 1, characterized in that: The first driving data includes a first moving distance and a first heading angle, and the second driving data includes a second moving distance and a second heading angle; The S2 is specifically: Obtaining GPS positioning information of the unmanned vehicle, wherein the GPS positioning information includes GPS positioning information at a previous moment and a current moment; Obtaining a first moving distance and a first heading angle of the unmanned vehicle according to the GPS positioning information at the previous moment and the current moment; At the same time, real-time positioning information of the unmanned vehicle is obtained, wherein the real-time positioning information includes real-time positioning information at a previous moment and a current moment; Obtaining a second moving distance and a second heading angle of the unmanned vehicle according to the real-time positioning information at the previous moment and the current moment; Determine whether the first moving distance and the first heading angle are consistent with the second moving distance and the second heading angle; if not, record the second moving distance and the second heading angle.

3. The unmanned vehicle tracking method based on the fusion of UWB and GPS according to claim 2 is characterized in that: The S3 is specifically: Acquire a point set of the target unmanned vehicle path trajectory, and determine a target point in the point set according to the real-time positioning information at the current moment and the second heading angle; Calculate the target heading angle of the real-time positioning information at the current moment relative to the target point; Obtaining steering information of the unmanned vehicle according to the target heading angle and the second heading angle; Controlling the unmanned vehicle to turn according to the turning information; Repeat S1-S3 until the real-time positioning information is consistent with the preset end point information in the target unmanned vehicle path trajectory.

4. The unmanned vehicle tracking method based on the fusion of UWB and GPS according to claim 3 is characterized in that: The target heading angle of the real-time positioning information at the current moment relative to the target point is calculated as follows: Target heading angle Ret 目标 =arcosθ; in, The real-time positioning information at the current moment is (lon0, lat0), and the target point is (lon1, lat1); wherein lon represents the longitude coordinate of the UWB tag, and lat represents the latitude coordinate of the UWB tag; Dist represents the distance between the real-time positioning information and the target point, and cosθ represents the cosine value of the target heading angle; The step of obtaining the steering information of the unmanned vehicle according to the target heading angle and the second heading angle is specifically: If the difference between the target heading angle and the second heading angle is greater than zero, the unmanned vehicle performs a left turn; If the difference between the target heading angle and the second heading angle is zero, the unmanned vehicle keeps going straight; If the difference between the target heading angle and the second heading angle is less than zero, the unmanned vehicle performs a right turn.

5. An unmanned vehicle tracking terminal based on the fusion of UWB and GPS, comprising a memory, a processor, and a computer program stored in the memory and running on the processor, characterized in that: When the processor executes the computer program, the various steps of the unmanned vehicle tracking method based on the fusion of UWB and GPS as described in any one of claims 1 to 4 are implemented.

Images