WO2022183785A1

WO2022183785A1 - Robot positioning method and apparatus, robot, and readable storage medium

Info

Publication number: WO2022183785A1
Application number: PCT/CN2021/132992
Authority: WO
Inventors: 谷雨隆; 张思民; 赵云
Original assignee: 深圳市优必选科技股份有限公司
Priority date: 2021-03-05
Filing date: 2021-11-25
Publication date: 2022-09-09
Also published as: CN113050116A; CN113050116B

Abstract

A robot positioning method and apparatus, a robot, and a readable storage medium. Said method comprises: performing positioning once on the position of a robot to obtain a first pose; and by taking the first pose as an initial pose of the robot, performing coordinate conversion on the coordinates of a laser point cloud, and performing gradient search on a score optimization function of the corresponding laser point cloud by using the converted coordinates so as to solve a scan matching pose, the scan matching pose being taken as a second pose of the robot. Finally, the first pose or the second pose is selected as the final positioning result according to whether the second pose satisfies a preset matching condition. The present invention can be adapted to different environments and scenarios, and can achieve stable positioning when very high accuracy is not required, whilst achieving required accuracy in some scenarios, and thus has relatively strong positioning robustness. The positioning apparatus corresponds to the positioning method; the robot executes the positioning method; and the readable storage medium stores a computer program for implementing the positioning method.

Description

Robot positioning method, device, robot and readable storage medium

CROSS-REFERENCE TO RELATED APPLICATIONS

This application claims the priority of the Chinese Patent Application No. 2021102469536 and entitled "Robot Positioning Method, Device, Robot and Readable Storage Medium" filed with the China Patent Office on March 05, 2021, the entire contents of which are incorporated by reference in this application.

technical field

The present application relates to the technical field of robot positioning, and in particular, to a robot positioning method, device, robot and readable storage medium.

Background technique

In order for robots to meet different needs in different scenarios, robot positioning accuracy is an important indicator. The principle of robot laser positioning technology is to calculate the position of the robot itself through the features scanned by lidar, which is a relative positioning technology.

This relative positioning technology is very easily affected by changes in the surrounding environment. At the same time, the map formed by the surrounding environment is also pre-scanned and established by lidar. During the map generation process, a resolution will be set for the map (for example, one pixel represents the actual 5cm ), usually this resolution will take into account both the navigation accuracy and the amount of calculation, and is affected by the map resolution, so the navigation accuracy will also be subject to certain constraints. Therefore, for scenarios that need to achieve high-precision positioning (such as within 3cm) and have high requirements for positioning stability, such as positioning deviation caused by uneven ground, and need to quickly correct positioning, etc., there is a high-precision and robust positioning at this time. The system is a technical problem that needs to be solved urgently.

Application content

In view of this, in order to overcome the deficiencies in the prior art, the present application provides a robot positioning method, device, robot and readable storage medium.

An embodiment of the present application provides a method for positioning a robot, where the robot is equipped with a lidar, and the method includes:

Position the robot once to get the first pose;

Take the first pose as the initial pose of the robot, and use the initial pose to convert the coordinates of the current laser point cloud in the grid map to the coordinates under the robot coordinate system to obtain the coordinates of the laser point cloud. convert coordinates;

Gradient optimization is performed on the score optimization function of the corresponding laser point cloud based on the initial pose and the transformed coordinates to solve the matching pose between the scanning data of the lidar and the grid map, and the matching pose is taken as The second pose of the robot; wherein, the score optimization function is constructed according to the converted coordinates of the laser point cloud and the Gaussian distribution information of the grid where the laser point cloud is located in the grid map;

In the case that the second pose satisfies the preset matching condition, the second pose is used as the current final pose of the robot, and in the case that the preset matching condition is not satisfied, the first pose One pose is the current final pose of the robot.

In some embodiments, the second pose satisfies a preset matching condition, including:

The score of the laser point cloud under the second pose is greater than or equal to a confidence threshold, and the difference between the second pose and the first pose is less than or equal to a preset error threshold.

In some embodiments, the gradient optimization is performed on the score optimization function of the corresponding laser point cloud based on the initial pose and the transformed coordinates to solve the matching position between the data scanned by the lidar and the grid map. posture, including:

Using the Gauss-Newton iterative algorithm to solve the corresponding gradient and Hessian matrix for the score optimization function of the laser point cloud based on the transformed coordinates;

Superimposing the gradients of all laser point clouds and the Hessian matrix to calculate the current pose increment;

A second pose is obtained by adding the initial pose and the current pose increment, wherein in the second pose, the scanning data of the lidar and the grid map have the highest matching degree.

In some embodiments, before solving the second pose of the robot, the method further includes:

Perform gridization on the map preloaded by the robot to obtain a grid map composed of several grids, and calculate the mean and variance of the Gaussian distribution of each grid according to the positions of obstacles in the grid map .

In some embodiments, the positioning of the position of the robot once to obtain the first pose includes:

A corresponding number of particles are randomly generated around the initial position of the robot, and each particle has its own initial pose and the same initial weight;

Make the robot move in the grid map according to the movement instruction, and update the pose of each particle after each movement to obtain the updated pose of each particle;

Update the weight of the corresponding particle according to the updated pose of the particle and the observation data obtained by the robot through the mounted lidar to obtain the updated weight of each particle;

Perform particle resampling according to the distribution of the updated weights of all particles to obtain the resampled number of particles, and return to the above-mentioned moving step to perform a preset number of iterations;

The first pose of the robot is calculated according to the updated pose and updated weight of each particle obtained by the final iteration.

In some embodiments, the transformation formula of the coordinate transformation is:

Among them, (x _i , y _i ) represents the coordinates of the i-th laser point cloud in the grid map; X′ _i represents the converted coordinates of the i-th laser point cloud after conversion; (T _x , T _y ) represents the The position of the robot, T _θ represents the attitude angle of the robot.

In the above embodiment, the expression of the score optimization function of the i-th laser point cloud is:

Among them, q _i and ∑ _i represent the mean and variance of the Gaussian distribution of the grid where the i-th laser point cloud is located, respectively.

The embodiment of the present application also provides a robot positioning device, the robot is equipped with a laser radar, and the device includes:

A positioning module is used to position the robot once to obtain the first pose;

The conversion module is used for taking the first pose as the initial pose of the robot, and using the initial pose to convert the coordinates of the current laser point cloud in the grid map to the coordinates in the robot coordinate system to obtain the The transformation coordinates of the laser point cloud;

The secondary positioning module is used to perform gradient optimization on the score optimization function of the laser point cloud based on the initial pose and the transformed coordinates to solve the matching position between the scanning data of the laser radar and the grid map. The matching pose is used as the second pose of the robot; wherein, the score optimization function is constructed according to the converted coordinates of the laser point cloud and the Gaussian distribution information of the grid where the laser point cloud is located in the grid map;

A positioning selection module, configured to use the second pose as the current final pose of the robot when the second pose satisfies a preset matching condition, and when the preset matching condition is not met Next, take the first pose as the current final pose of the robot.

An embodiment of the present application further provides a robot, the robot includes a processor and a memory, the memory stores a computer program, and when the computer program is executed on the processor, the above-mentioned robot positioning method is implemented.

Embodiments of the present application further provide a readable storage medium, which stores a computer program, and when the computer program is executed by a processor, implements the above-mentioned method for positioning a robot.

The embodiments of the present application have the following beneficial effects:

The robot positioning method of the embodiment of the present application first obtains the first positioning position of the robot; then, based on the first positioning position, combined with the surrounding environment information of the robot and using an iterative optimization method, the position of the robot is solved again, The second positioning position of the robot is obtained, and the current final positioning result of the robot is selected by comparing the two positioning positions. This method determines the final positioning position through two positioning, which can adapt to different environments and scenarios. When very high accuracy is required, the positioning can be stable, and in some scenarios, the required accuracy can be achieved, and it has strong positioning robustness.

Description of drawings

In order to illustrate the technical solutions of the embodiments of the present application more clearly, the following drawings will briefly introduce the drawings that need to be used in the embodiments. It should be understood that the following drawings only show some embodiments of the present application, and therefore do not It should be regarded as a limitation of the scope, and for those of ordinary skill in the art, other related drawings can also be obtained according to these drawings without any creative effort.

FIG. 1 shows a schematic flowchart of a robot positioning method according to an embodiment of the present application;

FIG. 2 shows a schematic flowchart of one positioning of the robot positioning method according to the embodiment of the present application;

FIG. 3 shows a schematic flowchart of the secondary positioning of the robot positioning method according to the embodiment of the present application;

FIG. 4 shows a schematic structural diagram of a robot positioning device according to an embodiment of the present application.

Detailed ways

The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments.

The components of the embodiments of the present application generally described and illustrated in the drawings herein may be arranged and designed in a variety of different configurations. Thus, the following detailed description of the embodiments of the application provided in the accompanying drawings is not intended to limit the scope of the application as claimed, but is merely representative of selected embodiments of the application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the present application.

Hereinafter, the terms "comprising", "having" and their cognates, which may be used in various embodiments of the present application, are only intended to denote particular features, numbers, steps, operations, elements, components, or combinations of the foregoing, and should not be construed as first excluding the presence of or adding one or more other features, numbers, steps, operations, elements, components or combinations of the foregoing or the possibility of a combination of the foregoing.

Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the various embodiments of this application belong. The terms (such as those defined in commonly used dictionaries) will be interpreted as having the same meaning as the contextual meaning in the relevant technical field and will not be interpreted as having an idealized or overly formal meaning, unless explicitly defined in the various embodiments of the present application.

In the prior art, for the real-time positioning technology based on particle filtering, the method of particle filtering is used to fuse odometer data, inertial measurement unit (IMU) data and lidar measurement data, and then calculate the state values of multiple particles. The average position is taken as the final robot position. However, due to the characteristics of particle filtering, a large number of sub-optimal particles will be retained to improve the robustness of positioning, so that particle filtering is not easy to cause positioning jumps due to changes in the environment, but the existence of sub-optimal particles allows the The positioning accuracy of the robot is reduced, and the final position it gets is an average value of all particles, which is near the optimal value, so it cannot achieve high positioning accuracy.

For another method, real-time positioning technology based on optimal matching, such as Newton optimal matching, correlation matching and other methods, this method uses the motion model of the sensor to estimate an initial pose, and then uses Newton iteration or least squares optimization, etc. method, find an optimal pose within a certain range near the initial pose, this method can obtain higher accuracy because it can calculate the optimal position when the environmental features are obvious, but when There are certain changes in the environment, or in areas with similar environments, the position will become unstable, resulting in position jumping, and when the position jumps to the wrong position, it will deviate from the correct position more and more due to the movement of the robot. After the range is set, the positioning cannot be recovered. For example, if the matching range is 1 meter, this method cannot be recovered after the position drifts more than 1 meter.

Since the above two navigation and positioning technologies respectively have corresponding defects, for this reason, an embodiment of the present application proposes a robot positioning method, which can not only resist environmental changes, but also have strong positioning robustness, and can also obtain high-precision positioning results. The method of the embodiment of the present application will combine the particle filter algorithm to perform a primary positioning of the robot, and then use the result of the primary positioning as the basis for the secondary positioning, and combine the optimization and matching method to perform the secondary positioning of the robot. By comparing the two positioning results The result is to select the final positioning result.

Example 1

Referring to FIG. 1 , this embodiment proposes a method for positioning a robot, which can be used for self-positioning of a robot equipped with a lidar in different situations.

The robot positioning method will be described in detail below.

In step S110, the position of the robot is positioned once to obtain the first pose.

Exemplarily, taking the particle swarm filtering algorithm as an example, the position of the robot is positioned once to ensure the robustness of the positioning. As shown in Figure 2, the steps for obtaining the first pose include:

In sub-step S111, a corresponding number of particles are randomly generated around the initial position of the robot, and each particle has its own initial pose and the same initial weight.

Exemplarily, a series of particles (ie, particle swarms) can be generated near the initial position of the robot, and these particles generally conform to a corresponding distribution law, such as a Gaussian distribution. Among them, the initial pose of each particle includes the distance r and the direction angle θ between it and the robot, and the distance r can be represented by two-dimensional plane coordinates (x, y). The weight w represents the probability that the pose of the particle is the real position of the robot. In the initial state, the weight normalization process will be performed on all particles, so that the initial weight value of each particle is the same.

Furthermore, by controlling the motion of the robot, that is, using the motion model to update the pose of these particles, and according to the observation data of the surrounding environment from the lidar mounted on the robot, that is, using the observation model to score these particles, and obtain the weight distribution of each particle . Through weight screening of these particles, the final particle pose is taken as the position of the robot.

In sub-step S112, the robot is moved in the grid map according to the movement instruction, and the pose of each particle is updated after each movement to obtain the updated pose of each particle.

Exemplarily, if the total number of particles is M, for M particles, their poses at time t can be expressed as follows:

When t=0, it is expressed as the initial pose of each particle. After the robot moves the corresponding distance in the grid map according to the set movement command, the updated pose of each particle can be calculated according to the motion model, which can be expressed as follows:

Among them, _ut represents the control information of the mobile robot; f _u represents the motion model.

Sub-step S113 , update the weight of the corresponding particle according to the updated pose of the particle and the observation data obtained by the robot through the mounted lidar to obtain the updated weight of each particle.

Exemplarily, by updating the weights of these particles through observation data, the pose of the particles can be made closer to the real position of the robot. For example, for the update weight of the mth particle, it can be expressed as follows:

Among them, z _t represents the observation data obtained by the lidar measurement at time t, and f _z represents the observation model.

Sub-step S114: Perform particle resampling according to the distribution of the updated weights of all particles to obtain the resampled number of particles, and return to the above-mentioned moving step to perform a preset number of iterations.

Exemplarily, after the weights of all particles are updated, the particle weights are sorted by the size of the particles, and particle resampling is performed according to the weight distribution of these particles, so as to obtain M particles with a constant number, and then return to the above-mentioned step S112, and repeat the above-mentioned steps. The step of updating the pose and updating the weight of 100 stops until the preset number of times is executed.

It can be understood that particles with high weights, that is, particles that are closer to the real position of the robot, will be retained during the resampling process, while particles with low weights, that is, particles with unreliable poses, will be gradually discarded. The above-mentioned preset times may be preset by the robot, may also be pre-generated by the robot, or may be preset by the user according to actual needs. Of course, the above-mentioned number of iterations can also be constrained according to other conditions, such as making the weight value of the particle meet corresponding requirements, etc., and is not limited to the set number of iterations.

Sub-step S115: Calculate the first pose of the robot according to the updated pose and updated weight of each particle obtained by the final iteration.

Exemplarily, for the finally obtained poses and weights of the particles, for example, the first pose of the robot may be calculated by using a weighted average algorithm or the like. Of course, other methods may also be used to calculate the position of the robot based on the poses of these particles, which is not limited here.

Considering that the weighted average method is used to solve the pose by particle filtering, a certain positioning accuracy may be lost. For this reason, this embodiment will solve the position of the robot again on the basis of solving the first pose. , at this time, according to the surrounding environment information of the robot, the Gauss-Newton iterative optimization method is used to obtain the high-precision positioning position of the robot.

Before performing the gradient optimization solution, this embodiment first performs gridization on the map preloaded by the robot, so as to obtain a grid map composed of several grids. Furthermore, the mean and variance of the Gaussian distribution of each grid are calculated according to the position of the obstacles in the map. Among them, the mean and variance of the Gaussian distribution of these grids will be reserved for subsequent matching optimization, and the mean and variance can be calculated by the corresponding calculation formula of the mean and variance of the Gaussian distribution.

Step S120, taking the first pose as the initial pose of the robot, and using the initial pose to convert the coordinates of the current laser point cloud in the grid map to the coordinates in the robot coordinate system to obtain the converted coordinates of the laser point cloud.

Exemplarily, by converting the coordinates of the current laser point cloud to the map to determine which grid each laser point cloud is in, then the score of each laser point cloud in the grid can be calculated. It can be understood that the higher the score, the higher the matching degree between the laser scan and the map in the pose represented by it, which means that the pose is closer to the real pose of the robot, otherwise, the matching degree is lower.

In one embodiment, if the coordinates of the laser point cloud in the grid map are (x _i , y _i ), the transformation coordinates of the i-th laser point cloud after transformation are X′ _i , so the transformation of the coordinate transformation The formula is:

Among them, for the pose of the robot T=(T _x , _Ty , T _θ ), (T _x , _Ty ) and T _θ represent the coordinate position and attitude angle of the robot, respectively.

Step S130, based on the initial pose and the transformed coordinates, perform gradient optimization on the score optimization function of the laser point cloud to solve the matching pose between the scanning data of the laser radar and the grid map, and the matching pose is used as the second position of the robot. pose.

Exemplarily, the score optimization function of each laser point cloud is pre-built according to the converted coordinates of the laser point cloud and the Gaussian distribution information of the grid where the laser point cloud is located in the grid map. For example, the Gaussian distribution information may include mean and variance. Wait. In one embodiment, the score optimization function of the i-th laser point cloud can be expressed as:

Among them, q _i and ∑ _i represent the mean and variance of the Gaussian distribution of the grid where the i-th laser point cloud is located, respectively. It can be understood that the score optimization function is the same for each laser point cloud. In addition, the expression of the above-mentioned score optimization function is only an example, and is not limited thereto.

Based on the pre-built score optimization function, the gradient solution of the score optimization function can be performed to obtain the optimal pose. Exemplarily, as shown in FIG. 3 , the steps of solving the second pose include:

Sub-step S131 , using the Gauss-Newton iterative algorithm to solve the corresponding gradient and Hessian matrix for the score optimization function of the corresponding laser point cloud based on the transformed coordinates.

Exemplarily, by taking the first-order derivation of the pose of the robot in the expression of the score optimization function, a Jacobian matrix composed of the first-order derivatives can be obtained, wherein the gradient is a direction vector composed of the first-order derivatives. Furthermore, the second-order partial derivative is performed on the expression of the first-order derivative, and the Hessian matrix composed of the second-order partial derivative can be obtained.

Sub-step S132, superimpose the gradients of all laser point clouds and the Hessian matrix to calculate the current pose increment.

For example, still taking the expression of the above score optimization function as an example, the pose of the robot is T, and for the i-th laser point cloud, the expression of its first derivative is:

And, the expression for the second-order partial derivative is:

Therefore, the solved gradient dTr is:

Among them, pi represents the matching probability of the _ith laser point cloud.

Exemplarily, after solving the gradient of each laser point cloud and the Hessian matrix superposition, the final pose optimization increment ΔT can be obtained, where ΔT=H ⁻¹ dTr.

Sub-step S133, adding the initial pose and the current pose increment to obtain a second pose.

Exemplarily, if the initial pose is T ₀ , then the second pose is T ₀ +ΔT. Among them, the second pose is used to achieve optimal alignment between the scan data of the lidar and the raster map.

In this embodiment, the first pose or the second pose is selected as the current final pose of the robot according to whether the second pose satisfies the preset matching condition. By comparing the two positioning positions and selecting one of them as the current final positioning result of the robot, the optimal positioning results can be obtained in different scenarios.

Step S140, in the case that the second pose satisfies the preset matching condition, the second pose is used as the current final pose of the robot.

In one embodiment, the preset matching condition may be: the score corresponding to the second pose is greater than or equal to a preset confidence threshold, and the difference between the second pose and the first pose is less than or equal to Preset error threshold. It can be understood that when the second pose itself is obtained relatively high and the error from the first pose is within an acceptable deviation range, the positioning position obtained later is selected as the final positioning result.

Step S150, in the case that the second pose does not meet the preset matching condition, the first pose is taken as the current final pose of the robot.

Exemplarily, if the score corresponding to the second pose is less than the aforementioned confidence threshold, or, the score corresponding to the second pose is greater than or equal to the aforementioned confidence threshold and the difference between the second pose and the first pose When it is greater than the above error threshold, the first pose is taken as the current final positioning position of the robot.

Due to the iterative optimization method, an accurate position can be obtained, but considering that the environment changes greatly, divergence occurs during the iterative optimization, and the position obtained at this time will be wrong. To this end, in this embodiment, two kinds of positioning results are combined to select a more accurate positioning result.

It can be understood that when the error of the two positioning results is large, it means that the environment at this time changes greatly, so the first pose obtained by the particle filtering method is used as the final positioning result; When the error is small, it means that the optimization result is relatively reliable, so the high-precision second pose obtained by the optimization method is used as the final positioning result.

The robot positioning method of this embodiment is firstly positioned by the particle swarm filtering method. Since the position of the robot is a slowly changing process, it will not jump when the environment diverges, so it has better robustness; and the second The secondary positioning method is based on the first positioning position, and according to the surrounding environment information of the robot, an iterative optimization method is used to obtain a higher-precision positioning. This method can adapt to different environments and scenarios, and has better robustness. performance and high positioning accuracy.

Example 2

Referring to FIG. 4 , based on the method of Embodiment 1 above, this embodiment provides a robot positioning device. The robot is equipped with a laser radar. Exemplarily, the robot positioning device 100 includes:

The primary positioning module 110 is used for once positioning the position of the robot to obtain the first pose;

The conversion module 120 is configured to use the first pose as the initial pose of the robot, and use the initial pose to convert the coordinates of the current laser point cloud in the grid map to the coordinates in the robot coordinate system to obtain the The transformed coordinates of the laser point cloud.

The secondary positioning module 130 is configured to perform gradient optimization on the score optimization function of the laser point cloud based on the initial pose and the transformed coordinates to solve the matching position between the scan data of the lidar and the grid map. pose, and the matching pose is used as the second pose of the robot. The score optimization function is constructed according to the converted coordinates of the laser point cloud and the Gaussian distribution information of the grid where the laser point cloud is located in the grid map.

The positioning selection module 140 is configured to use the second pose as the current final pose of the robot when the second pose satisfies a preset matching condition, and, when the preset matching condition is not met In the case of , the first pose is taken as the current final pose of the robot.

It can be understood that the apparatus of this embodiment corresponds to the method of the foregoing Embodiment 1, and the options in the foregoing Embodiment 1 are also applicable to this embodiment, so the description is not repeated here.

The present application also provides a robot, for example, the robot is a mobile robot, which can realize self-positioning during the moving process. Exemplarily, the robot includes a processor and a memory, wherein the memory stores a computer program, and the processor executes the computer program so that the robot executes the functions of the above-mentioned robot positioning method or each module in the above-mentioned robot positioning device.

The present application also provides a readable storage medium for storing the computer program used in the above-mentioned robot.

In the several embodiments provided in this application, it should be understood that the disclosed apparatus and method may also be implemented in other manners. The apparatus embodiments described above are only schematic. For example, the flowcharts and structural diagrams in the accompanying drawings show the possible implementation architectures and functions of the apparatuses, methods and computer program products according to various embodiments of the present application. and operation. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code that contains one or more functions for implementing the specified logical function(s) executable instructions. It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It is also noted that each block of the block diagrams and/or flow diagrams, and combinations of blocks in the block diagrams and/or flow diagrams, can be implemented using dedicated hardware-based systems that perform the specified functions or actions. be implemented, or may be implemented in a combination of special purpose hardware and computer instructions.

In addition, each functional module or unit in each embodiment of the present application may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application can be embodied in the form of a software product in essence, or the part that contributes to the prior art or the part of the technical solution. The computer software product is stored in a storage medium, including Several instructions are used to cause a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, Read-Only Memory (ROM, Read-Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes .

The above are only specific embodiments of the present application, but the protection scope of the present application is not limited to this. should be covered within the scope of protection of this application.

Claims

A robot positioning method, characterized in that the robot is equipped with a laser radar, and the method includes:

Position the robot once to get the first pose;

Take the first pose as the initial pose of the robot, and use the initial pose to convert the coordinates of the current laser point cloud in the grid map to the coordinates under the robot coordinate system to obtain the coordinates of the laser point cloud. convert coordinates;

Gradient optimization is performed on the score optimization function of the corresponding laser point cloud based on the initial pose and the transformed coordinates to solve the matching pose between the scanning data of the lidar and the grid map, and the matching pose is taken as The second pose of the robot; wherein, the score optimization function is constructed according to the converted coordinates of the laser point cloud and the Gaussian distribution information of the grid where the laser point cloud is located in the grid map;

In the case that the second pose satisfies the preset matching condition, the second pose is used as the current final pose of the robot, and in the case that the preset matching condition is not satisfied, the first pose One pose is the current final pose of the robot.
The robot positioning method according to claim 1, wherein the second pose satisfies a preset matching condition, comprising:

The score of the laser point cloud under the second pose is greater than or equal to a confidence threshold, and the difference between the second pose and the first pose is less than or equal to a preset error threshold.
The robot positioning method according to claim 1 or 2, wherein the score optimization function of the corresponding laser point cloud based on the initial pose and the transformed coordinates is subjected to gradient optimization to solve the lidar scanning The data matches the pose of the raster map, including:

Using the Gauss-Newton iterative algorithm to solve the corresponding gradient and Hessian matrix for the score optimization function of the laser point cloud based on the transformed coordinates;

Superimposing the gradients of all laser point clouds and the Hessian matrix to calculate the current pose increment;

A second pose is obtained by adding the initial pose and the current pose increment, wherein in the second pose, the scanning data of the lidar and the grid map have the highest matching degree.
The robot positioning method according to claim 1, wherein before solving the second pose of the robot, the method further comprises:

Perform gridization on the map preloaded by the robot to obtain a grid map composed of several grids, and calculate the mean and variance of the Gaussian distribution of each grid according to the positions of obstacles in the grid map .
robot positioning method according to claim 1, is characterized in that, the position of described robot is positioned once to obtain the first posture, comprising:

A corresponding number of particles are randomly generated around the initial position of the robot, and each particle has its own initial pose and the same initial weight;

Make the robot move in the grid map according to the movement instruction, and update the pose of each particle after each movement to obtain the updated pose of each particle;

Update the weight of the corresponding particle according to the updated pose of the particle and the observation data obtained by the robot through the mounted lidar to obtain the updated weight of each particle;

Perform particle resampling according to the distribution of the updated weights of all particles to obtain the resampled number of particles, and return to the above-mentioned moving step to perform a preset number of iterations;

The first pose of the robot is calculated according to the updated pose and updated weight of each particle obtained by the final iteration.
The robot positioning method according to claim 1, wherein the transformation formula of the coordinate transformation is:

Among them, (x i , y i ) represents the coordinates of the i-th laser point cloud in the grid map; X′ i represents the converted coordinates of the i-th laser point cloud after conversion; (T x , T y ) represents the The position of the robot, T θ represents the attitude angle of the robot.
The robot positioning method according to claim 6, wherein the expression of the score optimization function of the i-th laser point cloud is:

Among them, qi and Σ i represent the mean and variance of the Gaussian distribution of the grid where the ith laser point cloud is located, respectively.
A robot positioning device, characterized in that the robot is equipped with a laser radar, and the device includes:

A positioning module is used to position the robot once to obtain the first pose;

The conversion module is used for taking the first pose as the initial pose of the robot, and using the initial pose to convert the coordinates of the current laser point cloud in the grid map to the coordinates in the robot coordinate system to obtain the The transformation coordinates of the laser point cloud;

The secondary positioning module is used to perform gradient optimization on the score optimization function of the laser point cloud based on the initial pose and the transformed coordinates to solve the matching position between the scanning data of the laser radar and the grid map. The matching pose is used as the second pose of the robot; wherein, the score optimization function is constructed according to the converted coordinates of the laser point cloud and the Gaussian distribution information of the grid where the laser point cloud is located in the grid map;

A positioning selection module, configured to use the second pose as the current final pose of the robot when the second pose satisfies a preset matching condition, and when the preset matching condition is not met Next, take the first pose as the current final pose of the robot.
A robot, characterized in that the robot includes a processor and a memory, the memory stores a computer program, and when the computer program is executed on the processor, any one of claims 1-7 is implemented. robot positioning method.
A readable storage medium, characterized in that it stores a computer program, and when the computer program is executed by a processor, implements the robot positioning method according to any one of claims 1-7.