Track path planning system for automation equipment

Abstract

The invention relates to the technical field of path planning, and discloses a trajectory path planning system for automation equipment, which is characterized in that map data is acquired through a map establishment module, and is processed and analyzed to establish a map model; the path planning module generates a path planning strategy based on the map model, the equipment initial position and the supply point; the motion control module generates a motion control instruction according to the result of the path planning strategy to control the automation equipment; the safety monitoring module continuously monitors the terrain environment, obstacles and equipment states, and evaluates the safety of the motion control instruction in real time; and when the motion control instruction is not safe, the dynamic adjustment module generates a correction signal and adjusts the motion control instruction, so that autonomous positioning, path planning, safe navigation and dynamic adjustment of the automation equipment in a complex environment are realized, the operation efficiency of the automation equipment is improved, the operation safety is guaranteed, and the adaptive capacity of the automation equipment is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a trajectory path planning system for automated equipment. Background Art

[0002] In modern agricultural production, orchard operation robots are important automation equipment and are increasingly widely used. Orchard operation robots can perform multiple tasks such as pruning, spraying and picking, effectively improving the work efficiency and fruit yield of orchard management. The trajectory planning enables the orchard operation robots to quickly calculate and select the shortest path, reduce invalid movements or repeated coverage areas, and improve work efficiency. Accurate path planning helps avoid unnecessary damage to fruit trees and fruits during the operation process, especially in operations such as picking and spraying. However, most of them have not solved how to establish a map model by acquiring map data, generate a path planning strategy, generate motion control instructions, and then control the trajectory path of the automation equipment, and evaluate the safety of the motion control instructions in real time to correct the motion control instructions accordingly.

[0003] For example, the Chinese patent application with the publication number CN117606483A discloses a three-dimensional path planning method for UAVs based on a real-life model of a hilly orchard, which involves the field of precision agriculture technology. In order to solve the problem that the operation of existing plant protection UAVs in hilly and mountainous environments mainly relies on manual control, its accuracy and operation efficiency cannot be guaranteed, and the existing path planning method is mainly applicable to large fields with relatively flat terrain, and its evaluation indicators are mostly operation time and path length. There is a lack of three-dimensional path planning methods for plant protection UAVs applicable to hilly and mountainous orchard environments with frequent terrain changes, and a method for intuitively evaluating the quality of paths based on the size of operation energy consumption. It includes: S1: constructing a real-life three-dimensional model of a mountain orchard; S2: importing three-dimensional coordinate information into Matlab, applying the reciprocating ox-ploughing method, combining the changes in the altitude of the operation area and the operation height of the plant protection UAV for three-dimensional path planning, and obtaining 180 paths in the operation area by changing the operation heading angle of the plant protection UAV.

[0004] For example, a Chinese patent with the authorization announcement number CN116048104B discloses a path planning method, system and electronic device for an orchard operation robot, which relates to the field of path planning technology. The method includes: obtaining an orchard image; obtaining an area of ​​interest in the orchard image, and reconstructing the area of ​​interest into a three-dimensional image to obtain an orchard map; cropping the orchard map, and inputting the cropped orchard map into a semantic segmentation model to obtain a target object; generating an orchard classification map based on the target object splicing; using a threshold classification method to divide the fruit trees into individual fruit trees and rows of fruit trees based on the orchard classification map; converting individual fruit trees into circles, and converting rows of fruit trees into rectangles to obtain a fruit tree distribution map; and planning the operation path of the orchard operation robot based on the fruit tree distribution map. Based on this, the path of the orchard operation robot can be efficiently and simply planned without collision and accurately.

[0005] The above patents have the problems raised by this background technology: the above-mentioned UAV three-dimensional path planning method constructs a real-life three-dimensional model of a mountain orchard, and performs three-dimensional path planning in combination with the changes in the altitude of the operating area and the operating altitude of the plant protection UAV; the above-mentioned orchard operation robot path planning method obtains an orchard map by acquiring the area of ​​interest in the orchard image and reconstructing the three-dimensional image, and then crops the orchard map and inputs it into the semantic segmentation model to obtain the target object, splices it to obtain an orchard classification map, divides the orchard classification map into individual fruit trees and fruit tree rows, and converts it to obtain a fruit tree distribution map to plan the operating path of the orchard robot. The above two patents do not solve the problem of how to establish a map model by acquiring map data, generate a path planning strategy, control the trajectory path of the automation equipment after generating motion control instructions, and evaluate the safety of the motion control instructions in real time to correspondingly correct the motion control instructions. To solve this problem, the present invention proposes a trajectory path planning system for automation equipment. Summary of the invention

[0006] The purpose of this section is to summarize some aspects of the embodiments of the present invention and briefly introduce some preferred embodiments. Some simplifications or omissions may be made in this section and the specification abstract and the invention title of this application to avoid blurring the purpose of this section, the specification abstract and the invention title, and such simplifications or omissions cannot be used to limit the scope of the present invention.

[0007] In view of the above-mentioned problems existing in the existing trajectory path planning system for automation equipment, the present invention is proposed.

[0008] Therefore, an object of the present invention is to provide a trajectory path planning system for automated equipment.

[0009] In order to solve the above technical problems, the present invention provides the following technical solutions: a map building module, a path planning module, a motion control module, a safety monitoring module and a dynamic adjustment module; The map building module is used to obtain map data, process and analyze the map data, and build a map model through a model building strategy; The path planning module is used to generate path planning strategies based on the map model, the initial position of the equipment and the supply point; The motion control module is used to generate motion control instructions based on the results of the path planning strategy and control the automation equipment; The safety monitoring module is used to continuously monitor the terrain environment, obstacles and equipment status, and evaluate the safety of motion control instructions in real time through safety assessment strategies; The dynamic adjustment module is used to generate a correction signal and adjust the motion control instruction when the motion control instruction is unsafe.

[0010] As a preferred solution of the trajectory path planning system for automation equipment described in the present invention, the path planning strategy includes: The traversable area is distinguished from the grids marked as free in the map model. According to the stability of obstacles and terrain properties, the grids corresponding to the traversable area are labeled as traversable. According to the initial position of the equipment and the supply point, the shortest path in the traversable area is found through the path search strategy. If the shortest path is found, the motion control instruction is generated. If the shortest path is not found, the stability and terrain properties of the obstacle are re-differentiated, and the device position and obstacle are updated; The path search strategy includes: Calculate the cost function between the initial position of the device and the supply point, and find the shortest path in the passable area based on the calculation result of the cost function. The calculation formula of the cost function is as follows: ; In the formula, Represents the total cost from the initial location of the equipment to the replenishment point, Represents the distance cost between the initial location of the equipment and the supply point. Represents the terrain cost between the initial location of the equipment and the supply point. Represents the obstacle cost between the device's initial location and the supply point.

[0011] As a preferred solution of the trajectory path planning system for automation equipment described in the present invention, wherein: if the shortest path is found in the traversable area, a motion control instruction is generated, and the motion control instruction includes a speed instruction, a position instruction and a replenishment instruction; The motion control instruction generation strategy includes: dividing the shortest path found in the path planning strategy into path segments, determining the speed of the automation device in each path segment and the position and replenishment amount of the replenishment point, and controlling the automation device to move according to the speed of each path segment and the position of the replenishment point, and filling the replenishment amount of the replenishment point into the automation device; The speed instruction is used to control the speed of the automation equipment in each path; The position instruction is used to control the automation equipment to reach the position of the supply point; The replenishment amount instruction is used to control the replenishment amount filled by the automation equipment.

[0012] As a preferred solution of the trajectory path planning system for automated equipment described in the present invention, wherein: the map data includes equipment location, terrain environment and obstacles; The equipment position includes the equipment initial position and the supply point; Analyze the map data and establish a map model through a model building strategy, wherein the model building strategy includes a feature recognition strategy, a feature analysis strategy and a grid modeling strategy. The feature recognition strategy is used to recognize obstacle features, terrain features and supply point features. The feature analysis strategy is used to classify obstacles according to obstacle features, analyze terrain attributes according to terrain features, and generate a traversable area by combining the obstacle classification results and terrain changes. The grid modeling strategy is used to visualize the map model. Feature recognition strategies include: Obtain obstacles in the map data, identify obstacle features, which include the size and location of the obstacles, obtain terrain environment in the map data, identify terrain features, which include the slope and texture of the terrain, obtain supply points in the map data, identify supply point features, which include the location of the supply points; The function expression of the slope of the terrain is as follows: ; In the formula, Indicates the slope value of the terrain. represents the inverse tangent function, Represents the height difference between two points in the terrain, Indicates the change in the horizontal coordinate distance between two points in the terrain. Represents the change in the vertical coordinate distance between two points in the terrain.

[0013] As a preferred solution of the trajectory path planning system for automation equipment described in the present invention, the feature analysis strategy includes: Classify obstacles according to their characteristics, distinguish the stability of obstacles, identify the changes in the size and position of obstacles at different time points, and configure the change thresholds of the size and position of obstacles. The change thresholds include the size threshold and the position threshold. If the change value of the size of the obstacle is less than or equal to the size threshold, it means that the size of the obstacle has not changed; If the change in the size of the obstacle is greater than the size threshold, it means that the size of the obstacle has changed; If the change value of the obstacle's position is less than or equal to the position threshold, it means that the obstacle's position has not changed; If the change value of the obstacle's position is greater than the position threshold, it means that the obstacle's position has changed; If the size of the obstacle does not change and the position of the obstacle does not change, the stability of the obstacle is static; If the position of the obstacle changes, the stability of the obstacle is dynamic.

[0014] As a preferred solution of the trajectory path planning system for automation equipment described in the present invention, the feature analysis strategy also includes: Analyze terrain attributes according to terrain characteristics and configure the terrain slope threshold. If the terrain slope value is less than or equal to the slope threshold, it means that the terrain is a flat area. If the slope value of the terrain is greater than the slope threshold, it means that the terrain is an uneven area; The terrain attributes are analyzed by combining the slope and texture of the terrain. If the terrain is a flat area and the texture of the terrain is smooth, it means that the terrain attributes are suitable for traffic. If the terrain is a flat area and the texture of the terrain is not smooth, it means that the terrain attribute is to pass with caution; If the terrain is an uneven area and the texture of the terrain is smooth, it means that the terrain attribute is to pass with caution; If the terrain is an uneven area and the texture of the terrain is not smooth, it means that the terrain attribute is not suitable for travel; Combine the classification results of obstacles and terrain attributes to generate a passable area, which includes a suitable area for passage and a cautious area for passage. If the terrain attribute is not suitable for passage, it is an impassable area. If no obstacles are detected, it is a suitable area for passage; If an obstacle is detected, a comprehensive judgment is made based on the stability of the obstacle and the terrain properties. If the stability of the obstacle is static and the terrain properties are suitable for passage, then the area is suitable for passage; If the stability of the obstacle is dynamic or the terrain attribute is cautious passage, it is a cautious passage area.

[0015] As a preferred solution of the trajectory path planning system for automation equipment described in the present invention, the grid modeling strategy includes: Divide the map into two-dimensional grids, each grid represents an area of ​​the map, initially mark each grid as unknown, update the occupancy status of each grid according to the result of the feature analysis strategy, and mark the grid as occupied if an obstacle or supply point is detected in the grid; If no obstacles or supply points are detected in the grid, the grid is marked as free until the grid is marked; The grid attributes of the grid marked as occupied are further distinguished. If the grid marked as occupied is identified as an obstacle, the size, position and stability of the obstacle are determined and added to the grid attributes; If the grid marked as occupied is identified as a recharge point, the location and recharge amount of the recharge point are determined and added to the grid attributes; The grids marked as idle are distinguished into passable areas, and passable labels are added to the grids corresponding to the passable areas.

[0016] As a preferred solution of the trajectory path planning system for automation equipment described in the present invention, the safety of motion control instructions is evaluated in real time through a safety evaluation strategy, and the safety evaluation strategy includes: Continuously monitor the terrain environment of the traversable area. If the slope and texture of the terrain are inconsistent with the traversable labels in the map model, it means that the motion control command is unsafe and a correction signal is generated; If the slope and texture of the terrain are consistent with the passable labels in the map model, the automation equipment will continue to monitor the situation of passing the obstacle. If the automation equipment does not reduce the speed after passing the obstacle, it means that the motion control instruction is unsafe, and a correction signal is generated; If the automation equipment slows down when passing an obstacle, the equipment status of the automation equipment will continue to be monitored. If the equipment status of the automation equipment is abnormal, it means that the motion control instruction is unsafe, and a correction signal will be generated; If there is no abnormality in the device status of the automation equipment, it means that the motion control instruction is safe.

[0017] As a preferred solution of the trajectory path planning system for automation equipment described in the present invention, if the motion control instruction is unsafe, a correction signal is generated, and the correction signal includes: If the slope and texture of the terrain are inconsistent with the traversable labels in the map model, the correction signal is to update the map model and re-execute the path planning strategy; If the automated equipment does not reduce speed when passing an obstacle, the correction signal is to configure a safety buffer signal at the obstacle; If the device state of the automation device is abnormal, the correction signal is to stop the movement of the automation device and correct the automation device.

[0018] Beneficial effects of the present invention: The present invention obtains map data through a map establishment module, processes and analyzes the map data, and establishes a map model, which helps to ensure the accuracy and reliability of path planning and avoid erroneous path planning caused by inaccurate map data; the path planning module generates a path planning strategy based on the map model, the initial position of the equipment and the supply point, reduces travel time, saves energy consumption, and extends the operation cycle of the automation equipment; the motion control module generates motion control instructions according to the results of the path planning strategy, controls the automation equipment, and enables the automation equipment to move safely and efficiently along the planned trajectory path, thereby improving the operating efficiency of the automation equipment and the accuracy of task execution; the safety monitoring module continuously monitors the terrain environment, obstacles and equipment status, and evaluates the safety of the motion control instructions in real time to ensure that when new obstacles appear or the terrain environment changes, it can respond in time to prevent accidents; when the motion control instruction is unsafe, the dynamic adjustment module generates a correction signal and adjusts the motion control instruction, which can adapt to the dynamic changes of the terrain environment and ensure the safety of the automation equipment.

[0019] Through the interaction of the above modules, autonomous positioning, path planning, safe navigation and dynamic adjustment of automation equipment in complex environments are realized, which improves the operating efficiency of automation equipment, ensures operational safety, and enhances the adaptability of automation equipment. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative labor. Among them: Figure 1 A system structure diagram of a trajectory path planning system for automated equipment according to the present invention; Figure 2 A grid modeling strategy flow chart of a trajectory path planning system for automated equipment according to the present invention; Figure 3 A safety assessment strategy flow chart of a trajectory path planning system for automated equipment according to the present invention; Figure 4 The present invention is a method flow chart of a trajectory path planning method for automated equipment. DETAILED DESCRIPTION

[0021] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific implementation methods of the present invention are described in detail below in conjunction with the accompanying drawings.

[0022] In the following description, many specific details are set forth to facilitate a full understanding of the present invention, but the present invention can also be implemented in other ways different from those described herein, and those skilled in the art can make similar generalizations without violating the connotation of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0023] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The term "in one embodiment" that appears in different places in this specification does not necessarily refer to the same embodiment, nor does it refer to a separate or selective embodiment that is mutually exclusive with other embodiments.

[0024] Example 1

[0025] In this embodiment, a system structure diagram of a trajectory path planning system for an automation device is provided, such as Figure 1 As shown, a trajectory path planning system for automated equipment includes a map building module, a path planning module, a motion control module, a safety monitoring module and a dynamic adjustment module.

[0026] The map building module is used to obtain map data, process and analyze the map data, build a map model, ensure the accuracy and reliability of path planning, and avoid incorrect path planning due to inaccurate map data.

[0027] Map data includes device location, terrain environment, and obstacles.

[0028] The equipment location includes the equipment initial location and the supply point.

[0029] Analyze map data and build a map model through model building strategies. The model building strategies include feature recognition strategy, feature analysis strategy and grid modeling strategy. The feature recognition strategy is used to identify obstacle features, terrain features and supply point features. The feature analysis strategy is used to classify obstacles according to obstacle features, analyze terrain properties according to terrain features, and generate a traversable area based on the obstacle classification results and terrain changes. The grid modeling strategy is used to visualize the map model.

[0030] The map data of orchard operations is acquired through a variety of sensors to obtain comprehensive data, and the data from different sensors are integrated to improve the reliability and accuracy of the map data of orchard operations. At the same time, the map data of orchard operations is processed to remove noise and outliers, smooth the map data of orchard operations, and improve the quality of the map data of orchard operations.

[0031] Feature recognition strategies include: Obtain obstacles from map data, identify obstacle features, which include the size and location of obstacles, obtain terrain environment from map data, identify terrain features, which include the slope and texture of terrain, obtain supply points from map data, identify supply point features, which include the location of supply points.

[0032] The function expression of the obstacle size is as follows: ; In the formula, Indicates the size of the obstacle. Indicates obstacle The coordinates of the points, Indicates obstacle The coordinates of the points, Indicates obstacle The coordinates of the points, Indicates the coordinates of the first point of the obstacle.

[0033] What needs to be explained is: the point coordinates of the obstacle , is the starting point of the obstacle, and the point set representing the obstacle is closed.

[0034] The function expression of the obstacle position is as follows: The function expression of the obstacle position is as follows: ; In the formula, The center coordinates representing the location of the obstacle, The total number of points representing obstacles.

[0035] It should be explained that the central coordinate of the obstacle's position is the average value of the sum of the point coordinates corresponding to the obstacle.

[0036] The function expression of the slope of the terrain is as follows: ; In the formula, Indicates the slope value of the terrain. represents the inverse tangent function, Represents the height difference between two points in the terrain, Indicates the change in the horizontal coordinate distance between two points in the terrain. Represents the change in the vertical coordinate distance between two points in the terrain.

[0037] The function expression of the supply point position is as follows: ; In the formula, Indicates the central coordinates of the location of the supply point, Indicates the upper left corner coordinate of the supply point. Indicates the lower right corner coordinate of the supply point's location.

[0038] What needs to be explained is that the supply points are detected by the object detection algorithm, and a bounding box is output at each detected supply point location, where represents the coordinates of the upper left corner of the bounding box, The coordinates representing the lower right corner of the bounding box.

[0039] Obstacles and supply points in the orchard operation process are obtained through the camera, and the obstacles and supply points are detected through the object detection algorithm. The size and position of the detected obstacles are extracted, and the position of the detected supply points is extracted. It is assumed that the obstacles are represented as a set of points, and the size of the obstacles is calculated by the area formula of the closed polygon, and the rectangle with the minimum area surrounding the obstacle is obtained, and the center coordinates of the rectangle are used as the position of the obstacle; the position of the supply point represents the center coordinates of the bounding box.

[0040] The terrain environment of the orchard operation is obtained through a stereo camera. The terrain features are extracted by using image processing technology to analyze the pictures of the terrain environment captured by the stereo camera, and the slope and texture of the terrain are extracted respectively. The slope of the terrain is calculated in the form of points in the pictures of the terrain environment.

[0041] Characterization strategies include: Classify obstacles according to their characteristics, distinguish the stability of obstacles, identify the changes in the size and position of obstacles at different time points, and configure the change thresholds of the size and position of obstacles. The change thresholds include the size threshold and the position threshold. If the change value of the size of the obstacle is less than or equal to the size threshold, it means that the size of the obstacle has not changed; If the change in the size of the obstacle is greater than the size threshold, it means that the size of the obstacle has changed; If the change value of the obstacle's position is less than or equal to the position threshold, it means that the obstacle's position has not changed; If the change value of the obstacle's position is greater than the position threshold, it means that the obstacle's position has changed; If the size of the obstacle does not change and the position of the obstacle does not change, the stability of the obstacle is static; If the position of the obstacle changes, the stability of the obstacle is dynamic.

[0042] Analyze terrain attributes according to terrain characteristics and configure the terrain slope threshold. If the terrain slope value is less than or equal to the slope threshold, it means that the terrain is a flat area. If the slope value of the terrain is greater than the slope threshold, it means that the terrain is an uneven area.

[0043] The terrain attributes are analyzed by combining the slope and texture of the terrain. If the terrain is a flat area and the texture of the terrain is smooth, it means that the terrain attributes are suitable for traffic. If the terrain is a flat area and the texture of the terrain is not smooth, it means that the terrain attribute is to pass with caution; If the terrain is an uneven area and the texture of the terrain is smooth, it means that the terrain attribute is to pass with caution; If the terrain is an uneven area and the texture of the terrain is not smooth, it means that the terrain attributes are not suitable for travel.

[0044] Combine the classification results of obstacles and terrain attributes to generate a passable area, which includes a suitable area for passage and a cautious area for passage. If the terrain attribute is not suitable for passage, it is an impassable area. If no obstacles are detected, it is a suitable area for passage; If an obstacle is detected, a comprehensive judgment is made based on the stability of the obstacle and the terrain properties. If the stability of the obstacle is static and the terrain properties are suitable for passage, then the area is suitable for passage; If the stability of the obstacle is dynamic or the terrain attribute is cautious passage, it is a cautious passage area.

[0045] The changes in the size and position of obstacles are identified and monitored, and the stability of obstacles is distinguished for the changes in the size and position of obstacles within the change threshold, and the terrain properties are comprehensively judged based on the slope and texture of the terrain. Careful passage includes two situations: when the terrain is a flat area, but the texture of the terrain is not smooth and rough, the automated equipment, that is, the orchard operating robot, will have difficulty in operation due to the roughness of the ground during operation. At this time, it is necessary to pass carefully to prevent the orchard operating robot from colliding and overturning due to the bumpy ground; when the terrain is an uneven area and the texture of the terrain is smooth, the orchard operating robot may go uphill or downhill, so it is necessary to pass carefully to prevent slipping.

[0046] The classification results of the obstacles and the terrain attributes are combined to generate the passable area. When the terrain attribute is not suitable for passage, it means that the terrain belongs to the impassable area. When there are no obstacles in the terrain, it means that the terrain is suitable for passage. When there are obstacles in the terrain, if the obstacle is dynamic or the terrain attribute is cautious passage, then the terrain belongs to the cautious passage area, and it is necessary to avoid dynamic obstacles or pay attention to the uneven terrain or the uneven terrain texture.

[0047] Grid modeling strategies such as Figure 2 As shown, specifically including: The map is divided into two-dimensional grids, each grid represents the area of ​​the map. Initially, each grid is marked as unknown. The occupancy status of each grid is updated according to the results of the feature analysis strategy. If an obstacle or supply point is detected in the grid, the grid is marked as occupied.

[0048] If no obstacles or supply points are detected in the grid, the grid is marked as free until the grid is marked.

[0049] The grid attributes of the grid marked as occupied are further distinguished. If the grid marked as occupied is identified as an obstacle, the size, position and stability of the obstacle are determined and added to the grid attributes.

[0050] If a raster marked as occupied is identified as a recharge point, the location and amount of the recharge point are determined and added to the raster attributes.

[0051] The grids marked as idle are distinguished into passable areas, and passable labels are added to the grids corresponding to the passable areas.

[0052] The above map represents the entire orchard operation area. The identification of obstacles and supply points is of great significance for trajectory path planning. When the grid marked as occupied is identified as an obstacle, the stability of the obstacle refers to whether the obstacle is static or dynamic. The corresponding grid attribute is marked as the attribute of the corresponding obstacle. When the grid marked as occupied is identified as a supply point, the corresponding grid attribute is marked as the position and supply amount of the corresponding supply point. First, the position of the supply point is determined. The automated equipment here is the orchard operation robot. The shortest path of the orchard operation robot from the initial position of the equipment to the supply point is the trajectory path. The planning goal, and secondly, the supply amount of the supply point needs to be determined. During the orchard operation, the supply amount of the supply point is different, and the supply amount is preset manually. After the location and supply amount of the supply point are clarified, the orchard operation robot can receive the motion control instructions and perform the orchard supply operation through the planned trajectory path in a timely manner. For the grid marked as idle, it is necessary to add a passable label based on whether it is a passable area. The passable label describes the slope and texture of the passable area, which helps to ensure the accuracy and reliability of path planning and avoid incorrect path planning due to inaccurate map data.

[0053] The path planning module is used to generate a path planning strategy based on the map model, the initial location of the equipment and the supply point to reduce travel time and save energy consumption.

[0054] Path planning strategies include: The traversable area is distinguished from the grids marked as free in the map model. According to the stability of obstacles and terrain properties, the grids corresponding to the traversable area are labeled as traversable. According to the initial position of the equipment and the supply point, the shortest path in the traversable area is found through the path search strategy. If the shortest path is found, the motion control instruction is generated. If the shortest path is not found, the stability and terrain properties of the obstacles are re-differentiated, and the device position and obstacles are updated.

[0055] Re-classifying the stability of obstacles requires dynamic adjustment of the size threshold and change threshold. Re-classifying the terrain attributes requires dynamic adjustment of the slope threshold. At the same time, if new obstacles are found during the path search process, they need to be displayed in the map model and the location of the orchard operation robot needs to be updated in real time.

[0056] Path search strategies include: Calculate the cost function between the initial position of the device and the supply point, and find the shortest path in the passable area based on the calculation result of the cost function. The calculation formula of the cost function is as follows: ; In the formula, Represents the total cost from the initial location of the equipment to the replenishment point, Represents the distance cost between the initial location of the equipment and the supply point. Represents the terrain cost between the initial location of the equipment and the supply point. Represents the obstacle cost between the device's initial location and the supply point.

[0057] The function expression of distance cost is as follows: ; In the formula, Represents the distance cost between the initial location of the equipment and the supply point. represents the coordinates of the supply point, Coordinates representing the initial position of the device.

[0058] The function expression of terrain cost is as follows: ; In the formula, Represents the terrain cost between the initial location of the equipment and the supply point. Indicates the passable area. Indicates an impassable area.

[0059] The function expression of obstacle cost is as follows: ; In the formula, Represents the obstacle cost between the initial position of the device and the supply point, Indicates that there are no obstacles or the stability of obstacles is static, Indicates that the stability of the obstacle is dynamic.

[0060] The motion control module is used to generate motion control instructions according to the results of the path planning strategy, control the automation equipment, and improve the operating efficiency of the automation equipment and the accuracy of task execution.

[0061] If the shortest path is found in the traversable area, a motion control instruction is generated, which includes a speed instruction, a position instruction and a replenishment instruction.

[0062] The strategy for generating motion control instructions includes: dividing the shortest path found in the path planning strategy into path segments, determining the speed of the automation equipment in each path segment and the position and supply amount of the supply point, and controlling the automation equipment to move according to the speed of each path segment and the position of the supply point, and filling the supply amount of the supply point into the automation equipment.

[0063] Speed ​​command is used to control the speed of automation equipment in each path.

[0064] Position instructions are used to control the automation equipment to reach the position of the supply point.

[0065] The replenishment volume instruction is used to control the replenishment volume filled by the automation equipment.

[0066] Among them, the path segment is the key path segment that the orchard working robot needs to pass in sequence, which is decomposed into the shortest path from the initial position of the equipment to the supply point. It is necessary to determine the time required for the orchard working robot in each path segment, the location of the supply point that needs to be reached in the end, and the supply quantity required.

[0067] The safety monitoring module is used to continuously monitor the terrain environment, obstacles and equipment status, and evaluate the safety of motion control instructions in real time to ensure that when new obstacles or terrain environment changes appear, timely responses can be made to prevent accidents.

[0068] The safety of motion control instructions is evaluated in real time through safety assessment strategies. Figure 3 As shown, specifically including: Continuously monitor the terrain environment of the traversable area. If the slope and texture of the terrain are inconsistent with the traversable labels in the map model, it means that the motion control command is unsafe and a correction signal is generated; If the slope and texture of the terrain are consistent with the passable labels in the map model, the automation equipment will continue to monitor the situation of passing the obstacle. If the automation equipment does not reduce the speed after passing the obstacle, it means that the motion control instruction is unsafe, and a correction signal is generated; If the automation equipment slows down when passing an obstacle, the equipment status of the automation equipment will continue to be monitored. If the equipment status of the automation equipment is abnormal, it means that the motion control instruction is unsafe, and a correction signal will be generated; If there is no abnormality in the device status of the automation equipment, it means that the motion control instruction is safe.

[0069] When the terrain experiences landslides or collapses, the slope and texture of the terrain will change, indicating that there is a risk in the motion control instruction. At this time, the map model needs to be updated and the path planning strategy needs to be re-executed; when the orchard working robot moves on the path segment according to the speed instruction, it needs to reduce the speed when passing around obstacles; during the movement of the orchard working robot, its equipment status is monitored to see if there is any abnormality in the equipment status, including whether the orchard working robot has sufficient power, whether it encounters new obstacles on the path segment, and whether the orchard working robot performs corresponding operations when receiving the corresponding motion control instruction. If there are no problems with the slope and texture of the above-mentioned terrain, the speed adjustment when encountering obstacles, and the equipment status, then the motion control instruction is safe and the orchard working robot needs to move according to the motion control instruction. If there is a problem with one of the slope and texture of the above-mentioned terrain, the speed adjustment when encountering obstacles, and the equipment status, then a corresponding correction signal needs to be generated.

[0070] The dynamic adjustment module is used to generate correction signals when the motion control instructions are unsafe, adjust the motion control instructions, adapt to dynamic changes in the terrain environment, and ensure the safety of the automation equipment.

[0071] If the motion control instruction is unsafe, a correction signal is generated. The correction signal includes: If the slope and texture of the terrain are inconsistent with the traversable labels in the map model, the correction signal is to update the map model and re-execute the path planning strategy; If the automated equipment does not reduce speed when passing an obstacle, the correction signal is to configure a safety buffer signal at the obstacle; If the device state of the automation device is abnormal, the correction signal is to stop the movement of the automation device and correct the automation device.

[0072] After updating the map model, it is necessary to re-execute the path planning strategy to find the shortest path from the initial position of the equipment to the supply point, re-evaluate the terrain cost and obstacle cost according to the actual situation, and update the position instruction; the safety buffer signal is to control the orchard operation robot to increase vigilance and reduce speed when moving around obstacles in order to face emergencies, and update the speed instruction; after stopping the movement of the orchard operation robot, correct the automation equipment, including replenishing power, replanning the shortest path to the supply point after encountering new obstacles, and manual adjustment when the orchard operation robot does not move according to the motion control instruction. Manual adjustment reduces the speed of the orchard operation robot in the event of a landslide, and manually adjusts the position instruction in time when there is a deviation between the equipment position and the preset position to guide the orchard operation robot back to a safe path segment, and update the position instruction and speed instruction.

[0073] Example 2 In this embodiment, a method flow chart of a trajectory path planning method for an automation device is provided, such as Figure 4 As shown, a trajectory path planning method for automated equipment includes: S1. Obtain map data, process and analyze the map data, and establish a map model.

[0074] S2. Generate a path planning strategy based on the map model, the initial position of the equipment and the supply point.

[0075] S3. Generate motion control instructions based on the results of the path planning strategy to control the automation equipment.

[0076] S4. Continuously monitor the terrain environment, obstacles and equipment status, and evaluate the safety of motion control instructions in real time.

[0077] S5. When the motion control instruction is unsafe, a correction signal is generated to adjust the motion control instruction.

[0078] The specific contents of a trajectory path planning method for automated equipment can be found in a trajectory path planning system for automated equipment, which will not be described in detail here.

[0079] Example 3

[0080] In this embodiment, a computer device is provided, including a memory and a processor, the memory is used to store instructions, and the processor is used to execute the instructions, so that the computer device executes the steps of implementing the above-mentioned trajectory path planning method for automated equipment.

[0081] Example 4

[0082] In this embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed, the steps of the above-mentioned trajectory path planning method for automated equipment are implemented.

[0083] The computer-readable storage medium includes: a U disk, a mobile hard disk, a read-only memory, a random access memory, a magnetic disk or an optical disk, and other media for storing program codes.

[0084] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should be included in the scope of the claims of the present invention.

Claims

1. A trajectory path planning system for automated equipment, characterized in that: include: Map building module, path planning module, motion control module, safety monitoring module and dynamic adjustment module; The map building module is used to obtain map data, process and analyze the map data, and build a map model through a model building strategy; The path planning module is used to generate path planning strategies based on the map model, the initial position of the equipment and the supply point; The motion control module is used to generate motion control instructions based on the results of the path planning strategy and control the automation equipment; The safety monitoring module is used to continuously monitor the terrain environment, obstacles and equipment status, and evaluate the safety of motion control instructions in real time through safety assessment strategies; The dynamic adjustment module is used to generate a correction signal and adjust the motion control instruction when the motion control instruction is unsafe.

2. A trajectory path planning system for automated equipment according to claim 1, characterized in that: The path planning strategy includes: The traversable area is distinguished from the grids marked as free in the map model. According to the stability of obstacles and terrain properties, the grids corresponding to the traversable area are labeled as traversable. According to the initial position of the equipment and the supply point, the shortest path in the traversable area is found through the path search strategy. If the shortest path is found, the motion control instruction is generated. If the shortest path is not found, the stability and terrain properties of the obstacle are re-differentiated, and the device position and obstacle are updated; The path search strategy includes: Calculate the cost function between the initial position of the device and the supply point, and find the shortest path in the passable area based on the calculation result of the cost function. The calculation formula of the cost function is as follows: ； In the formula, Represents the total cost from the initial location of the equipment to the replenishment point, Represents the distance cost between the initial location of the equipment and the supply point. Represents the terrain cost between the initial location of the equipment and the supply point. Represents the obstacle cost between the device's initial location and the supply point.

3. A trajectory path planning system for automated equipment as claimed in claim 2, characterized in that: If the shortest path is found in the passable area, a motion control instruction is generated, and the motion control instruction includes a speed instruction, a position instruction and a supply amount instruction; The motion control instruction generation strategy includes: dividing the shortest path found in the path planning strategy into path segments, determining the speed of the automation device in each path segment and the position and replenishment amount of the replenishment point, and controlling the automation device to move according to the speed of each path segment and the position of the replenishment point, and filling the replenishment amount of the replenishment point into the automation device; The speed instruction is used to control the speed of the automation equipment in each path; The position instruction is used to control the automation equipment to reach the position of the supply point; The replenishment amount instruction is used to control the replenishment amount filled by the automation equipment.

4. A trajectory path planning system for automated equipment as claimed in claim 3, characterized in that: The map data includes device location, terrain environment and obstacles; The equipment position includes the equipment initial position and the supply point; Analyze the map data and establish a map model through a model building strategy, wherein the model building strategy includes a feature recognition strategy, a feature analysis strategy and a grid modeling strategy. The feature recognition strategy is used to recognize obstacle features, terrain features and supply point features. The feature analysis strategy is used to classify obstacles according to obstacle features, analyze terrain attributes according to terrain features, and generate a traversable area by combining the obstacle classification results and terrain changes. The grid modeling strategy is used to visualize the map model. Feature recognition strategies include: Obtain obstacles in the map data, identify obstacle features, which include the size and location of the obstacles, obtain terrain environment in the map data, identify terrain features, which include the slope and texture of the terrain, obtain supply points in the map data, identify supply point features, which include the location of the supply points; The function expression of the slope of the terrain is as follows: ； In the formula, Indicates the slope value of the terrain. represents the inverse tangent function, Represents the height difference between two points in the terrain, Indicates the change in the horizontal coordinate distance between two points in the terrain. Represents the change in the vertical coordinate distance between two points in the terrain.

5. A trajectory path planning system for automated equipment as claimed in claim 4, characterized in that: The feature analysis strategy includes: Classify obstacles according to their characteristics, distinguish the stability of obstacles, identify the changes in the size and position of obstacles at different time points, and configure the change thresholds of the size and position of obstacles. The change thresholds include the size threshold and the position threshold. If the change value of the size of the obstacle is less than or equal to the size threshold, it means that the size of the obstacle has not changed; If the change in the size of the obstacle is greater than the size threshold, it means that the size of the obstacle has changed; If the change value of the obstacle's position is less than or equal to the position threshold, it means that the obstacle's position has not changed; If the change value of the obstacle's position is greater than the position threshold, it means that the obstacle's position has changed; If the size of the obstacle does not change and the position of the obstacle does not change, the stability of the obstacle is static; If the position of the obstacle changes, the stability of the obstacle is dynamic.

6. A trajectory path planning system for automated equipment according to claim 5, characterized in that: The feature analysis strategy also includes: Analyze terrain attributes according to terrain characteristics and configure the terrain slope threshold. If the terrain slope value is less than or equal to the slope threshold, it means that the terrain is a flat area. If the slope value of the terrain is greater than the slope threshold, it means that the terrain is an uneven area; The terrain attributes are analyzed by combining the slope and texture of the terrain. If the terrain is a flat area and the texture of the terrain is smooth, it means that the terrain attributes are suitable for traffic. If the terrain is a flat area and the texture of the terrain is not smooth, it means that the terrain attribute is to pass with caution; If the terrain is an uneven area and the texture of the terrain is smooth, it means that the terrain attribute is to pass with caution; If the terrain is an uneven area and the texture of the terrain is not smooth, it means that the terrain attribute is not suitable for travel; Combine the classification results of obstacles and terrain attributes to generate a passable area, which includes a suitable area for passage and a cautious area for passage. If the terrain attribute is not suitable for passage, it is an impassable area. If no obstacles are detected, it is a suitable area for passage; If an obstacle is detected, a comprehensive judgment is made based on the stability of the obstacle and the terrain properties. If the stability of the obstacle is static and the terrain properties are suitable for passage, then the area is suitable for passage; If the stability of the obstacle is dynamic or the terrain attribute is cautious passage, it is a cautious passage area.

7. A trajectory path planning system for automated equipment according to claim 6, characterized in that: The grid modeling strategy includes: Divide the map into two-dimensional grids, each grid represents an area of ​​the map, initially mark each grid as unknown, update the occupancy status of each grid according to the result of the feature analysis strategy, and mark the grid as occupied if an obstacle or supply point is detected in the grid; If no obstacles or supply points are detected in the grid, the grid is marked as free until the grid is marked; The grid attributes of the grid marked as occupied are further distinguished. If the grid marked as occupied is identified as an obstacle, the size, position and stability of the obstacle are determined and added to the grid attributes; If the grid marked as occupied is identified as a recharge point, the location and recharge amount of the recharge point are determined and added to the grid attributes; The grids marked as idle are distinguished into passable areas, and passable labels are added to the grids corresponding to the passable areas.

8. A trajectory path planning system for automated equipment according to claim 7, characterized in that: The safety of motion control instructions is evaluated in real time through a safety evaluation strategy, which includes: Continuously monitor the terrain environment of the traversable area. If the slope and texture of the terrain are inconsistent with the traversable labels in the map model, it means that the motion control command is unsafe and a correction signal is generated; If the slope and texture of the terrain are consistent with the passable labels in the map model, the automation equipment will continue to monitor the situation of passing the obstacle. If the automation equipment does not reduce the speed after passing the obstacle, it means that the motion control instruction is unsafe, and a correction signal is generated; If the automation equipment slows down when passing an obstacle, the equipment status of the automation equipment will continue to be monitored. If the equipment status of the automation equipment is abnormal, it means that the motion control instruction is unsafe, and a correction signal will be generated; If there is no abnormality in the device status of the automation equipment, it means that the motion control instruction is safe.

9. A trajectory path planning system for automated equipment according to claim 8, characterized in that: If the motion control instruction is unsafe, a correction signal is generated, and the correction signal includes: If the slope and texture of the terrain are inconsistent with the traversable labels in the map model, the correction signal is to update the map model and re-execute the path planning strategy; If the automated equipment does not reduce speed when passing an obstacle, the correction signal is to configure a safety buffer signal at the obstacle; If the device state of the automation device is abnormal, the correction signal is to stop the movement of the automation device and correct the automation device.

Images