High-precision path planning method based on GPS and gyroscope and application

Abstract

The invention discloses a high-precision path planning method based on a GPS and a gyroscope and application, the navigation direction of a trolley and the rotating speed of a motor are adjusted in real time by collecting GPS data of a current position and combining yaw angle calculation of the gyroscope, it is ensured that the trolley stably travels to a target point, and the trolley is automatically switched to a next target after reaching the target, so that the accuracy of path planning is improved. And all paths are completed. According to the path planning method in combination with the GPS and the gyroscope, through real-time yaw angle error updating, motor dynamic speed regulation and accurate target point distance judgment, it is ensured that the intelligent trolley stably travels in a complex environment, and path deviation and errors are effectively avoided. A nine-axis sensor and a gradient descent algorithm are used for improving the yaw angle calculation precision, the anti-interference capability and the navigation precision of the system are enhanced, and the operation efficiency and the response speed are optimized.

Description

Technical Field

[0001] The present invention relates to the field of smart logistics technology, and specifically to a high-precision path planning method and application based on GPS and a gyroscope. Background Art

[0002] In the new era, the logistics industry plays an important role in modern society, and the demand for intelligent management and service models is increasing. With the advancement of economic globalization, the maturity of emerging technologies such as the Internet of Things, artificial intelligence, and big data, smart logistics has developed rapidly and has become the only way for the transformation of the logistics industry. Compared with traditional logistics, smart logistics has shown great advantages in reducing costs, improving circulation efficiency, and expanding services. Against this background, smart logistics vehicles have emerged as the times require, becoming a key tool for improving logistics efficiency, reducing operating costs, and enhancing transportation safety. In particular, embedded smart logistics vehicles play an important role in express delivery, automatic driving, intelligent navigation, obstacle avoidance, etc., and have technical characteristics such as real-time data processing and information control, which has greatly promoted the innovation and development of the logistics industry.

[0003] However, the existing background technology has some limitations. Existing logistics automation systems mostly rely on basic sensor technology, which may face problems such as insufficient accuracy, unstable positioning, and delayed system response in complex environments. Existing smart car navigation systems may have difficulty coping with high-precision path planning and dynamic environmental changes, especially when the GPS signal is weak or blocked, the positioning accuracy is affected. In addition, the system may face problems of insufficient energy efficiency and low system fault tolerance during long-term operation. These limitations restrict its application and popularization in a wider range of scenarios. Summary of the invention

[0004] In order to improve the success rate of logistics task delivery in smart logistics, this paper proposes a high-precision path planning method and application based on GPS and gyroscope, which solves the path planning and positioning accuracy problems of smart logistics vehicles in complex environments. The direction angle and distance are calculated by combining GPS and gyroscope, and the yaw angle error is corrected by PID control. At the same time, the motor speed is dynamically adjusted and the path is updated in real time, which improves the driving stability and accuracy of the vehicle, enhances the adaptability and fault tolerance of the system, and promotes the efficient application of smart logistics.

[0005] To achieve the above object, the present invention provides the following technical solutions:

[0006] The present invention provides a high-precision path planning method based on GPS and gyroscope, comprising the following steps:

[0007] S1, path data collection and storage: start the smart car and turn on the GPS module, collect the longitude and latitude coordinate information of the current position, and store the path data in the Flash memory of the microcontroller through the ring buffer;

[0008] S2, gyroscope initialization and yaw angle calculation: Initialize the gyroscope hardware and calibrate the zero drift, set the initial yaw angle value to the true north direction, obtain the gyroscope yaw angle data in real time, combine the current coordinates provided by GPS with the target point coordinates, and calculate the target direction angle and distance;

[0009] S3, navigation control and yaw angle error correction: compare the current yaw angle with the target direction angle, calculate the error between the two, and dynamically adjust the proportional, integral and differential parameters of the PID controller according to the path complexity and vehicle speed, control the servo to adjust the vehicle body direction so that the yaw angle gradually approaches the target direction angle, and ensure that the car moves in the target direction;

[0010] S4, motor speed optimization and dynamic braking: Based on the distance between the current position and the target point, the motor speed is adjusted through the dynamic braking curve function to ensure smooth driving and gradually decelerate when approaching the target point;

[0011] S5, target point distance judgment and path update: using multi-sensor fusion technology, combined with GPS and ultrasonic sensors to measure the distance between the current position and the target point in real time, to determine whether the target point has been reached; if not, refresh the GPS coordinates and repeat steps S2-S5 until the target point is reached; if the target point is reached, execute step S6;

[0012] S6, end judgment and stop operation: judge whether the last target point is reached; if reached, stop the car; if not, switch to the next target point and continue to refresh the GPS coordinates and repeat steps S2-S5.

[0013] As a further solution of the present invention, the specific method steps of step S1 are:

[0014] S11, start the smart car and turn on the GPS module, receive the longitude and latitude information collected by the GPS module through the serial port, and display the collected GPS coordinate information on the screen in real time;

[0015] S12, the user controls the collection and stop of GPS coordinates by pressing a button, and the button triggers the saving of the collected GPS path information into the Flash storage of the single-chip computer;

[0016] S13. During the path planning process, the saved GPS path data is exported from the Flash storage and loaded into the target path array for subsequent navigation.

[0017] As a further solution of the present invention, in step S2, the target direction angle and distance are calculated by combining the current coordinates provided by GPS with the target point coordinates, and the target direction angle and distance are calculated by the direction angle calculation formula and the Haversine formula respectively.

[0018] As a further solution of the present invention, the proportional, integral and differential parameters of the PID controller are dynamically adjusted in step S3 according to the following formula:

[0019] K p =K p0 +α·|Δθ|,K i =K i0 +β·V,K d =K d0 +γ·|Δθ′|, where K p0 , K i0 ,

[0020] K d0 is the initial parameter, Δθ is the yaw angle error, V is the current speed, Δθ′ is the rate of change of the yaw angle error, and α, β, γ are adaptive adjustment coefficients.

[0021] As a further solution of the present invention, the dynamic braking curve function in step S4 uses a quadratic Bezier curve, and the change in motor speed is described by the following function:

[0022] v(t)=(1-t) 2 ·v 0 +2t(1-t)·v m +t 2 ·v f , t∈[0,1];

[0023] Among them, v 0 is the initial velocity, v m is the intermediate speed, v f is the final speed of the target point, and t is the normalized time parameter; dynamic braking adjusts the motor speed in real time through a 20ms timer to ensure smooth driving and gradually decelerate when approaching the target point.

[0024] As a further solution of the present invention, the specific steps of step S5 are:

[0025] S51, collect GPS and ultrasonic sensor data in real time, and calculate the fusion distance between the current position and the target point;

[0026] S52, determining whether the distance between the current position and the target point exceeds 1.5 times of the maximum accuracy of the set threshold; if it is less than 1.5 times of the threshold, it is considered that the target point has been reached and step S6 is executed;

[0027] S53. If the fusion distance is greater than 1.5 times the set threshold, it is considered that the target point has not been reached, and the GPS coordinates are continuously refreshed and steps S2-S4 are repeated until the distance to the target point is within the allowable error range, and the target point is considered to have been reached.

[0028] As a further solution of the present invention, the yaw angle calculation involved in the steps S1-S6 uses a nine-axis sensor and a gradient descent-based attitude settlement algorithm to calculate the yaw angle of the Euler angle. The algorithm flow of the attitude settlement is as follows:

[0029] T1, gyroscope hardware initialization and zero drift calibration: initialize the gyroscope hardware and clear the zero drift error to ensure the accuracy of the initial data;

[0030] T2, start timer and data collection: start the 5ms timer, trigger interrupts regularly, and use the nine-axis sensor to collect raw physical quantity data, including acceleration, angular velocity, and magnetism;

[0031] T3, sensor data preprocessing: perform first-order low-pass filtering on the three-axis acceleration data and convert the three-axis gyroscope data into radians;

[0032] T4, unitization processing and error correction: unitize the acceleration and magnetometer data to ensure that the vector amplitude is 1, and make the X-axis of the navigation coordinate system point to the true north. Use the gradient descent method to calculate and correct the error, compensate the error vector, update the quaternion, and improve the accuracy of attitude solution;

[0033] T5, quaternion update and Euler angle calculation: Update the quaternion according to the corrected sensor data, convert the quaternion into Euler angle, and extract the yaw angle as a reference for the navigation direction;

[0034] T6. Euler angle high-pass filtering: Perform high-pass filtering on the Euler angle to remove low-frequency noise and ensure the accuracy and stability of the attitude angle.

[0035] As a further solution of the present invention, it is applied to path planning and navigation control in intelligent logistics carts, automatic driving systems, robot navigation systems, drone navigation and intelligent warehousing systems.

[0036] Compared with the prior art, the beneficial technical effects of the present invention are:

[0037] 1. The present invention combines GPS and gyroscope data fusion technology to accurately calculate the target direction angle and yaw angle, effectively avoid path deviation and error, and ensure the stable driving of smart cars or robots in complex environments;

[0038] 2. Dynamically adjust the motor speed based on the distance between the target point and the current position, and automatically decelerate when approaching the target point, which not only improves the operating efficiency, but also prevents collision or deviation caused by excessive speed, ensuring smooth driving;

[0039] 3. Set up an accurate judgment mechanism for the distance to the target point, combined with reasonable threshold settings, to ensure that the system will not misjudge or stop prematurely before reaching the target point, avoid wasting time or missing the target, and improve the reliability of path planning;

[0040] 4. Use the nine-axis sensor and the attitude solution algorithm based on gradient descent to reduce the gyroscope zero drift error, improve the yaw angle calculation accuracy, and enhance the system's anti-interference ability and stability;

[0041] 5. Dynamically adjust PID control parameters and combine the dynamic braking method of cubic Bezier curve to optimize the path planning process, ensure the smoothness of driving, and provide efficient and intelligent control solutions for smart logistics vehicles, autonomous driving systems and robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 is a flowchart of the steps of the path planning method of the present invention;

[0043] Figure 2 It is a gyroscope attitude settlement flow chart of the present invention;

[0044] Figure 3 A schematic diagram of determining the navigation direction through the direction angle and the yaw angle of the present invention;

[0045] Figure 4 This is the main interface diagram of the human-computer interaction of the present invention. DETAILED DESCRIPTION

[0046] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the implementation cases described are only part of the implementation content of the present invention, not all. Based on the implementation content of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0047] Embodiment 1:

[0048] See also Figures 1 to 4 In an embodiment of the present invention, a high-precision path planning method based on GPS and gyroscope is provided, comprising the following steps:

[0049] S1, path data collection and storage: start the smart car and turn on the GPS module, collect the longitude and latitude coordinate information of the current position, and store the path data in the Flash memory of the microcontroller through the ring buffer;

[0050] S2, gyroscope initialization and yaw angle calculation: Initialize the gyroscope hardware and calibrate the zero drift, set the initial yaw angle value to the true north direction, obtain the gyroscope yaw angle data in real time, combine the current coordinates provided by GPS with the target point coordinates, and calculate the target direction angle and distance;

[0051] S3, navigation control and yaw angle error correction: compare the current yaw angle with the target direction angle, calculate the error between the two (yaw angle error = current yaw angle - target direction angle), and dynamically adjust the proportional, integral and differential parameters of the PID controller according to the path complexity and vehicle speed, control the servo to adjust the vehicle body direction so that the yaw angle gradually approaches the target direction angle, and ensure that the car is moving in the target direction;

[0052] S4, dynamic motor speed regulation: Based on the distance between the current position and the target point, the motor speed is adjusted through the dynamic braking curve function to ensure smooth driving and gradually slow down when approaching the target point;

[0053] S5, target point distance judgment and path update: using multi-sensor fusion technology, combined with GPS and ultrasonic sensors to measure the distance between the current position and the target point in real time, to determine whether the target point has been reached; if not, refresh the GPS coordinates and repeat steps S2-S5 until the target point is reached; if the target point is reached, execute step S6;

[0054] S6, end judgment and stop operation: judge whether the last target point is reached; if reached, stop the car; if not, switch to the next target point and continue to refresh the GPS coordinates and repeat steps S2-S5.

[0055] This path planning method can effectively improve the navigation ability of the smart car in complex environments and ensure its stable driving by integrating multiple technologies such as GPS and gyroscope technology, PID control, dynamic motor speed regulation and precise target point judgment. Its main advantage is that it can rely on the gyroscope to correct the yaw angle and improve navigation accuracy when the GPS signal is unstable. At the same time, the PID control system optimizes the driving direction and the motor speed regulation technology ensures smooth operation.

[0056] Specifically, the specific method steps of step S1 are:

[0057] S11, start the smart car and turn on the GPS module, receive the longitude and latitude information collected by the GPS module through the serial port, and display the real-time GPS coordinates on the screen. This step ensures the real-time acquisition and monitoring of the coordinates of the current location, providing basic data for subsequent navigation.

[0058] S12, the user controls the collection and stop of GPS coordinates through buttons, and the button triggers the saving of the collected GPS path information into the Flash storage of the single-chip computer; this step allows the user to manually control the collection and storage of data for subsequent path planning.

[0059] S13, during the path planning process, the saved GPS path data is exported from the Flash storage and loaded into the target path array for subsequent navigation. This step ensures that the path data can be accessed and applied in real time during the navigation process, providing continuous navigation support for the smart car.

[0060] Specifically, in step S2, the target direction angle and distance are calculated according to the current coordinates and target point coordinates provided by the GPS, which are respectively achieved by using the direction angle calculation formula and the Haversine formula.

[0061] Calculate the direction angle (azimuth or initial bearing) between two longitude and latitude coordinates:

[0062] Given two longitude and latitude points A (LatA, LonA) and B (LatB, LonB), calculate the direction angle θ from A to B deg The steps are as follows:

[0063] Step 1: Convert longitude and latitude to radians:

[0064] LatA_rad=LatA·(π / 180), LonA_rad=LonA·(π / 180);

[0065] LatB_rad=LatB·(π / 180), LonB_rad=LonB·(π / 180);

[0066] Step ② Calculate the direction angle θ (radians):

[0067] ΔLon = LonB_rad - LonA_rad is the difference in longitude;

[0068] θ=atan2(sin(ΔLon)·cos(LatB_rad),cos(LatA_rad)·sin(LatB_rad)

[0069] -sin(LatA_rad)·cos(LatB_rad)·cos(ΔLon));

[0070] Step 3: Convert the direction angle to degrees: θ deg =θ·(180 / π);

[0071] Step ④ Ensure that the direction angle is within the range of 0° to 360°: If the calculated direction angle is less than 0, add 360° to ensure that it is between 0° and 360°

[0072] Calculate the distance between two longitude and latitude coordinates: Usually use the Haversine formula. The following are the detailed steps of the Haversine formula:

[0073] Step ① Convert degrees to radians: Convert longitude and latitude to radians, radians = degrees × (π / 180);

[0074] Step ② Use the Haversine formula to calculate the distance: Assume that the coordinates of point A are (lat1, lon1) and the coordinates of point B are (lat2, lon2), and these two coordinates have been converted to radians:

[0075]

[0076] Where R is the average radius of the Earth (about 6371 km), arcsin is the inverse sine function, sin2 and cos 2 represent the square of sine and cosine respectively.

[0077] Specifically, when dynamically adjusting the proportional, integral and differential parameters of the PID controller in step S3, it is achieved according to the following formula: K p =K p0 +α·|Δθ|,K i =K i0 +β·V,K d =K d0 +γ·|Δθ′|, where K p0 , K i0 , K d0 is the initial parameter, Δθ is the yaw angle error, V is the current speed, Δθ′ is the rate of change of the yaw angle error, and α, β, γ are adaptive adjustment coefficients, which are used to dynamically adjust the parameters of the PID controller according to the actual situation of the system, so as to better adapt to the current environment and system status, improve the control accuracy, and ensure that the car drives smoothly along the target direction.

[0078] In order to ensure that the smart car is moving in the correct target direction, in addition to controlling the servo to change direction so that the yaw angle approaches the target direction angle, a 5ms timer needs to be enabled to update the yaw angle error in real time. This can not only ensure the system's rapid response to the yaw angle error, but also improve the car's stability and accuracy, ensuring its reliable driving in complex environments.

[0079] Specifically, the dynamic braking curve function in step S4 uses a quadratic Bezier curve, and the change in motor speed is described by the following function:

[0080] v(t)=(1-t) 2 ·v 0 +2t(1-t)·v m +t 2 ·v f , t∈[0,1];

[0081] Among them, v0 is the initial speed, vm is the intermediate speed, vf is the final speed of the target point, and t is the normalized time parameter; dynamic braking adjusts the motor speed in real time through a 20ms timer to ensure smooth driving and gradually decelerates when approaching the target point.

[0082] Specifically, the specific steps of step S5 are:

[0083] S51, collect GPS and ultrasonic sensor data in real time, and calculate the fusion distance between the current position and the target point; the purpose of this step is to combine the data of GPS and ultrasonic sensors to calculate the distance between the current smart car or robot and the target point. The coordinate information provided by GPS can provide rough positioning, while the ultrasonic sensor is used to accurately measure the distance.

[0084] Through data fusion technology, the advantages of these two sensors are combined to improve positioning accuracy and reduce the errors that may be caused by a single sensor, thereby obtaining a more accurate distance to the target point.

[0085] S52, determining whether the distance between the current position and the target point exceeds 1.5 times of the maximum accuracy of a set threshold, and setting a threshold, which is usually based on the maximum error value of GPS positioning accuracy or ultrasonic ranging accuracy.

[0086] By using 1.5 times the maximum accuracy of the threshold as the judgment standard, misjudgment due to excessive sensor error can be prevented. This setting means that if the distance between the current position and the target point is within the threshold range, it can be considered that the target point has been approached, and subsequent stop or target point switching operations can be performed.

[0087] S53: If the fusion distance is greater than 1.5 times the set threshold, it is considered that the target point has not been reached, and the GPS coordinates are continuously refreshed and steps S2-S4 are repeated.

[0088] If the fused distance exceeds the set range, it is considered that the target point has not been reached. At this time, the system will continue to obtain updated GPS coordinate information, recalculate the direction angle and plan the path (by repeating steps S2-S4) until the distance to the target point is within the allowable error range, and finally enter step S6.

[0089] This mechanism ensures that the car can continue to adjust its path and orientation to avoid stopping prematurely or missing the target point, thereby improving the accuracy of path planning.

[0090] Specifically, the yaw angle calculation involved in steps S1-S6 uses a nine-axis sensor and a gradient descent-based attitude settlement algorithm to calculate the yaw angle of the Euler angle. The algorithm flow of the attitude settlement is as follows:

[0091] T1, gyroscope hardware initialization and zero drift calibration: initialize the gyroscope hardware and clear the zero drift error to ensure the accuracy of the initial data;

[0092] T2, start timer and data collection: start the 5ms timer, trigger interrupts regularly, and use the nine-axis sensor to collect raw physical quantity data, including acceleration, angular velocity, and magnetism;

[0093] T3, sensor data preprocessing: perform first-order low-pass filtering on the three-axis acceleration data and convert the three-axis gyroscope data into radians;

[0094] T4, unitization processing and error correction: unitize the acceleration and magnetometer data to ensure that the vector amplitude is 1, and make the X-axis of the navigation coordinate system point to the true north. Use the gradient descent method to calculate and correct the error, compensate the error vector, update the quaternion, and improve the accuracy of attitude solution;

[0095] T5, quaternion update and Euler angle calculation: Update the quaternion according to the corrected sensor data, convert the quaternion into Euler angle, and extract the yaw angle as a reference for the navigation direction;

[0096] T6. Euler angle high-pass filtering: Perform high-pass filtering on the Euler angle to remove low-frequency noise and ensure the accuracy and stability of the attitude angle.

[0097] Embodiment 2:

[0098] The embodiment of the present invention provides an application of a high-precision path planning method based on GPS and a gyroscope, which is applied to path planning and navigation control in intelligent logistics carts, automatic driving systems, robot navigation systems, drone navigation, and intelligent warehousing systems. By combining GPS positioning technology with the attitude perception of the gyroscope, accurate navigation and path planning can be achieved in complex environments to ensure the stability and efficiency of the equipment. Especially in intelligent logistics carts, this method can effectively improve the accuracy and transportation efficiency of terminal delivery. In automatic driving and robot navigation, it can also ensure that the system continuously optimizes the path in a dynamic environment and quickly responds to environmental changes, thereby improving the safety and intelligence level of the overall system.

[0099] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A high-precision path planning method based on GPS and gyroscope, characterized in that: The steps include: S1, path data collection and storage: start the smart car and turn on the GPS module, collect the longitude and latitude coordinate information of the current position, and store the path data in the Flash memory of the microcontroller through the ring buffer; S2, gyroscope initialization and yaw angle calculation: Initialize the gyroscope hardware and calibrate the zero drift, set the initial yaw angle value to the true north direction, obtain the gyroscope yaw angle data in real time, combine the current coordinates provided by GPS with the target point coordinates, and calculate the target direction angle and distance; S3, navigation control and yaw angle error correction: compare the current yaw angle with the target direction angle, calculate the error between the two, and dynamically adjust the proportional, integral and differential parameters of the PID controller according to the path complexity and vehicle speed, control the servo to adjust the vehicle body direction so that the yaw angle gradually approaches the target direction angle, and ensure that the car moves in the target direction; S4, motor speed optimization and dynamic braking: Based on the distance between the current position and the target point, the motor speed is adjusted through the dynamic braking curve function to ensure smooth driving and gradually decelerate when approaching the target point; S5, target point distance judgment and path update: using multi-sensor fusion technology, combined with GPS and ultrasonic sensors to measure the distance between the current position and the target point in real time, to determine whether the target point has been reached; if not, refresh the GPS coordinates and repeat steps S2-S5 until the target point is reached; if the target point is reached, execute step S6; S6, end judgment and stop operation: judge whether the last target point is reached; if reached, stop the car; if not, switch to the next target point and continue to refresh the GPS coordinates and repeat steps S2-S5.

2. A high-precision path planning method based on GPS and gyroscope according to claim 1, characterized in that: The specific method steps of step S1 are: S11, start the smart car and turn on the GPS module, receive the longitude and latitude information collected by the GPS module through the serial port, and display the collected GPS coordinate information on the screen in real time; S12, the user controls the collection and stop of GPS coordinates by pressing a button, and the button triggers the saving of the collected GPS path information into the Flash storage of the single-chip computer; S13. During the path planning process, the saved GPS path data is exported from the Flash storage and loaded into the target path array for subsequent navigation.

3. The high-precision path planning method based on GPS and gyroscope according to claim 1, characterized in that: In step S2, the target direction angle and distance are calculated by combining the current coordinates provided by the GPS with the target point coordinates, and the target direction angle and distance are calculated by the direction angle calculation formula and the Haversine formula respectively.

4. The high-precision path planning method based on GPS and gyroscope according to claim 1, characterized in that: When dynamically adjusting the proportional, integral and differential parameters of the PID controller in step S3, it is achieved according to the following formula: K p =K p0 +α·|Δθ|,K i =K i0 +β·V,K d =K d0 +γ·|Δθ′|, where K p0 ,K i0 , K d0 is the initial parameter, Δθ is the yaw angle error, V is the current speed, Δθ′ is the rate of change of the yaw angle error, and α, β, γ are adaptive adjustment coefficients.

5. The high-precision path planning method based on GPS and gyroscope according to claim 1, characterized in that: The dynamic braking curve function in step S4 uses a quadratic Bezier curve, and the change in motor speed is described by the following function: v(t)=(1-t) 2 ·v0+2t(1-t)·v m +t 2 ·v f ,t∈[0,1]; Among them, v0 is the initial velocity, v m is the intermediate speed, v f is the final speed of the target point, and t is the normalized time parameter; dynamic braking adjusts the motor speed in real time through a 20ms timer to ensure smooth driving and gradually decelerate when approaching the target point.

6. The high-precision path planning method based on GPS and gyroscope according to claim 1, characterized in that: The specific steps of step S5 are: S51, collect GPS and ultrasonic sensor data in real time, and calculate the fusion distance between the current position and the target point; S52, determining whether the distance between the current position and the target point exceeds 1.5 times of the maximum accuracy of the set threshold. If it is less than 1.5 times of the threshold, it is considered that the target point has been reached and step S6 is executed; S53. If the fusion distance is greater than 1.5 times the set threshold, it is considered that the target point has not been reached, and the GPS coordinates are continuously refreshed and steps S2-S4 are repeated until the distance to the target point is within the allowable error range, and the target point is considered to have been reached.

7. The high-precision path planning method based on GPS and gyroscope according to claim 1, characterized in that: The yaw angle calculation involved in the steps S1-S6 uses a nine-axis sensor and a gradient descent-based attitude settlement algorithm to calculate the yaw angle of the Euler angle. The algorithm flow of the attitude settlement is as follows: T1, gyroscope hardware initialization and zero drift calibration: initialize the gyroscope hardware and clear the zero drift error to ensure the accuracy of the initial data; T2, start timer and data collection: start the 5ms timer, trigger interrupts regularly, and use the nine-axis sensor to collect raw physical quantity data, including acceleration, angular velocity, and magnetism; T3, sensor data preprocessing: perform first-order low-pass filtering on the three-axis acceleration data and convert the three-axis gyroscope data into radians; T4, unitization processing and error correction: unitize the acceleration and magnetometer data to ensure that the vector amplitude is 1, and make the X-axis of the navigation coordinate system point to the true north. Use the gradient descent method to calculate and correct the error, compensate the error vector, update the quaternion, and improve the accuracy of attitude solution; T5, quaternion update and Euler angle calculation: Update the quaternion according to the corrected sensor data, convert the quaternion into Euler angle, and extract the yaw angle as a reference for the navigation direction; T6. Euler angle high-pass filtering: Perform high-pass filtering on the Euler angle to remove low-frequency noise and ensure the accuracy and stability of the attitude angle.

8. A high-precision path planning method based on GPS and gyroscope according to any one of claims 1 to 7, characterized in that: It is used for path planning and navigation control in intelligent logistics vehicles, autonomous driving systems, robot navigation systems, drone navigation and intelligent warehousing systems.

Images