Automatic following and autonomous navigation system for fruit and vegetable collection and transportation

Abstract

The present 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, including a control center, wherein the control center is communicatively connected with an environment perception module, a target identification and positioning module, a relative heading deflection detection module, a deflection 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 present invention enables the following vehicle to automatically and accurately follow the leading vehicle or autonomously plan a path through autonomous navigation and obstacle avoidance functions, and completes complex transportation tasks without human intervention. At the same time, the system can quickly plan the optimal path according to the target position and the current position, further shortening the transportation time and improving the transportation efficiency. It also has high-precision positioning and attitude estimation capabilities, and can obtain the position and attitude information of the following vehicle in real time, ensuring that it maintains stable driving in a complex environment.

Description

Technical Field

[0001] The invention relates to the technical field of following navigation, and in particular to an automatic following and autonomous navigation system for collecting and transporting fruits and vegetables. Background Art

[0002] With the advancement of science and technology and the acceleration of agricultural modernization, agricultural production methods are gradually transforming towards intelligence and automation. In the traditional fruit and vegetable collection and transportation process, a large amount of manpower is required for carrying and following, which is not only labor-intensive but also inefficient. However, with the improvement of consumers' requirements for fruit and vegetable quality and the expansion of agricultural production scale, the demand for efficient and intelligent fruit and vegetable collection and transportation systems is increasing. By realizing automatic following and autonomous navigation of fruit and vegetable collection and transportation follow-up vehicles, agricultural production efficiency can be significantly improved, labor costs can be reduced, which is in line with the development trend of agricultural modernization.

[0003] In the process of trajectory tracking and automatic following, the insufficient perception field of view will affect the heading deflection angle of the following vehicle relative to the leading vehicle, making it easy for the following vehicle to deviate from the predetermined direction and lose the target and become difficult to reset. Therefore, how to adjust the relative heading deflection angle between the following vehicle and the leading vehicle and realize continuous deflection angle detection to improve the control accuracy of the system is the problem we need to solve. To this end, an automatic following and autonomous navigation system for fruit and vegetable collection and transportation is proposed. Summary of the 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, so as to solve the problems raised in the above-mentioned background technology.

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

[0006] The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables comprises a control center, wherein the control center is communicatively connected with an environment perception module, a target identification and positioning module, a relative heading angle detection module, an 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;

[0007] The environmental perception module is used to collect environmental information around the fruit and vegetable collection and transportation follower vehicle, including the position, speed, direction, surrounding obstacles and road boundary information of the guide vehicle, and provide real-time and accurate perception data to provide a basis for subsequent decision-making and control;

[0008] The target recognition and positioning module identifies and locates the coordinate position and posture information of the guide vehicle in space based on the data obtained by the environment perception module, distinguishes the guide vehicle from other interfering objects, and continuously locks the guide vehicle to ensure that the following vehicle's following object is clear and that the following vehicle can always clearly know the target to be followed, thereby improving the accuracy of following;

[0009] The relative heading angle detection module compares the position and posture data of the following vehicle and the leading vehicle, analyzes the heading angle of the following vehicle relative to the leading vehicle, realizes continuous deflection angle detection, and provides an accurate basis for adjusting the direction of the following vehicle;

[0010] The deflection angle adjustment decision module determines whether the current following situation is normal based on the analysis result of the relative heading deflection angle detection module, and issues an early warning for the deviation trend, thereby updating the strategy for adjusting the direction of the following vehicle;

[0011] The power and steering execution module issues control instructions according to the strategy of the deflection angle adjustment decision module, and controls the steering, acceleration and braking actions of the following vehicle, thereby achieving precise control of the following vehicle and improving the control accuracy and response speed of the system;

[0012] The autonomous navigation planning module, when there is no guide vehicle or the guide vehicle signal is lost, relies on autonomous navigation technology to plan the optimal path from the current position to the target position for the follower vehicle by comprehensively considering obstacle factors, guides the follower vehicle forward, enables the follower vehicle to reach the destination safely and efficiently, and ensures continuous operation even in complex environments;

[0013] The human-computer interaction module provides the operator with a visual operation interface, displays the operating status of the system, including the position of the follower vehicle and the guide vehicle, the relative heading angle, and whether there is any abnormality, allowing the operator to set necessary parameters and manually intervene in control operations, so as to facilitate flexible adjustment of the system under special circumstances.

[0014] A further improvement of the technical solution of the present invention is that in the environment perception module, the process of collecting environmental information is as follows:

[0015] Various types of sensors such as cameras and lidar are deployed on the fruit and vegetable collection and transportation follow-up vehicle to capture the surrounding environment information, including the relative position, speed, and direction information of the guide vehicle, the distance, shape, and size information of surrounding obstacles, and the contour and position information of the road boundary;

[0016] The camera collects and processes images to identify lanes and obstacles ahead. The laser radar sends a light beam and receives the reflected light simultaneously. The time difference is used to measure the distance to the obstacle and draw a point cloud map to determine the shape, size, and distance of the obstacle.

[0017] The collected data of surrounding environment information is transmitted to the vehicle's electronic control unit for preprocessing, which includes denoising, filtering, and data format conversion to improve the accuracy and availability of the data, and calibrate and synchronize the data collected by different sensors to make the data consistent in time and space;

[0018] The data from different sensors are fused to obtain comprehensive and accurate environmental information, and the position, speed, acceleration and geometric shape features of obstacles and road boundaries are extracted from the fused data to obtain a feature data set.

[0019] A further improvement of the technical solution of the present invention is that in the target recognition and positioning module, the process of distinguishing the guide vehicle from other interfering objects is as follows:

[0020] From the feature data set obtained by the environment perception module, each feature data is traversed one by one, and the feature data of the size, shape, and color of the guide vehicle are extracted using image processing technology to facilitate identification and positioning;

[0021] Through the machine learning algorithm, the extracted guide vehicle feature data is matched with the pre-trained guide vehicle feature model, and candidate targets that meet the guide vehicle features are screened out from the matching results. The features of the candidate targets are compared with the obstacle features provided by the environmental perception module, and interference objects that do not match the guide vehicle features are eliminated.

[0022] Based on the selected features of the guide vehicle, the 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.

[0023] The coordinate position, orientation, and tilt angle information of the lead vehicle are associated with the point cloud data obtained from the sensor to improve the accuracy of positioning, and the tracking algorithm (Kalman filter) is used to continuously track and lock the lead vehicle to ensure that the follower vehicle can always accurately follow the lead vehicle. According to the real-time data provided by the environmental perception module, the position and posture information of the lead vehicle are dynamically updated to adapt to environmental changes.

[0024] A further improvement of the technical solution of the present invention is that the calculation formula of the coordinate position of the guide vehicle in space is:

[0025] ;

[0026] Among them, P is the coordinate position of the leading vehicle in space, x and y represent the coordinate position of the leading vehicle on the horizontal plane, d is the distance between the leading vehicle and the following vehicle in the horizontal direction, is the vertical distance between the leading vehicle and the following vehicle, is the inclination angle of the guide vehicle relative to the horizontal plane, It is the maximum value in the vertical direction, which is used to limit the range of y;

[0027] The calculation formula for the guide vehicle orientation is:

[0028] ;

[0029] Among them, Hd is the direction of the guide vehicle, The inverse tangent function taking the quadrant into account is used to calculate the heading angle of the guide vehicle. is the distance between the leading vehicle and the following vehicle in the vertical direction, and d is the distance between the leading vehicle and the following vehicle in the horizontal direction;

[0030] The calculation formula of the guide vehicle inclination angle is:

[0031] ;

[0032] in, To guide the vehicle's tilt angle, is the height change of the leading vehicle, d is the horizontal distance between the leading vehicle and the following vehicle, is the inverse tangent function, which is used to calculate the tilt angle of the guide vehicle.

[0033] A further improvement of the technical solution of the present invention is that in the relative heading angle detection module, the implementation process of continuous deflection angle detection is:

[0034] The position and attitude data of the following vehicle and the leading vehicle are obtained through the environment perception module, and the obtained position and attitude data are preprocessed, including denoising, filtering, and data alignment, to improve the accuracy and reliability of the data, and feature information for calculating the relative heading angle is extracted from the preprocessed data, where the position data is the GPS coordinates, and the attitude data includes the velocity vector (speed magnitude and direction) and the direction angle (vehicle orientation);

[0035] The heading angle of the following vehicle (based on the direction angle data) is compared with the heading angle of the leading vehicle, and the difference between the two is calculated, which is the relative heading angle. The calculation formula of the relative heading angle is: ; is the relative heading angle, is the heading angle of the guide vehicle, is the heading angle of the following vehicle;

[0036] Set a timer to trigger the data update and calculation process every 500 milliseconds, regularly update the position and attitude data of the follower vehicle and the guide vehicle, repeat the heading angle comparison process, and perform continuous deflection angle detection.

[0037] A further improvement of the technical solution of the present invention is that in the deflection angle adjustment decision module, the process of deviation trend warning is:

[0038] Set the normal range and warning threshold of the relative heading angle, where the normal range is ±5° and the warning threshold is ±10°, and receive the real-time analysis results from the relative heading angle detection module, including the current value and historical data of the relative heading angle;

[0039] Based on the received relative heading angle data, determine whether the following situation of the current following vehicle relative to the leading vehicle is within the normal range. If it is within the normal range, the current following situation is normal and monitoring continues. If it exceeds the normal range, enter the deviation warning and processing process;

[0040] Determine the degree of deviation based on the absolute value of the relative heading angle, and calculate the deviation warning coefficient based on the relative heading angle data at multiple consecutive time points to analyze the deviation trend of the relative heading angle;

[0041] Combined with the analysis results of the deviation trend and the calculated deviation warning coefficient, if the deviation warning coefficient approaches or exceeds the warning threshold, and the deviation trend continues or worsens, the warning mechanism is triggered to notify the driver through sound and light;

[0042] According to the analysis results of the deviation warning coefficient and the deviation trend, the strategy for adjusting the direction of the following vehicle is updated to correct the deviation and keep the following vehicle on the correct heading. According to the relative heading deviation angle and the deviation trend, the direction (left, right or maintain) that the following vehicle needs to adjust is determined. Combined with the deviation warning coefficient and the preset adjustment strategy, the angle or speed change that needs to be adjusted is calculated, and the adjustment command is sent to the power and steering execution module of the following vehicle to execute the direction adjustment operation.

[0043] A further improvement of the technical solution of the present invention is that the calculation formula of the deviation warning coefficient is:

[0044] ;

[0045] Among them, D is the deviation warning coefficient, is the current value of the relative heading angle, is the preset deviation warning threshold, is the standard deviation, which is used to adjust the sensitivity of the deviation warning coefficient. is the relative heading angle data of the i-th continuous time point, n is the number of continuous time points, is the maximum value of the relative heading deviation angle at consecutive time points, It is the minimum value of the relative heading deviation angle within consecutive time points. The value range of D is limited to between 0 and 1, where 0 indicates no deviation and 1 indicates severe deviation.

[0046] A further improvement of the technical solution of the present invention is that in the power and steering execution module, the implementation process of the following vehicle control is:

[0047] The power and steering execution module receives the adjustment strategy updated by the deflection angle adjustment decision module through the communication interface, extracts the control instructions in the adjustment strategy, and performs instruction parsing;

[0048] 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, which is steering, acceleration or braking;

[0049] For the execution of steering action, if the control instruction is steering action, the steering mechanism is controlled to steer according to the steering angle parameter. 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 action; for the execution of acceleration action, if the control instruction is acceleration action, the output power of the engine is controlled according to the acceleration parameter to achieve acceleration. During the acceleration process, the vehicle speed and acceleration are monitored in real time by sensors to ensure the stability and safety of the acceleration action; for the execution of braking action, if the control instruction is braking action, the braking system is controlled to generate corresponding braking force according to the braking force parameter to achieve deceleration or parking. During the braking process, the 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 performed simultaneously or sequentially to achieve precise control of the following vehicle;

[0050] During the process of steering, accelerating or braking, the actual state of the vehicle is monitored in real time through sensors, the actual state is compared with the expected target, the execution effect is evaluated, and the execution result (actual steering angle, vehicle speed, acceleration) is fed back to the deflection angle adjustment decision module through the communication interface. The deflection angle adjustment decision module further adjusts the control strategy according to the feedback result to improve the control accuracy and response speed of the system.

[0051] A further improvement of the technical solution of the present invention is that: in the autonomous navigation planning module, the process of autonomous navigation planning to guide the follower vehicle to move forward is:

[0052] Based on the surrounding environment information collected by the following vehicle, it is processed and converted into a grid map representation, and the grid map is used to intuitively reflect the obstacles and feasible areas in the environment;

[0053] The visual SLAM algorithm is used to estimate the current position of the following vehicle based on environmental information and known maps, and the posture information of the following vehicle is obtained in real time through the inertial measurement unit (IMU) sensor;

[0054] Based on sensor data and map information, obstacles in the environment are detected in real time. According to the location, shape and size of the obstacles, obstacle avoidance strategies are formulated, including detours and parking waiting. Obstacle cost functions are introduced into path planning to evaluate the impact of obstacles in the path, so as to avoid obstacles and optimize the path.

[0055] Based on the target location and current location, the search algorithm (A* algorithm) is used to search the map for the shortest path that avoids obstacles and complies with traffic rules. Based on the global path planning, dynamic adjustments and optimizations are made according to the real-time status of the following vehicle and changes in the surrounding environment to ensure that the following vehicle can travel smoothly.

[0056] According to the path tracking results and the real-time status of the following vehicle, control instructions are generated to control the steering, acceleration and braking actions of the following vehicle to achieve precise control.

[0057] A further improvement of the technical solution of the present invention is that the expression of the obstacle cost function is:

[0058] ;

[0059] in, is the obstacle cost function, which is used to evaluate the impact of obstacles in the path. is the distance from the following car to the jth obstacle, is the standard deviation of the obstacle distance, which is used to adjust the sensitivity of the cost function to the distance. m is the total number of obstacles in the path. k is a constant, which is used to adjust the curvature of the exponential function and affect the degree of influence of the obstacle distance on the cost function. is the maximum angular deviation allowed in the path, is the angle deviation between the following vehicle and the jth obstacle, The value range of is limited to 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.

[0060] Due to the adoption of the above technical solution, the present invention has the following technical advances compared with the prior art:

[0061] 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 following vehicle can automatically and accurately follow the leading vehicle or autonomously plan a path, and complete complex transportation tasks without human intervention. At the same time, the system can quickly plan the optimal path according to the target position and the current position, further shortening the transportation time and improving the transportation efficiency. It also has high-precision positioning and attitude estimation capabilities, and can obtain the position and attitude information of the following vehicle in real time to ensure that it maintains stable driving in complex environments.

[0062] The present invention provides an automatic following and autonomous navigation system for collecting and transporting fruits and vegetables. Through precise path planning and control command generation, the following vehicle can smoothly travel on the planned path, avoiding dangerous situations caused by improper operations such as sharp turns and sudden acceleration. In addition, through real-time monitoring and analysis of surrounding environment information, potential dangerous obstacles can be discovered and avoided in time, thereby avoiding the occurrence of collision accidents. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in the present invention. For ordinary technicians in this field, other drawings can also be obtained based on these drawings.

[0064] Figure 1 It is a module structure diagram of the present invention;

[0065] Figure 2 This is a flow chart for realizing continuous deflection angle detection of the present invention;

[0066] Figure 3 A flowchart of the deviation trend warning of the present invention;

[0067] Figure 4 The present invention is a flow chart of autonomous navigation planning to guide the follower vehicle to move forward. DETAILED DESCRIPTION

[0068] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0069] Embodiment 1, as Figure 1 , Figure 2 As shown, the present invention provides an automatic following and autonomous navigation system for collecting and transporting fruits and vegetables, including a control center, the control center is communicatively connected with an environment perception module, a target identification and positioning module, a relative heading angle detection module, an 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;

[0070] The environmental perception module is used to collect environmental information around the fruit and vegetable collection and transportation follow-up vehicle, including the position, speed, direction, surrounding obstacles and road boundary information of the lead vehicle, and provide real-time and accurate perception data to provide a basis for subsequent decision-making and control. Various types of sensors such as cameras and lidars are deployed on the fruit and vegetable collection and transportation follow-up vehicle to capture the surrounding environmental information, including the relative position, speed and direction information of the lead vehicle, the distance, shape and size information of the surrounding obstacles, and the contour and position information of the road boundary. The camera is used to collect and process images, identify lanes and obstacles ahead, send light beams through the lidar, and synchronously receive the reflected light. Use the time difference to measure the distance to the obstacle and draw a point cloud map to determine the shape, size and distance of the obstacle. Transmit the collected data of the surrounding environment information to the vehicle's electronic control unit for preprocessing. The preprocessing includes denoising, filtering, and data format conversion to improve the accuracy and availability of the data. Calibrate and synchronize the data collected by different sensors to make the data consistent in time and space. Fuse the data from different sensors to obtain comprehensive and accurate environmental information. Extract the position, speed, acceleration and geometric shape features of the road boundary of the obstacle from the fused data to obtain a feature data set.

[0071] The target recognition and positioning module, based on the data obtained by the environmental perception module, identifies and locates the coordinate position and posture information of the guide vehicle in space, distinguishes the guide vehicle from other interfering objects, continuously locks the guide vehicle, ensures that the following object of the follower vehicle is clear, ensures that the follower vehicle can always know the target to be followed, and improves the accuracy of following. From the feature data set obtained by the environmental perception module, each feature data is traversed one by one, and the feature data of the size, shape, and color of the guide vehicle are extracted using image processing technology for easy identification and positioning. Through the machine learning algorithm, the extracted guide vehicle feature data is matched with the pre-trained guide vehicle feature model, and candidate targets that meet the guide vehicle characteristics are screened out from the matching results. The features of the candidate targets are compared with the obstacle features provided by the environmental perception module, and interfering objects that do not match the guide vehicle characteristics, such as trees, buildings, and other vehicles, are eliminated. Among them, the pre-training of the guide vehicle feature model ensures that different types of guide vehicles are included in the data set by collecting a large amount of guide vehicle image data from multiple angles, lighting conditions, and backgrounds to improve the generalization ability of the model. The collected image data are annotated, including the bounding box and category label of the guide vehicle, and the image data are normalized and cropped. , rotation and other operations to improve the training efficiency and performance of the model. The labeled image data is input into the convolutional neural network for training. During the training process, the weight and bias parameters of the model are updated through the back propagation algorithm. The performance of the model is evaluated using the validation set. The architecture, parameters or data preprocessing methods of the model are adjusted according to the validation results to improve the performance of the model. The performance of the model is evaluated on the test set to ensure that the model has reliable recognition capabilities in practical applications. The trained model is deployed to the automatic following system of the fruit and vegetable collection and transportation follower vehicle and integrated into the environmental perception module for real-time identification and positioning of the guide vehicle. Based on the selected guide vehicle features, the 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 posture information of the guide vehicle is 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 associated with the point cloud data obtained from the sensor to improve the accuracy of positioning. The tracking algorithm (Kalman filter) is used to continuously track and lock the guide vehicle to ensure that the follower vehicle can always accurately follow the guide vehicle. According to the real-time data provided by the environmental perception module, the position and posture information of the guide vehicle are dynamically updated to adapt to environmental changes.

[0072] Furthermore, the calculation formula for the coordinate position of the guide vehicle in space is:

[0073] ;

[0074] Among them, P is the coordinate position of the leading vehicle in space, x and y represent the coordinate position of the leading vehicle on the horizontal plane, d is the distance between the leading vehicle and the following vehicle in the horizontal direction, is the vertical distance between the leading vehicle and the following vehicle, is the inclination angle of the guide vehicle relative to the horizontal plane, It is the maximum value in the vertical direction, which is used to limit the range of y;

[0075] The calculation formula for the guide vehicle direction is:

[0076] ;

[0077] Among them, Hd is the direction of the guide vehicle, The inverse tangent function taking the quadrant into account is used to calculate the heading angle of the guide vehicle. is the distance between the leading vehicle and the following vehicle in the vertical direction, and d is the distance between the leading vehicle and the following vehicle in the horizontal direction;

[0078] The calculation formula of the guide vehicle inclination angle is:

[0079] ;

[0080] in, To guide the vehicle's tilt angle, is the height change of the leading vehicle, d is the horizontal distance between the leading vehicle and the following vehicle, is the inverse tangent function, which is used to calculate the tilt angle of the guide vehicle;

[0081] The relative heading angle detection module compares the position and attitude data of the following vehicle and the leading vehicle, analyzes the heading angle of the following vehicle relative to the leading vehicle, realizes continuous deflection angle detection, and provides an accurate basis for adjusting the direction of the following vehicle. The position and attitude data of the following vehicle and the leading vehicle are obtained through the environmental perception module, and the obtained position and attitude data are preprocessed, including denoising, filtering, and data alignment to improve the accuracy and reliability of the data, and feature information for calculating the relative heading angle is extracted from the preprocessed data. Among them, the position data is the GPS coordinates, and the attitude data includes the velocity vector (speed magnitude and direction) and the direction angle (vehicle orientation). The heading angle of the following vehicle (based on the direction angle data) is compared with the heading angle of the leading vehicle, and the difference between the two is calculated, which is the relative heading angle. The calculation formula of the relative heading angle is: ; is the relative heading angle, is the heading angle of the guide vehicle, For the heading angle of the following vehicle, a timer is set to trigger the data update and calculation process every 500 milliseconds, regularly update the position and attitude data of the following vehicle and the leading vehicle, repeat the heading angle comparison process, and perform continuous deflection angle detection;

[0082] The deflection adjustment decision module determines whether the current following situation is normal based on the analysis results of the relative heading deflection detection module, and issues an early warning for the deviation trend, thereby updating the strategy for adjusting the direction of the following vehicle.

[0083] The power and steering execution module issues control instructions according to the strategy of the deflection angle adjustment decision module, and controls the steering, acceleration and braking actions of the following vehicle to achieve precise control of the following vehicle and improve the control accuracy and response speed of the system;

[0084] The autonomous navigation planning module, when there is no guide vehicle or the guide vehicle signal is lost, relies on autonomous navigation technology to comprehensively consider obstacle factors to plan the optimal path from the current position to the target position for the follower vehicle, guides the follower vehicle forward, enables the follower vehicle to reach the destination safely and efficiently, and ensures continuous operation even in complex environments;

[0085] The human-computer interaction module provides operators with a visual operation interface, displaying the system's operating status, including the position of the follower vehicle and the guide vehicle, the relative heading angle, and whether there are any abnormalities. It allows operators to set necessary parameters and manually intervene in control operations, making it convenient to flexibly adjust the system under special circumstances.

[0086] Embodiment 2, as Figure 3 , Figure 4 As shown, based on Example 1, the present invention provides a technical solution: Preferably, in the deflection adjustment decision module, the process of deviation trend warning is:

[0087] Set the normal range and warning threshold of the relative heading angle, where the normal range is ±5° and the warning threshold is ±10°, and receive the 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 following vehicle relative to the leading vehicle is within the normal range. If it is within the normal range, the current following situation is normal and monitoring continues. If it exceeds the normal range, enter the deviation warning and processing process, determine the degree of deviation according to the absolute value of the relative heading angle, and combine the relative heading angle data at multiple consecutive time points to calculate the deviation warning coefficient, and analyze the deviation of the relative heading angle. Trend, combined with the analysis results of the deviation trend and the calculated deviation warning coefficient, if the deviation warning coefficient approaches or exceeds the warning threshold, and the deviation trend continues or worsens, the warning mechanism is triggered, and the driver is notified by sound and light. According to the analysis results of the deviation warning coefficient and the deviation trend, the strategy for adjusting the direction of the following vehicle is updated to correct the deviation and keep the following vehicle on the correct course. According to the relative heading deflection angle and the deviation trend, the direction (left, right or maintain) that the following vehicle needs to adjust is determined. Combined with the deviation warning coefficient and the preset adjustment strategy, the angle or speed change that needs to be adjusted is calculated, and the adjustment command is sent to the power and steering execution module of the following vehicle to execute the direction adjustment operation;

[0088] Furthermore, the calculation formula of the deviation warning coefficient is:

[0089] ;

[0090] Among them, D is the deviation warning coefficient, is the current value of the relative heading angle, is the preset deviation warning threshold, is the standard deviation, which is used to adjust the sensitivity of the deviation warning coefficient. is the relative heading angle data of the i-th continuous time point, n is the number of continuous time points, is the maximum value of the relative heading deviation angle at consecutive time points, The value range of D is limited to between 0 and 1, where 0 indicates no deviation and 1 indicates severe deviation. Approaching or exceeding the warning threshold When the value of D tends to 1, it means that an immediate warning is needed. When the absolute value of is small and the deflection angle does not change much at multiple consecutive time points, the value of D will be small, indicating a low degree of deviation;

[0091] In the power and steering execution module, the implementation process of following vehicle control is as follows:

[0092] The power and steering execution module receives the adjustment strategy updated by the deflection adjustment decision module through the communication interface, extracts the control instructions in the adjustment strategy, and performs instruction parsing. Based on the parsing 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 performed is determined according to the parsing results, which are steering, acceleration or braking. For the execution of the steering action, if the control instruction is a steering action, the steering mechanism is controlled to steer according to the steering angle parameter. During the steering process, the steering angle and steering speed are monitored in real time through sensors to ensure the accuracy and stability of the steering action. For the execution of the acceleration action, if the control instruction is an acceleration action, the output power of the engine is controlled according to the acceleration parameter to achieve acceleration. During the acceleration process, the vehicle speed and acceleration are monitored in real time through sensors. Ensure the smoothness and safety of acceleration; for the execution of braking action, if the control instruction is braking action, then according to the braking force parameters, control the braking system to generate corresponding braking force to achieve deceleration or parking. During the braking process, the vehicle speed and deceleration are monitored in real time through sensors to ensure the timeliness and reliability of the braking action. Steering, acceleration and braking actions are performed simultaneously or sequentially 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 through sensors, and the actual state is compared with the expected target to evaluate the execution effect. The execution results (actual steering angle, vehicle speed, acceleration) are fed back to the deflection angle adjustment decision module through the communication interface. The deflection angle adjustment decision module further adjusts the control strategy according to the feedback results to improve the control accuracy and response speed of the system;

[0093] In the autonomous navigation planning module, the process of autonomous navigation planning to guide the follower vehicle forward is as follows:

[0094] Based on the surrounding environment information collected by the following vehicle, it is processed and converted into a grid map representation. The grid map is used to intuitively reflect the obstacles and feasible areas in the environment. The visual SLAM algorithm is used to estimate the current position of the following vehicle based on the environmental information and the known map, and the posture information of the following vehicle is obtained in real time through the inertial measurement unit (IMU) sensor. Based on the sensor data and map information, obstacles in the environment are detected in real time. According to the position, shape and size of the obstacles, obstacle avoidance strategies are formulated, including detours, parking and waiting. The obstacle cost function is introduced into the path planning to evaluate the impact of obstacles in the path to avoid obstacles and optimize the path. According to the target position and the current position, the search algorithm (A* algorithm) is used to search the shortest path in the map that avoids obstacles and follows traffic rules. Based on the global path planning, dynamic adjustments and optimizations are made according to the real-time status of the following vehicle and the changes in the surrounding environment to ensure that the following vehicle can travel smoothly. According to the path tracking results and the real-time status of the following vehicle, control instructions are generated to control the steering, acceleration and braking actions of the following vehicle to achieve precise control.

[0095] Furthermore, the expression of the obstacle cost function is:

[0096] ;

[0097] in, is the obstacle cost function, which is used to evaluate the impact of obstacles in the path. is the distance from the following car to the jth obstacle, is the standard deviation of the obstacle distance, which is used to adjust the sensitivity of the cost function to the distance. m is the total number of obstacles in the path. k is a constant, which is used to adjust the curvature of the exponential function and affect the degree of influence of the obstacle distance on the cost function. is the maximum angular deviation allowed in the path, is the angle deviation between the following vehicle and the jth obstacle, The value range of is limited to between 0 and 1, where 0 means the path completely avoids obstacles, 1 means the path passes directly through obstacles, and When it is small, the exponential term will increase, resulting in The value decreases, indicating that the path needs to avoid these obstacles. At the same time, when the angle deviation near When A decrease in the value indicates that the path needs to be adjusted to reduce the angle deviation. The values ​​of and k control the sensitivity of the cost function to obstacle distance and angle deviation to adapt to different driving environments and requirements.

[0098] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables includes a control center, which is characterized by: The control center is communicatively connected with an environment perception module, a target identification and positioning module, a relative heading angle detection module, an 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 environment perception module is used to collect environmental information around the fruit and vegetable collection and transportation follow-up vehicle; The target recognition and positioning module recognizes and locates the coordinate position and posture information of the guide vehicle in space, distinguishes the guide vehicle from other interfering objects, and continuously locks the guide vehicle; The relative heading angle detection module compares the position and posture data of the following vehicle and the leading vehicle, analyzes the heading angle of the following vehicle relative to the leading vehicle, and realizes continuous deflection angle detection. In the relative heading angle detection module, the implementation process of continuous deflection angle detection is as follows: The position and attitude data of the following vehicle and the leading vehicle are obtained through the environment perception module, and the obtained position and attitude data are preprocessed, including denoising, filtering, and data alignment, and feature information for calculating the relative heading angle is extracted from the preprocessed data, where the position data is the GPS coordinates, and the attitude data includes the velocity vector and the direction angle; The heading angle of the following vehicle is compared with the heading angle of the leading vehicle, and the difference between the two is calculated, which is the relative heading angle. The calculation formula of the relative heading angle is: Δθ=min(|θ Y -θ G |, 360°-|θ Y -θ G |); Δθ is the relative heading angle, θ Y is the heading angle of the guide vehicle, θ G is the heading angle of the following vehicle; Set a timer to trigger the data update and calculation process every 500 milliseconds, regularly update the position and attitude data of the follower vehicle and the guide vehicle, repeat the heading angle comparison process, and perform continuous deflection angle detection; The deflection adjustment decision module determines whether the current following situation is normal, and issues an early warning for the deviation trend that occurs, thereby updating the strategy for adjusting the direction of the following vehicle. In the deflection adjustment decision module, the process of the deviation trend early warning is as follows: Set the normal range and warning threshold of the relative heading angle, where the normal range is ±5° and the warning threshold is ±10°, and receive the 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 following vehicle relative to the leading vehicle is within the normal range. If it is within the normal range, the current following situation is normal and monitoring continues. If it exceeds the normal range, enter the deviation warning and processing process; Determine the degree of deviation based on the absolute value of the relative heading angle, and calculate the deviation warning coefficient based on the relative heading angle data at multiple consecutive time points to analyze the deviation trend of the relative heading angle; Combined with the analysis results of the deviation trend and the calculated deviation warning coefficient, if the deviation warning coefficient approaches or exceeds the warning threshold, and the deviation trend continues or worsens, the warning mechanism is triggered to notify the driver through sound and light; According to the analysis results of the deviation warning coefficient and the deviation trend, the strategy of adjusting the direction of the following vehicle is updated to correct the deviation and keep the following vehicle on the correct heading. According to the relative heading deflection angle and the deviation trend, the direction to be adjusted of the following 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, and the adjustment instruction is sent to the power and steering execution module of the following vehicle to execute the direction adjustment operation; The calculation formula of the deviation warning coefficient is: Where D is the deviation warning coefficient, Δθ D is the current value of the relative heading angle, θ0 is the preset deviation warning threshold, σ is the standard deviation, which is used to adjust the sensitivity of the deviation warning coefficient, Δθ i is the relative heading angle data at the i-th continuous time point, n is the number of continuous time points, Δθ max is the maximum value of the relative heading deviation angle at consecutive time points, Δθ min It is the minimum value of the relative heading deviation angle at consecutive time points. The value range of D is limited to between 0 and 1, where 0 indicates no deviation and 1 indicates severe deviation; The power and steering execution module issues control instructions according to the strategy of the deflection angle adjustment decision module, and controls the steering, acceleration and braking actions of the following vehicle to achieve precise control of the following vehicle; The autonomous navigation planning module plans the optimal path from the current position to the target position for the following vehicle by comprehensively considering obstacle factors when there is no leading vehicle or the leading vehicle signal is lost, and guides the following vehicle to move forward; The human-computer interaction module provides a visual operation interface to display the operating status of the system.

2. The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables according to claim 1 is characterized by: In the environment perception module, the process of collecting environmental information is as follows: Various types of sensors such as cameras and lidar are deployed on the fruit and vegetable collection and transportation follow-up vehicle to capture the surrounding environment information, including the relative position, speed, and direction information of the guide vehicle, the distance, shape, and size information of surrounding obstacles, and the contour and position information of the road boundary; The camera collects and processes images to identify lanes and obstacles ahead. The laser radar sends a light beam and receives the reflected light simultaneously. The time difference is used to measure the distance to the obstacle and draw a point cloud map to determine the shape, size, and distance of the obstacle. The collected data of the surrounding environment information is transmitted to the electronic control unit of the vehicle for preprocessing, which includes denoising, filtering, data format conversion, and calibration and synchronization of data collected by different sensors; The data from different sensors are fused to obtain comprehensive environmental information, and the position, speed, acceleration of obstacles and geometric shape features of road boundaries are extracted from the fused data to obtain a feature data set.

3. The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables according to claim 2 is characterized by: In the target recognition and positioning module, the process of distinguishing the guide vehicle from other interfering objects is as follows: From the feature data set obtained by the environment perception module, each feature data is traversed one by one, and the feature data of the size, shape and color of the guide vehicle are extracted using image processing technology; Through the machine learning algorithm, the extracted guide vehicle feature data is matched with the pre-trained guide vehicle feature model, and candidate targets that meet the guide vehicle features are screened out from the matching results. The features of the candidate targets are compared with the obstacle features provided by the environmental perception module, and interference objects that do not match the guide vehicle features are eliminated. Based on the selected features of the guide vehicle, the 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 associated with the point cloud data obtained from the sensor, and the tracking algorithm is used to continuously track and lock the guide vehicle. According to the real-time data provided by the environmental perception module, the position and posture information of the guide vehicle are dynamically updated to adapt to environmental changes.

4. The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables according to claim 3 is characterized by: The calculation formula of the coordinate position of the guide vehicle in space is: Where P is the coordinate position of the leading vehicle in space, x and y represent the coordinate position of the leading vehicle on the horizontal plane, d is the horizontal distance between the leading vehicle and the following vehicle, Δy is the vertical distance between the leading vehicle and the following vehicle, θ is the inclination angle of the leading vehicle relative to the horizontal plane, and y is the vertical distance between the leading vehicle and the following vehicle. max It is the maximum value in the vertical direction, which is used to limit the range of y; The calculation formula for the guide vehicle orientation is: Among them, Hd is the direction of the leading vehicle, Δy is the distance between the leading vehicle and the following vehicle in the vertical direction, and d is the distance between the leading vehicle and the following vehicle in the horizontal direction; The calculation formula of the guide vehicle inclination angle is: Among them, Ti is the inclination angle of the leading vehicle, Δh is the height change of the leading vehicle, and d is the distance between the leading vehicle and the following vehicle in the horizontal direction.

5. The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables according to claim 4 is characterized by: In the power and steering execution module, the implementation process of the following vehicle control is as follows: The power and steering execution module receives the adjustment strategy updated by the deflection angle adjustment decision module through the communication interface, extracts the control instructions in the adjustment strategy, and performs instruction parsing; 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, which is steering, acceleration or braking; For the execution of the steering action, if the control instruction is a steering action, the steering mechanism is controlled to steer according to the steering angle parameter; for the execution of the acceleration action, if the control instruction is an acceleration action, the output power of the engine is controlled according to the acceleration parameter to achieve acceleration; for the execution of the braking action, if the control instruction is a braking action, the braking system is controlled to generate 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 following vehicle; During steering, acceleration or braking, the actual status of the vehicle is monitored in real time through sensors, the actual status is compared with the expected target, the execution effect is evaluated, and the execution result is fed back to the deflection angle adjustment decision module through the communication interface. The deflection angle adjustment decision module further adjusts the control strategy according to the feedback result.

6. The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables according to claim 5 is characterized by: In the autonomous navigation planning module, the process of autonomous navigation planning to guide the following vehicle forward is as follows: Based on the surrounding environment information collected by the following vehicle, it is processed and converted into a grid map representation, and the grid map is used to reflect the obstacles and feasible areas in the environment; The visual SLAM algorithm is used to estimate the current position of the following vehicle based on environmental information and known maps, and the posture information of the following vehicle is obtained in real time through the inertial measurement unit sensor; Based on sensor data and map information, obstacles in the environment are detected in real time. According to the location, shape and size of the obstacles, obstacle avoidance strategies are formulated, including detours and parking waiting. Obstacle cost functions are introduced into path planning to evaluate the impact of obstacles in the path, so as to avoid obstacles and optimize the path. Based on the target location and current location, the search algorithm is used to search the map for the shortest path that avoids obstacles and complies with traffic rules. Based on the global path planning, dynamic adjustments and optimizations are made according to the real-time status of the following vehicle and changes in the surrounding environment. According to the path tracking results and the real-time status of the following vehicle, control instructions are generated to control the steering, acceleration and braking actions of the following vehicle to achieve precise control.

7. The automatic following and autonomous navigation system for collecting and transporting fruits and vegetables according to claim 6, characterized in that: The expression of the obstacle cost function is: Among them, C o is the obstacle cost function, which is used to evaluate the impact of obstacles in the path, d j is the distance from the following vehicle to the jth obstacle, σ j is the standard deviation of the obstacle distance, which is used to adjust the sensitivity of the cost function to the distance, m is the total number of obstacles in the path, k is a constant, which is used to adjust the curvature of the exponential function, θ max is the maximum angular deviation allowed in the path, θ j is the angle deviation between the following vehicle and the jth obstacle, C o The value range of is limited to 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.

Images