A ewe pregnancy detection method and system based on multi-sensor fusion

Abstract

The application discloses a ewe pregnancy detection method and system based on multi-sensor fusion, relates to the fields of pregnancy detection and intelligent robot technology, and comprises the following steps: acquiring a high-precision point cloud map of an area where ewes are located; positioning an ewe pregnancy detection robot in the high-precision point cloud map based on NDT and ICP according to the high-precision point cloud map; determining a path of the ewe pregnancy detection robot to the ewes by using an RRT algorithm based on the position of the ewe pregnancy detection robot in the area; and detecting the pregnancy of the ewes by the ewe pregnancy detection robot when the ewe pregnancy detection robot reaches a specified position. The application can realize efficient and automatic detection of the pregnancy state of the ewes.

Description

TECHNICAL FIELD

[0001] The present application relates to the fields of pregnancy detection and intelligent robot technology, in particular to a ewe pregnancy detection method and system based on multi-sensor fusion. BACKGROUND

[0002] The reproductive ability of ewes directly affects the production efficiency and economic benefits of sheep farming. Pregnancy diagnosis can improve the reproductive efficiency of sheep, reduce empty pregnancy, reduce feeding costs, and improve the benefits of sheep farming. Compared with cows, sheep have a short gestation period, may develop toxemia during the late pregnancy, and many breeds of sheep have multiple births per pregnancy, so pregnancy diagnosis of sheep is particularly important in intensive feeding.

[0003] Currently, pregnancy diagnosis of sheep relies on experienced veterinarians or workers, who determine whether a ewe is pregnant by palpation or observation of the ewe's behavior. The accuracy of the detection results of this method depends heavily on the operator's level. In addition, the use of scanning detection equipment requires special technical personnel training, and the operation is cumbersome, usually requiring three people to work together to fix and detect, making it difficult to promote in large-scale farms. SUMMARY

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

[0005] To achieve the above purpose, the present application provides the following solutions:

[0006] In a first aspect, the present application provides a ewe pregnancy detection method based on multi-sensor fusion, comprising:

[0007] Obtaining a high-precision point cloud map of the area where the ewe is located.

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

[0009] Based on the position of the ewe pregnancy detection robot in the area, determining the path of the ewe pregnancy detection robot to the ewe using the RRT algorithm.

[0010] When reaching the specified position, performing pregnancy detection on the ewe based on the ewe pregnancy detection robot.

[0011] Optionally, before obtaining the high-precision point cloud map of the area where the ewe is located, it further comprises:

[0012] Based on the laser radar module and the depth camera, respectively obtaining point cloud data and image data of the area where the ewe is located.

[0013] The point cloud data is subjected to de-distortion processing to obtain processed point cloud data.

[0014] Based on the trained semantic segmentation network model, object recognition is performed on the image data to obtain a recognition result of each object in the image data.

[0015] Dynamic point cloud information in the recognition result is removed, planar points and corner points are extracted, and the first frame of laser point cloud is set as a key frame.

[0016] 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 planar point cloud map is established based on the set frame.

[0017] The initial state of the current frame is estimated by calculating the IMU pre-integral between the previous frame of laser point cloud and the current frame of laser point cloud and the state of the previous frame, real-time publishing laser odometry, and generating a loop factor by selecting a key frame adjacent to the current frame in real time according to the space-time relationship.

[0018] A high-precision time synchronization method based on a 5G network is adopted to synchronize the data of the laser radar module, the IMU data processing submodule, the GPS odometry submodule, and the RFID detection submodule.

[0019] An unscented Kalman filter observation equation is established, and the UKF algorithm is used to filter the data information output by the sensor to fuse the estimates of each sensor to determine the global optimal estimate.

[0020] The key frame point cloud information is fused into a global point cloud map.

[0021] The global point cloud map is optimized to obtain a high-precision point cloud map by combining GPS information, laser odometry, loop closure, and RFID detection factors.

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

[0023] .

[0024] .

[0025] where f ( ) is a process model, is a state vector, , is a translation vector, is a velocity vector, is a rotation vector, is an accelerometer bias, is a gyroscope bias, g is gravity, is a system noise matrix, is a system noise, For the observation vector, To account for observation errors, the subscript k represents time k, k-1 represents time k-1, and h k () represents the nonlinear observation model function.

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

[0027] Optionally, based on the high-precision point cloud map and using NDT and ICP, the pregnancy detection robot for ewes in the high-precision point cloud map is located, specifically including:

[0028] The collected point cloud data is divided into multiple voxels of the same size.

[0029] For each set of points within a voxel, calculate the mean and covariance matrix of the point cloud data.

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

[0031] The initial transformation parameters are used to map the collected point cloud data to the target point cloud, 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 the target point cloud is fitted into a plane.

[0032] Establish the error function between the plane formed by the nearest neighbor points and the plane formed by the nearest neighbor points. .

[0033] Establish the least squares equations for point cloud data, rotation transformation matrix, and translation transformation vector.

[0034] Solving the least squares problem equation yields the optimal rotation transformation matrix R and translation transformation vector t.

[0035] The robot for detecting pregnancy in ewes is positioned based on the optimal rotation transformation matrix R and translation transformation vector t.

[0036] Optionally, the formula for the Gaussian probability function P(x) is:

[0037] .

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

[0039] Optionally, the error function The formula expression is:

[0040] .

[0041] wherein R is a rotation transformation matrix, t is a translation transformation vector, d is an intercept, and N is a column vector of unit length normals.

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

[0043] .

[0044] wherein, is the point cloud data.

[0045] Optionally, when reaching the specified position, based on the ewe pregnancy detection robot, the ewe is subjected to pregnancy detection, specifically including:

[0046] After reaching the specified position, when receiving an instruction for the ewe to enter the pregnancy detection robot, the position of the ewe's limbs is detected by the depth camera.

[0047] Based on the position of the ewe's limbs, the mechanical claw is controlled to fix the ewe's limbs, and the elastic bandage device is controlled to flexibly fix the ewe's abdomen.

[0048] Based on the mechanical arm carrying the B-ultrasound probe, the ewe is scanned to obtain a B-ultrasound image of the inside of the uterus.

[0049] Based on the collected B-ultrasound image, an image preprocessing technique is used to preprocess the image.

[0050] A Unet semantic segmentation algorithm is used 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 shape information of the embryo.

[0051] In a second aspect, the present application provides an ewe pregnancy detection system based on multi-sensor fusion, comprising:

[0052] A map acquisition module is configured to acquire a high-precision point cloud map of an area where the ewe is located.

[0053] A positioning module is configured to position an ewe pregnancy detection robot in the high-precision point cloud map based on NDT and ICP according to the high-precision point cloud map.

[0054] A path planning module is configured to determine a path of the ewe pregnancy detection robot to the ewe based on the position of the ewe pregnancy detection robot in the area using an RT algorithm.

[0055] A pregnancy detection module is configured to detect the pregnancy of the ewe based on the ewe pregnancy detection robot when reaching the specified position.

[0056] According to the specific embodiments provided in the present application, the following technical effects are disclosed:

[0057] The present application provides a ewe pregnancy detection method and system based on multi-sensor fusion. First, a high-precision point cloud map of the area where the ewe is located is obtained. Then, the ewe pregnancy detection robot in the high-precision point cloud map is accurately positioned using NDT and ICP algorithms. Then, according to the specific position of the ewe pregnancy detection robot in the 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. The present application aims to realize efficient and automatic detection of the pregnancy status of the ewe. BRIEF DESCRIPTION OF DRAWINGS

[0058] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed in the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0059] Figure 1 The flowchart of the ewe pregnancy detection method based on multi-sensor fusion provided by an embodiment of the present application.

[0060] Figure 2 The structure diagram of the ewe pregnancy detection system based on multi-sensor fusion provided by an embodiment of the present application. DETAILED DESCRIPTION

[0061] The technical solutions in the embodiments of the present application will be described in detail below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0062] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application will be further described in detail below with reference to the drawings and specific embodiments.

[0063] Embodiment one

[0064] The present embodiment provides a ewe pregnancy detection method based on multi-sensor fusion, comprising:

[0065] Step 101: Obtain a high-precision point cloud map of the area where the ewe is located.

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

[0067] Step 103: Based on the position of the ewe pregnancy detection robot in the area, the RT algorithm is used to determine the path of the ewe pregnancy detection robot to the ewe.

[0068] Step 104: After reaching the designated location, the ewe pregnancy detection robot performs pregnancy detection on the ewe.

[0069] Before step 101 is executed, it also includes:

[0070] A1: Based on the laser radar module and the depth camera, point cloud data and image data of the area where the ewe is located are obtained respectively.

[0071] A2: The point cloud data is de-distorted to obtain processed point cloud data.

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

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

[0074] A5: 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 plane point cloud map is established based on the set frame.

[0075] 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 laser odometry in real time, and generate a loop factor by selecting a key frame adjacent to the current frame in real time according to the space-time relationship.

[0076] A7: A high-precision time synchronization method based on 5G network is used to synchronize the data of the laser radar module, the IMU data processing sub-module, the GPS odometry sub-module, and the RFID detection sub-module.

[0077] A8: Establish an 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.

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

[0079] A10: Combine GPS information, laser odometry, loop, and RFID detection factors to optimize the global point cloud map to obtain a high-precision point cloud map.

[0080] In some embodiments, when performing steps A1-A6, the following can be specifically implemented:

[0081] The point cloud data is de-distorted, and the attitude (such as roll, pitch, and yaw angle) and acceleration information provided by the IMU are used to accurately compensate and correct the point cloud distortion caused by the motion 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 the laser radar scanning. By combining with the time synchronization information of the laser radar, the IMU data can help correct the spatial coordinates of the laser point cloud and remove the point cloud distortion caused by the motion of the radar.

[0082] Then, according to the acquired image data collected by the depth camera, a DeepLab V3+ image semantic segmentation network model is used to identify and classify objects into known and unknown categories (such as sheep in a sheep field, walking people, feeders, and feeding pens, etc.), after the unknown objects are manually labeled, a small incremental sample learning method is used to expand the object recognition categories, and the model is updated to improve the feature learning effect, according to the recognition result, the dynamic point cloud information is removed, the planar points and corner points are extracted, the first frame of laser point cloud is set as the key frame, and the key frame information is initialized based on the IMU data. According to the position and time between the current frame and the key frame, a suitable key frame is selected, a local corner point and planar point cloud map is established, and downsampling is performed; at the same time, 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, the initial state estimation of the current frame is calculated, the laser odometry is published in real time, and the loop closure factor is generated by selecting the adjacent key frame of the current frame in real time according to the space-time relationship.

[0083] In some embodiments, when performing steps A6-A10, the following can be specifically implemented:

[0084] 1) Time synchronization: A high-precision time synchronization method based on 5G network is adopted to synchronize the data of laser radar, IMU, GNSS, and RFID in time.

[0085] 2) Establish motion equation: The mobile robot adopts a navigation coordinate system (north-east-sky coordinate system), is the ideal transformation matrix of the navigation coordinate system (n system) and the carrier coordinate system (b system), and the motion equation is:

[0086] .

[0087] .

[0088] .

[0089] where p = [x y h]T denotes the longitude, latitude and altitude; vehicle speed denotes the velocity components in the East-North-Up coordinate system; is the attitude quaternion, is the time derivative of the attitude quaternion, i.e., the rate of change of the attitude; , and is the Earth radius parameter, Rc is the position update scaling matrix in the navigation coordinate system (East-North-Up coordinate system), which is used to convert the velocity components into the rate of change of longitude, latitude and altitude; is the vehicle acceleration component 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.

[0090] 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).

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

[0092] is the rotation of the navigation coordinate system (n system) caused by the movement of the inertial navigation system near the Earth's surface due to the curvature of the Earth's surface.

[0093] 3) Establish the observation equation of the unscented Kalman filter: the unscented Kalman filter (UKF) uses unscented transformation (UT transformation) to obtain sigma sampling points in the original state according to the mean and covariance of the prior probability, and approximates the system model in probability density, so it does not need to calculate the complex Jacobian determinant, and the simulation accuracy of the system model reaches three orders of magnitude, greatly improving the efficiency; the data information output by the sensor is filtered using the UKF algorithm, and then all sensor estimates are fused to obtain the globally optimal estimate, and the dynamic observation equation of the nonlinear system of the Kalman filter is given by the following formula:

[0094] .

[0095] .

[0096] 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 vector, is the observation error, subscript k is the kth time, k-1 is the k-1th time, h k () is a nonlinear observation model function, which maps the state vector x k to the observation z k , i.e. describes how to generate the expected sensor measurement from the current state.

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

[0098] Usually, sigma points are selected at the mean and symmetrically distributed at the covariance of the principal axis (two per dimension), and 2m+1sigma points are sampled according to the following method, then 2m+1sigma points can be sampled.

[0099] .

[0100] where x is the point set in the voxel, μ x represents the mean of the current state vector, i.e. the optimal estimate 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-1th column of the lower triangular matrix after Cholesky decomposition, Similarly.

[0101] Then the UKF filtering is performed, and the posterior probability distribution of the state quantity at k-1 time is sampled, and then the state transition nonlinear transformation is performed according to the sampled sigma point set, the approximate mean and variance of the prior probability of the state quantity at k time are calculated, and then the sigma sampling is performed on the prior probability distribution, and the cross-covariance of the state quantity and the observation quantity is calculated by using the sigma point set, and finally the posterior probability distribution of the state quantity at k time is obtained, and the laser odometer is released.

[0102] All key frame point cloud information is fused into a global point cloud map, and global factor graph optimization is performed in combination with GPS information, laser odometry, closed loop and RFID detection factors, so as to optimize the global point cloud map, eliminate cumulative errors and construct a high-precision point cloud map.

[0103] In some embodiments, when step 102 is performed, it specifically includes:

[0104] B1: The collected point cloud data is segmented into a plurality of voxels of the same size.

[0105] B2: For each set of points in each voxel, the mean and covariance matrix of the point cloud data are calculated.

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

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

[0108] B5: An error function between the plane formed by the nearest neighbor points and the point cloud data is established. .

[0109] B6: A least squares problem equation of the point cloud data, the rotation transformation matrix and the translation transformation vector is established.

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

[0111] B8: The ewe pregnancy detection robot is positioned according to the optimal rotation transformation matrix R and translation transformation vector t.

[0112] The formula expression of the Gaussian probability function P(x) is:

[0113] .

[0114] In the formula, x is the set of points x={ , … } in the voxel, 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, , and T is the transpose matrix.

[0115] The formula expression of the error function is:

[0116] .

[0117] wherein R is a rotation transformation matrix, t is a translation transformation vector, d is an intercept, and N is a column vector of unit length normals.

[0118] The expression of the least squares problem equation is:

[0119] .

[0120] wherein, is the point cloud data.

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

[0122] .

[0123] .

[0124] Then, the probability distribution of each point in the small block is represented using a Gaussian probability function P(x), that is, a normal distribution modeling:

[0125] .

[0126] wherein c is a constant.

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

[0128] The optimal transformation parameter p is solved according to the maximum likelihood function φ:

[0129] .

[0130] wherein T(p, ) represents moving .

[0131] After calculating the probability distribution function, the transformation parameter p is solved when the maximum likelihood function reaches the maximum value.

[0132] According to the preliminary solution of the transformation parameter p, the collected three-dimensional point cloud is mapped to the target point cloud, and the point cloud data The five nearest neighbors are fitted into a plane. Assume the parameters of the fitted plane are (N, d) e R4, where N is a column vector of unit length normals and d is the intercept.

[0133] Establish The error function between the plane formed by its nearest neighbors and the plane:

[0134] .

[0135] Where R is the rotation transformation matrix and t is the translation transformation vector.

[0136] Establish the least squares problem with q i , R, t as variables:

[0137] .

[0138] Solving the least squares problem, we get the optimal rotation transformation matrix R and translation transformation vector t. Through the solved R and t, the precise position of the mobile robot in the point cloud map can be calculated.

[0139] In some embodiments, when step 103 is performed, the following can be specifically implemented:

[0140] An improved RT (Rapidly-exploring Random Tree) algorithm is adopted to optimize the growth strategy or sampling method of the tree, speed up the path search, and introduce heuristic methods, optimization strategies and post-processing techniques to generate better paths. This algorithm is suitable for the dynamic environment of the farm and can quickly generate a feasible path from the starting point to the end point, especially for dynamic obstacles that may appear in the farm, such as the movement of staff or poultry.

[0141] Through the elastic bandage device mounted on the robot, the body and abdomen of the ewe are stabilized, and combined with the DeepLabV3+ image semantic segmentation network model, the pregnant area of the ewe is accurately identified. The position information identified will be transmitted to the mechanical arm to guide it to plan a reasonable path. At the same time, the mechanical arm is equipped with a force feedback module, which can monitor the pressure applied to the ewe's abdomen in real time to avoid causing harm to the ewe, thereby achieving safe and accurate pregnancy detection.

[0142] The specific workflow is as follows: the overall workflow: after establishing a high-precision map of the sheep field by laser radar, the target working point is set, the robot navigates to the specified position through path planning, then the ewe is pulled to the ewe feeding station, and the pregnancy detection robot starts to work. First, the mechanical arm is moved to a fixed position, then the position of the four limbs of the ewe is detected by the depth camera, the collected image is transmitted to the industrial computer, then the industrial computer controls the floating slider device to automatically adjust the position of the mechanical claw according to the position of the four limbs of the ewe, then the servo motor is controlled to control the mechanical claw, so as to ensure that the four limbs of the ewe can be fixed, wherein the mechanical claw is covered with rubber material, which can increase the friction with the abdomen of the ewe on the one hand and protect the abdomen of the ewe on the other hand, and the elastic bandage is installed in the middle of the feeding station, which is moved upward along the guide rail by controlling the floating slider to fix the abdomen of the ewe; the position of the pregnancy detection area of the ewe recognized by the depth camera installed on the mechanical arm is transmitted to the industrial computer, the path planning and control of the 5-degree-of-freedom mechanical arm are realized, and the pregnancy detection of the ewe is ensured.

[0143] In some embodiments, when step 104 is performed, the following can be specifically performed:

[0144] After reaching the specified position, when receiving the instruction of the ewe entering the pregnancy detection robot, the position of the four limbs of the ewe is detected by the depth camera.

[0145] Based on the position of the four limbs of the ewe, the mechanical claw is controlled to fix the four limbs of the ewe, and the elastic bandage device is controlled to flexibly fix the abdomen of the ewe.

[0146] Based on the scanning of the mechanical arm carrying the B-ultrasound probe on the ewe, the B-ultrasound image of the inside of the uterus of the ewe is obtained.

[0147] Based on the collected B-ultrasound image, the image is preprocessed by using image preprocessing technology.

[0148] The Unet semantic segmentation algorithm is adopted 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 shape information of the embryo.

[0149] Specifically, the mechanical arm carrying the B-ultrasound probe scans the ewe to obtain a clear and complete ultrasound image of the inside of the uterus of the ewe. Based on the collected B-ultrasound image, the image quality is improved by using image preprocessing technology (such as noise removal and contrast enhancement), and the key features are highlighted. The preprocessed image is transmitted to the Unet semantic segmentation algorithm for training, so as to obtain an ewe pregnancy diagnosis model, which can detect whether the ewe is pregnant and identify the number, size and shape of the embryo. Finally, the diagnosis result is presented to the breeder through a user-friendly interface, and comprehensive analysis is performed combined with historical data to help develop a scientific reproduction management plan.

[0150] Example 2

[0151] like Figure 2 As shown, this embodiment provides a pregnancy detection system for ewes based on multi-sensor fusion, including:

[0152] The map acquisition module is used to acquire a high-precision point cloud map of the area where the ewe is located.

[0153] The positioning module is used to locate the ewe pregnancy detection robot in the high-precision point cloud map based on NDT and ICP.

[0154] The path planning module is used to determine the path from the ewe pregnancy detection robot to the ewe based on the location of the ewe pregnancy detection robot in the area, using the RT algorithm.

[0155] The pregnancy detection module is used to detect pregnancy in ewes based on the ewe pregnancy detection robot after arriving at the designated location.

[0156] The technical features of the above embodiments can be combined in any way. For the sake of brevity, 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, they should be considered to be within the scope of this specification.

[0157] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for detecting pregnancy in ewes based on multi-sensor fusion, characterized in that, The method comprises the following steps: acquiring a high-precision point cloud map of the area where the ewes are located; based on the high-precision point cloud map, positioning the ewe pregnancy detection robot in the high-precision point cloud map based on NDT and ICP; based on the position of the ewe pregnancy detection robot in the area, determining the path of the ewe pregnancy detection robot to the ewe using the RRT algorithm; after reaching the specified position, detecting the pregnancy of the ewe based on the ewe pregnancy detection robot; Before acquiring the high-precision point cloud map of the area where the ewes are located, the method further comprises the following steps: based on the laser radar module and the depth camera, respectively acquiring point cloud data and image data of the area where the ewes are located; performing distortion removal processing on the point cloud data to obtain processed point cloud data; based on the trained semantic segmentation network model, performing object recognition on the image data to obtain the recognition results of each object in the image data; remove the dynamic point cloud information in the recognition result, extract the plane point and the corner point, and set the first frame laser point cloud as the key frame; 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; estimate the initial state of the current frame by calculating the IMU pre-integral between the previous frame laser point cloud and the current frame laser point cloud and the state of the previous frame, and publish the laser odometry in real time, and generate a loop factor by selecting the key frame adjacent to the current frame in real time according to the space-time relationship; using a high-precision time synchronization method based on a 5G network to synchronize the data of the laser radar module, the IMU data processing submodule, the GPS odometry submodule and the RFID detection submodule; establishing an unscented Kalman filter observation equation, filtering the data information output by the sensor using the UKF algorithm, and fusing the estimates of each sensor to determine the global optimal estimate; the unscented Kalman filter observation equation is specifically: x k = f(x k-1 ,k-1) + G k-1 w k-1 ; z k = h k (x k )+ θ k ; where f() is the process model, x k is the state vector, x k = (p n , v n , 0, b a , b g , g), p n is the translation vector, v n is the velocity vector, 0 is the rotation vector, b a is the accelerometer bias, b g is the gyroscope bias, g is the gravity, G k is the system noise matrix, w k is the system noise, z k is the observation vector, 0 k is the observation error, the subscript k is the kth time, k-1 is the k-1th time, h k () is the nonlinear observation model function; fuse the key frame point cloud information into a global point cloud map; combine the GPS information, laser odometry, closed loop and RFID detection factor to optimize the global point cloud map and obtain a high-precision point cloud map; based on the high-precision point cloud map, positioning the ewe pregnancy detection robot in the high-precision point cloud map based on NDT and ICP, specifically comprising: segment the collected point cloud data into a plurality of voxels of the same size; for each point set in each voxel, calculate the mean and covariance matrix of the point cloud data; model the probability distribution of each point as a normal distribution to obtain a Gaussian probability function P(x); the formula expression of the Gaussian probability function P(x) is: wherein x is a point set within a voxel x = {X1, X2…X n}, n is the total number of points within the voxel, c is a constant, is the mean of the q point cloud, C is a covariance matrix, T is a transpose matrix; The collected point cloud data is mapped to a target point cloud using initial transformation parameters, and a set number of nearest neighbor points most adjacent to the collected point cloud data in the target point cloud are found and fitted into a plane; an error function e between the plane formed by the nearest neighbor points is established i ; The formula expression of the error function is: e i = N T (Rq i + t) + d; wherein R is a rotation transformation matrix, t is a translation transformation vector, d is an intercept, and N is a column vector of a unit length normal. establishing a least squares problem equation of point cloud data, rotation transformation matrix and translation transformation vector; the expression of the least squares problem equation is: wherein q i is point cloud data; solving the least squares problem equation to obtain the optimal rotation transformation matrix R and translation transformation vector t; positioning the ewe pregnancy detection robot according to the optimal rotation transformation matrix R and translation transformation vector t; after reaching the specified position, detecting the pregnancy of the ewe based on the ewe pregnancy detection robot, specifically comprising: After arriving at the specified position, the position of the four limbs of the ewe is detected by the depth camera upon receiving an instruction to enter the pregnancy detection robot; Based on the position of the four limbs of the ewe, the mechanical claws are controlled to fix the four limbs of the ewe, and the elastic bandage device is controlled to flexibly fix the abdomen of the ewe; Based on the scanning of the ewe by the mechanical arm carrying the B-ultrasound probe, a B-ultrasound image of the uterus inside the ewe is obtained; Based on the collected B-ultrasound image, the image is preprocessed by using an image preprocessing technology; The Unet semantic segmentation algorithm is used 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 shape information of the embryo.

2. The method for detecting pregnancy of ewe based on multi-sensor fusion according to claim 1, characterized in that, The semantic segmentation network model is a DeepLabV3+ image semantic segmentation network model.

3. A multi-sensor fusion based pregnancy detection system for ewes, characterized by, It comprises: a map acquisition module configured to acquire a high-precision point cloud map of an area where the ewe is located; a positioning module configured to position 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; a path planning module configured to determine a path of the ewe pregnancy detection robot to the ewe by using an RRT algorithm based on the position of the ewe pregnancy detection robot in the area; a pregnancy detection module configured to detect the pregnancy of the ewe by the ewe pregnancy detection robot when arriving at the specified position; Before acquiring the high-precision point cloud map of the area where the ewe is located, it further comprises: point cloud data and image data of the area where the ewe is located are respectively acquired based on the laser radar module and the depth camera; the point cloud data is processed to remove distortion to obtain processed point cloud data; object recognition is performed on the image data based on the trained semantic segmentation network model to obtain recognition results of each object in the image data; dynamic point cloud information in the recognition results is removed, planar points and corner points are extracted, and the first frame of laser point cloud is set as a key frame; based on the position and time between the current frame and the key frame, a set frame is selected, and a local corner point and planar point cloud map is established based on the set frame; the initial state of the current frame is estimated 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, a laser odometry is published in real time, and a loop factor is generated by selecting a key frame adjacent to the current frame in real time according to the space-time relationship; a high-precision time synchronization method based on a 5G network is used to synchronize the data of the laser radar module, the IMU data processing submodule, the GPS odometry submodule and the RFID detection submodule in time; an unscented Kalman filter observation equation is established, and the data information output by the sensor is filtered by using a UKF algorithm to fuse the estimates of each sensor to determine the global optimal estimate; each key frame point cloud information is fused into a global point cloud map; the global point cloud map is optimized by combining GPS information, laser odometry, loop and RFID detection factor to obtain a high-precision point cloud map; based on NDT and ICP, the ewe pregnancy detection robot in the high-precision point cloud map is positioned according to the high-precision point cloud map, specifically comprising: Segmenting the collected point cloud data into a plurality of voxels of the same size; For each set of points within each voxel, calculating the mean and covariance matrix of the point cloud data; Modeling the probability distribution of each point as a normal distribution to obtain a Gaussian probability function P(x); Mapping the collected point cloud data to the target point cloud using the initial transformation parameters, and finding the nearest neighbor points of the collected point cloud data in the target point cloud, fitting the nearest neighbor points into a plane; Establishing an error function between the plane formed by the nearest neighbor points; Establishing a least squares problem equation of the point cloud data, the rotation transformation matrix and the translation transformation vector; Solving the least squares problem equation to obtain the optimal rotation transformation matrix R and translation transformation vector t; Positioning the ewe pregnancy detection robot according to the optimal rotation transformation matrix R and translation transformation vector t; When reaching the specified position, the ewe pregnancy detection robot is used to detect the pregnancy of the ewe, specifically including: After reaching the specified position, when receiving the instruction of the ewe entering the pregnancy detection robot, the position of the four limbs of the ewe is detected by the depth camera; Based on the position of the four limbs of the ewe, the mechanical claws are controlled to fix the four limbs of the ewe, and the elastic bandage device is controlled to flexibly fix the abdomen of the ewe; Based on the mechanical arm carrying the B-ultrasound probe, the ewe is scanned to obtain the B-ultrasound image of the uterus inside the ewe; Based on the collected B-ultrasound image, the image is preprocessed by using the image preprocessing technology; The Unet semantic segmentation algorithm is used 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 shape information of the embryo.

Images