Livestock farm inspection robot positioning and mapping method based on laser and vision assistance

Abstract

The invention belongs to the technical field of farm automatic inspection and positioning mapping, and discloses a farm inspection robot positioning mapping method based on laser and vision assistance. The method comprises the following steps: firstly, converting depth image data shot with similar depths into point cloud coordinates, and then converting the point cloud coordinates into laser data through an inverse projection method; then acquiring information of an unstructured region of the farm from an image shot by the depth camera by using an ORB-SLAM technology, and converting the information into a sparse three-dimensional point cloud map; converting the three-dimensional point cloud map into a grid map through point cloud mapping for subsequent map fusion; besides, a single-line laser radar is used for scanning the environment to obtain information of a structured part in the farm environment, and a Gmapping technology is used for establishing a laser radar map; and finally, fusing the laser radar map and the grid map subjected to visual conversion by adopting a Bayesian fusion technology to obtain a global grid map. According to the invention, an accurate and comprehensive map can be established in an indoor poultry breeding environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of automatic inspection and positioning mapping in farms, and particularly relates to a method for positioning and mapping a farm inspection robot based on laser and vision assistance. Background Art

[0002] According to statistics, at present, the inspection methods in farms are still mainly manual, which not only consumes a large amount of human resources, but also has low inspection efficiency and limited coverage, making it difficult to detect abnormal situations (such as temperature and humidity fluctuations, dead or diseased poultry, etc.) in the breeding environment in a timely manner. Therefore, how to achieve intelligent and automatic inspection in farms has become a key issue in improving the management level and economic benefits of the breeding industry. In recent years, the rapid development of inspection robots has provided a new solution for automatic monitoring in farms. By carrying various environmental sensors and vision detection devices, the inspection robot can collect data such as temperature, humidity, and gas concentration in the farm in real time, and use computer vision technology to identify dead or diseased poultry, greatly improving the inspection efficiency and accuracy. However, the farm environment is complex and changeable, with a large number of dynamic obstacles (such as moving flocks, feeding equipment, etc.), making the autonomous mapping and navigation of the robot face severe challenges.

[0003] Although traditional rail-type or track-following inspection robots can complete part of the inspection tasks on fixed paths, they lack flexibility and are difficult to adapt to unstructured environments, resulting in low mapping accuracy and poor navigation stability, affecting the overall inspection effect. To address this problem, the Simultaneous Localization and Mapping (SLAM) technology combined with multi-sensor information fusion can effectively improve the perception and decision-making ability of the robot in complex environments. The SLAM technology enables the robot to build a map in real time and determine its own position in an unknown environment, while multi-sensor fusion (such as lidar, vision camera, inertial measurement unit, etc.) can make up for the limitations of a single sensor and improve the robustness of environmental modeling. For example, lidar can provide high-precision spatial structure information, while the vision sensor can supplement the semantic features of objects, so as to more accurately identify structured (such as walls, fences) and unstructured (such as flocks, scattered feed) environmental elements and optimize the navigation path planning of the robot.

[0004] At present, SLAM technology has been widely applied in fields such as service robots, autonomous driving, and drones. The mapping and positioning capabilities of SLAM technology in dynamic and complex environments have been widely verified. However, in the indoor poultry farming scenario, due to factors such as strong environmental dynamics (e.g., frequent movement of poultry flocks) and variable lighting conditions (e.g., light interference, low-light areas), traditional SLAM algorithms still have problems such as positioning drift and map distortion. Therefore, how to study an optimized SLAM method based on multi-sensor fusion and make adaptive improvements in combination with the particularity of the farming environment is of great significance for achieving high-precision and high-stability automated inspection in farms, and will further promote the development of intelligent farming and agricultural intelligence. Summary of the Invention

[0005] In view of the above problems existing in the prior art, the present invention proposes a method for positioning and mapping of a farm inspection robot based on laser and vision assistance. By fusing the visual ORB-SLAM algorithm and the laser GMapping algorithm, and combining the depth image conversion laser data technology and the Bayesian grid map fusion method, high-precision modeling of unstructured areas is achieved, which is conducive to solving problems such as map distortion and cumulative error in complex farming environments.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] A method for positioning and mapping of a farm inspection robot based on laser and vision assistance includes the following steps:

[0008] Step 1. Obtain the depth images captured by the depth camera, convert the pixel points of each depth image into point cloud coordinates in three-dimensional space, and use the inverse projection method to convert each point cloud coordinate into laser data;

[0009] Step 2. Use the ORB-SLAM technology to obtain the information of the unstructured areas in the farm from the images captured by the depth camera, and convert the unstructured areas into a sparse three-dimensional point cloud map;

[0010] Step 3. Convert the sparse three-dimensional point cloud map into a grid map through point cloud mapping;

[0011] Step 4. Use a single-line lidar to scan the farm environment to obtain the information of the structured areas in the farm environment, and use the GMapping technology to establish a lidar map.

[0012] Step 5. Adopt the Bayesian fusion technology to fuse the lidar map in Step 4 with the grid map converted by vision in Step 3 to obtain a global grid map, thereby obtaining a complete mapping result.

[0013] The present invention has the following advantages:

[0014] As described above, the present invention relates to a method for positioning and mapping a patrol robot in a farm based on laser and vision assistance. The method for positioning and mapping the patrol robot in the farm is based on the fusion of multi-sensors (depth camera and 2D lidar sensor), and through the heterogeneous integration of the visual ORB-SLAM algorithm and the laser GMapping algorithm, combined with the depth image reverse projection conversion technology, to achieve sparse three-dimensional modeling of unstructured areas. The method of the present invention uses a Bayesian probability framework to fuse multi-source grid maps, and suppresses environmental dynamic interference and cumulative errors through a dynamic weight allocation and threshold decision mechanism, significantly improving the topological consistency and positioning accuracy of the global map. The method of the present invention can still maintain a robust mapping ability under complex lighting and high-dynamic scenarios, support fully automated patrol tasks, and effectively enhance the reliability of environmental anomaly detection and path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 is a flowchart of the method for positioning and mapping a patrol robot in a farm based on laser and vision assistance in an embodiment of the present invention;

[0016] Figure 2 is a diagram showing the structured and unstructured display of the indoor farm environment in an embodiment of the present invention;

[0017] Figure 3 is the fusion process of the two mapping methods of laser and vision in an embodiment of the present invention;

[0018] Figure 4 is a schematic diagram of the Gazebo simulation model of the farm in an embodiment of the present invention; wherein, Figure 4 in (a) is a top view of the farm model, Figure 4 in (b) is a map of the farm obstacles, Figure 4 in (c) is a map of the farm corner.

[0019] Figure 5 is the mapping effect of different algorithms in the simulation environment in an embodiment of the present invention; wherein, Figure 5 in (a) is the original floor plan, Figure 5 in (b) is the effect diagram of the GMapping mapping algorithm, Figure 5 in (c) is the effect diagram of the Karto mapping algorithm, Figure 5 in (d) is the effect diagram of the Hector mapping algorithm, Figure 5 in (e) is the effect diagram of the mapping algorithm of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0020] The present invention will be further described in detail below with reference to the drawings and specific embodiments:

[0021] Embodiment 1

[0022] In view of the current strict requirements for temperature and humidity monitoring in the indoor poultry breeding environment, as well as problems such as poor breeding environment, complex structure, and diverse obstacles, the present invention proposes a method for positioning and mapping of a patrol robot in a breeding farm based on laser and vision assistance. This method uses a vision camera to capture obstacle information at different heights, converts the depth image into laser data, and rasterizes the point cloud map generated by ORB-SLAM to achieve deep fusion of vision and laser information. Specifically, the processing flow of the method of the present invention is as follows: First, convert the depth image data taken at similar depths into point cloud coordinates, and then convert them into laser data through the inverse projection method. Then, use the ORB-SLAM technology to obtain information on the unstructured area of the breeding farm from the images captured by the depth camera and convert it into a sparse three-dimensional point cloud map. Immediately afterwards, convert the three-dimensional point cloud map into a grid map through point cloud mapping for subsequent map fusion. In addition, the present invention also uses a single-line lidar to scan the environment to obtain information on the structured part of the breeding farm environment and uses the Gmapping technology to establish a lidar map. Finally, adopt the Bayesian fusion technology to fuse the lidar map with the grid map converted by vision to obtain a global grid map.

[0023] As Figure 1 shown, the method for positioning and mapping of a patrol robot in a breeding farm based on laser and vision assistance includes the following steps:

[0024] Step 1. Obtain the depth images captured by the depth camera, convert the pixel points of each depth image into point cloud coordinates in three-dimensional space, and use the inverse projection method to convert each point cloud coordinate into laser data.

[0025] First, use an RGB-D depth camera to synchronously obtain the RGB image (color image) and depth image data in the target scene. By combining the two-dimensional coordinates and depth value measurements of the pixel points of each depth image point with the camera internal parameters, convert them into point cloud coordinates in three-dimensional space to generate high-density point cloud data containing geometric features and obtain a complete point cloud.

[0026] Among them, each point contains its coordinates in three-dimensional space; delete possible other information

[0027] Then, adopt the inverse projection method, combine the pose calibration parameters of the depth camera and the lidar, and map the three-dimensional point cloud data to the lidar coordinate system to generate radial distance information (not only including distance information, which can be understood as constructing a three-dimensional space map, each object has its own coordinates, and the distance can be calculated) adapted to the lidar data structure.

[0028] Of course, in this embodiment, it is not limited to using the above RGB-D depth sensing device, which will not be elaborated here.

[0029] Among them, the specific process of the back projection method is as follows:

[0030] I. Viewpoint calculation.

[0031] To transform the pixel point p of the depth image d (x,y) is transformed to world point p w (x,y,z), determine the midpoint p in the world coordinate system w The observation angle θ corresponding to (x, y, z) is calculated as follows:

[0032]

[0033] II. Laser Indexing and Distance Mapping.

[0034] Assuming that the maximum detection angle of the laser radar is α°, within the effective detection angle range of the laser radar (0-α°), when the system is configured with N laser beams, the corresponding laser beam number n can be calculated by the angle θ, and the depth image point p can be derived at the same time. d , calculate the corresponding laser index subscript, and find the distance from the point to the origin of the coordinate system.

[0035] Let point p on angle θ w Mapped to laser data, assuming that the maximum detection angle of the laser radar is α°, there are N laser beams in the detection angle range of (0-α°), then point p on angle θ w The corresponding laser beam is laser[n].

[0036] Where n is the index subscript of the laser, and the calculation formula of n is as follows:

[0037]

[0038] The pixel point p of the depth image d The distance d from the origin of the coordinate system is calculated as follows:

[0039]

[0040] III. Comprehensive mapping model.

[0041] The expression of the laser beam laser can be obtained by combining the parameters. The expression of the laser beam is shown as follows:

[0042]

[0043] Among them, laser represents the converted laser beam, x,z represents the pixel point p on the depth image d Transform to point p in the world coordinate system wThe lengths of the X-axis and Z-axis at that time, and α represents the maximum detection angle of the lidar.

[0044] Step 2. Use the ORB-SLAM technology to obtain the information of the unstructured area in the farm from the images captured by the depth camera, and convert the unstructured area into a sparse three-dimensional point cloud map.

[0045] In this embodiment, the unstructured area refers to the area where objects with irregular geometric shapes are located, such as the wires in the cage, and the structured area refers to the area where objects with regular geometric shapes are located, such as columns, walls, etc.

[0046] For the internal structure mutation areas in the farm (such as unstructured environments like corridor corners, railing joints, and peripheries of irregular obstacles), the vision-assisted G2 laser SLAM algorithm captures local geometric features (such as the texture and edge information of the railing) through the camera, and runs the visual ORB-SLAM to generate a sparse three-dimensional point cloud map to supplement the observation blind area of the lidar in such areas.

[0047] In this embodiment, the environmental area is divided into structured areas and unstructured areas according to characteristics. Among them, the structured areas include the farm walls, straight corridors, fixed fences, etc., as Figure 2 (a) shows that its regular geometric features are suitable for the laser SLAM to construct a high-precision grid map; the unstructured areas include corner joints, feed stacking areas, animal activity areas, etc., as Figure 2 (b) shows that it is necessary to fuse the texture features of the visual ORB-SLAM to make up for the sparsity of the laser point cloud.

[0048] For the mapping requirements of the unstructured environmental area, the present invention adopts a vision-enhanced G2 laser SLAM algorithm architecture. Through an integrated vision perception unit, it captures the geometric features of the scene corner area in real time, and realizes environmental modeling based on the ORB feature-driven simultaneous localization and mapping technology. The visual ORB-SLAM adopted by the present invention has three core modules of tracking, map construction, and loop detection working together, and can realize high-precision sparse mapping of unstructured areas such as corners.

[0049] First, the motion trajectory is estimated through dynamic feature point extraction and matching to ensure local positioning accuracy; then, effective feature points are screened based on the key frame selection strategy to generate an optimized sparse three-dimensional point cloud map; finally, the historical scene similarity is identified through the closed-loop correction module, and the pose correction is realized by calculating the Sim3 matrix to eliminate the cumulative error and complete the global Figure 1 consistency optimization.

[0050] Step 3. Convert the sparse three-dimensional point cloud map into a (two-dimensional) grid map through point cloud mapping, providing a preprocessing basis for multi-source map fusion. The process of converting the sparse three-dimensional point cloud map into a grid map through point cloud mapping is as follows:

[0051] Step 3.1. Three-dimensional data parsing and dimensionality reduction projection.

[0052] Perform rasterization conversion processing on the three-dimensional environmental characterization data generated based on visual SLAM technology. The process is as follows:

[0053] Parse the key frame three-dimensional pose data generated based on visual SLAM and the spatial coordinates of its associated mapping points, and use the plane projection conversion technology to project the three-dimensional space data onto the two-dimensional grid coordinate system with dimensionality reduction.

[0054] Step 3.2. Build a grid occupancy status determination model.

[0055] For the camera observation pose of each key frame, analyze the visibility of the mapping points using the ray casting principle, and establish a geometric model of the observation path in the grid space through the Bresenham algorithm.

[0056] Then update the grid state parameters along the observation path. The cumulative access counter P of the grid cells covered by the path (i,j) , and the occupancy counter Q of the grid cell corresponding to the end point of the path is updated (i,j) , and the grid occupancy status determination model P ocp is as shown in the following formula:

[0057]

[0058] Step 3.3. Incremental map construction and fusion.

[0059] Based on the set of counter parameters {P (i,j) , Q (i,j)} of the grid cells, calculate the grid state determination value according to the preset grid occupancy probability threshold function to realize the incremental construction and update of the two-dimensional grid map.

[0060] Through the probability fusion mechanism of multi-frame observation data, the present invention gradually updates the grid state, generates a two-dimensional rasterized environmental characterization with topological consistency and adapted to the requirements of multi-source map fusion, and ensures the global consistency and compatibility of the environmental characterization.

[0061] By outputting the 3D poses of all key frames in the depth image and the coordinates of all point clouds in each key frame to a text file; using the GripMapping technology to project all key frames, camera poses, and related mapped points onto the XOZ plane by removing the y coordinate, and then storing the key frames, camera poses, and related mapped points in a dictionary structure; then processing the key frames, the coordinates obtained by observation in a key frame are recorded as occupied at the corresponding positions in the grid map after projection, the positions along the line are recorded as free, and the remaining grids are recorded as positions.

[0062] Step 4. Use a single-line lidar to scan the farm environment to obtain information about the structured areas in the farm environment, and use the GMapping technology to build a lidar map.

[0063] As Figure 2 shown in (a) schematically shows the structured part of the indoor farm environment. This step 4 is specifically as follows:

[0064] Step 4.1. Structured area data acquisition and simulation platform construction.

[0065] The present invention uses a single-line lidar to continuously scan the structured features (such as chicken cage rows, aisles), so as to obtain high-resolution geometric profile data, and builds a three-dimensional physical simulation verification platform including structured aisles based on the scanned data.

[0066] The simulation verification platform built in this embodiment is, for example, a Gazebo model, as Figure 4 shown, for algorithm performance testing.

[0067] As Figure 2 shown in (a) of, this embodiment uses a single-line lidar to scan the white feeding box near the ground of the chicken cage row, and builds a Gazebo simulation environment for indoor poultry farming including only the structured aisle part.

[0068] Of course, the structured features in this embodiment are not limited to chicken cage rows, aisles, etc. in the poultry farming scenario, and will not be elaborated here.

[0069] Step 4.2. Pose and map information solution. The present invention realizes the joint optimization of the robot pose and the map based on a probability model, and constructs a high-precision grid map mainly based on lidar data.

[0070] Define the joint probability distribution of the current pose x 1:t of the robot and the map m as shown in the following formula:

[0071] p(x 1:t , m|z 1:t , u 1:t-1 ) = p(m|x 1:t , z1:t-1 )p(x 1:t |z 1:t ,u 1:t-1 ).

[0072] Where p(x 1:t ,m|z 1:t ,u 1:t-1 ) represents the control input sequence u 1:t-1 and the observation sequence z 1:t Solve the current position x of the robot under constraints 1:t and the probability of map m; p(x 1:t |z 1:t ,u 1:t-1 ) represents the control input sequence u 1:t-1 and the observation sequence z 1:t Solve the robot's current map x under constraints 1:t The probability of p(m|x 1:t ,z 1:t-1 ) means that in the observation sequence z 1:t and current position x 1:t The probability of solving the map m is known. This step controls the input sequence u 1:t-1 and the observation sequence z 1:t , estimate the robot's current position x 1:t The probability distribution of and the occupancy probability distribution of map m.

[0073] Step 5. Use Bayesian fusion technology to fuse the lidar map of step 4 with the raster map after visual conversion in step 3 to generate a high-precision global raster map, thereby obtaining a complete mapping result.

[0074] The present invention can obtain two grid maps through the laser GMapping mapping method and the visual ORB-SLAM mapping method. In order to obtain more accurate environmental information, it is necessary to fuse the map information obtained by these two different algorithms and use the Bayesian method to perform map fusion to obtain a complete mapping result.

[0075] Figure 3 It is a fusion process of laser and vision mapping methods. Figure 3 The fusion process involves the following steps:

[0076] Step 5.1. Construction and status definition of multi-source heterogeneous datasets.

[0077] A multi-source heterogeneous dataset is established, which includes structured area grid maps generated by laser SLAM and unstructured area grid maps generated by visual SLAM. The states of grid cells in the grid maps are defined, including vacant, occupied and unknown.

[0078] Step 5.2. Grid Map Fusion.

[0079] The essence of Bayesian inference is to solve the conditional probability of an event, which can be simply understood as using the prior probability and the likelihood function to solve the posterior probability problem. Based on the Bayesian conditional probability formula:

[0080]

[0081] Among them, P(A|B) represents the posterior probability of event A given that event B has occurred, that is, the posterior probability estimate after fusion; P(B|A) represents the relative likelihood that event B causes event A to occur, that is, the likelihood function of sensor observation data; P(A) represents the prior probability of event A when there is no information about event B, that is, the prior probability distribution of the grid cell state; finally, P(B) represents the marginal probability that event B occurs, that is, the probability that event B occurs when there is no information about event A.

[0082] The Bayesian rule is used to update the grid state value of the fused grid map, mainly calculating the grid state estimate value P r , and its calculation is shown in the following formula:

[0083]

[0084] Among them, P s represents the conditional probability that there is an obstacle in the grid cell at a distance r from the lidar, and P m represents the prior probability that the grid cell at a distance r from the sensor is occupied.

[0085] Step 5.3. Multi-Sensor Conflict Handling.

[0086] For multi-sensor data conflict scenarios including but not limited to vision and lidar, a dynamic weight allocation mechanism is established, and the grid cell state update rule is defined as shown in the following formula;

[0087]

[0088] Among them, P f represents the grid cell state, P l represents the occupancy probability of the grid cell observed in the laser method, and P c represents the occupancy probability of the grid cell observed in the vision method.

[0089] For example, two SLAM methods, lidar and vision, may generate different grid cell states for the same grid cell. When the grid maps obtained by the lidar GMapping method and the vision ORB-SLAM method are both in an unknown state for the same grid cell, the prior probability is used to calculate the probability that the grid is occupied by substituting it into the Bayesian formula.

[0090] Set a dynamic threshold judgment strategy. Preset the occupancy probability T ocp , and in this embodiment, T ocp can be set to 0.5.

[0091] When the posterior probability P of the grid cell f ≥ T ocp , it is forced to be set to the occupied state, and the occupancy probability value of the grid cell is taken as 1; otherwise, retain the observation confidence P of the laser SLAM l , as the occupancy probability value of the grid cell in the fusion result.

[0092] The following takes the positioning and mapping example of an indoor environment inspection robot in a chicken coop as an example to illustrate the positioning and mapping method of the inspection robot in a farm assisted by laser and vision in this embodiment. The specific steps of this method include:

[0093] Step 1. Synchronously obtain the color image and depth image data of the target scene through the RGB-D sensor Astar Pro, and perform three-dimensional coordinate conversion based on the depth image pixel coordinates, measurement values, and camera internal parameters (H58.4°*V45.7° (depth) H66.1°*V40.2° (RGB)) to generate high-density point cloud data containing geometric features.

[0094] Further, in combination with the pose calibration parameters of the depth camera and the lidar, map the point cloud data to the lidar coordinate system through the inverse projection algorithm to generate radial distance information adapted to the lidar data structure.

[0095] Step 2. Extract the image ORB feature points through the ORB-SLAM algorithm to construct a sparse three-dimensional point cloud map of unstructured areas (such as corner railings). Identify the similarity of historical scenes through the loop closure correction module, and use the Sim3 matrix calculation to achieve pose correction, eliminate the cumulative error, and complete the global ground Figure 1 consistency optimization.

[0096] Step 3. Perform three-dimensional data parsing and dimensionality reduction projection. Project the three-dimensional point cloud generated by ORB-SLAM onto the XOZ plane at a grid resolution of 10 cm / gird. After generating a two-dimensional grid map, construct a grid occupancy state judgment model.

[0097] The present invention uses the Bresenham algorithm to establish an observation path, and accumulates the access counter P of the grid cells covered by the path (i,j) , the termination point occupancy counter Q (i,j) , and calculate the occupancy probability P ocp .

[0098] Step 4. Collect data from the structured area. Use a single-line lidar (SICK LMS111) to scan the chicken cages (with a spacing of 1.1 - 1.3 m), obtain high-resolution geometric contour data, and build a 1:20 scaled-down chicken house model in the Gazebo simulation platform, including 6 rows of chicken cages and a structured aisle.

[0099] Estimate the probability distribution of the current pose x of the robot and the occupancy probability distribution of the map m based on the RBPF framework. 1:t

[0100] Step 5. Integrate the structured area grid map generated by laser SLAM and the unstructured area grid map generated by visual SLAM to establish a multi-source heterogeneous data set. Define the grid cell state model as shown in Table 1.

[0101] Table 1 Fusion rules for visual and laser grid maps

[0102]

[0103] Calculate the estimated value P of the grid state at the distance sensor r. For the sensor conflict scenario, calculate the dynamic fusion occupancy probability P. r f The final mapping effect of the model is shown in Table 2.

[0104] Table 2 Mapping effect evaluation of different algorithms under simulation

[0105] Evaluation Index GMapping Karto Hector VAGL SSIM 0.6036 0.5751 0.5654 0.6198 Number of Feature Matching Pairs 38 37 51 43

[0106] Figure 5 Schematically shows the mapping effects of different algorithms in the simulation environment.

[0107] Among them, the Karto algorithm, Hector algorithm, and GMapping algorithm are introduced as comparative algorithms of the present invention. In addition, the present invention uses SSIM and the logarithm of feature matching as evaluation indicators. SSIM is the structural similarity index. The larger SSIM is, the higher the geometric structure consistency between the mapping result and the real environment is, and the better the map accuracy is. The logarithm of feature matching is the number of successfully matched feature point pairs in the mapping result. The larger the logarithm of feature matching is, the stronger the feature tracking ability of the algorithm is.

[0108] It can be seen from the test results that the present invention has excellent mapping effects. It can obtain information about non-same-layer obstacles and unstructured part obstacles in the simulated indoor poultry environment, and finally generate a more comprehensive and accurate initial environment map, solving the defect that the commonly used laser algorithms at present can only complete the mapping of the structured part in the indoor poultry environment.

[0109] In view of the strict requirements for temperature and humidity monitoring in the indoor poultry breeding environment, as well as the problems such as poor breeding environment, complex structure and diverse obstacles, the method of the present invention proposes a positioning and mapping algorithm that fuses visual and laser data. The visual camera is used to capture obstacle information at different heights, and then the depth image is converted into laser data, and the point cloud map generated by ORB-SLAM is rasterized to achieve the deep fusion of visual and laser information. The test results in simulation and real datasets respectively show that the method of the present invention can establish an accurate and comprehensive map in the indoor poultry breeding environment.

[0110] Of course, the above description is only a preferred embodiment of the present invention. The present invention is not limited to listing the above embodiments. It should be noted that all equivalent substitutions and obvious deformation forms made by any person skilled in the art under the teaching of this specification fall within the substantial scope of this specification and should be protected by the present invention.

Claims

1. A method for positioning and mapping a farm inspection robot based on laser and vision assistance, characterized in that: The steps include: Step 1. Obtain the depth image taken by the depth camera, and convert the pixel points of each depth image into point cloud coordinates in three-dimensional space, and use the inverse projection method to convert each point cloud coordinate into laser data; Step 2. Use ORB-SLAM technology to obtain information about the unstructured area of ​​the farm from the images taken by the depth camera, and convert the unstructured area into a sparse three-dimensional point cloud map; Step 3. Convert the sparse 3D point cloud map into a raster map through point cloud mapping; Step 4. Use a single-line laser radar to scan the farm environment, obtain structured area information in the farm environment, and use GMapping technology to build a laser radar map; Step 5. Use Bayesian fusion technology to fuse the lidar map of step 4 with the raster map after visual conversion in step 3 to obtain a global raster map, thereby obtaining a complete mapping result.

2. The method for positioning and mapping of the inspection robot in the breeding farm based on laser and vision assistance according to claim 1, wherein The step 1 is specifically as follows: First, an RGB-D depth camera is used to obtain RGB images and depth images. By combining the two-dimensional coordinates and depth value of each pixel point in the depth image with the camera intrinsic parameters, it is converted into point cloud coordinates in three-dimensional space to generate a complete point cloud. Then, the inverse projection method is used to combine the posture calibration parameters of the depth camera and lidar to map the 3D point cloud data to the lidar coordinate system and generate radial distance information that is adapted to the lidar data structure.

3. The method for positioning and mapping of the inspection robot in the breeding farm based on laser and vision assistance according to claim 2, wherein, In step 1, the specific process of the back projection method is as follows: I. To convert the pixel point p d (x, y) of the depth image to the world point p w (x, y, z), to determine the observation angle θ corresponding to the point p w (x, y, z) in the world coordinate system, and the calculation formula of the angle θ is as follows: II. Assume the maximum detection angle of the lidar is α°. Within the effective detection angle range (0 - α°) of the lidar, when the system is configured with N laser beams, the laser beam sequence number n can be calculated through the angle θ, and at the same time, the radial distance of the depth image point p is deduced, the corresponding laser index subscript is calculated, and the distance of this point from the coordinate origin is obtained. d The radial distance is calculated, the corresponding laser index subscript is calculated, and the distance of this point from the coordinate origin is obtained. Where n is the index subscript of the laser, and the calculation formula of n is as follows: Pixel point p of the depth image d The calculation formula for the distance d from the coordinate origin is as follows: III. The expression of the laser beam laser can be obtained by combining the parameters. The expression of the laser beam is shown as follows: Among them, laser represents the converted laser beam, and x, z represent the pixel point p on the depth image d The point p converted to the world coordinate system w The lengths of the X-axis and Z-axis when, and α represents the detection angle range of the lidar 4. The method for positioning and mapping of the inspection robot in the breeding farm based on laser and vision assistance according to claim 1, wherein The step 3 is specifically as follows: Step 3.

1. 3D data analysis and dimensionality reduction projection; Analyze the key frame 3D pose data generated by visual SLAM and the spatial coordinates of its associated mapping points, and use plane projection transformation technology to reduce the dimensionality of the 3D spatial data and map it to a 2D grid coordinate system; Step 3.

2. Construct a grid occupancy state determination model; For each key frame’s camera observation pose, the visibility of the mapping points is analyzed using the ray casting principle, and the observation path geometry model is established in the grid space using the Bresen ham algorithm. Then update the grid state parameters along the observation path, and accumulate the access counter P for the grid cells covered by the path (i,j) , and update the occupancy counter Q for the grid cell corresponding to the end point of the path (i,j) , the grid occupancy determination model P ocp is as follows: Step 3.

3. Incremental map construction and fusion; Grid cell-based counter parameter set {P (i,j) , Q (i,j)}, calculate the grid state determination value according to the preset grid occupancy probability threshold function, and realize the incremental construction and update of the two-dimensional grid map.

5. The method for positioning and mapping of the inspection robot in the farm based on laser and vision assistance according to claim 1, characterized in that The step 4 is specifically as follows: Step 4.

1. Structured regional data collection and simulation platform construction; The structured features are continuously scanned by a single-line laser radar to obtain high-resolution geometric profile data, and a three-dimensional physical simulation verification platform containing the structured features is constructed; Step 4.

2. Solve the pose and map information; The robot's posture and map are jointly optimized based on the probability model, and a raster map based on lidar data is constructed.

6. The method for positioning and mapping of the inspection robot in the farm based on laser and vision assistance according to claim 5, characterized in that Define the current pose x of the robot 1:t and the probability model of the map m is as follows: p(x 1:t ,m|z 1:t ,u 1:t-1 ) = p(m|x 1:t ,z 1:t-1 )p(x 1:t |z 1:t ,u 1:t-1 ); Among them, p(x 1:t ,m|z 1:t ,u 1:t-1 ) indicates that in the control data u 1:t-1 and observation data z 1:t Solve the robot's current position x when it is known 1:t and the probability of map m; p(x 1:t |z 1:t ,u 1:t-1 ) is the control data u 1:t-1 and observation data z 1:t Given the following, find the current position x of the robot 1:t The probability of p(m|x 1:t ,z 1:t-1 ) indicates that in the observed data z 1:t and the current pose x 1:t The probability of solving map m is known.

7. The method for positioning and mapping of the inspection robot in the breeding farm based on laser and vision assistance according to claim 1, wherein, The step 5 is specifically as follows: Step 5.

1. Establish a multi-source heterogeneous data set including a structured area grid map generated by laser SLAM and an unstructured area grid map generated by visual SLAM. The grid cells of the grid map have three states: vacant, occupied, and unknown. Step 5.

2. Grid map fusion; Use Bayes' rule to update the grid state value of the fused grid map and calculate the state estimate value P of the grid at a distance r from the sensor r , the state estimate value P r is calculated as follows: where P s represents the conditional probability that there is an obstacle in the grid cell at a distance r from the lidar, and P m represents the prior probability of occupancy of the grid cell at a distance r from the sensor; Establish a dynamic weight allocation mechanism and define the update rules for the grid cell status as follows: Among them, the posterior probability P f represents the grid cell state, P l represents the occupancy probability of the grid cell observed in the laser method, and P c represents the occupancy probability of the grid cell observed in the vision method.

8. The method for positioning and mapping of the inspection robot in the farm based on laser and vision assistance according to claim 7, characterized in that, In step 5, set a dynamic threshold judgment strategy; When the posterior probability P of the grid cell f ≥T ocp , it is forced to be set to the occupied state, and the probability value of the occupied grid cell is taken as 1; Otherwise, retain the observation confidence P of the laser SLAM l , which is used as the occupancy probability value of the grid cell in the fusion result.

Images