Outdoor combined laser positioning system and method based on AMCL and NDT

Abstract

The present invention relates to the field of robot navigation and positioning technology, and discloses an outdoor combined laser positioning system and method based on AMCL and NDT. The system includes: a point cloud processing module for preprocessing the original laser radar data of the radar sensor to obtain preprocessed data; a map processing module for loading and transforming the map to obtain the transformed map; a point cloud positioning module for performing initial positioning based on the AMCL algorithm and precise positioning based on the NDT algorithm according to the preprocessed data and the transformed map. In this way, an indoor robot that is not equipped with a satellite module can realize the outdoor point cloud positioning process, which is beneficial to the indoor and outdoor compatibility and cost reduction of platform robots.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation and positioning technology, and in particular to an outdoor combined laser positioning system and method based on AMD and NDT. Background Art

[0002] The application fields of laser point cloud-based positioning algorithms are wide, including but not limited to robot navigation, positioning and path planning of autonomous driving vehicles, scene construction of virtual reality and augmented reality, etc.

[0003] The laser point cloud positioning algorithm mainly relies on the information extracted from the point cloud data for positioning. The point cloud data includes two-dimensional point cloud and three-dimensional point cloud, which are mainly obtained by laser radar, depth camera or other 3D scanning equipment. These data contain the three-dimensional coordinate information of the surface points of each object in the scene; then the extracted feature points are matched with the known reference model or map, and the position and posture of the object or camera in space are determined by calculating the spatial relationship between the feature points;

[0004] In the laser point cloud positioning problem, when the map scene is large, the positioning algorithm needs to give a better initial positioning value: in outdoor application scenarios, this initial value is usually given by the satellite positioning module; in indoor scenes, the initial value is generally solved by matching positioning through visual means. Whether the initial value of point cloud positioning is appropriate is directly related to the positioning effect of the algorithm.

[0005] Existing laser point cloud positioning algorithms usually use satellite positioning to give matching initial values ​​in outdoor application scenarios. Some indoor unmanned platforms are generally not equipped with satellite modules and cannot be directly applied to outdoor scenarios; at the same time, when considering hardware reliability and cost control issues, indoor unmanned platforms will not be equipped with satellite modules. Summary of the invention

[0006] The invention provides an outdoor combined laser positioning system and method based on AMCL and NDT, which can solve the technical problems in the prior art.

[0007] The present invention provides an outdoor combined laser positioning system based on AMCL and NDT, wherein the system comprises:

[0008] The point cloud processing module is used to preprocess the raw lidar data of the radar sensor to obtain preprocessed data;

[0009] A map processing module is used to load and transform the map to obtain the transformed map;

[0010] The point cloud positioning module is used to perform initial positioning based on the amcl algorithm and precise positioning based on the ndt algorithm according to the preprocessed data and the transformed map.

[0011] Preferably, preprocessing the raw laser radar data of the radar sensor to obtain preprocessed data includes:

[0012] Perform radar protocol analysis on the original lidar data to obtain three-dimensional point cloud data;

[0013] Filter the ground point cloud from the 3D point cloud data to obtain ground-free point cloud data;

[0014] The point cloud data without ground is planarized to obtain two-dimensional point cloud data.

[0015] Preferably, loading and transforming the map to obtain the transformed map includes:

[0016] Load the 3D point cloud map file in PCD format and output the original 3D point cloud map;

[0017] Compress and transform the original 3D point cloud map into a 2D map.

[0018] Preferably, performing initial positioning based on the amcl algorithm includes:

[0019] S1, particle initialization: randomly generate particles according to the transformed map, each particle has an equal weight, and each particle represents a two-dimensional pose X = (x, y) Monte Carlo sampling;

[0020] S2, particle iteration: Estimate the pose at the next moment based on the motion model sample_motion_model, u is the system input; at the same time, according to the observation model measurement_model and the observation value z and the transformed two-dimensional map information m, update the weight of the particle's pose to obtain the updated weight w;

[0021] Among them, the posture at the next moment is estimated and the weight of the particle's posture is updated by the following formula:

[0022] X = sample_motion_model(u,X)

[0023] ω=measurement_model(z,X,m),

[0024] S3, particle update: Sample particles according to the updated weight w, and estimate w by long-term likelihood averaging slow , short-term likelihood average estimate w fast , long-term exponential filter attenuation rate α slowand the short-term exponential filter decay rate α fast Construct a criterion c to determine whether the correct particle is discarded. If the correct particle is discarded, randomly inject new particles to ensure the credibility of the group.

[0025] Among them, the criterion c for judging whether a particle is discarded is constructed by the following formula:

[0026]

[0027] ω slow =ω slow +α slow (ω avg -ω slow )

[0028] ω fast =ω fast +α fast (ω avg -ω fast )

[0029] c=1-ω fast / ω slow ,

[0030] S4, pose solution: Calculate the mean and variance of the updated particle swarm 2D pose. The mean is the best position estimate X p .

[0031] Preferably, accurate positioning based on the ndt algorithm includes:

[0032] S1, solve the NDT distribution of the original 3D point cloud map: divide the space into grids, calculate the mean q and covariance ∑ in each grid, and construct a normal distribution through the following formula, where xi is the i-th input point cloud information, p is the probability density distribution function of the NDT distribution, and n is the number of point clouds in the grid:

[0033]

[0034] S2, construct transformation relationship: For each point xi in the 3D point cloud data to be registered, the transformed point cloud position is solved as x` through the transformation matrix P = [R, T]. i =Rx i +T, where R is the rotation matrix and T is the translation matrix, and the distribution probability score and f of each point are solved by the following formula, where score is a function of the transformation matrix P,

[0035]

[0036] S3, calculate the gradient g, Jacobian matrix J and Hessian matrix H by the following formula, and estimate X according to the best position obtained by AMCL algorithm p , construct Newton's equation to solve the iteration direction Δp, and iteratively solve until the optimal transformation value is obtained as the optimal transformation matrix P of the current point cloud.

[0037]

[0038] HΔp=-g.

[0039] The present invention also provides an outdoor combined laser positioning method based on AMCL and NDT, wherein the method comprises:

[0040] Preprocessing the original lidar data of the radar sensor to obtain preprocessed data;

[0041] Load and transform the map to obtain the transformed map;

[0042] According to the preprocessed data and the transformed map, initial positioning is performed based on the amcl algorithm and precise positioning is performed based on the ndt algorithm.

[0043] Preferably, preprocessing the raw laser radar data of the radar sensor to obtain preprocessed data includes:

[0044] Perform radar protocol analysis on the original lidar data to obtain three-dimensional point cloud data;

[0045] Filter the ground point cloud from the 3D point cloud data to obtain ground-free point cloud data;

[0046] The point cloud data without ground is planarized to obtain two-dimensional point cloud data.

[0047] Preferably, loading and transforming the map to obtain the transformed map includes:

[0048] Load the 3D point cloud map file in PCD format and output the original 3D point cloud map;

[0049] Compress and transform the original 3D point cloud map into a 2D map.

[0050] Preferably, performing initial positioning based on the amcl algorithm includes:

[0051] S1, particle initialization: randomly generate particles according to the transformed map, each particle has an equal weight, and each particle represents a two-dimensional pose X = (x, y) Monte Carlo sampling;

[0052] S2, particle iteration: Estimate the pose at the next moment based on the motion model sample_motion_model, u is the system input; at the same time, according to the observation model measurement_model and the observation value z and the transformed two-dimensional map information m, update the weight of the particle's pose to obtain the updated weight w;

[0053] Among them, the posture at the next moment is estimated and the weight of the particle's posture is updated by the following formula:

[0054] X = sample_motion_model(u,X)

[0055] ω=measurement_model(z,X,m),

[0056] S3, particle update: Sample particles according to the updated weight w, and estimate w by long-term likelihood averaging slow , short-term likelihood average estimate w fast , long-term exponential filter attenuation rate α slow and the short-term exponential filter decay rate α fast Construct a criterion c to determine whether the correct particle is discarded. If the correct particle is discarded, randomly inject new particles to ensure the credibility of the group.

[0057] Among them, the criterion c for judging whether a particle is discarded is constructed by the following formula:

[0058]

[0059] ω slow =ω slow +α slow (ω avg -ω slow )

[0060] ω fast =ω fast +α fast (ω avg -ω fast )

[0061] c=1-ω fast / ω slow ,

[0062] S4, pose solution: Calculate the mean and variance of the updated particle swarm 2D pose. The mean is the best position estimate X p .

[0063] Preferably, accurate positioning based on the ndt algorithm includes:

[0064] S1, solve the NDT distribution of the original 3D point cloud map: divide the space into grids, calculate the mean q and covariance ∑ in each grid, and construct a normal distribution through the following formula, where xi is the i-th input point cloud information, p is the probability density distribution function of the NDT distribution, and n is the number of point clouds in the grid:

[0065]

[0066] S2, construct transformation relationship: For each point xi in the 3D point cloud data to be registered, the transformed point cloud position is solved as x` through the transformation matrix P = [R, T]. i =Rx i +T, where R is the rotation matrix and T is the translation matrix, and the distribution probability score and f of each point are solved by the following formula, where score is a function of the transformation matrix P,

[0067]

[0068] S3, calculate the gradient g, Jacobian matrix J and Hessian matrix H by the following formula, and estimate X according to the best position obtained by AMCL algorithm p , construct Newton's equation to solve the iteration direction Δp, and iteratively solve until the optimal transformation value is obtained as the optimal transformation matrix P of the current point cloud.

[0069]

[0070]

[0071] HΔp=-g.

[0072] Through the above technical solution, the AMCL algorithm is applied in the initial positioning stage to realize the initial positioning of the platform based on the reduced-dimensional two-dimensional map; the point cloud positioning method based on the NDT is adopted in the precise positioning stage to realize the precise positioning based on the three-dimensional point cloud. The present invention can enable indoor robots without satellite modules to realize the outdoor point cloud positioning process, which is beneficial to the indoor and outdoor compatibility and cost reduction of platform robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] The included drawings are used to provide a further understanding of the embodiments of the present invention, which constitute a part of the specification, are used to illustrate the embodiments of the present invention, and together with the text description, explain the principles of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0074] Figure 1 A block diagram of an outdoor combined laser positioning system based on AMCL and NDT according to an embodiment of the present invention is shown;

[0075] Figure 2 A schematic diagram of the operation flow of an outdoor combined laser positioning method based on AMCL and NDT according to an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0076] It should be noted that, in the absence of conflict, the embodiments in this application and the features in the embodiments can be combined with each other. The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0077] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0078] Unless otherwise specifically stated, the relative arrangement of the parts and steps described in these embodiments, numerical expressions and numerical values ​​do not limit the scope of the present invention. At the same time, it should be understood that, for ease of description, the sizes of the various parts shown in the accompanying drawings are not drawn according to the actual proportional relationship. The technology, method and equipment known to ordinary technicians in the relevant field may not be discussed in detail, but in appropriate cases, the technology, method and equipment should be regarded as a part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters represent similar items in the following drawings, so once a certain item is defined in an accompanying drawing, it does not need to be further discussed in subsequent drawings.

[0079] Figure 1 A block diagram of an outdoor combined laser positioning system based on AMCL and NDT according to an embodiment of the present invention is shown.

[0080] like Figure 1 As shown, an embodiment of the present invention provides an outdoor combined laser positioning system based on AMCL and NDT, wherein the system includes:

[0081] The point cloud processing module is used to preprocess the raw lidar data of the radar sensor to obtain preprocessed data;

[0082] A map processing module is used to load and transform the map to obtain the transformed map;

[0083] The point cloud positioning module is used to perform initial positioning (amcl coarse positioning) based on the amcl algorithm and precise positioning (ndt precise positioning) based on the ndt algorithm according to the preprocessed data and the transformed map.

[0084] Through the above technical solution, the AMCL algorithm is applied in the initial positioning stage to realize the initial positioning of the platform based on the reduced-dimensional two-dimensional map; the point cloud positioning method based on the NDT is adopted in the precise positioning stage to realize the precise positioning based on the three-dimensional point cloud. The present invention can enable indoor robots without satellite modules to realize the outdoor point cloud positioning process, which is beneficial to the indoor and outdoor compatibility and cost reduction of platform robots.

[0085] According to an embodiment of the present invention, preprocessing the original laser radar data of the radar sensor to obtain the preprocessed data includes:

[0086] Perform radar protocol analysis on the original lidar data to obtain three-dimensional point cloud data;

[0087] Filter the ground point cloud from the 3D point cloud data to obtain ground-free point cloud data;

[0088] The point cloud data without ground is planarized to obtain two-dimensional point cloud data.

[0089] That is, the point cloud processing module realizes the analysis and preprocessing functions of radar sensor information, which specifically includes radar protocol analysis, ground point cloud filtering and point cloud planarization sub-modules.

[0090] According to an embodiment of the present invention, loading and transforming a map to obtain a transformed map includes:

[0091] Load the 3D point cloud map file in PCD format and output the original 3D point cloud map;

[0092] Compress and transform the original 3D point cloud map into a 2D map.

[0093] That is, the map processing module implements map loading and map transformation functions, and specifically includes two-dimensional and three-dimensional map loading and three-dimensional map transformation submodules.

[0094] According to an embodiment of the present invention, performing initial positioning based on the amcl algorithm includes:

[0095] S1, particle initialization: randomly generate particles according to the transformed map, each particle has an equal weight, and each particle represents a two-dimensional pose X = (x, y) Monte Carlo sampling;

[0096] S2, particle iteration: Estimate the pose at the next moment based on the motion model sample_motion_model, u is the system input; at the same time, according to the observation model measurement_model and the observation value z and the transformed two-dimensional map information m, update the weight of the particle's pose to obtain the updated weight w;

[0097] Among them, the posture at the next moment is estimated and the weight of the particle's posture is updated by the following formula:

[0098] X = sample_motion_model(u,X)

[0099] ω=measurement_model(z,X,m),

[0100] S3, particle update: Sample particles according to the updated weight w, and estimate w by long-term likelihood averaging slow , short-term likelihood average estimate w fast , long-term exponential filter attenuation rate α slow and the short-term exponential filter decay rate α fast Construct a criterion c for judging whether the correct particle is discarded, and judge whether the correct particle is discarded according to the constructed criterion c. If the correct particle is discarded, randomly inject new particles to ensure the credibility of the group (that is, remove particles with low credibility and add new particles: for the removed particles, it can be judged according to the criterion c whether the correct particle is discarded by mistake, and randomly inject new particles if the correct particle is discarded by mistake);

[0101] Among them, the criterion c for judging whether a particle is discarded is constructed by the following formula:

[0102]

[0103] ω slow =ω slow +α slow (ω avg -ω slow )

[0104] ω fast =ω fast +α fast (ω avg -ω fast )

[0105] c=1-ω fast / ωslow ,

[0106] S4, pose solution: Calculate the mean and variance of the updated particle swarm 2D pose. The mean is the best position estimate X p .

[0107] By calculating the variance, the convergence of the particle swarm can be reflected.

[0108] According to an embodiment of the present invention, accurate positioning based on the ndt algorithm includes:

[0109] S1, solve the NDT distribution of the original 3D point cloud map: divide the space into grids, calculate the mean q and covariance ∑ in each grid, and construct a normal distribution through the following formula, where xi is the i-th input point cloud information, p is the probability density distribution function of the NDT distribution, and n is the number of point clouds in the grid:

[0110]

[0111] S2, construct transformation relationship: For each point xi in the 3D point cloud data to be registered, the transformed point cloud position is solved as x` through the transformation matrix P = [R, T]. i =Rx i +T, where R is the rotation matrix and T is the translation matrix, and the distribution probability score and f of each point are solved by the following formula, where score is a function of the transformation matrix P,

[0112]

[0113] S3, calculate the gradient g, Jacobian matrix J and Hessian matrix H by the following formula, and estimate X according to the best position obtained by AMCL algorithm p , construct Newton's equation to solve the iteration direction Δp, and iteratively solve until the optimal transformation value is obtained as the optimal transformation matrix P of the current point cloud.

[0114]

[0115] HΔp=-g.

[0116] like Figure 2 As shown, an embodiment of the present invention provides an outdoor combined laser positioning method based on AMCL and NDT, wherein the method includes:

[0117] Preprocessing the original lidar data of the radar sensor to obtain preprocessed data;

[0118] Load and transform the map to obtain the transformed map;

[0119] According to the preprocessed data and the transformed map, initial positioning is performed based on the amcl algorithm and precise positioning is performed based on the ndt algorithm.

[0120] Through the above technical solution, the AMCL algorithm is applied in the initial positioning stage to realize the initial positioning of the platform based on the reduced-dimensional two-dimensional map; the point cloud positioning method based on the NDT is adopted in the precise positioning stage to realize the precise positioning based on the three-dimensional point cloud. The present invention can enable indoor robots without satellite modules to realize the outdoor point cloud positioning process, which is beneficial to the indoor and outdoor compatibility and cost reduction of platform robots.

[0121] According to an embodiment of the present invention, preprocessing the original laser radar data of the radar sensor to obtain the preprocessed data includes:

[0122] Perform radar protocol analysis on the original lidar data to obtain three-dimensional point cloud data;

[0123] Filter the ground point cloud from the 3D point cloud data to obtain ground-free point cloud data;

[0124] The point cloud data without ground is planarized to obtain two-dimensional point cloud data.

[0125] According to an embodiment of the present invention, loading and transforming a map to obtain a transformed map includes:

[0126] Load the 3D point cloud map file in PCD format and output the original 3D point cloud map;

[0127] Compress and transform the original 3D point cloud map into a 2D map.

[0128] Specifically, continue to refer to Figure 2 The system input is 3D point cloud / points_raw and 3D point cloud map / map_3d file, and the system output is the platform in the map coordinate system / pose. The input and output of each module in the intermediate process and the implementation principle are as follows:

[0129] Point cloud processing process: read the original laser radar data / points_raw (raw radar information), parse the point cloud data in the process, and output standard three-dimensional point cloud data / clouds; filter out the ground information output and output no-ground point cloud / clouds_noground; flatten the point cloud information into two-dimensional information for initial positioning, and output two-dimensional point cloud / laser.

[0130] Map processing process: read the 3D point cloud map file in pcd format, output the original 3D point cloud map / map_3d in the process; compress the 3D point cloud map into a 2D map / map_2d.

[0131] Point cloud positioning process: including amcl rough positioning process and ndt precise positioning process:

[0132] The amcl algorithm refers to Adaptive Monte Carlo Localization. The input of the amcl rough positioning process includes a two-dimensional map / map_2d, a two-dimensional point cloud / laser, and a system input u. The output is a rough two-dimensional pose X. p , the specific matching solution process is described as follows.

[0133] According to an embodiment of the present invention, performing initial positioning based on the amcl algorithm includes:

[0134] S1, particle initialization: randomly generate particles according to the transformed map (two-dimensional map), each particle has an equal weight, and each particle represents a two-dimensional pose X = (x, y) Monte Carlo sampling;

[0135] S2, particle iteration: Estimate the pose at the next moment based on the motion model sample_motion_model, u is the system input; at the same time, according to the observation model measurement_model and the observation value z (the observation value comes from the two-dimensional point cloud / laser) and the transformed two-dimensional map information m, update the weight of the particle's pose to obtain the updated weight w;

[0136] Among them, the posture at the next moment is estimated and the weight of the particle's posture is updated by the following formula:

[0137] X = sample_motion_model(u,X)

[0138] ω=measurement_model(z,X,m),

[0139] S3, particle update: Sample particles according to the updated weight w, and estimate w by long-term likelihood averaging slow , short-term likelihood average estimate w fast , long-term exponential filter attenuation rate α slow and the short-term exponential filter decay rate α fast Construct a criterion c to determine whether the correct particle is discarded. If the correct particle is discarded, randomly inject new particles to ensure the credibility of the group.

[0140] Among them, the criterion c for judging whether a particle is discarded is constructed by the following formula:

[0141]

[0142] ω slow =ωslow +α slow (ω avg -ω slow )

[0143] ω fast =ω fast +α fast (ω avg -ω fast )

[0144] c=1-ω fast / ω slow ,

[0145] S4, pose solution: Calculate the mean and variance of the updated particle swarm 2D pose. The mean is the best position estimate X p .

[0146] The Ndt algorithm refers to the normal distribution transform algorithm Ndt (Normal Distribution Transform). The input of the precise positioning process includes the two-dimensional pose X p , M3 = three-dimensional point cloud map / map_3d and X3d = standard three-dimensional point cloud data / clouds, the output is the precise transformation matrix (optimal transformation matrix) P, and the specific matching solution process is described as follows.

[0147] According to an embodiment of the present invention, accurate positioning based on the ndt algorithm includes:

[0148] S1, solve the NDT distribution of the original 3D point cloud map: divide the space into grids, calculate the mean q and covariance ∑ in each grid, and construct a normal distribution through the following formula, where xi is the i-th input point cloud information, p is the probability density distribution function of the NDT distribution, and n is the number of point clouds in the grid:

[0149]

[0150] S2, construct transformation relationship: For each point x in the 3D point cloud data to be registered i , through the transformation matrix P = [R, T], the transformed point cloud position is solved as x` i =Rx i +T, where R is the rotation matrix and T is the translation matrix, and the distribution probability score and f of each point are solved by the following formula, where score is a function of the transformation matrix P,

[0151]

[0152] S3, calculate the gradient g, Jacobian matrix J and Hessian matrix H by the following formula, and estimate X according to the best position obtained by AMCL algorithm p, construct Newton's equation to solve the iteration direction Δp, and iteratively solve until the optimal transformation value is obtained as the optimal transformation matrix P of the current point cloud.

[0153]

[0154] HΔp=-g.

[0155] Among them, g i represents the i-th row in the gradient, J i represents the i-th row in the Jacobian matrix, H ij represents the i-th row and j-th column of the Hessian matrix H.

[0156] In the description of the present invention, it is necessary to understand that the directions or positional relationships indicated by directional words such as "front, back, up, down, left, right", "lateral, vertical, perpendicular, horizontal" and "top, bottom" are usually based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description. Unless otherwise specified, these directional words do not indicate or imply that the devices or elements referred to must have a specific direction or be constructed and operated in a specific direction. Therefore, they cannot be understood as limiting the scope of protection of the present invention. The directional words "inside and outside" refer to the inside and outside relative to the contours of each component itself.

[0157] For ease of description, spatially relative terms such as "above", "above", "on the upper surface of", "above", etc. may be used here to describe the spatial positional relationship between a device or feature and other devices or features as shown in the figure. It should be understood that spatially relative terms are intended to include different orientations of the device in use or operation in addition to the orientation described in the figure. For example, if the device in the accompanying drawings is inverted, the device described as "above other devices or structures" or "above other devices or structures" will be positioned as "below other devices or structures" or "below other devices or structures". Thus, the exemplary term "above" can include both "above" and "below". The device can also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatially relative descriptions used here are interpreted accordingly.

[0158] In addition, it should be noted that the use of terms such as "first" and "second" to limit components is only for the convenience of distinguishing the corresponding components. If not otherwise stated, the above terms have no special meaning and therefore cannot be understood as limiting the scope of protection of the present invention.

[0159] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An outdoor combined laser positioning system based on AMCL and NDT, characterized in that: The system includes: The point cloud processing module is used to preprocess the raw lidar data of the radar sensor to obtain preprocessed data; A map processing module is used to load and transform the map to obtain the transformed map; The point cloud positioning module is used to perform initial positioning based on the amcl algorithm and precise positioning based on the ndt algorithm according to the preprocessed data and the transformed map.

2. The system according to claim 1, characterized in that The raw lidar data of the radar sensor is preprocessed to obtain the preprocessed data including: Perform radar protocol analysis on the original lidar data to obtain three-dimensional point cloud data; Filter the ground point cloud from the 3D point cloud data to obtain ground-free point cloud data; The point cloud data without ground is planarized to obtain two-dimensional point cloud data.

3. The system according to claim 2, characterized in that Loading and transforming the map to obtain the transformed map includes: Load the 3D point cloud map file in PCD format and output the original 3D point cloud map; Compress and transform the original 3D point cloud map into a 2D map.

4. The system according to claim 3, characterized in that Initial positioning based on the amcl algorithm includes: S1, particle initialization: randomly generate particles according to the transformed map, each particle has an equal weight, and each particle represents a two-dimensional pose X = (x, y) Monte Carlo sampling; S2, particle iteration: Estimate the pose at the next moment based on the motion model sample_motion_model, u is the system input; at the same time, according to the observation model measurement_model and the observation value z and the transformed two-dimensional map information m, update the weight of the particle's pose to obtain the updated weight w; Among them, the posture at the next moment is estimated and the weight of the particle's posture is updated by the following formula: X = sample_motion_model(u,X) ω=measurement_model(z,X,m), S3, particle update: Sample particles according to the updated weight w, and estimate w by long-term likelihood averaging slow , short-term likelihood average estimate w fast , long-term exponential filter attenuation rate α slow and the short-term exponential filter decay rate α fast Construct a criterion c to determine whether the correct particle is discarded. Determine whether the correct particle is discarded based on the constructed criterion c. If the correct particle is discarded, randomly inject new particles to ensure the credibility of the group. Among them, the criterion c for judging whether a particle is discarded is constructed by the following formula: oh slow =ω slow +a slow (oh avg -oh slow ) oh fast =ω fast +a fast (oh avg -oh fast ) c=1-ω fast / h slow , S4, pose solution: Calculate the mean and variance of the updated particle swarm 2D pose. The mean is the best position estimate X p .

5. The system according to claim 4, characterized in that Precise positioning based on the NDT algorithm includes: S1, solve the NDT distribution of the original 3D point cloud map: divide the space into grids, calculate the mean q and covariance ∑ in each grid, and construct a normal distribution by the following formula, where x i is the i-th input point cloud information, p is the probability density distribution function of NDT distribution, and n is the number of point clouds in the grid: S2, construct transformation relationship: For each point x in the 3D point cloud data to be registered i , through the transformation matrix P = [R, T], the transformed point cloud position is solved as x `i =Rx i +T, where R is the rotation matrix and T is the translation matrix, and the distribution probability score and f of each point are solved by the following formula, where score is a function of the transformation matrix P, S3, calculate the gradient g, Jacobian matrix J and Hessian matrix H by the following formula, and estimate X according to the best position obtained by AMCL algorithm p , construct Newton's equation to solve the iteration direction Δp, and iteratively solve until the optimal transformation value is obtained as the optimal transformation matrix P of the current point cloud. HΔp=-g.

6. An outdoor combined laser positioning method based on AMCL and NDT, characterized in that: The method comprises: preprocessing original laser radar data of a radar sensor to obtain preprocessed data; Load and transform the map to obtain the transformed map; According to the preprocessed data and the transformed map, initial positioning is performed based on the amcl algorithm and precise positioning is performed based on the ndt algorithm.

7. The method according to claim 6, characterized in that The raw lidar data of the radar sensor is preprocessed to obtain the preprocessed data including: Perform radar protocol analysis on the original lidar data to obtain three-dimensional point cloud data; Filter the ground point cloud from the 3D point cloud data to obtain ground-free point cloud data; The point cloud data without ground is planarized to obtain two-dimensional point cloud data.

8. The method according to claim 7, characterized in that Loading and transforming the map to obtain the transformed map includes: Load the 3D point cloud map file in PCD format and output the original 3D point cloud map; Compress and transform the original 3D point cloud map into a 2D map.

9. The method according to claim 8, characterized in that Initial positioning based on the amcl algorithm includes: S1, particle initialization: randomly generate particles according to the transformed map, each particle has an equal weight, and each particle represents a two-dimensional pose X = (x, y) Monte Carlo sampling; S2, particle iteration: Estimate the pose at the next moment based on the motion model sample_motion_model, u is the system input; at the same time, according to the observation model measurement_model and the observation value z and the transformed two-dimensional map information m, update the weight of the particle's pose to obtain the updated weight w; Among them, the posture at the next moment is estimated and the weight of the particle's posture is updated by the following formula: X = sample_motion_model(u,X) ω=measurement_model(z,X,m), S3, particle update: Sample particles according to the updated weight w, and estimate w by long-term likelihood averaging slow , short-term likelihood average estimate w fast , long-term exponential filter attenuation rate α slow and the short-term exponential filter decay rate α fast Construct a criterion c to determine whether the correct particle is discarded. Determine whether the correct particle is discarded based on the constructed criterion c. If the correct particle is discarded, randomly inject new particles to ensure the credibility of the group. Among them, the criterion c for judging whether a particle is discarded is constructed by the following formula: oh slow =ω slow +a slow (oh avg -oh slow ) oh fast =ω fast +a fast (oh avg -oh fast ) c=1-ω fast / h slow , S4, pose solution: Calculate the mean and variance of the updated particle swarm 2D pose. The mean is the best position estimate X p .

10. The method according to claim 9, characterized in that Precise positioning based on the NDT algorithm includes: S1, solve the NDT distribution of the original 3D point cloud map: divide the space into grids, calculate the mean q and covariance ∑ in each grid, and construct a normal distribution by the following formula, where x i is the i-th input point cloud information, p is the probability density distribution function of NDT distribution, and n is the number of point clouds in the grid: S2, construct transformation relationship: For each point x in the 3D point cloud data to be registered i , through the transformation matrix P = [R, T], the transformed point cloud position is solved as x` i =Rx i +T, where R is the rotation matrix and T is the translation matrix, and the distribution probability score and f of each point are solved by the following formula, where score is a function of the transformation matrix P, S3, calculate the gradient g, Jacobian matrix J and Hessian matrix H by the following formula, and estimate X according to the best position obtained by AMCL algorithm p , construct Newton's equation to solve the iteration direction Δp, and iteratively solve until the optimal transformation value is obtained as the optimal transformation matrix P of the current point cloud. HΔp=-g.

Images