Wheelbarrow tracking planning method and system based on integrated navigation

Abstract

The invention relates to the technical field of integrated navigation, in particular to a wheelbarrow tracking planning method and system based on integrated navigation, and the method comprises the steps: employing a built-in GPS module of a wheelbarrow to carry out tracking point position collection and storage; the accelerated speed and the angular speed collected by the IMU are processed through an extended Kalman filtering method, and then attitude quaternion is updated in real time through a Mahony complementary filtering algorithm and converted into an attitude angle; the GPS module receives the data in real time and converts the stored latitude and longitude coordinates of the tracking point into Cartesian coordinates; calculating an Euclidean distance and an azimuth angle between the current point and the target point by using the obtained Cartesian coordinates; generating steering angle data according to the attitude angle obtained by the IMU data and the azimuth angle obtained by the GPS data; therefore, the control parameters of the wheelbarrow are adjusted. The advantages of the integrated navigation technology are fully utilized, an efficient and accurate solution is provided for road tracking planning of the monocycle, and the method has wide application prospects and market value.

Description

Technical Field

[0001] The present invention relates to the technical field of integrated navigation, and particularly to a unicycle path tracking and planning method and system based on integrated navigation. Background Art

[0002] With the continuous development of the field of intelligent technology, intelligent unicycles have been developed based on intelligent technology, and various competitions have emerged. Especially for the path tracking requirements in outdoor restricted spaces, due to problems such as outdoor lens exposure, common vision methods are greatly limited. In view of this, we propose a unicycle path tracking and planning method and system based on integrated navigation, which uses integrated navigation technology and the control balance of the unicycle for automatic adjustment, so that the intelligent unicycle can automatically complete the competition. Summary of the Invention

[0003] The purpose of the present invention is to provide a unicycle path tracking and planning method and system based on integrated navigation to solve the problems raised in the above background art.

[0004] To achieve the above purpose, the present invention provides the following technical solutions:

[0005] A unicycle path tracking and planning method based on integrated navigation, comprising the following steps:

[0006] Step S1: Use the GPS module built in the unicycle to collect and store path tracking points.

[0007] Step S2: Use the extended Kalman filter method to process the acceleration and angular velocity collected by the IMU unit, and then use Mahony's complementary filter algorithm to update the attitude quaternion in real time and convert it into an attitude angle.

[0008] Step S3: The GPS module receives data in real time and converts the longitude and latitude coordinates of the stored path tracking points into Cartesian coordinates.

[0009] Step S4: Calculate the Euclidean distance and azimuth angle between the current point and the target point using the obtained Cartesian coordinates.

[0010] Step S5: Generate steering angle data based on the attitude angle obtained from the IMU data and the azimuth angle obtained from the GPS data, so as to adjust the control parameters of the unicycle.

[0011] Preferably, in step S1, the following steps are included:

[0012] Step S11: Through the GPS receiving module integrated in the unicycle, real-time collect three-dimensional geographical coordinate data at a sampling frequency not lower than 10 Hz.

[0013] Step S12: Receive the input of path key points marked manually through the human-computer interaction device, and generate auxiliary positioning data;

[0014] Step S13: Establish a cyclic storage structure with a predefined capacity. After performing smoothing filtering on the positioning data that has passed the validity verification, store it in the non-volatile memory in a floating-point format with a time stamp;

[0015] Step S14: Provide a visual trajectory editing interface, and perform anomaly detection and manual correction on the stored trajectory point set based on the preset data verification rules.

[0016] Preferably, step S2 includes the following steps:

[0017] Step S21: Through the IMU module set on the unicycle, collect three-dimensional acceleration and angular velocity data in real time at a sampling frequency of not less than 1000 Hz;

[0018] Step S22: Perform multi-level filtering and fusion processing on the data collected by the IMU module.

[0019] Preferably, step S22 includes:

[0020] S221: Use the extended Kalman filter algorithm to perform state estimation on the acceleration and angular velocity data, and output preliminary attitude information including attitude error and angular velocity deviation;

[0021] S222: Based on the preliminary attitude information, perform iterative update on the quaternion differential equation through the Mahony complementary filter algorithm, and construct the gravity vector observation error in combination with the acceleration data, and correct the quaternion through proportional-integral feedback;

[0022] S223: Convert the corrected attitude quaternion into roll angle, pitch angle and yaw angle, and output them as real-time attitude angles.

[0023] Preferably, step S3 includes the following steps:

[0024] Step S31: Receive the longitude and latitude coordinates of the tracking points of the unicycle in real time through the GPS module;

[0025] Step S32: Store the received longitude and latitude coordinates of the tracking points in the storage module;

[0026] Step S33: Convert the stored longitude and latitude coordinates of the tracking points into Cartesian coordinates.

[0027] Preferably, step S4 further includes the following steps:

[0028] Compare the Euclidean distance with a preset distance threshold, and dynamically update the target path point according to the comparison result:

[0029] If the Euclidean distance is greater than the preset distance threshold, the current target path point is maintained;

[0030] If the Euclidean distance is less than or equal to the preset distance threshold, the target path point is updated to the next path point in the pre-stored path sequence.

[0031] Preferably, the preset distance threshold is dynamically adjusted according to the real-time motion speed of the unicycle. When the speed increases, the threshold increases accordingly. The adjustment of the preset distance threshold satisfies the following relational expression:

[0032] Dthreshold = k·v + D0

[0033] where v is the real-time motion speed, k is the proportionality coefficient, and D0 is the minimum safety distance constant.

[0034] Preferably, the azimuth angle calculates the heading deflection angle through the coordinate difference between the current point and the target point, and combines the yaw angle in the real-time attitude angle for heading correction.

[0035] Preferably, step S5 includes the following steps:

[0036] S51. Generate the steering angle data of the unicycle based on the attitude angle data of the IMU and the azimuth angle data of the GPS;

[0037] S52. Adjust the control parameters of the unicycle based on the steering angle data;

[0038] S53. Execute the drive optimization operation of the unicycle based on the adjusted control parameters;

[0039] S54. Realize the tracking planning and control of the unicycle according to the optimized drive strategy.

[0040] A unicycle tracking planning system based on integrated navigation, comprising:

[0041] A GPS operation unit, arranged inside the unicycle, for collecting three-dimensional geographic coordinate data and generating tracking positioning data;

[0042] A storage unit, for storing the data collected by the GPS operation unit and the target path;

[0043] An IMU operation unit, for collecting the self-state of the unicycle and generating attitude angle data;

[0044] An integrated navigation unit, respectively communicatively connected to the GPS operation unit and the IMU operation unit, for performing integrated operation on the collected GPS / IMU data and generating steering angle data;

[0045] The unicycle control module is communicatively connected to the integrated navigation unit and is used to make decisions based on the steering signal so as to complete the body motion control for path tracking.

[0046] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0047] 1. The unicycle path tracking and planning method based on integrated navigation uses the GPS receiving module integrated in the unicycle to collect three-dimensional geographical coordinate data in real time at a sampling frequency not lower than 10 Hz, ensuring the positioning accuracy of the unicycle during the path tracking process. At the same time, by combining the acceleration and angular velocity data collected by the IMU module and performing multi-level filtering and fusion processing, the accuracy of attitude estimation is further improved.

[0048] 2. The unicycle path tracking and planning method based on integrated navigation uses Mahony's complementary filtering algorithm to update the attitude quaternion in real time and convert it into attitude angles, enabling the unicycle to perceive and adjust its own attitude in real time, ensuring stability and safety during driving.

[0049] 3. The unicycle path tracking and planning method based on integrated navigation can generate real-time unicycle steering control data according to the GPS data pre-stored in the track, guiding the unicycle to travel along the predetermined trajectory. At the same time, by dynamically adjusting the preset distance threshold, the unicycle can automatically adjust the path point update strategy according to the real-time motion speed, improving the flexibility and adaptability of path planning.

[0050] 4. The unicycle path tracking and planning method based on integrated navigation performs drive optimization operations based on the unicycle control parameters. This technical solution can not only achieve precise control of the unicycle, but also adjust the control parameters in real time according to the actual driving conditions, thereby further improving the performance and driving efficiency of the unicycle. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 is a flowchart of the unicycle path tracking and planning method based on integrated navigation of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0052] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0053] Please refer to Figure 1 as shown, the present invention provides a technical solution:

[0054] It should be noted that in this technical solution, the unicycle body integrates a high-precision MEMS-IMU module (including a three-axis accelerometer and a gyroscope), a dual-frequency GPS receiving module (supporting RTK positioning), an embedded main control unit (TriCore kernel), and a motor drive module. The GPS module and the IMU communicate with the main control unit through the SPI bus, and the data sampling frequencies are set to 10Hz and 1000Hz respectively. The non-volatile memory uses an eeprom chip with a storage capacity of ≥32GB to store the trajectory point sequence and the original sensor data.

[0055] A unicycle path tracking and planning method based on integrated navigation, comprising the following steps:

[0056] Step S1: Use the built-in GPS module of the unicycle to collect and store the path tracking points;

[0057] Step S2: Use the extended Kalman filter method to process the acceleration and angular velocity collected by the IMU unit, and then use Mahony's complementary filter algorithm to update the attitude quaternion in real time and convert it into an attitude angle;

[0058] Step S3: The GPS module receives data in real time and converts the longitude and latitude coordinates of the stored path tracking points into Cartesian coordinates;

[0059] Step S4: Calculate the Euclidean distance and azimuth angle between the current point and the target point using the obtained Cartesian coordinates;

[0060] Step S5: Generate steering angle data based on the attitude angle obtained from the IMU data and the azimuth angle obtained from the GPS data; thereby adjusting the control parameters of the unicycle.

[0061] As a preferred implementation manner of this embodiment, in step S1, the following steps are included:

[0062] Step S11: Through the GPS receiving module integrated in the unicycle, collect three-dimensional geographical coordinate data in real time at a sampling frequency of not less than 10Hz. That is, the GPS module outputs longitude, latitude, and altitude data in the WGS-84 coordinate system at a frequency of 10Hz, and stores them in the circular buffer after CRC verification. The buffer adopts a queue structure with a capacity of 1000 points, and automatically overwrites the earliest data when the data volume exceeds the threshold;

[0063] Step S12: Receive the input of the path key points marked manually through the human-computer interaction device to generate auxiliary positioning data. Specifically, receive the manual marking instruction through the touch screen human-computer interface, overlay and display the marked points with the real-time GPS data, and use the cubic spline interpolation algorithm to generate a smooth auxiliary path;

[0064] Step S13: Establish a circular storage structure with a predefined capacity, perform smoothing filtering on the validity-verified positioning data, and then store it in a non-volatile memory in a floating-point format with a timestamp. Specifically, perform a sliding window mean filter (window length = 5) on the original coordinate data, remove jump values (set thresholds: horizontal error ±3σ, vertical error ±2σ), and the final storage format is <Timestamp, Latitude, Longitude, Altitude>;

[0065] Step S14: Provide a visual trajectory editing interface, perform abnormality detection and manual correction on the stored trajectory point set based on preset data verification rules. Specifically, load the trajectory point set into the PC host software, automatically detect abnormal distances between adjacent points (such as >2m) and sudden changes in heading (>90°), highlight abnormal points, and provide a drag correction function.

[0066] As a preferred implementation of this embodiment, step S2 includes the following steps:

[0067] Step S21: Using an IMU module installed on the unicycle, collect three-dimensional acceleration and angular velocity data in real time at a sampling frequency of not less than 1000 Hz;

[0068] Step S22: Perform multi-stage filtering and fusion processing on the data collected by the IMU module.

[0069] Specifically, step S22 includes:

[0070] S221, using the extended Kalman filter algorithm to perform state estimation on the acceleration and angular velocity data, and output preliminary attitude information including attitude error and angular velocity deviation;

[0071] S222. Based on the preliminary attitude information, iteratively update the quaternion differential equation using the Mahony complementary filtering algorithm, construct the gravity vector observation error in combination with the acceleration data, and correct the quaternion using proportional-integral feedback.

[0072] S223: Convert the corrected attitude quaternion into roll angle, pitch angle and yaw angle, and output them as real-time attitude angles.

[0073] Specifically,

[0074] (1) Extended Kalman filter (EKF) initialization:

[0075] State vector: X = [q0,q1,q2,q3,ω bx ,ω by ,ω bz ] T (quaternion + gyro bias)

[0076] Observation vector: Z = [a x , a y , a z T (Accelerometer data)

[0077] The state X contains two elements, the position p and the velocity v, and the process noise covariance is:

[0078]

[0079] Observation noise covariance

[0080]

[0081] Mahony complementary filtering:

[0082] Gravity error calculation: e = a measured × a estimated (Cross product after normalization)

[0083] PI gain parameters: Kp = 0.5, Ki = 0.001

[0084] The quaternion update frequency is synchronized with the IMU sampling (1 kHz), and the first-order Runge-Kutta integration method is used.

[0085] (2) Attitude angle output:

[0086] The formula for converting quaternion to attitude angle is:

[0087] Roll angle

[0088] Pitch angle θ = arcsin(2(q0q2 - q3q1))

[0089] Yaw angle

[0090] In this embodiment, the step S3 includes the following steps:

[0091] Step S31: Receive the longitude and latitude coordinates of the tracking points of the unicycle in real time through the GPS module;

[0092] Step S32: Store the received longitude and latitude coordinates of the tracking points in the storage module;

[0093] Step S33: Convert the stored longitude and latitude coordinates of the tracking points into Cartesian coordinates.

[0094] Specifically,

[0095] (1) Gauss-Kruger projection formula:

[0096] ​Let the longitude of the central meridian be L0, the longitude of a certain point be L, and the latitude be B. Then the plane coordinates (x, y) are calculated as follows:

[0097] l = L - L0

[0098]

[0099] where η 2 = e' 2 cos 2 B, is the semi-major axis of the ellipsoid and is the first eccentricity.

[0100] (2) Real-time projection processing: Adopt the zonal projection strategy (6° zone), quickly match the central meridian through the look-up table, and retain the coordinate accuracy to the millimeter level (0.001 m).

[0101] In this embodiment, step S4 further includes the following steps:

[0102] Compare the Euclidean distance with a preset distance threshold, and dynamically update the target path point according to the comparison result:

[0103] If the Euclidean distance is greater than the preset distance threshold, maintain the current target path point;

[0104] If the Euclidean distance is less than or equal to the preset distance threshold, update the target path point to the next path point in the pre-stored path sequence.

[0105] The preset distance threshold is dynamically adjusted according to the real-time movement speed of the unicycle. When the speed increases, the threshold increases accordingly. The adjustment of the preset distance threshold satisfies the following relationship:

[0106] Dthreshold = k·v + D0

[0107] where v is the real-time movement speed, k is the proportionality coefficient, and D0 is the minimum safety distance constant.

[0108] Specifically,

[0109] The real-time speed v is obtained by weighted fusion of the GPS differential speed and the IMU integrated speed (weight ratio 7:3);

[0110] The distance threshold Dthreshold = 0.8·v + 0.5 (unit: meter). When v > 5 m / s, Dthreshold ≤ 6 m is limited.

[0111] In addition, the azimuth angle calculates the heading deflection angle through the coordinate difference between the current point and the target point, and corrects the heading in combination with the yaw angle in the real-time attitude angle.

[0112] In this embodiment, step S5 includes the following steps:

[0113] S51. Generate the steering angle data of the unicycle based on the attitude angle data of the IMU and the azimuth angle data of the GPS;

[0114] S52. Adjust the control parameters of the unicycle based on the steering angle data;

[0115] S53. Perform the drive optimization operation of the unicycle based on the adjusted control parameters;

[0116] S54. Implement the path tracking planning and control of the unicycle according to the optimized drive strategy.

[0117] Specifically,

[0118] Desired heading angle ψ target = arctan2(y next - y current , x next - x current )

[0119] Yaw error Δψ = ψ target - ψ IMU

[0120] Steering angle δ = K p ·Δψ + K d ·(Δψ - Δψ prev ) / Δt (K P = 0.6, K d = 0.2)

[0121] Abnormal handling mechanism

[0122] Switch to the pure inertial navigation mode when the GPS fails, and trigger the emergency brake if the duration exceeds 2 seconds;

[0123] Automatically enter the closed-loop holding state after the path point sequence traversal is completed, and maintain the end point position error < 0.2 m.

[0124] This technical solution also provides a unicycle path tracking planning system based on integrated navigation, including:

[0125] A GPS operation unit, which is arranged inside the unicycle and is used for collecting three-dimensional geographic coordinate data and generating path tracking positioning data;

[0126] A storage unit, which is used for storing the data collected by the GPS operation unit and the target path;

[0127] An IMU operation unit, which is used for collecting the self-state of the unicycle and generating attitude angle data;

[0128] The integrated navigation unit is communicatively connected to the GPS operation unit and the IMU operation unit respectively, and is used to perform integrated operations on the collected GPS / IMU data to generate steering angle data;

[0129] The unicycle control module is communicatively connected to the integrated navigation unit and is used to make decisions according to the steering signal so as to complete the body movement control for path tracking.

[0130] The unicycle path tracking planning method of this technical solution is applicable to unicycles lacking visual sensors and precision inertial sensors outdoors, especially those that need to complete the path tracking task of positioning points within a region. By processing GPS / IMU data, integrated navigation is achieved, thereby planning the overall driving path of the unicycle, making the unicycle path tracking planning efficient, and being able to effectively correct the deviation of the driving distance of the unicycle. Then, by continuously optimizing and adjusting the control parameters of the unicycle, the driving distance of the unicycle is more in line with the planned distance. This technical solution provides an efficient and accurate solution for the path tracking planning of unicycles by making full use of the advantages of integrated navigation technology, and has broad application prospects and market value.

[0131] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments. The above embodiments and the descriptions in the specification are only preferred examples of the present invention and are not used to limit the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements all fall within the scope of the present invention claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A unicycle path tracking and planning method based on integrated navigation, characterized in that It includes the following steps: Step S1: Use the built-in GPS module of the unicycle to collect and store the tracking point positions; Step S2: Use the extended Kalman filter method to process the acceleration and angular velocity collected by the IMU unit, and then use Mahony's complementary filter algorithm to update the attitude quaternion in real time and convert it into attitude angles; Step S3: The GPS module receives data in real time and converts the longitude and latitude coordinates of the stored tracking point positions into Cartesian coordinates; Step S4: Calculate the Euclidean distance and azimuth angle between the current point and the target point using the obtained Cartesian coordinates; Step S5: Generate steering angle data based on the attitude angles obtained from the IMU data and the azimuth angle obtained from the GPS data; Thereby, adjust the control parameters of the unicycle.

2. The unicycle path tracking and planning method based on integrated navigation according to claim 1, wherein In the said Step S1, it includes the following steps: Step S11: Through the GPS receiving module integrated in the unicycle, collect three-dimensional geographic coordinate data in real time at a sampling frequency not lower than 10Hz; Step S12: Receive the input of the path key points marked manually through the human-computer interaction device and generate auxiliary positioning data; Step S13: Establish a circular storage structure with a predefined capacity, perform smoothing filtering processing on the positioning data after validity verification, and store it in the non-volatile memory in a floating-point format with a timestamp; Step S14: Provide a visual trajectory editing interface, and perform anomaly detection and manual correction on the stored trajectory point set based on the preset data verification rules.

3. The unicycle path tracking and planning method based on integrated navigation according to claim 1, wherein The said Step S2 includes the following steps: Step S21: Through the IMU module set on the unicycle, collect three-dimensional acceleration and angular velocity data in real time at a sampling frequency not lower than 1000Hz; Step S22: Perform multi-level filtering and fusion processing on the data collected by the IMU module.

4. The unicycle path tracking and planning method based on integrated navigation according to claim 3, characterized in that The said Step S22 includes: S221: Use the extended Kalman filter algorithm to perform state estimation on the acceleration and angular velocity data, and output preliminary attitude information including attitude error and angular velocity deviation; S222: Based on the preliminary attitude information, use the Mahony complementary filter algorithm to iteratively update the quaternion differential equation, and construct the gravity vector observation error in combination with the acceleration data, and correct the quaternion through proportional-integral feedback; S223: Convert the corrected attitude quaternion into roll angle, pitch angle and yaw angle, and output them as real-time attitude angles.

5. The unicycle path tracking and planning method based on integrated navigation according to claim 1, wherein, The said Step S3 includes the following steps: Step S31: Receive the longitude and latitude coordinates of the tracking point positions of the unicycle in real time through the GPS module; Step S32: Store the received longitude and latitude coordinates of the tracking point positions in the storage module; Step S33: Convert the stored longitude and latitude coordinates of the tracking point positions into Cartesian coordinates.

6. The unicycle path tracking and planning method based on integrated navigation according to claim 1, wherein, The said Step S4 further includes the following steps: Compare the Euclidean distance with a preset distance threshold, and dynamically update the target path point according to the comparison result: If the Euclidean distance is greater than the preset distance threshold, maintain the current target path point; If the Euclidean distance is less than or equal to the preset distance threshold, update the target path point to the next path point in the pre-stored path sequence.

7. The unicycle path tracking and planning method based on integrated navigation according to claim 6, characterized in that The preset distance threshold is dynamically adjusted according to the real-time movement speed of the unicycle. When the speed increases, the threshold increases accordingly. The adjustment of the preset distance threshold satisfies the following relational expression: Dthreshold = k·v + D0 where v is the real-time movement speed, k is the proportionality coefficient, and D0 is the constant of the minimum safety distance.

8. The unicycle path tracking and planning method based on integrated navigation according to claim 1, wherein The azimuth angle calculates the course deflection angle through the coordinate difference between the current point and the target point, and corrects the course in combination with the yaw angle in the real-time attitude angle.

9. The unicycle path tracking and planning method based on integrated navigation according to claim 1, wherein The step S5 includes the following steps: S51. Generate the steering angle data of the unicycle based on the attitude angle data of the IMU and the azimuth angle data of the GPS; S52. Adjust the control parameters of the unicycle based on the steering angle data; S53. Execute the drive optimization operation of the unicycle based on the adjusted control parameters; S54. Realize the path tracking planning and control of the unicycle according to the optimized drive strategy.

10. A unicycle path tracking and planning system based on integrated navigation, characterized in that, Including: A GPS operation unit, arranged inside the unicycle, for collecting three-dimensional geographical coordinate data and generating path tracking positioning data; A storage unit for storing the data collected by the GPS operation unit and the target path; An IMU operation unit for collecting the self-state of the unicycle and generating attitude angle data; A combined navigation unit, communicatively connected to the GPS operation unit and the IMU operation unit respectively, for performing integrated operations on the collected GPS / IMU data and generating steering angle data; A unicycle control module, communicatively connected to the combined navigation unit, for making decisions according to the steering signal so as to complete the body movement control for path tracking.

Images