Dynamic obstacle avoidance system and method for livestock house cleaning robot based on laser sensing

The three-dimensional point cloud data and decision tree model are obtained through lidar to distinguish obstacle types, and combined with RRT and RRT* algorithms for path planning, solving the perceived uncertainty and decision-making lag problems of dynamic and static obstacle avoidance in the prior art. More efficient obstacle avoidance and path optimization are achieved.

CN118760195BActive Publication Date: 2024-11-26北京佳沃天河智能科技有限公司
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411224259.4
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2024-09-03
Publication Date
2024-11-26
Estimated Expiration
2044-09-03

AI Technical Summary

Technical Problem

The existing technology has problems such as large perception uncertainty and lagging obstacle avoidance decisions in the handling of dynamic and static obstacle avoidance, and cannot effectively identify and build obstacle avoidance strategies, resulting in the impact of robot mobility efficiency.

Method used

LiDAR is used to obtain three-dimensional point cloud data, extract obstacles through denoising and clustering algorithms, and distinguish static and dynamic obstacles in combination with the decision tree model, and use the improved RRT and RRT* algorithms to perform local path planning to predict dynamic obstacles. Future movement trajectory, real-time adjustment of obstacle avoidance strategies.

Benefits of technology

It improves the robot's perception accuracy and path planning efficiency of obstacles, can avoid obstacles safely and efficiently in complex environments, and improves movement efficiency and path quality.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118760195B_ABST
    Figure CN118760195B_ABST
Patent Text Reader

Abstract

The present invention relates to the field of robot navigation technology, and specifically to a dynamic obstacle avoidance system and method for a livestock house cleaning robot based on laser sensing. A laser radar is used to obtain environmental point cloud data, and obstacle positions and contour information are extracted to generate a grid map. Obstacles are clustered and classified to be divided into static obstacles and dynamic obstacles. For static obstacles, an improved RRT algorithm is used for local path planning. For dynamic obstacles, their movement trends are estimated through velocity vectors, and the collision risks in the future are predicted, and the RRT* algorithm is used to dynamically adjust the local path planning. The path planning results are converted into motion control instructions for the robot, and the motion actuator of the robot is controlled to achieve obstacle avoidance. The present invention utilizes a variety of technical means to effectively improve the navigation and obstacle avoidance capabilities and safety of mobile robots in dynamic environments.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a dynamic obstacle avoidance system and method of a livestock house cleaning robot based on laser sensing. Background Art

[0002] Robot technology is widely used in various industries, including animal husbandry. Livestock cleaning robots can automatically clean and disinfect livestock houses, reducing the intensity of manual labor. However, the livestock house environment is complex, with a large number of dynamic and static obstacles, which places higher demands on the robot's perception, decision-making, and control. LiDAR has become the mainstream environmental perception sensor due to its precision and high efficiency.

[0003] Mobile robot obstacle avoidance and navigation is an important research direction in the field of robotics. With the continuous development of robotics technology, higher requirements are placed on the autonomy and intelligence of robots. Robots need to be able to autonomously plan paths in complex environments, avoid obstacles, and reach the target location safely and efficiently.

[0004] The patent with publication number CN115328130A discloses a robot obstacle avoidance system and its obstacle avoidance method, including a terminal processor, the input end of the terminal processor is bidirectionally electrically connected to an environment acquisition module, and the output end of the terminal processor is electrically connected to an environment model building module. The invention collects indoor environment images such as shopping malls through the environment image acquisition module and injects them into the environment model building module. The environment model building module imports the data and generates it into a route prediction model. The neural network prediction model performs self-learning judgment based on the robot's route. The route prediction model selects a path with fewer obstacles and controls the servo motor to start and move through the drive circuit, which solves the problem that the existing robot only has the effect of judging nearby obstacles, but cannot effectively judge the overall obstacles on the route, resulting in a significant impact on the robot's movement efficiency.

[0005] The existing technology has problems in handling dynamic and static obstacle avoidance, and there is no dynamic and static obstacle recognition, and no further development and construction of corresponding obstacle avoidance strategies. In dynamic unknown environments, existing obstacle avoidance methods face problems such as large perception uncertainty and delayed obstacle avoidance decision-making. Summary of the invention

[0006] The purpose of the present invention is to address the deficiencies of the prior art and to provide a dynamic obstacle avoidance system and method for a livestock house cleaning robot based on laser sensing.

[0007] To achieve the above object, the present invention provides the following technical solution: a dynamic obstacle avoidance method for a livestock house cleaning robot based on laser sensing, comprising:

[0008] Preferably, the livestock house cleaning robot uses a laser radar to scan the surrounding environment, obtains three-dimensional point cloud data of the surrounding environment, and represents the point cloud data as a set of three-dimensional coordinate points ,in , is the number of points in the point cloud;

[0009] The acquired point cloud data is preprocessed, and the preprocessing includes denoising and downsampling; the denoising includes statistical filtering denoising and radius filtering denoising.

[0010] Preferably, based on the preprocessed point cloud data, a DBSCAN clustering algorithm is used to extract obstacles from the point cloud data; the steps of the DBSCAN clustering algorithm are as follows:

[0011] a. For each point in the point cloud , calculate its Number of points in the neighborhood ; b. If , then Mark as core point; c. For each core point , find all the points that can be reached by its density to form a cluster; d. Repeat step c until all core points are processed; where, is the neighborhood radius threshold, is the minimum point threshold in the neighborhood;

[0012] Cluster the extracted obstacles, calculate the bounding box, and obtain the location and contour information of the obstacles;

[0013] At the same time, the environment is divided into a two-dimensional grid, namely a grid map, and the occupied grids and free grids are marked according to the locations of obstacles for subsequent path planning.

[0014] Preferably, for each obstacle cluster, shape features and position features are calculated; the shape features include the length, width and height of the bounding box. and proportions, main axis length and scale; obstacle location features include obstacle center coordinates ; where l, w, and h represent the length, width, and height of the bounding box, respectively. represents the length of the obstacle in the x-axis direction, Indicates the length of the obstacle in the y-axis direction, Indicates the length of the obstacle in the z-axis direction;

[0015] According to the shape and location characteristics of obstacles, based on the decision tree model, obstacles are divided into static obstacles and dynamic obstacles.

[0016] Preferably, based on the decision tree model, the process of dividing obstacles into static obstacles and dynamic obstacles is as follows:

[0017] Construct a training data set for the decision tree model. For each obstacle sample in the training data set, extract the obstacle speed. , aspect ratio , the main axis ratio As a feature, the category label of the obstacle is obtained according to prior knowledge. Assuming obstacle samples are collected, the training data set is expressed as:

[0018] ;

[0019] in, Represents a static obstacle, Represents a dynamic obstacle, is the Euclidean norm of the obstacle velocity; ; represents the training data set;

[0020] Using the training dataset Recursively generate a decision tree for the current node , enumerate each feature and all values ​​of the feature , calculated according to Information gain brought by partitioning:

[0021] ;

[0022] in, Is a node The empirical entropy of is the information gain value; m is the number of source symbols, is the mth symbol in the information Length; is the information entropy of the mth symbol km;

[0023] Select the one with the largest information gain As the optimal partition attribute and threshold, the node The samples are divided into two child nodes; for each child node, the division process is recursively applied until the stopping condition is met;

[0024] Mark the nodes that meet the stopping condition as leaf nodes, and set their categories to the category with the most samples in the node;

[0025] After the decision tree is generated, for a new obstacle sample ,in, is the velocity magnitude of the new obstacle sample , is the aspect ratio, is the principal axis ratio; starting from the root node, go to the corresponding leaf node according to its attribute value, and the category label of the leaf node is the predicted category of the obstacle.

[0026] Preferably, for static obstacles, firstly, the position and shape information of the obstacles are obtained according to the environment map and sensor data; and the improved RRT method is used to perform local path planning in combination with the current robot position and the target point position;

[0027] The algorithm flow of improving the RRT method for local path planning of static obstacles is as follows:

[0028] Step 1: Initialize the starting tree and the target point tree , the root nodes are and , the root node is the current position of the robot, is the target position of the robot;

[0029] Step 2: Randomly sample a point , with probability Directly sample the target point;

[0030] Step 3: Respectively and Find the nearest node and ;

[0031] Step 4: According to and Distance calculation expansion step , respectively expand to get new nodes and ;

[0032] Step 5: Check and Whether it collides with a static obstacle. If not, add the corresponding tree and connect the parent node.

[0033] Step 6: Check and Do they intersect? If they do, the path planning is completed and go to step 8;

[0034] Step 7: Repeat steps 2-6 until the maximum number of iterations is reached;

[0035] Step 8: Backtrack from the intersection and , get the complete path, and smooth the path.

[0036] Preferably, for a dynamic obstacle, the velocity of the dynamic obstacle is expressed as a vector Represent and predict the future speed through the historical speed of dynamic obstacles;

[0037] Use Kalman filter algorithm to predict the future speed of dynamic obstacles;

[0038] Use Kalman filter algorithm to predict the future speed of dynamic obstacles;

[0039] Assume that the state variables of the dynamic obstacle are position and velocity, , Represents the state variable of the obstacle at time t; represents the position of the obstacle at time t, [·] T is the transpose of the column vector, and the observed variable is the position , then the state space model is:

[0040] ,

[0041] ,

[0042] in, is the state transfer matrix, is the observation matrix, and are process noise and observation noise respectively;

[0043] The prediction steps of Kalman filter are:

[0044] ,

[0045] ,

[0046] in, is the prior state estimate, is the prior error covariance, is the process noise covariance; is the posterior state estimate, is the posterior error covariance matrix;

[0047] The update steps are:

[0048] ,

[0049] ,

[0050] ,

[0051] in, is the Kalman gain, is the posterior state estimate, is the posterior error covariance, is the observation noise covariance, is the unit matrix; by recursively predicting and updating, the predicted speed of the dynamic obstacle is obtained .

[0052] Preferably, according to the position of the dynamic obstacle and prediction speed , predict its movement trajectory in the future :

[0053] ,

[0054] in, To predict the duration; based on the current position of the robot ,speed and acceleration , predicting dynamic obstacles at future times Location:

[0055] ,

[0056] in, For dynamic obstacles in the future location;

[0057] Calculate the robot and dynamic obstacles at future moments The distance between the predicted positions :

[0058] ,

[0059] Define the collision risk function , Value and distance Inversely proportional to the predicted duration is proportional to;

[0060] Construct the collision risk function: ;

[0061] Calculate the cumulative collision risk over a period of time in the future: ;

[0062] in, the time frame for the risk assessment;

[0063] like Exceeding the preset risk threshold , then enter the dynamic obstacle avoidance path planning.

[0064] Preferably, when the collision risk between the robot and the dynamic obstacle exceeds the risk threshold, an improved RRT* method is introduced in the local path planning to perform dynamic obstacle avoidance path planning;

[0065] The steps of the improved RRT* method for dynamic obstacle avoidance path planning are as follows:

[0066] Initialize the starting point tree and the target point tree, with the root nodes being the starting point and the target point respectively;

[0067] Randomly sample a point and directly sample the target point with a preset probability;

[0068] Find the closest node in the starting point tree and the target point tree;

[0069] The expansion step is calculated according to the distance of the nearest node, and a new node is obtained by expansion;

[0070] Perform collision risk assessment on new nodes, including predicting the future motion trajectory of dynamic obstacles and calculating the collision risk at the new node. If the risk threshold is exceeded, it will be discarded, otherwise it will be added to the tree;

[0071] For a new node, search its neighborhood to see if there is a path with a lower cost. If so, update the parent node and path information.

[0072] Check whether the new node is in the neighboring area of ​​the other party. If so, try to connect the path and evaluate the collision risk of the connection path. If the risk is acceptable, it is used as a potential optimal path.

[0073] The algorithm is stopped when the maximum number of iterations is reached; the complete obstacle avoidance path is obtained by backtracking from the connection point to the starting point and the target point, and the obstacle avoidance path is smoothed to obtain the final obstacle avoidance path.

[0074] A dynamic obstacle avoidance system for a livestock house cleaning robot based on laser sensing, which is implemented based on the dynamic obstacle avoidance method for a livestock house cleaning robot based on laser sensing, and comprises: a data acquisition module, an obstacle analysis module, a static obstacle avoidance module, a dynamic obstacle avoidance module and an execution module;

[0075] The data acquisition module uses a laser radar to obtain environmental point cloud data, performs noise reduction processing through a Gaussian filter algorithm, extracts the location and contour information of obstacles, and generates a grid map;

[0076] The obstacle analysis module performs cluster analysis on the extracted obstacles and divides the obstacles into two types: dynamic obstacles and static obstacles according to the shape characteristics and positional relationships of the obstacles;

[0077] The static obstacle avoidance module uses an improved RRT algorithm to perform local path planning for static obstacles in combination with the current robot position and the target point position;

[0078] The dynamic obstacle avoidance module estimates the movement trend of dynamic obstacles through velocity vectors, predicts the collision risk in the future, and dynamically adjusts the local path planning using the RRT* algorithm;

[0079] The execution module converts the path planning results into the motion control instructions of the robot, adopts the model predictive control MPC algorithm, and considers the kinematic and dynamic constraints of the robot to generate an obstacle avoidance trajectory and control the robot's motion actuator to achieve obstacle avoidance.

[0080] An electronic device comprises: a processor and a memory, wherein the memory stores a computer program that can be called by the processor;

[0081] The processor executes the laser sensing-based livestock house cleaning robot dynamic obstacle avoidance method by calling the computer program stored in the memory.

[0082] A computer-readable storage medium includes: instructions stored therein, and when the instructions are run on a computer, the computer is enabled to execute the dynamic obstacle avoidance method of a livestock house cleaning robot based on laser sensing.

[0083] Beneficial effects of the present invention: The present invention adopts laser radar to obtain high-precision point cloud data, and by removing noise, it can accurately detect surrounding obstacles and provide reliable environmental information for obstacle avoidance; the high precision and long-distance perception capability of laser radar helps to improve the perception range and perception quality of the robot.

[0084] The present invention can effectively distinguish dynamic obstacles from static obstacles through cluster analysis and shape feature extraction, providing more targeted input for subsequent path planning; the introduction of obstacle semantic information can help the robot better understand the environment and make more accurate decisions.

[0085] The present invention adopts an improved RRT algorithm for static obstacles, which can speed up the exploration speed and improve the path quality; adopts the RRT* algorithm for dynamic obstacles, which can adjust the obstacle avoidance strategy in real time, comprehensively consider the safety and path cost, and plan a safer and smoother obstacle avoidance path.

[0086] The present invention estimates the movement trend of dynamic obstacles through velocity vectors and predicts the collision risk in the future, which can help the robot make decisions in advance and actively avoid obstacles. Combined with real-time sensing and trajectory optimization, the control strategy can be continuously corrected, allowing the robot to flexibly respond to dynamically changing environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 This is a flow chart of the dynamic obstacle avoidance method of the livestock house cleaning robot based on laser sensing of the present invention;

[0088] Figure 2 This is a structural diagram of a dynamic obstacle avoidance system of a livestock house cleaning robot based on laser sensing according to the present invention;

[0089] Figure 3 It is a schematic diagram of the structure of an electronic device provided by the present invention;

[0090] Figure 4 It is a schematic diagram of the structure of the computer-readable storage medium provided by the present invention. DETAILED DESCRIPTION

[0091] In order to better understand the present application, a more detailed description will be made of various aspects of the present application with reference to the accompanying drawings. It should be understood that these detailed descriptions are only descriptions of exemplary embodiments of the present application, and are not intended to limit the scope of the present application in any way. Throughout the specification, the same reference numerals refer to the same elements. The expression "and / or" includes any and all combinations of one or more of the associated listed items.

[0092] In the accompanying drawings, the size, dimensions and shapes of the elements have been slightly adjusted for ease of illustration. The drawings are for illustration only and are not strictly drawn to scale. As used herein, the terms "substantially", "approximately" and similar terms are used as terms of approximation, not as terms of degree, and are intended to illustrate the inherent deviations in measurements or calculations that will be recognized by those of ordinary skill in the art. In addition, in this application, the order in which the steps are described does not necessarily represent the order in which these processes occur in actual operation, unless otherwise specified or can be derived from the context.

[0093] It should also be understood that expressions such as "include", "including", "have", "contain" and / or "comprising" are open rather than closed expressions in this specification, which indicate the presence of the stated features, elements and / or components, but do not exclude the presence of one or more other features, elements, components and / or combinations thereof. In addition, when expressions such as "at least one of..." appear after a list of listed features, they modify the entire list of features rather than just the individual elements in the list. In addition, when describing embodiments of the present application, "may" is used to mean "one or more embodiments of the present application". And, the term "exemplary" is intended to refer to an example or illustration.

[0094] Unless otherwise defined, all words (including engineering terms and scientific and technological terms) used in this article have the same meaning as those commonly understood by ordinary technicians in the field to which this application belongs. It should also be understood that unless there is a clear explanation in this application, words defined in commonly used dictionaries should be interpreted as having the same meaning as their meaning in the context of the relevant technology, and should not be interpreted in an idealized or overly formal sense.

[0095] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0096] Example 1

[0097] Reference Figure 1 The first embodiment of the present invention provides a dynamic obstacle avoidance method for a livestock house cleaning robot based on laser sensing.

[0098] S1: Use LiDAR to obtain environmental point cloud data, perform noise reduction processing through Gaussian filtering algorithm, extract the location and contour information of obstacles, and generate a grid map.

[0099] The livestock cleaning robot uses LiDAR to scan the surrounding environment, obtains 3D point cloud data of the surrounding environment, and represents the point cloud data as a set of 3D coordinate points. ,in , is the number of points in the point cloud.

[0100] The working principle of lidar is to emit a laser beam and receive the returned reflected signal. By measuring the flight time TOF or phase difference of the laser beam, the distance and angle of each sampling point are calculated to obtain the three-dimensional coordinates.

[0101] The acquired point cloud data is preprocessed, and the preprocessing includes denoising and downsampling; the denoising includes statistical filtering denoising and radius filtering denoising.

[0102] The statistical filtering denoising is to remove outliers based on the statistical characteristics of the local area of ​​the point cloud. , calculate the mean of the points in the neighborhood and standard deviation ,like , then are considered as outliers and removed, where is the threshold coefficient.

[0103] The radius filter denoising is to set the radius for each point , to determine whether there are enough points in the neighborhood of the set radius, that is, point Whether the number of points in the set radius neighborhood is greater than the point threshold , if the number of points in the radius neighborhood is less than the point threshold , then Eliminate.

[0104] The point cloud is downsampled using voxelization and uniform downsampling methods; the voxelization is to divide the three-dimensional space scanned by the laser radar into multiple A cubic grid is formed, and each point in the grid is replaced by its centroid; the uniform downsampling is to select points in the point cloud according to a predetermined interval.

[0105] S2: Perform cluster analysis on the extracted obstacles and divide them into two types: dynamic and static obstacles according to their shape characteristics and positional relationships.

[0106] Based on the preprocessed point cloud data, the DBSCAN clustering algorithm is used to extract obstacles from the point cloud data. The clustering algorithm classifies points with similar distances in space into the same category, and each category represents an obstacle.

[0107] The basic steps of the DBSCAN clustering algorithm are as follows: a. For each point in the point cloud , calculate its Number of points in the neighborhood ; b. If , then Mark as core point; c. For each core point , find all the points that can be reached by its density to form a cluster; d. Repeat step c until all core points are processed; where, is the neighborhood radius threshold, is the minimum point number threshold in the neighborhood.

[0108] A density-reachable point means starting from a core point and passing through a path no longer than In the DBSCAN algorithm, all points that are density-reachable from a core point are classified into the same cluster, and the density-reachable relationship is unidirectional.

[0109] The extracted obstacles are clustered and bounding boxes are calculated to obtain the location and contour information of the obstacles.

[0110] At the same time, the environment is divided into a two-dimensional grid, namely a grid map, and the occupied grids and free grids are marked according to the locations of obstacles for subsequent path planning.

[0111] For each obstacle cluster, calculate the shape features and position features; the shape features include the length, width and height of the bounding box and proportions, main axis length and scale; obstacle location features include obstacle center coordinates ; where l, w, and h represent the length, width, and height of the bounding box, respectively. represents the length of the obstacle in the x-axis direction, Indicates the length of the obstacle in the y-axis direction, Indicates the length of the obstacle in the z-axis direction.

[0112] According to the shape and location characteristics of obstacles, based on the decision tree model, obstacles are divided into static obstacles and dynamic obstacles.

[0113] The process of training a decision tree model to classify obstacles is as follows:

[0114] Construct a training data set for the decision tree model. For each obstacle sample in the training data set, extract the obstacle speed, aspect ratio, and , the main axis ratio As a feature, we get the category label of the obstacle based on prior knowledge. Suppose we have collected obstacle samples, the training set is expressed as:

[0115] ;

[0116] in, Represents a static obstacle, Represents a dynamic obstacle, is the Euclidean norm of the obstacle velocity; ;

[0117] Using the training set Recursively generate a decision tree for the current node , enumerate each feature and all values ​​of the feature , calculated according to Information gain brought by partitioning:

[0118] ;

[0119] in, Is a node The empirical entropy of is the information gain value; m is the number of source symbols, is the mth symbol in the information Length; is the information entropy of the mth symbol km.

[0120] Select the one with the largest information gain As the optimal partition attribute and threshold, the node The samples are divided into two child nodes; for each child node, the above division process is recursively applied until the stopping condition is met.

[0121] The stop conditions include: (1) the number of samples in the node is less than a preset threshold; (2) the samples in the node belong to the same category; (3) all attributes have been used for partitioning, and the partitioning process is stopped when any of the stop conditions is met.

[0122] The nodes that meet the stopping condition are marked as leaf nodes, and their categories are set to the category with the most samples in the node.

[0123] After the decision tree is generated, for a new obstacle sample ,in, is the velocity magnitude of the new obstacle sample , is the aspect ratio, is the principal axis ratio; starting from the root node, go to the corresponding leaf node according to its attribute value, and the category label of the leaf node is the predicted category of the obstacle.

[0124] S3: For static obstacles, the improved RRT algorithm is used to perform local path planning based on the current robot position and the target point position.

[0125] For static obstacles, we first obtain the position and shape information of the obstacles based on the environment map and sensor data; then we use the improved RRT method to perform local path planning based on the current robot position and the target point position.

[0126] The algorithm flow of improving the RRT method for local path planning of static obstacles is as follows:

[0127] Step 1: Initialize the starting tree and the target point tree , the root nodes are and , the root node is the current position of the robot, is the target position of the robot;

[0128] Step 2: Randomly sample a point , with probability Directly sample the target point;

[0129] Step 3: Respectively and Find the nearest node and ;

[0130] Step 4: According to and Distance calculation expansion step , respectively expand to get new nodes and ;

[0131] Step 5: Check and Whether it collides with a static obstacle. If not, add the corresponding tree and connect the parent node.

[0132] Step 6: Check and Do they intersect? If they do, the path planning is completed and go to step 8;

[0133] Step 7: Repeat steps 2-6 until the maximum number of iterations is reached;

[0134] Step 8: Backtrack from the intersection and , get the complete path, and smooth the path.

[0135] By introducing improved strategies such as target point bias sampling, dynamic step size adjustment, bidirectional RRT and path smoothing, the efficiency and quality of path planning of the RRT algorithm in a static obstacle environment can be improved. Starting from the current position, the robot uses the improved RRT algorithm to explore the configuration space, quickly find a collision-free path to the target point, and optimize the path, ultimately achieving efficient and smooth local path planning.

[0136] S4: For dynamic obstacles, the movement trend of the dynamic obstacles is estimated through the velocity vector, the collision risk in the future is predicted, and the RRT* algorithm is used to dynamically adjust the local path planning.

[0137] The velocity of the dynamic obstacle is expressed as a vector Representation, and predict the future speed through the historical speed of dynamic obstacles.

[0138] Since lidar sensors can only provide the instantaneous position of obstacles, future speeds need to be estimated through historical observation data.

[0139] The Kalman filter algorithm is used to predict the future speed of dynamic obstacles. The basic principle of using Kalman filter to predict the speed of dynamic obstacles is to use a state space model to describe the movement of obstacles and recursively estimate the state variables through two steps of prediction and update.

[0140] Assume that the state variables of the dynamic obstacle are position and velocity, , Represents the state variable of the obstacle at time t; represents the position of the obstacle at time t, [·] T is the transpose of the column vector, and the observed variable is the position , then the state space model is:

[0141] ;

[0142] ;

[0143] in, is the state transfer matrix, is the observation matrix, and are process noise and observation noise respectively;

[0144] The prediction steps of Kalman filter are:

[0145] ;

[0146] ;

[0147] in, is the prior state estimate, is the prior error covariance, is the process noise covariance; is the posterior state estimate, is the posterior error covariance matrix;

[0148] The update steps are:

[0149] ;

[0150] ;

[0151] ;

[0152] in, is the Kalman gain, is the posterior state estimate, is the posterior error covariance, is the observation noise covariance, is the unit matrix; by recursively predicting and updating, the predicted speed of the dynamic obstacle is obtained .

[0153] According to the location of dynamic obstacles and speed estimation , predict its movement trajectory in the future:

[0154] ;

[0155] in, For dynamic obstacles in the future location; To predict the duration; based on the current position of the robot ,speed and acceleration , predicting dynamic obstacles at future times Location:

[0156] ;

[0157] Calculate the robot and dynamic obstacles at future moments The distance between the predicted positions:

[0158] ;

[0159] Define the collision risk function , Value and distance Inversely proportional to the predicted duration Directly proportional.

[0160] Construct the collision risk function: .

[0161] Calculate the cumulative collision risk over a period of time in the future: .

[0162] in, The time frame for the risk assessment.

[0163] like Exceeding the preset risk threshold , then enter the dynamic obstacle avoidance path planning.

[0164] When the collision risk between the robot and the dynamic obstacle exceeds the risk threshold, the improved RRT* method is introduced in the local path planning to perform dynamic obstacle avoidance path planning;

[0165] The steps of the improved RRT* method for dynamic obstacle avoidance path planning are as follows:

[0166] Initialize the starting point tree and the target point tree, with the root nodes being the starting point and the target point respectively;

[0167] Randomly sample a point and directly sample the target point with a certain probability;

[0168] Find the closest node in the starting point tree and the target point tree;

[0169] The expansion step is calculated according to the distance of the nearest node, and a new node is obtained by expansion;

[0170] Perform collision risk assessment on new nodes, including predicting the future motion trajectory of dynamic obstacles and calculating the collision risk at the new node. If the risk threshold is exceeded, it will be discarded, otherwise it will be added to the tree;

[0171] For a new node, search its neighborhood to see if there is a path with a lower cost. If so, update the parent node and path information.

[0172] Check whether the new node is in the other party's neighborhood. If so, try to connect the path and evaluate the collision risk of the connection path. If the risk is acceptable, it will be used as a potential optimal path.

[0173] The algorithm is stopped at the maximum number of iterations; backtracking from the connection point to the starting point and the target point to obtain the complete obstacle avoidance path, and the obstacle avoidance path is smoothed to obtain the final obstacle avoidance path.

[0174] S5: Convert the path planning results into the robot's motion control instructions, use the model predictive control MPC algorithm, and consider the robot's kinematic and dynamic constraints to generate obstacle avoidance trajectories and control the robot's motion actuators to achieve obstacle avoidance.

[0175] Example 2

[0176] Reference Figure 2 The second embodiment of the present invention provides a dynamic obstacle avoidance system for a livestock house cleaning robot based on laser sensing.

[0177] The system includes a data acquisition module, an obstacle analysis module, a static obstacle avoidance module, a dynamic obstacle avoidance module and an execution module.

[0178] The data acquisition module uses a laser radar to obtain environmental point cloud data, performs noise reduction processing through a Gaussian filter algorithm, extracts the location and contour information of obstacles, and generates a grid map.

[0179] The obstacle analysis module performs cluster analysis on the extracted obstacles and divides the obstacles into two types: dynamic obstacles and static obstacles according to the shape characteristics and positional relationships of the obstacles.

[0180] The static obstacle avoidance module uses an improved RRT algorithm to perform local path planning for static obstacles in combination with the current robot position and the target point position.

[0181] The dynamic obstacle avoidance module estimates the movement trend of dynamic obstacles through velocity vectors, predicts the collision risk in the future, and dynamically adjusts the local path planning using the RRT* algorithm.

[0182] The execution module converts the path planning results into the motion control instructions of the robot, adopts the model predictive control MPC algorithm, and considers the kinematic and dynamic constraints of the robot to generate an obstacle avoidance trajectory and control the robot's motion actuator to achieve obstacle avoidance.

[0183] Example 3

[0184] Figure 3 FIG. 1 is a schematic diagram of the structure of an electronic device provided by an embodiment of the present invention. Figure 3As shown, the present invention 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 the one or more processors, the above-mentioned dynamic obstacle avoidance method of the livestock house cleaning robot based on laser sensing can be executed.

[0185] The method or system according to the embodiment of the present invention can also be used by Figure 3 The architecture of the electronic device shown is implemented.

[0186] like Figure 3 As shown, the electronic device 500 may include a bus 501, one or more CPUs 502, a read-only memory (ROM) 503, a random access memory (RAM) 504, a communication port 505 connected to a network, an input / output component 506, a hard disk 507, and the like.

[0187] The storage device in the electronic device 500, such as the ROM 503 or the hard disk 507, can store the dynamic obstacle avoidance method of the livestock house cleaning robot based on laser sensing provided by the present invention.

[0188] A dynamic obstacle avoidance method for a livestock house cleaning robot based on laser sensing includes: using a laser radar to obtain environmental point cloud data, performing noise reduction processing through a Gaussian filtering algorithm, extracting the position and contour information of obstacles, and generating a grid map; performing cluster analysis on the extracted obstacles, and dividing the obstacles into two types, dynamic and static obstacles, according to the shape characteristics and positional relationships of the obstacles; for static obstacles, combining the current robot position and the target point position, using an improved RRT algorithm for local path planning; for dynamic obstacles, estimating the movement trend of the dynamic obstacles through a velocity vector, predicting the collision risk in the future, and using an RRT* algorithm to dynamically adjust the local path planning; converting the path planning results into the robot's motion control instructions, using a model predictive control MPC algorithm, and considering the robot's kinematic and dynamic constraints, generating an obstacle avoidance trajectory, and controlling the robot's motion actuator to achieve obstacle avoidance.

[0189] Furthermore, the electronic device 500 may also include a user interface 508. Figure 3 The architecture shown is only exemplary and can be omitted according to actual needs when implementing different devices. Figure 3 One or more components of an electronic device are shown.

[0190] Example 4

[0191] Figure 4 It is a schematic diagram of the structure of a computer-readable storage medium provided by an embodiment of the present invention.

[0192] like Figure 4 As shown, a computer readable storage medium 600 according to one embodiment of the present invention.

[0193] Computer readable storage medium 600 has computer readable instructions stored thereon.

[0194] When the computer-readable instructions are executed by the processor, the dynamic obstacle avoidance method of the livestock house cleaning robot based on laser sensing according to the embodiments of the present invention described with reference to the above drawings can be executed.

[0195] 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. In addition, according to an embodiment of the present invention, the process described above with reference to the flowchart may be implemented as a computer software program.

[0196] For example, the present invention provides a non-temporary machine-readable storage medium, which stores machine-readable instructions, and the machine-readable instructions can be run by a processor to execute instructions corresponding to the method steps provided by the present invention, for example: using a laser radar to obtain environmental point cloud data, performing noise reduction processing through a Gaussian filtering algorithm, extracting the position and contour information of obstacles, and generating a grid map; performing cluster analysis on the extracted obstacles, and dividing the obstacles into two types: dynamic and static obstacles according to the shape characteristics and positional relationships of the obstacles; for static obstacles, combining the current robot position and the target point position, using an improved RRT algorithm for local path planning; for dynamic obstacles, estimating the movement trend of the dynamic obstacles through a velocity vector, predicting the collision risk in the future, and using the RRT* algorithm to dynamically adjust the local path planning; converting the path planning results into the robot's motion control instructions, using the model predictive control MPC algorithm, and considering the robot's kinematic and dynamic constraints, generating obstacle avoidance trajectories, and controlling the robot's motion actuators to achieve obstacle avoidance.

[0197] When the computer program is executed by the central processing unit (CPU), the above functions defined in the method of the present invention are performed. The method, apparatus, and device of the present invention may be implemented in many ways. For example, the method, apparatus, and device of the present invention may be implemented by software, hardware, firmware, or any combination of software, hardware, and firmware.

[0198] The above sequence for the steps of the method is for illustration only, and the steps of the method of the present invention are not limited to the sequence specifically described above unless otherwise specifically stated.

[0199] In addition, in some embodiments, the present invention can also be implemented as a program recorded in a recording medium, which includes machine-readable instructions for implementing the method according to the present invention. Therefore, the present invention also covers a recording medium storing a program for executing the method according to the present invention.

[0200] In addition, the parts of the above technical solutions provided in the embodiments of the present invention that are consistent with the implementation principles of the corresponding technical solutions in the prior art are not described in detail to avoid excessive redundancy.

[0201] The specific implementation modes as described above further describe the purpose, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation mode of the present invention and is not intended to limit the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A dynamic obstacle avoidance method for a livestock house cleaning robot based on laser sensing, characterized in that: include: Use laser radar to obtain environmental point cloud data, perform noise reduction processing through Gaussian filtering algorithm, extract the location and contour information of obstacles, and generate a grid map; Perform cluster analysis on the extracted obstacles and divide them into two types: dynamic and static obstacles according to their shape characteristics and position relationships; For static obstacles, the improved RRT algorithm is used to perform local path planning based on the current robot position and the target point position; For dynamic obstacles, the movement trend of dynamic obstacles is estimated through velocity vectors, the collision risk in the future is predicted, and the RRT* algorithm is used to dynamically adjust the local path planning; The path planning results are converted into motion control instructions for the robot. The model predictive control (MPC) algorithm is used to generate obstacle avoidance trajectories and control the robot's motion actuators to avoid obstacles, taking into account the robot's kinematic and dynamic constraints. For each obstacle cluster, calculate the shape features and position features; the shape features include the length, width and height of the bounding box and proportions, main axis length and proportion; The obstacle location features include the coordinates of the obstacle center point ; where l, w, and h represent the length, width, and height of the bounding box, respectively. Indicates the length of the obstacle in the x-axis direction, Indicates the length of the obstacle in the y-axis direction, Indicates the length of the obstacle in the z-axis direction; According to the shape and location characteristics of obstacles, based on the decision tree model, obstacles are divided into static obstacles and dynamic obstacles; Based on the decision tree model, the process of dividing obstacles into static obstacles and dynamic obstacles is as follows: Construct a training data set for the decision tree model. For each obstacle sample in the training data set, extract the obstacle speed. , aspect ratio , the main axis ratio As a feature, we get the category label of the obstacle based on prior knowledge. Suppose we have collected obstacle samples, the training data set is expressed as: ; in, Represents a static obstacle, Represents a dynamic obstacle, is the Euclidean norm of the obstacle velocity; ; represents the training data set; Using the training dataset Recursively generate a decision tree for the current node , enumerate each feature and all values ​​of the feature , calculated according to Information gain brought by partitioning: ; in, Is a node The empirical entropy of is the information gain value; m is the number of source symbols, is the mth symbol in the information Length; is the information entropy of the mth symbol km; Select the one with the largest information gain As the optimal partition attribute and threshold, the node The samples are divided into two child nodes; for each child node, the division process is recursively applied until the stopping condition is met; Mark the nodes that meet the stopping condition as leaf nodes, and set their categories to the category with the most samples in the node; After the decision tree is generated, for a new obstacle sample ,in, is the velocity magnitude of the new obstacle sample , is the aspect ratio, is the principal axis ratio; starting from the root node, go to the corresponding leaf node according to its attribute value, and the category label of the leaf node is the predicted category of the obstacle; For static obstacles, firstly, the location and shape information of the obstacles are obtained according to the environment map and sensor data; then, the improved RRT method is used to perform local path planning based on the current robot position and the target point position; The algorithm flow of improving the RRT method for local path planning of static obstacles is as follows: Step 1: Initialize the starting tree and target point tree The root nodes are and , the root node is the current position of the robot, is the target position of the robot; Step 2: Randomly sample a point , with probability Directly sample the target point; Step 3: Respectively and Find the nearest node and ; Step 4: According to and Distance calculation expansion step , respectively expand to get new nodes and ; Step 5: Check and Whether it collides with a static obstacle. If not, add the corresponding tree and connect the parent node. Step 6: Check and Do they intersect? If they do, the path planning is completed and go to step 8; Step 7: Repeat steps 2-6 until the maximum number of iterations is reached; Step 8: Backtrack from the intersection and , get the complete path, and smooth the path; For dynamic obstacles, the velocity of the dynamic obstacle is expressed as a vector Representation, and predicting the future speed by the historical speed of dynamic obstacles; Use Kalman filter algorithm to predict the future speed of dynamic obstacles; Assume that the state variables of the dynamic obstacle are position and velocity, , Represents the state variable of the obstacle at time t; represents the position of the obstacle at time t, [·] T is the transpose of the column vector, and the observed variable is the position , then the state space model is: , , in, is the state transfer matrix, is the observation matrix, and are process noise and observation noise respectively; The prediction steps of Kalman filter are: , , in, is the prior state estimate, is the prior error covariance, is the process noise covariance; is the posterior state estimate, is the posterior error covariance matrix; The update steps are: , , , in, is the Kalman gain, is the posterior state estimate, is the posterior error covariance, is the observation noise covariance, is the unit matrix; by recursively predicting and updating, the predicted speed of the dynamic obstacle is obtained ; According to the location of dynamic obstacles and prediction speed , predict its movement trajectory in the future : , in, To predict the duration; based on the current position of the robot ,speed and acceleration , predicting dynamic obstacles at future times Location: , in, For dynamic obstacles in the future location; Calculate the robot and dynamic obstacles at future moments The distance between the predicted positions : , Define the collision risk function , Value and distance Inversely proportional to the predicted duration is proportional to; Construct the collision risk function: ; Calculate the cumulative collision risk over a period of time in the future: ; in, the time frame for the risk assessment; like Exceeding the preset risk threshold , then enter the dynamic obstacle avoidance path planning; When the collision risk between the robot and the dynamic obstacle exceeds the risk threshold, the improved RRT* method is introduced in the local path planning to perform dynamic obstacle avoidance path planning; The steps of the improved RRT* method for dynamic obstacle avoidance path planning are as follows: Initialize the starting point tree and the target point tree, with the root nodes being the starting point and the target point respectively; Randomly sample a point and directly sample the target point with a preset probability; Find the closest node in the starting point tree and the target point tree; The expansion step is calculated according to the distance of the nearest node, and a new node is obtained by expansion; Perform collision risk assessment on new nodes, including predicting the future motion trajectory of dynamic obstacles and calculating the collision risk at the new node. If the risk threshold is exceeded, it will be discarded, otherwise it will be added to the tree; For a new node, search its neighborhood to see if there is a path with a lower cost. If so, update the parent node and path information. Check whether the new node is in the neighboring area of ​​the other party. If so, try to connect the path and evaluate the collision risk of the connection path. If the risk is acceptable, it is used as a potential optimal path. The algorithm is stopped when the maximum number of iterations is reached; the complete obstacle avoidance path is obtained by backtracking from the connection point to the starting point and the target point, and the obstacle avoidance path is smoothed to obtain the final obstacle avoidance path.

2. The laser sensing-based livestock house cleaning robot dynamic obstacle avoidance method according to claim 1, characterized in that: The livestock cleaning robot uses LiDAR to scan the surrounding environment, obtains 3D point cloud data of the surrounding environment, and represents the point cloud data as a set of 3D coordinate points. ,in , is the number of points in the point cloud; Preprocessing the acquired point cloud data, wherein the preprocessing includes denoising and downsampling; The denoising includes statistical filtering denoising and radius filtering denoising.

3. The laser sensing-based livestock house cleaning robot dynamic obstacle avoidance method according to claim 2, characterized in that: Based on the preprocessed point cloud data, the DBSCAN clustering algorithm is used to extract obstacles from the point cloud data; the steps of the DBSCAN clustering algorithm are as follows: a. For each point in the point cloud , calculate its Number of points in the neighborhood ; b. If , then Mark as core point; c. For each core point , find all the points that can be reached by its density to form a cluster; d. Repeat step c until all core points are processed; where, is the neighborhood radius threshold, is the minimum point threshold in the neighborhood; Cluster the extracted obstacles, calculate the bounding box, and obtain the location and contour information of the obstacles; At the same time, the environment is divided into a two-dimensional grid, namely a grid map, and the occupied grids and free grids are marked according to the locations of obstacles for subsequent path planning.

4. A laser-sensing-based livestock house cleaning robot dynamic obstacle avoidance system, which is used to implement the laser-sensing-based livestock house cleaning robot dynamic obstacle avoidance method according to any one of claims 1 to 3, characterized in that: include: Data acquisition module, obstacle analysis module, static obstacle avoidance module, dynamic obstacle avoidance module and execution module; The data acquisition module uses a laser radar to obtain environmental point cloud data, performs noise reduction processing through a Gaussian filter algorithm, extracts the location and contour information of obstacles, and generates a grid map; The obstacle analysis module performs cluster analysis on the extracted obstacles and divides the obstacles into two types: dynamic obstacles and static obstacles according to the shape characteristics and positional relationships of the obstacles; The static obstacle avoidance module uses an improved RRT algorithm to perform local path planning for static obstacles in combination with the current robot position and the target point position; The dynamic obstacle avoidance module estimates the movement trend of dynamic obstacles through velocity vectors, predicts the collision risk in the future, and dynamically adjusts the local path planning using the RRT* algorithm; The execution module converts the path planning results into the motion control instructions of the robot, adopts the model predictive control MPC algorithm, and considers the kinematic and dynamic constraints of the robot to generate an obstacle avoidance trajectory and control the robot's motion actuator to achieve obstacle avoidance.

5. An electronic device, characterized in that: include: A processor and a memory, wherein the memory stores a computer program that can be called by the processor; The processor executes the laser sensing-based livestock house cleaning robot dynamic obstacle avoidance method as described in any one of claims 1 to 3 by calling the computer program stored in the memory.

6. A computer-readable storage medium, characterized in that: include: Instructions are stored, and when the instructions are run on a computer, the computer executes the dynamic obstacle avoidance method for a livestock house cleaning robot based on laser sensing as described in any one of claims 1 to 3.