Sheep farm inspection robot positioning and navigation method and system

Abstract

The invention discloses a sheep farm inspection robot positioning and navigation method and system, and the method comprises the following steps: 1, collecting a real-time state diagram of a sheep farm environment, preliminarily filtering dynamic point clouds to construct a global static prior map, and filtering the dynamic point clouds by adopting a frame-level dynamic target filtering strategy; 2, carrying out ESEKF-based local positioning on the sheep yard inspection robot; and 3, carrying out NDT-ICP-based global positioning on the sheep yard inspection robot. According to the invention, through the point cloud filtering and fusion positioning method, the dependence on the environment is reduced, and the adaptability under complex indoor conditions is improved. The problem of low positioning precision in a dynamic environment in the prior art is solved. According to the method, the dynamic point cloud is filtered, so that the influence of dynamic factors such as sheep movement is effectively eliminated, and the accuracy and robustness of positioning are improved.

Description

Technical Field

[0001] This invention belongs to the field of animal husbandry technology, specifically relating to a positioning and navigation method and system for a sheep farm inspection robot. Background Technology

[0002] With the rapid development of modern animal husbandry, the livestock industry is gradually moving towards intelligent and precise operations. my country is the world's largest sheep producer, but the level of mechanization and intelligence in sheep farming still lags significantly. Monitoring the health status of sheep still relies on regular manual inspections. Large-scale sheep pens cover a vast area, with a dozen sheep in each pen, requiring staff to periodically check the sheep's health and estrus status. This method is not only labor-intensive but also inefficient, failing to meet the needs of modern sheep farming and prone to missed inspections. Applying inspection robots to sheep pens to replace manual inspections can significantly reduce labor intensity and facilitate the automation and modernization of sheep farming. Precise autonomous navigation is the key technology for enabling inspection robots to navigate autonomously in the complex environment of sheep pens.

[0003] However, current inspection robots are sensitive to environmental factors and have poor adaptability. Existing navigation technologies, such as BeiDou satellite RTK, RGB-D cameras, and ultra-wideband technology, are easily affected by external environmental factors (such as weather, lighting, and electromagnetic interference), leading to unstable positioning in indoor agricultural environments (such as livestock sheds). For example, BeiDou RTK cannot work indoors with many obstructions; RGB-D cameras experience performance degradation in sheep sheds with changing lighting; and ultra-wideband technology fails in electromagnetic environments with dense metal enclosures.

[0004] Furthermore, existing technologies such as LiDAR and visual sensors are susceptible to interference from dynamic point clouds in dynamic scenes (such as a sheep farm with moving animals), leading to positioning drift or failure. LiDAR may experience "scene degradation" in simple scenes, failing to distinguish between static and dynamic objects. While magnetic strip navigation and magnet / RFID tags are low-cost, their paths are fixed, they are easily damaged, and have high maintenance costs. Additionally, the magnetic strips are difficult to adjust flexibly according to operational needs. Summary of the Invention

[0005] To address the aforementioned problems in existing technologies, this invention provides a method and system for positioning and navigation of a sheep farm inspection robot. This invention employs a viewpoint visibility method to initially filter out dynamic point clouds. Simultaneously, it proposes an enhanced dynamic point detection method that integrates both height and distance features to further filter out interfering dynamic point clouds. Then, it uses ESEKF based on laser odometry and IMU to achieve high-precision local positioning, and employs an enhanced adaptive Monte Carlo algorithm that integrates NDT-ICP to achieve stable global positioning.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: A method for positioning and navigation of a sheep farm inspection robot, comprising the following steps: 1. Collect real-time status images of the sheep farm environment, initially filter out dynamic point clouds to construct a global static prior map, and use a frame-level dynamic target filtering strategy to filter out dynamic point clouds. II. Local localization of the sheep farm inspection robot based on ESEKF; 3. Global localization of the sheep farm inspection robot based on NDT-ICP.

[0007] Preferably, in step one, the coordinate system M of the global map (an initial map containing both dynamic and static point clouds, without any dynamic point cloud filtering operation) and the coordinate system Q of the local sensor are set; let P be the set of query scanned point clouds in the local sensor coordinate system. Q The map point cloud in the global map coordinate system is P M ; P M It is divided into two mutually exclusive subsets: a static map P consisting of static point clouds. SM And a dynamic map P composed of dynamic point clouds. DM ; Point clouds are divided into dynamic and static states based on the following conditions: The above formula represents the static point set P. SM It consists of points TP that are correctly identified as static and points FP that are incorrectly identified as static; the set of dynamic points P DM It consists of points TN that are correctly identified as dynamic and points FN that are incorrectly identified as dynamic; P Q The point cloud coordinates are converted into spherical coordinates expressed in azimuth, polar angle, and distance, and then converted into depth image coordinates I from its own viewpoint. Q ; P M Convert to P Q In the coordinate system, P after coordinate transformation M Also converted to depth map I M ; in P M In the same direction, take the depth of the nearest point as I. M The depth value of the corresponding pixel; Where u is the horizontal coordinate and v is the vertical coordinate; n col s represents the number of rows in the image. el For azimuth, pxmin p is the minimum value of the x-coordinate of the image. xmax n represents the maximum value of the x-coordinate of the image. row s is the number of columns in the image. az For the vertical market angle, p vmin p is the minimum value of the ordinate of the image. ymax This represents the maximum value of the image's ordinate. If the pixel value corresponding to the map point is different Greater than the threshold Then mark it as a dynamic map point: A progressive depth difference strategy is adopted, moving from high resolution to low resolution; firstly, the depth image I of the current frame is compared pixel by pixel at the highest resolution layer. Q With global map depth image I M When the depth deviation exceeds the adaptive threshold, it is marked as a dynamic point, and the initial static point cloud is output. With dynamic point clouds Subsequently, the resolution level was reduced for static point clouds. With the current frame point cloud P Q Perform depth value difference analysis and iteratively filter to add point clouds that meet dynamic characteristics. This process is repeated until the lowest resolution layer is reached, resulting in the final static point cloud set. The marked dynamic points are stored in a temporary set. The retained static points are then used to iteratively update the global map.

[0008] Starting from the last resolution layer after static point cloud filtering, traverse the current frame point cloud P. Q The dynamic map point cloud obtained by filtering The depth image differences are analyzed, and dynamic points with depth deviations less than the adaptive recovery threshold are reclassified as static points, resulting in a filtered static point cloud. and dynamic point clouds At the same time, static point cloud Supplement The resolution was further reduced later, targeting dynamic point clouds. and the current frame point cloud P Q Perform depth difference analysis, a process that is repeated until the lowest resolution layer is reached, to obtain the final static point cloud map.

[0009] Preferably, in the global static prior map, point clouds with heights lower than a set value are filtered out, and two point clouds with different heights in the sector point cloud are extracted. Their average height is calculated as the ground height of the sector point cloud. Point clouds within ±0.5m of the ground height are used as the preliminary point clouds for calculating the ground reference plane. Then, the ground reference plane is estimated, and the plane equation ax + by + cz + d = 0 is fitted. The sector point cloud is divided into ground and non-ground parts according to a preset distance threshold t. Here, a, b, and c are constants, x, y, and z are the coordinates of the points, and d is the intercept of the plane coordinate equation. For each sector point cloud, calculate H. max H min And ΔH, and if the number of point clouds in the sector is less than 5, then ΔH is set to zero; Among them, H max and H min z represents the extreme values ​​of the sector point cloud. max and z min ΔH represents the extreme value of the z-value of the sector point cloud, and ΔH represents the height difference of the sector point cloud. Normalize ΔH; Where, ΔH * The height difference after chemical treatment This represents the statistical mean of the point cloud height difference for a sector. Given the standard deviation of the point cloud height difference in the sector, clip ensures ΔH * The value falls within [0,1]; Let the set of laser points in the current scan frame within the i-th sector divided by polar coordinates be: in, These are the azimuth and polar angles in spherical coordinates, respectively. Considering only the radial component r k The Euclidean distance to the point is obtained as follows: Where Davg is the average value of the radial components, and n is the number of point clouds; Introducing the exponential decay mapping: in, These are the relative dispersion coefficients; Apply formula (15) to the global static map P g ,get Furthermore, define: Where the superscripts c and g correspond to the current frame and the global map, respectively, and R D It is a feature of distance difference; Distance difference feature R D Together with the height difference feature after normalization, they form a linear weighted discriminant: in, The height difference feature of the current frame within this sector. This represents the height difference feature of the global map within this sector. W is a constant used to prevent division by zero. H W D The coefficients are linearly weighted, and W H +W D =1.

[0010] Preferably, in step two, the state variables and equations of motion of the IESKF are: In state prior estimation, it is necessary to predict the nominal state x at time k. k With error state variables The input state variable is the measurement value of the IMU. The predicted noise term is ,Right now: in, Represents change, p represents the measurement value of the IMU. k R represents the position coordinates. k Let v represent the rotation matrix. k b represents the velocity of the three axes. ak Indicates triaxial acceleration bias, b gk Indicates the triaxial angular velocity offset. These are white noises for velocity, angle, acceleration bias, and angular velocity bias, respectively. Based on the IMU error model, the discrete-time recursive equation for the nominal state variables is derived: Where Δt represents the time variation and g represents the Earth's acceleration; Error state in discrete time The recurrence relation is: in, Indicates antisymmetric matrix The vector form; In the prediction process, the nominal state variable x k and error state The error state is updated according to the above formula, and the variance is updated according to the following formula: Among them, Q k Let P be the noise matrix, P be the state covariance of the previous time step, and F be the linear form of the motion error. Where I is the identity matrix and R is the rotation matrix.

[0011] Preferably, feature extraction is performed on the point cloud obtained by scanning each laser line in the lidar; continuous points and edge points with step are distinguished according to the curvature; the point cloud obtained by scanning each laser line is divided into six intervals according to the circumference; points with curvature greater than the threshold are classified as corner points and included in the edge feature set; points with curvature less than the threshold are treated as surface points and included in the planar feature set. The formula for calculating curvature is as follows: Among them, c (n,i) Let X represent the curvature of the i-th point of the m-th laser line, (X) i ,Y i Z i (X) represents the coordinates of a point. j ,Y j Z j () represents the curvature of a nearby point; Incremental NDT is used to construct laser mileage for observation of a mobile robot. First, the in-voxel estimate is updated. Assume there are *m* historical point clouds preceding the voxel, forming a Gaussian distribution. , After adding n new points, the mean and variance of these n points are: and Then the estimated value after merging and for: An iterative process is added during the observation; the ESEKF update process is as follows: Among them, P k It is the predicted covariance matrix, H k It is the Jacobian matrix of the observation equation relative to the error state, V is the covariance matrix of the noise, z is the observed data, and h(x) is the Jacobian matrix of the observation equation relative to the error state. k (This refers to actual measurement data;) According to the SWM identity of the Kalman filter: Where A, B, C, and D are matrix blocks, P k H k Substituting matrices V and others into the following equation, we get: In each iteration of incremental NDT, the residual r associated with NDT is used. j Normal distribution parameters , and r j The Jacobian matrix J relative to rotation and translation j To represent K k The relevant parameters in; Where 03 is a three-dimensional zero vector, J k For the Jacobian matrix P in the first equation pred For the predicted covariance matrix, This is the transpose of the Jacobian matrix.

[0012] After the NDT iteration converges, P is updated. k+1 and J j After the error state is returned to zero, a linear transformation is performed on the corresponding covariance matrix. .

[0013] Preferably, step three is as follows: The acquired 3D point cloud data is divided into multiple voxels of the same size; for each voxel, the point set x = {x1, x2, ..., x...} n}, where n represents the total number of points within a voxel, and the mean q and covariance matrix C of the point cloud are calculated: The point cloud distribution in each voxel is modeled using a multivariate Gaussian probability density function: Where c is a constant; The acquired point cloud data is mapped to the target point cloud space using initial transformation parameters p; the optimal transformation parameters p are then solved by maximizing the likelihood function. In the formula, P is the probability of maximizing the likelihood estimate, and T(p,x) i This indicates that the attitude transformation T is used to move x. i When the likelihood function φ reaches its maximum value, the corresponding transformation parameter p is the preliminary optimal solution. Based on the NDT coarse registration results, for each point q i Search for its five nearest neighbors and fit the local plane parameters (n',d)∈R. 4, where n' is the unit normal vector and d is the plane intercept; Establish the error function from point to plane: Where R is the rotation transformation matrix and t is the translation transformation vector; the optimal pose is solved by least squares optimization: The obtained rotation matrix R and translation vector t represent the robot's precise pose on the map; applying the precise pose output by ICP to the AMCL particle set, the observation likelihood is: Among them, Z t For the observed values, To observe the variance of the noise, T f The precise pose obtained by NDT-ICP This represents the pose transformation corresponding to the particle. At time t, the particle set is represented as: Where n is the number of particles in the set. Let j be the j-th particle at time t. This represents the weight of the particle. The particle weight update formula is: Where β is the normalization factor; Finally, the particles are resampled based on their weights, and after multiple iterations, the particle swarm converges to the true pose of the inspection robot. This process is driven by the motion model. Description, where u t This represents the control input at time t.

[0014] The present invention also discloses a positioning and navigation system for a sheep farm inspection robot, used to perform the above method, comprising the following modules: Point cloud filtering module: Collects real-time status images of the sheep farm environment, initially filters out dynamic point clouds to construct a global static prior map, and uses a frame-level dynamic target filtering strategy to filter out dynamic point clouds; Robot Local Localization Module: Performs local localization of the sheep farm inspection robot based on ESEKF; Robot Global Localization Module: Performs global localization of the sheep farm inspection robot based on NDT-ICP.

[0015] This invention reduces dependence on the environment and improves adaptability in complex indoor conditions by using point cloud filtering and fusion positioning methods. It solves the problem of low positioning accuracy in dynamic environments found in existing technologies.

[0016] This invention effectively eliminates the influence of dynamic factors such as sheep movement by filtering out dynamic point clouds, thereby improving the accuracy and robustness of positioning.

[0017] This invention, based on NDT and ICP global localization, allows robots to autonomously map and plan paths without the need for pre-laying infrastructure, thus achieving path flexibility and reconfigurability.

[0018] This invention integrates local and global positioning methods, utilizes point cloud data to extract stable features in a static environment, and flexibly adjusts the path according to operational requirements. Attached Figure Description

[0019] Figure 1 This is a positioning accuracy test diagram from an embodiment of the present invention.

[0020] Figure 2 This is a distribution diagram of test sampling points in an embodiment of the present invention.

[0021] Figure 3 This is a schematic diagram of accuracy measurement according to an embodiment of the present invention.

[0022] Figure 4 This is a test diagram of robot positioning accuracy according to an embodiment of the present invention.

[0023] Figure 5 This is a diagram showing the test results of navigation accuracy at different speeds according to an embodiment of the present invention.

[0024] Figure 6 These are test diagrams of navigation accuracy under different motion modes in embodiments of the present invention.

[0025] Figure 7 This is the global static prior map construction graph of this invention embodiment.

[0026] Figure 8 This is a local positioning map based on IESKF in an embodiment of the present invention.

[0027] Figure 9 This is a block diagram of a sheep farm inspection robot positioning and navigation system according to an embodiment of the present invention. Detailed Implementation

[0028] The preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0029] This embodiment discloses a method for positioning and navigation of a sheep farm inspection robot, the specific steps of which are as follows: I. Dynamic Point Cloud Filtering The dense flocks of sheep, workers, and mobile devices in sheep farm environments lead to a large number of dynamic interference objects in the laser point cloud. Traditional graph-optimized SLAM algorithms suffer from pose drift due to this dynamic interference, severely impacting navigation accuracy. Furthermore, the presence of dynamic objects introduces challenges during map building, manifesting as numerous "ghosting" artifacts. These artifacts reduce overall mapping accuracy, create a chaotic map structure, and fail to meet the requirements of tasks such as path planning. To construct a robust localization method, this invention proposes a hierarchical processing strategy that integrates global static prior map construction with frame-level dynamic target filtering, balancing environmental structure consistency analysis with real-time requirements.

[0030] First, define a global map coordinate system M and a local sensor coordinate system Q. Let P be the set of query scan point clouds in the local coordinate system. Q The map point cloud in the global coordinate system is P M By comparison (The set of query scan point clouds in local coordinate system, where k represents one of many scans) and P M The corresponding point cloud is used to identify and remove dynamic point clouds in the global map.

[0031] During the comparison process, P M It is divided into two mutually exclusive subsets: a static map P consisting of static point clouds. SM And a dynamic map P composed of dynamic point clouds. DM .

[0032] Because the initial method for judging dynamic points is rather conservative, state prediction errors are inevitable. The point cloud is divided into two states: dynamic and static, based on the following conditions: The above formula represents the static point set P. SM It consists of points correctly identified as static (TP) and points incorrectly identified as static (FP). The dynamic point set P... DM It consists of points that are correctly identified as dynamic (TN) and points that are incorrectly identified as dynamic (FN).

[0033] To reduce the number of elements in FP and FN point cloud sets, P should be strictly limited. SM The generation of P is used to reduce the number of elements in the FP set. This study uses the Hesai XT32 mid-range mechanical lidar, with a vertical field of view of 31° (resolution 1°) and a horizontal field of view of 360° (resolution 0.18°). For ease of processing, P is first... Q The point cloud coordinates are converted into spherical coordinates expressed in azimuth, polar angle, and distance, and then further converted into depth image coordinates from its own viewpoint, denoted as I.Q .

[0034] At the same time, P M Convert to P Q In the coordinate system, P after coordinate transformation M It is also converted into a depth map, denoted as I. M In P M There may be multiple points in the same direction; the depth of the nearest point is taken as I. M The depth value of the corresponding pixel.

[0035] Where u is the horizontal coordinate and v is the vertical coordinate. col s represents the number of rows in the image. el For azimuth, p xmin p is the minimum value of the x-coordinate of the image. xmax n represents the maximum value of the x-coordinate of the image. row s is the number of columns in the image. az For the vertical market angle, p vmin p is the minimum value of the ordinate of the image. ymax This represents the maximum value of the image's ordinate.

[0036] Then, based on the principle of depth residual analysis, and if the pixel value difference corresponding to the map point is... Greater than a certain threshold Then mark it as a dynamic map point. To improve the accuracy of static point cloud acquisition, this embodiment employs a progressive depth differencing strategy from high resolution to low resolution, based on the principle of depth residual analysis. First, the depth image I of the current frame is compared pixel-by-pixel at the highest resolution layer. Q With global map depth image I M When the depth deviation exceeds the adaptive threshold, it is marked as a dynamic point, and the initial static point cloud is output. With dynamic point clouds Then, the resolution level was reduced for static point clouds. With the current frame point cloud P Q Perform depth value difference analysis and iteratively filter to add point clouds that meet dynamic characteristics. This process is repeated until the lowest resolution layer is reached, resulting in the final static point cloud set. This achieves a progressive filtering process, from fine-grained targets to macroscopic motion. The marked dynamic points are stored in a temporary set. Meanwhile, the retained static points iteratively update the global map.

[0037] To correct for potential erroneous filtering of static points, this embodiment further employs a top-down static recovery strategy. Starting from the last resolution layer of the static point cloud filtering, the current frame point cloud P is traversed. Q The dynamic map point cloud obtained by filtering The depth image differences are analyzed, and dynamic points with depth deviations less than the adaptive recovery threshold are reclassified as static points, resulting in a filtered static point cloud. and dynamic point clouds At the same time, static point cloud Supplement Then, the resolution was further reduced to target dynamic point clouds. and the current frame point cloud P Q Perform depth difference analysis, repeating this process cyclically until the lowest resolution layer is reached, to obtain the final static point cloud map. Complete the refined recovery of erroneously filtered points, such as Figure 7 As shown.

[0038] Based on the construction of a global static prior map, a frame-level dynamic target filtering strategy is further adopted. Because in a farm environment, dynamic targets and static facilities overlap in height distribution, relying solely on height features is insufficient for accurate differentiation between dynamic and static point clouds. Therefore, this embodiment proposes an enhanced dynamic point detection method that integrates height and distance features.

[0039] The farm ground has structures such as slopes and drainage ditches, which can cause drastic changes in point cloud height. Therefore, in this embodiment, point clouds with heights below -1m are filtered out, and the 5th and 15th highest point clouds in the sector point cloud are extracted. Their average height is calculated as the ground height of the sector point cloud. Point clouds with ground heights within ±0.5m are used as the preliminary point clouds for calculating the ground reference plane. Then, the RANSAC algorithm is used to estimate the ground reference plane, and the plane equation ax+by+cz+d is fitted. Based on the preset distance threshold t, the sector point cloud is divided into ground and non-ground parts.

[0040] For each sector point cloud, calculate H. max H min And ΔH, and if the number of point clouds in the sector is less than 5, then ΔH is set to zero.

[0041] Among them, H max and H min z represents the extreme values ​​of the sector point cloud. max and z min ΔH represents the extreme value of the z-value of the sector point cloud, and ΔH represents the height difference of the sector point cloud.

[0042] Meanwhile, since the environments of different sheep farms are different, and the installation height and line of sight of the LiDAR on different robots are also different, the ΔH value distribution of the same algorithm may vary greatly in different application scenarios. Therefore, by normalizing ΔH, the height difference of each sector is converted into the degree of deviation relative to the whole, so that it is independent of the application scenario and only reflects the degree of relative difference.

[0043] Where, ΔH * The height difference after chemical treatment This represents the statistical mean of the point cloud height difference for a sector. Given the standard deviation of the point cloud height difference for each sector, clip (a fixed function) guarantees ΔH. * The value falls into [0,1].

[0044] Meanwhile, let the set of laser points in the current scan frame within the i-th sector divided by polar coordinates be: in, The meanings are azimuth and polar angle in spherical coordinates, respectively. Considering only the radial component r k (That is, the Euclidean distance from any point on the LiDAR (Light Detection and Ranging) system to the point on the other side), from which we can obtain: Where Davg is the average value of the radial components, and n is the number of point clouds; To obtain a dimensionless depth consistency measure bounded by (0,1], an exponential decay mapping is introduced: in, The relative dispersion coefficient can suppress distant sectors. and near sector The difference in magnitude.

[0045] To further capture the changes in depth distribution of similar and dissimilar sectors over time, formula (15) is also applied to the global static map P. g ,get Furthermore, we define: Where the superscripts c and g correspond to the current frame and the global map, respectively, and R D It is a distance difference feature.

[0046] In the improved method of this invention, the distance difference feature R D Together with the height difference feature after normalization, they form a linear weighted discriminant: in, The height difference feature of the current frame within this sector. This represents the height difference feature of the global map within this sector. To minimize the amount and prevent division by zero, W H W D The coefficients are linearly weighted, and W H +W D =1.

[0047] II. Local localization based on ESEKF For the state estimation problem of nonlinear systems, the traditional Extended Kalman Filter (EKF) relies on first-order Taylor expansion to achieve local linearization, but it introduces significant nonlinear truncation errors by neglecting higher-order terms. To improve estimation accuracy, this embodiment employs the Error State Extended Kalman Filter (ESEKF), which introduces error state variables, representing the true system state as a superposition of the nominal state and the error state, thereby effectively suppressing the impact of nonlinear truncation errors on filtering performance.

[0048] This embodiment designs a multi-sensor fusion positioning scheme based on ESEKF, such as Figure 8 As shown, this scheme integrates IMU attitude calculation and laser SLAM odometry observation information to achieve high-precision pose estimation. First, the data acquired by the sensors is preprocessed and features are extracted. Then, the prediction step is performed using the IMU error state as the nominal state. In the update phase, the error state is corrected using measurement information from both the IMU and the laser SLAM odometry, and the correction result is fed back to update the IMU error state. Through this iterative processing method, fusion positioning based on error state extended Kalman filtering is achieved, outputting high-precision pose information.

[0049] The pose estimation results obtained by multi-sensor fusion outperform those of single-sensor solutions in terms of both reliability and robustness. The entire ESEKF-based local localization system architecture is hierarchical, enabling it to effectively handle localization problems in complex environments.

[0050] 1) IESKF state variables and equations of motion In state prior estimation, it is necessary to predict the nominal state x at time k. k With error state variables The input state quantity is the measurement value from the IMU (Inertial Measurement Unit). The predicted noise term is ,Right now: in, p represents the measurement value of the IMU. k R represents the position coordinates. k Let v represent the rotation matrix. k b represents the velocity of the three axes. ak Indicates triaxial acceleration bias, b gk Indicates the triaxial angular velocity offset. These are white noises for velocity, angle, acceleration bias, and angular velocity bias, respectively.

[0051] Based on the IMU error model, the discrete-time recursive equation for the nominal state variables can be derived: Where Δt represents the time variation and g represents the Earth's acceleration; Error state in discrete time The recurrence relation is: in, Indicates antisymmetric matrix In vector form.

[0052] In the prediction process, the nominal state variable x k and error state The error state is updated according to the above formula, and the variance is updated according to the following formula: Among them, Q k Let P be the noise matrix, P be the state covariance of the previous time step, and F be the linear form of the motion error. Where I is the identity matrix and R is the rotation matrix; Due to the influence of white noise and bias noise, the IMU error state estimation will drift significantly over time. Therefore, it is necessary to use lidar data to update the ESEKF measurement for correction.

[0053] (2) Observation equations based on incremental NDT and their iterative processing To address the complex and ever-changing external environment within the farm, this embodiment extracts features from the point cloud obtained by scanning each of the 32 laser lines of the lidar. Based on the curvature, continuous points and edge points with step changes are distinguished. The point cloud obtained by each laser line is divided into six intervals along a circumference. Points with curvature greater than a threshold are classified as corner points and included in the edge feature set; points with curvature less than a threshold are treated as surface points and included in the planar feature set.

[0054] The formula for calculating curvature is as follows: Among them, c (n,i) Let X represent the curvature of the i-th point of the m-th laser line, (X) i ,Y i Z i (X) represents the coordinates of that point. j ,Y j Z j ) represents the curvature of its nearest point.

[0055] Then, an incremental NDT method is used to construct a laser odometry system to observe the mobile robot. First, the in-voxel estimation needs to be updated. Let a voxel have m previous historical point clouds, forming a Gaussian distribution. , After adding n new points, the mean and variance of these n points are: and Then the estimated value after merging and for: In addition to updating the Gaussian distribution within the voxels to adapt to changes in the point cloud, a queue model is also implemented, placing the most recently updated voxels at the front of the queue. When the entire queue exceeds the expected capacity, the oldest voxels are deleted to ensure the system's real-time performance and storage efficiency.

[0056] Since the NDT algorithm requires iteration to converge, iterative processing needs to be added during the observation process. The observation model for Iterative EKF is the same as that for Traditional EKF, and the update process for ESEKF is as follows: Among them, P k It is the predicted covariance matrix, H k It is the Jacobian matrix of the observation equation relative to the error state, V is the covariance matrix of the noise, z is the observed data, and h(x) is the Jacobian matrix of the observation equation relative to the error state. k (This refers to actual measurement data;) According to the SWM (Sherman-Morrison-Woodbury) identity of the Kalman filter: Where A, B, C, and D are matrix blocks, P k H k Substituting matrices V and others into this expression, we get: Therefore, in each iteration of incremental NDT, the residual r related to NDT can be utilized. j Normal distribution parameters , and r j The Jacobian matrix J relative to rotation and translation j To represent K k The relevant parameters in.

[0057] Where 03 is a three-dimensional zero vector, J k For the Jacobian matrix P in the first equation pred For the predicted covariance matrix, This is the transpose of the Jacobian matrix; After the NDT iteration converges, P is updated. k+1 and J j Furthermore, since resetting the mean part also affects the covariance of the rotation part, after the error state is zeroed, a linear transformation of the corresponding covariance matrix is ​​required to ensure the consistency and stability of the filter.

[0058] III. Global Localization Based on NDT and ICP Although the accuracy of Extended Kalman Filter (ADF) fusion localization is significantly higher than that of wheeled odometry, its error still accumulates over time. To address the issues of ADF error accumulation and localization ambiguity in traditional AMCL in environments with similar features, this embodiment proposes an improved adaptive Monte Carlo (AMCL) localization method based on NDT (Normal Distribution Transformation)-ICP (Interactive Closest Point) fusion. This method uses the precise registration results of NDT and ICP as the observation model and optimizes the AMCL particle weight update strategy, thereby improving localization accuracy and system robustness. To improve computational efficiency, the acquired 3D point cloud data is first divided into multiple voxels of equal size. For each voxel, the point set x = {x1, x2, ..., x...} is defined as follows: n}, where n represents the total number of points within a voxel, and the mean q and covariance matrix C of the point cloud are calculated: The point cloud distribution in each voxel is modeled using a multivariate Gaussian probability density function: Where c is a constant.

[0059] The acquired point cloud data is mapped to the target point cloud space using initial transformation parameters p. The optimal transformation parameters p are then solved by maximizing the likelihood function. In the formula, P is the probability of maximizing the likelihood estimate, and T(p,x) i This indicates that the attitude transformation T is used to move x. i When the likelihood function φ reaches its maximum value, the corresponding transformation parameter p is the preliminary optimal solution.

[0060] Based on the NDT coarse registration results, for each point q i Search for its five nearest neighbors and fit the local plane parameters (n',d)∈R. 4 Where n' is the unit normal vector and d is the plane intercept. Establish the error function from point to plane: Where R is the rotation transformation matrix and t is the translation transformation vector. The optimal pose is solved using least squares optimization: The obtained rotation matrix R and translation vector t represent the robot's precise pose on the map. Applying the precise pose output by ICP to the AMCL (Adaptive Monte Carlo Localization) particle set, the observation likelihood is: Among them, Z t For the observed values, To observe the variance of the noise, T f The precise pose obtained by NDT-ICP This represents the pose transformation corresponding to the particle.

[0061] At time t, the particle set is represented as: Where n is the number of particles in the set. Let j be the j-th particle at time t. This represents the weight of the particle.

[0062] The particle weight update formula is: Where β is the normalization factor, ensuring that the sum of the weights is 1.

[0063] This process dynamically adjusts particle weights based on sensor observation data, enabling probabilistic sampling of the robot's state space.

[0064] Finally, particles are resampled based on their weights, retaining high-weight particles. Through multiple iterations, the particle swarm converges to the robot's true pose. This process is driven by the motion model. Description (u t(This represents the control input at time t).

[0065] To verify the performance of the sheep farm mobile robot localization and navigation method designed in this invention, localization accuracy tests after dynamic point cloud filtering, navigation accuracy tests under multiple motion modes, and navigation accuracy tests at different motion speeds were conducted. All experiments were carried out at the Future Ranch in Shangmu Village, Qianjin Town, Huzhou City, Zhejiang Province.

[0066] 1. Positioning accuracy test after dynamic point cloud filtering Accurately acquiring the robot's own position information is the primary prerequisite for achieving autonomous navigation. However, the numerous moving obstacles in the sheep farm environment severely impact the accuracy of the positioning system. To evaluate the effect of the dynamic point cloud filtering algorithm on improving the robot's positioning accuracy in this complex environment, this experiment selected five typical locations in different passages within the sheep farm as test target points, labeled as target A, target B, target C, target D, and target E, respectively. The specific locations are shown below. Figure 1 As shown.

[0067] The specific setup is as follows: Targets A and E are located at the bend in the sheepfold passageway; Target B is located at the beginning of the passageway; Target C is located in the middle of the passageway; and Target D is located at the end of the passageway. During the experiment, the robot sequentially reaches targets A, B, C, D, and E, and records the coordinates of each target point. After each set of experiments, the robot returns to its initial position to begin the next round of testing. Each set of experiments is repeated 5 times to ensure the accuracy and reliability of the data.

[0068] When the robot arrives at each target point, it records not only the robot's actual coordinates (x, y) (unit: m), but also the expected position of the mobile platform calculated by the positioning method (x, y). c ,y c (m). Positioning error through the x-direction positioning error d x Positioning error d in the y direction y To evaluate this, the formula for calculating the positioning error Δx in the x-direction is as follows: y-direction deviation d y (m): 2. Navigation accuracy test at different speeds This experiment used the centerline of the sheepfold road as the reference path for target navigation. The mobile robot traveled along this path at three speeds: 0.15 m / s, 0.30 m / s, and 0.50 m / s, successively passing between rows and turning points to complete the autonomous navigation task. At each target point, the robot paused for 20 seconds, during which navigation deviations (including lateral, longitudinal, and directional deviations, i.e., the angle between the positive direction of the mobile platform and the y-axis) at different speeds and positions were collected as an evaluation index of navigation accuracy. Each experiment was repeated 5 times, and all pose data were recorded in real time for subsequent analysis.

[0069] Measurement scheme for navigation accuracy of mobile platform, such as Figure 2 As shown, a two-dimensional Cartesian coordinate system was established with the initial pose as the origin, the forward direction of the mobile platform as the positive y-axis, and the right side of the platform as the positive x-axis. Using the geometric center point of the mobile platform as the measurement reference point, the centerline of the sheepfold road and the target point at the turning point were marked on a two-dimensional grid map using the RVIZ visualization tool. Three different sheepfold passages within the sheep farm were selected for testing to ensure the diversity and representativeness of the test environment. By analyzing the statistical characteristics of various deviation indicators at three speeds, the impact of speed variations on the navigation accuracy of the mobile robot in the complex environment of the sheep farm was evaluated.

[0070] 3. Navigation accuracy tests under different motion modes Based on navigation performance tests at different speeds, further motion mode adaptability experiments were conducted. The robot executed three typical motion modes—forward, backward, and changeover—at a constant speed of 0.3 m / s. The forward motion mode traveled along a straight path in the positive x-axis direction, the backward motion mode moved backward in the negative x-axis direction, and the changeover motion mode executed a rectangular zigzag path containing a complex sequence of forward, turning, and backward motions. The robot paused for 15 seconds at each target point, and each motion mode was repeated three times. The robot's position, attitude, and motion information were recorded in real time. Evaluation metrics included lateral, longitudinal, and heading deviations, as well as motion stability. Figure 3 As shown.

[0071] Results and Analysis: 1. Positioning accuracy test after dynamic point cloud filtering Based on the comparative analysis of the robot's position deviation at different target points before and after dynamic point cloud filtering, as follows: Figure 4 As shown in the figure, data calculations show that after dynamic point cloud filtering, the robot's positional accuracy at all target points was improved to varying degrees, indicating that the dynamic point cloud filtering algorithm is effective.

[0072] The positioning errors of targets A and E at the aisle bends are the smallest. After filtering, the average horizontal and vertical positioning errors of target A are 3.24cm and 3.15cm, respectively, and those of target E are 3.21cm and 3.14cm. Because there are many structured features at the aisle bends, such as fences, guardrails, and utility poles, these provide abundant reference points for robot positioning. The positioning error of target C in the center of the aisle is the largest. Without filtering, its average horizontal and vertical positioning errors are 6.21cm and 5.23cm, respectively, and after filtering, they remain at 4.23cm and 3.25cm. This is because the center of the aisle is entirely unstructured and lacks stable geometric reference points, and dynamic obstacles (such as sheep) cause significant interference. The positioning accuracy of targets B at the start and D at the end of the aisle is between that of the bends and the center positions. These positions have both structured and unstructured features, providing a certain number of positioning features.

[0073] From the perspective of deviation direction, the lateral deviation is greater than the longitudinal deviation. The main reason for this is the geometric characteristics of the sheep farm passageway. While there are fixed boundary constraints in the longitudinal direction, there are no fixed boundary constraints in the lateral direction. Localization is mainly achieved through point cloud feature matching, which is easily interfered with by dynamic obstacles. The richness of environmental fitting features directly affects the final localization effect. Comparative analysis shows that the test points using the dynamic point cloud filtering algorithm show significant improvement, with an average lateral deviation improvement rate of 35.2% and an average longitudinal deviation improvement rate of 28.7%, resulting in an overall localization accuracy improvement of 31.8%. This proves that the algorithm can achieve precise navigation of agricultural robots even in complex livestock environments.

[0074] 2. Navigation accuracy test at different speeds The experimental results of navigation accuracy under different speed conditions are as follows: Figure 5 As shown. By Figure 5 It can be seen that the distribution pattern of heading deviation is basically the same under different driving speeds. At the three speeds of 0.3 m / s, 0.4 m / s, and 0.5 m / s, the mean absolute errors of heading deviation are 1.96°, 2.31°, and 2.35°, respectively, and the standard deviations are 3.12°, 2.86°, and 3.20°, respectively. The AD index is slightly larger at 0.4 m / s and 0.5 m / s compared to 0.3 m / s, but the difference is not significant, indicating that the heading can be stably controlled under different speed conditions.

[0075] The robot's lateral deviation remained stable across different travel speeds. At speeds of 0.3 m / s, 0.4 m / s, and 0.5 m / s, the deviations (AD) were 2.21 cm, 2.35 cm, and 3.45 cm, respectively, and the deviations (SD) were 2.14 cm, 2.41 cm, and 2.85 cm, respectively. It can be seen that the lateral error increases significantly at speeds up to 0.5 m / s, increasing by 1.24 cm at lower speeds. This may be due to a slight sluggishness in system response at higher speeds. Furthermore, the lateral deviation in the longitudinal direction directly affects the overall navigation accuracy. The statistical results at different speeds are as follows: AD were 3.21 cm, 3.25 cm, and 3.11 cm, and SD were 2.26 cm, 2.36 cm, and 2.65 cm, respectively. The deviation fluctuations were small at all speeds, with a maximum difference of only 0.14 cm. These results indicate that the adopted navigation strategy can achieve smooth and accurate trajectory tracking at different travel speeds, possessing high longitudinal control accuracy.

[0076] Comprehensive analysis shows that as the travel speed increases, both the total deviation (AD) and the speed deviation (SD) of the navigation system increase, but the magnitude of the change is small. This result indicates that the proposed navigation method has good adaptability to speed changes and exhibits strong robustness in dynamic environments. In terms of overall performance, the method demonstrates high reliability within the normal operating speed range. Within the operating speed range of 0.3-0.5 m / s, the heading deviation (AD) is less than 2.4°, and the SD is less than 3.2°. The lateral and longitudinal deviations (AD and SD) are both less than 3.5 cm and less than 2.9 cm, respectively, meeting the accuracy requirements for robot operation in a farm.

[0077] 3.3 Navigation accuracy tests under different motion modes Figure 6 The diagram illustrates the distribution of lateral, longitudinal, and directional deviations of the robot under different operating conditions. Forward mode is the most commonly used operating state, and it offers the highest navigation accuracy. In this mode, the mean directional deviation is 3.54° with a standard deviation of 2.13°, the mean lateral deviation is 2.15cm with a standard deviation of 2.36cm, and the mean longitudinal deviation is 2.16cm with a standard deviation of 2.98cm. All these indicators are at relatively low levels, demonstrating the robot's operational stability.

[0078] The navigation accuracy in backward mode is lower than that in forward mode, but still within an acceptable range. Its average offset is 4.17°, an increase of 0.63° compared to forward mode; the average lateral deviation is 3.64cm, an increase of 1.49cm; and the average longitudinal deviation is 1.92cm, a slight decrease. The changeover mode is the most complex movement mode, with navigation accuracy significantly lower than other modes. The average heading angle deviation reaches 5.22° (standard deviation 2.45°), the average lateral deviation reaches 4.21cm (standard deviation 2.31cm), and the average longitudinal deviation reaches 2.53cm (standard deviation 3.12cm). Because turning operations have strict requirements for heading control, this results in a large heading angle deviation.

[0079] Although the three motion modes differ in navigation accuracy, all deviation indicators are within the allowable error range for precise agricultural robot operations. The forward mode exhibits the highest accuracy, with an average heading deviation of less than 3.6° and lateral and longitudinal deviations controlled within 2.2cm, making it suitable for high-precision tasks. While the backward and changeover modes show a decreasing trend in accuracy, they can still stably perform routine operations such as cross-row transfers and path switching in a typical sheep farm environment, indicating the feasibility of this system design.

[0080] like Figure 7 As shown, this embodiment discloses a sheep farm inspection robot positioning and navigation system for performing the above method, which includes the following modules: Point cloud filtering module: Collect real-time status images of the sheep farm environment, initially filter out dynamic point clouds based on viewpoint visibility, construct a global static prior map, and use a frame-level dynamic target filtering strategy to filter out dynamic point clouds; Robot Local Localization Module: Performs local localization of the sheep farm inspection robot based on ESEKF; Robot Global Localization Module: Performs global localization of the sheep farm inspection robot based on NDT-ICP.

[0081] Other aspects of this embodiment can be found in the above method embodiments.

[0082] In summary, this invention overcomes the influence of the complex dynamic environment of sheep farms, achieves highly accurate map construction, positioning, and navigation, ensures the autonomous operation capability of mobile robots in sheep farm environments, and lays the foundation for the application of autonomous mobile platforms in complex agricultural environments.

[0083] The preferred embodiments and principles of the present invention have been described in detail above. For those skilled in the art, there may be changes in the specific implementation based on the ideas provided by the present invention, and these changes should also be considered within the scope of protection of the present invention.

Claims

1. A method for positioning and navigating a sheep station inspection robot, characterised by According to the following steps: I. Collect the real-time state diagram of the sheep farm environment, preliminarily filter out the dynamic point cloud, and construct a global static prior map. A frame-level dynamic target filtering strategy is adopted to filter out the dynamic point cloud; II. Local positioning of the sheep farm inspection robot based on ESEKF; III. Global positioning of the sheep farm inspection robot based on NDT-ICP.

2. The sheep farm inspection robot positioning and navigation method according to claim 1, characterized in that: In step one, a global map coordinate system M and a local sensor coordinate system Q are set; a query scan point cloud set in the local sensor coordinate system is P Q A map point cloud in the global map coordinate system is P M ; P M is divided into two mutually exclusive subsets: a static map P SM consisting of static point clouds, and a dynamic map P DM consisting of dynamic point clouds; The point cloud is divided into dynamic and static states according to the following conditions: The above equation represents a static point set P SM consisting of points TPcorrectly judged as static and points FPincorrectly judged as static; dynamic point set P DM consisting of points TNcorrectly judged as dynamic and points FNincorrectly judged as dynamic Converting the point cloud coordinates of P Q to spherical coordinates expressed in azimuth, polar angle and distance, and then to depth image coordinates I Q under its own perspective; Convert P M to the coordinate system of P Q , and then convert the coordinate-transformed P M to a depth map I M ; in the same direction in P M , take the depth of the nearest point as the depth value of the corresponding pixel in I M ; where u is the horizontal coordinate and v is the vertical coordinate; n col is the number of rows of the image, s el is the azimuth angle, p xmin is the minimum value of the horizontal coordinate of the image, p xmax is the maximum value of the horizontal coordinate of the image, n row is the number of columns of the image, s az is the vertical market angle, p vmin is the minimum value of the vertical coordinate of the image, p ymax is the maximum value of the vertical coordinate of the image; If the pixel value difference of the map point is greater than a threshold , then mark it as a dynamic map point: Adopting a progressive depth difference strategy from high to low resolution; first compare the current frame depth image I Q with the global map depth image I M , mark as dynamic points when the depth deviation exceeds the adaptive threshold, output the initial static point cloud with the dynamic point cloud ; Subsequently, the resolution level is reduced, and the static point cloud set is obtained by filtering the dynamic points from the current frame point cloud P and the previous frame point cloud P Q The depth value difference analysis is performed, and the point cloud meeting the dynamic characteristics is supplemented by iterative filtering This process is cycled until the lowest resolution level, and the final static point cloud set is obtained The labeled dynamic points are stored in a temporary set The remaining static points are iteratively updated in the global map.

3. The positioning and navigation method for a sheep farm inspection robot as described in claim 2, characterized in that: Starting from the last resolution layer of static point cloud filtering, traverse the current frame point cloud P Q and the filtered dynamic map point cloud The depth image difference is calculated, and the dynamic points with a depth deviation less than the adaptive recovery threshold are reclassified as static points to obtain the filtered static point cloud and the dynamic point cloud At the same time, the static point cloud is supplemented into , and the resolution is further reduced. The depth difference analysis is performed on the dynamic point cloud and the current frame point cloud P Q This process is repeated until the lowest resolution layer to obtain the final static point cloud map.

4. The positioning and navigation method for a sheep farm inspection robot as described in claim 3, characterized in that: in In the global static prior map, filter out the point cloud with a height lower than the set value, and extract two point clouds with different heights in the sector point cloud. Calculate the average height as the ground height of the sector point cloud. The point cloud within ±0.5m of the ground height is the preliminary point cloud for calculating the ground reference surface. Then estimate the ground reference surface to fit the plane equation ax+by+cz+d=0. According to the pre-set distance threshold t, the sector point cloud is divided into ground and non-ground two parts; wherein a, b, c are constants, x, y, z are point coordinates, and d is the intercept of the plane coordinate equation; For each sector point cloud, calculate H max , H min and ΔH, and if the number of sector point clouds is less than 5, set ΔH to zero; wherein H max and H min are the extreme values of the sector point cloud, z max and z min are the extreme values of the z values of the sector point cloud, and ΔH is the height difference of the sector point cloud. Normalize ΔH to; where ΔH * is the height difference after the treatment, is the statistical mean of the sector point cloud height difference, is the standard deviation of the sector point cloud height difference, clip ensures the value of ΔH * falls into [0, 1]; Let the laser point set of the current scanning frame in the i-th sector divided by the polar coordinate be: wherein respectively the azimuth angle, the polar angle of the spherical coordinates; Consider only the radial component r k The Euclidean distance to the point, gives: Wherein, Davg is the average value of the radial component, and n is the number of point clouds. Introduce the exponential decay mapping: wherein Rdis the relative dispersion coefficient; Applying equation (15) to the global static map P g , we get ; and further define: wherein the superscripts c, g correspond to the current frame and the global map, respectively, R D is the distance difference feature; Distance difference feature R D The height difference feature after normalization processing and the distance difference feature jointly constitute a linear weighted discriminant: wherein, is a height difference characteristic of the current frame in the sector, is a height difference characteristic of the global map in the sector, is a constant to prevent division by zero, W H , W D is a linear weighting factor, and W H + W D = 1.

5. The method of Claim 4, wherein: In step II, the state variable and motion equation of IESKF are: In the state prior estimation, the nominal state x k and the error state variable The input state quantity is the measurement value of the IMU The pre-estimation noise term is That is: wherein, represent changes, denote the measurements of the IMU, p k denote position coordinates, R k denote rotation matrices, v k denote three-axis velocities, b ak denote three-axis acceleration biases, b gk denote three-axis angular velocity biases, are white noises of the velocities, angles, acceleration biases and angular velocity biases, respectively; According to the IMU error model, the discrete time recursive equation of the nominal state variable is derived: Where Δt is the time change, and g is the earth's acceleration. Error state in discrete time The recursive equation for the error state is wherein denotes the vector form of the skew-symmetric matrix denotes the vector form of the skew-symmetric matrix In the prediction process, the nominal state variable x k and the error state are updated according to the above equations, and the variance of the error state is updated according to the following equation: where Q k is the noise matrix, P is the state covariance at the previous time, and F is given by the linear form of the motion error: Where I is the unit matrix, and R is the rotation matrix.

6. The method of Claim 5, wherein: Feature extraction is performed on the point cloud obtained by scanning each laser line of the laser radar. According to the curvature size, continuous points and edge points with steps are distinguished. The point cloud obtained by scanning each laser line is divided into six intervals according to one week. Points with curvature greater than the threshold are classified as corner points and included in the edge feature set. Points with curvature less than the threshold are classified as face points and included in the plane feature set. The curvature calculation formula is as follows: wherein c (n,i) represents the curvature of the i-th point of the m-th laser line, (X i ,Y i ,Z i ) represents the coordinates of the point, (X j ,Y j ,Z j ) represents the curvature of the adjacent point; The laser odometer is constructed by incremental NDT method, and the observation of mobile robot is completed; firstly, the estimation in voxel is updated, assuming that there are m historical point clouds before voxel, the Gaussian distribution is , , after adding n new points, the mean and variance of the n points are and , and the combined estimation and are: Increase the iteration process in the observation process; the update process of ESEKF is: where P k is the predicted covariance matrix, H k is the Jacobian matrix of the observation equation with respect to the error state, V is the covariance matrix of the noise, z is the observation data, and h(x k ) is the actual measurement data. According to the SWM identity of the Kalman filter: Wherein, A, B, C, D are all matrix blocks, P k , H k , V matrix into the following formula, we get: In each iteration of the incremental NDT, the NDT-related residual r j is expressed in terms of the normal distribution parameters and r j with respect to the rotation and translation Jacobian matrix J j for the related parameters in K k ; where 03 is a three-dimensional 0 vector, J k is the Jacobian matrix in the first equation, P pred is the predicted covariance matrix, is the transpose of the Jacobian matrix; After the NDT iteration converges, then update P k+1 and J j After the error state is zeroed, a linear transformation is performed on the corresponding covariance matrix; 。 7. The method of Claim 6, wherein: Step three is specifically as follows: the collected three-dimensional point cloud data is divided into a plurality of same size voxels; for the point set x = {x1, x2, …, xN} in each voxel, the mean value q and the covariance matrix C of the point cloud are calculated: n wherein n represents the total number of points in the voxel. The point cloud distribution in each voxel is modeled by a multivariate Gaussian probability density function: Where c is a constant. Map the collected point cloud data to the target point cloud space using the initial transformation parameter p. Solve the optimal transformation parameter p by maximizing the likelihood function: where P is the probability of maximizing the likelihood estimate, T(p, x i ) denotes moving x i using the pose transformation T; when the likelihood function φ takes the maximum value, the corresponding transformation parameter p is the preliminary optimal solution; Based on the NDT coarse registration results, for each point q i Search for its five nearest neighbors, fit a local plane parameter (n', d) e R 4 where n' is the unit normal vector and d is the plane intercept; Establish the point-to-plane error function: Where R is the rotation transformation matrix, and t is the translation transformation vector. The optimal pose is solved by least squares optimization: The rotation matrix R and translation vector t obtained represent the accurate pose of the robot in the map; the accurate pose output by ICP is applied to the AMCL particle set, and the observation likelihood is: where Z t is the observation, is the variance of the observation noise, T f is the accurate pose obtained by NDT-ICP, is the pose transformation corresponding to the particle. At time t, the particle set is represented as: where n is the number of particle sets, is the jth particle at the current time t, is the weight of the particle; The particle weight update formula is: Where β is the normalization factor. Finally, the particles are resampled according to the weights, and the particle group converges to the real pose of the inspection robot through multiple iterations. The process is shown in the motion model is described, where u t represents the control input at time t.

8. A sheep station inspection robot positioning and navigation system for carrying out the method of any one of claims 1 to 7, characterised in that, It includes the following modules: Point cloud filtering module: collect the real-time state diagram of the sheep farm environment, preliminarily filter out the dynamic point cloud, and construct a global static prior map. A frame-level dynamic target filtering strategy is adopted to filter out the dynamic point cloud; Robot local positioning module: local positioning of the sheep farm inspection robot based on ESEKF; Robot global positioning module: global positioning of the sheep farm inspection robot based on NDT-ICP.

Images