Livestock house terrain modeling and path planning navigation system and method using laser scanning
Through laser scanning technology, three-dimensional data of the livestock house are collected and processed, dirt areas and dynamic obstacles are identified, and path planning is optimized, which solves the problem of insufficient path planning in traditional robots in dynamic environments, and improves the efficiency and safety of livestock house cleaning operations.
Patent Information
- Application Number
- CN202411200062.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-29
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-08-29
AI Technical Summary
Traditional livestock house cleaning robots have insufficient path planning in dynamic environments, making it difficult to deal with terrain changes and dynamic obstacles in real time, resulting in poor path planning, low safety and low efficiency.
Laser scanning technology is used to collect three-dimensional point cloud data of the animal house, perform pre-processing and terrain modeling, divide areas to identify the dirt area, plan the best path, and realize the intelligence of the robot in a dynamic environment through dynamic obstacle identification and path optimization. navigation.
It improves the efficiency and quality of livestock cleaning operations, enhances the stability and safety of the system, avoids the risks of falling and collision, and realizes automated path planning throughout the process.
Smart Images

Figure CN118706131B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of path planning, and more specifically, to a livestock house terrain modeling and path planning navigation system and method using laser scanning. Background Art
[0002] With the rapid development of modern animal husbandry, the area and scale of livestock houses are constantly expanding. Manual cleaning of livestock manure can no longer meet the demand. Therefore, the application of robots in livestock house management is becoming more and more popular. However, traditional robots have many shortcomings:
[0003] Insufficient data collection accuracy: Traditional methods rely on 2D drawings or manual measurements, which are not very accurate and are easily affected by human factors, making it difficult to obtain accurate terrain data;
[0004] Limited path planning capabilities: Traditional path planning algorithms are mostly based on static environments, ignoring dynamic changes and various complex factors in the livestock house, resulting in poor path planning results;
[0005] Poor system real-time performance: Traditional methods lag behind in data processing and path planning calculations, making it difficult to achieve real-time navigation and dynamic adjustment, resulting in slow system response;
[0006] The Chinese patent with the authorization announcement number CN112229419B discloses a dynamic path planning navigation method and system; the method includes: taking the starting point as the search center, searching the path in the current static map using the jump point search algorithm, and driving the robot to move along the search path; generating jump points near the current position according to the dynamic jump point generation rules and specifying a new motion path when the environment changes; judging the straight-line reachability of the jump points near the current position and the target point: if they can be reached in a straight line, driving the robot to move along the specified new motion path; if they cannot be reached in a straight line, taking the current point as the search center, using the jump point search algorithm in the current static map to continue searching the path and driving the robot to move until the target point is reached; the dynamic path can be formulated in real time when the environment changes, and the local oscillation problem can be avoided, which can improve the real-time and stability of the inspection robot when carrying out the path planning link;
[0007] Although the above technology can automatically generate maps, realize real-time navigation and dynamic adjustment in unknown environments to adapt to dynamic environments and improve path planning effects; however, it only considers obstacles in a static state. If it is used in a livestock house cleaning robot, it will ignore the moving livestock in the livestock house, causing the livestock house cleaning robot to frequently replan its path during movement, which not only wastes computing resources and reduces operating efficiency, but also poses a risk of collision accidents; in addition, the above technology still uses a flat map when planning the path, and cannot perceive the height changes of the terrain, such as slopes, steps, etc., which causes the livestock house cleaning robot to fall when running along a predetermined path;
[0008] In view of this, the present invention proposes a livestock house terrain modeling and path planning navigation system and method using laser scanning to solve the above problems. Summary of the invention
[0009] In order to overcome the above-mentioned defects of the prior art and achieve the above-mentioned purpose, the present invention provides the following technical solution: a livestock house terrain modeling and path planning navigation method using laser scanning, comprising:
[0010] Collect 3D point cloud data of livestock houses;
[0011] Preprocess the three-dimensional point cloud data to obtain point cloud processing data;
[0012] Perform livestock house terrain modeling based on point cloud processing data to obtain a three-dimensional terrain map of the livestock house;
[0013] Divide the three-dimensional terrain map into regions;
[0014] Collect the dirt area in each area;
[0015] Plan the best path according to the dirt area of each area and control the robot to work;
[0016] While the robot is working, it identifies dynamic obstacles in the livestock house and analyzes the movement path;
[0017] According to the moving path of dynamic obstacles, the best path is optimized.
[0018] Furthermore, the method for obtaining three-dimensional point cloud data includes:
[0019] use A laser radar is used to scan the livestock house and obtain 3D point cloud data of the group, ; Using point cloud registration algorithm The three-dimensional point cloud data are fused and aligned to obtain the three-dimensional point cloud data;
[0020] The preprocessing includes denoising, downsampling, resampling and boundary removal;
[0021] The step of obtaining the three-dimensional topographic map of the livestock house comprises:
[0022] Step sa: using octree voxelization technology to process the point cloud processing data and mark it as voxelized point cloud data;
[0023] Step sb: using a surface reconstruction algorithm to convert the voxelized point cloud data into a continuous three-dimensional surface model;
[0024] Step sc: using a three-dimensional mesh generation algorithm on the three-dimensional surface model to generate a continuous and non-overlapping triangular mesh structure;
[0025] Step sd: smooth the triangular grid structure to obtain a three-dimensional topographic map of the livestock house;
[0026] The step of processing the point cloud processing data using the octree voxelization technology includes:
[0027] Step a1: preset voxel size and quantity thresholds;
[0028] Step a2: Take the entire barn as the parent node;
[0029] Step a3: Divide each parent node into eight child nodes, divide each child node into voxel units according to the voxel size, and count the number of voxel units b of each child node, where b is an integer greater than 1;
[0030] Step a4: Set the quantity threshold Compared with the number of voxel units b; if , then each child node is regarded as a parent node and return to step a3; if , then go to step a5;
[0031] Step a5: Map the point cloud processing data to the corresponding voxel unit to complete the octree voxelization processing of the point cloud processing data.
[0032] Furthermore, the method for dividing the three-dimensional topographic map into regions includes:
[0033] Obtain the robot area, which is the area occupied by the robot, and the robot is a livestock house cleaning robot; use orthographic projection to project the three-dimensional topographic map onto a two-dimensional plane to obtain a two-dimensional plane map of the livestock house; divide the two-dimensional plane map into c areas according to the robot area, where c is an integer greater than 1;
[0034] The method of collecting the dirt area of each region includes:
[0035] Collect regional images of each region, perform grayscale processing on each regional image, and obtain the grayscale values of d pixels, where d is the number of all pixels in c regional images;
[0036] Preset a gray value threshold, compare and analyze the gray values of d pixels with the gray value threshold respectively, mark the pixels whose gray values are greater than or equal to the gray value threshold as dirt points, and do not mark the pixels whose gray values are less than the gray value threshold;
[0037] Count the number of dirt spots in each area and calculate the dirt area;
[0038] The expression of dirt area is: ;
[0039] In the formula, is the dirt area of the ith region, is the number of dirt points in the ith region, is the area of a dirt spot, is the scaling factor;
[0040] The area of a dirt spot is obtained by the resolution of the regional image, and the resolution of the regional image is obtained by the attributes of the regional image;
[0041] The method for obtaining the regional image is: Image sensor acquisition Zhang distribution image, ,Will The distribution images are stitched to obtain the livestock house image; the livestock house image is segmented into c regions to obtain the regional image of each region.
[0042] Furthermore, the step of planning the best path includes:
[0043] Step 1: Randomly select an area from the area corresponding to the entrance of the barn as the initial area; start a depth-first search from the initial area to obtain m selection paths;
[0044] Step 2: Mark the area where the dirt area is not 0 as the dirt area, and select from the m selection paths according to the dirt area. Paths to be screened, The paths to be screened are all paths that pass through all the dirt areas. ; Then according to Point cloud processing data corresponding to the paths to be screened, from Filter out the paths to be filtered Filter paths;
[0045] Step 3: Count the number of areas that each screening path passes through, and perform weighted summation based on the dirt area of each dirt area to calculate the excellence of each screening path; select the screening path with the greatest excellence as the best path;
[0046] The step of obtaining m selection paths comprises:
[0047] Step 1a: Starting from the initial region, explore the adjacent regions of the initial region, select one of the adjacent regions of the initial region as the next region, and mark the next region as the visited region;
[0048] Step 1b: Explore the adjacent areas of the next area, select one of the adjacent areas of the next area that is not marked as a visited area as the new next area, and mark the new next area as a visited area;
[0049] Step 1c: Loop step 1b until the adjacent areas of the next area are marked as visited areas, obtain a simulation path, and determine whether the area where the robot is currently located is the initial area; if the area where the robot is currently located is the initial area, mark the simulation path as the selected path; if the area where the robot is currently located is not the initial area, do not mark the simulation path;
[0050] Step 1d: Loop steps 1a to 1c to obtain m selection paths.
[0051] Furthermore, the Filter out the paths to be filtered The methods for filtering paths include:
[0052] The point cloud processing data of each area in each path to be screened is used as a set of analysis data, and each set of analysis data is input into the trained driving judgment model to output the judgment results, which include normal driving and unable to drive. According to the judgment results, it is determined whether the robot can drive in the corresponding area;
[0053] If the judgment result is normal driving, the corresponding area is marked as a normal area; if the judgment result is unable to drive, the corresponding area is marked as an abnormal area; Analyze the paths to be screened respectively. If all the paths to be screened are normal areas, the corresponding paths to be screened are marked as screening paths; if there are abnormal areas in the paths to be screened, the paths to be screened are not marked.
[0054] The training process of the driving judgment model includes:
[0055] Pre-set corresponding judgment results for e groups of analysis data, where e is an integer greater than 1, and set different digital labels for normal driving and unable to drive; mark the digital labels of the judgment results as judgment labels, and convert the analysis data and the corresponding judgment labels into a corresponding set of feature vectors;
[0056] Each group of feature vectors is used as the input of the driving judgment model. The driving judgment model takes a group of predicted judgment labels corresponding to each group of analysis data as output, and takes the actual judgment labels corresponding to each group of analysis data as prediction targets, where the actual judgment labels are pre-set digital labels of the judgment results corresponding to the analysis data; minimizing the sum of prediction errors of all analysis data is used as the training target; the driving judgment model is trained until the sum of prediction errors reaches convergence and the training is stopped; the driving judgment model is a deep neural network model.
[0057] Furthermore, the method for identifying dynamic obstacles in a livestock house includes:
[0058] A sampling frequency is preset, and regional images of each region are collected according to the sampling frequency, and marked as recognition images; a trained dynamic recognition model is used to recognize the recognition images collected in real time, and the recognition results are output, which include dynamic obstacles and non-dynamic obstacles;
[0059] The training process of the dynamic recognition model includes:
[0060] Collect g recognition images in advance, where g is an integer greater than 1, and annotate each recognition image, including dynamic obstacles and non-dynamic obstacles; convert dynamic obstacles and non-dynamic obstacles into digital annotations respectively; divide the annotated recognition images into a training set and a test set; use the training set to train the dynamic recognition model, and use the test set to test the dynamic recognition model; preset an error threshold, and when the mean of the prediction errors of all recognition images in the test set is less than the error threshold, output the dynamic recognition model; the dynamic recognition model is a convolutional neural network model.
[0061] Furthermore, the method for analyzing the moving path includes:
[0062] Preset area-time mapping table to obtain the corresponding cleaning time according to the dirt area of each area in the optimal path;
[0063] Preset moving time, which is the time it takes for the robot to move from one area to an adjacent area; based on the moving time and cleaning time, calculate the total working time of the robot when it works according to the optimal path; the expression of the total working time is: ; is the total working time, is the number of regions in the optimal path, For the moving time, For the The cleaning time of the dirty area, h is the number of dirty areas in the optimal path, ;
[0064] The area where the dynamic obstacle is identified is marked as the obstacle area. According to the preset time interval, the laser radar corresponding to the obstacle area is used to perform laser scanning on the dynamic obstacle, and f groups of three-dimensional coordinates of the dynamic obstacles are obtained and marked as dynamic coordinates, where f is an integer greater than 1; the laser radar corresponding to the obstacle area is: the laser radar used to obtain the sub-three-dimensional point cloud data of the obstacle area; the point set fitting algorithm is used to fit the dynamic coordinates obtained each time, and the bounding sphere model of the dynamic obstacle is fitted, and the coordinates of the sphere center are extracted from each bounding sphere model and marked as moving coordinates;
[0065] According to the moving coordinates of the f groups of dynamic obstacles, a moving coordinate time series is constructed; the moving coordinate time series is input into the trained coordinate prediction model, the moving coordinates after the time interval are predicted, and marked as predicted coordinates; the predicted coordinates are then used to replace the first moving coordinates in the moving coordinate time series, a new moving coordinate time series is obtained, and the new moving coordinate time series is then input into the trained coordinate prediction model to predict the predicted coordinates again; a total of A groups of predicted coordinates are predicted, ,in is the time interval; according to the predicted coordinates of group A, the area passed by the dynamic obstacle is obtained through the three-dimensional terrain map, and the moving path of the dynamic obstacle is obtained.
[0066] Furthermore, the training process of the coordinate prediction model includes:
[0067] Define the architecture of the coordinate prediction model, including input state, hidden state, number of output units, and hyperparameters, including learning rate and number of iterations; Use the deep learning framework to create the coordinate prediction model network layer, which includes an input layer, multiple LSTM layers, and an output layer, where each LSTM layer consists of an input gate, a forget gate, an output gate, and a cell state; Connect the created network layers to form a complete coordinate prediction model, where the number of units in the input layer and the output layer are both 3;
[0068] Pre-collection Group movement coordinates, , the first f groups of moving coordinates are arranged in chronological order to construct a moving coordinate time series; the moving coordinate time series is used as the input of the coordinate prediction model, the coordinate prediction model takes the moving coordinates after the time interval as the output, and the actual moving coordinates as the prediction target, the coordinate prediction model is trained, and the prediction results are evaluated for model accuracy using the mean absolute percentage error MAPE. When the calculated MAPE is less than the preset MAPE, the coordinate prediction model training is completed.
[0069] Furthermore, the method for optimizing the best path includes:
[0070] The optimal path is compared with the moving path, and if there is an identical area, a preliminary optimization instruction is generated; if there is no identical area, no preliminary optimization instruction is generated;
[0071] If a preliminary optimization instruction is generated, the same area in the optimal path and the moving path is marked as the same area, and the predicted coordinates corresponding to the same area are obtained and marked as the same coordinates; according to the prediction order of each same coordinate, the number of time intervals corresponding to each same coordinate is obtained; the number of time intervals corresponding to each same coordinate is multiplied by the time interval to obtain the same time corresponding to each same coordinate, and the same time with the largest value and the same time with the smallest value are used as the time range for the moving obstacle to stay in the same area, and are marked as the stay time range;
[0072] Obtain the collection time, which is the time from when the robot starts working to when it collects the three-dimensional coordinates of f groups of dynamic obstacles; obtain the number of areas cleaned by the robot before cleaning the same area in the optimal path, and mark them as the number of front areas; obtain the number of front dirt areas in the optimal path, and mark them as the number of front dirt areas, where the front dirt areas are the dirt areas cleaned by the robot before cleaning the same area; and obtain the cleaning time corresponding to each front dirt area through the area-time mapping table; calculate the time when the robot arrives at the same area according to the number of front areas, the number of front dirt areas, the moving time, and the cleaning time corresponding to each front dirt area, and mark it as the arrival time; the calculation method of the arrival time is the same as the calculation method of the total working time; subtract the collection time from the arrival time to obtain the arrival time; compare the arrival time with the stay time range, and if the arrival time is within the stay time range, generate an optimization instruction; if the arrival time is not within the stay time range, do not generate an optimization instruction;
[0073] If an optimization instruction is generated, the maximum value of the stay time range is subtracted from the arrival time to obtain the waiting time. When the robot reaches the previous area of the same area according to the optimal path, it stays according to the waiting time. After the waiting time is over, the robot enters the same area again.
[0074] Furthermore, the method further comprises: analyzing whether a normal region can be divided between the abnormal region and the adjacent region, and if a normal region can be divided, replacing the abnormal region with the divided normal region;
[0075] The adjacent area is an adjacent area perpendicular to the driving direction of the robot among the adjacent areas of the abnormal area;
[0076] Mark two adjacent areas as the first area and the second area respectively; mark the range formed by the first area and the abnormal area as the first range, and mark the range formed by the second area and the abnormal area as the second range; use a rectangular frame with an area equal to the area of the robot to select the first range and the second range respectively, and obtain M selected areas, where M is an integer greater than 1; the selected area is not the first area, the second area, and the abnormal area;
[0077] Obtain the point cloud processing data corresponding to each selected area and mark it as selected data. Input each group of selected data into the trained driving judgment model and output the judgment result. If the judgment result shows normal driving, mark the selected data corresponding to normal driving as normal data. Obtain the corresponding selected area according to the normal data and mark it as the normal selected area, and replace the abnormal area with the normal selected area.
[0078] The livestock house terrain modeling and path planning navigation system using laser scanning implements the livestock house terrain modeling and path planning navigation method using laser scanning, including:
[0079] Data acquisition module, used to collect three-dimensional point cloud data of the livestock house;
[0080] A data processing module is used to pre-process the three-dimensional point cloud data and obtain point cloud processing data;
[0081] A terrain modeling module is used to perform terrain modeling of the livestock house based on point cloud processing data to obtain a three-dimensional terrain map of the livestock house;
[0082] A region division module is used to divide the three-dimensional terrain map into regions;
[0083] A data collection module for collecting the dirt area of each area;
[0084] The path planning module is used to plan the best path according to the dirt area of each area and control the robot to work;
[0085] The dynamic analysis module is used to identify dynamic obstacles in the livestock house and analyze the movement path during the robot's operation;
[0086] The path optimization module is used to optimize the best path according to the moving path of dynamic obstacles.
[0087] An electronic device comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the livestock house terrain modeling and path planning navigation method using laser scanning is implemented.
[0088] A computer-readable storage medium stores a computer program, which, when executed, implements the livestock house terrain modeling and path planning navigation method using laser scanning.
[0089] The technical effects and advantages of the livestock house terrain modeling and path planning navigation system and method using laser scanning of the present invention are as follows:
[0090] 1. Use laser scanning technology to accurately model the three-dimensional terrain map of the livestock house, realize automatic area division and dirt area identification, and perform intelligent optimal path planning. It can effectively sense the height changes of the livestock house terrain and avoid the risk of the robot falling when working according to the optimal path; at the same time, when the robot works according to the optimal path, it can realize real-time identification of dynamic obstacles and mobile path analysis, and judge whether the robot will collide with dynamic obstacles. If a collision occurs, the optimal path is optimized, thereby effectively avoiding dynamic obstacles interfering with the robot's cleaning process; the full process automation of livestock house cleaning operations is realized, the optimal path is accurately planned, the efficiency and quality of livestock house cleaning operations are greatly improved, and the stability and safety of the system are improved.
[0091] 2. Perform multiple area selections on the range formed by the abnormal area and the two adjacent areas, and use deep learning technology to determine whether the robot can drive in the selected area. If it can drive normally, the selected area will replace the original abnormal area; detailed analysis and intelligent judgment of the abnormal area and its adjacent areas can be achieved, which can effectively identify feasible paths hidden in complex environments and avoid directly abandoning paths that have abnormalities but can be corrected, thereby greatly improving the accuracy and reliability of path planning, and further improving the efficiency and quality of livestock house cleaning operations. BRIEF DESCRIPTION OF THE DRAWINGS
[0092] Figure 1 This is a schematic diagram of a livestock house terrain modeling and path planning navigation system using laser scanning according to Example 1 of the present invention;
[0093] Figure 2 This is a schematic diagram of a livestock house terrain modeling and path planning navigation system using laser scanning according to Example 2 of the present invention;
[0094] Figure 3 This is a schematic diagram of a framed area according to Embodiment 2 of the present invention;
[0095] Figure 4 This is a flow chart of a livestock house terrain modeling and path planning navigation method using laser scanning according to Embodiment 3 of the present invention;
[0096] Figure 5 This is a schematic diagram of an electronic device according to Embodiment 4 of the present invention;
[0097] Figure 6 This is a schematic diagram of a storage medium according to Embodiment 5 of the present invention. DETAILED DESCRIPTION
[0098] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. 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 are within the scope of protection of the present invention.
[0099] Example 1
[0100] See also Figure 1 As shown, the livestock house terrain modeling and path planning navigation system using laser scanning described in this embodiment includes a data acquisition module, a data processing module, a terrain modeling module, a region division module, a data collection module, a path planning module, a dynamic analysis module and a path optimization module; each module is connected by wired and / or wireless means to achieve data transmission between modules;
[0101] The data acquisition module is used to collect three-dimensional point cloud data of the livestock house.
[0102] 3D point cloud data is a representation of a 3D environment, describing the 3D geometry and features of the environment through a large number of discrete 3D coordinate points;
[0103] Methods for obtaining three-dimensional point cloud data include:
[0104] use A laser radar is used to scan the livestock house and obtain 3D point cloud data of the group, ; Each laser radar is deployed on the roof of the barn, and technicians in the field will make reasonable deployments based on the actual terrain of the barn to ensure The total coverage of the laser radar can cover the entire livestock house, and there is no blind spot caused by obstructions (such as racks, roof beams, etc.); point cloud registration algorithms (such as ICP algorithm, SoftAssign algorithm, etc.) are used to The three-dimensional point cloud data are fused and aligned to obtain the three-dimensional point cloud data.
[0105] The data processing module is used to pre-process the three-dimensional point cloud data and obtain point cloud processing data.
[0106] Preprocessing includes denoising, downsampling, resampling and boundary removal; the role of denoising is to remove noise in 3D point cloud data; the purpose of downsampling is to reduce the amount of data in 3D point cloud data, improve processing efficiency, and retain geometric characteristics; the purpose of resampling is to make the 3D point cloud data more uniform and regular; the purpose of boundary removal is to improve the consistency of 3D point cloud data, reduce error propagation, and improve the quality of 3D point cloud data; preprocessing of 3D point cloud data is an existing technology and will not be elaborated on here.
[0107] The terrain modeling module is used to perform terrain modeling of the livestock house based on the point cloud processing data and obtain a three-dimensional terrain map of the livestock house.
[0108] The steps to obtain a three-dimensional topographic map of a livestock building include:
[0109] Step sa: using octree voxelization technology to process the point cloud processing data and mark it as voxelized point cloud data;
[0110] Step sb: using a surface reconstruction algorithm (such as Poisson reconstruction) to convert the voxelized point cloud data into a continuous three-dimensional surface model;
[0111] Step sc: using a 3D mesh generation algorithm (such as Delaunay triangulation) on the 3D surface model to generate a continuous and non-overlapping triangular mesh structure;
[0112] Step sd: Smoothing the triangular mesh structure (such as Laplacian smoothing) to obtain a three-dimensional topographic map of the livestock house.
[0113] The steps of processing point cloud data using octree voxelization technology include:
[0114] Step a1: presetting voxel size and number thresholds; the voxel size and number thresholds are pre-set by those skilled in the art according to the accuracy requirements of the three-dimensional topographic map. The greater the accuracy requirements of the three-dimensional topographic map, the smaller the voxel size and number thresholds are; conversely, the smaller the accuracy requirements of the three-dimensional topographic map, the larger the voxel size and number thresholds are;
[0115] Step a2: Take the entire barn as the parent node;
[0116] Step a3: Divide each parent node into eight child nodes, divide each child node into voxel units according to the voxel size, and count the number of voxel units b of each child node, where b is an integer greater than 1;
[0117] Step a4: Set the quantity threshold Compared with the number of voxel units b; if , then each child node is regarded as a parent node and return to step a3; if , then go to step a5;
[0118] Step a5: Map the point cloud processing data to the corresponding voxel unit to complete the octree voxelization processing of the point cloud processing data.
[0119] A region division module is used to divide the three-dimensional terrain map into regions;
[0120] The methods for dividing the three-dimensional terrain map into regions include:
[0121] Obtain the robot area, which is the area occupied by the robot. The robot is a livestock house cleaning robot, and the robot area is obtained by measuring the robot by technical personnel in this field. Use orthographic projection to project the three-dimensional topographic map onto a two-dimensional plane to obtain a two-dimensional plan view of the livestock house. According to the robot area, divide the two-dimensional plan view into c areas, where c is an integer greater than 1.
[0122] The data collection module is used to collect the dirt area of each area.
[0123] Methods for collecting the dirt area of each area include:
[0124] Collect regional images of each region, perform grayscale processing on each regional image, and obtain the grayscale values of d pixels, where d is the number of all pixels in c regional images;
[0125] A gray value threshold is preset, and the gray values of d pixels are compared with the gray value threshold respectively for analysis, and the pixels whose gray values are greater than or equal to the gray value threshold are marked as dirt points, and the pixels whose gray values are less than the gray value threshold are not marked.
[0126] It should be noted that the grayscale value threshold is determined by technical personnel in this field before the livestock house is used. The technicians collect multiple livestock house images and perform grayscale processing. The grayscale value of each pixel in an livestock house image is used as a pixel set; the grayscale value with the lowest value in each pixel set is obtained and marked as the lowest grayscale value; the average of multiple lowest grayscale values is used as the grayscale value threshold.
[0127] Count the number of dirt spots in each area and calculate the dirt area;
[0128] The expression of dirt area is: ;
[0129] In the formula, is the dirt area of the ith region, is the number of dirt points in the ith region, is the area of a dirt spot, is the scaling factor.
[0130] The area of a dirt spot is obtained by the resolution of the regional image, and the resolution of the regional image is obtained by the attributes of the regional image; the proportional coefficient is determined by technical personnel in this field when determining the grayscale value threshold, by measuring the livestock house image area and the actual livestock house area corresponding to multiple livestock house images, the livestock house image area is the livestock house area measured in the livestock house image, and the actual livestock house area is the livestock house area measured in the field; the actual livestock house area is divided by the livestock house image area to obtain the area ratio, and the average of multiple area ratios is used as the scaling coefficient.
[0131] The method for obtaining the regional image is: Image sensor acquisition Zhang distribution image, ,Will The distribution images are stitched to obtain the livestock house image; the livestock house image is segmented into c regions to obtain the regional image of each region.
[0132] The path planning module is used to plan the best path according to the dirt area of each area and control the robot to work.
[0133] The steps to plan the best path include:
[0134] Step 1: Randomly select an area from the area corresponding to the entrance of the livestock house as the initial area; start a depth-first search from the initial area to obtain m selection paths.
[0135] The steps of obtaining m selection paths include:
[0136] Step 1a: Starting from the initial region, explore the adjacent regions of the initial region, select one of the adjacent regions of the initial region as the next region, and mark the next region as the visited region;
[0137] Step 1b: Explore the adjacent areas of the next area, select one of the adjacent areas of the next area that is not marked as a visited area as the new next area, and mark the new next area as a visited area;
[0138] Step 1c: Loop step 1b until the adjacent areas of the next area are marked as visited areas, obtain a simulation path, and determine whether the area where the robot is currently located is the initial area; if the area where the robot is currently located is the initial area, mark the simulation path as the selected path; if the area where the robot is currently located is not the initial area, do not mark the simulation path;
[0139] Step 1d: loop step 1a to step 1c to obtain m selection paths; the m selection paths are all different because the next area selected is different each time the depth-first search is performed.
[0140] Step 2: Mark the area where the dirt area is not 0 as the dirt area, and select from the m selection paths according to the dirt area. Paths to be screened, The paths to be screened are all paths that pass through all the dirt areas. ; Then according to Point cloud processing data corresponding to the paths to be screened, from Filter out the paths to be filtered Filter paths.
[0141] from Filter out the paths to be filtered The methods for filtering paths include:
[0142] The point cloud processing data of each area in each path to be screened is used as a set of analysis data, and each set of analysis data is input into the trained driving judgment model to output the judgment results, which include normal driving and unable to drive. According to the judgment results, it is determined whether the robot can drive in the corresponding area;
[0143] If the judgment result is normal driving, the corresponding area is marked as a normal area; if the judgment result is unable to drive, the corresponding area is marked as an abnormal area; The paths to be screened are analyzed respectively. If all the paths to be screened are normal areas, the corresponding paths to be screened are marked as screening paths; if there are abnormal areas in the paths to be screened, the paths to be screened are not marked.
[0144] The specific training process of the driving judgment model includes:
[0145] The corresponding judgment results are set in advance for e groups of analysis data, where e is an integer greater than 1, and different digital labels are set for normal driving and unable to drive. For example, the digital label is set to 0 for normal driving, and the digital label is set to 1 for unable to drive; the judgment results corresponding to the analysis data are collected by the technicians in this field during the process of the robot cleaning the livestock house in the past, and the technicians in this field judge in turn based on actual experience whether the robot can drive normally in the corresponding area under the conditions of e groups of different analysis data, and set corresponding judgment results for the e groups of different analysis data in turn;
[0146] Mark the digital labels of the judgment results as judgment labels, and convert the analysis data and the corresponding judgment labels into a corresponding set of feature vectors;
[0147] Each set of feature vectors is used as the input of the driving judgment model. The driving judgment model uses a set of predicted judgment labels corresponding to each set of analysis data as output, and uses the actual judgment labels corresponding to each set of analysis data as the prediction target. The actual judgment label is the pre-set digital label of the judgment result corresponding to the analysis data; the training goal is to minimize the sum of the prediction errors of all analysis data; the calculation formula of the prediction error is: ,in is the prediction error, K is the group number of the eigenvector corresponding to the analysis data, is the predicted judgment label corresponding to the Kth group of analysis data, is the actual judgment label corresponding to the Kth group of analysis data; the driving judgment model is trained until the sum of the prediction errors reaches convergence and the training is stopped.
[0148] The above-mentioned driving judgment model is specifically a deep neural network model; it includes an input layer, a hidden layer and an output layer; each hidden layer includes multiple neurons, each neuron is connected to the neurons in the next layer, and the connection contains weights, which determine the importance and influence of data transmission in the neural network; an activation function is applied to each neuron between the hidden layer and the output layer, and the activation function reflects nonlinearity, allowing the network to learn more complex patterns and features.
[0149] Step 3: Count the number of areas passed by each screening path, and calculate the excellence of each screening path based on the dirt area of each dirt area; select the screening path with the greatest excellence as the best path.
[0150] The method for calculating the excellence of each screening path includes:
[0151] ;
[0152] In the formula, is the excellence of the jth screening path, is the number of areas passed by the j-th screening path, is the dirt area of the yth region in the jth screening path, , , , is the preset proportional coefficient; each area in each screening path is named according to the area order, that is, the area that the robot passes through first in the screening path is the first area, the area that the robot passes through second is the second area, and so on. The area passed is the area.
[0153] It should be understood that the specific value of the proportional coefficient in the formula can be set according to actual conditions. The proportional coefficient reflects the degree of influence of the number of regions in the screening path and the cleaning order of each region on the corresponding degree of excellence of the screening path. Technical personnel in this field can preset the corresponding proportional coefficient based on the actual degree of influence of the number of regions in the screening path and the cleaning order of each region on the corresponding degree of excellence of the screening path, so as to accurately evaluate the excellence of the screening path; the excellence is only a criterion for judging the screening path, and the calculation process of the excellence is a dimensionless calculation.
[0154] It should be noted that the larger the dirt area, the higher the priority for cleaning. That is, when the robot cleans the livestock house according to the optimal path, it should first clean the dirt area with a larger dirt area. Therefore, the earlier the dirt area with a larger dirt area is cleaned, the greater the excellence of the corresponding screening path, and vice versa. At the same time, the more areas the screening path passes through, the greater the energy consumption of the robot when cleaning the livestock house, the longer the cleaning time, and the greater the complexity of the path, resulting in reduced operating efficiency and reliability of the robot. Therefore, the fewer areas the screening path passes through, the greater the excellence of the corresponding screening path, and vice versa.
[0155] The dynamic analysis module is used to identify dynamic obstacles in the livestock house and analyze the movement path during the robot's operation.
[0156] Methods for identifying dynamic obstacles in livestock buildings include:
[0157] A sampling frequency is preset, and regional images of each area are collected according to the sampling frequency and marked as recognition images; a trained dynamic recognition model is used to recognize the recognition images collected in real time, and a recognition result is output, which includes dynamic obstacles and non-dynamic obstacles; dynamic obstacles include animals (such as dogs, pigs, cows, etc.), people (such as breeders, equipment maintenance personnel, veterinarians, etc.), etc.; the sampling frequency is pre-set by technical personnel in this field according to the required degree of timeliness of dynamic obstacle recognition. The smaller the sampling frequency is set, the more timely the dynamic obstacles appearing in the livestock house can be recognized.
[0158] The specific training process of the dynamic recognition model includes:
[0159] Collect g recognition images in advance, where g is an integer greater than 1, and annotate each recognition image, including dynamic obstacles and non-dynamic obstacles; convert dynamic obstacles and non-dynamic obstacles into digital annotations respectively, for example, dynamic obstacles are 2, and non-dynamic obstacles are converted into 3; divide the annotated recognition images into a training set and a test set, with 70% of the recognition images used as the training set and 30% of the recognition images used as the test set; use the training set to train the dynamic recognition model, and use the test set to test the dynamic recognition model; preset an error threshold, and when the mean of the prediction errors of all recognition images in the test set is less than the error threshold, output the dynamic recognition model; wherein the calculation formula for the mean prediction error is: ,in is the prediction error, To identify the image number, For the The predicted digital annotation corresponding to the group recognition image, For the The actual digital annotation corresponding to the group recognition image, U is the number of recognition images in the test set; the error threshold is preset according to the accuracy required by the dynamic recognition model;
[0160] The above dynamic recognition model is specifically a convolutional neural network model.
[0161] Methods for analyzing movement paths include:
[0162] A preset area-time mapping table is used to obtain the corresponding cleaning time according to the dirt area of each area in the optimal path; the area-time mapping table is stored in a database and can be a two-dimensional mapping table, which includes the dirt area and the cleaning time corresponding to the dirt area; the area-time mapping table is constructed by technicians in this field by collecting the cleaning time consumed by the robot under conditions of different dirt areas during the historical robot cleaning of livestock houses, based on different dirt areas and the cleaning time corresponding to the dirt areas.
[0163] The preset moving time is the time taken by the robot to move from one area to an adjacent area. The moving time is collected by technicians in the field during the robot cleaning of livestock sheds. The average of the collected multiple times is used as the moving time. The total working time of the robot working according to the optimal path is calculated based on the moving time and the cleaning time. The expression of the total working time is: ; is the total working time, is the number of regions in the optimal path, For the moving time, For the The cleaning time of the dirty area, h is the number of dirty areas in the optimal path, .
[0164] The area where the dynamic obstacle is identified is marked as the obstacle area. According to the preset time interval, the laser radar corresponding to the obstacle area is used to perform laser scanning on the dynamic obstacle, and f groups of three-dimensional coordinates of the dynamic obstacles are obtained, and marked as dynamic coordinates, where f is an integer greater than 1; the laser radar corresponding to the obstacle area is: the laser radar used when obtaining the sub-three-dimensional point cloud data of the obstacle area; the point set fitting algorithm (such as the RANSAC algorithm, the Levenberg-Marquardt algorithm, etc.) is used to fit the dynamic coordinates obtained each time, and the bounding sphere model of the dynamic obstacle is fitted, and the coordinates of the center of the sphere are extracted from each bounding sphere model and marked as moving coordinates; the time interval is pre-set by technical personnel in this field according to the moving speed of the dynamic obstacle. The faster the moving speed of the dynamic obstacle, the shorter the time interval is set, so as to timely identify the moving path of the dynamic obstacle.
[0165] According to the moving coordinates of the f groups of dynamic obstacles, a moving coordinate time series is constructed; the moving coordinate time series is input into the trained coordinate prediction model, the moving coordinates after the time interval are predicted, and marked as predicted coordinates; the predicted coordinates are then used to replace the first moving coordinates in the moving coordinate time series, a new moving coordinate time series is obtained, and the new moving coordinate time series is then input into the trained coordinate prediction model to predict the predicted coordinates again; a total of A groups of predicted coordinates are predicted, ,in For the time interval.
[0166] The training process of the coordinate prediction model includes:
[0167] Define the architecture of the coordinate prediction model, including input state, hidden state, number of output units, and hyperparameters, such as learning rate, number of iterations, etc.; Use a deep learning framework (such as TensorFlow, PyTorch, etc.) to create a coordinate prediction model network layer, which includes an input layer, multiple LSTM layers, and an output layer, where each LSTM layer consists of an input gate, a forget gate, an output gate, and a cell state; Connect the created network layers to form a complete coordinate prediction model, where the number of units in the input layer and the output layer are both 3;
[0168] Pre-collection Group movement coordinates, , the first f groups of moving coordinates are arranged in time order to construct a moving coordinate time series; the moving coordinate time series is used as the input of the coordinate prediction model, the coordinate prediction model takes the moving coordinates after the time interval as the output, and takes the actual moving coordinates as the prediction target, the coordinate prediction model is trained, and the prediction results are evaluated for model accuracy using the mean absolute percentage error MAPE. When the calculated MAPE is less than the preset MAPE, the coordinate prediction model training is completed; wherein, the calculation formula of MAPE is , where q is q time intervals, is the actual moving coordinate after q time intervals, is the predicted moving coordinate after the qth time interval, and R is the number of predicted moving coordinates.
[0169] It should be noted that the preset MAPE is preset by those skilled in the art according to the accuracy required by the model.
[0170] According to the predicted coordinates of group A, the area passed by the dynamic obstacle is obtained through the three-dimensional terrain map, and the moving path of the dynamic obstacle is obtained.
[0171] The path optimization module is used to optimize the best path according to the moving path of dynamic obstacles.
[0172] Methods for optimizing the best path include:
[0173] The optimal path is compared with the moving path. If there is an identical area, a preliminary optimization instruction is generated, indicating that the optimal path and the moving path have the same area, and the robot may collide with a dynamic obstacle; if there is no identical area, no preliminary optimization instruction is generated, indicating that the optimal path and the moving path do not have the same area, and the robot cannot collide with a dynamic obstacle.
[0174] If a preliminary optimization instruction is generated, the same area in the optimal path and the moving path is marked as the same area, and the predicted coordinates corresponding to the same area are obtained and marked as the same coordinates; according to the prediction order of each same coordinate, the number of time intervals corresponding to each same coordinate is obtained; illustratively, if the same coordinates are predicted for the first and second times respectively, the corresponding number of time intervals is 1 and 2; the number of time intervals corresponding to each same coordinate is multiplied by the time interval to obtain the same time corresponding to each same coordinate, and the same time with the largest value and the same time with the smallest value are used as the time range for the moving obstacle to stay in the same area, and are marked as the stay time range.
[0175] The acquisition time is obtained, which is the time from when the robot starts working to when it collects the three-dimensional coordinates of the f groups of dynamic obstacles; the number of areas cleaned by the robot before cleaning the same area in the optimal path is obtained, and marked as the number of front areas; the number of front dirt areas in the optimal path is obtained, and marked as the number of front dirt areas, where the front dirt areas are the dirt areas cleaned by the robot before cleaning the same area; and the cleaning time corresponding to each front dirt area is obtained through the area-time mapping table; the time when the robot arrives at the same area is calculated according to the number of front areas, the number of front dirt areas, the moving time and the cleaning time corresponding to each front dirt area, and marked as the arrival time; the calculation method of the arrival time is the same as the calculation method of the total working time; the arrival time is subtracted from the acquisition time to obtain the arrival time; the arrival time is compared with the stay time range, and if the arrival time is within the stay time range, an optimization instruction is generated, indicating that the robot will collide with the dynamic obstacle and the optimal path needs to be optimized; if the arrival time is not within the stay time range, no optimization instruction is generated, indicating that the robot will not collide with the dynamic obstacle and the optimal path does not need to be optimized.
[0176] If an optimization instruction is generated, the maximum value of the stay time range is subtracted from the arrival time to obtain the waiting time. When the robot reaches the previous area of the same area according to the optimal path, it stays according to the waiting time. After the waiting time is over, the robot enters the same area again.
[0177] This embodiment uses laser scanning technology to accurately model a three-dimensional terrain map of a livestock house, realizes automatic area division and dirt area identification, and performs intelligent optimal path planning. It can effectively sense the height changes of the livestock house terrain and avoid the risk of the robot falling when working according to the optimal path; at the same time, when the robot works according to the optimal path, it realizes real-time identification of dynamic obstacles and analysis of the moving path, and determines whether the robot will collide with dynamic obstacles. If a collision occurs, the optimal path is optimized, thereby effectively avoiding dynamic obstacles interfering with the robot's cleaning process; the full process automation of the livestock house cleaning operation is realized, the optimal path is accurately planned, the efficiency and quality of the livestock house cleaning operation are greatly improved, and then the stability and safety of the robot are improved.
[0178] Example 2
[0179] See also Figure 2 As shown, this embodiment further improves the design on the basis of embodiment 1. In embodiment 1, when planning the best path, if there is an abnormal area in the path to be screened, the path to be screened is directly removed. However, in actual situations, although the robot cannot travel in the abnormal area, a normal area can be divided from the abnormal area and the adjacent area so that the robot can travel smoothly. Figure 3As shown in the figure, the dotted line is the area boundary line, the thick dotted line is the robot contour line, the thick solid line is the box selection area contour line, and the thin solid line is the robot drivable road boundary line in the livestock house; if this situation is not considered, it will lead to the inability to accurately plan the best path; therefore, this embodiment provides a livestock house terrain modeling and path planning navigation system using laser scanning, and also includes a driving analysis module;
[0180] The driving analysis module is used to analyze whether a normal area can be divided between the abnormal area and the adjacent area. If a normal area can be divided, the abnormal area is replaced by the divided normal area.
[0181] The adjacent area is an adjacent area perpendicular to the driving direction of the robot among the adjacent areas of the abnormal area;
[0182] The two adjacent areas are marked as the first area and the second area respectively; the range formed by the first area and the abnormal area is marked as the first range, and the range formed by the second area and the abnormal area is marked as the second range; a rectangular frame with an area equal to the area of the robot is used to select the first range and the second range respectively, and M selected areas are obtained, where M is an integer greater than 1; the selected area is not the first area, the second area, and the abnormal area.
[0183] Obtain the point cloud processing data corresponding to each selected area and mark it as selected data. Input each group of selected data into the trained driving judgment model and output the judgment result. If the judgment result shows normal driving, mark the selected data corresponding to normal driving as normal data. Obtain the corresponding selected area according to the normal data and mark it as the normal selected area, and replace the abnormal area with the normal selected area.
[0184] This embodiment performs multiple area selections on the range formed by the abnormal area and the two adjacent areas, and uses deep learning technology to determine whether the robot can drive in the selected area. If it can drive normally, the selected area replaces the original abnormal area. This achieves detailed analysis and intelligent judgment of the abnormal area and its adjacent areas, and can effectively identify feasible paths hidden in complex environments, avoiding directly abandoning paths that have abnormalities but can be corrected, thereby greatly improving the accuracy and reliability of path planning, and further improving the efficiency and quality of livestock house cleaning operations.
[0185] Example 3
[0186] See also Figure 4 As shown, the parts not described in detail in this embodiment are described in Example 1 and Example 2, and a method for terrain modeling and path planning navigation of a livestock house using laser scanning is provided, and the method includes:
[0187] Collect 3D point cloud data of livestock houses;
[0188] Preprocess the three-dimensional point cloud data to obtain point cloud processing data;
[0189] Perform livestock house terrain modeling based on point cloud processing data to obtain a three-dimensional terrain map of the livestock house;
[0190] Divide the three-dimensional terrain map into regions;
[0191] Collect the dirt area in each area;
[0192] Plan the best path according to the dirt area of each area and control the robot to work;
[0193] While the robot is working, it identifies dynamic obstacles in the livestock house and analyzes the movement path;
[0194] According to the moving path of dynamic obstacles, the best path is optimized.
[0195] Furthermore, the method for obtaining three-dimensional point cloud data includes:
[0196] use A laser radar is used to scan the livestock house and obtain 3D point cloud data of the group, ; Using point cloud registration algorithm The three-dimensional point cloud data are fused and aligned to obtain the three-dimensional point cloud data;
[0197] The preprocessing includes denoising, downsampling, resampling and boundary removal;
[0198] The step of obtaining the three-dimensional topographic map of the livestock house comprises:
[0199] Step sa: using octree voxelization technology to process the point cloud processing data and mark it as voxelized point cloud data;
[0200] Step sb: using a surface reconstruction algorithm to convert the voxelized point cloud data into a continuous three-dimensional surface model;
[0201] Step sc: using a three-dimensional mesh generation algorithm on the three-dimensional surface model to generate a continuous and non-overlapping triangular mesh structure;
[0202] Step sd: smooth the triangular grid structure to obtain a three-dimensional topographic map of the livestock house;
[0203] The step of processing the point cloud processing data using the octree voxelization technology includes:
[0204] Step a1: preset voxel size and quantity thresholds;
[0205] Step a2: Take the entire barn as the parent node;
[0206] Step a3: Divide each parent node into eight child nodes, divide each child node into voxel units according to the voxel size, and count the number of voxel units b of each child node, where b is an integer greater than 1;
[0207] Step a4: Set the quantity threshold Compared with the number of voxel units b; if , then each child node is regarded as a parent node and return to step a3; if , then go to step a5;
[0208] Step a5: Map the point cloud processing data to the corresponding voxel unit to complete the octree voxelization processing of the point cloud processing data.
[0209] Furthermore, the method for dividing the three-dimensional topographic map into regions includes:
[0210] Obtain the robot area, which is the area occupied by the robot, and the robot is a livestock house cleaning robot; use orthographic projection to project the three-dimensional topographic map onto a two-dimensional plane to obtain a two-dimensional plane map of the livestock house; divide the two-dimensional plane map into c areas according to the robot area, where c is an integer greater than 1;
[0211] The method of collecting the dirt area of each region includes:
[0212] Collect regional images of each region, perform grayscale processing on each regional image, and obtain the grayscale values of d pixels, where d is the number of all pixels in c regional images;
[0213] Preset a gray value threshold, compare and analyze the gray values of d pixels with the gray value threshold respectively, mark the pixels whose gray values are greater than or equal to the gray value threshold as dirt points, and do not mark the pixels whose gray values are less than the gray value threshold;
[0214] Count the number of dirt spots in each area and calculate the dirt area;
[0215] The expression of dirt area is: ;
[0216] In the formula, is the dirt area of the ith region, is the number of dirt points in the ith region, is the area of a dirt spot, is the scaling factor;
[0217] The area of a dirt spot is obtained by the resolution of the regional image, and the resolution of the regional image is obtained by the attributes of the regional image;
[0218] The method for obtaining the regional image is: Image sensor acquisition Zhang distribution image, ,Will The distribution images are stitched to obtain the livestock house image; the livestock house image is segmented into c regions to obtain the regional image of each region.
[0219] Furthermore, the step of planning the best path includes:
[0220] Step 1: Randomly select an area from the area corresponding to the entrance of the barn as the initial area; start a depth-first search from the initial area to obtain m selection paths;
[0221] Step 2: Mark the area where the dirt area is not 0 as the dirt area, and select from the m selection paths according to the dirt area. Paths to be screened, The paths to be screened are all paths that pass through all the dirt areas. ; Then according to Point cloud processing data corresponding to the paths to be screened, from Filter out the paths to be filtered Filter paths;
[0222] Step 3: Count the number of areas that each screening path passes through, and perform weighted summation based on the dirt area of each dirt area to calculate the excellence of each screening path; select the screening path with the greatest excellence as the best path;
[0223] The step of obtaining m selection paths comprises:
[0224] Step 1a: Starting from the initial region, explore the adjacent regions of the initial region, select one of the adjacent regions of the initial region as the next region, and mark the next region as the visited region;
[0225] Step 1b: Explore the adjacent areas of the next area, select one of the adjacent areas of the next area that is not marked as a visited area as the new next area, and mark the new next area as a visited area;
[0226] Step 1c: Loop step 1b until the adjacent areas of the next area are marked as visited areas, obtain a simulation path, and determine whether the area where the robot is currently located is the initial area; if the area where the robot is currently located is the initial area, mark the simulation path as the selected path; if the area where the robot is currently located is not the initial area, do not mark the simulation path;
[0227] Step 1d: Loop steps 1a to 1c to obtain m selection paths.
[0228] Furthermore, the Filter out the paths to be filtered The methods for filtering paths include:
[0229] The point cloud processing data of each area in each path to be screened is used as a set of analysis data, and each set of analysis data is input into the trained driving judgment model to output the judgment results, which include normal driving and unable to drive. According to the judgment results, it is determined whether the robot can drive in the corresponding area;
[0230] If the judgment result is normal driving, the corresponding area is marked as a normal area; if the judgment result is unable to drive, the corresponding area is marked as an abnormal area; Analyze the paths to be screened respectively. If all the paths to be screened are normal areas, the corresponding paths to be screened are marked as screening paths; if there are abnormal areas in the paths to be screened, the paths to be screened are not marked.
[0231] The training process of the driving judgment model includes:
[0232] Pre-set corresponding judgment results for e groups of analysis data, where e is an integer greater than 1, and set different digital labels for normal driving and unable to drive; mark the digital labels of the judgment results as judgment labels, and convert the analysis data and the corresponding judgment labels into a corresponding set of feature vectors;
[0233] Each group of feature vectors is used as the input of the driving judgment model. The driving judgment model takes a group of predicted judgment labels corresponding to each group of analysis data as output, and takes the actual judgment labels corresponding to each group of analysis data as prediction targets, where the actual judgment labels are pre-set digital labels of the judgment results corresponding to the analysis data; minimizing the sum of prediction errors of all analysis data is used as the training target; the driving judgment model is trained until the sum of prediction errors reaches convergence and the training is stopped; the driving judgment model is a deep neural network model.
[0234] Furthermore, the method for identifying dynamic obstacles in a livestock house includes:
[0235] A sampling frequency is preset, and regional images of each region are collected according to the sampling frequency, and marked as recognition images; a trained dynamic recognition model is used to recognize the recognition images collected in real time, and the recognition results are output, which include dynamic obstacles and non-dynamic obstacles;
[0236] The training process of the dynamic recognition model includes:
[0237] Collect g recognition images in advance, where g is an integer greater than 1, and annotate each recognition image, including dynamic obstacles and non-dynamic obstacles; convert dynamic obstacles and non-dynamic obstacles into digital annotations respectively; divide the annotated recognition images into a training set and a test set; use the training set to train the dynamic recognition model, and use the test set to test the dynamic recognition model; preset an error threshold, and when the mean of the prediction errors of all recognition images in the test set is less than the error threshold, output the dynamic recognition model; the dynamic recognition model is a convolutional neural network model.
[0238] Furthermore, the method for analyzing the moving path includes:
[0239] Preset area-time mapping table to obtain the corresponding cleaning time according to the dirt area of each area in the optimal path;
[0240] Preset moving time, which is the time it takes for the robot to move from one area to an adjacent area; based on the moving time and cleaning time, calculate the total working time of the robot when it works according to the optimal path; the expression of the total working time is: ; is the total working time, is the number of regions in the optimal path, For the moving time, For the The cleaning time of the dirty area, h is the number of dirty areas in the optimal path, ;
[0241] The area where the dynamic obstacle is identified is marked as the obstacle area. According to the preset time interval, the laser radar corresponding to the obstacle area is used to perform laser scanning on the dynamic obstacle, and f groups of three-dimensional coordinates of the dynamic obstacles are obtained and marked as dynamic coordinates, where f is an integer greater than 1; the laser radar corresponding to the obstacle area is: the laser radar used to obtain the sub-three-dimensional point cloud data of the obstacle area; the point set fitting algorithm is used to fit the dynamic coordinates obtained each time, and the bounding sphere model of the dynamic obstacle is fitted, and the coordinates of the sphere center are extracted from each bounding sphere model and marked as moving coordinates;
[0242] According to the moving coordinates of the f groups of dynamic obstacles, a moving coordinate time series is constructed; the moving coordinate time series is input into the trained coordinate prediction model, the moving coordinates after the time interval are predicted, and marked as predicted coordinates; the predicted coordinates are then used to replace the first moving coordinates in the moving coordinate time series, a new moving coordinate time series is obtained, and the new moving coordinate time series is then input into the trained coordinate prediction model to predict the predicted coordinates again; a total of A groups of predicted coordinates are predicted, ,in is the time interval; according to the predicted coordinates of group A, the area passed by the dynamic obstacle is obtained through the three-dimensional terrain map, and the moving path of the dynamic obstacle is obtained.
[0243] Furthermore, the training process of the coordinate prediction model includes:
[0244] Define the architecture of the coordinate prediction model, including input state, hidden state, number of output units, and hyperparameters, including learning rate and number of iterations; Use the deep learning framework to create the coordinate prediction model network layer, which includes an input layer, multiple LSTM layers, and an output layer, where each LSTM layer consists of an input gate, a forget gate, an output gate, and a cell state; Connect the created network layers to form a complete coordinate prediction model, where the number of units in the input layer and the output layer are both 3;
[0245] Pre-collection Group movement coordinates, , the first f groups of moving coordinates are arranged in chronological order to construct a moving coordinate time series; the moving coordinate time series is used as the input of the coordinate prediction model, the coordinate prediction model takes the moving coordinates after the time interval as the output, and the actual moving coordinates as the prediction target, the coordinate prediction model is trained, and the prediction results are evaluated for model accuracy using the mean absolute percentage error MAPE. When the calculated MAPE is less than the preset MAPE, the coordinate prediction model training is completed.
[0246] Furthermore, the method for optimizing the best path includes:
[0247] The optimal path is compared with the moving path, and if there is an identical area, a preliminary optimization instruction is generated; if there is no identical area, no preliminary optimization instruction is generated;
[0248] If a preliminary optimization instruction is generated, the same area in the optimal path and the moving path is marked as the same area, and the predicted coordinates corresponding to the same area are obtained and marked as the same coordinates; according to the prediction order of each same coordinate, the number of time intervals corresponding to each same coordinate is obtained; the number of time intervals corresponding to each same coordinate is multiplied by the time interval to obtain the same time corresponding to each same coordinate, and the same time with the largest value and the same time with the smallest value are used as the time range for the moving obstacle to stay in the same area, and are marked as the stay time range;
[0249] Obtain the collection time, which is the time from when the robot starts working to when it collects the three-dimensional coordinates of f groups of dynamic obstacles; obtain the number of areas cleaned by the robot before cleaning the same area in the optimal path, and mark them as the number of front areas; obtain the number of front dirt areas in the optimal path, and mark them as the number of front dirt areas, where the front dirt areas are the dirt areas cleaned by the robot before cleaning the same area; and obtain the cleaning time corresponding to each front dirt area through the area-time mapping table; calculate the time when the robot arrives at the same area according to the number of front areas, the number of front dirt areas, the moving time, and the cleaning time corresponding to each front dirt area, and mark it as the arrival time; the calculation method of the arrival time is the same as the calculation method of the total working time; subtract the collection time from the arrival time to obtain the arrival time; compare the arrival time with the stay time range, and if the arrival time is within the stay time range, generate an optimization instruction; if the arrival time is not within the stay time range, do not generate an optimization instruction;
[0250] If an optimization instruction is generated, the maximum value of the stay time range is subtracted from the arrival time to obtain the waiting time. When the robot reaches the previous area of the same area according to the optimal path, it stays according to the waiting time. After the waiting time is over, the robot enters the same area again.
[0251] Furthermore, the method further comprises: analyzing whether a normal region can be divided between the abnormal region and the adjacent region, and if a normal region can be divided, replacing the abnormal region with the divided normal region;
[0252] The adjacent area is an adjacent area perpendicular to the driving direction of the robot among the adjacent areas of the abnormal area;
[0253] Mark two adjacent areas as the first area and the second area respectively; mark the range formed by the first area and the abnormal area as the first range, and mark the range formed by the second area and the abnormal area as the second range; use a rectangular frame with an area equal to the area of the robot to select the first range and the second range respectively, and obtain M selected areas, where M is an integer greater than 1; the selected area is not the first area, the second area, and the abnormal area;
[0254] Obtain the point cloud processing data corresponding to each selected area and mark it as selected data. Output the trained driving judgment model for each group of selected data and output the judgment result. If the judgment result shows normal driving, mark the selected data corresponding to normal driving as normal data. Obtain the corresponding selected area according to the normal data and mark it as the normal selected area, and replace the abnormal area with the normal selected area.
[0255] Example 4
[0256] See also Figure 5As shown, the present application also provides an electronic device 500. The electronic device 500 may include one or more processors and one or more memories. The memories store computer readable codes, and when the computer readable codes are run by one or more processors, the livestock house terrain modeling and path planning navigation method using laser scanning as described above may be executed.
[0257] The method or system according to the embodiment of the present application can also be used Figure 5 The electronic device architecture shown in FIG. Figure 5 As shown, the electronic device 500 may include a bus 501, one or more CPUs 502, ROM 503, RAM 504, a communication port 505 connected to a network, an input / output 506, a hard disk 507, etc. The storage device in the electronic device 500, such as ROM 503 or hard disk 507, may store the livestock house terrain modeling and path planning navigation method using laser scanning provided in the present application. Furthermore, the electronic device 500 may also include a user interface 508. Of course, Figure 5 The architecture shown is only exemplary and can be omitted according to actual needs when implementing different devices. Figure 5 One or more components of an electronic device are shown.
[0258] Example 5
[0259] See also Figure 6 As shown, one embodiment of the present application discloses a computer-readable storage medium 600. Computer-readable instructions are stored on the computer-readable storage medium 600. When the computer-readable instructions are executed by the processor, the livestock house terrain modeling and path planning navigation method using laser scanning according to the embodiment of the present application described with reference to the above figures can be executed. The storage medium 600 includes, but is not limited to, for example, volatile memory and / or non-volatile memory. Volatile memory may include, for example, random access memory (RAM) and cache memory (cache), etc. Non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc.
[0260] In addition, according to the embodiments of the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the present application provides a non-transitory machine-readable storage medium, which stores machine-readable instructions, and the machine-readable instructions can be executed by a processor to execute instructions corresponding to the method steps provided in the present application, such as: livestock house terrain modeling and path planning navigation method using laser scanning. When the computer program is executed by a central processing unit (CPU), the above functions defined in the method of the present application are executed.
[0261] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.
[0262] Finally: The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims
1. A livestock house terrain modeling and path planning navigation method using laser scanning, characterized in that: include: Collect 3D point cloud data of livestock houses; Preprocess the three-dimensional point cloud data to obtain point cloud processing data; Perform livestock house terrain modeling based on point cloud processing data to obtain a three-dimensional terrain map of the livestock house; Divide the three-dimensional terrain map into regions; The method for dividing the three-dimensional topographic map into regions comprises: Obtain the robot area, which is the area occupied by the robot, and the robot is a livestock house cleaning robot; use orthographic projection to project the three-dimensional topographic map onto a two-dimensional plane to obtain a two-dimensional plane map of the livestock house; divide the two-dimensional plane map into c areas according to the robot area, where c is an integer greater than 1; The dirt area of each region is collected; the method for collecting the dirt area of each region comprises: Collect regional images of each region, perform grayscale processing on each regional image, and obtain the grayscale values of d pixels, where d is the number of all pixels in c regional images; Preset a gray value threshold, compare and analyze the gray values of d pixels with the gray value threshold respectively, mark the pixels whose gray values are greater than or equal to the gray value threshold as dirt points, and do not mark the pixels whose gray values are less than the gray value threshold; Count the number of dirt spots in each area and calculate the dirt area; The expression of dirt area is: ; In the formula, is the dirt area of the ith region, is the number of dirt points in the ith region, is the area of a dirt spot, is the scaling factor; The area of a dirt spot is obtained by the resolution of the regional image, and the resolution of the regional image is obtained by the attributes of the regional image; The method for obtaining the regional image is: Image sensor acquisition Zhang distribution image, ,Will The distribution images are stitched to obtain a livestock house image; the livestock house image is segmented into c regions to obtain a regional image of each region; According to the dirt area of each area, the best path is planned and the robot is controlled to work; the steps of planning the best path include: Step 1: Randomly select an area from the area corresponding to the entrance of the barn as the initial area; start a depth-first search from the initial area to obtain m selection paths; Step 2: Mark the area where the dirt area is not 0 as the dirt area, and select from the m selection paths according to the dirt area. Paths to be screened, The paths to be screened are all paths that pass through all the dirt areas. ; Then according to Point cloud processing data corresponding to the paths to be screened, from Filter out the paths to be filtered Filter paths; Step 3: Count the number of areas that each screening path passes through, and perform weighted summation based on the dirt area of each dirt area to calculate the excellence of each screening path; select the screening path with the greatest excellence as the best path; The step of obtaining m selection paths comprises: Step 1a: Starting from the initial region, explore the adjacent regions of the initial region, select one of the adjacent regions of the initial region as the next region, and mark the next region as the visited region; Step 1b: Explore the adjacent areas of the next area, select one of the adjacent areas of the next area that is not marked as a visited area as the new next area, and mark the new next area as a visited area; Step 1c: Loop step 1b until the adjacent areas of the next area are marked as visited areas, obtain a simulation path, and determine whether the area where the robot is currently located is the initial area; if the area where the robot is currently located is the initial area, mark the simulation path as the selected path; if the area where the robot is currently located is not the initial area, do not mark the simulation path; Step 1d: loop step 1a to step 1c to obtain m selection paths; While the robot is working, it identifies dynamic obstacles in the livestock house and analyzes the movement path; According to the moving path of dynamic obstacles, the best path is optimized.
2. The method for livestock house terrain modeling and path planning navigation using laser scanning according to claim 1, characterized in that: Methods for obtaining three-dimensional point cloud data include: use A laser radar is used to scan the livestock house and obtain 3D point cloud data of the group, ; Using point cloud registration algorithm The three-dimensional point cloud data are fused and aligned to obtain the three-dimensional point cloud data; The preprocessing includes denoising, downsampling, resampling and boundary removal; The step of obtaining the three-dimensional topographic map of the livestock house comprises: Step sa: using octree voxelization technology to process the point cloud processing data and mark it as voxelized point cloud data; Step sb: using a surface reconstruction algorithm to convert the voxelized point cloud data into a continuous three-dimensional surface model; Step sc: using a three-dimensional mesh generation algorithm on the three-dimensional surface model to generate a continuous and non-overlapping triangular mesh structure; Step sd: smooth the triangular grid structure to obtain a three-dimensional topographic map of the livestock house; The step of processing the point cloud processing data using the octree voxelization technology includes: Step a1: preset voxel size and quantity thresholds; Step a2: Take the entire barn as the parent node; Step a3: Divide each parent node into eight child nodes, divide each child node into voxel units according to the voxel size, and count the number of voxel units b of each child node, where b is an integer greater than 1; Step a4: Set the quantity threshold Compared with the number of voxel units b; if , then each child node is regarded as a parent node and return to step a3; if , then go to step a5; Step a5: Map the point cloud processing data to the corresponding voxel unit to complete the octree voxelization processing of the point cloud processing data.
3. The method for livestock house terrain modeling and path planning navigation using laser scanning according to claim 2, characterized in that: Said from Filter out the paths to be filtered The methods for filtering paths include: The point cloud processing data of each area in each path to be screened is used as a set of analysis data, and each set of analysis data is input into the trained driving judgment model to output the judgment results, which include normal driving and unable to drive. According to the judgment results, it is determined whether the robot can drive in the corresponding area; If the judgment result is normal driving, the corresponding area is marked as a normal area; if the judgment result is unable to drive, the corresponding area is marked as an abnormal area; Analyze the paths to be screened respectively. If all the paths to be screened are normal areas, the corresponding paths to be screened are marked as screening paths; if there are abnormal areas in the paths to be screened, the paths to be screened are not marked. The training process of the driving judgment model includes: Pre-set corresponding judgment results for e groups of analysis data, where e is an integer greater than 1, and set different digital labels for normal driving and unable to drive; mark the digital labels of the judgment results as judgment labels, and convert the analysis data and the corresponding judgment labels into a corresponding set of feature vectors; Each group of feature vectors is used as the input of the driving judgment model. The driving judgment model takes a group of predicted judgment labels corresponding to each group of analysis data as output, and takes the actual judgment labels corresponding to each group of analysis data as prediction targets, where the actual judgment labels are pre-set digital labels of the judgment results corresponding to the analysis data; minimizing the sum of prediction errors of all analysis data is used as the training target; the driving judgment model is trained until the sum of prediction errors reaches convergence and the training is stopped; the driving judgment model is a deep neural network model.
4. The method for livestock house terrain modeling and path planning navigation using laser scanning according to claim 3, characterized in that: The method for identifying dynamic obstacles in a livestock house comprises: A sampling frequency is preset, and regional images of each region are collected according to the sampling frequency, and marked as recognition images; a trained dynamic recognition model is used to recognize the recognition images collected in real time, and the recognition results are output, which include dynamic obstacles and non-dynamic obstacles; The training process of the dynamic recognition model includes: Collect g recognition images in advance, where g is an integer greater than 1, and annotate each recognition image, including dynamic obstacles and non-dynamic obstacles; convert dynamic obstacles and non-dynamic obstacles into digital annotations respectively; divide the annotated recognition images into a training set and a test set; use the training set to train the dynamic recognition model, and use the test set to test the dynamic recognition model; preset an error threshold, and when the mean of the prediction errors of all recognition images in the test set is less than the error threshold, output the dynamic recognition model; the dynamic recognition model is a convolutional neural network model.
5. The method for livestock house terrain modeling and path planning navigation using laser scanning according to claim 4, characterized in that: The method for analyzing the moving path comprises: Preset area-time mapping table to obtain the corresponding cleaning time according to the dirt area of each area in the optimal path; Preset moving time, which is the time it takes for the robot to move from one area to an adjacent area; based on the moving time and cleaning time, calculate the total working time of the robot when it works according to the optimal path; the expression of the total working time is: ; is the total working time, is the number of regions in the optimal path, For the moving time, For the The cleaning time of the dirty area, h is the number of dirty areas in the optimal path, ; The area where the dynamic obstacle is identified is marked as the obstacle area. According to the preset time interval, the laser radar corresponding to the obstacle area is used to perform laser scanning on the dynamic obstacle, and f groups of three-dimensional coordinates of the dynamic obstacles are obtained and marked as dynamic coordinates, where f is an integer greater than 1; the laser radar corresponding to the obstacle area is: the laser radar used to obtain the sub-three-dimensional point cloud data of the obstacle area; the point set fitting algorithm is used to fit the dynamic coordinates obtained each time, and the bounding sphere model of the dynamic obstacle is fitted, and the coordinates of the sphere center are extracted from each bounding sphere model and marked as moving coordinates; According to the moving coordinates of the f groups of dynamic obstacles, a moving coordinate time series is constructed; the moving coordinate time series is input into the trained coordinate prediction model, the moving coordinates after the time interval are predicted, and marked as predicted coordinates; the predicted coordinates are then used to replace the first moving coordinates in the moving coordinate time series, a new moving coordinate time series is obtained, and the new moving coordinate time series is then input into the trained coordinate prediction model to predict the predicted coordinates again; a total of A groups of predicted coordinates are predicted, ,in is the time interval; according to the predicted coordinates of group A, the area passed by the dynamic obstacle is obtained through the three-dimensional terrain map, and the moving path of the dynamic obstacle is obtained.
6. The method for livestock house terrain modeling and path planning navigation using laser scanning according to claim 5, characterized in that: The training process of the coordinate prediction model includes: Define the architecture of the coordinate prediction model, including input state, hidden state, number of output units, and hyperparameters, including learning rate and number of iterations; Use the deep learning framework to create the coordinate prediction model network layer, which includes an input layer, multiple LSTM layers, and an output layer, where each LSTM layer consists of an input gate, a forget gate, an output gate, and a cell state; Connect the created network layers to form a complete coordinate prediction model, where the number of units in the input layer and the output layer are both 3; Pre-collection Group movement coordinates, , construct the first f groups of moving coordinates into a moving coordinate time series according to the time sequence; use the moving coordinate time series as the input of the coordinate prediction model, the coordinate prediction model takes the moving coordinates after the time interval as the output, and takes the actual moving coordinates as the prediction target, trains the coordinate prediction model, and uses the mean absolute percentage error MAPE to evaluate the model accuracy of the prediction results. When the calculated MAPE is less than the preset MAPE, the coordinate prediction model training is completed.
7. The method for livestock house terrain modeling and path planning navigation using laser scanning according to claim 6, characterized in that: Methods for optimizing the best path include: The optimal path is compared with the moving path, and if there is an identical area, a preliminary optimization instruction is generated; if there is no identical area, no preliminary optimization instruction is generated; If a preliminary optimization instruction is generated, the same area in the optimal path and the moving path is marked as the same area, and the predicted coordinates corresponding to the same area are obtained and marked as the same coordinates; according to the prediction order of each same coordinate, the number of time intervals corresponding to each same coordinate is obtained; the number of time intervals corresponding to each same coordinate is multiplied by the time interval to obtain the same time corresponding to each same coordinate, and the same time with the largest value and the same time with the smallest value are used as the time range for the moving obstacle to stay in the same area, and are marked as the stay time range; Obtain the collection time, which is the time from when the robot starts working to when it collects the three-dimensional coordinates of f groups of dynamic obstacles; obtain the number of areas cleaned by the robot before cleaning the same area in the optimal path, and mark them as the number of front areas; obtain the number of front dirt areas in the optimal path, and mark them as the number of front dirt areas, where the front dirt areas are the dirt areas cleaned by the robot before cleaning the same area; and obtain the cleaning time corresponding to each front dirt area through the area-time mapping table; calculate the time when the robot arrives at the same area according to the number of front areas, the number of front dirt areas, the moving time, and the cleaning time corresponding to each front dirt area, and mark it as the arrival time; the calculation method of the arrival time is the same as the calculation method of the total working time; subtract the collection time from the arrival time to obtain the arrival time; compare the arrival time with the stay time range, and if the arrival time is within the stay time range, generate an optimization instruction; if the arrival time is not within the stay time range, do not generate an optimization instruction; If an optimization instruction is generated, the maximum value of the stay time range is subtracted from the arrival time to obtain the waiting time. When the robot reaches the previous area of the same area according to the optimal path, it stays according to the waiting time. After the waiting time is over, the robot enters the same area again.
8. The method for livestock house terrain modeling and path planning navigation using laser scanning according to claim 7, characterized in that: The method further includes: analyzing whether a normal region can be divided between the abnormal region and the adjacent region, and if a normal region can be divided, replacing the abnormal region with the divided normal region; The adjacent area is an adjacent area perpendicular to the driving direction of the robot among the adjacent areas of the abnormal area; Mark two adjacent areas as the first area and the second area respectively; mark the range formed by the first area and the abnormal area as the first range, and mark the range formed by the second area and the abnormal area as the second range; use a rectangular frame with an area equal to the area of the robot to select the first range and the second range respectively, and obtain M selected areas, where M is an integer greater than 1; the selected area is not the first area, the second area, and the abnormal area; Obtain the point cloud processing data corresponding to each selected area and mark it as selected data. Input each group of selected data into the trained driving judgment model and output the judgment result. If the judgment result shows normal driving, mark the selected data corresponding to normal driving as normal data. Obtain the corresponding selected area according to the normal data and mark it as the normal selected area, and replace the abnormal area with the normal selected area.
9. A livestock house terrain modeling and path planning navigation system using laser scanning, implementing a livestock house terrain modeling and path planning navigation method using laser scanning as claimed in any one of claims 1 to 8, characterized in that: include: Data acquisition module, used to collect three-dimensional point cloud data of the livestock house; A data processing module is used to pre-process the three-dimensional point cloud data and obtain point cloud processing data; A terrain modeling module is used to perform terrain modeling of the livestock house based on point cloud processing data to obtain a three-dimensional terrain map of the livestock house; A region division module is used to divide the three-dimensional terrain map into regions; A data collection module for collecting the dirt area of each area; The path planning module is used to plan the best path according to the dirt area of each area and control the robot to work; The dynamic analysis module is used to identify dynamic obstacles in the livestock house and analyze the movement path during the robot's operation; The path optimization module is used to optimize the best path according to the moving path of dynamic obstacles.
10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the livestock house terrain modeling and path planning navigation method using laser scanning as described in any one of claims 1-8 is implemented.
11. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed, implements the livestock house terrain modeling and path planning navigation method using laser scanning as described in any one of claims 1 to 8.
Citation Information
Patent Citations
A dynamic path planning and navigation method and system
CN112229419B
Intelligent cleaning robot
CN112004645A
Livestock cleaning method for automatic driving vehicle fusing laser radar and machine vision
CN114219910A
Autonomous navigation control system and control method based on animal house operation robot
CN116700297A
Path collision avoidance
US20230418288A1
