Path planning method of fermented grain taking robot in fermentation workshop

Abstract

The invention belongs to the technical field of AGV robot motion control, and discloses a path planning method for a fermented grain taking robot in a fermentation workshop, aiming at a special operation environment of the fermented grain fermentation workshop, a path planning task is divided into two parts, namely a large-scene aisle corridor and a laying ground cylinder area, and the large-scene aisle corridor is based on a constructed two-dimensional grid map; a global path planning algorithm of dynamic obstacle avoidance is realized by adopting an A * algorithm and a dynamic window method, algorithm optimization is performed in combination with a security neighborhood, a deviation function and the like, the path efficiency is improved, and the path redundancy is reduced; for a ground cylinder laying area, an image enhancement technology based on an S membership function and a non-sharpening mask and an ORB-SLAM2 algorithm of adaptive feature points are adopted to realize robot self-positioning, ground cylinders are detected through a YOLOv10 target detection algorithm, key point information is extracted, coordinates of drivable path points and robot coordinates under a world coordinate system are obtained, and the robot is positioned through a robot. The path planning of a laying ground cylinder area is realized; the device realizes automatic operation of the fermented grain discharge link of the winery, improves the production efficiency of the winery, and improves the economic benefits of the winery.

Description

Technical Field

[0001] The invention belongs to the technical field of AGV robot motion control, and relates to a wine-wine mash taking robot in a winery, and specifically to a path planning method of the wine-wine mash taking robot in a fermentation plant. Background Art

[0002] At present, light-fragrance liquor mainly uses ground jars as fermentation containers. The method of taking materials for light-fragrance liquor is mainly the traditional manual method of taking mash, which has problems such as large workload and high labor intensity. Most of the existing mash out-of-jar devices on the market still require human-machine collaboration and have not achieved full automation and intelligence.

[0003] In order to reduce the workload of workers, reduce the labor intensity of workers, realize the intelligent and automated material extraction of mash in the winery, and complete industrial upgrading, it is necessary to study a new type of fermentation plant mash extraction robot to replace workers in the fermentation plant to complete the mash extraction. The mash extraction robot relies on the AGV chassis, robotic arms and material extraction devices to complete the positioning, digging, transportation and placement of the mash in the ground tank, thereby replacing manual work to complete the mash extraction operation for a certain ground tank; the role of the AGV chassis for the robot is to enable the robot to walk in the mash fermentation plant, reach the designated location for digging the ground tank, and complete the robot's autonomous navigation operation.

[0004] The environment inside the fermentation plant includes a ground tank floor area with low texture, narrow spacing and uneven distribution of ground tanks, and a relatively spacious large-scene aisle corridor area. Compared with the large-scene aisle corridor area, the fermentation plant is special in the ground tank area. Fenxiang liquor uses ground tanks as fermentation containers. Ground tanks are wide-mouthed containers made of clay and buried underground. The mouth of the ground tank is level with the ground. They are densely distributed in the fermentation plant. The raw materials are fermented in the ground tanks to produce mash. The geology of the deployment area is soft, and the spacing between ground tanks is narrow. Limited by the manufacturing process of old ground tanks, they are not absolutely circular, the specifications are different, and the distribution is not absolutely uniform, and there are deviations. For this special environment in the mash fermentation plant, the path planning method of the robot AGV chassis has become the key to realizing the fully automated operation of the mash out of the tank link in the brewing process of light-fragrance liquor. Summary of the invention

[0005] The purpose of the present invention is to provide a path planning method for a mash taking robot in a fermentation plant, so as to help the mash taking robot in the fermentation plant to complete path planning scientifically and efficiently, thereby achieving the improvement of the economic benefits and production efficiency of the winery.

[0006] The technical solution adopted by the present invention to achieve the above-mentioned purpose is:

[0007] A path planning method for a fermentation plant's mash taking robot comprises the following steps:

[0008] S1. Divide the mash fermentation plant into a large-scene corridor area and a floor tank area.

[0009] S2. In the mash fermentation plant, a global grid map is constructed using two-dimensional lidar as map information for path planning in large-scale corridor areas.

[0010] S3. Based on the constructed global grid map, the paved ground cylinder area that cannot be identified by the radar is manually added as a non-drivable area, that is, by manually adding obstacles, the paved ground cylinder area is isolated by obstacles in the constructed global grid map.

[0011] S4. Use a dynamic path planning algorithm based on safe neighborhood and path efficiency optimization to complete global obstacle avoidance path planning in large-scale corridor areas, and reach the designated target point from the starting point in an optimal way;

[0012] The specific steps include:

[0013] S41, select the starting point and the target point of the global grid map, create an OpenList list and a CloseList list for storing the nodes traversed and searched by the A* algorithm, and add the starting point to the OpenList list;

[0014] S42, taking the starting point as the initial node, according to the node search direction, using the A* algorithm to implement the expansion node evaluation through the cost function F, evaluating the node cost, and the node with a smaller cost has a higher priority; repeating the above search process until the target point is found;

[0015] After node cost evaluation, the selected path nodes are all minimum cost nodes, so the path with the minimum path cost is the optimal path;

[0016] The cost function F is:

[0017] F=G+ln|H D +1|×H D +μ×S n +ρ×distance

[0018] Where F is the total estimated path cost from the starting point to the target point of the path where the current node is located; G is the actual path cost between the starting point and the current node; H D is the heuristic function in the form of diagonal distance, ln|H D +1| is the dynamic weight given to the heuristic function; S n is the number of obstacles in the safe neighborhood matrix; distance is the deviation distance function; μ and ρ are weight coefficients. The safe neighborhood matrix is:

[0019]

[0020] Where n is the current search node; p1, p d These are neighborhood nodes of different ranges in the grid map.

[0021] When the safe neighborhood is 1, the number of obstacles in the neighboring node p1 of node n is judged. When the safe neighborhood is d, the number of obstacles in p d The selection of the safe distance depends on the size of the robot and the map resolution. Here, the robot's minimum circumscribed circle radius is set to R, the grid map resolution is b, and the unit is m / pixel. Then its safe neighborhood is d=R / b. S43. Filter the path points with a large number of redundant nodes obtained by the A* algorithm;

[0022] The specific steps include:

[0023] 1) Primary path point screening: delete consecutive points in the same direction from the path points;

[0024] 2) Secondary path point screening: retain path points where the straight line between path points does not pass through obstacles;

[0025] S44, the path is evaluated using the improved DWA trajectory evaluation function;

[0026] In order to realize the random obstacle avoidance of the robot, the dynamic window method DWA is used to predict the trajectory evaluation function, and the distance between the terminal trajectory point and the global path is added as the trajectory deviation function. The smaller the value of the trajectory deviation function, the higher the score of the evaluation function; and the larger the value of Eval(v,ω), the higher the score. The improved evaluation function is:

[0027] Eval(v,ω)=α·Heading(v,ω)+β·Dist(v,ω)+γ·Vel(v,ω)

[0028] N_Eval(v,ω)=Eval(v,ω)-σ·F_line(v,ω)

[0029] Where Eval(v,ω) is the traditional trajectory evaluation function; N_Eval(v,ω) is the improved trajectory evaluation function; Heading(v,ω) is the azimuth evaluation sub-function; Dist(v,ω) is the obstacle distance evaluation sub-function; Vel(v,ω) is the speed evaluation sub-function; F_line(v,ω) is the trajectory deviation evaluation sub-function; α, β, γ, σ are the evaluation sub-function weight coefficients.

[0030] S45, for the path points obtained by path optimization, the dynamic window method DWA algorithm is combined to realize dynamic obstacle avoidance under the global map. At this time, the path points obtained by the A* algorithm are set as the sub-starting point and sub-target point of the DWA algorithm in sequence to complete the local path planning. The sub-target point is judged by the current coordinates of the robot to see whether it is the global target point coordinate. When the sub-target point is the global target point, the path planning of this stage is completed. The robot reaches the global target point from the global starting point to realize global dynamic path planning.

[0031] S5. When the robot completes the path planning of the large-scale aisle corridor area and reaches the specified global target point, the robot is adjusted according to the given posture information so that it faces the target column of ground cylinders. The robot path planning sensor is switched from a two-dimensional laser radar to an RGBD camera. The robot posture at the switching task is used as the initial posture for path planning in the ground cylinder area.

[0032] S6, RGBD camera looks down at the ground area including the ground tank, and estimates the robot's posture information through the visual odometer. The specific steps include:

[0033] S61. Use the ORB-SLAM2 algorithm to realize the visual odometer pose estimation. Collect image information in real time through the camera. Before the image is passed to the ORB-SLAM2 algorithm, convert the image from the RGB color space to the HSI color space. The calculation formula of the brightness I in the HSI space is:

[0034]

[0035] Extract the I channel in the HSI color space and process it as a grayscale image;

[0036] S62, realizing image histogram adaptive cropping based on S-type membership function, the selected S-type membership function is:

[0037]

[0038] In the formula, x max and x min is the maximum and minimum value of the grayscale image pixel;

[0039] The clipping values ​​of different gray levels of the histogram are calculated according to the following formula;

[0040]

[0041] Where m is the number of pixels in the image; SMF(l) is the calculated function value corresponding to gray level l; L is the maximum gray level of the gray image; ave is the gray mean; mse is the pixel mean square error; ave / mse constitutes the maximum allowable slope; Crop is the upper limit of the histogram clipping value;

[0042] In the histogram equalization process, when the number of pixels with a certain gray value exceeds the calculated clipping upper limit, the excess pixels are evenly distributed on other gray levels to obtain a new histogram, thereby achieving image enhancement by improving image contrast.

[0043] S63, enhance image details through unsharp masking technology;

[0044] S64, the grayscale image processed by S62 and S63 is combined with the color gamut image H and the saturation image S to obtain an enhanced HSI image, which is then converted into a color image in the RGB color space;

[0045] S65. Combine the adaptive threshold to improve the FAST corner extraction method in the ORB-SLAM2 algorithm, and calculate the deviation of the pixel gray value in the image as its adaptive threshold T:

[0046]

[0047] In the formula, is the mean value of image pixels; H and W are the image height and width; λ is the scale factor. Initially set λ = 1. If the feature points that meet the conditions cannot be extracted in the divided grid, set the step size to 0.05 and gradually reduce λ until the feature point extraction requirements are met, and set the minimum threshold T min ;

[0048] S66. Use the enhanced color image as the input of ORB-SLAM2, and combine it with the ORB feature points with adaptive thresholds to perform feature point matching and filter out incorrect matching pairs, and finally obtain the estimated pose transformation. Then, through the corresponding pose relationship between the robot and the installed camera, calculate the pose information of the robot in the world coordinate system to achieve self-positioning of the floor tank paving area.

[0049] S7. In the area where the ground cylinder is laid, use an RGBD camera to collect ground color images, detect the ground cylinder through a target detection algorithm, obtain the edge information of the ground cylinder, and obtain the drivable image coordinate system path points; the specific steps include:

[0050] S71. Take images of the ground tank and create a data set for model training of the YOLOv10 algorithm.

[0051] S72, using the trained model to perform ground cylinder target detection on the collected ground color image, and obtain the ground cylinder detection frame coordinate information in the image coordinate system;

[0052] S73, extracting the drivable road section information between two laterally adjacent circular ground cylinders and the center of the current row of ground cylinders through the detection frame coordinate information of two laterally adjacent ground cylinders;

[0053] S74, determining the path points in the final image coordinate system according to the left and right wheel tracks, track width and identified drivable road section information of the mash taking robot.

[0054] S8. In the area where the ground vat is laid, the target detection algorithm and the visual odometry are combined to obtain the drivable path points in the image coordinate system, which are converted into coordinates in the camera coordinate system. Based on the relative position of the camera to the chassis of the mash retrieval robot and the current position of the robot in the world coordinate system estimated by the visual odometry, the coordinates of the robot and the path points in the world coordinate system are obtained. The path points are used as guidance to control the robot to reach the designated digging position for digging the ground vat.

[0055] Compared with the prior art, the present invention has the following beneficial effects:

[0056] Aiming at the special working environment of the mash fermentation plant, the present invention divides the robot path planning in the fermentation plant into two parts: the path planning task under the two-dimensional grid map of the large-scene aisle corridor and the path planning task of the ground cylinder paving area with many potholes and few features, which are considered separately and applied in a composite manner; the path planning in the large-scene aisle corridor relies on the grid map constructed by laser SLAM and manually improved, and combines the A* algorithm and the dynamic window method to propose a dynamic path planning algorithm for optimizing the safe neighborhood and path efficiency; the path planning in the ground cylinder paving area is realized by the visual odometer of the ORB-SLAM2 algorithm and the YOLOv10 target detection algorithm, which comprehensively considers the ground environment characteristics of the ground cylinder paving area with many potholes and few features, and then obtains drivable path points to complete the path planning task. The method of the present invention enables the material-grabbing robot in the mash fermentation plant to autonomously navigate in the mash fermentation plant including the ground vat floor with low texture, narrow spacing and uneven distribution and the large-scene aisle corridor, so as to realize the fully automated operation of the mash out of the vat link in the brewing process of light-fragrance liquor, thereby improving the production efficiency of the winery and enhancing the economic benefits of the winery. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 It is a design flow chart of the method of the present invention;

[0058] Figure 2 This is a schematic diagram of the fermentation plant and the robot operating in the corridor of the large scene;

[0059] Figure 3 This is a schematic diagram of the fermentation plant and the robot working in the area where the ground tank is laid;

[0060] Figure 4 It is a global grid map of the fermentation plant constructed by two-dimensional laser radar;

[0061] Figure 5 It is a global grid map of the fermentation plant that has been manually improved;

[0062] Figure 6 It is a schematic diagram of deviation distance;

[0063] Figure 7 It is a schematic diagram of the ground cylinder detection bounding box in the image coordinate system. DETAILED DESCRIPTION

[0064] In order to make the above-mentioned purposes, features and beneficial effects of the present invention more obvious and understandable, the technical scheme of the present invention will be clearly and completely described below in combination with the embodiments of the present invention and the accompanying drawings. Obviously, the described embodiments are only 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 belong to the scope of protection of the present invention.

[0065] Example

[0066] Aiming at the material-taking process of ground-jar fermentation of light-fragrant liquor, a mash-taking robot consisting of an AGV chassis, a robotic arm and a material-taking device is suitable for complex mash-taking operations. In order to realize the path planning task of the AGV chassis of the mash-taking robot in a mash fermentation plant consisting of a large-scene corridor and a ground-jar area, a path planning method for the mash-taking robot in a fermentation plant is provided. The specific process is as follows: Figure 1 As shown, including:

[0067] S1, divide the path planning task of the fermentation plant into large-scale corridor path planning and ground tank area path planning, and adopt different path planning strategies;

[0068] Since the large-scale corridor is a flat road section, the focus is on the obstacle information on the ground; the paved ground cylinder area is a section with many potholes, and the robot movement mode is that the robot straddles the current row of ground cylinders, and the left and right wheels of the AGV respectively drive on the drivable plane between the current row of ground cylinders and the left and right rows of ground cylinders; the schematic diagram of the fermentation plant and the robot operation in the large-scale corridor and the paved ground cylinder area is shown in the figure. Figure 2 and Figure 3 shown.

[0069] S2, a global grid map is constructed in the mash fermentation plant through two-dimensional lidar, which serves as a map for large-scale corridor path planning.

[0070] S3, based on the constructed global grid map, manually add the non-drivable areas that cannot be identified by the radar, that is, the areas where the ground tank is laid, to improve the global grid map information;

[0071] Since the two-dimensional laser radar installed on the robot can only complete the collection of horizontal obstacle information, it cannot identify the multi-hole environment in the paving area. Therefore, manual obstacle information is added to the constructed global grid map, and the paving area is isolated by obstacles to make it a non-drivable area. The global grid map constructed by the two-dimensional laser radar is as follows: Figure 4 As shown in the figure, the global grid map after manual improvement is as follows Figure 5 shown.

[0072] S4, in the large-scale corridor area, uses a dynamic path planning algorithm based on safe neighborhood and path efficiency optimization to complete global obstacle avoidance path planning, and controls the robot to reach the specified global target point, i.e., a corridor of a row of ground cylinders, from the starting point in an optimal way;

[0073] The specific steps are as follows:

[0074] S41, select the starting point and the target point of the global grid map, create an OpenList list and a CloseList list for storing the nodes traversed and searched by the A* algorithm, and add the starting point to the OpenList list;

[0075] S42, taking the starting point as the initial node, according to the node search direction, implementing the expansion node evaluation through the cost function F, evaluating the node cost, and the node with a smaller cost has a higher priority; repeating the above search process until the target point is found, after the node cost evaluation, the selected path nodes are all the minimum cost nodes, so the obtained path is the optimal path with the minimum path cost;

[0076] The cost function F is:

[0077] F=G+ln|H D +1|×H D +μ×S n +ρ×distance

[0078] Where F is the total estimated path cost from the starting point to the target point of the path where the current node is located; G is the actual path cost between the starting point and the current node; H D is the heuristic function in the form of diagonal distance, ln|H D +1| is the dynamic weight given to the heuristic function; S n is the number of obstacles in the safe neighborhood matrix; distance is the deviation distance function; μ and ρ are weight coefficients.

[0079] The safe neighborhood matrix is:

[0080]

[0081] Where n is the current search node; p1, pd These are neighborhood nodes of different ranges in the grid map.

[0082] When the safe neighborhood is 1, the number of obstacles in the neighboring node p1 of node n is judged. When the safe neighborhood is d, the number of obstacles in p d The choice of safety distance depends on the size of the robot and the map resolution. Different safety distances should be selected in combination with the size of the robot. The safety distance should not be less than the size of the robot. Here it is set to the radius R of the robot's minimum circumscribed circle. The grid map resolution is b, in units of m / pixel, and its safety neighborhood is d=R / b.

[0083] The deviation distance is Figure 6 Schematic diagram, n is the current node, (x goal ,y goal ) is the target point, (x start ,y start ) is the starting point, and the straight line L connecting the starting point and the target point has the linear equation:

[0084]

[0085] The number of adjacent nodes of the current node is n i ,by Figure 6 Take n1 and n2 as examples, the vertical distance from the line L is i for:

[0086]

[0087] In the formula, (x n_i ,y n_i ) is the current node n adjacent node n i coordinate.

[0088] Here, the cost function adds the deviation distance, assigning higher costs to nodes that are farther away from the straight line L, so that the A* algorithm prioritizes searching for nodes near the straight line L when traversing the nodes. As a result, the purpose of reducing the redundancy of the planned path is achieved.

[0089] S43, screening the path points obtained by the A* algorithm;

[0090] There are a lot of redundant nodes in the path points obtained by the A* algorithm, so the obtained path points need to be screened. The path point set is set as path, and the two path point screening strategies are as follows: 1) Delete the consecutive points in the same direction in the path as the first path point screening; 2) The second path point screening is based on judging whether the straight line between the path points passes through obstacles;

[0091] The specific secondary path point screening strategy is: start detecting from the target point to the starting point. Assuming the target point is node N, first add node N to the new path point set path_new. There are naturally no obstacles between node N and node N+1, so check whether there are obstacles on the line between node N and node N+2. If there are no obstacles and the two path points do not belong to a diagonal motion relationship, then enter the next step to detect nodes N and N+3. Otherwise, add node N+1 to path_new and set node N+1 as the new node N. Repeat the above detection until the starting point is reached and the starting point is added to path_new.

[0092] S44, using the improved DWA evaluation function to evaluate the path;

[0093] In order to achieve random obstacle avoidance of the robot, the dynamic window method (DWA) is used to predict the trajectory evaluation function, and the distance between the terminal trajectory point and the global path is added as the trajectory deviation function. The smaller the value of the trajectory deviation function, the higher the evaluation function has. The larger the value of the traditional trajectory evaluation function Eval(v,ω), the higher the score, so the improved evaluation function is:

[0094] Eval(v,ω)=α·Heading(v,ω)+β·Dist(v,ω)+γ·Vel(v,ω)

[0095] N_Eval(v,ω)=Eval(v,ω)-σ·F_line(v,ω)

[0096] Where Eval(v,ω) is the traditional trajectory evaluation function; N_Eval(v,ω) is the improved evaluation function; Heading(v,ω) is the azimuth evaluation sub-function; Dist(v,ω) is the obstacle distance evaluation sub-function; Vel(v,ω) is the speed evaluation sub-function; F_line(v,ω) is the trajectory deviation evaluation sub-function; α, β, γ, σ are the evaluation sub-function weight coefficients.

[0097] S45, for the path points obtained by path optimization, combined with DWA, dynamic obstacle avoidance under the global map can be achieved. At this time, the path points obtained by the A* algorithm are set as the sub-starting points and sub-target points of the DWA algorithm in turn to complete the local path planning. The sub-target point is judged by the current coordinates of the robot to see whether it is the global target point coordinate. If the sub-target point is the global target point and the path planning of this stage is completed, the robot reaches the global target point from the global starting point, and the global dynamic path planning task is achieved. S5, the robot arrives at the row of the earth cylinders to be excavated, which means the path planning of the large-scale aisle corridor is completed. At this time, the robot is adjusted according to the given posture information so that its camera faces the row of earth cylinders, and then the path planning task of laying the earth cylinder area begins; in the switching link of the two-stage path planning task, the path planning area for laying the earth cylinder area is not the part where all the earth cylinders are located, but part of the row where the earth cylinders to be excavated and the rows where the earth cylinders on both sides are located. The cylinder and ground information of the other earth cylinders are irrelevant to the current excavation task; in the robot pose estimation, the sensor used in the path planning application is converted from a two-dimensional laser radar to an RGBD camera, and the path planning task for laying the earth cylinder area is realized with the help of an RGBD camera as an information collection device, and the robot pose at the switching task is used as the initial pose.

[0098] S6, in the area where the ground cylinder is laid, use the RGBD camera installed on the robot arm to look down at the ground area including the ground cylinder in front of the robot, and estimate the position information of the robot in the area where the ground cylinder is laid through the visual odometer;

[0099] The specific steps include:

[0100] S61, select the ORB-SLAM2 algorithm to realize the visual odometer pose estimation; collect image information in real time through the camera, and convert the image from RGB color space to HSI color space before passing the image to the ORB-SLAM2 algorithm. The calculation formula of brightness I in HSI space is as follows:

[0101]

[0102] Extract the I channel in the HSI color space and process it as a grayscale image;

[0103] S62, realizing image histogram adaptive cropping based on S-type membership function, the selected S-type membership function is:

[0104]

[0105] In the formula, x max and x min is the maximum and minimum value of the grayscale image pixel;

[0106] The clipping values ​​of different gray levels of the histogram are calculated according to the following formula;

[0107]

[0108] Where m is the number of pixels in the image; SMF(l) is the calculated function value corresponding to gray level l; L is the maximum gray level of the gray image; ave is the gray mean; mse is the pixel mean square error; ave / mse constitutes the maximum allowable slope; Crop is the upper limit of the histogram clipping value;

[0109] In the histogram equalization process, when the number of pixels with a certain gray value exceeds the upper limit of the calculated clipping value, the excess pixels are evenly distributed on other gray levels to obtain a new histogram, and image enhancement is achieved by improving the image contrast. S63, enhancing image details through unsharp masking technology;

[0110] The Gaussian filtering algorithm is used to convolve the image with a Gaussian kernel to implement the filtering operation. Its mathematical model is:

[0111] f(x,y)=I(x,y)*h(x,y)

[0112] Where * represents the convolution operator, h(x,y) is a low-pass filter, and I(x,y) is the enhanced image after adaptively clipping the histogram;

[0113] Unsharp Maskg mask (x,y) can be calculated by the following formula:

[0114] g mask (x,y)=I(x,y)-f(x,y)

[0115] The sharpened image can be calculated as follows:

[0116] g sa (x,y)=I(x,y)+k·g mask (x,y)

[0117] Where k represents the sharpening level. For the unsharp masking technique, k is set to 1.

[0118] S64, the grayscale image processed by S62 and S63 is combined with the color gamut image H and the saturation image S to obtain an enhanced HSI image, which is then converted into a color image in the RGB color space.

[0119] S65, combined with the adaptive threshold to improve the FAST corner extraction method in the ORB-SLAM2 algorithm, calculate the deviation of the pixel gray value in the image as its adaptive threshold T:

[0120]

[0121] In the formula, is the mean value of image pixels; H and W are the image height and width; λ is the scale factor. Initially set λ = 1. If the feature points that meet the conditions cannot be extracted in the divided grid, set the step size to 0.05 and gradually reduce λ until the feature point extraction requirements are met, and set the minimum threshold T min ;

[0122] S66, using the enhanced color image as the input of ORB-SLAM2, and combining it with the ORB feature points of the adaptive threshold, performing feature point matching and filtering out the wrong matching pairs, and finally obtaining the estimated pose transformation, and then calculating the pose information of the robot in the world coordinate system through the corresponding pose relationship between the robot and the installed camera to realize the self-positioning of the robot in the area where the ground tank is laid.

[0123] S7, in the area where the ground cylinder is laid, using the RGBD camera installed on the robot arm to collect ground color images, detecting the ground cylinder through the target detection algorithm, obtaining the edge information of the ground cylinder, and obtaining the drivable image coordinate system path points; specifically including the following steps: S71, taking images of the ground cylinder and making a data set for model training of the YOLOv10 algorithm;

[0124] S72, using the trained model, perform ground cylinder target detection on the collected ground color image to obtain the ground cylinder detection frame coordinate information in the image coordinate system, such as Figure 7 As shown;

[0125] S73, extracting the drivable road section information between two laterally adjacent circular ground cylinders and the center of the current row of ground cylinders through the detection frame coordinate information of two laterally adjacent ground cylinders;

[0126] Here, the detection frame coordinate information of two horizontally adjacent ground cylinders is processed as a set of data. The left frame coordinates of the current row of ground cylinders and the right frame range of the left ground cylinder belong to the narrowest drivable section of the robot's left wheel. Similarly, the right frame of the current row of ground cylinders and the left frame range of the right ground cylinder belong to the narrowest drivable section of the right wheel. In this way, the robot's drivable section information is obtained, and the center parameters of the current row of ground cylinders are extracted based on the detection frame information of the current row of ground cylinders.

[0127] S74, taking the center of the current row of ground cylinders as the reference path point of the image coordinate system, and according to the left and right wheel tracks and track width of the mash picking composite robot, calculating the relationship between the position information of the robot's two wheels and the drivable ground when taking this as the path point. Specifically, if the position occupied by the left wheel is close to or belongs to the range of the middle row of ground cylinders, the set path point should be adjusted in the direction of the left wheel in combination with the position of the right wheel to ensure that the positions occupied by both wheels belong to safe drivable ground. The same is true for the right wheel, and finally the drivable path point in the image coordinate system is obtained.

[0128] S8, in the area where the ground vats are laid, the target detection algorithm and the visual odometry are combined to obtain the drivable path points in the image coordinate system, which are converted into coordinates in the camera coordinate system. According to the relative position of the camera to the chassis of the mash retrieval composite robot and the current position of the robot in the world coordinate system estimated by the visual odometry, the coordinates of the robot and the path points in the world coordinate system are obtained, and the robot is controlled to reach the digging position of the designated ground vat in the column with the path points as guidance.

[0129] The present invention specification describes the path planning method of the robot for taking materials from the ground tank in the fermentation plant, but is not limited to this scenario and can also be applied to environments with the same characteristics. The contents not described in detail in the specification belong to the prior art known to professional and technical personnel in the relevant field. The present invention does not strictly limit the specific implementation method and scope. The relevant modifications and descriptions within the scope of the relevant claims of the present invention are all protected by the patent of the present invention.

Claims

1. A path planning method for a fermentation plant robot for taking mash materials, characterized in that: The steps include: S1. Divide the fermentation plant into a large-scale corridor and a ground tank area; S2. Use two-dimensional laser radar to construct a global grid map in the fermentation plant, which serves as map information for large-scale corridor path planning; S3. Based on the constructed global grid map, manually add the paved ground cylinder area that cannot be identified by the radar as a non-drivable area, that is, manually add obstacles to isolate the paved ground cylinder area through obstacles in the constructed grid map; S4. Use a dynamic path planning algorithm based on safe neighborhood and path efficiency optimization to complete global obstacle avoidance path planning for large-scale corridors, and reach the designated target point from the starting point in an optimal way; S5. When the robot completes the path planning for the large-scale corridor and reaches the designated global target point, the robot is adjusted to face the target cylinder row in front according to the given posture information. The path planning sensor is switched from the two-dimensional laser radar to the RGBD camera. The robot posture at the switching task is used as the initial posture for the path planning of the cylinder laying area. S6, RGBD camera looks down at the ground area in front of the robot including the ground tank, and estimates the robot's position information through the visual odometry; S7. In the area where the ground cylinder is laid, use an RGBD camera to collect ground color images, detect the ground cylinder through a target detection algorithm, obtain the edge information of the ground cylinder, and obtain the drivable image coordinate system path points; S8. In the area where the ground vat is laid, the target detection algorithm and the visual odometry are combined to obtain the drivable path points in the image coordinate system, which are converted into coordinates in the camera coordinate system. Based on the relative position of the camera to the chassis of the mash retrieval robot and the current position of the robot in the world coordinate system estimated by the visual odometry, the coordinates of the robot and the path points in the world coordinate system are obtained. The path points are used as guidance to control the robot to reach the designated digging position for digging the ground vat.

2. The path planning method of a fermentation plant robot for taking out fermented grains according to claim 1, characterized in that: The specific steps of S4 include: S41, select the starting point and the target point of the global grid map, create an OpenList list and a CloseList list for storing the nodes traversed and searched by the A* algorithm, and add the starting point to the OpenList list; S42, taking the starting point as the initial node, according to the node search direction, using the A* algorithm to implement the expansion node cost evaluation through the cost function F, the node with a smaller cost has a higher priority, repeating the search process until the target point is found, after the node cost evaluation, the selected path nodes are all the minimum cost nodes, the obtained path is the optimal path, and has the minimum path cost; The cost function F is: F=G+ln|H D +1|×H D +μ×S n +ρ×distance Where F is the total estimated path cost from the starting point to the target point of the path where the current node is located; G is the actual path cost between the starting point and the current node; H D is the heuristic function in the form of diagonal distance, ln|H D +1| is the dynamic weight given to the heuristic function; S n is the number of obstacles in the safe neighborhood matrix; distance is the deviation distance function; μ and ρ are weight coefficients. S43. The path obtained by the A* algorithm has a large number of redundant nodes. The steps of screening the path points include: 1) Primary path point screening: delete consecutive points in the same direction from the path points; 2) Secondary path point screening: retain path points where the straight line between path points does not pass through obstacles; S44, using the improved DWA trajectory evaluation function to evaluate the path; S45, the path points obtained by the A* algorithm are set as the sub-starting point and sub-target point of the DWA algorithm in turn, and the local path planning is completed. The sub-target point is judged by the current coordinates of the robot whether it is the global target point coordinate. When the sub-target point is the global target point and the path planning of this stage is completed, the robot reaches the global target point from the global starting point, and the global dynamic path planning is realized.

3. The path planning method of a fermentation plant robot for taking out fermented grains according to claim 2, characterized in that: The safe neighborhood matrix is: Where n is the current search node; p1, p d These are neighborhood nodes of different ranges in the global grid map. When the safe neighborhood is 1, the number of obstacles in the neighboring node p1 of node n is judged. When the safe neighborhood is d, the number of obstacles in p d The choice of safe distance depends on the size of the robot and the resolution of the map. Here, the minimum radius of the robot's circumscribed circle is set to R, the grid map resolution is set to b, and the unit is m / pixel. Then its safe neighborhood is d=R / b.

4. The path planning method of a fermentation plant robot for taking out fermented grains according to claim 2, characterized in that: The S44 adopts the dynamic window method DWA prediction trajectory evaluation function to realize the random obstacle avoidance of the robot; the distance between the terminal trajectory point and the global path is added as the trajectory deviation function, and the smaller the value of the trajectory deviation function is, the higher the evaluation function has a score, and the larger the value of Eval(v,ω), the higher the score; The improved trajectory evaluation function is: Eval(v,ω)=α·Heading(v,ω)+β·Dist(v,ω)+γ·Vel(v,ω) N_Eval(v,ω)=Eval(v,ω)-σ·F_line(v,ω) Where Eval(v,ω) is the traditional trajectory evaluation function; N_Eval(v,ω) is the improved trajectory evaluation function; Heading(v,ω) is the azimuth evaluation sub-function; Dist(v,ω) is the obstacle distance evaluation sub-function; Vel(v,ω) is the speed evaluation sub-function; F-line(v,ω) is the trajectory deviation evaluation sub-function; α, β, γ, σ are the evaluation sub-function weight coefficients.

5. The path planning method for a fermentation plant robot for taking out fermented grains according to claim 1, characterized in that: The specific steps of S6 include: S61. Use the ORB-SLAM2 algorithm to realize the visual odometer pose estimation. Collect image information in real time through the camera. Before the image is passed to the ORB-SLAM2 algorithm, convert the image from the RGB color space to the HSI color space. The calculation formula of the brightness I in the HSI space is: Extract the I channel in the HSI color space and process it as a grayscale image; S62, realizing image histogram adaptive cropping based on S-type membership function, the selected S-type membership function is: In the formula, x max and x min is the maximum and minimum value of the grayscale image pixel; The clipping values ​​of different gray levels of the histogram are calculated according to the following formula; Where m is the number of pixels in the image; SMF(l) is the calculated function value corresponding to gray level l; L is the maximum gray level of the gray image; ave is the gray mean; mse is the pixel mean square error; ave / mse constitutes the maximum allowable slope; Crop is the upper limit of the histogram clipping value; In the process of histogram equalization, when the number of pixels with a certain gray value exceeds the calculated clipping upper limit, the excess pixels are evenly distributed on other gray levels to obtain a new histogram, and image enhancement is achieved by improving the image contrast. S63, enhance image details through unsharp masking technology; S64, the grayscale image processed by S62 and S63 is combined with the color gamut image H and the saturation image S to obtain an enhanced HSI image, which is then converted into an enhanced color image in the RGB color space. S65. Improve the FAST corner point extraction method in the ORB-SLAM2 algorithm by combining the adaptive threshold, and calculate the deviation of the pixel gray value in the image as its adaptive threshold T; The calculation formula of adaptive threshold T is: In the formula, is the mean value of image pixels; H and W are the image height and width; λ is the scale factor. Initially set λ = 1. If the feature points that meet the conditions cannot be extracted in the divided grid, set the step size to 0.05 and gradually reduce λ until the feature point extraction requirements are met, and set the minimum threshold T min ; S66. Use the enhanced color image as the input of ORB-SLAM2, and combine it with the ORB feature points of the adaptive threshold to perform feature point matching and filter out the wrong matching pairs, and finally obtain the estimated pose transformation. Then, through the corresponding pose relationship between the robot and the installed camera, calculate the pose information of the robot in the world coordinate system, and realize the self-positioning of the robot in the area where the ground tank is laid.

6. The path planning method for a fermentation plant robot for taking out fermented grains according to claim 1, characterized in that: The specific steps of S7 include: S71. Take images of the ground tank and create a data set for model training of the YOLOv10 algorithm. S72, using the trained model to perform ground cylinder target detection on the ground color image, and obtaining detection frame coordinate information in the image coordinate system; S73, extracting the drivable road section information between the two adjacent circular ground cylinders and the center of the current row of ground cylinders through the detection frame coordinate information of the two adjacent ground cylinders in the horizontal direction; S74. Determine the path points in the final image coordinate system according to the left and right wheel tracks, track width and identified drivable road section information of the mash taking robot.

Images