A CAD pre-mapping assisted positioning and mapping method for large-scale farms

Abstract

This invention belongs to the field of localization and mapping technology, and discloses a CAD pre-map-assisted localization and mapping method for large-scale farms. First, the structure of the farm's two-dimensional CAD layout is analyzed and uniformly calibrated. Then, an occupancy grid map and a continuous gradient geometric constraint cost field are constructed and their coordinates are correlated and fused to form a priori topological and geometric map. During robot operation, multi-source sensor data is collected, and front-end matching is performed by combining the prior topological and geometric map to obtain the current pose estimation result. The degree and direction of scene geometric degradation are determined based on the front-end matching residual. A factor map containing multi-source sensor constraints and CAD geometric constraints is constructed, and the weights of CAD geometric constraint information are dynamically adjusted to obtain an optimized global pose sequence. Coordinate transformation and fusion updates are performed on LiDAR point cloud data at different times to obtain a global map that maintains global consistency with the farm's fixed structure and the robot's real-time pose estimation result.

Description

Technical Field

[0001] This invention belongs to the field of positioning and mapping technology, and particularly relates to a CAD pre-map-assisted positioning and mapping method for large-scale farms. It is especially suitable for global positioning and environmental map construction of mobile devices such as chicken inspection robots and transport robots in complex and structured scenarios of large-scale farms. Background Technology

[0002] With the increasing automation of large-scale farms, mobile devices such as inspection robots and autonomous transport robots are gradually being applied to chicken house status inspections, material transportation, and daily operations. Existing mobile robots mostly employ a simultaneous localization and mapping method that integrates LiDAR, inertial measurement units, and wheel speed odometers. This involves collecting environmental data to complete front-end point cloud matching and using back-end mapping to optimize and estimate robot pose and construct an environmental map. Some systems also generate static grid maps based on 2D CAD drawings of the farm buildings for initial path planning or obstacle labeling.

[0003] Large-scale farms typically feature numerous storage facilities, long and narrow passageways, parallel cage arrangements, and repetitive distribution of fixed infrastructure. When using autonomous exploration for mapping, the robot needs to perform a complete scan of each facility, resulting in a long initialization cycle and repetitive mapping costs due to multiple deployments. Existing CAD drawings are mostly stored in vector format, and their scale, orientation, coordinate system, layer semantics, and facility connectivity are difficult to directly use for robot localization and mapping. Simply converting CAD drawings into occupancy grid maps only provides discrete occupancy and vacancy information, making it difficult to simultaneously describe the continuous distances of fixed structural boundaries, gradient directions, regional semantics, and topological connections between multiple facilities.

[0004] In the long passageways and parallel cage areas of the chicken coop, the geometric constraints of laser point clouds in some pose directions are weak. Dust occlusion, local dynamic interference, and repetitive structures can further reduce the reliability of point cloud matching. Traditional front-end matching methods struggle to determine the degree of geometric degradation and its corresponding direction in a timely manner, easily leading to cumulative drift in the passageway extension direction or heading direction. Although existing multi-sensor fusion methods can utilize inertial measurement units and wheel speed-odometers for short-term motion prediction, relying solely on relative motion constraints during long-distance, cross-farm operation still results in the accumulation of global pose errors.

[0005] Existing backend optimization methods often employ fixed-weight fusion of constraints from laser odometry, inertial measurement units, and wheel speed odometry. This makes it difficult to dynamically adjust the intensity of geometric constraints established based on the fixed CAD structure in the corresponding pose direction according to the degree and direction of geometric degradation in the scene. When environmental geometric constraints are insufficient, fixed weights are insufficient to effectively suppress pose drift; when environmental features are sufficient, overly strong prior constraints increase the system's sensitivity to errors in the CAD drawings. Therefore, during continuous operation across multiple environments, robots may still encounter problems such as point cloud map misalignment, local overlap, discontinuous channel connections, and inconsistencies between real-time pose and the fixed structure of the environment.

[0006] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art. Summary of the Invention

[0007] The purpose of this invention is to propose a CAD pre-built map-assisted positioning and mapping method for large-scale farms. This method standardizes and analyzes the 2D CAD layout of the farm, unifies the organization of grid occupancy information, continuous geometric constraint information, and farm building topology connections, and combines multi-sensor front-end matching, geometric degradation assessment, and degradation direction adaptive factor map optimization to achieve global pose correction and point cloud mapping. Figure 1 This consistent update helps to solve the problems in the existing mobile robot localization and mapping process in farms, such as the difficulty in directly converting two-dimensional CAD layout maps into structured prior maps that can be called by the robot, the difficulty in expressing continuous geometric constraints and topological connections of multiple farm buildings simultaneously by ordinary grid maps, and the problem that laser point cloud matching is prone to directional degradation and continuous accumulation of global pose error in long passage, parallel cage and repetitive structure scenarios.

[0008] To achieve the above objectives, the present invention adopts the following technical solution: A CAD pre-built map-assisted positioning and mapping method for large-scale farms includes the following steps: Step 1. Import the 2D CAD layout of the farm and perform analysis and unified calibration, converting it to a unified farm coordinate system to obtain standardized CAD structural data, farm topology connection relationships, and coordinate mapping relationships; Step 2. Based on standardized CAD structural data, perform discrete rasterization and encoding of occupancy status and semantic attributes. After obstacle raster region expansion, raster boundary optimization and accessibility correction, obtain the occupancy raster map. Step 3. Calculate the Euclidean symbolic distance from each spatial location to the nearest obstacle boundary based on standardized CAD structural data, generate the Euclidean symbolic distance field, calculate the spatial gradient vector, and construct the continuous gradient geometric constraint cost field. Step 4. Based on the coordinate mapping relationship, the topological connection relationship of the site and the occupied grid map and the continuous gradient geometric constraint cost field are correlated and fused to obtain the prior topological and geometric map; Step 5. Collect data from multiple sensor sources and predict the robot's initial pose, then extract the corresponding local geometric regions; The current frame of LiDAR point cloud is matched with the local geometric region to obtain the robot's current pose estimation result and front-end matching residual. The degree and direction of scene geometric degradation are obtained based on the front-end matching residual. Step 6. Construct a factor graph containing multi-source sensor constraints and CAD geometric constraints based on the robot's current pose estimation results. Adjust the information weights of the CAD geometric constraints according to the degree and direction of scene geometric degradation and perform joint optimization. Step 7. Based on the global pose sequence obtained by joint optimization in Step 6, transform the LiDAR point cloud data at different times to a unified field coordinate system and fuse and update it, outputting a global map and the robot's real-time pose estimation results.

[0009] The present invention has the following advantages: As described above, this invention proposes a CAD pre-built map-assisted localization and mapping method for large-scale farms. This method analyzes and uniformly calibrates the fixed structures, passageways, connection relationships, and coordinate information of farm buildings in a two-dimensional CAD layout drawing. This converts the CAD drawings used for engineering design into standardized CAD structural data that can be used for robot localization and mapping. Furthermore, it constructs an occupancy grid map, a continuous gradient geometric constraint cost field, and a farm building topology index, forming a priori topology and geometric map that simultaneously includes occupancy status, region semantics, fixed structure geometric constraints, and farm building topology connection relationships. This reduces repetitive exploration and mapping by the robot in multi-farm building environments. During robot operation, the initial pose is predicted using multi-source sensor data, and front-end matching is performed using the prior topology and geometric map. An approximate Hessian matrix is ​​constructed based on the front-end matching residual to determine the degree and direction of scene geometric degradation, thereby characterizing insufficient directional constraints in long passageways, parallel cages, and repetitive structure scenes. Based on this, the information weights of CAD geometric constraints are adjusted according to the degree and direction of scene geometric degradation, and jointly optimized with laser odometry constraints, inertial measurement unit pre-integration constraints, and wheel speed odometry constraints to obtain a global pose sequence. Through the synergistic effect of the above-mentioned CAD structure prior construction, front-end matching and degradation detection, degradation adaptive factor map optimization, and point cloud fusion update, the CAD fixed structure prior can be used to supplement the positioning constraints in the geometric degradation direction, reducing the cumulative pose error of the robot during long-distance and cross-site operation. Based on the optimized global pose, the laser radar point cloud at each moment is continuously corrected and fused, thereby reducing point cloud misalignment, local overlap, and discontinuity in channel connection, improving the global consistency between the robot's real-time pose and the fixed structure of the site, as well as the structural consistency of the final global map. Attached Figure Description

[0010] Figure 1 This is a flowchart of a CAD pre-built map-assisted positioning and mapping method for large-scale farms, as described in an embodiment of the present invention. Figure 2 This is a flowchart illustrating the CAD parsing standardization and grid map construction process in an embodiment of the present invention. Figure 3 This is a flowchart illustrating the construction process of the Euclidean symbolic distance field and the continuous gradient geometric constraint cost field of this invention. Figure 4 This is a flowchart illustrating the prior topology and geometric map generation process in an embodiment of the present invention. Figure 5 This is a flowchart of multi-sensor front-end matching and scene geometry degradation detection in an embodiment of the present invention; Figure 6 This is a flowchart of the degeneracy adaptive factor graph optimization process in an embodiment of the present invention; Figure 7This is a flowchart of global map fusion and update and real-time robot pose estimation in an embodiment of the present invention. Detailed Implementation

[0011] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments: Example 1 To address the problems of long autonomous mapping cycles, insufficient local geometric constraints, and easy degradation in localization in scenarios such as long passageways, regularly arranged cages, and repetitive fixed structures in large-scale farms, this invention proposes a CAD pre-map-assisted localization and mapping method for large-scale farms, such as... Figure 1 As shown. The overall inventive concept of this method is as follows: First, import the two-dimensional CAD layout of the farm, analyze and uniformly calibrate the fixed structure, channels and connections of the farm buildings, and obtain standardized CAD structural data, farm building topology connections and coordinate mapping relationships; then construct an occupation grid map and a continuous gradient geometric constraint cost field, and perform coordinate association and fusion to form a priori topology and geometric map. During robot operation, LiDAR point cloud data, inertial measurement unit data and wheel speed odometer data are collected, and front-end matching is completed in combination with the priori topology and geometric map to obtain the robot's current pose estimation result, and the scene geometric degradation degree and degradation direction are determined according to the front-end matching residual. On this basis, a factor map containing multi-source sensor constraints and CAD geometric constraints is further constructed, and the information weight of CAD geometric constraints is dynamically adjusted according to the scene geometric degradation degree and degradation direction to obtain an optimized global pose sequence; finally, the global pose sequence is used to perform coordinate transformation and fusion update of the LiDAR point cloud data at different times to obtain a global map that is globally consistent with the fixed structure of the farm and the robot's real-time pose estimation result.

[0012] The CAD pre-built map-assisted positioning and mapping method for large-scale farms in this embodiment includes the following steps: Step 1. Import the 2D CAD layout of the farm. By parsing and uniformly calibrating the 2D CAD layout, the 2D CAD layout is converted to a preset unified farm coordinate system to obtain standardized CAD structural data, farm topology connection relationship and coordinate mapping relationship.

[0013] Analysis refers to identifying walls, cages, fixed facilities, chicken coop passages, farm boundaries, and farm connection areas in a two-dimensional CAD layout drawing, and extracting the corresponding geometric attributes and spatial location information.

[0014] Unified calibration refers to converting CAD structural elements and site topology connections to a preset unified site coordinate system based on the correspondence between CAD drawings and actual site buildings in terms of scale, orientation, and position.

[0015] like Figure 2 As shown, step 1 specifically involves: Step 1.1. CAD structural element analysis.

[0016] Read the layer information, geometric entity types, and geometric attributes from the 2D CAD layout drawing of the breeding farm.

[0017] CAD structural elements are identified based on CAD layer identifiers, geometric entity types, and geometric attributes. These CAD structural elements include walls, cages, fixed facilities, chicken coop passages, farm boundaries, and farm connection areas.

[0018] Among them, walls, cages and fixed facilities are used to represent the fixed structures in the farm; chicken coop passages and farm connection areas are used to represent the space that robots can pass through and the connection space between different farms; farm boundaries are used to define the spatial range of each farm and serve as the basis for determining the spatial range of the raster map in the subsequent discrete rasterization process.

[0019] Extract the structural category, boundary coordinates, dimensions, orientation, and spatial location information of each CAD structural element, and associate this information as the attribute information of the corresponding CAD structural element to form a set of CAD structural elements.

[0020] The CAD structural element set is used to uniformly describe the geometric attributes and spatial distribution of each CAD structural element in the 2D CAD layout of the farm, providing basic data for subsequent determination of spatial adjacency and connectivity, construction of farm building topology connections, coordinate transformation, and construction of the occupied grid map.

[0021] Step 1.2. Constructing the topological connection relationship between the site and the building.

[0022] Based on the set of CAD structural elements, the spatial adjacency and connectivity relationships between each CAD structural element are determined according to its structural category and spatial location information. Based on this, the passable areas, obstacle areas, and connection areas of the site are determined.

[0023] Among them, spatial adjacency is used to characterize the adjacency relationship of different CAD structural elements in spatial location, and is determined based on the boundary coordinates and spatial location information of each CAD structural element; connectivity is used to characterize the spatial connection relationship between adjacent passable areas, and is determined based on the spatial adjacency between chicken coop passages, farm entrances and exits, and farm connecting areas.

[0024] The intersection of chicken house passages is determined based on the spatial adjacency between them, and the entrances and exits of the farm and the connection points between different farms are determined based on the spatial correspondence between the farm boundaries and the chicken house passages and farm connection areas.

[0025] The intersection of chicken coop passages, the entrance and exit of the farm, and the corresponding connection points of the farm connection area are determined as topological nodes. The continuous passable passages connecting adjacent topological nodes are determined as topological edges to obtain the farm topological connection relationship.

[0026] Topological nodes are used to characterize key locations such as the intersection of chicken coop passageways, entrances and exits of farmhouses, and connections between different farmhouses; topological edges are used to characterize continuous, passable passageways between adjacent topological nodes. Farmhouse connection regions are used to describe the local connection spaces between different farmhouses, and play a different role in the topological connection relationship between farmhouses than continuous, passable passageways that serve as topological edges.

[0027] The aforementioned topological nodes and topological edges are used together to represent the internal passages of the chicken coop and the spatial connection relationships between different farmhouses, thereby forming farmhouse topological connection relationships. In step 4, these relationships are associated with the occupied grid map to form a farmhouse topological index.

[0028] Step 1.3. Standardize the calibration of CAD drawings.

[0029] The scale transformation parameters are determined based on the proportional relationship between the dimensions marked in the 2D CAD layout drawing and the corresponding dimensions of the actual site, so as to establish the scale correspondence between the dimensions of the CAD drawing and the dimensions of the actual site space.

[0030] The rotation transformation parameters are determined based on the correspondence between the coordinate axes of the CAD drawing coordinate system and the preset unified site coordinate system, and the translation transformation parameters are determined based on the positional relationship between the corresponding reference positions in the two coordinate systems.

[0031] Among them, the scale transformation parameter is used to characterize the scaling ratio between the CAD drawing size and the actual site space size.

[0032] Rotation transformation parameters are used to characterize the directional change relationship between the CAD drawing coordinate system and the unified site coordinate system, while translation transformation parameters are used to characterize the positional offset relationship between the two coordinate systems.

[0033] Based on scale transformation parameters, rotation transformation parameters, and translation transformation parameters, coordinate transformation is performed on the CAD structural element set and the site topology connection relationship. Each CAD structural element, topology node, and topology edge in the CAD drawing coordinate system is transformed to the unified site coordinate system, obtaining standardized CAD structural data and site topology connection relationship expressed in the unified site coordinate system, and establishing the coordinate mapping relationship between the CAD drawing coordinate system and the unified site coordinate system.

[0034] Among them, the coordinate mapping relationship is used to characterize the transformation relationship between the position in the CAD drawing coordinate system and the corresponding position in the unified site coordinate system, providing a unified spatial coordinate reference for subsequent construction of the occupation grid map, construction of the Euclidean symbol distance field, and generation of prior topological and geometric maps.

[0035] This invention transforms the CAD structural element set obtained in step 1.1 and the site topology connection relationship obtained in step 1.2 into coordinates, so that each CAD structural element and site topology connection relationship is expressed in a unified site coordinate system, thereby obtaining standardized CAD structural data and establishing a coordinate mapping relationship between the CAD drawing coordinate system and the unified site coordinate system.

[0036] Step 2. Based on the standardized CAD structural data, perform discrete rasterization processing in a unified site coordinate system, and encode the occupancy status and semantic attributes of each raster unit to form an initial occupancy raster map.

[0037] Further, based on the robot's dimensions and access conditions, the initial occupied grid map is expanded by obstacle grid area expansion, grid boundary optimization, and accessibility correction to obtain the occupied grid map.

[0038] Step 2.1. Standardize the CAD structural data into a discrete raster.

[0039] Based on the standardized CAD structure data obtained in step 1, the discrete rasterized spatial range of the corresponding site is determined according to the site boundaries, and a two-dimensional raster index is established in the unified site coordinate system according to the preset map resolution.

[0040] The spatial range is divided into grid cells with fixed spatial dimensions.

[0041] Based on the boundary coordinates and spatial location information of each CAD structural element in the standardized CAD structural data, the corresponding raster unit in the two-dimensional raster index is determined for each CAD structural element, thereby obtaining a discrete raster set.

[0042] Through the above discrete rasterization process, the standardized CAD structure data expressed in the form of continuous geometric structure in step 1 is converted into a discrete raster form corresponding to the two-dimensional raster index, which provides a foundation for subsequent occupancy state encoding and semantic attribute encoding.

[0043] Step 2.2. Occupation state encoding and semantic attribute encoding.

[0044] Based on the structural categories of walls, cages, fixed facilities, chicken coop passages, and farm connection areas in the standardized CAD structural data, and their spatial correspondence with grid cells, the occupancy status and semantic attribute encoding of each grid cell in the discrete grid set are performed.

[0045] The grid units corresponding to walls, cages, and fixed facilities are coded as occupied grids, and the grid units corresponding to chicken house passages and farm building connection areas are coded as vacant grids.

[0046] While encoding the occupancy status, corresponding semantic attributes are assigned according to the CAD structural element category of each grid unit, and the semantic attributes are encoded as wall area, cage area, fixed facility area, chicken coop passage area or farm building connection area, thus forming the initial occupancy grid map.

[0047] The site boundary obtained in step 1 is used to define the spatial range of the corresponding site and is used to determine the spatial range of the raster map in the discrete rasterization process in step 2.1. It is not encoded as an independent occupancy status or semantic attribute category.

[0048] When the boundary of the site building coincides with the geometric boundary of fixed structures such as walls, the corresponding grid unit is coded with occupancy status and semantic attributes according to the CAD structural elements such as walls it corresponds to.

[0049] Among them, the occupancy status is used to characterize whether the grid cell is occupied by the fixed structure of the site, and the semantic attribute is used to characterize the CAD structural element category corresponding to the grid cell, so that each grid cell contains both spatial occupancy information and site structure semantic information.

[0050] Step 2.3. Obstacle grid region expansion, grid boundary optimization, and accessibility correction.

[0051] The area in the initial occupied grid map consisting of occupied grids corresponding to walls, cages, and fixed facilities is defined as the obstacle grid area. The expansion range of the obstacle grid area is determined based on the robot's dimensions and a preset safety distance. Using the boundary of the obstacle grid area as a reference, the distance between the surrounding free grids and the boundary is calculated. Free grids whose distance is not greater than the expansion range are re-encoded as occupied grids.

[0052] Through the above expansion process, a non-passable space corresponding to the robot's external dimensions and preset safety distance is reserved around the walls, cages, and fixed facilities.

[0053] Based on this, the isolated grids and jagged boundaries generated by discretization are corrected according to the occupancy status of adjacent grid cells and the continuity of grid boundaries.

[0054] For isolated graticles whose occupancy status is inconsistent with that of their neighboring graticles and for which there is no corresponding fixed structure in the standardized CAD structural data, they are re-encoded based on the occupancy status of their neighboring graticles.

[0055] For local protrusions or depressions in the boundaries generated by discretization, the occupancy status of the relevant boundary grids is corrected according to the CAD geometric boundaries of the walls, cages and fixed facilities.

[0056] Furthermore, the positional consistency of the corrected grid boundary with the corresponding geometric boundary in the standardized CAD structural data is checked to ensure that the discrete grid boundary corresponds to the geometric boundary of the fixed structure of the site.

[0057] Narrow and turning areas are identified within the free grid area after obstacle grid expansion and grid boundary correction. For narrow areas, the local passable width is determined based on the distance between the obstacle grid boundaries on both sides.

[0058] For areas where the channel direction changes or where the robot needs to turn near topological nodes, the available turning radius is determined based on the available space range formed by the continuous free grids in that area.

[0059] The local passable width is compared with the robot's minimum passable width. When the local passable width is not less than the robot's minimum passable width, the corresponding narrow area is determined to meet the passability conditions.

[0060] The available turning radius is compared with the robot's minimum turning radius. When the available turning radius is not less than the robot's minimum turning radius, the corresponding turning area is determined to meet the passage conditions.

[0061] For any idle grid cells whose local passable width is less than the robot's minimum passable width or whose available turning radius is less than the robot's minimum turning radius, recode them as occupied grid cells.

[0062] Step 2, based on the site topology connection relationship obtained in Step 1, uses the semantic grid regions corresponding to adjacent topology nodes as the starting and target regions for connectivity determination, respectively. Based on the spatial adjacency relationship between grid units, it determines the continuity of idle grids between the starting and target regions. When a series of spatially adjacent and all idle grid units can continuously reach the target region from the starting region, it is determined that there is a continuous idle grid channel between adjacent topology nodes.

[0063] The continuous free grid channel is composed of multiple spatially adjacent free grids and is used to represent the actual traversable connection relationship between adjacent topological nodes in the occupied grid map. Through the above-mentioned obstacle grid region expansion, grid boundary correction, local passage condition determination, and topological connectivity verification, traversability correction is completed, and the occupied grid map is obtained.

[0064] Step 3. Based on standardized CAD structural data, extract the obstacle boundary geometric information corresponding to walls, cages and fixed facilities, calculate the Euclidean symbolic distance from each spatial location to the nearest obstacle boundary in the unified field coordinate system, and generate the Euclidean symbolic distance field.

[0065] Further calculate the spatial gradient vector and CAD geometric constraint cost corresponding to each spatial location, and establish the correspondence between spatial location, CAD geometric constraint cost and spatial gradient vector to construct a continuous gradient geometric constraint cost field.

[0066] like Figure 3 As shown, step 3 specifically involves: Step 3.1. Construct the Euclidean symbolic distance field.

[0067] Based on standardized CAD structural data, obstacle areas are determined according to the boundary coordinates and spatial coverage of walls, cages, and fixed facilities, and the boundary positions on the boundaries of the obstacle areas are extracted to form an obstacle boundary set.

[0068] In this embodiment, the obstacle region is used to characterize the actual space occupied by the walls, cages, and fixed facilities, and the obstacle boundary set is used to describe the geometric boundary between the obstacle region and the surrounding space.

[0069] Calculate the shortest Euclidean distance from each spatial location in the unified field coordinate system to the set of obstacle boundaries, and determine the distance sign based on three mutually exclusive spatial states: the spatial location is inside the obstacle region, on the obstacle boundary, or outside the obstacle region, and generate an Euclidean-signed distance field.

[0070] For the current spatial position to be calculated Traverse or query the boundary positions in the obstacle boundary set. The Euclidean distances from the current spatial position to each boundary position are calculated, and the minimum value is taken as the shortest Euclidean distance from the current spatial position to the nearest obstacle boundary. The sign of the distance is further determined based on the spatial state of the current spatial position, thus obtaining the Euclidean sign distance. Wherein, the Euclidean sign distance... Represented as: ; In the formula, Indicates the current spatial location. This indicates the obstacle area determined based on standardized CAD structural data. Represents the set of obstacle boundaries. Indicates the boundary position in the obstacle boundary set; It represents the Euclidean distance between the current spatial location and the boundary location.

[0071] Therefore, spatial locations within the obstacle region but not on the boundary have negative Euclidean signed distances, spatial locations on the obstacle boundary have zero Euclidean signed distances, and spatial locations outside the obstacle region have positive Euclidean signed distances. The Euclidean signed distances for each spatial location in the unified field coordinate system are determined according to the above formula. The Euclidean symbolic distance field is formed by each spatial location and its corresponding Euclidean symbolic distance.

[0072] Step 3.2. Calculate the spatial gradient vector.

[0073] By spatially differentiating the Euclidean symbolic distance field, we obtain the spatial gradient vector corresponding to each spatial location. The spatial gradient vector represents the direction of maximum change of the Euclidean symbolic distance, thus forming a continuous geometric gradient field.

[0074] Among them, the spatial gradient vector Represented as: ; In the formula and These represent the coordinate components of the current spatial location along the two coordinate axes of the unified field coordinate system.

[0075] This invention performs spatial differentiation on the Euclidean symbolic distance field obtained in step 3.1, calculates the rate of change of distance of each spatial location in the two coordinate axes of the unified field coordinate system, and obtains the corresponding spatial gradient vector.

[0076] The spatial gradient vector is used to characterize the direction of the maximum change in the Euclidean symbolic distance at the current spatial position, thus giving the direction of the geometric change of the current spatial position relative to the nearest obstacle boundary.

[0077] This ensures that each spatial location in the Euclidean symbolic distance field simultaneously possesses a corresponding spatial gradient vector, providing constraint direction information for constructing the continuous gradient geometric constraint cost field in step 3.3.

[0078] Step 3.3. Construction of continuous gradient geometric constraint cost field.

[0079] Based on the Euclidean sign distance of each spatial location in the field... Using preset distance field smoothing parameters, calculate the CAD geometric constraint cost corresponding to each spatial location.

[0080] The CAD geometric constraint value corresponding to the current spatial location for: ; In the formula, The preset distance field smoothing parameter is used to adjust the smoothness of the change in CAD geometric constraint value with spatial position and distance from obstacle boundary.

[0081] Among them, the distance field smoothing parameter has the same length dimension as the Euclidean symbolic distance, and its value is determined according to the map spatial resolution and the range of the preset geometric constraints.

[0082] When the distance field smoothing parameter is increased, the change in the CAD geometric constraint cost with the distance to the obstacle boundary becomes more gradual; when the distance field smoothing parameter is decreased, the change in the CAD geometric constraint cost near the obstacle boundary becomes more concentrated.

[0083] As can be seen from the above formula, when the spatial location is on the boundary of a wall, cage, or fixed facility, The corresponding CAD geometric constraint cost is zero; as the distance between the spatial location and the boundary increases, the cost of the CAD geometric constraint increases accordingly, thus using the cost to characterize the distance deviation of the spatial location relative to the fixed structural boundary of the site.

[0084] By establishing a correspondence between each spatial location, the corresponding CAD geometric constraint cost, and the spatial gradient vector, a continuous gradient geometric constraint cost field is constructed to characterize the distance deviation and constraint direction of the spatial location relative to the obstacle boundary.

[0085] Specifically, this invention establishes a one-to-one correspondence between each spatial location in the unified field coordinate system, the corresponding CAD geometric constraint cost, and the spatial gradient vector obtained in step 3.2, so that each spatial location simultaneously has a CAD geometric constraint cost reflecting its deviation from the boundary distance of the wall, cage, and fixed facilities, as well as a spatial gradient vector representing the corresponding geometric constraint direction, thereby constructing a continuous gradient geometric constraint cost field.

[0086] The obstacle boundary described in step 3 is composed of the geometric boundaries of the walls, cages, and fixed facilities. The walls, cages, and fixed facilities together constitute the fixed structure of the stadium, so the obstacle boundary corresponds to the fixed structure boundary of the stadium.

[0087] The continuous gradient geometric constraint cost field is used to characterize the distance deviation and constraint direction of the spatial location relative to the fixed structure boundary of the site, and is associated with the fixed structure boundary of the site in the subsequent step 4 to form the geometric constraint set of the fixed structure of the site.

[0088] Step 4. Based on the coordinate mapping relationship, the site topology connection relationship, the occupied grid map and the continuous gradient geometric constraint cost field are correlated and fused to obtain the prior topology and geometric map.

[0089] This invention, based on the coordinate mapping relationship obtained in step 1, performs correlation processing on standardized CAD structural data, site / building topology connection relationships, the occupied grid map obtained in step 2, and the continuous gradient geometric constraint cost field obtained in step 3. The fixed structure of the site / building is determined according to the occupied state and semantic attributes in the occupied grid map, and a set of geometric constraints for the fixed structure of the site / building is formed by combining it with the continuous gradient geometric constraint cost field. The topological nodes and edges in the site / building topology connection relationships are correlated with the corresponding semantic grid regions and continuous free grid channels in the occupied grid map, forming a site / building topology index. Finally, the above information is unified and fused to obtain a priori topology and geometric map containing geometric information of the fixed structure of the site / building, regional semantic information, continuous geometric constraint information, and channel topology connection relationships.

[0090] like Figure 4 As shown, step 4 specifically involves: Step 4.1. Construct the set of geometric constraints for the fixed structure of the site.

[0091] Based on the occupancy status and semantic attributes in the occupancy grid map obtained in step 2, determine the grid areas corresponding to the walls, cages, and fixed facilities, where the walls, cages, and fixed facilities together constitute the fixed structure of the site.

[0092] By combining standardized CAD structural data, the structural category, boundary position, boundary direction and spatial occupation range of each site building's fixed structure are extracted, thereby establishing a geometric description of the site building's fixed structure in a unified site building coordinate system.

[0093] Based on the continuous gradient geometric constraint cost field obtained in step 3, the corresponding CAD geometric constraint cost and spatial gradient vector are determined at the boundary position corresponding to each fixed structure of the site, and associated with the structure category, boundary position, boundary direction and spatial occupancy range of the corresponding fixed structure of the site to form a set of geometric constraints for the fixed structure of the site.

[0094] Among them, the CAD geometric constraint cost is used to characterize the distance deviation of the spatial position relative to the fixed structural boundary of the corresponding site, and the spatial gradient vector is used to characterize the geometric constraint direction of the corresponding position.

[0095] This establishes a correspondence between the geometric boundary information of the walls, cages, and fixed facilities and the continuous geometric constraint information formed in step 3, providing a foundation for the robot to perform front-end matching based on the fixed structure of the facility and to establish CAD geometric constraints.

[0096] Step 4.2. Construct the site topology index.

[0097] Based on the site topology connection relationship obtained in step 1, set topology identifiers for each topology node and topology edge.

[0098] Based on the intersection of the chicken coop passages, the entrances and exits of the farm, and the connection locations of the farm connecting areas determined in step 1.2, the corresponding topological nodes are associated with the corresponding semantic grid areas in the occupied grid map, so that each topological node can correspond to a specific area location in the actual farm space.

[0099] The topological edges connecting adjacent topological nodes are associated with the continuous free grid channels between adjacent topological nodes determined in step 2.3. This establishes a correspondence between the topological nodes and topological edges in the field building topological connection relationship and the semantic grid regions and continuous free grid channels in the occupied grid map, thereby forming a field building topological index. The field building topological index describes the index relationship between topological nodes, topological edges, and the corresponding semantic grid regions and continuous free grid channels in the occupied grid map. It is also used in the subsequent robot localization process to determine the corresponding field building area and local geometric region based on the robot's initial pose.

[0100] Step 4.3. Coordinate unification and correlation fusion.

[0101] Based on the coordinate mapping relationship obtained in step 1, the occupied grid map obtained in step 2, the continuous gradient geometric constraint cost field obtained in step 3, the set of fixed structure geometric constraints of the site obtained in step 4.1, and the site topology index obtained in step 4.2 are unified into a unified spatial coordinate reference, so that all the above information is expressed in a unified site coordinate system and then associated and fused.

[0102] A correspondence is established between topological nodes and semantic grid regions, between topological edges and continuous free grid channels, and between the boundaries of fixed structures in the chicken coop and their corresponding CAD geometric constraints and spatial gradient vectors. This yields a priori topological and geometric map containing geometric information of the fixed structures in the chicken coop, regional semantic information, continuous geometric constraint information, and channel topological connections. Specifically, the occupancy grid map provides the occupancy status and regional semantic information of the chicken coop space; the set of geometric constraints for the fixed structures in the chicken coop provides the geometric information of walls, cages, and fixed facilities, along with their corresponding continuous geometric constraints; and the chicken coop topological index provides the topological connections between chicken coop channels and different chicken coops.

[0103] Step 5. Collect LiDAR point cloud, inertial measurement unit (IMU), and wheel speed odometry (WSO) data during robot operation. Perform time alignment and coordinate transformation on the multi-source sensor data, and use IMU and WSO data to predict the robot's initial pose. Based on the initial pose and the site topology index, extract the corresponding local geometric region from the prior topology and geometric map. Perform front-end matching between the current frame LiDAR point cloud and the local geometric region to obtain the robot's current pose estimation result and front-end matching residual. Construct an approximate Hessian matrix based on the front-end matching residual, and determine the degree and direction of scene geometric degradation through eigenvalue decomposition. Figure 5 As shown, step 5 specifically involves: Step 5.1. Multi-sensor time alignment, initial pose prediction, and local geometric region extraction.

[0104] Using the acquisition time of the current frame's lidar point cloud data as the time reference, the inertial measurement unit and wheel speed odometer data are timestamped to ensure that the multi-source sensor data participating in the current frame processing correspond to a unified time reference.

[0105] By utilizing the pre-calibrated extrinsic parameter transformation relationships of the LiDAR coordinate system, inertial measurement unit (IMU) coordinate system, and wheel odometer coordinate system relative to the robot's body coordinate system, the LiDAR point cloud, IMU, and wheel odometer data are transformed into the robot's body coordinate system to unify the spatial representation of various sensor data. The angular velocity and linear acceleration of the IMU between adjacent LiDAR point cloud frame acquisition times are integrated to obtain the robot's attitude and velocity increments; the wheel odometer data within the same time interval are integrated to obtain the robot's displacement increment.

[0106] Based on the robot's pose state in the unified field coordinate system at the previous moment, as well as its attitude increment, velocity increment, and displacement increment, the robot's initial pose in the unified field coordinate system at the current moment is predicted. This initial pose serves as the initial pose value for front-end matching of the current frame's LiDAR point cloud. Centered on this initial pose, a local search range is determined according to a preset search distance. Combining this with the prior topology and the field topology index in the geometric map, the fixed structural boundaries of the field and their continuous geometric constraints are extracted from this local search range to form a local geometric region.

[0107] Specifically, using the initial pose's position coordinates as the center, a local spatial range around the initial pose is defined according to a preset search distance, and this local spatial range is determined as the local search range. Combining the prior topology and the site topology index in the geometric map described in step 4, the site area corresponding to the robot's current location is determined, and the fixed structure boundary of the site and its continuous geometric constraints are extracted from the local search range, thereby forming the local geometric region used for front-end matching in the current frame. Through local geometric region extraction, only the fixed structure geometric information of the site corresponding to the robot's current predicted position is used as the geometric reference for front-end matching of the LiDAR point cloud in the current frame.

[0108] Step 5.2. Front-end matching of the current frame LiDAR point cloud and estimation of the robot's current pose.

[0109] Using the current frame of LiDAR point cloud data that has been converted to the robot body coordinate system in step 5.1, extract the point cloud positions of the LiDAR point cloud data in the plane corresponding to the prior topology and geometric map.

[0110] Based on the initial pose obtained in step 5.1, the positions of each point cloud are transformed into a unified field building coordinate system, and the corresponding nearest fixed structure boundary position and its unit normal vector are determined for each point cloud position within a local geometric region.

[0111] The front-end matching residual is constructed based on the distance along the boundary normal direction between the transformed point cloud position and the corresponding nearest fixed structure boundary position. The robot's current pose estimation result is obtained by minimizing the weighted sum of squares of the front-end matching residual.

[0112] For the first in the current frame For each point cloud location, the front-end matching residual is represented as: ; In the formula, Indicates the first The front-end matching residual corresponding to each point cloud location; Indicates the robot in the The two-dimensional pose state at a given time. and These represent the two position coordinates of the robot in the unified field coordinate system. Indicates the robot's heading angle; Indicates the robot's heading angle A defined two-dimensional rotation matrix, Represents the robot's position coordinates and The resulting two-dimensional translation vector; This indicates that the current frame of the LiDAR point cloud, after being transformed to the robot's body coordinate system in step 5.1, is located in the corresponding plane. Point cloud location, In the unified field coordinate system, the coordinates of the first... The point cloud location corresponds to the nearest fixed structural boundary of the site. This represents the unit normal vector corresponding to the nearest fixed structural boundary position of the site.

[0113] The aforementioned front-end matching residuals are constructed for each point cloud location involved in the front-end matching. The robot's current pose is then solved by minimizing the weighted sum of squares of each front-end matching residual. The optimization objective is expressed as: ; In the formula, This represents the robot's current pose estimation result obtained through front-end matching. Indicates the first The weights of the front-end matching residuals. This indicates the number of point cloud locations involved in the front-end matching.

[0114] By iteratively solving the optimization objective, the current frame of the LiDAR point cloud and the fixed structure boundary of the field in the local geometric region are made to achieve the corresponding geometric matching state, thereby obtaining the robot's current pose estimation result. At the same time, the front-end matching residuals corresponding to each point cloud position are retained for scene geometric degradation analysis in step 5.3.

[0115] Step 5.3. Determine the degree and direction of scene geometric degradation.

[0116] Calculate each front-end matching residual obtained in step 5.2 relative to the robot's two-dimensional pose state. The Jacobian matrix is ​​obtained, and an approximate Hessian matrix is ​​constructed based on the Jacobian matrix and the corresponding residual weights.

[0117] Among them, the approximate Hessian matrix Constructed from the Jacobian matrix of each front-end matching residual and the corresponding residual weight, it is used to characterize the strength of the local geometric constraints provided by the front-end matching of the current frame to each two-dimensional pose orientation of the robot, and is expressed as: ; In the formula Indicates the first The Jacobian matrix of each front-end matching residual relative to the robot's current pose state; Compared with step 5.2 The weights used for each front-end matching residual correspond to the weights.

[0118] The approximate Hessian matrix is ​​used to characterize the geometric constraint strength provided by each front-end match in the current frame to different two-dimensional pose orientations of the robot. (The approximate Hessian matrix is ​​then described.) Perform eigenvalue decomposition: ; , ; Among them, the eigenvector matrix Represented as: In the formula, Represented by the approximate Hessian matrix The eigenvector matrix is ​​composed of the eigenvectors of each eigenvector, where In order to be with the first eigenvalues The corresponding feature vector.

[0119] Characteristic diagonal matrix Represented as: ; This represents a diagonal matrix composed of eigenvalues. This indicates an operation that constructs a diagonal matrix from the elements within the parentheses; The dimension representing the robot's pose state.

[0120] The feature values ​​are arranged in ascending order: .

[0121] For the robot's two-dimensional pose state defined in step 5.2 ,have These correspond to the two position coordinate components of the robot in the unified field coordinate system. , and heading angle components .

[0122] Larger eigenvalues ​​in an approximate Hessian matrix indicate stronger geometric constraints on the corresponding pose direction, while smaller eigenvalues ​​indicate weaker effective geometric constraints on the corresponding pose direction.

[0123] Based on the minimum eigenvalue With the largest eigenvalue The relationship between the two is used to calculate the degree of geometric degradation of the current scene, and the minimum eigenvalue is used to calculate the degree of geometric degradation. The corresponding eigenvectors are determined as the degradation direction. The degree of scene geometric degradation at any given moment Represented as: ; In the formula, This represents a preset minimum positive number to prevent the denominator from being zero; in this embodiment, the value is taken as... .

[0124] When the minimum eigenvalue is smaller than the maximum eigenvalue, the degree of scene geometric degradation increases, indicating that the geometric constraints of the current scene in at least one pose direction are relatively insufficient; when the constraint strengths corresponding to each pose direction are relatively balanced, the degree of scene geometric degradation decreases accordingly.

[0125] Minimum eigenvalue corresponding feature vector The degradation direction determined for the current scene is denoted as... The degradation direction is used to characterize the robot's two-dimensional pose direction with the weakest geometric constraints in the current scene.

[0126] Therefore, step 5 obtains the robot's current pose estimation result, the scene's geometric degradation degree, and the degradation direction, and then passes these results to step 6. Specifically, the robot's current pose estimation result establishes relevant pose constraints and CAD geometric constraints, and the scene's geometric degradation degree and degradation direction are used to dynamically adjust the information weights of the CAD geometric constraints in the corresponding pose directions.

[0127] Step 6. Based on the robot's current pose estimation results, scene geometric degradation degree and degradation direction obtained in Step 5, construct a factor graph including laser odometry constraints, inertial measurement unit pre-integration constraints, wheel speed odometry constraints and CAD geometric constraints; dynamically adjust the information weight of CAD geometric constraints in the corresponding pose direction according to the scene geometric degradation degree and degradation direction, and perform joint optimization in combination with various constraints to obtain the optimized global pose sequence.

[0128] This invention establishes pose nodes corresponding to the robot at different times based on the robot's current pose estimation result, scene geometric degradation degree and direction obtained in step 5, and the prior topology and geometric map obtained in step 4. Laser odometry constraints, inertial measurement unit pre-integration constraints, and wheel speed odometry constraints are established between adjacent pose nodes. Simultaneously, based on the prior topology and geometric map, CAD geometric constraints are established between the pose nodes and the robot's current pose estimation result obtained in step 5, constructing a factor graph. On this basis, the information weights of the CAD geometric constraints in the corresponding pose direction are dynamically adjusted according to the scene geometric degradation degree and direction. A multi-source constraint joint optimization problem is constructed by combining various constraints, and the optimized global pose sequence is obtained through iterative solution. Figure 6 As shown, step 6 specifically involves: Step 6.1. Construction of pose nodes, multi-source constraints and factor graphs.

[0129] Using the robot's two-dimensional pose state corresponding to each LiDAR point cloud frame as pose nodes, the set of pose states formed during the robot's continuous operation is denoted as: .in, This represents the set of pose states to be optimized. Indicates the first The robot's two-dimensional pose state in a unified field coordinate system at any given time.

[0130] Based on the robot's current pose estimation results obtained in step 5, calculate the... Time to the The relative pose transformation at each moment is used to obtain the relative pose measurement of the laser odometry; the angular velocity and linear acceleration of the inertial measurement unit are pre-integrated based on the acquisition time of adjacent lidar point cloud frames to obtain the pre-integrated relative pose measurement of the inertial measurement unit; the relative motion increment of the robot is calculated based on the wheel speed odometry data within the same time interval to obtain the wheel speed odometry relative pose measurement.

[0131] Based on the relative pose measurements from the laser odometry, the pre-integration relative pose measurements from the inertial measurement unit (IMU), and the wheel speed odometry, constraints are established between adjacent pose nodes, including laser odometry constraints, IMU pre-integration constraints, and wheel speed odometry constraints. For the... Time to the The first moment The residuals of the relative pose constraints are expressed as follows: In the formula, L, I, and O represent the laser odometer, the inertial measurement unit, and the wheel speed odometer, respectively. Represents the residuals of each relative pose constraint; , and They represent the first Time to the The relative pose measurement of the laser odometry at any given time, the relative pose measurement of the inertial measurement unit pre-integration, and the relative pose measurement of the wheel speed odometry; For the first The reversed position of the position at a given time. This represents pose composition operations. This represents the position difference calculation.

[0132] Meanwhile, based on the prior topology and geometric map obtained in step 4 and the robot current pose estimation result obtained in step 5, the robot current pose estimation result obtained in step 5 is used as the CAD prior pose observation of the corresponding pose node, and CAD geometric constraints are established between the pose node and the CAD prior pose observation.

[0133] CAD geometric constraints residuals Represented as: ; This indicates the robot's current pose estimation result obtained in step 5 by front-end matching the current frame's LiDAR point cloud with the prior topology and geometric map.

[0134] The factor graph used in step 6 is composed of the above pose nodes, laser odometry constraints, inertial measurement unit pre-integration constraints, wheel speed odometry constraints, and CAD geometric constraints.

[0135] Step 6.2. Calculation of CAD geometric constraint direction weights and adjustment of information matrix.

[0136] According to the result obtained in step 5 Moment-time scene geometric degradation and the direction of degradation Calculate the first The CAD geometric constraint direction weights are assigned to each pose node, and the information matrix of the CAD geometric constraints is adjusted according to the direction weights.

[0137] The directional weight increases with the degree of scene geometric degradation, enabling CAD geometric constraints to play a stronger supplementary role in pose directions where geometric constraints are weaker; when the degree of scene geometric degradation decreases, the information weight of CAD geometric constraints in that direction is reduced accordingly, so as to reduce the over-reliance on CAD prior information when environmental geometric information is sufficient.

[0138] The directional weights of CAD geometric constraints are represented as follows: ; The adjusted CAD geometric constraint information matrix is ​​represented as follows: In the formula, Indicates the first The CAD geometric constraint direction weights corresponding to each pose node. and These represent the preset minimum and maximum directional weights, respectively. .

[0139] A matrix representing the basic information of CAD geometric constraints.

[0140] The degradation direction obtained in step 5 Perform normalization to ensure that it satisfies: The direction of degradation after normalization is still denoted as , This represents the direction projection matrix of the degradation direction in the robot's two-dimensional pose state space.

[0141] As the degree of scene geometric degradation increases, the information weight of CAD geometric constraints in the degradation direction increases accordingly; as the degree of scene geometric degradation decreases, the additional information weight in that direction decreases accordingly. Through the above processing, the degree and direction of scene geometric degradation obtained in step 5 directly participate in the adjustment of the CAD geometric constraint information matrix, and further affect the multi-source constraint joint optimization in step 6.3.

[0142] Step 6.3. Multi-source constraint joint optimization and global pose sequence solution.

[0143] Based on the factor graph constructed in step 6.1 and the CAD geometric constraint information matrix obtained in step 6.2, a multi-source constraint joint optimization problem is constructed, and the optimized pose state set is obtained. Represented as: .

[0144] In the formula, , and These represent the information matrices for laser odometer constraints, inertial measurement unit pre-integration constraints, and wheel speed odometer constraints, respectively. This indicates step 6.2 based on the... Moment-time scene geometric degradation and the direction of degradation Adjusted CAD geometric constraint information matrix. For any residual vector ,have: .

[0145] This invention incorporates laser odometry constraints, inertial measurement unit pre-integration constraints, wheel speed odometry constraints, and CAD geometric constraints into a multi-source constraint joint optimization problem, and uses the information matrices corresponding to various constraints to weight the residuals, so that pose constraints from different sources act together on the set of pose states to be optimized.

[0146] For each constraint residual in the factor graph in the current pose state set Linearization is performed at each constraint factor, the Jacobian matrix of each constraint factor is calculated, and the normal equation is constructed based on the residuals, Jacobian matrices, and information matrices of each constraint factor: .

[0147] In the formula, This represents the constraint factors in the factor graph. This represents the residual of the corresponding constraint factor at the current pose state set. The Jacobian matrix represents the residual of the constraint factor relative to the relevant pose state. This represents the information matrix corresponding to the constraint factors; The normal equation coefficient matrix, consisting of the Jacobian matrix and information matrix of each constraint factor, is used to characterize the comprehensive constraint strength of each constraint factor on the pose state increment. The gradient vector, composed of the residuals, Jacobian matrix, and information matrix of each constraint factor, is used to characterize the first-order change of the joint optimization objective relative to the pose state increment at the current pose state set.

[0148] Represents the set of pose states to be optimized The corresponding pose state increment is composed of the two-dimensional pose state increment corresponding to each pose node, that is: ; Among them, the The 2D pose state increment corresponding to each pose node is: .

[0149] Solving the normal equation yields the pose state increment. Based on the two-dimensional pose state increment corresponding to each pose node Update the corresponding pose nodes and recalculate the residuals and Jacobian matrices for each constraint factor.

[0150] Repeat the linearization, normal equation solving, and pose node update process described above until the position increment and heading angle increment of each pose node simultaneously satisfy the preset convergence condition. The preset convergence condition is expressed as: and .

[0151] In the formula, This indicates the preset position convergence threshold. This indicates a preset heading angle convergence threshold; when both of the above conditions are met simultaneously, the multi-source constraint joint optimization process is deemed to have converged, and the optimized global pose sequence is obtained. .

[0152] Will The optimized global pose sequence is the output of step 6.

[0153] Step 7. Based on the global pose sequence obtained in Step 6 and the coordinate mapping relationship obtained in Step 1, transform the LiDAR point cloud data collected at different times in Step 5 to a unified farm coordinate system, and determine the unified farm coordinate system as the global map coordinate system; fuse and update the transformed LiDAR point cloud data, and output a global map that is globally consistent with the fixed structure of the farm and the robot's real-time pose estimation results.

[0154] Based on the coordinate mapping relationship obtained in step 1, this invention determines the unified farm coordinate system used in the prior topology and geometric map described in step 4 as the global map coordinate system. According to the optimized global pose sequence obtained in step 6 and the extrinsic parameter transformation relationship between the LiDAR coordinate system and the robot body coordinate system pre-calibrated in step 5.1, the LiDAR point cloud data collected at different times during robot operation are uniformly transformed to the global map coordinate system. For historical moments where pose changes occur due to global optimization in step 6, the LiDAR point cloud data for the corresponding historical moment is transformed again according to the updated optimized pose. Furthermore, the association between point cloud data and farm spatial structure is established using the farm topology index in the prior topology and geometric map described in step 4, and a global map that maintains global consistency with the fixed structure of the farm is obtained through point cloud spatial indexing, merging and deduplication, and fusion updates. Simultaneously, the optimized pose at the latest moment in the global pose sequence obtained in step 6 is determined as the robot's real-time pose estimation result. Figure 7 As shown, step 7 specifically involves: Step 7.1. Determine the global map coordinate system and transform the lidar point cloud coordinates.

[0155] Based on the coordinate mapping relationship obtained in step 1, the unified field coordinate system used by the prior topology and geometric map in step 4 is determined as the global map coordinate system, so that the point cloud data obtained during robot operation and the prior topology and geometric map adopt a unified spatial coordinate reference. According to the global pose sequence obtained in step 6 and the external parameter transformation relationship between the LiDAR coordinate system and the robot body coordinate system pre-calibrated in step 5.1, the LiDAR point cloud data collected in step 5 at each time moment is transformed from the LiDAR coordinate system to the robot body coordinate system and the global map coordinate system at the corresponding time moment.

[0156] For the The constantly collected LiDAR point cloud is first transformed from the LiDAR coordinate system to the robot's body coordinate system according to the external parameter transformation relationship. Then, based on the first step obtained in step 6... The pose is continuously optimized and further transformed to the global map coordinate system. The first time in the point cloud of lidar The coordinate transformation relationship of the point cloud positions is expressed as follows:

[0157] In the formula, Indicates the first The first time in the point cloud of lidar The coordinates of each point cloud location in the lidar coordinate system; This represents the extrinsic transformation matrix from the lidar coordinate system to the robot's body coordinate system. This indicates the result obtained from step 6. Optimize pose continuously The pose transformation matrix from the robot's body coordinate system to the global map coordinate system. This indicates the position of the point cloud after transformation to the global map coordinate system.

[0158] Based on the coordinate transformation relationship described above, the LiDAR point cloud data collected at each time point is processed to ensure that the point cloud positions collected at different times are uniformly represented in the global map coordinate system. When step 6 updates the optimized pose at historical time points, the pose transformation matrix from the robot body coordinate system to the global map coordinate system is re-determined based on the updated optimized pose, and the LiDAR point cloud data at the corresponding historical time points are transformed again to obtain point cloud data corrected by the global pose.

[0159] Step 7.2. Point cloud topology association, spatial indexing, and global map fusion and update.

[0160] Based on the site and building topology index in the prior topology and geometric map described in step 4, the point cloud data corrected by global pose is associated with its site and building area, site and building connection area and corresponding topology node, so that the LiDAR point cloud data actually collected by the robot establishes a correspondence with the site and building spatial structure and topology connection information in the prior topology and geometric map.

[0161] The global map coordinate system is divided into multiple spatial index units according to a preset spatial resolution. Based on the coordinates of each point cloud position after global pose correction within the global map coordinate system, each point cloud position is assigned to a corresponding spatial index unit, thus establishing a point cloud spatial index. Point cloud positions falling into the same spatial index unit are merged, and duplicate point cloud positions are removed to reduce redundancy in point cloud data caused by repeated robot traversal of the same area or overlapping observations of adjacent LiDAR point cloud frames. The point cloud data at each time point, after completing the farm topology association, spatial indexing, and merging and deduplication processing, is then fused with the existing global map. When the optimized pose of a historical time point is updated, the historical point cloud data affected by the pose update is determined based on the acquisition time of the point cloud. The original point cloud position at the corresponding historical time point is replaced with the point cloud position obtained by re-transforming based on the updated optimized pose, resulting in a global map that maintains global consistency with the fixed structure of the farm.

[0162] Specifically, when new LiDAR point cloud data is added, the newly added point cloud data, after transformation in step 7.1, is assigned to the corresponding spatial indexing unit and fused with the existing point cloud data. When step 6 updates the optimized pose at a certain historical moment, the LiDAR point cloud data corresponding to that historical moment is transformed again according to the updated optimized pose, replacing the original point cloud position with the transformed point cloud position, and the corresponding spatial indexing and fusion processing are performed again. Through the above process, the global map is continuously updated to obtain a global map that is globally consistent with the fixed structure of the aquaculture farm.

[0163] Step 7.3. Determine the robot's real-time pose and dynamically update the global map.

[0164] The optimized pose at the latest moment in the global pose sequence obtained in step 6 is determined as the robot's real-time pose estimation result. For the current latest moment... The robot's real-time pose estimation result is expressed as: In the formula, This represents the latest moment in the current global pose sequence. This represents the real-time pose estimation result of the robot at time k. and These represent the robot's optimized position coordinates in the global map coordinate system. This represents the robot's optimized heading angle. Therefore, the robot's real-time pose estimation result is directly taken from the global pose sequence obtained in step 6, eliminating the need to perform independent pose optimization again in step 7.

[0165] As the robot continues to operate and collects new LiDAR point cloud data, inertial measurement unit data, and wheel speed odometer data, steps 5 and 6 are executed sequentially to obtain new robot current pose estimation results, scene geometric degradation degree, and degradation direction, and to update the global pose sequence. Based on the updated global pose sequence, steps 7.1 and 7.2 are executed to perform coordinate transformation and fusion updates on the newly added point cloud data and historical point cloud data affected by the pose update. At the same time, the optimized pose at the latest moment in the updated global pose sequence is determined as the new robot real-time pose estimation result, thereby continuously outputting the updated global map and robot real-time pose estimation results.

[0166] This invention provides a method for structural analysis and unified calibration of a two-dimensional CAD layout of a farm, resulting in standardized CAD structural data, farm building topology connections, and coordinate mapping relationships. It also constructs an occupation grid map and a continuous gradient geometric constraint cost field, forming a priori topological and geometric map through coordinate association and information fusion. During robot operation, the initial robot pose is predicted using inertial measurement unit (IMU) data and wheel odometer data. Front-end matching is then performed using LiDAR point cloud data and the prior topological and geometric map to obtain the robot's current pose estimation. An approximate Hessian matrix is ​​constructed based on the front-end matching residual to determine the degree and direction of scene geometric degradation. Furthermore, a factor map is constructed, incorporating LiDAR constraints, IMU pre-integration constraints, wheel odometer constraints, and CAD geometric constraints. The information weights of the CAD geometric constraints are dynamically adjusted based on the degree and direction of scene geometric degradation to obtain an optimized global pose sequence. The LiDAR point cloud data at each time step is then transformed and fused based on the global pose sequence, outputting a global map consistent with the fixed structure of the farm and the robot's real-time pose estimation results. This invention utilizes prior CAD structural information to supplement positioning constraints along geometrically degenerate directions, thereby reducing cumulative pose errors in long passageways and repetitive structural scenarios. This improves the positioning consistency and accuracy of the robot during continuous operation across multiple rooms. Figure 1 To the point of being responsive.

[0167] Example 2 This embodiment 2 describes a computer device including a memory and one or more processors. Executable code is stored in the memory. When the processor executes the executable code, it implements the steps of the CAD pre-mapping-assisted positioning and mapping method for large-scale farms described in embodiment 1 above.

[0168] In this embodiment, the computer device can be any device or apparatus with data processing capabilities, and will not be described in detail here.

[0169] Example 3 This embodiment 3 describes a computer-readable storage medium storing a program that, when executed by a processor, is used to implement the steps of the CAD pre-map assisted positioning and mapping method for large-scale farms described in embodiment 1 above.

[0170] The computer-readable storage medium can be an internal storage unit of any device or apparatus with data processing capabilities, such as a hard disk or memory, or an external storage device of any device with data processing capabilities, such as a plug-in hard disk, smart media card (SMC), SD card, flash card, etc.

[0171] Of course, the above description is only a preferred embodiment of the present invention. The present invention is not limited to the above-described embodiments. It should be noted that any equivalent substitutions or obvious modifications made by those skilled in the art under the guidance of this specification fall within the scope of this specification and should be protected by the present invention.

Claims

1. A CAD pre-built map-assisted positioning and mapping method for large-scale farms, characterized in that, Includes the following steps: Step 1. Import the 2D CAD layout of the farm and perform analysis and unified calibration, converting it to a unified farm coordinate system to obtain standardized CAD structural data, farm topology connection relationships, and coordinate mapping relationships; Step 2. Based on standardized CAD structural data, perform discrete rasterization and encoding of occupancy status and semantic attributes. After obstacle raster region expansion, raster boundary optimization and accessibility correction, obtain the occupancy raster map. Step 3. Calculate the Euclidean symbolic distance from each spatial location to the nearest obstacle boundary based on standardized CAD structural data, generate the Euclidean symbolic distance field, calculate the spatial gradient vector, and construct the continuous gradient geometric constraint cost field. Step 4. Based on the coordinate mapping relationship, the topological connection relationship of the site and the occupied grid map and the continuous gradient geometric constraint cost field are correlated and fused to obtain the prior topological and geometric map; Step 5. Collect data from multiple sensor sources and predict the robot's initial pose, then extract the corresponding local geometric regions; The current frame of LiDAR point cloud is matched with the local geometric region to obtain the robot's current pose estimation result and front-end matching residual. The degree and direction of scene geometric degradation are obtained based on the front-end matching residual. Step 6. Construct a factor graph containing multi-source sensor constraints and CAD geometric constraints based on the robot's current pose estimation results. Adjust the information weights of the CAD geometric constraints according to the degree and direction of scene geometric degradation and perform joint optimization. Step 7. Based on the global pose sequence obtained by joint optimization in Step 6, transform the LiDAR point cloud data at different times to a unified field coordinate system and fuse and update it, outputting a global map and the robot's real-time pose estimation results.

2. The CAD pre-built map-assisted positioning and mapping method for large-scale farms according to claim 1, characterized in that, Step 1 specifically involves: Step 1.

1. Read the layer information, geometric entity types, and geometric attributes from the 2D CAD layout drawing of the farm; Based on CAD layer identifiers, geometric entity types, and geometric attributes, CAD structural elements are identified, and the structural category, boundary coordinates, dimensions, orientation, and spatial location information of each CAD structural element are extracted to form a set of CAD structural elements. The CAD structural elements include walls, cages, fixtures, chicken coop passages, farm boundaries, and farm connection areas; Step 1.

2. Based on the CAD structural element set, determine the spatial adjacency and connectivity relationships between each CAD structural element according to its structural category and spatial location information; Based on this, the passable areas, obstacle areas, and connecting areas of the venue are determined; The intersection of chicken coop passages, the entrance and exit of the farm, and the connection points of the farm connection areas are defined as topological nodes. The continuous passable passages connecting adjacent topological nodes are defined as topological edges to obtain the farm topological connection relationship. Step 1.

3. Determine the scale transformation parameters based on the proportional relationship between the dimensions marked in the 2D CAD layout drawing and the corresponding dimensions of the actual site, so as to establish the scale correspondence between the dimensions of the CAD drawing and the dimensions of the actual site space; The rotation transformation parameters are determined based on the directional correspondence between the CAD drawing coordinate system and the preset unified site coordinate system, and the translation transformation parameters are determined based on the positional correspondence between the two coordinate systems. Based on scale transformation parameters, rotation transformation parameters, and translation transformation parameters, coordinate transformation is performed on the CAD structural element set and the topological connection relationship of the site to obtain standardized CAD structural data and the coordinate mapping relationship between the two coordinate systems.

3. The CAD pre-built map-assisted positioning and mapping method for large-scale farms according to claim 2, characterized in that, Step 2 specifically involves: Step 2.

1. Based on the standardized CAD structure data obtained in Step 1, determine the spatial range of the discrete rasterization according to the site boundary, and establish a two-dimensional raster index in the unified site coordinate system according to the preset map resolution; Based on the boundary coordinates and spatial location information of each CAD structural element in the standardized CAD structural data, the grid cell corresponding to each CAD structural element in the two-dimensional grid index is determined, and a discrete grid set is obtained. Step 2.

2. Based on the structural categories of walls, cages, fixed facilities, chicken coop passages and farm building connection areas in the standardized CAD structural data and their spatial correspondence with grid units, perform occupancy status encoding and semantic attribute encoding on each grid unit in the discrete grid set; The grid units corresponding to walls, cages and fixed facilities are coded as occupied grids, and the grid units corresponding to chicken house passages and farm building connection areas are coded as vacant grids. Based on the CAD structural element category corresponding to each grid unit, its semantic attributes are encoded as wall area, cage area, fixed facility area, chicken coop passage area or farm connection area to form an initial occupied grid map. Among them, the boundary of the venue is not encoded as an independent occupancy status and semantic attribute category; Step 2.

3. Determine the area in the initial occupied grid map that consists of occupied grids corresponding to walls, cages, and fixed facilities as the obstacle grid area; The expansion range of the obstacle grid area is determined based on the robot's external dimensions and a preset safety distance. Empty grids whose distance from the boundary of the obstacle grid area is not greater than the expansion range are re-encoded as occupied grids. Based on the occupancy status of adjacent grid cells and the continuity of grid boundaries, isolated grids and sawtooth boundaries generated by discretization are corrected, and the corrected grid boundaries are checked for consistency according to the corresponding geometric boundaries of walls, cages and fixed facilities in standardized CAD structural data. After completing the expansion of the obstacle grid area and the correction of the grid boundary, identify the narrow area and the turning area in the free grid area; determine the local passable width based on the distance between the obstacle grid boundaries on both sides of the narrow area, and determine the available turning radius based on the range of free grid space in the turning area; The local passable width is compared with the robot's minimum passable width, and the available turning radius is compared with the robot's minimum turning radius. The relevant empty grids that do not meet the corresponding passability requirements are recoded as occupied grids. Based on the topological connection relationship of the site, it is determined whether there is a grid path formed by the continuous connection of adjacent free grids between the semantic grid areas corresponding to adjacent topological nodes. The continuous free grid areas that can connect the semantic grid areas corresponding to the adjacent topological nodes are identified as continuous free grid channels, and the traversability correction is completed to obtain the occupied grid map.

4. The CAD pre-built map-assisted positioning and mapping method for large-scale farms according to claim 3, characterized in that, Step 3 specifically involves: Step 3.

1. Based on standardized CAD structural data, determine the obstacle area according to the boundary coordinates and spatial coverage of the walls, cages and fixed facilities, and extract the boundary positions on the boundary of the obstacle area to form an obstacle boundary set; Calculate the shortest Euclidean distance from each spatial location in the unified field coordinate system to the set of obstacle boundaries, and determine the distance sign based on whether the spatial location is inside the obstacle region, on the obstacle boundary, or outside the obstacle region, generating an Euclidean symbolic distance field; where, the Euclidean symbolic distance... Represented as: ； In the formula, Indicates the current spatial location. This indicates the obstacle area determined based on standardized CAD structural data. Represents the set of obstacle boundaries. Indicates the boundary position in the obstacle boundary set; This represents the Euclidean distance between the current spatial location and the boundary location; Step 3.

2. Take the spatial derivative of the Euclidean symbolic distance field to obtain the spatial gradient vector corresponding to each spatial location, and use the spatial gradient vector to represent the direction of maximum change of the Euclidean symbolic distance, thus forming a continuous geometric gradient field. Among them, the spatial gradient vector Represented as: ； In the formula, and These represent the coordinate components of the current spatial position along the two coordinate axes, respectively. Step 3.

3. Based on the Euclidean symbolic distances corresponding to each spatial position in the field... Using preset distance field smoothing parameters, calculate the CAD geometric constraint cost corresponding to each spatial location; Among them, the CAD geometric constraint cost corresponding to the current spatial location for: ； In the formula, The preset distance field smoothing parameter is used to adjust the smoothness of the change in CAD geometric constraint value with spatial position and distance from obstacle boundary; By establishing a correspondence between each spatial location, the corresponding CAD geometric constraint cost, and the spatial gradient vector, a continuous gradient geometric constraint cost field is constructed to characterize the distance deviation and constraint direction of the spatial location relative to the obstacle boundary.

5. The CAD pre-built map-assisted positioning and mapping method for large-scale farms according to claim 4, characterized in that, Step 4 specifically involves: Step 4.

1. Based on the occupation status and semantic attributes in the occupation grid map obtained in Step 2, determine the grid areas corresponding to the walls, cages, and fixed facilities, where the walls, cages, and fixed facilities together constitute the fixed structure of the site. By combining standardized CAD structural data, the structural category, boundary location, boundary direction and spatial occupancy of each site building's fixed structure are extracted; and based on the obtained continuous gradient geometric constraint cost field, the CAD geometric constraint cost and spatial gradient vector corresponding to the boundary of each site building's fixed structure are determined, thus forming a set of geometric constraints for the site building's fixed structure. Step 4.

2. Based on the farm building topology connection relationship obtained in Step 1, set topology identifiers for each topology node and topology edge, associate the topology nodes corresponding to the intersection of chicken coop passages, farm building entrances and exits, and farm building connection areas with the corresponding semantic grid areas in the occupied grid map, and associate the topology edges connecting adjacent topology nodes with the continuous free grid channels between adjacent topology nodes in the occupied grid map to form a farm building topology index; Step 4.

3. Based on the coordinate mapping relationship obtained in Step 1, coordinate unification and association fusion are performed on the occupied grid map, the continuous gradient geometric constraint cost field, the set of fixed structure geometric constraints of the venue obtained in Step 4.1, and the venue topology index obtained in Step 4.

2. The correspondence between topology nodes and semantic grid regions, topology edges and continuous free grid channels, and the boundaries of fixed structure of venues and the corresponding CAD geometric constraint cost and spatial gradient vector are established. The obstacle area is determined according to the boundary coordinates and spatial coverage of walls, cages and fixed facilities, and a priori topology and geometric map containing geometric information of fixed structure of venues, regional semantic information, continuous geometric constraint information and channel topology connection relationship is obtained.

6. The CAD pre-built map-assisted positioning and mapping method for large-scale farms according to claim 5, characterized in that, Step 5 specifically involves: Step 5.

1. Using the acquisition time of the current frame of LiDAR point cloud data as the time reference, align the timestamps of the inertial measurement unit (IMU) and wheel speed odometer data, and convert the LiDAR point cloud, IMU, and wheel speed odometer data to the robot body coordinate system; integrate the angular velocity and linear acceleration of the IMU between adjacent LiDAR point cloud frame acquisition times to obtain the robot's attitude increment and velocity increment, and integrate the wheel speed odometer data to obtain the displacement increment; Based on the robot's pose state in the unified field coordinate system at the previous moment, as well as the pose increment, velocity increment, and displacement increment, predict the robot's initial pose in the unified field coordinate system at the current moment. Centered on the initial pose, a local search range is determined according to a preset search distance. Combined with the prior topology and the site topology index in the geometric map, the fixed structural boundary of the site and its continuous geometric constraint information are extracted from the local search range to form a local geometric region. Step 5.

2. Using the current frame LiDAR point cloud data that has been transformed to the robot body coordinate system in Step 5.1, extract the point cloud position of the LiDAR point cloud data in the plane corresponding to the prior topology and geometric map; Based on the initial pose obtained in step 5.1, the positions of each point cloud are transformed into a unified field building coordinate system, and the corresponding nearest fixed structure boundary position and its unit normal vector are determined for each point cloud position within a local geometric region. The front-end matching residual is constructed based on the distance along the boundary normal direction between the transformed point cloud position and the corresponding nearest fixed structure boundary position. The robot's current pose estimation result is obtained by minimizing the weighted sum of squares of the front-end matching residual. Among them, the The front-end matching residual corresponding to each point cloud location is represented as follows: ； In the formula Indicates the first The front-end matching residual corresponding to each point cloud location; Indicates the robot in the The two-dimensional pose state at any given time. , and These represent the two position coordinates of the robot in the unified field coordinate system. Indicates the robot's heading angle; Indicates the heading angle A defined two-dimensional rotation matrix, Represents a two-dimensional translation vector; This indicates that the current frame of the LiDAR point cloud, after being transformed to the robot's body coordinate system in step 5.1, is located in the corresponding plane. Point cloud location, In the unified field coordinate system, the coordinates of the first... The point cloud location corresponds to the nearest fixed structural boundary of the site. This represents the unit normal vector corresponding to the boundary position of the nearest fixed structure of the site; The robot's current pose estimation result obtained through front-end matching. Represented as: ； In the formula, Indicates the first The weights of the front-end matching residuals. This indicates the number of point cloud locations involved in the front-end matching; Step 5.

3. Calculate the residuals of each front-end matching obtained in Step 5.2 relative to the robot's two-dimensional pose state. The Jacobian matrix is ​​obtained, and an approximate Hessian matrix is ​​constructed based on the Jacobian matrix and the corresponding residual weights; Among them, the first The approximate Hessian matrix at time t Represented as: ； In the formula For the first Front-end matching residual Compared to Jacobian matrix, To match step 5.2 The weights corresponding to the front-end matching residuals. The number of point cloud locations participating in the front-end matching; For approximate Hessian matrices The resulting eigenvalues, after eigenvalue decomposition and arranged in ascending order, are represented as follows: ； Among them, the eigenvector matrix Represented as: In the formula, The diagram is derived from the approximate Hessian matrix. The eigenvector matrix formed by the eigenvectors of each eigenvector, Indicates the relationship with the first eigenvalues The corresponding feature vector; Eigenvalue diagonal matrix Represented as: In the formula, This represents a diagonal matrix composed of eigenvalues. This indicates an operation that constructs a diagonal matrix from the elements within the parentheses; The feature values ​​are arranged in ascending order: ; The dimension representing the robot's two-dimensional pose state; for the robot's two-dimensional pose state defined in step 5.2 Its pose dimension ; Based on the minimum eigenvalue With the largest eigenvalue The relationship between the two is used to calculate the degree of geometric degradation of the scene, and the minimum eigenvalue is used to calculate the degree of geometric degradation of the scene. corresponding feature vector Determined as the direction of degradation, denoted as ; Among them, the The degree of scene geometric degradation at any given moment Represented as: ； In the formula, This represents a preset minimum positive number used to prevent the denominator from being zero.

7. The CAD pre-built map-assisted positioning and mapping method for large-scale farms according to claim 6, characterized in that, Step 6 specifically involves: Step 6.

1. Use the robot's two-dimensional pose state corresponding to each LiDAR point cloud frame as the pose node; determine the relative pose measurement of the laser odometry based on the robot's current pose estimation results obtained in Step 5, determine the inertial measurement unit pre-integration relative pose measurement based on the inertial measurement unit data between the acquisition times of adjacent LiDAR point cloud frames, and determine the relative pose measurement of the wheel speed odometry based on the wheel speed odometry data between the acquisition times of adjacent LiDAR point cloud frames; establish laser odometry constraints, inertial measurement unit pre-integration constraints, and wheel speed odometry constraints between adjacent pose nodes based on the relative pose measurement of the laser odometry, the relative pose measurement of the inertial measurement unit, and the relative pose measurement of the wheel speed odometry, respectively. Based on the prior topology and geometric map obtained in step 4 and the robot's current pose estimation result obtained in step 5, the robot's current pose estimation result obtained in step 5 is used as the CAD prior pose observation for the corresponding pose node. CAD geometric constraints are established between the pose node and the CAD prior pose observation to obtain a factor map containing the pose node and each constraint; wherein, the residuals of each relative pose constraint... Represented as: ； In the formula, L, I, and O represent the laser odometer, the inertial measurement unit, and the wheel speed odometer, respectively; This represents the set of pose states to be optimized. , and They represent the first Time to the The relative pose measurement of the laser odometry at any given time, the relative pose measurement of the inertial measurement unit pre-integration, and the relative pose measurement of the wheel speed odometry. This represents pose composition operations. This represents the position difference calculation; CAD geometric constraints residuals Represented as: ； Step 6.

2. Based on the scene geometry degradation degree obtained in Step 5 and the direction of degradation Calculate the first The CAD geometric constraint direction weights are corresponding to each pose node, and the information matrix of the CAD geometric constraints is adjusted according to the direction weights. The CAD geometric constraint direction weights and the adjusted information matrix are represented as follows: ； ； In the formula, Indicates the first The CAD geometric constraint direction weights corresponding to each pose node. and These represent the preset minimum and maximum directional weights, respectively. ; The matrix representing the basic information of CAD geometric constraints. Indicates the direction of degradation after normalization. This represents the direction projection matrix of the degradation direction in the robot's pose state space; Step 6.

3. Based on the factor graph constructed in Step 6.1 and the CAD geometric constraint information matrix obtained in Step 6.2, construct a multi-source constraint joint optimization problem, and obtain the optimized pose state set. Represented as: ； In the formula, , and These represent information matrices for laser odometer constraints, inertial measurement unit pre-integration constraints, and wheel speed odometer constraints, respectively. This indicates step 6.2 based on the... The CAD geometric constraint information matrix after adjusting the degree and direction of scene geometric degradation at any given time; for any residual vector ,have For each constraint factor residual in the factor graph, in the current pose state set Linearization is performed at each constraint factor, and the Jacobian matrix of each constraint factor is calculated separately. Normal equations are then constructed based on the residuals, Jacobian matrices, and information matrices of each constraint factor. ； In the formula, This represents the constraint factors in the factor graph. This represents the residual of the corresponding constraint factor at the current pose state set. The Jacobian matrix represents the residual of the constraint factor relative to the relevant pose state. This represents the information matrix corresponding to the constraint factor; The normal equation coefficient matrix, consisting of the Jacobian matrix and information matrix of each constraint factor, is used to characterize the comprehensive constraint strength of each constraint factor on the pose state increment. The gradient vector, composed of the residuals, Jacobian matrix, and information matrix of each constraint factor, is used to characterize the first-order change of the joint optimization objective relative to the pose state increment at the current pose state set; wherein, the residuals of the laser odometry constraint, inertial measurement unit pre-integration constraint, wheel speed odometry constraint, and CAD geometric constraint are determined by step 6.1, and the information matrix of the CAD geometric constraint is obtained by adjustment in step 6.2; Represents the set of pose states to be optimized The corresponding pose state increment is composed of the two-dimensional pose state increment corresponding to each pose node, that is: ; Among them, the The 2D pose state increment corresponding to each pose node is: ; Solving the normal equations yields the pose state increment. Based on the two-dimensional pose state increment corresponding to each pose node Update the corresponding pose nodes and recalculate the residuals and Jacobian matrices for each constraint factor; Repeat the above linearization, normal equation solving, and pose node update process until the position increment and heading angle increment of each pose node simultaneously satisfy the preset convergence condition; wherein, the preset convergence condition is expressed as: and ; In the formula, This indicates the preset position convergence threshold. This indicates the preset heading angle convergence threshold; when both of the above convergence conditions are met simultaneously, the factor graph optimization process converges, and the optimized global pose sequence is obtained. .

8. The CAD pre-built map-assisted positioning and mapping method for large-scale farms according to claim 7, characterized in that, Step 7 specifically involves: Step 7.

1. Based on the coordinate mapping relationship obtained in Step 1, the unified field coordinate system used by the prior topology and geometric map in Step 4 is determined as the global map coordinate system; based on the global pose sequence obtained in Step 6 and the external parameter transformation relationship between the lidar coordinate system and the robot body coordinate system in Step 5, the lidar point cloud data collected in Step 5 at each time moment is transformed from the lidar coordinate system to the robot body coordinate system and the global map coordinate system at the corresponding time moment. Among them, the The first time in the point cloud of lidar The coordinate transformation relationship of the point cloud positions is expressed as follows: ； In the formula, Indicates the first The first time in the point cloud of lidar The coordinates of each point cloud location in the lidar coordinate system This represents the extrinsic transformation matrix from the lidar coordinate system to the robot's body coordinate system. This indicates the result obtained from step 6. Optimize pose continuously The pose transformation matrix from the robot's body coordinate system to the global map coordinate system. This indicates the position of the point cloud after transformation to the global map coordinate system; When step 6 updates the optimized pose of the historical moment, the lidar point cloud data of the corresponding historical moment is transformed again according to the updated optimized pose to obtain point cloud data with global pose correction. Step 7.

2. Based on the farm topology index in the prior topology and geometric map in Step 4, establish associations between the globally pose-corrected point cloud data and its corresponding farm area, farm connection area, and topology nodes; divide the space in the global map coordinate system into multiple spatial index units according to the preset spatial resolution, and assign each point cloud position to the corresponding spatial index unit according to the coordinates of each point cloud position after global pose correction in the global map coordinate system to establish a point cloud spatial index; merge point cloud positions falling into the same spatial index unit and remove duplicate point cloud positions; fuse the processed point cloud data at each time step with the existing global map, and when the optimized pose at a historical time step is updated, replace the corresponding original point cloud position with the newly transformed point cloud position to obtain a global map that is globally consistent with the fixed structure of the farm; Step 7.

3. Determine the optimized pose at the latest moment in the global pose sequence obtained in Step 6 as the robot's real-time pose estimation result, whereby the real-time pose estimation result is expressed as: ; In the formula, This represents the real-time pose estimation result of the robot at time k. and These represent the robot's optimized position coordinates in the global map coordinate system. This represents the robot's optimized heading angle; When new lidar point cloud data, inertial measurement unit data, and wheel speed odometer data are collected, steps 5 and 6 are executed sequentially to update the robot's current pose estimation result and global pose sequence. Then, based on the updated global pose sequence, steps 7.1 and 7.2 are executed to perform coordinate transformation and fusion updates on the newly added point cloud data and historical point cloud data affected by the pose update, and output the updated global map and the robot's real-time pose estimation result.

9. A computer device, comprising a memory and one or more processors; characterized in that, The memory stores executable code, which, when executed by the processor, implements the steps of the CAD pre-mapping-assisted positioning and mapping method for large-scale farms as described in any one of claims 1 to 8.

10. A computer-readable storage medium having a program stored thereon; characterized in that, When the program is executed by the processor, it is used to implement the steps of the CAD pre-map assisted positioning and mapping method for large-scale farms as described in any one of claims 1 to 8.

Images