Ewe pregnancy detection method and system based on multi-sensor fusion

Abstract

The invention discloses an ewe pregnancy detection method and system based on multi-sensor fusion, and relates to the technical field of pregnancy detection and intelligent robots, and the method comprises the steps: obtaining a high-precision point cloud map of an area where an ewe is located; positioning the ewe pregnancy detection robot in the high-precision point cloud map based on NDT and ICP according to the high-precision point cloud map; based on the position of the ewe pregnancy detection robot in the area, determining a path from the ewe pregnancy detection robot to the ewe by adopting an RRT algorithm; and after reaching the appointed position, performing pregnancy detection on the ewe based on the ewe pregnancy detection robot. According to the invention, efficient automatic detection of the ewe pregnancy state can be realized.

Description

Technical Field

[0001] This application relates to the technical fields of pregnancy detection and intelligent robot, and particularly to a method and system for detecting pregnancy of ewes based on multi-sensor fusion. Background Art

[0002] The reproductive capacity of ewes directly affects the production efficiency and economic benefits of the sheep industry. Pregnancy diagnosis can improve the reproductive efficiency of sheep, reduce non-pregnancy, lower feeding costs, and increase the benefits of sheep farming. Compared with cattle, ewes have a shorter gestation period, may develop pregnancy toxemia in the late pregnancy, and many breeds of ewes give birth to multiple lambs at a time. Therefore, pregnancy diagnosis of ewes is particularly important in intensive farming.

[0003] Currently, the pregnancy diagnosis of ewes mostly relies on experienced veterinarians or workers to judge whether a ewe is pregnant by palpation or observing the behavior of the ewe. The accuracy of the detection results of this method highly depends on the level of the operator. In addition, the use of scanning detection equipment requires specialized technical personnel training, the operation is cumbersome, usually three people are required to cooperate for fixation and detection, and it is difficult to be popularized in large-scale farms. Summary of the Invention

[0004] The purpose of this application is to provide a method and system for detecting pregnancy of ewes based on multi-sensor fusion, which can realize the efficient and automatic detection of the pregnancy status of ewes.

[0005] To achieve the above purpose, this application provides the following solutions: In the first aspect, this application provides a method for detecting pregnancy of ewes based on multi-sensor fusion, including: Obtain a high-precision point cloud map of the area where the ewe is located.

[0006] Based on the high-precision point cloud map, and based on NDT and ICP, locate the ewe pregnancy detection robot in the high-precision point cloud map.

[0007] Based on the position of the ewe pregnancy detection robot in the area, use the RRT algorithm to determine the path from the ewe pregnancy detection robot to the ewe.

[0008] When reaching the specified position, based on the ewe pregnancy detection robot, perform pregnancy detection on the ewe.

[0009] Optionally, before obtaining the high-precision point cloud map of the area where the ewe is located, it further includes: Based on the lidar module and the depth camera, obtain the point cloud data and image data of the area where the ewe is located respectively.

[0010] Perform distortion removal processing on the point cloud data to obtain the processed point cloud data.

[0011] Based on the trained semantic segmentation network model, perform object recognition on the image data to obtain the recognition results of each object in the image data.

[0012] Remove the dynamic point cloud information in the recognition results, extract planar points and corner points, and set the first-frame lidar point cloud as the key frame.

[0013] Select the set frame according to the position and time between the current frame and the key frame, and establish a local corner point and planar point cloud map based on the set frame.

[0014] Estimate the initial state of the current frame by calculating the IMU pre-integration between the previous-frame lidar point cloud and the current-frame lidar point cloud and the state of the previous frame, publish the lidar odometry in real time, and generate a loop closure factor by selecting the key frames adjacent to the current frame in real time according to the spatio-temporal relationship.

[0015] Adopt a high-precision time synchronization method based on the 5G network to synchronize the data of the lidar module, IMU data processing sub-module, GPS odometry sub-module, and RFID detection sub-module.

[0016] Establish an unscented Kalman filter observation equation, use the UKF algorithm to filter the data information output by the sensors, and fuse the estimates of each sensor to determine the global optimal estimate.

[0017] Fuse the point cloud information of each key frame into a global point cloud map.

[0018] Combine the GPS information, lidar odometry, loop closure, and RFID detection factor to optimize the global point cloud map to obtain a high-precision point cloud map.

[0019] Optionally, the unscented Kalman filter observation equation is specifically: 。

[0020] 。

[0021] Where f( ) is the process model, is the state vector, , is the translation vector, is the velocity vector, is the rotation vector, is the accelerometer bias, is the gyroscope bias, g is the gravity, is the system noise matrix, is the system noise, is the observation value vector, is the observation error, the subscript k is the k-th moment, k - 1 is the (k - 1)-th moment, h k() is a non-linear observation model function.

[0022] Optionally, the semantic segmentation network model is a DeepLab V3+ image semantic segmentation network model.

[0023] Optionally, based on the high-precision point cloud map and using NDT and ICP, the positioning of the ewe pregnancy detection robot in the high-precision point cloud map is performed, specifically including: The collected point cloud data is segmented into a plurality of voxels of the same size.

[0024] For the point set within each voxel, the mean value and covariance matrix of the point cloud data are calculated.

[0025] The probability distribution of each point is modeled as a normal distribution to obtain the Gaussian probability function P(x).

[0026] The collected point cloud data is mapped to the target point cloud using the initial transformation parameters, and a set number of nearest neighbor points closest to the collected point cloud data are found in the target point cloud and fitted into a plane.

[0027] An error function is established between the plane formed by the nearest neighbor points .

[0028] An equation of the least squares problem for the point cloud data, rotation transformation matrix, and translation transformation vector is established.

[0029] The least squares problem equation is solved to obtain the optimal rotation transformation matrix R and translation transformation vector t.

[0030] Based on the optimal rotation transformation matrix R and translation transformation vector t, the ewe pregnancy detection robot is positioned.

[0031] Optionally, the formula expression of the Gaussian probability function P(x) is: .

[0032] Where x is the point set within the voxel x = { , … }, n is the total number of points within the voxel, c is a constant, is the mean value of the q point cloud, C is the covariance matrix, , T is the transpose matrix.

[0033] Optionally, the formula expression of the error function is: .

[0034] Where R is the rotation transformation matrix, t is the translation transformation vector, d is the intercept, and N is the column vector of the unit length normal.

[0035] Optionally, the expression of the least squares problem equation is: .

[0036] Wherein, is point cloud data.

[0037] Optionally, after reaching the specified position, based on the ewe pregnancy detection robot, pregnancy detection is performed on the ewe, specifically including: After reaching the specified position, when receiving the instruction that the ewe enters the pregnancy detection robot, control the depth camera to detect the positions of the four limbs of the ewe.

[0038] Based on the positions of the four limbs of the ewe, control the robotic claw to fix the four limbs of the ewe, and control the elastic bandage device to flexibly fix the abdomen of the ewe.

[0039] Based on the robotic arm carrying the B-ultrasound probe, scan the ewe to obtain the B-ultrasound image of its uterus.

[0040] Based on the collected B-ultrasound image, use image preprocessing technology to preprocess the image.

[0041] Adopt the Unet semantic segmentation algorithm to train the preprocessed image to obtain an ewe pregnancy diagnosis model; the ewe pregnancy diagnosis model is used to detect whether the ewe is pregnant and identify the number, size and morphological information of the embryos.

[0042] In a second aspect, the present application provides an ewe pregnancy detection system based on multi-sensor fusion, including: A map acquisition module, configured to acquire a high-precision point cloud map of the area where the ewe is located.

[0043] A positioning module, configured to locate the ewe pregnancy detection robot in the high-precision point cloud map based on NDT and ICP according to the high-precision point cloud map.

[0044] A path planning module, configured to determine the path from the ewe pregnancy detection robot to the ewe by using the RT algorithm based on the position of the ewe pregnancy detection robot in the area.

[0045] A pregnancy detection module, configured to perform pregnancy detection on the ewe based on the ewe pregnancy detection robot after reaching the specified position.

[0046] According to the specific embodiments provided by the present application, the following technical effects are disclosed in the present application: This application provides a method and system for detecting ewes' pregnancy based on multi-sensor fusion. First, a high-precision point cloud map of the area where the ewes are located is obtained. Then, the NDT and ICP algorithms are used to accurately locate the ewe pregnancy detection robot in the high-precision point cloud map. Next, according to the specific position of the ewe pregnancy detection robot in this area, the RT algorithm is used to plan the best path from the robot to the ewe. After reaching the specified position, the ewe pregnancy detection robot is used to detect the pregnancy status of the ewe. This application aims to achieve efficient and automatic detection of the pregnancy status of ewes. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present application. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0048] Figure 1 It is a schematic flowchart of a method for detecting ewes' pregnancy based on multi-sensor fusion provided by an embodiment of the present application.

[0049] Figure 2 It is a structural diagram of a system for detecting ewes' pregnancy based on multi-sensor fusion provided by an embodiment of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0050] The following will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some embodiments of the present application, rather than all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present application.

[0051] To make the above objects, features, and advantages of the present application more obvious and understandable, the present application will be further described in detail below with reference to the drawings and specific embodiments.

[0052] Embodiment 1 This embodiment provides a method for detecting ewes' pregnancy based on multi-sensor fusion, including: Step 101: Obtain a high-precision point cloud map of the area where the ewes are located.

[0053] Step 102: Based on the high-precision point cloud map, and based on NDT and ICP, locate the ewe pregnancy detection robot in the high-precision point cloud map.

[0054] Step 103: Based on the position of the ewe pregnancy detection robot in the area, use the RT algorithm to determine the path from the ewe pregnancy detection robot to the ewe.

[0055] Step 104: When reaching the designated position, perform pregnancy detection on the ewe based on the ewe pregnancy detection robot.

[0056] Among them, before executing step 101, it further includes: A1: Based on the lidar module and the depth camera, obtain the point cloud data and image data of the area where the ewe is located respectively.

[0057] A2: Perform distortion removal processing on the point cloud data to obtain the processed point cloud data.

[0058] A3: Based on the trained semantic segmentation network model, perform object recognition on the image data to obtain the recognition results of each object in the image data.

[0059] A4: Remove the dynamic point cloud information in the recognition results, extract the plane points and corner points, and set the first frame of laser point cloud as the key frame.

[0060] A5: According to the position and time between the current frame and the key frame, select the set frame, and establish a local corner point and plane point cloud map based on the set frame.

[0061] A6: Estimate the initial state of the current frame by calculating the IMU pre-integration between the previous frame of laser point cloud and the current frame of laser point cloud and the state of the previous frame, publish the laser odometer in real time, and generate the loop closure factor by selecting the key frames adjacent to the current frame in real time according to the spatio-temporal relationship.

[0062] A7: Adopt a high-precision time synchronization method based on the 5G network to synchronize the data of the lidar module, the IMU data processing sub-module, the GPS odometer sub-module and the RFID detection sub-module in time.

[0063] A8: Establish an unscented Kalman filter observation equation, use the UKF algorithm to filter the data information output by the sensors, and fuse the estimates of each sensor to determine the global optimal estimate.

[0064] A9: Fuse the point cloud information of each key frame into a global point cloud map.

[0065] A10: Combine the GPS information, the laser odometer, the loop closure and the RFID detection factor to optimize the global point cloud map to obtain a high-precision point cloud map.

[0066] Among them, in some embodiments, when executing steps A1 - A6, specifically, it can be as follows: Distort the point cloud data, and use the attitude (such as roll, pitch, and yaw angles) and acceleration information provided by the IMU to accurately compensate and correct the point cloud distortion caused by the movement or attitude change of the sensor itself. During the scanning process, the IMU can record the dynamic information of the radar sensor in real time, such as the rotation angle and acceleration, so as to calculate the attitude change during lidar scanning. Combined with the time synchronization information of the lidar, the IMU data can help correct the spatial coordinates of the laser point cloud and remove the point cloud distortion caused by the movement of the radar.

[0067] Then, according to the image data acquired by the depth camera, use the DeepLab V3+ image semantic segmentation network model to identify and classify objects into known and unknown categories (such as sheep in a sheep farm, walking people, feed trucks, and feeding pens, etc.). After manually annotating the unknown objects, use the small incremental sample learning method to expand the object recognition categories and update the model to improve the feature learning effect. Remove the dynamic point cloud information according to the recognition results, extract plane points and corner points, set the first frame of laser point cloud as the key frame, and initialize the key frame information based on the IMU data. Select appropriate key frames according to the position and time between the current frame and the key frame, establish a local corner point and plane point cloud map, and perform downsampling; at the same time, calculate the IMU pre-integration between the previous frame of laser point cloud and the current frame of laser point cloud and the state of the previous frame to deduce the initial state estimate of the current frame, publish the laser odometer in real time, and generate loop closure factors by selecting key frames adjacent to the current frame in real time according to the spatio-temporal relationship.

[0068] Among them, in some embodiments, when performing steps A6 - A10, it can be specifically as follows: 1) Time synchronization: Adopt a high-precision time synchronization method based on the 5G network to synchronize the data of the lidar, IMU, GNSS, and RFID in time.

[0069] 2) Establish the motion equation: The mobile robot adopts a navigation coordinate system (north-east-up coordinate system), is the ideal transformation matrix between the navigation coordinate system (n system) and the vehicle body coordinate system (b system), and the motion equation is: .

[0070] .

[0071] .

[0072] Among them, the position p = represents longitude, latitude, and altitude; the vehicle speed represents the velocity components in the north-east-up coordinates; is the attitude quaternion, is the time derivative of the attitude quaternion, that is, the rate of attitude change; , and are the Earth radius parameters, and Rc is the position update scaling matrix in the navigation coordinate system (north-east-up coordinate system), which is used to convert the velocity components into the rates of change of longitude, latitude, and altitude; are the vehicle acceleration components in the body coordinate system; and are the biases of the accelerometer and gyroscope respectively; , and are the angular velocities in different coordinate systems, is the vehicle acceleration.

[0073] is the projection of the angular velocity of the body coordinate system (b system) relative to the navigation coordinate system (n system) in the navigation coordinate system (n system).

[0074] is the rotation of the navigation coordinate system (n system) caused by the Earth's rotation, and the angular velocity of the Earth coordinate system (e system) relative to the inertial coordinate system (i system) is projected in the navigation coordinate system (n system).

[0075] is the rotation of the navigation coordinate system (n system) caused by the curvature of the Earth's surface when the inertial navigation system moves near the Earth's surface.

[0076] 3) Establish the unscented Kalman filter observation equation: The unscented Kalman filter (UKF) uses the unscented transform (UT transform) to obtain the sigma sampling points in the original state according to the mean and covariance of the prior probability, and approximates the system model on the probability density. Therefore, it does not need to calculate the complex Jacobian determinant, and the simulation accuracy of the system model reaches the third order, which greatly improves the efficiency; the UKF algorithm is used to filter the data information output by the sensor, and then fuse the estimates of all sensors to obtain the global optimal estimate. The dynamic observation equation of the Kalman filter for the nonlinear system is given by the following formula: .

[0077] .

[0078] where f( ) is the process model, is the state vector, , is the translation vector, is the velocity vector, is the rotation vector, is the accelerometer zero bias, is the gyroscope zero bias, and g is the gravity. is the system noise matrix, is the system noise, is the observation value vector, is the observation error. The subscript k represents the k-th moment, k - 1 represents the (k - 1)-th moment, and h k ( ) is the non-linear observation model function, which maps the state vector x k to the observation value z k , that is, it describes how to generate the expected sensor measurement values from the current state.

[0079] After establishing the observation equation, a set of sigma points need to be generated first. These points represent the possible values of the current state. According to the current state vector and the covariance matrix 2m + 1 sigma points are generated (where m is the dimension of the state). These sigma points will be used to approximate the output of the non-linear function through the unscented transformation.

[0080] Usually, sigma points located at the mean value and symmetrically distributed at the covariance of the main axis (two for each dimension) are selected. By sampling according to the following method, 2m + 1 sigma points can be obtained. Then 2m + 1 sigma points can be sampled.

[0081] .

[0082] Among them, x is the point set in the voxel, and μ x represents the mean value of the current state vector, that is, the optimal estimated value of the system state; λ is the scaling parameter, which is used to adjust the distribution range of the sigma points and control the scaling degree of the covariance matrix; represents the matrix as the (i - 1)-th column of the lower triangular matrix after Cholesky decomposition, Similarly.

[0083] Then, the UKF filter is performed, and the posterior probability distribution of the state quantity at the (k - 1)-th moment is sampled. Then, according to the sampled sigma point set, a non-linear transformation of the state transfer is performed, and the approximate mean value and variance of the prior probability of the state quantity at the k-th moment are calculated. After that, sigma sampling is performed on this prior probability distribution, and the cross-covariance between the state quantity and the observation quantity is calculated using the sigma point set. Finally, the posterior probability distribution of the state quantity at the k-th moment is obtained, and the laser odometer is published.

[0084] All the key frame point cloud information is fused into the global point cloud map. Combining GPS information, laser odometer, loop closure, and RFID detection factors, global factor graph optimization is carried out to optimize the global point cloud map, eliminate the cumulative error, and construct a high-precision point cloud map.

[0085] In some embodiments, when performing step 102, it specifically includes: B1: Divide the collected point cloud data into a plurality of voxels of the same size.

[0086] B2: For the point set within each voxel, calculate the mean value and covariance matrix of the point cloud data.

[0087] B3: Model the probability distribution of each point with a normal distribution to obtain the Gaussian probability function P(x).

[0088] B4: Map the collected point cloud data to the target point cloud using the initial transformation parameters, and find a set number of the nearest neighbor points in the target point cloud that are closest to the collected point cloud data, and fit them into a plane.

[0089] B5: Establish an error function between the plane formed by the nearest neighbor points .

[0090] B6: Establish a least-squares problem equation for the point cloud data, rotation transformation matrix, and translation transformation vector.

[0091] B7: Solve the least-squares problem equation to obtain the optimal rotation transformation matrix R and translation transformation vector t.

[0092] B8: Locate the ewe pregnancy detection robot according to the optimal rotation transformation matrix R and translation transformation vector t.

[0093] Among them, the formula expression of the Gaussian probability function P(x) is: .

[0094] In the formula, x is the point set within the voxel x = { , … },n is the total number of points within the voxel, c is a constant, is the mean value of the q point cloud, C is the covariance matrix, , T is the transpose matrix.

[0095] The error function The formula expression of is: .

[0096] Among them, R is the rotation transformation matrix, t is the translation transformation vector, d is the intercept, and N is the column vector of the unit length normal.

[0097] The expression of the least-squares problem equation is: .

[0098] Among them, is the point cloud data.

[0099] Specifically, when executing B1 - B8, first, the collected three - dimensional point cloud data is segmented into multiple voxels of the same size. The size of the voxel determines the degree of point cloud downsampling: the larger the voxel, the higher the downsampling degree and the faster the registration speed; the smaller the voxel, the higher the registration accuracy. For the point set x = { , … } within each voxel, where n is the total number of points in the voxel, calculate the mean q and covariance matrix C of the point cloud: .

[0100] .

[0101] Then, represent the probability distribution of each point in the small cube using the Gaussian probability function P(x), that is, perform normal distribution modeling: .

[0102] where c is a constant.

[0103] Use the initial transformation parameter p to map the collected point cloud data to the target point cloud, so that the point cloud data is converted into a multi - segment smooth three - dimensional space representation based on the normal distribution.

[0104] Solve for the optimal transformation parameter p according to the maximum likelihood function φ: .

[0105] In the formula, T(p, ) represents using the pose transformation T to move .

[0106] After calculating the probability distribution function, when the maximum likelihood function reaches the maximum value, solve for the transformation parameter p.

[0107] According to the initially solved transformation parameter p, map the collected three - dimensional point cloud to the target point cloud, and find the five nearest neighbor points in the target point cloud that are closest to the collected point cloud data and fit them into a plane. Assume the parameters of the fitted plane are (N, d) ∈ R4, where N is the column vector of the unit - length normal and d is the intercept.

[0108] Establish the error function between the plane formed by its closest points: .

[0109] where R is the rotation transformation matrix and t is the translation transformation vector.

[0110] Establish a least - squares problem with q i , R, and t as variables: 。

[0111] Solve the least - squares problem to obtain the optimal rotation transformation matrix \(R\) and translation transformation vector \(t\). Through the obtained \(R\) and \(t\), the exact position of the mobile robot in the point - cloud map can be calculated.

[0112] In some embodiments, when performing step 103, it can be specifically as follows: Adopt an improved RT (Rapidly - exploring Random Tree) algorithm, optimize the tree - growth strategy or sampling method to speed up the path - search speed, and introduce heuristic methods, optimization strategies, and post - processing techniques to generate better - quality paths. This algorithm is applicable to the dynamically changing environment of the farm and can quickly generate a feasible path from the starting point to the end point, especially for the dynamic obstacles that may appear in the farm, such as the movement of staff or poultry.

[0113] Through the elastic bandage device mounted on the robot, stabilize the body and abdomen of the ewe, and combine with the DeepLabV3+ image semantic segmentation network model to accurately identify the pregnancy area of the ewe. The identified position information will be transmitted to the robotic arm to guide its reasonable path planning. At the same time, the robotic arm is equipped with a force - feedback module, which can monitor the pressure applied to the ewe's abdomen in real time to avoid harming the ewe, thus realizing safe and accurate pregnancy detection.

[0114] The specific work process is as follows: Overall work process: After establishing a high - precision map of the sheep farm through a lidar, set the target working point. After the robot navigates to the specified position through path planning, lead the ewe to the ewe feeding station, and the pregnancy - detection robot starts to work. First, control the robotic arm to move to a fixed position, then detect the positions of the ewe's limbs through a depth camera, transmit the collected images to the industrial control computer. Then, the industrial control computer controls the floating slider device to automatically adjust the position of the robotic claw according to the positions of the ewe's limbs, and then controls the servo motor to control the robotic claw to ensure that it can fix the ewe's limbs. The robotic claw is covered with a rubber material, which can increase the friction with the ewe's abdomen on the one hand and protect the ewe's abdomen on the other hand. There is also an elastic bandage installed in the middle of the feeding station, which moves upward along the guide rail by controlling the floating slider to fix the ewe's abdomen; identify the pregnancy - detection area of the ewe through the depth camera installed on the robotic arm, transmit its position to the industrial control computer, and perform path planning and control on the 5 - degree - of - freedom robotic arm to ensure its pregnancy detection of the ewe.

[0115] In some embodiments, when performing step 104, it can be specifically as follows: After reaching the specified position, when receiving the instruction that the ewe enters the pregnancy - detection robot, control the depth camera to detect the positions of the ewe's limbs.

[0116] Based on the positions of the ewe's four limbs, control the robotic claws to fix the ewe's four limbs, and control the elastic bandage device to flexibly fix the ewe's abdomen.

[0117] Based on the robotic arm carrying the B-ultrasound probe to scan the ewe, obtain the B-ultrasound image inside its uterus.

[0118] Based on the collected B-ultrasound image, use image preprocessing technology to preprocess the image.

[0119] Adopt the Unet semantic segmentation algorithm to train the preprocessed image to obtain an ewe pregnancy diagnosis model; the ewe pregnancy diagnosis model is used to detect whether the ewe is pregnant and identify the number, size, and morphological information of the embryos.

[0120] Specifically, the robotic arm carries the B-ultrasound probe to scan the ewe to obtain a clear and complete ultrasound image inside its uterus. Based on the collected B-ultrasound image, use image preprocessing technology (such as noise removal, contrast enhancement, etc.) to improve the image quality and highlight the key features. The preprocessed image is transmitted to the Unet semantic segmentation algorithm for training to obtain an ewe pregnancy diagnosis model. This model can detect whether the ewe is pregnant and identify information such as the number, size, and morphology of the embryos. Finally, the diagnosis result will be presented to the farmers through a user-friendly interface and comprehensively analyzed in combination with historical data to help formulate a scientific breeding management plan.

[0121] Embodiment 2 As Figure 2 shown, this embodiment provides a ewe pregnancy detection system based on multi-sensor fusion, including: A map acquisition module for acquiring a high-precision point cloud map of the area where the ewe is located.

[0122] A positioning module for positioning the ewe pregnancy detection robot in the high-precision point cloud map based on NDT and ICP according to the high-precision point cloud map.

[0123] A path planning module for determining the path from the ewe pregnancy detection robot to the ewe using the RT algorithm based on the position of the ewe pregnancy detection robot in the area.

[0124] A pregnancy detection module for performing pregnancy detection on the ewe based on the ewe pregnancy detection robot when it reaches the specified position.

[0125] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope recorded in this specification.

[0126] In this text, specific examples are used to illustrate the principles and implementation modes of the present application. The description of the above embodiments is only used to help understand the method and its core idea of the present application; at the same time, for those of ordinary skill in the art, according to the idea of the present application, there will be changes in the specific implementation modes and application scopes. In summary, the content of this specification should not be construed as a limitation to the present application.

Claims

1. A method for detecting ewe pregnancy based on multi-sensor fusion, characterized in that: include: Obtain a high-precision point cloud map of the area where the ewes are located; According to the high-precision point cloud map, based on NDT and ICP, positioning the ewe pregnancy detection robot in the high-precision point cloud map; Based on the position of the ewe pregnancy detection robot in the area, the RRT algorithm is used to determine the path from the ewe pregnancy detection robot to the ewe; When arriving at the designated location, the ewe pregnancy detection robot is used to perform pregnancy detection on the ewe.

2. A method for detecting ewe pregnancy based on multi-sensor fusion according to claim 1, characterized in that: Before obtaining a high-precision point cloud map of the area where the ewes are located, it also includes: Based on the LiDAR module and the depth camera, the point cloud data and image data of the area where the ewe is located are obtained respectively; Performing dedistortion processing on the point cloud data to obtain processed point cloud data; Based on the trained semantic segmentation network model, perform object recognition on the image data to obtain recognition results of each object in the image data; Remove the dynamic point cloud information in the recognition result, extract the plane points and corner points, and set the first frame of laser point cloud as a key frame; According to the position and time between the current frame and the key frame, a set frame is selected, and a local corner point and a plane point cloud map are established based on the set frame; By calculating the IMU pre-integration between the laser point cloud of the previous frame and the laser point cloud of the current frame and the state of the previous frame, the initial state of the current frame is estimated, the laser odometer is released in real time, and the key frame adjacent to the current frame is selected in real time according to the spatiotemporal relationship to generate the loop factor; A high-precision time synchronization method based on the 5G network is used to synchronize the data of the lidar module, IMU data processing submodule, GPS odometer submodule, and RFID detection submodule; Establish the unscented Kalman filter observation equation, use the UKF algorithm to filter the data information output by the sensor, and fuse the estimates of each sensor to determine the global optimal estimate; Fuse the point cloud information of each key frame into a global point cloud map; The global point cloud map is optimized by combining GPS information, laser odometer, closed loop and RFID detection factors to obtain a high-precision point cloud map.

3. A method for detecting ewe pregnancy based on multi-sensor fusion according to claim 2, characterized in that: The unscented Kalman filter observation equation is specifically: ； ； Where f ( ) is the process model, is the state vector, , is the translation vector, is the velocity vector, is the rotation vector, is the accelerometer bias, is the gyroscope zero bias, g is gravity, is the system noise matrix, is the system noise, is the observation vector, is the observation error, the subscript k is the kth moment, k-1 is the k-1th moment, and h k () is the nonlinear observation model function.

4. The method for detecting ewe pregnancy based on multi-sensor fusion according to claim 2, characterized in that: The semantic segmentation network model is the DeepLab V3+ image semantic segmentation network model.

5. The method for detecting ewe pregnancy based on multi-sensor fusion according to claim 2, characterized in that: According to the high-precision point cloud map, based on NDT and ICP, the ewe pregnancy detection robot in the high-precision point cloud map is positioned, specifically including: Segment the collected point cloud data into multiple voxels of the same size; For each point set within each voxel, calculate the mean and covariance matrix of the point cloud data; The probability distribution of each point is modeled as a normal distribution to obtain the Gaussian probability function P(x); The collected point cloud data is mapped to the target point cloud using the initial transformation parameters, and a set number of nearest neighbor points that are closest to the collected point cloud data are found in the target point cloud, and they are fitted into a plane; Establish the error function between the plane formed by the nearest neighbor points ; Establish the least squares problem equation of point cloud data, rotation transformation matrix and translation transformation vector; Solve the least squares problem equation to obtain the optimal rotation transformation matrix R and translation transformation vector t; According to the optimal rotation transformation matrix R and translation transformation vector t, the ewe pregnancy detection robot is positioned.

6. A method for detecting ewe pregnancy based on multi-sensor fusion according to claim 5, characterized in that: The formula expression of the Gaussian probability function P(x) is: ； Where x is the point set within the voxel x={ , … }, n is the total number of points in the voxel, c is a constant, is the mean of the q point cloud, C is the covariance matrix, , T is the transposed matrix.

7. The method for detecting ewe pregnancy based on multi-sensor fusion according to claim 5, characterized in that: The error function The formula expression is: ； Among them, R is the rotation transformation matrix, t is the translation transformation vector, d is the intercept, and N is the column vector of the unit length normal.

8. The method for detecting ewe pregnancy based on multi-sensor fusion according to claim 5, characterized in that: The expression of the least squares problem equation is: ； in, Point cloud data.

9. The method for detecting ewe pregnancy based on multi-sensor fusion according to claim 1, characterized in that: When arriving at the designated location, the ewe pregnancy detection robot will perform pregnancy detection on the ewe, including: After arriving at the designated location, upon receiving the instruction of the ewe to enter the pregnancy detection robot, the depth camera is controlled to detect the position of the ewe's limbs; Based on the position of the ewe's limbs, the mechanical claws are controlled to fix the ewe's limbs, and the elastic bandage device is controlled to flexibly fix the ewe's abdomen; The robot arm carries a B-ultrasound probe to scan the ewe and obtain the B-ultrasound image of the inside of its uterus; Based on the acquired B-ultrasound images, the images are preprocessed using image preprocessing technology; The preprocessed images are trained using the Unet semantic segmentation algorithm to obtain a ewe pregnancy diagnosis model; the ewe pregnancy diagnosis model is used to detect whether the ewe is pregnant and to identify the number, size and morphological information of the embryos.

10. A system for detecting ewe pregnancy based on multi-sensor fusion, characterized in that: include: A map acquisition module is used to obtain a high-precision point cloud map of the area where the ewes are located; A positioning module, used for positioning the ewe pregnancy detection robot in the high-precision point cloud map based on the NDT and ICP according to the high-precision point cloud map; A path planning module, for determining a path from the ewe pregnancy detection robot to the ewe using an RRT algorithm based on the position of the ewe pregnancy detection robot in the area; The pregnancy detection module is used to perform pregnancy detection on the ewe based on the ewe pregnancy detection robot after arriving at the designated location.

Images