CN118154676A - Scene positioning method and system based on laser radar - Google Patents

Abstract

The invention provides a scene positioning method and a scene positioning system based on a laser radar, which belong to the technical field of robot robust positioning, and the method comprises the following steps: determining a local map according to the prior map point cloud of the target scene; dividing first point cloud voxels in a local map, storing the divided first point cloud voxels as first voxel lattices, and calculating a first mean value and a first variance of each first voxel lattice; dividing real-time point cloud voxels, storing the real-time point cloud voxels into a second voxel grid, and calculating a second mean value and a second variance of the second voxel grid; determining the mean value of the local map to be registered according to the first mean value and the second mean value; determining variances of the local maps to be registered according to the first variances and the second variances, and finishing incremental updating; and positioning the target point cloud according to the position of the voxel grid in the local map to be registered after the increment updating of the target point cloud. Based on the method, a scene positioning system based on the laser radar is also provided. The invention improves the accuracy and the robustness of the positioning system in complex and changeable environments.

Description

Scene positioning method and system based on laser radar

Technical Field

The invention belongs to the technical field of robot robust positioning, and particularly relates to a scene positioning method and system based on a laser radar.

Background

In the field of mobile robots, laser radar positioning is a common positioning mode, and a laser radar obtains distance information of surrounding environments to generate point clouds. However, in practical application, the local environments of factories and power plants often have the phenomena of goods shelf placement replacement, idle vehicle stopping randomly, unstable point cloud registration observation information of the prior map, registration failure and the like. This is a significant challenge for long-term operation of robots in complex and diverse scenarios of factories, substations, etc. At the same time, the consistency of multiple vehicles using the same prior map for multiple vehicles also deviates due to environmental changes.

The traditional laser radar positioning is divided into the following two steps of drawing and positioning, wherein the drawing is constructed by taking first frame point cloud data as a local map, new key frames are continuously acquired according to the robot gesture obtained by carrying out point cloud registration on the current point cloud and the local map, and then all key frames are corrected and spliced finally through observation of fixed gestures such as loop detection, so that a stable and reliable prior map is obtained. Although this is also a localization process, according to his procedure, it is known that there is a cumulative error in the pose during the construction process, and a back-end optimization is required to correct the error of the entire pose to obtain an accurate a priori map. So this is essentially an off-line operation and does not meet the high precision positioning requirements of the robot during actual operation. And the second step of positioning is to perform point cloud registration based on the prior map to obtain the real-time gesture of the robot. The inspection route of robot operation and related business demands such as charging, picking up goods, preventing goods and the like can be marked on the prior inspection map. The fixed prior map is the first step in the robotic system and is also performed substantially only once. However, the operation scene is not always consistent with the prior map in actual situations, and long-term stable positioning of the laser radar is difficult.

Disclosure of Invention

In order to solve the technical problems, the invention provides a scene positioning method and a scene positioning system based on a laser radar, which are used for incrementally updating voxel information of a local map to be registered in a point cloud registration process according to real-time scene change, so that the accuracy and the robustness of the positioning system in a complex and changeable environment are improved.

In order to achieve the above purpose, the present invention adopts the following technical scheme:

a scene positioning method based on a laser radar comprises the following steps:

Determining a local map of the target scene according to the prior map point cloud of the target scene; performing voxel division on a first point cloud in a local map, storing the first point cloud as first voxel lattices, and calculating a first mean value and a first variance covered by each first voxel lattice;

Performing voxel division on the acquired real-time point cloud, storing the real-time point cloud as a second voxel grid by using a Hash table, and calculating a second mean value and a second variance covered by the second voxel grid; determining the mean value of the local map to be registered according to the first mean value and the second mean value; determining the variance of the local map to be registered according to the first variance and the second variance, and finishing incremental updating of the local map to be registered;

and in the local map to be registered, which is updated in an increment, positioning the target point cloud according to the position of the voxel grid of the target point cloud in the local map to be registered.

Further, the process of determining the local map of the target scene according to the prior map point cloud of the target scene comprises the following steps:

Acquiring a priori map point cloud of a target scene;

acquiring a specific position of a current robot prior inspection map through repositioning;

And filtering the prior map in the coordinate axis direction on the initial position of the prior map where the current robot is positioned to obtain the local map.

Further, the filtering the prior map in the coordinate axis direction at the initial position where the current robot is located on the prior map to obtain the local map specifically includes: and obtaining a local map of the target scene through the straight-through filters of positive and negative preset ranges for three directions of the prior map xyz on the initial position of the prior map of the current robot.

Further, the voxel classifying process of the first point cloud in the local map includes:

Dividing a first point cloud in a local map by 3dbox voxels of 1x1x1 m; the method comprises the following steps:

；

Wherein, Index for a first point cloud in the local map; /(I)For/>First voxel lattices corresponding to the first point clouds; /(I)Is the total number of first point clouds; /(I)An abscissa after dividing the first point cloud voxels; /(I)An ordinate divided for the first point cloud; /(I)A vertical coordinate divided for the first point cloud voxel; /(I)For/>Rounding; For/> Rounding; /(I)For/>Rounding;

the process of storing the first voxel grid includes:

storing first voxel lattices generated by each first point cloud by using a Hash table;

。

Further, the process of calculating the first mean and the first variance covered by each first voxel grid includes: calculating a first mean value and a first variance covered by each first voxel grid in the Hash table;

the calculation formula of the first mean value is:

；

；

The first variance is calculated as:

Wherein, A voxel mean value of the first point cloud; /(I)The number of first voxel lattices; /(I)For/>Corresponding stored index sequences in the first voxel lattices; /(I)For/>A total number of point clouds in the first voxel grid; /(I)For/>The abscissa value of the first voxel grid; /(I)For/>Ordinate values of the first voxel grids; /(I)For/>A vertical coordinate value of the first voxel grid; Is the voxel variance of the first point cloud.

Further, the process of storing the obtained real-time point cloud as the second voxel lattice by using the Hash table after voxel division includes:

Dividing the real-time point cloud by 3dbox voxels of 1x1x1 m; the method comprises the following steps:

；

Storing a second voxel grid generated by each real-time point cloud by using a Hash table;

；

Wherein, Index for real-time point cloud; /(I)For/>Second voxel lattices corresponding to the second point clouds; /(I)The total number of the real-time point clouds; /(I)An abscissa after dividing the second point cloud voxels; /(I)An ordinate divided for the second point cloud; /(I)A vertical coordinate divided for the second point cloud voxel; /(I)For/>Rounding; /(I)Is thatRounding; /(I)For/>And (5) rounding.

Further, the process of calculating the second mean and the second variance covered by the second voxel grid includes:

the calculation formula of the second mean value is:

；

；/>；

The second variance is calculated as:

Wherein, A voxel mean value of the second point cloud; /(I)The number of second voxel lattices; /(I)For/>A corresponding stored index sequence in a second voxel grid; /(I)For/>Total number of point clouds in the second voxel grid; /(I)For/>The abscissa value of the second voxel grid; /(I)Is the firstOrdinate values of the second voxel grids; /(I)For/>A vertical coordinate value of the second voxel grid; is the voxel variance of the second point cloud.

Further, determining a mean value of the local map to be registered according to the first mean value and the second mean value; the process of determining the variance of the local map to be registered from the first variance and the second variance comprises:

the mean value of the local map to be registered is as follows:

；

the variance of the local map to be registered is:

；

Wherein, The mean value of the local map to be registered is obtained; /(I)Is the variance of the local map to be registered.

Further, in the local map to be registered, in which the incremental updating is completed, the process of positioning the target point cloud according to the position of the target voxel grid in the local map to be registered includes:

Determining the position of a target voxel grid of the target point cloud in the local map to be registered according to the current predicted gesture of the target point cloud;

If voxel grids generated by the local map to be registered exist around the target voxel grids, the mean value and the variance of the local map to be registered are obtained, and Gaussian Newton least square expansion is carried out;

and judging whether the increment from the target point cloud to the nearest neighbor voxel of the local map to be registered meets an increment threshold, and if so, outputting a positioning result.

The invention also provides a scene positioning system based on the laser radar, which comprises a first calculation module, a second calculation module and a positioning module;

The first calculation module is used for determining a local map of the target scene according to the prior map point cloud of the target scene; performing voxel division on a first point cloud in a local map, storing the first point cloud as first voxel lattices, and calculating a first mean value and a first variance covered by each first voxel lattice;

The second calculation module is used for storing the acquired real-time point cloud as a second voxel grid by using a Hash table after voxel division, and calculating a second mean value and a second variance covered by the second voxel grid; determining the mean value of the local map to be registered according to the first mean value and the second mean value; determining the variance of the local map to be registered according to the first variance and the second variance, and finishing incremental updating of the local map to be registered;

The positioning module is used for positioning the target point cloud according to the position of the voxel grid in the local map to be registered, wherein the incremental updating of the local map to be registered is completed.

The effects provided in the summary of the invention are merely effects of embodiments, not all effects of the invention, and one of the above technical solutions has the following advantages or beneficial effects:

The invention provides a scene positioning method and a scene positioning system based on a laser radar, which belong to the technical field of robot robust positioning, and the method comprises the following steps: determining a local map of the target scene according to the prior map point cloud of the target scene; performing voxel division on a first point cloud in a local map, storing the first point cloud as first voxel lattices, and calculating a first mean value and a first variance covered by each first voxel lattice; performing voxel division on the acquired real-time point cloud, storing the real-time point cloud as a second voxel grid by using a Hash table, and calculating a second mean value and a second variance covered by the second voxel grid; determining the mean value of the local map to be registered according to the first mean value and the second mean value; determining the variance of the local map to be registered according to the first variance and the second variance, and finishing incremental updating of the local map to be registered; and in the local map to be registered, which is updated in an increment, positioning the target point cloud according to the position of the voxel grid of the target point cloud in the local map to be registered. Based on a scene positioning method based on the laser radar, a scene positioning system based on the laser radar is also provided. According to the invention, a point cloud incremental updating mechanism of the current point cloud and the prior map is established under the condition of more scene changes of factories, power plants and the like, the point cloud registration is performed according to the voxel values of the local map to be registered which are updated in an incremental mode according to the real-time scene changes, and the accuracy and the robustness of the positioning system under complex and changeable environments are improved.

Drawings

Fig. 1 is a flowchart of a scene positioning method based on a laser radar according to embodiment 1 of the present invention;

fig. 2 is a schematic diagram of a priori map effect according to embodiment 1 of the present invention;

fig. 3 is a schematic view of a real-time point cloud map effect according to embodiment 1 of the present invention;

FIG. 4 is a schematic view of a registered local map effect according to embodiment 1 of the present invention;

fig. 5 is a schematic diagram of a scene positioning system based on a laser radar according to embodiment 2 of the present invention.

Detailed Description

In order to clearly illustrate the technical features of the present solution, the present invention will be described in detail below with reference to the following detailed description and the accompanying drawings. The following disclosure provides many different embodiments, or examples, for implementing different structures of the invention. In order to simplify the present disclosure, components and arrangements of specific examples are described below. Furthermore, the present invention may repeat reference numerals and/or letters in the various examples. This repetition is for the purpose of simplicity and clarity and does not in itself dictate a relationship between the various embodiments and/or configurations discussed. It should be noted that the components illustrated in the figures are not necessarily drawn to scale. Descriptions of well-known components and processing techniques and processes are omitted so as to not unnecessarily obscure the present invention.

Example 1

The embodiment 1 of the invention provides a scene positioning method based on a laser radar, and particularly relates to a positioning method under a scene, which is maintained by applying a priori map constructed by the laser radar and incrementally updating voxels of a local map to be registered in combination with a real-time point cloud under the condition of more scene changes such as factories, variable electric fields and the like.

The scene positioning method based on the laser radar provided by the embodiment 1 of the invention comprises the following steps:

Determining a local map of the target scene according to the prior map point cloud of the target scene; performing voxel division on a first point cloud in a local map, storing the first point cloud as first voxel lattices, and calculating a first mean value and a first variance covered by each first voxel lattice;

Performing voxel division on the acquired real-time point cloud, storing the real-time point cloud as a second voxel grid by using a Hash table, and calculating a second mean value and a second variance covered by the second voxel grid; determining the mean value of the local map to be registered according to the first mean value and the second mean value; determining the variance of the local map to be registered according to the first variance and the second variance, and finishing incremental updating of the local map to be registered;

and in the local map to be registered, which is updated in an increment, positioning the target point cloud according to the position of the voxel grid of the target point cloud in the local map to be registered.

Fig. 1 is a flowchart of a scene positioning method based on a laser radar according to embodiment 1 of the present invention;

In step S100, a priori map point clouds are acquired. According to the prior map point cloud of the target scene, the method comprises the steps of determining the prior map point cloud of the target scene;

in step S101, repositioning is performed. According to the prior map point cloud of the target scene, the local map of the target scene is determined. And acquiring the specific position of the current robot prior map through repositioning according to the prior map point cloud of the target scene.

In step S102, a priori map is straight-through, voxel filtering. According to the invention, the prior map is filtered in the coordinate axis direction on the initial position of the prior map where the current robot is located, so that the local map is obtained.

And carrying out xyz three directions on the prior map at the given initial position, and carrying out a direct filter of plus or minus 50m to obtain the local map.

In step S103, a hash point cloud lattice list of point clouds to be configured is constructed. And carrying out voxel division and storage on the first point cloud in the local map as a first voxel grid.

Dividing a first point cloud in a local map by 3dbox voxels of 1x1x1 m; the method comprises the following steps:

；

Wherein, Index for a first point cloud in the local map; /(I)For/>First voxel lattices corresponding to the first point clouds; /(I)Is the total number of first point clouds; /(I)An abscissa after dividing the first point cloud voxels; /(I)An ordinate divided for the first point cloud; /(I)A vertical coordinate divided for the first point cloud voxel; /(I)For/>Rounding; For/> Rounding; /(I)For/>Rounding;

the process of storing the first voxel grid includes:

storing first voxel lattices generated by each first point cloud by using a Hash table;

。

the above formula combines point clouds belonging to the same voxel grid.

In step S104, the mean and variance of the hash voxel grid of the point cloud to be configured are updated. A first mean and a first variance covered by each first voxel grid are calculated.

Calculating a first mean value and a first variance covered by each first voxel grid in the Hash table;

the calculation formula of the first mean value is:

；

；

The first variance is calculated as:

Wherein, A voxel mean value of the first point cloud; /(I)The number of first voxel lattices; /(I)For/>Corresponding stored index sequences in the first voxel lattices; /(I)For/>A total number of point clouds in the first voxel grid; /(I)For/>The abscissa value of the first voxel grid; /(I)Is the firstOrdinate values of the first voxel grids; /(I)For/>A vertical coordinate value of the first voxel grid; Is the voxel variance of the first point cloud.

In step S105, a current real-time point cloud is acquired.

In step S106, the current point cloud is straight-through, voxel filtering. The current real-time point cloud in the invention carries out a direct-pass filter of plus or minus 50 m.

In step S107, the acquired real-time point cloud is voxel-divided and then stored as a second voxel lattice using a Hash table, and a second mean and a second variance covered by the second voxel lattice are calculated.

Dividing the real-time point cloud by 3dbox voxels of 1x1x1 m; the method comprises the following steps:

；

Storing a second voxel grid generated by each real-time point cloud by using a Hash table;

；

Wherein, Index for real-time point cloud; /(I)For/>Second voxel lattices corresponding to the second point clouds; /(I)The total number of the real-time point clouds; /(I)An abscissa after dividing the second point cloud voxels; /(I)An ordinate divided for the second point cloud; /(I)A vertical coordinate divided for the second point cloud voxel; /(I)For/>Rounding; /(I)Is thatRounding; /(I)For/>And (5) rounding.

The calculation formula of the second mean value is:

；

；/>；

The second variance is calculated as:

Wherein, A voxel mean value of the second point cloud; /(I)The number of second voxel lattices; /(I)For/>A corresponding stored index sequence in a second voxel grid; /(I)For/>Total number of point clouds in the second voxel grid; /(I)For/>The abscissa value of the second voxel grid; /(I)For/>Ordinate values of the second voxel grids; /(I)For/>A vertical coordinate value of the second voxel grid; /(I)Is the voxel variance of the second point cloud.

In step S108, a residual is calculated. According to the method, the mean value of the local map to be registered is determined according to the first mean value and the second mean value; and determining the variance of the local map to be registered according to the first variance and the second variance, and finishing incremental updating of the local map to be registered.

The mean value of the local map to be registered is as follows:

；

the variance of the local map to be registered is:

；

Wherein, The mean value of the local map to be registered is obtained; /(I)Is the variance of the local map to be registered. And the current positioning position moves for 2m, the posture changes for 30 degrees, the process of calculating the mean and the variance of the current point cloud is repeatedly executed, and incremental updating of the local map to be registered is completed.

In step S109, in the local map to be registered, in which the incremental update is completed, the target point cloud is positioned according to the position of the voxel grid in the local map to be registered. It is determined whether the posture calculation is completed, if so, step S111 is performed, otherwise step S110 is performed.

In the local map to be registered, which is updated in an increment, the process of positioning the target point cloud according to the position of the target voxel grid in the local map to be registered comprises the following steps:

Determining the position of a target voxel grid of the target point cloud in the local map to be registered according to the current predicted gesture of the target point cloud; if voxel grids generated by the local map to be registered exist around the target voxel grids, the mean value and the variance of the local map to be registered are obtained, and Gaussian Newton least square expansion is carried out; and judging whether the increment from the target point cloud to the nearest neighbor voxel of the local map to be registered meets an increment threshold, and if so, outputting a positioning result.

In step S110, the obtained current point cloud is registered, and step S103 is circularly executed.

In step S111, the flow ends.

When the current environment and the constructed prior map have large difference, the problem of unstable positioning caused by environment transformation can be well solved by maintaining a local map on the basis of the prior map. Fig. 2 is a schematic diagram of a priori map effect according to embodiment 1 of the present invention; it can be seen that fig. 2 is supporting the positioning without much point cloud information on the road of travel. Fig. 3 is a schematic view of a real-time point cloud map effect according to embodiment 1 of the present invention; when the prior inspection map lacks a lot of current environment information, maintaining and updating the local map of the current environment on the basis of the prior inspection map according to the real-time point cloud information. FIG. 4 is a schematic view of a registered local map effect according to embodiment 1 of the present invention; it can be seen that figure four can ensure stable operation of positioning.

The embodiment 1 of the invention provides a scene positioning method based on a laser radar, which aims at the condition that the scene changes of factories, power plants and the like are more, establishes a point cloud incremental updating mechanism of a current point cloud and a priori map, and performs point cloud registration according to voxel values of a local map to be registered in a real-time scene change incremental updating mode, thereby improving the accuracy and the robustness of a positioning system in a complex and changeable environment.

Example 2

Based on the scene positioning method based on the laser radar according to the embodiment 1 of the present invention, the embodiment 2 of the present invention further provides a scene positioning system based on the laser radar, and fig. 5 is a schematic diagram of the scene positioning system based on the laser radar according to the embodiment 2 of the present invention, where the system includes: the device comprises a first computing module, a second computing module and a positioning module;

The first calculation module is used for determining a local map of the target scene according to the prior map point cloud of the target scene; performing voxel division on a first point cloud in a local map, storing the first point cloud as first voxel lattices, and calculating a first mean value and a first variance covered by each first voxel lattice;

The second calculation module is used for storing the acquired real-time point cloud as a second voxel grid by using a Hash table after voxel division, and calculating a second mean value and a second variance covered by the second voxel grid; determining the mean value of the local map to be registered according to the first mean value and the second mean value; determining the variance of the local map to be registered according to the first variance and the second variance, and finishing incremental updating of the local map to be registered;

The positioning module is used for positioning the target point cloud according to the position of the voxel grid in the local map to be registered, wherein the incremental updating of the local map to be registered is completed.

In a first computing module: acquiring a priori map point cloud of a target scene; acquiring a specific position of a current robot prior inspection map through repositioning; and obtaining a local map of the target scene through the straight-through filters of positive and negative preset ranges for three directions of the prior map xyz on the initial position of the prior map of the current robot.

The process of voxel classifying the first point cloud in the local map comprises the following steps:

Dividing a first point cloud in a local map by 3dbox voxels of 1x1x1 m; the method comprises the following steps:

；

Wherein, Index for a first point cloud in the local map; /(I)For/>First voxel lattices corresponding to the first point clouds; /(I)Is the total number of first point clouds; /(I)An abscissa after dividing the first point cloud voxels; /(I)An ordinate divided for the first point cloud; /(I)A vertical coordinate divided for the first point cloud voxel; /(I)For/>Rounding; For/> Rounding; /(I)For/>Rounding;

the process of storing the first voxel grid includes:

storing first voxel lattices generated by each first point cloud by using a Hash table;

。

the process of calculating the first mean and the first variance covered by each first voxel grid comprises: calculating a first mean value and a first variance covered by each first voxel grid in the Hash table;

the calculation formula of the first mean value is:

；

；

The first variance is calculated as:

Wherein, A voxel mean value of the first point cloud; /(I)The number of first voxel lattices; /(I)For/>Corresponding stored index sequences in the first voxel lattices; /(I)For/>A total number of point clouds in the first voxel grid; /(I)For/>The abscissa value of the first voxel grid; /(I)For/>Ordinate values of the first voxel grids; /(I)For/>A vertical coordinate value of the first voxel grid; /(I)Is the voxel variance of the first point cloud.

In the second calculation module, the process of storing the acquired real-time point cloud as a second voxel grid by using a Hash table after voxel division is as follows:

Dividing the real-time point cloud by 3dbox voxels of 1x1x1 m; the method comprises the following steps:

；

Storing a second voxel grid generated by each real-time point cloud by using a Hash table;

；

Wherein, Index for real-time point cloud; /(I)For/>Second voxel lattices corresponding to the second point clouds; /(I)The total number of the real-time point clouds; /(I)An abscissa after dividing the second point cloud voxels; /(I)An ordinate divided for the second point cloud; /(I)A vertical coordinate divided for the second point cloud voxel; /(I)For/>Rounding; /(I)Is thatRounding; /(I)For/>And (5) rounding.

The process of calculating the second mean and the second variance covered by the second voxel grid comprises:

the calculation formula of the second mean value is:

；

；/>；

The second variance is calculated as:

Wherein, A voxel mean value of the second point cloud; /(I)The number of second voxel lattices; /(I)For/>A corresponding stored index sequence in a second voxel grid; /(I)For/>Total number of point clouds in the second voxel grid; /(I)For/>The abscissa value of the second voxel grid; /(I)For/>Ordinate values of the second voxel grids; /(I)For/>A vertical coordinate value of the second voxel grid; is the voxel variance of the second point cloud.

Determining the mean value of the local map to be registered according to the first mean value and the second mean value; the process of determining the variance of the local map to be registered from the first variance and the second variance comprises:

the mean value of the local map to be registered is as follows:

；

the variance of the local map to be registered is:

；

Wherein, The mean value of the local map to be registered is obtained; /(I)Is the variance of the local map to be registered.

In the positioning module, in the local map to be registered, the process of positioning the target point cloud according to the position of the target voxel grid in the local map to be registered by the target point cloud comprises the following steps:

Determining the position of a target voxel grid of the target point cloud in the local map to be registered according to the current predicted gesture of the target point cloud;

If voxel grids generated by the local map to be registered exist around the target voxel grids, the mean value and the variance of the local map to be registered are obtained, and Gaussian Newton least square expansion is carried out;

and judging whether the increment from the target point cloud to the nearest neighbor voxel of the local map to be registered meets an increment threshold, and if so, outputting a positioning result.

The embodiment 2 of the invention provides a scene positioning system based on a laser radar, which establishes a point cloud incremental updating mechanism of a current point cloud and a priori map under the condition of more scene changes of factories, power plants and the like, performs point cloud registration by incrementally updating voxel values of a local map to be registered according to real-time scene changes, and improves the accuracy and the robustness of the positioning system under complex and changeable environments.

The description of the relevant parts in the laser radar-based scene positioning system provided in embodiment 2 of the present application may refer to the detailed description of the corresponding parts in the laser radar-based scene positioning method provided in embodiment 1 of the present application, which is not repeated here.

It is noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements is inherent to. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element. In addition, the parts of the above technical solutions provided in the embodiments of the present application, which are consistent with the implementation principles of the corresponding technical solutions in the prior art, are not described in detail, so that redundant descriptions are avoided.

While the specific embodiments of the present invention have been described above with reference to the drawings, the scope of the present invention is not limited thereto. Other modifications and variations to the present invention will be apparent to those of skill in the art upon review of the foregoing description. It is not necessary here nor is it exhaustive of all embodiments. On the basis of the technical scheme of the invention, various modifications or variations which can be made by the person skilled in the art without the need of creative efforts are still within the protection scope of the invention.

Claims (10)

1. The scene positioning method based on the laser radar is characterized by comprising the following steps of:

Determining a local map of the target scene according to the prior map point cloud of the target scene; performing voxel division on a first point cloud in a local map, storing the first point cloud as first voxel lattices, and calculating a first mean value and a first variance covered by each first voxel lattice;

Performing voxel division on the acquired real-time point cloud, storing the real-time point cloud as a second voxel grid by using a Hash table, and calculating a second mean value and a second variance covered by the second voxel grid; determining the mean value of the local map to be registered according to the first mean value and the second mean value; determining the variance of the local map to be registered according to the first variance and the second variance, and finishing incremental updating of the local map to be registered;

and in the local map to be registered, which is updated in an increment, positioning the target point cloud according to the position of the voxel grid of the target point cloud in the local map to be registered.

2. The method for locating a scene based on lidar as recited in claim 1, wherein the determining the local map of the target scene based on the prior map point cloud of the target scene comprises:

Acquiring a priori map point cloud of a target scene;

acquiring a specific position of a current robot prior inspection map through repositioning;

And filtering the prior map in the coordinate axis direction on the initial position of the prior map where the current robot is positioned to obtain the local map.

3. The method for positioning a scene based on a laser radar according to claim 2, wherein the filtering the prior map in the coordinate axis direction at the initial position where the current robot is located on the prior map to obtain the local map specifically comprises: and obtaining a local map of the target scene through the straight-through filters of positive and negative preset ranges for three directions of the prior map xyz on the initial position of the prior map of the current robot.

4. The method of claim 1, wherein voxel classifying the first point cloud in the local map comprises:

Dividing a first point cloud in a local map by 3dbox voxels of 1x1x1 m; the method comprises the following steps:

；

Wherein, Index for a first point cloud in the local map; /(I)For/>First voxel lattices corresponding to the first point clouds; /(I)Is the total number of first point clouds; /(I)An abscissa after dividing the first point cloud voxels; /(I)An ordinate divided for the first point cloud; /(I)A vertical coordinate divided for the first point cloud voxel; /(I)For/>Rounding; /(I)Is thatRounding; /(I)For/>Rounding;

the process of storing the first voxel grid includes:

storing first voxel lattices generated by each first point cloud by using a Hash table;

。

5. The method of claim 4, wherein the calculating the first mean and the first variance covered by each first voxel grid comprises: calculating a first mean value and a first variance covered by each first voxel grid in the Hash table;

the calculation formula of the first mean value is:

；

；

The first variance is calculated as:

Wherein, A voxel mean value of the first point cloud; /(I)The number of first voxel lattices; /(I)Is the firstCorresponding stored index sequences in the first voxel lattices; /(I)For/>A total number of point clouds in the first voxel grid; /(I)For/>The abscissa value of the first voxel grid; /(I)Is the firstOrdinate values of the first voxel grids; /(I)For/>A vertical coordinate value of the first voxel grid; Is the voxel variance of the first point cloud.

6. The method for positioning a scene based on a laser radar according to claim 5, wherein the process of storing the acquired real-time point cloud as the second voxel lattice by using a Hash table after voxel division is as follows:

Dividing the real-time point cloud by 3dbox voxels of 1x1x1 m; the method comprises the following steps:

；

Storing a second voxel grid generated by each real-time point cloud by using a Hash table;

；

Wherein, Index for real-time point cloud; /(I)For/>Second voxel lattices corresponding to the second point clouds; The total number of the real-time point clouds; /(I) An abscissa after dividing the second point cloud voxels; /(I)An ordinate divided for the second point cloud; /(I)A vertical coordinate divided for the second point cloud voxel; /(I)For/>Rounding; /(I)For/>Rounding; /(I)For/>And (5) rounding.

7. The method of claim 6, wherein the calculating the second mean and the second variance covered by the second voxel grid comprises:

the calculation formula of the second mean value is:

；

；/>；

The second variance is calculated as:

Wherein, A voxel mean value of the second point cloud; /(I)The number of second voxel lattices; /(I)Is the firstA corresponding stored index sequence in a second voxel grid; /(I)For/>Total number of point clouds in the second voxel grid; /(I)For/>The abscissa value of the second voxel grid; /(I)Is the firstOrdinate values of the second voxel grids; /(I)For/>A vertical coordinate value of the second voxel grid; is the voxel variance of the second point cloud.

8. The method for positioning a scene based on a lidar according to claim 7, wherein the determining the average value of the local map to be registered according to the first average value and the second average value; the process of determining the variance of the local map to be registered from the first variance and the second variance comprises:

the mean value of the local map to be registered is as follows:

；

the variance of the local map to be registered is:

；

Wherein, The mean value of the local map to be registered is obtained; /(I)Is the variance of the local map to be registered.

9. The method for positioning a scene based on a laser radar according to claim 1, wherein in the incremental update-completed local map to be registered, the process of positioning the target point cloud according to the position of the target voxel grid in the local map to be registered includes:

Determining the position of a target voxel grid of the target point cloud in the local map to be registered according to the current predicted gesture of the target point cloud;

If voxel grids generated by the local map to be registered exist around the target voxel grids, the mean value and the variance of the local map to be registered are obtained, and Gaussian Newton least square expansion is carried out;

and judging whether the increment from the target point cloud to the nearest neighbor voxel of the local map to be registered meets an increment threshold, and if so, outputting a positioning result.

10. The scene positioning system based on the laser radar is characterized by comprising a first calculation module, a second calculation module and a positioning module;

The first calculation module is used for determining a local map of the target scene according to the prior map point cloud of the target scene; performing voxel division on a first point cloud in a local map, storing the first point cloud as first voxel lattices, and calculating a first mean value and a first variance covered by each first voxel lattice;

The second calculation module is used for storing the acquired real-time point cloud as a second voxel grid by using a Hash table after voxel division, and calculating a second mean value and a second variance covered by the second voxel grid; determining the mean value of the local map to be registered according to the first mean value and the second mean value; determining the variance of the local map to be registered according to the first variance and the second variance, and finishing incremental updating of the local map to be registered;

The positioning module is used for positioning the target point cloud according to the position of the voxel grid in the local map to be registered, wherein the incremental updating of the local map to be registered is completed.