AUTOMATIC TRACKING AND AUTONOMOUS NAVIGATION SYSTEM FOR THE COLLECTION AND TRANSPORT OF FRUITS AND VEGETABLES

Abstract

The present invention discloses an automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables, falling within the technical field of navigation with tracking. The system comprises a control center, which communicates with an environmental perception module, a target recognition and localization module, a relative heading deviation angle detection module, a deviation angle correction decision module, a propulsion and steering execution module, an autonomous navigation planning module, and a human-machine interaction module. These modules are interconnected by electrical signals.The present invention, thanks to its autonomous navigation and obstacle avoidance functions, enables the following vehicle to automatically and accurately follow a guide vehicle or autonomously plan a route, thus accomplishing complex transport tasks without human intervention. Furthermore, the system is capable of rapidly planning an optimal route based on the target and current positions, reducing transport time and improving efficiency. It also features high-precision localization and attitude estimation capabilities, enabling real-time determination of the following vehicle's position and attitude, thereby ensuring stable driving in complex environments.

Description

Title of the invention: AUTOMATIC TRACKING AND AUTONOMOUS NAVIGATION SYSTEM FOR THE COLLECTION AND TRANSPORT OF FRUITS AND VEGETABLES technical field

[0001] The present invention relates to the technical field of tracking and navigation, and more particularly to an automatic tracking and autonomous navigation system intended for the collection and transport of fruits and vegetables. TECHNICAL BACKGROUND

[0002] With technological advancements and the accelerating modernization of agriculture, agricultural production methods are gradually shifting towards intelligence and automation. Traditional fruit and vegetable harvesting and transport processes require a large amount of labor for handling and tracking, resulting in not only high labor intensity but also low efficiency. Furthermore, with increasing consumer demands for fruit and vegetable quality, coupled with the expansion of agricultural production scales, the demand for an intelligent and efficient fruit and vegetable harvesting and transport system continues to grow.The implementation of an automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables, through the implementation of automatic tracking and autonomous navigation of a follower vehicle, would significantly improve the efficiency of agricultural production, reduce labor costs, and meet the development trend of modern agriculture.

[0003] During trajectory tracking and automatic following processes, an insufficient field of view can affect the heading deviation angle of the following vehicle relative to the guide vehicle. This can lead to the following vehicle deviating from the predefined direction, or even losing the target and making realignment difficult. Therefore, the problem to be solved is to adjust the relative heading angle between the following and guide vehicles and to achieve continuous detection of the deviation angle in order to improve the system's control accuracy. To this end, the present invention proposes an automatic tracking and autonomous navigation system for the harvesting and transport of fruits and vegetables.

[0004] SUMMARY OF THE INVENTION

[0005] The object of the present invention is to provide an automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables, in order to solve the problems mentioned in the aforementioned prior art.

[0006] To solve these technical problems, the technical solution adopted by the present invention is as follows:

[0007] an automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables, comprising a control center, said control center being connected by electrical communication to the following modules: environmental perception module, target recognition and localization module, relative heading deviation angle detection module, deviation angle correction decision module, propulsion and steering execution module, autonomous navigation planning module and a human-machine interaction module, said modules are connected to each other by electrical signals;

[0008] The environmental perception module is intended to collect environmental information around the following vehicle for collecting and transporting fruits and vegetables, including the position, speed, direction of the guide vehicle, as well as surrounding obstacles and road limits, providing accurate and real-time perception data, serving as a basis for subsequent decisions and controls;

[0009] The target recognition and localization module, based on data acquired by the environmental perception module, identifies and localizes the spatial coordinates and attitude of the guide vehicle, distinguishes the guide vehicle from other disturbing objects, continuously locks onto the guide vehicle to ensure clarity and accuracy of the target to be followed, thus improving the accuracy of the follower vehicle's tracking;

[0010] The relative heading deviation angle detection module compares the position and attitude data of the following vehicle and the guide vehicle, analyzes the relative heading deviation angle of the following vehicle with respect to the guide vehicle, performing continuous detection of the deviation angle, providing an accurate basis for adjusting the direction of the following vehicle;

[0011] said deviation angle correction decision module, based on the analysis results of the relative heading deviation angle detection module, judges whether the current tracking is normal and issues alerts in case of deviation tendency, updating the steering adjustment strategy of the following vehicle;

[0012] the propulsion and steering execution module, which receives strategic control instructions via the deviation angle correction decision module, controls the steering, acceleration and braking actions of the following vehicle, achieving precise control, thereby improving the accuracy and speed of response of the system;

[0013] the autonomous navigation planning module, which, in the absence of the guide vehicle or in the event of loss of the latter's signal, relies on autonomous navigation technology to plan the optimal trajectory of the following vehicle from its current position to the target position, taking into account obstacles, guiding the following vehicle forward so that it reaches its destination safely and efficiently, ensuring continuous operation even in complex environments;

[0014] the human-machine interaction module, providing the operator with a visual interface displaying the operating status of the system, including the position of the following vehicle and the guide vehicle, the relative angle of deviation of heading, the possible presence of anomalies, allowing the operator to parameterize, intervene manually and control the system, thus facilitating flexible adjustments in the event of a particular situation.

[0015] The further improvement of the present device lies in the fact that, in the environmental perception module, the process of collecting environmental information is as follows:

[0016] Various types of sensors, such as cameras and laser radar, are deployed on the follower vehicle intended for collecting fruits and vegetables, in order to capture information from the surrounding environment, including the relative position, speed and direction of the guide vehicle, the distance, shape and size of surrounding obstacles, as well as the contour and position of road edges;

[0017] Images are collected and processed via cameras to identify traffic lanes and obstacles ahead, light beams are emitted by the laser radar and reflected signals are received synchronously, the return time is used to measure the distance to obstacles, a point map is established to determine the shape, size and distance of obstacles;

[0018] The data relating to the collected environmental information are transmitted to the vehicle's electronic control unit for preprocessing, this preprocessing including noise suppression, filtering and data format conversion in order to improve the accuracy and usability of the data. Furthermore, the data collected by different sensors are calibrated and synchronized to ensure temporal and spatial consistency;

[0019] Data from different sensors are then merged to obtain complete and accurate environmental information. From the merged data, the position, speed, and acceleration of obstacles, as well as the geometric characteristics of the road boundaries, are extracted, thus constituting a set of characteristic data.

[0020] The further improvement of the present invention lies in the target recognition and localization module, where the process of distinguishing between the guide vehicle and other interfering objects is as follows:

[0021] From the set of characteristic data obtained by the environmental perception module, each characteristic data point is examined individually. Image processing technology is used to extract the dimension, shape, and color characteristics of the guide vehicle, thus facilitating its recognition and localization;

[0022] using machine learning algorithms, the features extracted from the guide vehicle are compared to a previously trained reference model, candidate targets corresponding to the characteristics of the guide vehicle are selected from the matching results, and extraneous objects not corresponding to the characteristics of the guide vehicle are discarded by comparison with the obstacle characteristics provided by the environmental perception module;

[0023] Based on the filtered characteristics of the guide vehicle, image processing technology based on image alignment is used to calculate the spatial coordinates of the guide vehicle; shape and size information of the guide vehicle are combined to estimate attitude data such as orientation and tilt angle;

[0024] The spatial coordinates, orientation, and tilt angle of the guide vehicle are associated with the point cloud data obtained by the sensors to improve localization accuracy. Furthermore, a tracking algorithm (Kalman filter) is used to ensure continuous tracking and locking of the guide vehicle, guaranteeing that the following vehicle can always accurately follow the guide vehicle. Based on real-time data provided by the environmental perception module, the position and attitude of the guide vehicle are dynamically updated to adapt to changes in the environment.

[0025] The further improvement of the present invention lies in the formula for calculating the spatial coordinates of the guide vehicle, which is as follows:

[0026] / I \ 2 { ^y < j P=(x,y) = \jd +(,535))

[0027] where P represents the spatial position of the guide vehicle, x and y respectively denote the coordinates of the guide vehicle in the horizontal plane, d is the horizontal distance between the guide vehicle and the following vehicle, Ay is the vertical distance between the guide vehicle and the following vehicle, 9 is the angle of inclination of the vehicle guide relative to the horizontal plane, ymax is the maximum allowed value in the vertical direction, used to limit the range of y;

[0028]

[0029] The formula for calculating the orientation of said guide vehicle is as follows: Hd = atan2(~y) ;

[0030] where Hd represents the orientation of the guide vehicle, atan2 the arctangent function taking into account the quadrants, used to calculate the orientation angle of the guide vehicle, A y the vertical distance between the guide vehicle and the follower vehicle, and d the horizontal distance between the guide vehicle and the follower vehicle;

[0031] The formula for calculating the angle of inclination of the guide vehicle is as follows:

[0032] Ti = arctan^)"

[0033] where Ti represents the angle of inclination of the guide vehicle, A h the variation in height of the guide vehicle, d the horizontal distance between the guide vehicle and the follower vehicle, and arctan the arctangent function used to calculate the angle of inclination of the guide vehicle.

[0034] The further improvement of the present invention lies in the relative heading deviation angle detection module, where the continuous deviation angle detection process is carried out as follows:

[0035] The environmental perception module acquires position and attitude data from the following vehicle and the guide vehicle. The acquired position and attitude data are preprocessed, including denoising, filtering, and data alignment, to improve data accuracy and reliability. From the preprocessed data, the characteristic information necessary for calculating the relative deviation angle is extracted. The position data are GPS coordinates, while the attitude data include the velocity vector (magnitude and direction) and the steering angle (vehicle orientation);

[0036] The heading angle of the following vehicle, obtained from the steering angle data, is compared to the heading angle of the guide vehicle to calculate their difference, which constitutes the relative deviation angle, according to the following formula: &0 = min(\0 Y - 0 G |, 360°- |0 y - 0 G |) ; where A 0 is the relative deviation angle, 0y the heading angle of the guide vehicle, and 0G the heading angle of the follower vehicle;

[0037] A timer is configured to trigger an update and calculation of the data every 500 milliseconds, thus periodically updating the position and attitude data of the follow vehicle and the guide vehicle, repeating the process of comparing heading angles in order to achieve continuous detection of the deviation angle.

[0038] The further improvement of the present invention lies in the deviation angle correction decision module, where the deviation trend alert process is as follows:

[0039] normal ranges and alert thresholds for the relative heading deviation angle are defined, the normal range being ±5°, the alert threshold ±10°, and real-time analysis results from the relative heading deviation angle detection module are received, including the current value and historical data for the relative heading deviation angle;

[0040] based on the received relative heading deviation angle data, it is determined whether the following vehicle's tracking situation relative to the guide vehicle is within the normal range; if so, the tracking situation is considered normal and monitoring continues; if the normal range is exceeded, the alert and deviation processing is triggered;

[0041] the degree of deviation is determined according to the absolute value of the angle of deviation of the relative heading, and by combining the data of the angle of deviation of the relative heading at several successive times, a deviation warning coefficient is calculated, allowing to analyze the tendency of deviation of the angle of deviation of the relative heading;

[0042] depending on the results of the analysis of the deviation trend and the calculated deviation alert coefficient, if the deviation alert coefficient approaches or exceeds the alert threshold, and if the deviation trend persists or worsens, the alert mechanism is triggered, and a notification is transmitted to the driver via an audible or visual signal;

[0043] Based on the deviation warning coefficient and the results of the deviation trend analysis, the following vehicle's steering adjustment strategy is updated to correct the deviation and keep the following vehicle on the correct trajectory. More specifically, depending on the relative deviation angle and deviation trend, the direction in which the following vehicle should adjust (left, right, or maintain) is determined. By combining the deviation warning coefficient with a predefined adjustment strategy, the angle or speed change required for the adjustment is calculated, and then the adjustment command is sent to the following vehicle's propulsion and steering execution module to perform the steering adjustment operation.

[0044] The further improvement of the present invention lies in the formula for calculating the deviation warning coefficient, which is as follows:

[0045] 1 / D = ^^7) X min\ -5— ' X

[0046] Where D is the deviation alert coefficient, A the current value of the relative deviation angle, Qq the predefined alert threshold, Πthe standard deviation used to adjust the sensitivity of the coefficient, A the heading angle data relative to the 1 continuous time point, n the number of consecutive time points, A 0max the maximum value of the relative heading angle at consecutive times, A the minimum value of the relative heading angle at consecutive times, the value of D is limited between 0 and 1, where 0 means no deviation, and 1 means severe deviation.

[0047] A further improvement of the present invention lies in the propulsion and steering execution module, the process of which controls the following vehicle is carried out as follows:

[0048] The propulsion and steering execution module receives, via a communication interface, the adjustment strategy updated by the deviation angle correction decision module, extracts the control instructions from this adjustment strategy, and then analyzes these instructions;

[0049] based on the results of the analysis of the control instructions, the specific control parameters of the steering angle, acceleration or braking force are extracted and, according to the results of the analysis, the type of action to be performed is determined, namely steering, acceleration or braking;

[0050] For the execution of the steering action, if the control instruction corresponds to a steering action, the module commands the steering mechanism according to the steering angle parameter; during the steering process, sensors monitor the steering angle and speed in real time to ensure the precision and stability of the action. For the execution of the acceleration action, if the control instruction corresponds to an acceleration action, the module controls the engine output power according to the acceleration parameter to achieve acceleration; during this process, sensors monitor the vehicle speed and acceleration in real time to ensure the smoothness and safety of the action.For the execution of the braking action, if the control instruction corresponds to a braking action, the module commands the braking system according to the braking force parameter in order to generate the appropriate force to slow down or stop the vehicle; during this process, sensors monitor the speed and deceleration in real time to ensure the speed and reliability of the action. Steering, acceleration and braking actions can be executed simultaneously or sequentially, in order to achieve precise control of the following vehicle.

[0051] During the execution of steering, acceleration, or braking actions, the sensors monitor the actual state of the vehicle in real time, which is compared to the planned targets in order to evaluate the effectiveness of the execution. The execution results (angle Actual steering angle, actual speed, and actual acceleration are sent via the communication interface to the steering angle correction decision module. This steering angle correction decision module then adjusts the control strategy based on the received results, thus improving control accuracy and system response time.

[0052] A further improvement of the present invention lies in the autonomous navigation planning module, where the navigation planning process guiding the following vehicle is carried out as follows:

[0053] based on environmental information collected by the following vehicle, this is processed and then converted into a grid-type map representation, allowing an intuitive visualization of obstacles and passable areas in the environment by this grid-type map;

[0054] The visual SLAM algorithm is used to estimate the current position of the following vehicle based on environmental information and the known map, while the inertial measurement unit (IMU) sensor provides real-time attitude information for the following vehicle;

[0055] Based on sensor data and mapping information, obstacles in the environment are detected in real time. Depending on the position, shape, and size of the obstacles, an avoidance strategy is developed, including bypassing them or stopping to wait. A cost function related to the obstacles is integrated into the trajectory planning to assess the impact of the obstacles on the path, thus making it possible to avoid them and optimize the trajectory;

[0056] Depending on the target's position and the current position, a search algorithm (algorithm A*) is used to search the map for the shortest path that avoids obstacles and complies with traffic rules. Based on the overall path trajectory planning, dynamic adjustments and optimizations are made according to the real-time state of the following vehicle and changes in the surrounding environment, in order to ensure smooth driving of the following vehicle;

[0057] According to the results of the trajectory tracking and the actual state of the following vehicle, control instructions are generated to command steering, acceleration and braking actions, in order to ensure precise control.

[0058] A further improvement of the present invention lies in the expression of the cost function related to obstacles, which is as follows:

[0059] / Where? \ C o = exp\ - —p-- ) x min^ 1, )

[0060] where Co is the cost function related to obstacles on the path, which is used to evaluate the impact of obstacles on the path, dj the distance between the following vehicle and the j obstacle, the standard deviation of the distance of the obstacle, which is used to adjust the sensitivity of the cost function to the distance, m the total number of obstacles, & the constant used to adjust the curvature of the exponential function, this affects the degree to which the distance of the obstacle affects the cost function, 9max the maximum angular deviation allowed in the path, 6j the angular deviation between the following vehicle and the j obstacle, the range of values ​​of Co is limited between 0 and 1, where 0 means that the path completely avoids the obstacle and 1 means that the path passes directly through the obstacle.

[0061] Thanks to the adoption of the present technical solution, the invention achieves the following technological advances compared to the state of the art:

[0062] 1. The present invention proposes an automatic tracking and navigation system Autonomous for the collection and transport of fruits and vegetables. Thanks to its autonomous navigation and obstacle avoidance capabilities, the follower vehicle can automatically and precisely track the lead vehicle or autonomously plan a route, thus accomplishing complex transport tasks without human intervention. Furthermore, based on the target and current positions, the system can quickly plan an optimal route, reducing transport time and improving efficiency. It also features high-precision localization and attitude estimation capabilities, providing real-time position and attitude data for the follower vehicle, ensuring stable driving in complex environments.

[0063] 2. The present invention proposes an automatic tracking and navigation system This autonomous system for collecting and transporting fruits and vegetables uses precise trajectory planning and control instructions to allow the following vehicle to travel smoothly along the planned route. This system avoids dangerous situations resulting from abrupt maneuvers such as sharp turns or sudden acceleration. Furthermore, by monitoring and analyzing real-time information about the surrounding environment, it can quickly detect and avoid potential obstacles, thus preventing collisions.

[0064] BRIEF DESCRIPTIONS OF THE FIGURES

[0065] In order to better illustrate the technical solution implemented in the embodiments of this application or in the prior art, the figures used in said embodiments are briefly described below. It is evident that the figures described below represent only certain embodiments of the This invention is presented. Other figures can be obtained from these by a professional in the relevant technical field.

[0066] [Fig. 1] is a diagram of the modular structure of the system according to the present invention;

[0067] [Fig.2] is a flowchart illustrating the implementation of continuous detection of the deviation angle according to the present invention;

[0068] [Fig.3] is a flowchart illustrating the early warning system for deviation trends according to the present invention;

[0069] [Fig.4] is a flowchart illustrating the autonomous navigation planning to guide the advance of the following vehicle according to the present invention. Description of the invention

[0070] In order to better understand the objectives, technical solutions, and advantages of the embodiments of the present invention, a clear and complete description of the technical solutions of said embodiments will be given below with reference to the figures of the embodiments of the present invention. It is evident that the embodiments described constitute only a part of the embodiments of the present invention and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by a person skilled in the art without inventive effort also fall within the scope of protection of the present invention.

[0071] As illustrated in Figures 1 and 2, embodiment 1 of the present invention provides an automatic tracking and autonomous navigation system for collecting and transporting fruits and vegetables, comprising a control center. The control center is connected in communication with the following modules: an environmental perception module, a target recognition and localization module, a relative heading deviation angle detection module, a deviation angle correction decision module, a propulsion and steering execution module, an autonomous navigation planning module, and a human-machine interaction module. The modules are interconnected by electrical signals;

[0072] The environmental perception module is designed to gather environmental information surrounding the following vehicle used for collecting and transporting fruits and vegetables. This information includes the position, speed, and direction of the guide vehicle, as well as data relating to surrounding obstacles and roadway boundaries. It provides accurate, real-time perception data, which serves as the basis for subsequent decision and control modules. Various types of sensors, such as cameras and laser radar, are Deployed on the following vehicle, sensors capture information about the surrounding environment. These sensors detect the relative position, speed, and direction of the guide vehicle, the distance, shape, and size of nearby obstacles, as well as the outline and position of road boundaries. Cameras are used for image acquisition and processing to identify traffic lanes and obstacles ahead. Laser radar emits light beams and simultaneously receives reflected signals, allowing the distance to obstacles to be measured based on the time difference between emission and reception. This generates a point cloud that helps determine the shape, size, and distance of the obstacles.The environmental data thus collected is transmitted to the vehicle's electronic control unit for preprocessing, including noise reduction, filtering, and data format conversion, to improve accuracy and usability. Furthermore, data from different sensors are calibrated and synchronized to ensure temporal and spatial consistency. Data from the various sensors is then fused, providing comprehensive and reliable environmental information. From the fused data, the following characteristics are extracted: the position, speed, and acceleration of obstacles, as well as the geometric characteristics of road boundaries, resulting in a set of characteristic data.

[0073] The target recognition and localization module, based on data obtained by the environmental perception module, identifies and locates the spatial position and attitude of the guide vehicle, distinguishing it from other interfering objects. This module maintains continuous lock onto the guide vehicle to ensure that the following vehicle clearly and unambiguously targets the object to be tracked, thereby improving tracking accuracy. From the set of characteristic data provided by the environmental perception module, each characteristic is individually analyzed. Image processing technology is used to extract features related to the size, shape, and color of the guide vehicle, thus facilitating its recognition and localization.Using machine learning algorithms, the features extracted from the guide vehicle are compared to a pre-trained feature model of the guide vehicle. From the results of this matching, candidate targets conforming to the guide vehicle's features are selected. A comparison of the candidate target features with those of obstacles provided by the environmental perception module is performed to eliminate disruptive objects incompatible with the guide vehicle's features, such as trees, buildings, other vehicles, etc. The model of... The guide vehicle's characteristics are pre-trained using a large number of images collected from different angles, lighting conditions, and backgrounds, ensuring the inclusion of various guide vehicle types in the dataset, which improves the model's generalization capabilities. The collected images are annotated with bounding frames and guide vehicle category labels. They undergo normalization, clipping, and rotation to optimize the efficiency and performance of the model training. The annotated images are then fed into a convolutional neural network for training. During this process, a backpropagation algorithm is used to update the model's weights and biases. The model's performance is evaluated using a validation set.Based on the results, the model structure, its parameters, or the data preprocessing methods are adjusted to improve performance. The trained model is then evaluated on a test suite to ensure reliable recognition capabilities under real-world conditions. Finally, the trained model is deployed in the automatic tracking system for the guide vehicle used for fruit and vegetable collection and transport. This system is integrated with the environmental perception module to enable real-time recognition and localization of the guide vehicle. Based on the selected guide vehicle characteristics, image registration techniques are used to calculate the guide vehicle's spatial position. By combining information about the guide vehicle's shape and size, its attitude, including its orientation and tilt angle, is estimated.The position, orientation, and tilt angle information of the guide vehicle is combined with point cloud data obtained from the sensors to improve localization accuracy. A tracking algorithm, such as the Kalman filter, is used to ensure continuous tracking and locking of the guide vehicle, guaranteeing that the following vehicle always accurately tracks the guide vehicle. Finally, based on real-time data provided by the environmental perception module, the position and attitude of the guide vehicle are dynamically updated to adapt to changes in the environment.

[0074] Furthermore, the formula for calculating the spatial position of the guide vehicle is as follows:

[0075] / I \ 2 { ^y < j ^=(^.7) = (^+(7^) ■“W^y+tssêj) /

[0076] where P represents the spatial position of the guide vehicle, x and y respectively denote the coordinates of the guide vehicle in the horizontal plane, d is the horizontal distance between the guide vehicle and the follower vehicle, Ay is the vertical distance between the guide vehicle and the follower vehicle, 9 is the angle of inclination of the vehicle guide relative to the horizontal plane, ymax is the maximum allowed value in the vertical direction, used to limit the range of y;

[0077] The formula for calculating the orientation of the guide vehicle is as follows: [°° 78 1 Hd = atan2(%y

[0079] where Hd represents the orientation of the guide vehicle, atan2 the arctangent function taking into account the quadrants, used to calculate the orientation angle of the guide vehicle, A y the vertical distance between the guide vehicle and the follower vehicle, and d the horizontal distance between the guide vehicle and the follower vehicle;

[0080] The formula for calculating the angle of inclination of the guide vehicle is as follows:

[0081] y} = ar c £an ( ;

[0082] where Ti represents the angle of inclination of the guide vehicle, A h the variation in height of the guide vehicle, d the horizontal distance between the guide vehicle and the follower vehicle, arctan the arctangent function used to calculate the angle of inclination;

[0083] The relative heading deviation angle detection module compares the position and posture data of the following vehicle and the guide vehicle to analyze the heading deviation angle of the following vehicle relative to the guide vehicle, thus achieving continuous detection of the deviation angle and providing a precise basis for adjusting the direction of the following vehicle. The position and posture data of the following and guide vehicles are obtained via the environmental perception module. This data is then preprocessed (including denoising, filtering, and data alignment) to improve its accuracy and reliability. From the preprocessed data, the characteristic information necessary for calculating the relative heading deviation angle is extracted.Position data is expressed as GPS coordinates, while posture data includes velocity vectors (magnitude and direction) and steering angle (vehicle orientation). The heading angle of the following vehicle, obtained from the steering angle data, is compared to that of the lead vehicle to calculate their difference, corresponding to the relative heading deviation angle. The formula for calculating the relative heading deviation angle is as follows: A 6 = min( |0 y - 0 G |, 360°- |0y- 0 G | ) ; A 6 represents the relative heading deviation angle, QY constant the heading angle of the guide vehicle, Qg is the heading angle of the follower vehicle. A timer is configured to trigger the data update and calculation process every 500 milliseconds. The position and posture data of the follower and guide vehicles are thus regularly updated, allowing for repeated comparison of heading angles and continuous detection of the deviation angle;

[0084] The deviation angle correction decision module, based on the analysis results of the relative heading deviation angle detection module, evaluates whether the current following situation is normal and issues an alert in case of a tendency to deviation, then updates the steering adjustment strategy of the following vehicle;

[0085] The propulsion and steering execution module, according to the strategy transmitted by the deviation angle correction decision module, issues control instructions, and controls the steering, acceleration and braking actions of the following vehicle, in order to achieve precise control of the following vehicle, and improve the control accuracy as well as the response speed of the system;

[0086] The autonomous navigation planning module, in the absence of a guide vehicle or in the event of loss of the guide vehicle signal, relies on autonomous navigation technology to plan an optimal path from the current position of the following vehicle to the target position, taking into account surrounding obstacles, thus guiding the following vehicle forward so that it reaches its destination safely and efficiently, ensuring continuity of operation even in complex environments;

[0087] The human-machine interaction module provides the operator with a visual interface displaying the system's operating status, including the positions of the following and guide vehicles, the relative heading deviation angle, and information regarding any anomalies. It allows the operator to adjust the necessary parameters and manually intervene in the control, thus facilitating flexible system adjustments in specific situations.

[0088] As illustrated in Figures 3 and 4, based on embodiment 1, embodiment 2 of the present invention proposes a solution#: preferably, in the deviation angle correction decision module, the alert process in case of a tendency to deviation is defined#:

[0089] A normal range and an alert threshold are set for the relative deviation angle, with the normal range being ±5° and the alert threshold ±10°. Real-time analysis results are then received from the relative heading deviation angle detection module, including the current value and historical data for the relative deviation angle. Based on the received data, it is determined whether the following vehicle's position relative to the guide vehicle is within the normal range. If so, the following situation is normal and monitoring continues. If the value exceeds the normal range, a deviation alert and management process is triggered. The degree of deviation is determined based on the absolute value of the relative deviation angle, and then a deviation alert coefficient is calculated from the data over several successive time points to analyze the deviation trend.By combining the results of the trend analysis and the calculated coefficient, if this .

[0090]

[0091]

[0092]

[0093]

[0094] If the coefficient approaches or exceeds the warning threshold, and the tendency to deviate persists or worsens, the warning mechanism is triggered, alerting the driver with audible and visual signals. Based on the warning coefficient and trend analysis, the following vehicle's steering adjustment strategy is updated to correct the deviation and keep the following vehicle on the correct trajectory. Depending on the relative angle and the tendency to deviate, the adjustment direction (left, right, or hold) is determined, and then the angle or speed variation to be applied is calculated based on the warning coefficient and the predefined adjustment strategy. The adjustment instruction is transmitted to the drive and steering control module for execution. Furthermore, the formula for calculating the deviation alert coefficient is as follows: D = X min1'--5--' X where D is the deviation warning coefficient, A the current value of the relative deviation angle, Qq the predefined warning threshold, a the standard deviation used to adjust the coefficient's sensitivity, a0j the relative heading angle data at the first continuous time point, n the number of consecutive time points, A0max the maximum value of the relative heading angle at consecutive times, A0min the minimum value of the relative heading angle at consecutive times. The value of D is limited between 0 and 1, where 0 means no deviation, and 1 a severe deviation requiring immediate warning. When the relative heading deviation angle A approaches or exceeds the warning threshold 00, the value of D will tend towards 1, indicating that immediate warning is necessary. When the absolute value of A 0D is small and the angle of deviation does not change much over several consecutive times, the value of D will be small, indicating that the degree of deviation is small; In the propulsion and steering execution module, the process for implementing control of the following vehicle is as follows: The propulsion and steering execution module receives, via the communication interface, the adjustment strategy updated by the steering angle correction decision module, extracts the control instructions from this strategy, and then analyzes these instructions. Based on the analysis results, it extracts the specific control parameters, such as steering angle, acceleration, or braking force, and determines the type of action to execute: steering, acceleration, or braking. For steering, if the control instruction corresponds to a steering action, the module commands the steering mechanism according to the steering angle parameter; during the process For steering, sensors monitor the steering angle and speed in real time to ensure precision and stability. For acceleration, if the control instruction corresponds to acceleration, the module controls the engine output power according to the acceleration parameter to achieve acceleration; during this process, sensors monitor the vehicle speed and acceleration in real time to ensure smooth and safe operation. For braking, if the control instruction corresponds to braking, the module controls the braking system according to the braking force parameter to generate the appropriate braking force to slow down or stop the vehicle; during this process, sensors monitor the vehicle speed and deceleration in real time to ensure the speed and reliability of the operation.Steering, acceleration, and braking actions can be executed simultaneously or sequentially to achieve precise control of the following vehicle. During the execution of steering, acceleration, or braking actions, the vehicle's actual state is monitored in real time by sensors and compared to the intended target to assess the effectiveness of the execution; the execution results (actual steering angle, actual speed, actual acceleration) are transmitted to the steering angle correction decision module via the communication interface. This steering angle correction decision module then adjusts the control strategy based on the results obtained to improve control accuracy and system response speed.

[0095] In the autonomous navigation planning module, the process of guiding the following vehicle is as follows:

[0096] Based on environmental information collected by the following vehicle, this data is processed and converted into a grid-type map (raster map), visually representing obstacles and accessible areas. The visual SLAM algorithm is used to estimate the current position of the following vehicle based on environmental information and a known map. Simultaneously, the inertial measurement unit (IMU) provides real-time attitude information for the following vehicle. Using sensor data and map information, the system detects obstacles in the environment in real time. Depending on the position, shape, and size of the obstacles, an avoidance strategy is formulated, including bypass maneuvers or temporary stops.An obstacle-related cost function is integrated into the trajectory planning to assess the influence of obstacles and thus optimize the route while avoiding them. Based on the current and target positions, a search algorithm, such as the A* algorithm, is used to plan the shortest path on the map while respecting traffic rules and bypassing obstacles. Based on this overall trajectory planning, dynamic adjustments are made according to the real-time status of the following vehicle and changes in the environment, in order to ensure smooth driving. Depending on the results of the tracking of based on the trajectory and real-time status of the following vehicle, control instructions are generated to command the steering, acceleration and braking actions of the following vehicle, thus achieving precise control;

[0097] Furthermore, the cost function related to obstacles is expressed as follows:

[0098] / y™ (dd21 C o = exp\ - 1 x min J1, j

[0099] where Co is the cost function related to obstacles on the path, which is used to evaluate the impact of obstacles on the path, dj is the distance between the following vehicle and the jth obstacle, the standard deviation of the obstacle distance is used to adjust the sensitivity of the cost function to distance, 111 is the total number of obstacles, k is the constant used to adjust the curvature of the exponential function and affect the degree of influence of the obstacle distance on the cost function, 9niax is the maximum permissible angular deviation in the path, and 6j is the angular deviation between the following vehicle and the jth obstacle. The range of Co values ​​is limited between 0 and 1, where 0 means that the path completely avoids the obstacles and 1 means that the path passes directly through the obstacles.When the obstacle distance dj is small, the exponential term will increase, leading to a decrease in the Co value, indicating that the path should avoid these obstacles. At the same time, when the angle deviation 0j is close to 6max, the minimum function will reduce the value, indicating that the path should be adjusted to reduce the angle deviation Co. By adjusting the values ​​of and k, the sensitivity of the cost function to obstacle distance and angle deviation is controlled to adapt to different environments and driving needs.

[0100] The aforementioned embodiments represent only specific examples of implementation. Any modification or substitution readily conceivable by an expert in the technical field, within the scope of the present invention, shall be considered as included within the scope of protection of this application. Therefore, the scope of protection of this application shall be defined by the accompanying claims.

Claims

1. Demands An automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables, comprising a control center, characterized in that: said control center is connected in communication with an environmental perception module, a target recognition and localization module, a relative heading deviation angle detection module, a deviation angle correction decision module, a propulsion and steering execution module, an autonomous navigation planning module and a human-machine interaction module, said modules are connected to each other by electrical signals; said environmental perception module, designed to collect environmental information around the following vehicle for the collection of fruits and vegetables; said target recognition and localization module, identifies and locates the spatial position and attitude data of the guide vehicle, distinguishes the guide vehicle from other extraneous objects, and locks onto it continuously; said module for detecting the relative heading deviation angle compares the position and attitude data of the following vehicle and the guide vehicle, analyzes the heading deviation angle of the following vehicle relative to the guide vehicle, and enables continuous detection of the deviation angle; in said module for detecting the relative heading deviation angle, the continuous detection process is carried out as follows: The position and attitude data of the following vehicle and the guide vehicle are obtained via the environmental perception module; this data is then pre-processed, including denoising, filtering, data alignment, then the characteristic information necessary for calculating the angle of deviation of the relative heading is extracted from the pre-processed data, the position data corresponding to GPS coordinates, the attitude data including a velocity vector and a directional angle; The heading angles of the following vehicle and the guide vehicle are compared to calculate the difference between the two, i.e., the angle of deviation from the relative heading. The formula for calculating this angle is A 0 = min(\0 Y — ^gL 360° - |0 y - 0 G | ) ; A 0 the angle of deviation of the relative heading, 0Y the heading angle of the guide vehicle, and 0G the heading angle of the follower vehicle; a timer is configured to trigger an update and data calculation every 500 milliseconds, thus periodically updating the position and attitude data of the follow vehicle and the guide vehicle, repeating the heading angle comparison process to achieve continuous detection of the deviation angle; said deviation angle correction decision module determines whether the current following situation is normal, and issues an alert in case of a deviation tendency, then updates the steering adjustment strategy of the following vehicle; in said deviation angle correction decision module, the deviation tendency alert process is carried out as follows: normal ranges and alert thresholds for relative heading deviation angle are defined, with the normal range being ±5° and the alert threshold ±10°, and real-time analysis results from the relative heading deviation angle detection module are received, including the current value and historical data for the relative heading deviation angle; based on the received relative heading deviation angle data, it is determined whether the following vehicle's tracking situation relative to the guide vehicle is within the normal range; if so, the tracking situation is considered normal and monitoring continues; if the normal range is exceeded, the alert and deviation handling process is triggered; The degree of deviation is determined according to the absolute value of the angle of deviation of the relative heading, and by combining the data of the angle of deviation of the relative heading at several successive times, a deviation warning coefficient is calculated, allowing analysis of the tendency of deviation of the angle of deviation of the relative heading; based on the results of the deviation trend analysis and the calculated deviation alert coefficient, if the deviation alert coefficient approaches or exceeds the alert threshold, and if the deviation trend persists or worsens, the alert mechanism is triggered, and a notification is sent to the driver via an audible or visual signal; based on the deviation warning coefficient and the results of the deviation trend analysis, the following vehicle's steering adjustment strategy is updated to correct the deviation and maintain the correct heading of the following vehicle; based on the angle of deviation of the relative heading and the deviation trend, the necessary adjustment direction of the following vehicle is determined, and by combining the deviation warning coefficient with the predefined adjustment strategy, the adjustment angle or speed variation is calculated, then the adjustment command is transmitted to the propulsion and steering execution module of the following vehicle, in order to perform the steering adjustment; The formula for calculating the aforementioned deviation alert coefficient is as follows: where D is the deviation alert coefficient, A0D the current value of the relative deviation angle, the predefined alert threshold, a the standard deviation used to adjust the sensitivity of the coefficient, A0- the relative heading angle data at the 1 continuous time point, n the number of consecutive time points, A0max the maximum value of the relative heading angle at consecutive times, A0mj the minimum value of the relative heading angle at consecutive times, the value of D is limited between 0 and 1, where 0 means no deviation, and 1 means severe deviation; said propulsion and steering execution module, in accordance with the strategies issued by the deviation angle correction decision module, transmits control instructions and controls the steering, acceleration and braking of the following vehicle, allowing precise control of the following vehicle; said autonomous navigation planning module, in the absence of a guide vehicle or in the event of loss of the guide vehicle's signal, relies on autonomous navigation technology to plan an optimal route from the vehicle's current position follower to the target position, taking into account surrounding obstacles; said human-machine interaction module provides the operator with a visual interface showing the operating status of the system.

2. An automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables according to claim 1, characterized in that: in said environmental perception module, the process of collecting environmental information is as follows: various types of sensors, such as cameras and laser radar, are deployed on the follower vehicle intended for the collection of fruits and vegetables, in order to capture information from the surrounding environment, including the relative position, speed and direction of the guide vehicle, the distance, shape and size of surrounding obstacles, as well as the contour and position of road edges;Images are collected and processed via cameras to identify traffic lanes and obstacles ahead; light beams are emitted by the laser radar and the reflected signals are received synchronously; the return time is used to measure the distance to obstacles; a point map is established to determine the shape, size, and distance of obstacles; the collected environmental data is transmitted to the vehicle's electronic control unit for preprocessing, which includes noise reduction, filtering, and data format conversion; data from different sensors are also calibrated and synchronized; the data from different sensors is merged to obtain comprehensive environmental information;The position, speed, and acceleration characteristics of obstacles, as well as the geometric shapes of road edges, are extracted from the merged data to create a set of characteristic data.

3. An automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables according to claim 2, characterized in that: in said recognition and

4. Target localization, the process of distinguishing between the guide vehicle and other extraneous objects is as follows: in the characteristic dataset obtained from the environmental perception module, each data point is scanned individually, image processing technology is used to extract the size, shape and color characteristics of the guide vehicle; using machine learning algorithms, the features extracted from the guide vehicle are compared to a previously trained reference model, candidate targets corresponding to the characteristics of the guide vehicle are selected from the matching results, and extraneous objects not corresponding to the characteristics of the guide vehicle are discarded by comparison with the obstacle characteristics provided by the environmental perception module; Based on the filtered characteristics of the guide vehicle, image processing technology based on image alignment is used to calculate the spatial coordinates of the guide vehicle; shape and size information of the guide vehicle is combined to estimate attitude data such as orientation and tilt angle; The position, orientation, and tilt information of the guide vehicle is associated with point cloud data from the sensors; a tracking algorithm is applied to perform continuous tracking and locking of the guide vehicle; the position and attitude information of the guide vehicle is dynamically updated according to real-time data provided by the environmental perception module, in order to adapt to environmental changes. An automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables according to claim 3, characterized in that: the formula for calculating the spatial position of said guide vehicle is as follows: I, 2 / Av \ “ . / Av \ I + (ta®) '“'^(y^ry+ïas)) where P represents the spatial position of the guide vehicle, x and y respectively denote the coordinates of the guide vehicle in the horizontal plane, d is the horizontal distance between the guide vehicle and the follower vehicle, A y is the vertical distance between the guide vehicle and the follower vehicle, 0 is the angle of inclination of the guide vehicle with respect to the horizontal plane, ymax is the maximum allowed value in the vertical direction, used to limit the range of y-, The formula for calculating the orientation of said guide vehicle is as follows:

5. where Hd represents the orientation of the guide vehicle, Ay is the vertical distance between the guide vehicle and the following vehicle, d is the horizontal distance between the guide vehicle and the following vehicle; the formula for calculating the angle of inclination of the guide vehicle is as follows: Ti = arctan(^); where Ti is the angle of inclination of the guide vehicle, AZ? is the variation in height of the guide vehicle, d is the horizontal distance between the guide vehicle and the follower vehicle. An automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables according to claim 4, characterized in that: in said propulsion and steering execution module, the process of implementing control of the following vehicle is as follows: The propulsion and steering execution module receives, via a communication interface, the adjustment strategy updated by the deviation angle correction decision module, extracts the control instructions from this adjustment strategy, and then analyzes these instructions; based on the results of the analysis of the control instructions, the specific control parameters of the steering angle, acceleration or braking force are extracted and, according to the results of the analysis, the type of action to be performed is determined, namely steering, acceleration or braking; For the execution of the steering action, if the control instruction corresponds to a steering action, the module commands the steering mechanism according to the steering angle parameter; for the execution of the acceleration action, if the instruction When the control command corresponds to an acceleration action, the module controls the engine output power according to the acceleration parameter in order to achieve acceleration; for the execution of the braking action, if the control instruction corresponds to a braking action, the module controls the braking system according to the braking force parameter in order to generate the appropriate braking force to slow down or stop the vehicle; steering, acceleration and braking actions can be executed simultaneously or successively, in order to achieve precise control of the following vehicle; during the execution of steering, acceleration or braking actions, the actual state of the vehicle is monitored in real time by sensors, compared to the intended target in order to evaluate the effectiveness of the execution;The execution results are transmitted to the deviation angle correction decision module via the communication interface; this deviation angle correction decision module then adjusts the control strategy based on the results obtained.

6. An automatic tracking and autonomous navigation system for the collection and transport of fruits and vegetables according to claim 5, characterized in that: in said autonomous navigation planning module, the guidance process of the following vehicle is as follows: the environmental information collected by the following vehicle is processed and then converted into a grid-type map representation, using this grid-type map to reflect obstacles and accessible areas in the environment; using the visual SLAM algorithm, the current position of the following vehicle is estimated based on the environmental information and the known map, while the inertial measurement unit provides real-time attitude information for the following vehicle; based on sensor data and map information, obstacles in the environment are detected in real time;depending on the position, shape and size of the obstacles, an avoidance strategy is developed, including going around or stopping to wait; a cost function related to the obstacles is integrated into the trajectory planning to evaluate; the impact of obstacles on the path, thus allowing them to be avoided and the trajectory to be optimized; based on the target position and the current position, a search algorithm is used to find on the map the shortest path that avoids obstacles and respects traffic rules; on this basis of global trajectory planning, dynamic adjustments and optimizations are made according to the real-time state of the following vehicle and changes in the environment; according to the results of the trajectory tracking and the actual state of the following vehicle, control instructions are generated to command steering, acceleration and braking actions, in order to ensure precise control.

7. An automatic tracking and autonomous navigation system for collecting and transporting fruit and vegetables according to claim 6, characterized in that: the cost function related to obstacles is expressed as follows: / <'dA2 \ Co = exp\ - J k / x min^ 1, j where Co is the cost function related to obstacles on the trajectory, which is used to evaluate the impact of obstacles on the path, dj the distance between the following vehicle and the jth obstacle, the standard deviation of the obstacle distance, which is used to adjust the sensitivity of the cost function to distance, m the total number of obstacles, k the constant used to adjust the curvature of the exponential function, 6max the maximum permissible angular deviation in the path, 6j the angular deviation between the following vehicle and the jth obstacle, the range of Co values ​​is limited between 0 and 1,where 0 means that the path completely avoids the obstacle and 1 means that the path passes directly through the obstacle.