The automatic following and autonomous navigation system for collecting and transporting fruit and vegetables

Abstract

The invention discloses an automatic following and autonomous navigation system for collecting and transporting fruits and vegetables and relates to the field of following navigation technology. The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables comprises a control center that is communicatively connected to an environment perception module, a target detection and positioning module, a module for detecting the relative heading angle, a module for deciding on deviation angle adjustment, a power and steering execution module, a module for planning autonomous navigation, and a human-computer interaction module, wherein the modules are connected by electrical signals.Through autonomous navigation and obstacle avoidance functions, the present invention enables the escort vehicle to automatically and precisely follow the lead vehicle or to independently plan a route and perform complex transport tasks without human intervention. Simultaneously, the system can quickly plan the optimal route according to the destination and current positions, further reducing transport time and improving transport efficiency. It also features highly accurate positioning and attitude estimation functions and can retrieve the escort vehicle's position and attitude information in real time to ensure stable operation in complex environments.

Description

Technical field

[0001] The invention relates to the technical field of tracking navigation and in particular to an automatic tracking and autonomous navigation system for collecting and transporting fruit and vegetables. State of the art

[0002] With advances in science and technology and the accelerating modernization of agriculture, agricultural production methods are gradually shifting towards smarter and more automated production. Traditional methods of harvesting and transporting fruits and vegetables require significant labor for carrying and following, making them not only labor-intensive but also inefficient. However, with rising consumer demands for fruit and vegetable quality and the expansion of agricultural production, the demand for efficient and intelligent harvesting and transport systems for fruits and vegetables is also increasing.The automatic tracking and autonomous navigation of support vehicles for fruit and vegetable harvesting and transport can significantly improve agricultural production efficiency and reduce labor costs, which corresponds to the development trend of agricultural modernization.

[0003] During trajectory tracking and automatic following, the insufficient field of view affects the tracking angle of the escort vehicle relative to the lead vehicle, causing the escort vehicle to easily deviate from the intended direction, lose sight of the target, and struggle to return to it. Therefore, we must solve the problem of adjusting the relative tracking angle between the escort and lead vehicles and implementing continuous deviation angle detection to improve the system's control accuracy. To this end, an automatic tracking and autonomous navigation system for harvesting and transporting fruits and vegetables is proposed. Content of the present invention

[0004] The purpose of the present invention is to provide an automatic following and autonomous navigation system for collecting and transporting fruits and vegetables in order to solve the problems raised in the above-mentioned background technology.

[0005] To solve the aforementioned technical problems, the following technical solution is applied in the present invention: The automatic following and autonomous navigation system for collecting and transporting fruit and vegetables comprises a control center, wherein the control center is communicatively connected to an environment perception module, a target identification and positioning module, a relative heading angle detection module, a deviation angle adjustment decision module, a power and steering execution module, an autonomous navigation planning module, and a human-computer interaction module, wherein electrical signals are connected between the modules; The environmental perception module is used to collect environmental information around the escort vehicle for collecting and transporting fruit and vegetables, including the position, speed, direction, surrounding obstacles and road boundary information of the escort vehicle, and provides accurate perception data in real time as a basis for subsequent decision-making and control; The target detection and positioning module detects and positions the coordinate position and orientation information of the lead vehicle in space based on data obtained from the environmental perception module, distinguishes the lead vehicle from other interfering objects, and continuously locks the lead vehicle to ensure that the object following the escort vehicle is clearly identifiable and that the escort vehicle always clearly recognizes the target being tracked, thereby improving the accuracy of the tracking; The module for detecting the relative course angle compares the position and attitude data of the escort vehicle and the lead vehicle, analyzes the course angle of the escort vehicle relative to the lead vehicle, implements continuous deflection angle detection and provides an accurate basis for the direction adjustment of the escort vehicle; The module for deciding on the deviation angle adjustment uses the analysis results of the module for detecting the relative course angle to determine whether the current following situation is normal and issues an early warning in case of deviation tendencies, thereby updating the strategy for adjusting the direction of the escort vehicle; The power and steering execution module issues control instructions according to the module's strategy for deciding on the deviation angle adjustment and controls the steering, acceleration, and braking actions of the escort vehicle, thereby achieving precise control of the escort vehicle and improving the system's control accuracy and response speed; If no lead vehicle is present or the lead vehicle's signal is lost, the autonomous navigation planning module uses autonomous navigation technology to plan the optimal route from the current position to the target position for the escort vehicle, taking comprehensive account of obstacle factors, to guide the escort vehicle forward, to enable it to reach the target safely and efficiently, and to ensure continuous operation even in complex environments.

[0006] The human-computer interaction module provides the operator with a visual user interface and displays the system's operating status, including the position of the escort and lead vessels, the relative course angle, and any anomalies. This allows the operator to adjust the necessary parameters and manually intervene in the control action to enable flexible adaptation of the system under special circumstances.

[0007] A further improvement of the technical solution of the present invention consists in the fact that the process for collecting environmental information in the environmental perception module proceeds as follows: The escort vehicle for collecting and transporting fruit and vegetables uses various types of sensors for cameras and lidar to gather information about the environment, including the relative position, speed and direction of the lead vehicle, information about the distance, shape and size of obstacles in the environment, and information about the contour and position of the road boundary; The camera collects and processes images to detect lane markings and obstacles in front of the vehicle; the laser radar sends out a beam of light and simultaneously receives the reflected light; the time difference is used to measure the distance to the obstacle and to draw a point cloud map to determine the shape, size, and distance of the obstacle; The collected environmental data is transmitted to the vehicle's electronic control unit for preprocessing; this includes noise reduction, filtering, and data format conversion to improve data accuracy and availability. Additionally, the data collected by various sensors is calibrated and synchronized to ensure temporal and spatial consistency. Data from various sensors are fused to obtain comprehensive and accurate environmental information. From this fused data, the position, speed, and acceleration of obstacles, as well as the geometric shape features of road boundaries, are extracted to create a feature dataset.

[0008] A further improvement of the technical solution of the present invention consists in the fact that in the target recognition and positioning module the process for distinguishing the guide vehicle from other interfering objects proceeds as follows: From the feature data set obtained from the environmental perception module, all feature data are processed individually, and the feature data relating to the size, shape, and color of the lead vehicle are extracted using image processing technology to facilitate identification and positioning; The machine learning algorithm compares the extracted feature data of the lead vehicle with the pre-trained feature model of the lead vehicle, and candidate targets that meet the features of the lead vehicle are filtered out from the comparison results. The features of the candidate targets are then compared with the obstacle features provided by the environment perception module, and interfering objects that do not match the features of the lead vehicle are eliminated. Based on the selected features of the guide vehicle, image processing technology of image registration is used to calculate the coordinate position of the guide vehicle in space, and the orientation and tilt angle of the guide vehicle are estimated in combination with the shape and size information of the guide vehicle; The coordinate position, orientation, and tilt angle information of the lead vehicle are combined with point cloud data acquired from the sensor to improve positioning accuracy. Additionally, a tracking algorithm (Kalman filter) is used to continuously track and lock the lead vehicle, ensuring the escort vehicle can always follow it precisely. Based on real-time data from the environmental perception module, the lead vehicle's position and attitude information are dynamically updated to adapt to environmental changes.

[0009] A further improvement of the technical solution of the present invention consists in the fact that the formula for calculating the coordinate position of the guide vehicle in space is: P=(x,y)=(d2+(Δytan(θ))2,min(ymax,y+Δytan(θ)))

[0010] Here, P is the coordinate position of the lead vehicle in space, x and y represent the coordinate position of the lead vehicle on the horizontal plane, d is the distance between the lead vehicle and the escort vehicle in the horizontal direction, Δy is the distance between the lead vehicle and the escort vehicle in the vertical direction, θ is the tilt angle of the lead vehicle relative to the horizontal plane, y max is to limit the maximum value in the vertical direction around the range of y;

[0011] The formula for calculating the alignment of the lead vehicle is: Hd=a tan 2(Δyd)

[0012] This includes Hd, the leading vehicle orientation; a tan 2, the arctangent function taking into account the quadrant, with which the orientation angle of the leading vehicle is calculated; Δy, the vertical distance between the leading vehicle and the escort vehicle; and d, the horizontal distance between the leading vehicle and the escort vehicle.

[0013] The formula for calculating the tilt angle of the lead vehicle is: Ti=arc tan(Δhd)

[0014] This includes Ti, the tilt angle of the lead vehicle, Δh, the change in altitude of the lead vehicle, d, the horizontal distance between the lead vehicle and the escort vehicle, and arctan, the arctangent function used to calculate the tilt angle of the lead vehicle.

[0015] A further improvement of the technical solution of the present invention consists in the fact that in the module for the detection of the relative heading angle, the implementation process for continuous deflection angle detection looks like this: The position and attitude data of the escort and lead vehicles are determined by the environmental perception module and preprocessed, including noise reduction, filtering, and data alignment, to improve accuracy and reliability. Feature information for calculating the relative heading angle is also extracted from the preprocessed data, where the position data consists of GPS coordinates and the attitude data comprises the velocity vector (magnitude and direction of speed) and the heading angle (vehicle orientation).

[0016] The course angle of the escort vessel (based on the heading angle data) is compared with the course angle of the lead vessel, and the difference between the two is calculated, representing the relative course angle. The formula for calculating the relative course angle is: Δθ = min(|θ) Y - θ G |, 360° - |θ Y - θ G |), Δθ is the relative course angle, θ Y is the course angle of the lead vessel and θ G is the course angle of the escort vehicle;

[0017] Set a timer to trigger the data update and calculation process every 500 milliseconds, regularly update the position and attitude data of the escort and lead vehicles, repeat the course angle comparison process, and perform continuous deflection angle detection.

[0018] A further improvement of the technical solution of the present invention consists in the fact that in the module for deciding on the deviation angle adjustment, the process of deviation trend warning proceeds as follows: Set the normal range and warning threshold for the relative course angle, where the normal range is ±5° and the warning threshold is ±10°, and receive real-time analysis results from the relative course angle detection module, including the current value and historical data of the relative course angle; Based on the received relative course angle data, determine whether the following position of the current escort vessel is within the normal range relative to the lead vessel. If the value is within the normal range, the current following position is normal and monitoring continues. If the normal range is exceeded, initiate the deviation warning and processing process. Determine the degree of deviation based on the absolute value of the relative price angle and calculate the deviation warning coefficient based on the relative price angle data at several consecutive time points in time to analyze the deviation trend of the relative price angle; In combination with the analysis results of the deviation trend and the calculated deviation warning coefficient, the warning mechanism is triggered to notify the driver by sound and light when the deviation warning coefficient approaches or exceeds the warning threshold and the deviation trend continues or worsens; Based on the analysis results of the deviation warning coefficient and the deviation trend, the strategy for adjusting the direction of the escort vehicle is updated to correct the deviation and keep the escort vehicle on the correct course. Using the relative course deviation angle and the deviation trend, the direction to be adjusted (left, right, or maintain) of the escort vehicle is determined, and the angle or speed change to be adjusted is calculated in combination with the deviation warning coefficient and the preset adjustment strategy. Subsequently, the adjustment instruction is sent to the power and steering execution module of the escort vehicle to execute the direction adjustment. A further improvement of the technical solution of the present invention consists in the fact that the calculation formula for the deviation warning coefficient is: D=11+e−(ΔθD−θ0σ)×min(1,∑i=1n|Δθi|n)×ΔθmaxΔθmin Among these are D, the deviation warning coefficient, ΔΘ. D the current value of the relative course angle, θ0 the preset deviation warning threshold, σ the standard deviation used to adjust the sensitivity of the deviation warning coefficient, Δθ i the relative course angle data at the i-th continuous time point, n the number of continuous time points, Δθ max Maximum value of the relative course angle within the continuous time periods, Δθ min Minimum value of the relative course angle within the continuous time points and the value range of D limited to 0 to 1, where 0 indicates no deviation and 1 indicates a significant deviation; A further improvement of the technical solution of the present invention consists in the fact that the implementation process of the escort vehicle control takes place in the power and steering execution module: The performance and steering execution module receives the adaptation strategy updated by the module for deciding on the deviation angle adjustment via the communication interface, extracts the control instructions in the adaptation strategy and performs an instruction analysis; Based on the analysis results of the control instructions, the specific control parameters of the steering angle, acceleration or braking force are extracted and, according to the analysis results, the type of action to be performed is determined, namely steering, accelerating or braking; To execute a steering action, the steering mechanism is controlled, if the control instruction is for a steering action, to steer according to the steering angle parameter. During the steering process, steering angle and steering speed are monitored in real time by sensors to ensure the accuracy and stability of the steering action. To execute an acceleration action, if the control instruction is for an acceleration action, the motor's output power is controlled according to the acceleration parameter to achieve acceleration. During the acceleration process, vehicle speed and acceleration are monitored in real time by sensors to ensure the stability and safety of the acceleration action.To execute a braking action when the control instruction is a braking action, the braking system is controlled to generate the appropriate braking force according to the braking force parameter in order to achieve deceleration or parking. During the braking process, vehicle speed and deceleration are monitored in real time by sensors to ensure the timeliness and reliability of the braking action. Steering, acceleration, and braking actions are executed simultaneously or sequentially to achieve precise control of the escort vehicle. During steering, acceleration, or braking, the vehicle's actual state is monitored in real time by sensors. This actual state is compared with the expected target, the effect of the action is evaluated, and the result (actual steering angle, vehicle speed, acceleration) is reported back to the deviation angle adjustment module via the communication interface. The deviation angle adjustment module then further adapts the control strategy based on the feedback to improve the system's control accuracy and response speed.

[0019] A further improvement of the technical solution of the present invention consists in the fact that in the autonomous navigation planning module, the process of autonomous navigation planning for steering the forward movement of the escort vehicle proceeds as follows: Based on the environmental information captured by the escort vehicle, this information is processed and converted into a grid map representation; the grid map is used to display the obstacles and potential areas in the environment; The visual SLAM algorithm is used to estimate the current position of the escort vehicle based on environmental information and known maps, and the escort vehicle's position information is retrieved in real time via the inertial measurement unit (IMU) sensor; Based on sensor data and map information, obstacles in the environment are detected in real time; depending on the location, shape, and size of the obstacles, strategies for obstacle avoidance are developed, including detours and waiting parking; obstacle cost functions are incorporated into path planning to assess the impact of obstacles on the path in order to avoid obstacles and optimize the path; Based on the destination and current position, the search algorithm (A*algorithm) is used to find the shortest route on the map, avoiding obstacles and complying with traffic regulations; based on global path planning, dynamic adjustments and optimizations are made according to the real-time status of the escort vehicle and changes in the environment to ensure that the escort vehicle can drive smoothly; Based on the results of the path tracking and the real-time status of the escort vehicle, control instructions are generated to control the steering, acceleration, and braking actions of the escort vehicle, thus achieving precise control.

[0020] A further improvement of the technical solution of the present invention consists in the fact that the expression of the obstacle cost function reads: Co=exp(−∑j=1m(djσj)k)×min(1,θmaxθj)

[0021] To this C oThis includes the obstacle cost function, which is used to assess the impact of obstacles on the path, d j the distance from the escort vehicle to the j-th obstacle, σ j the standard deviation of the obstacle distance, used to adjust the distance sensitivity of the cost function, m the total number of obstacles on the path, k a constant used to adjust the curvature of the exponential function and affecting the degree of influence of the obstacle distance on the cost function, θ max the maximum permissible angular deviation on the path, θ j the angular deviation between the escort vehicle and the j-th obstacle and the range of values ​​of C o is limited to 0 to 1, where 0 means that the path completely avoids the obstacle and 1 means that the path goes directly through the obstacle.

[0022] By adopting the above-mentioned technical solution, the present invention exhibits the following technical advances compared to the prior art: 1. The present invention provides an automatic following and autonomous navigation system for collecting and transporting fruits and vegetables. Through autonomous navigation and obstacle avoidance functions, the escort vehicle can automatically and precisely follow the lead vehicle or independently plan a route and perform complex transport tasks without human intervention. Simultaneously, the system can quickly plan the optimal route according to the destination and current positions, further reducing transport time and improving transport efficiency. It also features highly accurate positioning and attitude estimation functions and can retrieve the escort vehicle's position and attitude information in real time to ensure stable operation in complex environments. 2. The present invention provides an automatic following and autonomous navigation system for collecting and transporting fruit and vegetables. Through precise route planning and the generation of control commands, the accompanying vehicle can travel smoothly along the planned route and avoid dangerous situations caused by improper operation, such as sharp turns and sudden acceleration. Furthermore, real-time monitoring and analysis of environmental information allows potential hazards to be detected and avoided in a timely manner, thereby preventing collisions. Brief description of the drawing

[0023] To more clearly illustrate the embodiments of the present application and the prior art technical solutions, the drawings required for use in these embodiments are briefly presented below. Obviously, the drawings described below represent only some embodiments of the present invention. Other drawings can also be obtained from these drawings by ordinary engineers in this field. Fig. 1 is a module structure diagram of the present invention; Fig. Figure 2 is a flowchart of the implementation of the continuous deflection angle detection of the present invention; Fig. 3 is a flowchart of the deviation trend warning of the present invention; Fig. Figure 4 is a flowchart of the autonomous navigation planning and guidance of the escort vehicle for forward movement of the present invention. Detailed description

[0024] To clarify the purpose, technical solutions, and advantages of the embodiments of the present invention, the technical solutions in the embodiments of the present invention are described below in detail, in conjunction with the drawings. Obviously, the described embodiments represent only a subset of the embodiments of the present invention and not all of them. Based on the embodiments of the present invention, all other embodiments that can be achieved by ordinary engineers in this field without any creative work fall within the scope of protection of the present invention.

[0025] embodiment 1, as in Fig. 1 and Fig. 2 shown. The present invention relates to an automatic following and autonomous navigation system for collecting and transporting fruit and vegetables, comprising a control center, wherein the control center is communicatively connected to an environment perception module, a target identification and positioning module, a module for detecting the relative heading angle, a module for deciding on the deviation angle adjustment, a power and steering execution module, a module for planning autonomous navigation and a module for human-computer interaction, wherein the modules are connected by electrical signals;

[0026] The environmental perception module is used to gather environmental information around the escort vehicle for collecting and transporting fruits and vegetables, including the position, speed, direction, surrounding obstacles, and road boundary information of the lead vehicle. It provides accurate, real-time perception data as a basis for subsequent decision-making and control. The escort vehicle uses various types of sensors, including cameras and lidar, to capture environmental information, such as the relative position, speed, and direction of the lead vehicle; the distance, shape, and size of obstacles in the environment; and contour and position information of the road boundary.The camera captures and processes images, identifies lane markings and obstacles in front of the vehicle, transmits light beams through the lidar, and synchronously receives the reflected light. Use the time difference to measure the distance to the obstacle and create a point cloud map to determine the obstacle's shape, size, and distance. Transfer the collected environmental information data to the vehicle's electronic control unit for preprocessing. Preprocessing includes noise reduction, filtering, and data format conversion to improve data accuracy and availability. Calibrate and synchronize data from various sensors to ensure temporal and spatial consistency. Combine data from different sensors to obtain comprehensive and accurate environmental information.Extract the position, velocity, acceleration, and geometric shape features of the obstacle's road boundary from the merged data to obtain a feature dataset.

[0027] The target detection and positioning module uses data from the environmental perception module to detect and position the lead vehicle's coordinates and orientation in space. It distinguishes the lead vehicle from other interfering objects, continuously locks the lead vehicle, ensures the escort vehicle cannot see the object it is following, guarantees the escort vehicle always knows the target being tracked, and improves tracking accuracy. From the feature data set received by the environmental perception module, all feature data are processed individually, and image processing technology is used to extract the lead vehicle's size, shape, and color for easy identification and positioning.The machine learning algorithm compares the extracted feature data of the lead vehicle with the pre-trained feature model of the lead vehicle, filtering out candidate targets that match the lead vehicle's features. The candidate targets' features are then compared with obstacle features provided by the environmental perception module, eliminating interfering objects that do not match the lead vehicle's features, such as trees, buildings, and other vehicles. Among other things, the pre-training of the lead vehicle feature model ensures that various types of lead vehicles are included in the dataset by collecting a large amount of image data of the lead vehicles from multiple angles, lighting conditions, and backgrounds to improve the model's generalizability.The collected image data is annotated, including the bounding box and category label of the lead vehicle. Normalizing, cropping, rotating, and other operations on the image data improve the training efficiency and performance of the model. The annotated image data is fed into the convolutional neural network for training. During the training process, the model's weight and bias parameters are updated by the backpropagation algorithm. The model's performance is evaluated using the validation set. The model's architecture, parameters, or data preprocessing methods are adjusted based on the validation results to improve the model's performance. The model's performance is evaluated using the test set to ensure that the model has reliable detection capabilities in practical applications.The trained model is used in the automated tracking system of the escort vehicle for collecting and transporting fruits and vegetables and is integrated into the environmental perception module to identify and position the lead vehicle in real time. Based on selected features of the lead vehicle, image registration technology is used to calculate the coordinate position of the lead vehicle in space, and the orientation and tilt angle information of the lead vehicle is estimated in combination with the shape and size information of the lead vehicle. The coordinate position, orientation, and tilt angle of the lead vehicle are combined with the point cloud data acquired from the sensor to improve positioning accuracy.The tracking algorithm (Kalman filter) is used for the continuous tracking and locking of the lead vehicle to ensure that the escort vehicle can always follow the lead vehicle precisely. Based on the real-time data from the environmental perception module, the position and orientation information of the lead vehicle is dynamically updated to adapt to changes in the environment.

[0028] Furthermore, the formula for calculating the coordinate position of the lead vehicle in space is: P=(x,y)=(d2+(Δytan(θ))2,min(ymax,y+Δytan(θ)))

[0029] Here, P is the coordinate position of the lead vehicle in space, x and y represent the coordinate position of the lead vehicle on the horizontal plane, d is the distance between the lead vehicle and the escort vehicle in the horizontal direction, Δy is the distance between the lead vehicle and the escort vehicle in the vertical direction, θ is the tilt angle of the lead vehicle relative to the horizontal plane, y max is to limit the maximum value in the vertical direction around the range of y;

[0030] The formula for calculating the alignment of the lead vehicle is: Hd=a tan 2(Δyd)

[0031] This includes Hd, the leading vehicle orientation; a tan 2, the arctangent function taking into account the quadrant, with which the orientation angle of the leading vehicle is calculated; Δy, the vertical distance between the leading vehicle and the escort vehicle; and d, the horizontal distance between the leading vehicle and the escort vehicle.

[0032] The formula for calculating the tilt angle of the lead vehicle is: Ti=arc tan(Δhd)

[0033] This includes Ti, the tilt angle of the lead vehicle, Δh, the change in altitude of the lead vehicle, d, the horizontal distance between the lead vehicle and the escort vehicle, and arctan, the arctangent function used to calculate the tilt angle of the lead vehicle;

[0034] The relative heading angle detection module compares the position and attitude data of the escort and lead vehicles, analyzes the escort's heading angle relative to the lead vehicle, performs continuous deviation angle detection, and provides an accurate basis for the escort's course correction. The position and attitude data of the escort and lead vehicles are determined via the environmental perception module. This data is preprocessed, including noise reduction, filtering, and data alignment, to improve accuracy and reliability. Furthermore, feature information for calculating the relative heading angle is extracted from the preprocessed data. The position data consists of GPS coordinates, and the attitude data comprises the velocity vector (speed magnitude and direction) and the heading angle (vehicle orientation).The course angle of the escort vessel (based on the heading angle data) is compared with the course angle of the lead vessel, and the difference between the two is calculated, representing the relative course angle. The formula for calculating the relative course angle is: Δθ = min(|θ). Y - O G |, 360° - |θ Y - θ G |), Δθ is the relative course angle, θ Y is the course angle of the lead vessel and θ G The course angle of the escort vessel is measured, and a timer is set to trigger the data update and calculation process every 500 milliseconds. The position and attitude data of the escort and lead vessels are updated regularly, and the course angle comparison process is repeated to perform continuous deviation angle detection.

[0035] The module for deciding on the deviation angle adjustment, based on the analysis results of the module for detecting the relative course angle, determines whether the current following situation is normal and issues an early warning in the event of deviation tendencies, thereby updating the strategy for adjusting the direction of the escort vessel;

[0036] The power and steering execution module issues control instructions according to the module's strategy for deciding on the deviation angle adjustment and controls the steering, acceleration, and braking actions of the escort vehicle to achieve precise control of the escort vehicle and improve the system's control accuracy and response speed;

[0037] If no lead vehicle is present or the lead vehicle's signal is lost, the autonomous navigation planning module uses autonomous navigation technology to comprehensively consider obstacle factors and thus plan the optimal route from the current position to the target position for the escort vehicle. It guides the escort vehicle forward, enabling it to reach the target safely and efficiently, and ensures continuous operation even in complex environments.

[0038] The human-computer interaction module provides operators with a visual user interface that displays the system's operational status, including the position of the escort and lead vessels, the relative course angle, and any anomalies. It allows operators to adjust necessary parameters and manually intervene in the control actions, enabling convenient and flexible adaptation of the system under specific circumstances.

[0039] embodiment 2, as in Fig. 3 and Fig. Figure 4 shows that, based on embodiment 1, the present invention offers a technical solution: In the module for deciding on the deviation angle adjustment, the deviation trend warning process proceeds as follows: Define the normal range and warning threshold for the relative heading angle, where the normal range is ±5° and the warning threshold is ±10°, and receive real-time analysis results from the relative heading angle detection module, including the current value and historical data of the relative heading angle; Based on the received relative heading angle data, determine whether the following situation of the current escort vessel relative to the lead vessel is within the normal range; If the value is within the normal range, the current following situation is normal and monitoring continues; if the normal range is exceeded, initiate the deviation warning and processing process;Determine the degree of deviation based on the absolute value of the relative course angle and calculate the deviation warning coefficient based on the relative course angle data at several consecutive time points to analyze the deviation trend of the relative course angle; In combination with the analysis results of the deviation trend and the calculated deviation warning coefficient, the warning mechanism is triggered to notify the driver by sound and light when the deviation warning coefficient approaches or exceeds the warning threshold and the deviation trend continues or worsens;Based on the analysis results of the deviation warning coefficient and the deviation trend, the strategy for adjusting the direction of the escort vehicle is updated to correct the deviation and keep the escort vehicle on the correct course. The direction to be adjusted (left, right, or maintain) of the escort vehicle is determined based on the relative course deviation angle and the deviation trend. The angle or speed change to be adjusted is calculated in combination with the deviation warning coefficient and the preset adjustment strategy. Subsequently, the adjustment instruction is sent to the power and steering execution module of the escort vehicle to execute the direction adjustment. Furthermore, the formula for calculating the deviation warning coefficient is: D=11+e−(ΔθD−θ0σ)×min(1,∑i=1n|Δθi|n)×ΔθmaxΔθmin Among these are D, the deviation warning coefficient, Δθ Dthe current value of the relative course angle, θ0 the preset deviation warning threshold, σ the standard deviation used to adjust the sensitivity of the deviation warning coefficient, Δθ i the relative course angle data at the i-th continuous time point, n the number of continuous time points, Δθ max Maximum value of the relative course angle within the continuous time periods, Δθ min The minimum value of the relative course angle within the continuous time periods and the value range of D are limited to 0 to 1, where 0 indicates no deviation and 1 indicates a significant deviation. If the relative course deviation Δθ D As the warning threshold θ0 approaches or exceeds it, the value of D tends towards 1, indicating that an immediate warning is required. When the absolute value of Δθ DIf the deviation is small and changes little over several successive time points, the value of D is small, indicating a low degree of deviation; The implementation process for the escort vehicle control takes place in the performance and steering execution module: The power and steering execution module receives the adaptation strategy updated by the module for deciding on the deviation angle adjustment via the communication interface, extracts the control instructions in the adaptation strategy, and performs an instruction analysis. Based on the analysis results of the control instructions, the specific control parameters of the steering angle, acceleration, or braking force are extracted, and the type of action to be executed—steering, accelerating, or braking—is determined according to the analysis results. To execute the steering action, the steering mechanism is controlled to steer according to the steering angle parameter if the control instruction is for a steering action. During the steering process, the steering angle and steering speed are monitored in real time by sensors to ensure the accuracy and stability of the steering movement.To execute an acceleration action, if the control instruction is for acceleration, the engine output is controlled according to the acceleration parameter to achieve acceleration. During acceleration, vehicle speed and acceleration are monitored in real time by sensors to ensure smooth and safe acceleration. To execute a braking action, if the control instruction is for braking, the braking system is controlled to generate the appropriate braking force according to the braking force parameter to decelerate or park the vehicle. During braking, vehicle speed and deceleration are monitored in real time by sensors to ensure the timeliness and reliability of the braking action. To achieve precise control of the escort vehicle, steering, acceleration, and braking actions are executed simultaneously or sequentially.During steering, acceleration, or braking actions, the vehicle's actual state is monitored in real time by sensors, and this state is compared to the expected target to evaluate the effectiveness of the action. The results (actual steering angle, vehicle speed, acceleration) are fed back to the deviation angle adjustment module via the communication interface. The deviation angle adjustment module further refines the control strategy based on the feedback results to improve the system's control accuracy and response speed. In the autonomous navigation planning module, the process of autonomous navigation planning for steering the forward movement of the escort vehicle proceeds as follows: Based on environmental information captured by the escort vehicle, this information is processed and converted into a grid map representation; the grid map is used to display obstacles and potential areas in the environment; the visual SLAM algorithm is used to estimate the escort vehicle's current position based on environmental information and known maps, and the escort vehicle's attitude information is retrieved in real time via the inertial measurement unit (IMU) sensor; based on the sensor data and map information, obstacles in the environment are detected in real time; depending on the location, shape, and size of the obstacles, obstacle avoidance strategies are developed, including detours and waiting; obstacle cost functions are incorporated into path planning to evaluate the impact of obstacles on the path in order to avoid obstacles and optimize the path;Based on the destination and current position, the search algorithm (A* algorithm) is used to find the shortest route on the map, avoiding obstacles and complying with traffic regulations; based on global path planning, dynamic adjustments and optimizations are made according to the real-time status of the escort vehicle and changes in the environment to ensure the escort vehicle can travel smoothly; based on the results of path tracking and the real-time status of the escort vehicle, control instructions are generated to manage the steering, acceleration, and braking actions of the escort vehicle, thus achieving precise control; Furthermore, the expression of the obstacle cost function is: Co=exp(−∑j=1m(djσj)k)×min(1,θmaxθj) To this C o This includes the obstacle cost function, which is used to assess the impact of obstacles on the path, d jthe distance from the escort vehicle to the j-th obstacle, σ j the standard deviation of the obstacle distance, used to adjust the distance sensitivity of the cost function, m the total number of obstacles on the path, k a constant used to adjust the curvature of the exponential function and affecting the degree of influence of the obstacle distance on the cost function, θ max the maximum permissible angular deviation on the path, θ j the angular deviation between the escort vehicle and the j-th obstacle and the range of values ​​of C o is limited to 0 to 1, where 0 means that the path completely avoids the obstacle and 1 means that the path goes directly through the obstacle. If the obstacle distance d j As the value is small, the exponential term increases, leading to C oThis leads to a decrease in value and indicates that the path must avoid these obstacles. At the same time, the minimum function reduces the value of C. o , if the angular deviation θ j near θ max This indicates that the path needs to be adjusted to reduce the angular deviation. By adjusting the values ​​of σ j and the sensitivity of the cost function to obstacle distance and angular deviation is controlled to adapt it to different driving environments and requirements.

[0040] The above description merely represents one specific implementation of the present application; however, the scope of protection of the present application is not limited to this. Any engineer familiar with the technical field can easily imagine modifications or substitutions within the scope of the technical information disclosed in the present application that should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be aligned with the scope of protection of the claims.

Claims

[1] The automatic tracking and autonomous navigation system for collecting and transporting fruit and vegetables comprises a control center, characterized in that the control center is communicatively connected to an environment perception module, a target detection and positioning module, a relative heading angle detection module, a deviation angle adjustment decision module, a power and steering execution module, an autonomous navigation planning module and a human-computer interaction module, wherein the modules are connected by electrical signals; The environmental perception module serves to capture environmental information around the escort vehicle for collecting and transporting fruit and vegetables; The target recognition and positioning module detects and positions the coordinate position and orientation information of the command vehicle in space, distinguishes the command vehicle from other interfering objects, and continuously locks the command vehicle; The module for detecting the relative heading angle compares the position and attitude data of the escort vessel and the lead vessel, analyzes the heading angle of the escort vessel relative to the lead vessel, and implements continuous deflection angle detection; in the module for detecting the relative heading angle, the implementation process of continuous deflection angle detection is as follows: The position and attitude data of the escort vehicle and the lead vehicle are determined by the environmental perception module, and the determined position and attitude data are preprocessed, including noise reduction, filtering, and data alignment. Feature information for calculating the relative course angle is extracted from the preprocessed data, where the position data are the GPS coordinates and the attitude data include the velocity vector and the heading angle. The course angle of the escort vessel is compared with the course angle of the lead vessel, and the difference between the two is calculated, representing the relative course angle; the formula for calculating the relative course angle is: Δθ = min(|θ Y - θ G |, 360° - |θ Y - θ G ), Δθ is the relative course angle, θ Y is the course angle of the lead vessel and θ G is the course angle of the escort vehicle; Set a timer to trigger the data update and calculation process every 500 milliseconds, regularly update the position and attitude data of the escort and lead vehicles, repeat the course angle comparison process, and perform continuous deflection angle detection; The module for deciding on the deviation angle adjustment determines whether the current following situation is normal and issues an early warning in the event of deviation tendencies, thereby updating the strategy for adjusting the direction of the escort vehicle; in the module for deciding on the deviation angle adjustment, the early warning process for deviation trends proceeds as follows: Set the normal range and warning threshold for the relative course angle, where the normal range is ±5° and the warning threshold is ±10°, and receive real-time analysis results from the relative course angle detection module, including the current value and historical data of the relative course angle; Based on the received relative course angle data, determine whether the following situation of the current escort vessel is within the normal range relative to the lead vessel; if the value is within the normal range, the current following situation is normal and monitoring continues; if the normal range is exceeded, start the deviation warning and processing process; Determine the degree of deviation based on the absolute value of the relative price angle and calculate the deviation warning coefficient based on the relative price angle data at several consecutive time points in time to analyze the deviation trend of the relative price angle; In combination with the analysis results of the deviation trend and the calculated deviation warning coefficient, the warning mechanism is triggered to notify the driver by sound and light when the deviation warning coefficient approaches or exceeds the warning threshold and the deviation trend continues or worsens; Based on the analysis results of the deviation warning coefficient and the deviation trend, the strategy for adjusting the direction of the escort vehicle is updated to correct the deviation and keep the escort vehicle on the correct course. The direction of the escort vehicle to be adjusted is determined based on the relative course angle and the deviation trend, and the angle or change in speed to be adjusted is calculated in combination with the deviation warning coefficient and the preset adjustment strategy. Subsequently, the adjustment instruction is sent to the power and steering execution module of the escort vehicle to execute the direction adjustment. The formula for calculating the deviation warning coefficient is: D=11+e−(ΔθD−θ0σ)×min(1,∑i=1n|Δθi|n)×ΔθmaxΔθmin Among these are D, the deviation warning coefficient, Δθ Dthe current value of the relative course angle, θ0 the preset deviation warning threshold, σ the standard deviation used to adjust the sensitivity of the deviation warning coefficient, Δθ i the relative course angle data at the i-th continuous time point, n the number of continuous time points, Δθ max Maximum value of the relative course angle within the continuous time periods, Δθ min Minimum value of the relative course angle within the continuous time points and the value range of D limited to 0 to 1, where 0 indicates no deviation and 1 indicates a significant deviation; The power and steering execution module issues control instructions according to the module's strategy for deciding on the deviation angle adjustment and controls the steering, acceleration, and braking actions of the escort vehicle to achieve precise control of the escort vehicle; The autonomous navigation planning module plans the optimal route from the current position to the target position for the escort vehicle by comprehensively considering obstacle factors when no lead vehicle is present or the lead vehicle signal is lost, and directs the escort vehicle to drive forward; The human-computer interaction module provides a visual user interface to display the operating status of the system. [2] The automatic following and autonomous navigation system for collecting and transporting fruit and vegetables according to claim 1 is characterized by , that in the environmental perception module the process for collecting environmental information proceeds as follows: The escort vehicle for collecting and transporting fruit and vegetables uses various types of sensors for cameras and lidar to gather information about the environment, including the relative position, speed and direction of the lead vehicle, information about the distance, shape and size of obstacles in the environment, and information about the contour and position of the road boundary; The camera collects and processes images to detect lane markings and obstacles in front of the vehicle; the laser radar sends out a beam of light and simultaneously receives the reflected light; the time difference is used to measure the distance to the obstacle and to draw a point cloud map to determine the shape, size, and distance of the obstacle; The collected environmental information data is transmitted to the vehicle's electronic control unit for preprocessing; this includes noise reduction, filtering, data format conversion, as well as calibration and synchronization of the data collected by various sensors; Data from various sensors are fused to obtain comprehensive environmental information; from the fused data, position, speed and acceleration of obstacles as well as geometric shape features of road boundaries are extracted to obtain a feature dataset. [3] The automatic following and autonomous navigation system for collecting and transporting fruit and vegetables according to claim 2 is characterized by , that in the target recognition and positioning module, the process for distinguishing the lead vehicle from other interfering objects proceeds as follows: From the feature data set obtained from the environmental perception module, all feature data are processed individually and the feature data for the size, shape and color of the lead vehicle are extracted using image processing technology; The machine learning algorithm compares the extracted feature data of the lead vehicle with the pre-trained feature model of the lead vehicle, and candidate targets that meet the features of the lead vehicle are filtered out from the comparison results. The features of the candidate targets are then compared with the obstacle features provided by the environment perception module, and interfering objects that do not match the features of the lead vehicle are eliminated. Based on the selected features of the guide vehicle, image processing technology of image registration is used to calculate the coordinate position of the guide vehicle in space, and the orientation and tilt angle of the guide vehicle are estimated in combination with the shape and size information of the guide vehicle; The coordinate position, orientation, and tilt angle information of the guide vehicle are linked with the point cloud data received from the sensor, and the tracking algorithm is used to continuously track and lock the guide vehicle; based on the real-time data from the environment perception module, the position and orientation information of the guide vehicle is dynamically updated to adapt to changes in the environment. [4] The automatic tracking and autonomous navigation system for collecting and transporting fruit and vegetables according to claim 3, characterized by, that the formula for calculating the coordinate position of the lead vehicle in space is: P=(x,y)=(d2+(Δytan(θ))2,min(ymax,y+Δytan(θ))) Here, P is the coordinate position of the lead vehicle in space, x and y represent the coordinate position of the lead vehicle on the horizontal plane, d is the distance between the lead vehicle and the escort vehicle in the horizontal direction, Δy is the distance between the lead vehicle and the escort vehicle in the vertical direction, θ is the tilt angle of the lead vehicle relative to the horizontal plane, y max is to limit the maximum value in the vertical direction around the range of y; The formula for calculating the alignment of the lead vehicle is: Hd=a tan 2(Δyd) This includes Hd, the leading vehicle orientation, Δy, the vertical distance between the leading vehicle and the escort vehicle, and d, the horizontal distance between the leading vehicle and the escort vehicle; The formula for calculating the tilt angle of the lead vehicle is: Ti=arc tan(Δhd) This includes Ti, the tilt angle of the lead vehicle, Δh, the change in altitude of the lead vehicle, and d, the distance between the lead vehicle and the escort vehicle in the horizontal direction. [5] The automatic following and autonomous navigation system for collecting and transporting fruit and vegetables according to claim 4 is characterized by , that the implementation process of the escort vehicle control takes place in the performance and steering execution module: The performance and steering execution module receives the adaptation strategy updated by the module for deciding on the deviation angle adjustment via the communication interface, extracts the control instructions in the adaptation strategy and performs an instruction analysis; Based on the analysis results of the control instructions, the specific control parameters of the steering angle, acceleration or braking force are extracted and, according to the analysis results, the type of action to be performed is determined, namely steering, accelerating or braking; To execute the steering action, the steering mechanism is controlled, if the control instruction is a steering action, so that it steers according to the steering angle parameter; to execute the acceleration action, if the control instruction is an acceleration action, the engine output power is controlled according to the acceleration parameter to achieve acceleration; to execute the braking action, if the control instruction is a braking action, the braking system is controlled so that it generates a corresponding braking force according to the braking force parameter to achieve deceleration or parking, and the steering, acceleration, and braking actions are executed simultaneously or sequentially to achieve precise control of the escort vehicle; During steering, acceleration, or braking, the actual state of the vehicle is monitored in real time by sensors, the actual state is compared with the expected target, the execution effect is evaluated, and the execution result is reported back to the deviation angle adjustment module via the communication interface; the deviation angle adjustment module further adjusts the control strategy according to the feedback result. [6] The automatic following and autonomous navigation system for collecting and transporting fruit and vegetables according to claim 5 is characterized by , that in a module for planning autonomous navigation, the process of autonomous navigation planning for steering the forward movement of the escort vehicle proceeds as follows: Based on the environmental information captured by the escort vehicle, this information is processed and converted into a grid map representation; the grid map is used to display the obstacles and potential areas in the environment; The visual SLAM algorithm is used to estimate the current position of the escort vehicle based on environmental information and known maps, and the escort vehicle's position information is retrieved in real time via the inertial measurement unit sensor; Based on sensor data and map information, obstacles in the environment are detected in real time; depending on the location, shape, and size of the obstacles, strategies for obstacle avoidance are developed, including detours and waiting parking; obstacle cost functions are incorporated into path planning to assess the impact of obstacles on the path in order to avoid obstacles and optimize the path; Based on the destination and current position, the search algorithm uses the map to find the shortest route that avoids obstacles and complies with traffic regulations; based on global path planning, dynamic adjustments and optimizations are made according to the real-time status of the escort vehicle and changes in the environment; Based on the results of the path tracking and the real-time status of the escort vehicle, control instructions are generated to control the steering, acceleration, and braking actions of the escort vehicle, thus achieving precise control. [7] The automatic tracking and autonomous navigation system for collecting and transporting fruit and vegetables according to claim 6, characterized by , that the expression of the obstacle cost function is: Co=exp(−∑j=1m(djσj)k)×min(1,θmaxθj) To this C oThis includes the obstacle cost function, which is used to assess the impact of obstacles on the path, d j the distance from the escort vehicle to the j-th obstacle, σ j the standard deviation of the obstacle distance used to adjust the distance sensitivity of the cost function, m the total number of obstacles on the path, k a constant used to adjust the curvature of the exponential function, θ max the maximum permissible angular deviation on the path, θ j the angular deviation between the escort vehicle and the j-th obstacle and the range of values ​​of C o is limited to 0 to 1, where 0 means that the path completely avoids the obstacle and 1 means that the path goes directly through the obstacle.

Images