Robot intelligent grazing device and method based on point cloud BEV recognition algorithm

Abstract

The application discloses a robot intelligent grazing device and method based on a point cloud BEV recognition algorithm. The robot is provided with a laser radar, an IMU, a binocular depth camera and an RTK sensor, can obtain different data from the sensing devices, combines DWA and Pointnet, performs navigation and path planning through a processing system, and performs point cloud classification and point cloud cutting on a target. The processing system is based on a ROS operating system, performs mapping from a bird's eye view, automatically navigates and avoids obstacles based on A* and DWA, identifies the target based on a point cloud diagram through a Pointnet network, controls the energization time of a motor through corresponding instructions to enable a whip throwing mechanism to reach a target point, and thus can drive the livestock out of a circle. The device is good in fusion degree with a natural environment, can solve the problem of autonomous route searching and whip throwing activities based on a point cloud diagram, and realizes true intelligentization of a grazing process.

Description

Technical Field

[0001] This invention belongs to the technical field of intelligent robots, and particularly relates to a robot intelligent herding device based on a point cloud BEV recognition algorithm, and a robot intelligent herding method based on a point cloud BEV recognition algorithm. Background Technology

[0002] Traditional animal husbandry relies on humans to manage a group of livestock, resulting in high dependence on manual labor, low levels of information technology, and frequent occurrences of lost or stranded livestock.

[0003] With the continuous development of technology, the livestock industry is also undergoing digital upgrades. Existing intelligent grazing generally uses GPS positioning technology to achieve real-time tracking of livestock, and sets up electronic fence systems to regulate grazing behavior, improve management accuracy, and prevent livestock from being lost or stranded.

[0004] However, this kind of intelligent grazing lacks means to drive livestock that are about to leave the enclosure, cannot solve the problems of poor integration between the equipment and the natural environment, and lack of interactive behavior between the equipment and the livestock, and cannot achieve true intelligence in the grazing process. Summary of the Invention

[0005] To overcome the shortcomings of existing technologies, the technical problem to be solved by this invention is to provide a robotic intelligent herding device based on a point cloud BEV recognition algorithm. This device can drive livestock that are about to leave the enclosure. The device has a good degree of integration with the natural environment and can solve the problems of autonomous pathfinding and whip-swinging activities using BEV recognition based on point cloud maps, thereby realizing true intelligence in the herding process.

[0006] The technical solution of this invention is: a robotic intelligent herding device based on a point cloud BEV recognition algorithm. The robot is equipped with a lidar, IMU, and binocular depth camera RTK sensor. Data obtained from these sensors, combined with the Dynamic Window Method (DWA) and Pointnet network structure, is processed by a processing system to perform navigation and path planning, and to classify and segment the target point cloud. The processing system is based on the ROS operating system and includes:

[0007] The BEV mapping module creates an overhead view of the environment, obtains a NumPy array, and converts it into a PIL image.

[0008] The path planning module, based on the A* and DWA algorithms, performs automatic navigation and obstacle avoidance.

[0009] The target classification and segmentation module identifies targets using the PointNet network based on point cloud images and trained models to obtain relevant information.

[0010] The control module controls the motor's energizing time by sending corresponding commands to enable the whip-whipping mechanism to reach the target point;

[0011] Repeatedly execute the target classification and segmentation module and the control module until the livestock move away from the boundary threshold, at which point the grazing and herding process ends.

[0012] The robot of this invention is equipped with a lidar, an IMU, a binocular depth camera, and an RTK sensor. It can acquire different data from these sensing devices. Combining the Dynamic Window Method (DWA) and the Pointnet network structure, the processing system performs navigation and path planning, and classifies and segments the target into point clouds. The processing system is based on the ROS operating system. It performs top-down mapping of the environment, acquires NumPy arrays, and converts them into PIL images. Based on the A* and DWA algorithms, it performs automatic navigation and obstacle avoidance. Based on the point cloud map and the trained model, it identifies the target through the Pointnet network to obtain relevant information. By sending corresponding commands to control the motor energizing time, the whip-swinging mechanism can reach the target point. Therefore, this device can drive livestock that are about to leave the enclosure. The device integrates well with the natural environment and can solve the problem of autonomous pathfinding and whip-swinging activities using BEV recognition based on point cloud maps, thereby realizing true intelligence in the grazing process.

[0013] A method for a robotic intelligent herding device based on a point cloud BEV recognition algorithm is also provided, which includes the following steps:

[0014] (1) Construct a BEV diagram from a point cloud diagram;

[0015] (2) Obtain the coordinates of the work area, use the A* algorithm for global path planning, use the Dynamic Window Algorithm (DWA) for local path planning, and automatically navigate to the area.

[0016] (3) The input point cloud image is processed using the Pointnet algorithm to detect the head or tail of livestock, determining whether any livestock are about to leave the enclosure, and obtaining the coordinates (x, y, y) of the head or tail target. a0 ,y a0 ,z a0 );

[0017] (4) Drive the animals by controlling the whip-whipping mechanism with a motor;

[0018] (5) Repeat steps (3) and (4) to complete the expulsion work in the area. Attached Figure Description

[0019] Figure 1 The flowchart shows the robot intelligent herding method based on the point cloud BEV recognition algorithm according to the present invention.

[0020] Figure 2 This is a schematic diagram of the path planning process according to the present invention.

[0021] Figure 3 This is a simplified rear-wheel drive diagram according to the present invention.

[0022] Figure 4 This is a simplified schematic diagram of the whip motion according to the present invention.

[0023] Figure 5 This describes the relationship between the image coordinate system and the point cloud coordinate system coordinate axes when generating a BEV map according to the present invention. Detailed Implementation

[0024] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0025] To make the description of this disclosure more detailed and complete, illustrative descriptions of embodiments and specific examples of the present invention are provided below; however, these are not the only forms of implementing or utilizing the specific examples of the present invention. The embodiments cover features of multiple specific examples and methods and steps for constructing and operating these specific examples, and their order. However, other specific examples may also be used to achieve the same or equivalent functions and order of steps.

[0026] This robotic intelligent herding device, based on a point cloud BEV recognition algorithm, utilizes data from a LiDAR, IMU, and binocular depth camera RTK sensor. This data, combined with Dynamic Windowing (DWA) and a PointNet network structure, is processed by a system to perform navigation and path planning, and to classify and segment the target point cloud. The processing system is based on the ROS operating system and includes:

[0027] The BEV mapping module creates an overhead view of the environment, obtains a NumPy array, and converts it into a PIL image.

[0028] The path planning module, based on the A* and DWA algorithms, performs automatic navigation and obstacle avoidance.

[0029] The target classification and segmentation module identifies targets using the PointNet network based on point cloud images and trained models to obtain relevant information.

[0030] The control module controls the motor's energizing time by sending corresponding commands to enable the whip-whipping mechanism to reach the target point;

[0031] Repeatedly execute the target classification and segmentation module and the control module until the livestock move away from the boundary threshold, at which point the grazing and herding process ends.

[0032] The robot of this invention is equipped with a lidar, IMU, and binocular depth camera RTK sensor. Data obtained from these sensors, combined with the Dynamic Window Method (DWA) and Pointnet network structure, is processed by a system to perform navigation and path planning, and to classify and segment point clouds of targets. The processing system, based on the ROS operating system, performs overhead mapping of the environment, acquires NumPy arrays, converts them into PIL images, and performs automatic navigation and obstacle avoidance based on the A* and DWA algorithms. Based on the point cloud map and the trained model, it identifies targets through the Pointnet network to obtain relevant information. By sending corresponding commands to control the motor's energizing time, the whip-swinging mechanism can reach the target point. Therefore, this device can drive livestock that are about to leave the enclosure. The device integrates well with the natural environment and solves the problem of autonomous pathfinding and whip-swinging activities using BEV recognition based on point cloud maps, thus achieving true intelligence in the grazing process.

[0033] like Figure 1 As shown, a method for a robotic intelligent herding device based on a point cloud BEV recognition algorithm is also provided, which includes the following steps:

[0034] (1) Construct a BEV diagram from a point cloud diagram;

[0035] (2) Obtain the coordinates of the work area, use the A* algorithm for global path planning, use the Dynamic Window Algorithm (DWA) for local path planning, and automatically navigate to the area.

[0036] (3) The input point cloud image is processed using the Pointnet algorithm to detect the head or tail of livestock, determining whether any livestock are about to leave the enclosure, and obtaining the coordinates (x, y, y) of the head or tail target. a0 ,y a0 ,z a0 );

[0037] (4) Drive the animals by controlling the whip-whipping mechanism with a motor;

[0038] (5) Repeat steps (3) and (4) to complete the expulsion work in the area.

[0039] Preferably, step (1) includes the following sub-steps:

[0040] (1.1) Extracting point cloud data: Extract the X, Y, Z coordinates from the input point cloud data, representing them as an array of the form [N,3].

[0041]

[0042] Where N is the number of point clouds, x N ,y N ,zN This indicates the position of each point in the coordinate system;

[0043] (1.2) Determine the region of interest:

[0044] Define a cube centered on the robot, with the following dimensions: left-right, front-back, and up-down.

[0045] side_range∈(-d x ,d x )

[0046] fwd_range∈(-d y ,d y )

[0047] height_range∈(0,d z )

[0048] side_rand, fwd_range, and height_rand together form the region of interest (ROI) indices of the point cloud. Relevant data within this region is retained, while the remaining point cloud data is filtered.

[0049] x_points = x_points[indices]

[0050] y_points = y_points[indices]

[0051] z_points = z_points[indices]

[0052] Where x_points, y_points, and z_points represent the point cloud coordinates along the x, y, and z axes, respectively (e.g., ...). Figure 5 (as shown);

[0053] (1.3) Convert point cloud positions to pixel coordinates: Set a resolution res, and represent the pixel position value as:

[0054]

[0055]

[0056] x image y image These are the x-axis and y-axis coordinates in the image coordinate system, respectively.

[0057] (1.4) Limit the height to a fixed value d z Furthermore, it is rescaled within the range (0, 255) to create an empty image array suitable for the image size, and then filled with pixel values ​​representing the height information of the point cloud data, scaled using the following formula:

[0058] pixel_values=((a-min) / (max-min))*255

[0059] Where a is (y image x image The z-axis coordinates at point (), min represents the minimum z-axis coordinate value, max represents the maximum z-axis coordinate value, and pixel_values ​​stores the z-axis information of each point;

[0060] (1.5) Based on the visualization requirements, convert the 2D NumPy array returned by the function into a PIL image.

[0061] like Figure 2 As shown, preferably, in step (2), the coordinates of the destination point are input on the map, with the default coordinate system being wgs84. The robot completes automatic navigation and path planning based on the DWA algorithm, including the following sub-steps:

[0062] (2.1) Rasterized map: The BEV map is rasterized, and the region is simplified into a two-dimensional array. Each element in the array has two states: unobstructed and obstructed.

[0063] (2.2) Global path planning using the A* algorithm:

[0064] Choosing a suitable point as the origin O, and taking the current coordinates as A, the cost distance is calculated using Manhattan distance, expressed as:

[0065] d = |x1 - x0| + |y1 - y0|

[0066] Where d represents the target distance from the origin to the destination, and (x1, y1) and (x0, y0) represent the coordinates of the destination and the initial point in the grid map;

[0067] Cost estimation using the A* algorithm is expressed as follows:

[0068] f(n) = g(n) + h(n)

[0069] Where f(n) represents the cost estimate from the initial position through state n to the target state, g(n) is the actual cost, and h(n) is the optimal path cost. The optimal path obtained in this way is taken as the navigation path.

[0070] (2.3) Velocity Space Sampling: Based on the robot's hardware and obstacle avoidance requirements, the sampling and constraint conditions of the relevant velocity space are expressed as follows:

[0071] V m ={(v,ω)|v∈[v min ,v max],ω∈[ω min ,ω min ]}

[0072]

[0073]

[0074] Among them, V m V d V a These represent the boundary limits for speed, acceleration, and obstacle distance, respectively. vmax a ωmax v represents the maximum linear / angular acceleration. min ,v max ω min ,ω min v represents the robot's minimum / maximum linear / angular velocity. c ω c The linear velocity and angular velocity at the current moment are represented by dist(v,ω), and the distance between the trajectory and the obstacle is represented in space as follows:

[0075] V s =V a ∩V d ∩V m ;

[0076] (2.4) Trajectory Prediction: The robot chassis is considered as a rear-wheel-drive single-vehicle model, and the kinematic model with the rear axle center as the vehicle center is expressed as:

[0077]

[0078]

[0079] Where δ f The central angle corresponding to the arc. Let x and y represent the linear velocity components in the x and y directions, respectively; ψ represents the angle between the velocity direction and the x-axis; and L is the wheelbase between the front and rear wheels of the chassis. v is the angular velocity of the rear wheel, and v is the linear velocity of the rear wheel;

[0080] (2.5) Trajectory Evaluation: For multiple trajectories, evaluation and selection are performed using azimuth, velocity, and distance, and the trajectories are normalized. A higher score indicates a better trajectory, thereby determining v. c ,ω c With two parameters, the trajectory evaluation function is expressed as:

[0081] G(v i ,ω i )=σ(α*heading(vi ,ω i ))+σ(β*dist(v i ,ω i ))+σ(γ*velocity(v i ,ω i ))

[0082] Where α, β, and γ represent the weighting coefficients in the evaluation, used to balance the importance of different evaluation indicators;

[0083] If the angle between the velocity direction and the target point is θ, then to satisfy the condition that a higher score indicates a better trajectory, it is represented as follows:

[0084] heading(v i ,ω i )=2π-θ

[0085] dist(v i ,ω i ) represents the center of motion of the chassis (x0, y0) and the obstacle (x). d y d The distance between them is expressed as:

[0086]

[0087] Speed ​​evaluation takes linear velocity v i To indicate:

[0088] velocity(v i ,ω i ) = v i

[0089] σ represents the normalization process, expressed as:

[0090]

[0091]

[0092]

[0093] By comparing the values ​​of the trajectory evaluation function under different linear velocities and angular velocities through the above processing, a better trajectory can be obtained.

[0094] like Figure 3 , 4 As shown, preferably, in step (3), the obtained point cloud map is processed to obtain the coordinates (x, y) of the animal's head or tail based on Pointnet. a0 ,y a0 ,z a0 The process includes the following steps:

[0095] (3.1) Input a point cloud network containing information about N points

[0096] (3.2) Design the network architecture, including: input layer, multiple feature extraction layers, global feature integration layer, and output layer;

[0097] (3.3) The PointNet processing network is used to detect and segment the point cloud, and to identify the coordinates (x, y) of the head or tail end of the livestock. a0 ,y a0 ,z a0 ).

[0098] Preferably, step (4) includes the following sub-steps:

[0099] (4.1) Determine the coordinate system and related positions: Establish a coordinate system with the lidar as the origin O. The whip is parallel to the xz plane. Since the mechanical structure of the whip is fixed, obtain the default position coordinates of its end point W(x w0 ,y w0 ,z w0 The other end's coordinates are point A(x0, y0, z0), and the target coordinates (x0, y0, z0) are obtained from the BEV diagram and point cloud diagram. a0 ,y a0 ,z a0 );

[0100] (4.2) Moving the whip to the target point: The whip's movement is simplified to a model. Since the relevant coordinates are already obtained, all the relevant joints of the whip are controlled by motors. Only the start and stop times of the motors need to be controlled. The angular velocity of the whip-lifting motor is known to be ω. up The linear velocity of the whip extension motor is v. stretch The length of the whip base is l w The angle between the whip and the xy plane at a certain moment is expressed as:

[0101]

[0102] Wherein, W(x) wt ,y wt ,z wt ) represents the coordinates of the end point of the whip, θ0 represents the angle between the whip and the xy plane in the initial case, and tup represents the energizing time of the lifting motor;

[0103] In the xz plane, the whip moves around point A during both its lifting and lowering phases, so its final trajectory can be considered as an arc, represented as:

[0104] (x-x0) 2 +(z-z0) 2 =l w 2

[0105] From the default position through lifting / lowering motion t up The coordinate position of the end point after time is obtained from the above formula. At this time, z t and x t :

[0106]

[0107]

[0108] The trajectory of the whip's end during rotational motion is an arc in the xy-plane, represented as:

[0109] (x-x0) 2 +(y-y0) 2 =(l w cosθ t ) 2

[0110] After the whip handle has undergone raising, lowering, and rotating operations to allow its end to extend and reach the target point, the energizing time is denoted as t. stretch At this time, the coordinates of the end of the whip are The distance D between the target point and the end of the whip is expressed as:

[0111]

[0112] D = t stretch *v stretch

[0113] Upon reaching the target point, the whip motor is activated to drive the animal away.

[0114] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any simple modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A robotic intelligent herding device based on a point cloud BEV recognition algorithm, characterized in that: The robot is equipped with LiDAR, IMU, binocular depth camera, and RTK sensor data. The data is combined with the Dynamic Window Method (DWA) and Pointnet network structure. The processing system performs navigation and path planning, and performs point cloud classification and point cloud segmentation for the target. The processing system is based on the ROS operating system and includes: The BEV mapping module creates an overhead view of the environment, obtains NumPy arrays, and converts them into PIL images. The path planning module, based on the A* and DWA algorithms, performs automatic navigation and obstacle avoidance. The target classification and segmentation module identifies targets using the PointNet network based on point cloud images and trained models to obtain relevant information. The control module controls the motor's energizing time by sending corresponding commands to enable the whip-whipping mechanism to reach the target point; Repeat the target classification and segmentation module and the control module until the livestock move away from the boundary threshold, and the grazing and driving process ends. In the path planning module, the coordinates of the destination point are input on the map, with the default coordinate system being WGS84. The robot completes automatic navigation and path planning based on the DWA algorithm, including the following steps: (2.1) Rasterized map: The BEV map is rasterized, and the region is simplified into a two-dimensional array. Each element in the array has two states: unobstructed and obstructed. (2.2) Global path planning using the A* algorithm: Choosing a suitable point as the origin O, and taking the current coordinates as A, the cost distance is calculated using Manhattan distance, expressed as: ， in This represents the target distance from the origin to the destination. and Indicates the coordinates of the endpoint and the initial point in the raster map; Cost estimation using the A* algorithm is expressed as follows: ， in This represents the cost estimate for traveling from the initial position through state n to the target state. For the actual cost, The optimal path is chosen as the navigation path, taking the cost of the optimal path. (2.3) Velocity Space Sampling: Based on the robot's hardware and obstacle avoidance requirements, the sampling and constraint conditions of the relevant velocity space are expressed as follows: ， ， ， in, These represent the boundary limits for speed, acceleration, and obstacle distance, respectively. Indicates the maximum linear / angular acceleration. , , , This represents the robot's minimum / maximum linear / angular velocity. , This represents the linear velocity and angular velocity at the current moment. Indicates the distance between the trajectory and the obstacle; The velocity space is represented as: ； (2.4) Trajectory Prediction: The robot chassis is considered as a rear-wheel-drive single-vehicle model, and the kinematic model with the rear axle center as the vehicle center is expressed as: ， ， in The central angle corresponding to the arc. , They represent linear velocities at... , The linear velocity component in the direction, Indicates the direction of velocity and The included angle between the axles, where L is the wheelbase between the front and rear wheels of the chassis. Let be the angular velocity of the rear wheel. The linear velocity of the rear wheel; (2.5) Trajectory Evaluation: For multiple trajectories, evaluation and selection are performed using azimuth, velocity, and distance, and the trajectories are normalized. Higher scores indicate better trajectories, thus determining two trajectories. , The parameters, the trajectory evaluation function are expressed as: ， in, , , This represents the weighting coefficient in the evaluation, used to balance the importance of different evaluation indicators; If the angle between the velocity direction and the target point is... To satisfy the condition that a higher score indicates a better trajectory, it is represented as follows: ， Indicates the center of motion of the chassis and obstacles The distance between them is expressed as: ， Speed ​​evaluation takes line speed To indicate: ， This indicates normalization, expressed as: ， ， ， By comparing the values ​​of the trajectory evaluation function under different linear velocities and angular velocities through the above processing, a better trajectory can be obtained.

2. The robotic intelligent herding device based on point cloud BEV recognition algorithm according to claim 1, characterized in that: It includes the following steps: (1) Construct a BEV diagram from a point cloud diagram; (2) Obtain the coordinates of the working area, use the A* algorithm for global path planning, and the Dynamic Window Algorithm (DWA) for local path planning, and automatically navigate to the area; (3) The input point cloud map is processed by the Pointnet algorithm to detect the head or tail of livestock, determine whether there are livestock about to leave the fence, and obtain the coordinates of the head or tail target. ; (4) Drive the animal away by controlling the whip-whipping mechanism with a motor; (5) Repeat steps (3) and (4) to complete the expulsion work in the area.

3. The robotic intelligent herding device based on point cloud BEV recognition algorithm according to claim 2, characterized in that: Step (1) includes the following sub-steps: (1.1) Extracting point cloud data: Extracting point cloud data from the input point cloud data. The coordinates are represented as in the form of Array: ， in For the number of point clouds, Indicates the position of each point in the coordinate system; (1.2) Determine the region of interest: Define a cube centered on the robot, with the following dimensions: left-right, front-back, and up-down. ， ， ， Together they form the region of interest in the point cloud. Relevant internal data is retained, while the remaining point cloud data is filtered. ， ， ， in , , They represent , , The coordinates of the point cloud system of the axis; (1.3) Convert point cloud positions to pixel coordinates: Set a resolution The position value of a pixel is represented as: ， ， , In the image coordinate system shaft and Axis coordinates; (1.4) Limit the height to a fixed value , and again in To scale the image, create an empty image array that fits the image size and fill it with pixel values ​​representing the height information of the point cloud data. Scale it using the following formula: ， in for place Axis coordinates express Minimum coordinate value of the axis express Maximum coordinate value of axis Store each point Axis information; (1.5) Based on the visualization requirements, convert the 2D NumPy array returned by the function into a PIL image.

4. The robotic intelligent herding device based on point cloud BEV recognition algorithm according to claim 3, characterized in that: In step (3), the obtained point cloud map is processed to obtain the coordinates of the head or tail of the livestock based on Pointnet. This includes the following steps: (3.1) Input a point cloud network containing information about N points ; (3.2) Design the network architecture, including: input layer, multiple feature extraction layers, global feature integration layer, and output layer; (3.3) The PointNet processing network is used to detect and segment the point cloud, and identify the coordinates of the head or tail end of the livestock. .

5. The robotic intelligent herding device based on point cloud BEV recognition algorithm according to claim 4, characterized in that: Step (4) includes the following sub-steps: (4.1) Determine the coordinate system and related positions: with the lidar as the origin. Establish a coordinate system, whip and Since the mechanical structure of the whip is fixed, the default coordinates of its end point are obtained when the planes are parallel. The other end coordinate point The target coordinates are obtained from the BEV map and point cloud map. ; (4.2) Moving the whip to the target point: The whip's movement is simplified to a model. Since the relevant coordinates are already obtained, all joints of the whip are controlled by motors. Only the start and stop times of the motors need to be controlled. The angular velocity of the whip-lifting motor is known to be... The linear velocity of the whip extension motor is The length of the whip base is At a certain moment, the whip and The included angle between planes is expressed as: ， in, The coordinates of the point at the end of the whip are: This indicates the initial state of the whip and... Angle between planes Indicates the energizing time of the lifting motor; Since the whip moves around point A during both its lifting and lowering in the plane, its final trajectory can be considered as an arc, represented as: ， From the default position through lifting / lowering motion The coordinate position of the end point after time is obtained from the above formula. and : ， ， The trajectory of the whip tip during rotational motion is In a plane, a circular arc is represented as: ， After the whip handle has undergone raising, lowering, and rotating operations to allow its end to extend and reach the target point, the energizing time is expressed as follows: At this time, the coordinates of the end of the whip are The distance between the target point and the end of the whip is... Represented as: ， ， Upon reaching the target point, the whip motor is activated to drive the animal away.

Images