CN110174894B - Robot and repositioning method thereof - Google Patents
Robot and repositioning method thereof Download PDFInfo
- Publication number
- CN110174894B CN110174894B CN201910446793.2A CN201910446793A CN110174894B CN 110174894 B CN110174894 B CN 110174894B CN 201910446793 A CN201910446793 A CN 201910446793A CN 110174894 B CN110174894 B CN 110174894B
- Authority
- CN
- China
- Prior art keywords
- pose
- laser radar
- scanning frame
- robot
- grid
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
Abstract
The invention discloses a robot and a repositioning method thereof, wherein the method comprises the following steps: acquiring a pose increment of an end point of a laser radar scanning frame of the robot relative to an original point under a radar coordinate system; traversing each pose in the known grid map to obtain the updated grid probability corresponding to the end point of the laser radar scanning frame under the current pose; and determining the pose corresponding to the updated grid probability which maximizes the response of the correlation function as the repositioning result of the robot. The method realizes the solution of the binding frame problem of the robot based on the relocation realized by the relevant scanning matching, the algorithm does not depend on the initial value, the global optimal value is obtained through global search based on the geometric starting, and the algorithm is simple and effective. After the robot is repositioned, the work such as SLAM can be continuously carried out, and the task failure of the robot is avoided.
Description
Technical Field
The invention relates to the field of robot positioning, in particular to a robot and a repositioning method of the robot.
Background
With the development of robotics, robots are becoming more and more popular. The robot may be "kidnapped" during use, such as when the robot is being held up, kicked, or slipped. The "kidnapping" condition described above may cause the robot itself to be positioned inefficiently, at which time the robot needs to be repositioned. The relocation is an important basis for intelligent navigation and environment exploration of the robot and is also one of key technologies for realizing true complete autonomy of the mobile robot.
In the prior art, robot repositioning is performed using non-linear optimized scan matching. And the nonlinear optimization scanning matching is based on the fact that the mismatching degree of the current frame radar data occupation grid probability and the map occupation grid probability is minimum, and the pose is solved and the map is built through nonlinear optimization iteration.
However, the nonlinear optimization needs an estimated initial value, then a local optimal value is searched near the initial value, if the estimated initial value is not near the global optimal value, the algorithm is converged to an error value, and it can be seen that the nonlinear optimization scan matching is sensitive to the estimated initial value and is not suitable for finding the global optimal problem.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: in the prior art, the robot is repositioned by adopting nonlinear optimization scanning matching, which is sensitive to an initial estimation value and is not suitable for searching for a global optimum problem.
In order to solve the technical problem, the invention provides a robot and a repositioning method thereof.
According to an aspect of the present invention, there is provided a repositioning method of a robot, including:
the method comprises the steps of obtaining a first grid probability occupied by an endpoint of a laser radar scanning frame of the robot and a pose increment of the endpoint relative to an origin of the laser radar scanning frame of the robot in a radar coordinate system;
and traversing, namely traversing each pose in the known grid map in a world coordinate system, and executing the following steps for each pose respectively:
transforming a laser radar scanning frame of the robot from a radar coordinate system to a world coordinate system, enabling the current pose to be the original point pose of the laser radar scanning frame after transformation, and keeping the pose increment of the end point of the laser radar scanning frame relative to the original point before and after transformation unchanged;
acquiring a second grid probability occupied by the end points of the laser radar scanning frame after transformation;
obtaining an updated grid probability corresponding to the end point of the laser radar scanning frame under the current pose according to the first grid probability and the second grid probability;
and a repositioning result determining step, namely determining the pose corresponding to the updated grid probability which enables the correlation function to respond to the maximum as the repositioning result of the robot.
In a preferred embodiment, acquiring the pose increment of the end point of the laser radar scanning frame of the robot relative to the origin comprises:
collecting a laser radar scanning frame by using a laser radar of the robot;
determining the number of endpoints included in the laser radar scanning frame;
and under the condition that the number of the end points of the laser radar scanning frame is determined to be 1, determining the pose increment of the end points of the laser radar scanning frame relative to the origin.
In a preferred embodiment, obtaining an updated grid probability corresponding to an endpoint of the lidar scanning frame in the current pose according to the first grid probability and the second grid probability includes:
calculating a first ratio probability corresponding to a first grid probability occupied by the end points of the laser radar scanning frame before transformation and a second ratio probability corresponding to a second grid probability occupied by the end points of the laser radar scanning frame after transformation;
obtaining an updated ratio probability according to the first ratio probability and the second ratio probability;
and obtaining the updated grid probability according to the updated ratio probability.
In a preferred embodiment, the relocation result determining step includes:
respectively substituting the obtained updated grid probability corresponding to the end point of the laser radar scanning frame under each pose into a correlation function to obtain correlation function response corresponding to each pose;
determining the maximum correlation function response from the correlation function responses corresponding to all traversed poses;
and taking the pose corresponding to the maximum correlation function response as the repositioning result.
In a preferred embodiment, acquiring a pose increment of an endpoint of a laser radar scanning frame of the robot relative to an origin further includes:
and under the condition that the number of the endpoints of the laser radar scanning frame is determined to be more than 1, acquiring the pose increment of each endpoint relative to the origin for each endpoint in the laser radar scanning frame.
In a preferred embodiment, the traversing step includes:
under a world coordinate system, traversing each pose in the known grid map, and respectively executing the following steps for each pose:
transforming the laser radar scanning frame from the radar coordinate system to the world coordinate system, enabling the current pose to be the origin pose of the laser radar scanning frame after transformation, and keeping the pose increment of the end point of the laser radar scanning frame relative to the origin before and after transformation unchanged;
respectively executing the following steps aiming at each end point of the converted laser radar scanning frame:
obtaining a second grid probability occupied by the endpoint after transformation;
judging whether the end point is matched with a known grid map or not after transformation according to the second grid probability;
if the matching is judged, the end point is reserved, and the updated grid probability corresponding to the end point under the current pose is obtained according to the first grid probability and the second grid probability occupied by the end point after transformation;
in the case where a mismatch is determined, the endpoint is deleted.
In a preferred embodiment, determining whether the transformed endpoint matches a known grid map comprises:
judging whether the second grid probability occupied by the end point after transformation indicates that the end point and the known grid map have overlapped grids after transformation;
in the presence of overlapping grids, determining that the transformed endpoint matches a known grid map;
in the case where it is determined that there is no overlapping grid, it is determined that the end point after transformation does not match the known grid map.
In a preferred embodiment, the relocation result determining step includes:
the following steps are respectively executed for each pose traversed:
for each reserved end point, substituting the updated grid probability corresponding to the end point in the current pose into a correlation function to obtain a correlation function response corresponding to the end point in the current pose;
accumulating the correlation function responses corresponding to all the reserved end points under the current pose, and taking the accumulation result as the correlation function response corresponding to the current pose;
determining the maximum correlation function response from the correlation function responses corresponding to all traversed poses;
and taking the pose corresponding to the maximum correlation function response as the repositioning result.
In a preferred embodiment, the correlation function is a non-linear function of the updated grid probabilities corresponding to the end points of the lidar scanning frame at any pose of the traversal.
In a preferred embodiment, the correlation function Rrs(xi) satisfies:
wherein p isξRepresenting the updated grid probability, odd (p), corresponding to the end point of the lidar scanning frame at any pose xi of traversalξ) Is pξThe ratio probability of (c).
According to another aspect of the present invention, there is provided a robot comprising a processor and a computer readable storage medium having a computer program stored therein, the computer program, when executed by the processor, implementing the robot relocation method as described above.
Compared with the prior art, one or more embodiments in the above scheme can have the following advantages or beneficial effects:
the robot repositioning method adopts the relevant scanning matching based on the geometric matching, the relevant scanning matching is based on the geometric correlation between the current frame radar data and the map, and the pose and the map building result with the highest geometric coincidence degree are obtained through global search. The correlated scan matching is a global algorithm, and a global optimal result is easily obtained due to the fact that the method is based on geometric registration. In addition, since the related scan matching is a global search, which does not depend on an initial value, it is only required to search a complete map space, and thus the problem that the nonlinear optimization scan matching in the prior art easily causes the algorithm to converge to an error value can be solved.
Therefore, the method realizes the solution of the robot 'kidnapping' problem based on the relocation realized by the relevant scanning matching, obtains the global optimal value through the global search based on the geometric starting, and has simple and effective algorithm. Compared with a nonlinear optimization algorithm, the relocation algorithm provided by the invention has the characteristics of independence on an initial value and capability of providing global optimum. In addition, after the pose of the robot is recovered (after the robot is repositioned), the work such as SLAM can be continuously carried out, and the task failure of the robot is avoided.
Drawings
The scope of the present disclosure may be better understood by reading the following detailed description of exemplary embodiments in conjunction with the accompanying drawings. Wherein the included drawings are:
FIG. 1 shows a schematic diagram of a robot kidnapping solution flow based on repositioning;
FIG. 2 is a flow chart illustrating a repositioning method of a robot according to an embodiment of the present invention;
FIG. 3 shows a schematic diagram of a lidar scanning frame origin and its endpoints;
FIG. 4 is a flow chart illustrating a robot relocation method when the number of endpoints of a lidar scanning frame is 1;
fig. 5 shows a flow chart of the robot relocation method when the number of endpoints of the lidar scanning frame is greater than 1.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention clearer, the following will describe in detail an implementation method of the present invention with reference to the accompanying drawings and embodiments, so that how to apply technical means to solve the technical problems and achieve the technical effects can be fully understood and implemented.
In the prior art, robot repositioning is typically performed using non-linear optimized scan matching. And the nonlinear optimization scanning matching is based on the fact that the mismatching degree of the current frame radar data occupation grid probability and the map occupation grid probability is minimum, and the pose is solved and the map is built through nonlinear optimization iteration. However, the nonlinear optimization needs an estimated initial value, then a local optimal value is searched near the initial value, if the estimated initial value is not near the global optimal value, the algorithm is converged to an error value, and it can be seen that the nonlinear optimization scan matching is sensitive to the estimated initial value and is not suitable for finding the global optimal problem. The embodiment of the invention provides a robot repositioning method aiming at the problems that in the prior art, nonlinear optimization scanning matching is sensitive to an estimated initial value and is not suitable for finding global optimum.
The embodiment of the invention realizes the solution of the 'kidnapping' problem of the robot based on the relocation realized by the relevant scanning matching, obtains the global optimal value through the global search based on the geometric starting, and has simple and effective algorithm.
Before describing in detail various embodiments of the present invention, the following terms are first defined.
SLAM: simultaneous Localization And Mapping. And obtaining the pose and the environment map of the robot by using the laser radar data according to the SLAM algorithm. The SLAM algorithm is a process of incrementally constructing a map and calculating the pose of a robot. The process is realized by using an algorithm called scanning matching, the scanning matching uses the data of the current frame of the laser radar and the constructed map for registration, and the registration result is the position and posture of the spliced map and the robot.
Pose: the position and attitude of the robot.
Scanning and matching: the scanning data of the laser sensor is registered with the existing map or two frames of laser data, and rigid body transformation parameters (translation and rotation) are obtained while registration is carried out.
Fig. 1 shows a schematic diagram of a robot kidnapping solution flow based on repositioning. As shown in fig. 1, asynchronous "kidnapping" phenomena occur during the operation of the SLAM algorithm of the robot, including slippage, and position changes such as movement due to various reasons. At the moment, the pose xi of the robot needs to be carried out*This process is called robot repositioning. Robot repositioning is achieved by using relevant scan matching, and the relevant scan matching obtains the optimal position of the robot in the global map through global search. Restore the pose xi of the robot*Then, the work such as SLAM can be continuously carried out, and the task of the robot cannot be failed. The repositioning algorithm recovers the pose xi of the robot*Can then take the position xi*The normal SLAM algorithm continues for an initial value.
The repositioning scheme adopts related scanning matching, performs related operation on the current frame laser radar data and the existing map, and acquires the position corresponding to the maximum response of a related function as a final matching result to obtain the position of the robot in the map.
Specific repositioning method fig. 2 shows a specific repositioning method, and fig. 2 shows a flow chart of the repositioning method of the robot according to the embodiment of the invention. The robot relocation method of the present embodiment mainly includes a step S101 of acquiring, a step S102 of traversing, and a step S103 of determining a relocation result.
In the acquisition step of step S101, a first grid probability that an end point of a laser radar scan frame of the robot occupies and a pose increment of the end point with respect to an origin of the laser radar scan frame of the robot are acquired in a radar coordinate system.
In particular, the robot is provided with a laser radar. The scanning frame of the laser radar obtained by the measurement of the laser radar is S ═ (d ═i,θi,pi) I is 1 … k … n, where i is the serial number of the end points (also called scanning points) included in the lidar scanning frame, the lidar scanning frame includes n end points in total, and d isiIs the distance from the end point to the origin in the scan frame, given by the lidar, representing the distance between the robot and the location of the obstacle projected by the lidar beam, θiIs and diCorresponding angle information in the radar coordinate system, which represents an included angle between a connecting line of the positions of the robot and the obstacle and a horizontal plane in the radar coordinate system, piIs the endpoint i occupancy probability (referred to as the first grid probability in this embodiment) of the scan frame, which is not set a priori to be p due to its a priori measurement propertiesi0.75. The pose of the origin of the scanning frame S coordinate system (radar coordinate system) (namely the origin of the laser radar scanning frame) in the world system W is xiwEstimating xi according to the pose of the origin of the radar coordinate system in the world coordinate systemwThe position and the attitude of the end point in the world coordinate system can be calculated
(ep denotes end point).
The information collected by the lidar is described below in conjunction with fig. 3. FIG. 3 shows a laserSchematic diagram of radar scanning frame origin and its end point. As shown in fig. 3, the scanning frame obtained by the laser radar measurement in the current pose ξ includes 6 end points, and the serial numbers of the 6 end points are 1, 2, 3, 4, 5, and 6 in sequence. Here, one obstacle may correspond to one end point of the lidar scanning frame, and one obstacle may correspond to a plurality of end points of the lidar scanning frame. The starting point of the scanning frame is an origin o, namely the position of the robot is at the origin position of the radar coordinate system, and the pose of the origin at the world system W is ξw，
Is the pose of each end point of the scan frame within the world coordinate system. Here, the poses of the 6 end points of the scan frame in the world coordinate system can be expressed as
And
the distance between the position of the barrier corresponding to the first end point and the robot is d1The included angle between the horizontal plane and the connecting line between the position of the barrier and the robot is theta1. According to the relation of trigonometric functions, the position xi of the origin of the scanning frame of the laser radar in the world coordinate system is knownwPosition and pose of the first end point in world coordinate system
The relation between:
extending to the case where the lidar scanning frame includes n endpoints (i.e., the case where there are n scanning points), we can obtain: in the world coordinate system, the laser radar scans the frameOrigin pose xiwPose with end point i
The relation between:
in the traversing step of step S102, each pose in the known grid map is traversed under the world coordinate system. And the following steps are respectively executed for each pose: transforming a laser radar scanning frame of the robot from a radar coordinate system to a world coordinate system, enabling the current pose to be the original point pose of the laser radar scanning frame after transformation, and keeping the pose increment of the end point of the laser radar scanning frame relative to the original point before and after transformation unchanged; acquiring a second grid probability occupied by the end points of the laser radar scanning frame after transformation; and obtaining the updated grid probability corresponding to the end point of the laser radar scanning frame under the current pose according to the first grid probability and the second grid probability.
Specifically, assume that the plane map is Mxm×ynWhereinxmRepresenting a range of x-coordinates of the map with m discrete grids,ynthere are n discrete grids representing the y-coordinate range of the map. The grid resolution is determined according to the actual map, for example 5 cm.
To implement a global search, each pose of the robot in the known grid map is traversed in this step. And determining the updated grid probability corresponding to the end points of the laser radar scanning frame under each traversed pose.
And aiming at any traversed pose, transforming the laser radar scanning frame of the robot from a radar coordinate system to a world coordinate system: the position of the original point of the laser radar scanning frame before transformation is xi, and the position of the original point of the laser radar scanning frame after transformation is xiwWhen traversing the current pose, the original position xi of the laser radar scanning frame after transformationwThe current pose is obtained; the end position and pose of the scanning frame of the laser radar before transformation is xiep(i) End position and pose of laser radar scanning frame after transformationIs composed of
The pose increment obtained in step S101 represents the relative positional relationship between the end point and the origin of the laser radar scan frame. After the coordinate system is converted into world coordinates from radar coordinates, the relative position relation is unchanged, and the pose increment is unchanged. That is, the pose increment of the end point of the laser radar scan frame with respect to the origin is unchanged before and after the coordinate system transformation.
After the lidar scanning frame is transformed to the world coordinate system, a second grid probability occupied by the end point of the lidar scanning frame after transformation can be determined by collecting the coincident grid of the end point of the lidar scanning frame and the known grid map. And determining the updated grid probability corresponding to the end point of the laser radar scanning frame at the current pose by combining the first grid probability occupied by the end point of the laser radar scanning frame acquired under the radar coordinate system.
In a preferred embodiment of the present invention, obtaining an updated grid probability corresponding to an endpoint of the laser radar scanning frame in the current pose according to the first grid probability and the second grid probability includes: calculating a first ratio probability corresponding to a first grid probability occupied by the end points of the laser radar scanning frame before transformation and a second ratio probability corresponding to a second grid probability occupied by the end points of the laser radar scanning frame after transformation; obtaining an updated ratio probability according to the first ratio probability and the second ratio probability; and obtaining the updated grid probability according to the updated ratio probability.
Wherein p isξThe probability that a certain endpoint of a laser radar scanning frame occupies a grid in the position xi is updated. p is a radical ofξThe product of the specific probability of the grid probability occupied by the end point of the scanning frame in the radar coordinate system and the specific probability of the grid probability occupied by the grid point overlapped by the end point converted to the map coordinate system (world coordinate system) in the current pose xi through the formula (2), namely odd (p)ξ)＝odd(pmap)odd(plds) (3)。
The ratio probability of the end points or grid points is calculated according to the grid probability occupied by the end points or grid points, i.e., odd (p) ═ p/(1-p) (4).
Calculating the ratio probability of the upper end point of each frame and the grid point under the map system coincident with the upper end point according to a formula (4), calculating the ratio probability of the updated grid probability corresponding to the end point of the laser radar scanning frame under any position xi by the formula (3), and substituting the calculated ratio probability of the updated grid probability into the updated occupied grid probability p at the position xi of the inverse position xi of the formula (4)ξ＝odd(pξ)/(1+odd(pξ)) (5)。
For example, the scanning frame of the laser radar at the current pose ξ comprises 300 end points, and after being converted into the map coordinate system through the formula (2), 100 end points { A, A1, … … A99} respectively coincide with 100 grid points { B, B1, … … 99} of the known map. Taking an end point A under radar coordinates and a grid point B which is converted to be coincident with the end point A under a map system as an example to calculate and update the grid probability, wherein the grid probability occupied by the end point A is PAThe ratio probability odd (p) of the endpoint A is obtained from the formula (4)lds_A)＝PA/(1-PA) Wherein, due to the prior property of the lidar, PAFor a predetermined a priori value, e.g. PA0.75, of course PAOther values are also possible, and are not limited herein. The probability of the grid occupied by the grid point B is PBThe ratio probability odd (p) of the grid point B is obtained from the formula (4)map_B)＝PB/(1-PB) Wherein P isBCan be read from a known map. The calculated ratio probability odd (p) of the endpoint Alds_A) And the ratio probability odd (p) of the grid point Bmap_B) Substituting the value into a formula (3) to obtain a ratio probability odd (p) of the updated grid probability of the endpoint A in the attitude xiξA)＝odd(pmap_B)odd(plds_A) Odd (p)ξA) Substituting the obtained value into a formula (4), and reversely calculating to obtain the updated grid probability p of the endpoint A in the attitude xiξA＝odd(pξA)/(1+odd(pξA)). Similarly, the updated grid profile of all the end points which coincide with the grid points converted into the map system on the scanning frame in the current pose xi can be obtainedRate pξi。
In the repositioning result determining step of step S103, the pose corresponding to the updated grid probability that maximizes the correlation function response is determined as the repositioning result of the robot.
Specifically, the calculation process for obtaining the maximum correlation function response is to search the translation and rotation space to obtain the correlation function Rrs(ξ), the maximum response corresponding to the maximized match.
In this embodiment, the obtained updated grid probabilities corresponding to the end points of the laser radar scanning frame at each pose are respectively substituted into the correlation function to obtain a correlation function response corresponding to each pose; determining the maximum correlation function response from the correlation function responses corresponding to all traversed poses; and taking the pose corresponding to the maximum correlation function response as the repositioning result.
The robot repositioning method provided by the embodiment of the invention adopts the relevant scanning matching based on the geometric matching, the relevant scanning matching is based on the geometric correlation between the current frame radar data and the map, and the pose and the map building result with the highest geometric coincidence degree are obtained through global search. The correlated scan matching is a global algorithm, and a global optimal result is easily obtained due to the fact that the method is based on geometric registration. In addition, since the related scan matching is a global search, which does not depend on an initial value, it is only required to search a complete map space, and thus the problem that the nonlinear optimization scan matching in the prior art easily causes the algorithm to converge to an error value can be solved.
Therefore, the solution of the robot 'kidnapping' problem is realized by the relocation realized based on the relevant scanning matching, the global optimal value is obtained by the global search based on the geometric starting, and the algorithm is simple and effective. Compared with a nonlinear optimization algorithm, the relocation algorithm provided by the embodiment has the characteristics of independence on an initial value and capability of providing global optimization. In addition, after the pose of the robot is recovered (after the robot is repositioned), the work such as SLAM can be continuously carried out, and the task failure of the robot is avoided.
The relocation method will be described in detail below with reference to fig. 4 and 5 in the case where one end point and a plurality of end points are included in the lidar scanning frame, respectively.
Fig. 4 shows a flowchart of the robot relocation method when the number of endpoints included in the laser radar scan frame is 1. As shown in fig. 4, the repositioning method of the robot of the present embodiment mainly includes steps 201 to 207.
In step 201, a lidar scan frame is acquired with a lidar of the robot.
In step 202, the number of endpoints included in the lidar scan frame is determined.
In step 203, in the case that the number of the end points of the laser radar scanning frame is determined to be 1, determining the pose increment of the end points of the laser radar scanning frame relative to the origin. For example, if the lidar scan frame includes only one endpoint in fig. 3, the lidar scan frame is denoted as S ═ (d)1,θ1,p1) And the pose increment of the end point of the scanning frame of the laser radar relative to the origin is
The pose of the end point of the laser radar scanning frame calculated by the formula (2) in the world coordinate system is expressed as
In step 204, traversing each pose of the robot in the known grid map, transforming the laser radar scanning frame of the robot into a world coordinate system, enabling the current pose to be the origin pose of the laser radar scanning frame after transformation, and keeping the pose increment of the end point of the laser radar scanning frame relative to the origin before and after transformation unchanged.
Specifically, an initial value xi of the pose of the scanning frame S in the world system is givenw(i) Without loss of generality, xi can be assumedw(i) (0,0, 0). I.e. the starting point of traversal is the starting pose xi in the known grid mapw(i)＝(0,0,0)。
When the robot is positioned at the initial pose xiw(i) When the position of the origin of the laser radar scan frame after transformation is (0,0,0) — 00,0), end point pose of
When in the next position xiw(i) When the position of the origin of the scanning frame of the laser radar is (0,0,1), the position of the end point is (0,0,1)
When in the next position xiw(i) When the position of the origin of the scanning frame of the laser radar is (0,0,2), the position of the end point is (0,0,2)
And by analogy, the origin position and the end position of the laser radar scanning frame under each position are obtained.
In step 205, an updated grid probability corresponding to the end point of the lidar scanning frame at the current pose is obtained according to the first grid probability and the second grid probability occupied by the end point of the converted lidar scanning frame.
Specifically, after the lidar scanning frame is transformed into the world coordinate system, the second grid probability occupied by the end point of the lidar scanning frame after transformation can be determined by acquiring the coincident grid of the end point of the lidar scanning frame and the known grid map. And determining the updated grid probability corresponding to the end point of the laser radar scanning frame at the current pose by combining the first grid probability occupied by the end point of the laser radar scanning frame acquired under the radar coordinate system. The detailed process can refer to the detailed description of step S102 in the previous embodiment.
In step 206, the maximum correlation function response is determined from the correlation function responses of all updated trellis probabilities.
Specifically, each calculated updated grid probability, i.e., the updated grid probability p corresponding to the end point of the (0,0,0) lidar scanning frame in the initial pose is determinedξ(0,0,0) and updated grid probability p corresponding to the end point of the laser radar scanning frame under the position posture (0,0,1)ξ(0,0,1), update grid corresponding to end point of laser radar scanning frame under position (0,0,2)Probability pξ(0,0,2) … … and the like are substituted into the correlation function to obtain the correlation function R corresponding to each posers(pξ(0,0,0))、Rrs(pξ(0,0，1))、Rrs(pξ(0,0,2)), etc.
In step 206, the maximum correlation function response is determined from the correlation function responses of all updated trellis probabilities.
In step 207, the pose corresponding to the maximum correlation function response is determined as the repositioning result.
Specifically, assume that a correlation function response R corresponding to when the robot is at pose (0,0,2) is obtained by sorting all correlation function responsesrs(pξ(0,0,2)) is maximum, the pose (0,0,2) is determined as the repositioning result of the robot. That is, (0,0,2) is the repositioned pose of the robot. And after the repositioning algorithm recovers the pose of the robot, the normal SLAM algorithm can be continuously carried out by taking the pose (0,0 and 2) as an initial value.
The robot repositioning method of the present embodiment employs geometric matching-based correlation scan matching. The correlation scan matching macroscopically utilizes the correlation between the images, calculates the correlation coefficient, has the highest correlation coefficient (matching degree) when being matched correctly, and can nest the scanning frame and the images in three loops (in xi) by correlating the positions and the positionsx,ξy,ξφThree dimensions) are searched one by one, and a correlation function response R is calculated in each complete search steprs(xi), the pose xi with the highest phase relation number*. The correlated scan matching focuses on the use of spatial (geometric) information, which has a suppressing effect on noise.
Fig. 5 shows a flow chart of a robot relocation method when the number of endpoints included in a lidar scanning frame is greater than 1. As shown in fig. 5, the repositioning method of the robot of the present embodiment mainly includes steps 301 to 305.
In step 301, a lidar scan frame is acquired with a lidar of the robot.
In step 302, the number of endpoints included in the lidar scan frame is determined.
In step 303, in a case that it is determined that the number of endpoints of the lidar scanning frame is greater than 1, for each endpoint in the lidar scanning frame, a pose increment of the endpoint with respect to an origin is obtained.
Specifically, referring to fig. 3, it is determined that 6 endpoints are included in the lidar scan frame. The first endpoint corresponding information has d1,θ1,p1The second endpoint corresponding information has d2,θ2,p2The third endpoint corresponding information has d3,θ3,p3The fourth endpoint corresponding information has d4,θ4,p4The fifth endpoint corresponding information has d5,θ5,p5The sixth endpoint corresponding information has d6,θ6,p6And the position and pose increment of 6 endpoints of the laser radar scanning frame relative to the origin is as follows in sequence:
and
then, the pose of 6 end points of the scanning frame converted to the world system under the current pose is sequentially obtained according to the formula (2)
In step 304, traversing each pose in the known grid map, transforming the lidar scanning frame from the radar coordinate system to the world coordinate system, and enabling the current pose to be the origin pose of the lidar scanning frame, and keeping the increment of the poses of the end points of the lidar scanning frame before and after transformation relative to the origin of the lidar scanning frame unchanged.
Giving an initial value xi of the pose of the scanning frame S in the world systemw(i) Without loss of generality, xi can be assumedw(i) (0,0, 0). I.e. the starting point of the traversal is the starting point in the known grid mapStarting pose xiw(i)＝(0,0,0)。
The steps performed for any end point of the lidar scanning frame are similar, and the first end point of the lidar scanning frame is taken as an example for explanation:
when the robot is positioned at the initial pose xiw(i) When the position of the origin of the laser radar scanning frame after transformation is (0,0,0), the position of the first end point under the world system is (0,0,0)
When in the next position xiw(i) When the position of the origin of the laser radar scanning frame after transformation is (0,0,1), the position of the first end point under the world system is (0,0,1)
When in the next position xiw(i) When the position of the origin of the laser radar scan frame after transformation is (0,0,2), the position of the first end point under the world system is (0,0,2)
And in the same way, the position of the origin of the scanning frame of the laser radar and the position of the first end point under each position are obtained. And then the position and the posture of the origin of the laser radar scanning frame and the position and the posture of each end point under each position and posture can be obtained.
In step 305, traversing each pose in the known grid map under a world coordinate system, and respectively executing the following steps for each pose: and transforming the laser radar scanning frame from the radar coordinate system to the world coordinate system, enabling the current pose to be the origin pose of the laser radar scanning frame after transformation, and keeping the pose increment of the end point of the laser radar scanning frame relative to the origin before and after transformation unchanged. After transforming the lidar scanning frame to the current pose, respectively executing the following steps for each endpoint of the lidar scanning frame: acquiring a second grid probability occupied by the transformed endpoint, and judging whether the transformed endpoint is matched with a known grid map or not according to the second grid probability; under the condition that matching is judged, the end point is reserved, and according to the first grid probability and the second grid probability occupied by the end point after transformation, the updated grid probability corresponding to the end point under the current pose is obtained; in the case where a mismatch is determined, the endpoint is deleted.
According to the content, the updated grid probabilities corresponding to all the reserved end points in the current pose can be calculated, the updated grid probabilities corresponding to the reserved end points in the current pose are substituted into the correlation function, and the correlation function response corresponding to the end points in the current pose is obtained. And then, accumulating and multiplying the correlation function responses corresponding to all the reserved end points under the current pose, and taking the accumulation and multiplication result as the correlation function response corresponding to the current pose. And finally, determining the maximum correlation function response from the correlation function responses corresponding to all the traversed poses, and taking the pose corresponding to the maximum correlation function response as the repositioning result.
Specifically, firstly, determining that each end point of the laser radar scanning frame under the current pose is converted into a pose under a world system according to the mode described in the formula (2), and judging whether each end point of the laser radar scanning frame is matched with the known grid map or not according to the pose of the world system of each end point. And deleting unmatched end points, only keeping end points matched with the known grid map, and solving the updated grid probability according to the step 102 for each reserved end point so as to obtain the correlation function response of the updated grid probability (the specific method is as described in the step 206).
Here, the method for determining whether the end point of the laser radar scan frame after transformation matches with the known grid map includes: firstly, judging whether the pose of the end point of the laser radar scanning frame in the world system after transformation has overlapped grids with the known grid map; determining that the endpoint matches the known grid map in the presence of overlapping grids; in the case where it is determined that there is no overlapping grid, it is determined that the end point does not match the known grid map.
And then, for any traversed pose, accumulating and multiplying the correlation function responses of the updated grid probability corresponding to the reserved end points under the pose, and taking the accumulation result as the correlation function response corresponding to the current pose.
And traversing each pose in the known grid map, and executing the steps to determine the corresponding correlation function response of each pose.
Then, determining the maximum correlation function response from the correlation function responses corresponding to all the poses, and taking the pose corresponding to the maximum correlation function response as the repositioning result.
The robot repositioning method of the present embodiment employs geometric matching-based correlation scan matching. The correlation scan matching macroscopically utilizes the correlation between the images, calculates the correlation coefficient, has the highest correlation coefficient (matching degree) when being matched correctly, and can nest the scanning frame and the images in three loops (in xi) by correlating the positions and the positionsx,ξy,ξφThree dimensions) are searched one by one, and a correlation function response R is calculated in each complete search steprs(xi), the pose xi with the highest phase relation number*. The correlated scan matching focuses on the use of spatial (geometric) information, which has a suppressing effect on noise.
In a preferred embodiment of the present invention, the correlation function is preferably a non-linear function of the updated grid probability corresponding to the end point of the lidar scanning frame at any pose of the robot in the known grid map. In particular, the correlation function Rrs(xi) satisfies:
wherein p isξRepresenting the updated grid probability, odd (p), corresponding to the end point of the lidar scanning frame at any pose xi of traversalξ) Is pξThe ratio probability of (c). It should be noted that the above description relates to the correlation function RrsThe calculation formula of (ξ) is merely an example and is not intended to be limiting.
The embodiment of the invention also provides a robot. The robot of this embodiment mainly includes a processor and a computer-readable storage medium storing a computer program, which when executed by the processor implements the repositioning method of the robot according to any of the above embodiments.
Although the embodiments of the present invention have been described above, the above description is only for the convenience of understanding the present invention, and is not intended to limit the present invention. It will be understood by those skilled in the art that various changes in form and details may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.
Claims (10)
1. A method of repositioning a robot, comprising:
the method comprises the steps of obtaining a first grid probability occupied by an endpoint of a laser radar scanning frame of the robot and a pose increment of the endpoint relative to an origin of the laser radar scanning frame of the robot in a radar coordinate system;
and traversing, namely traversing each pose in the known grid map in a world coordinate system, and executing the following steps for each pose respectively:
transforming a laser radar scanning frame of the robot from a radar coordinate system to a world coordinate system, enabling the current pose to be the original point pose of the laser radar scanning frame after transformation, and keeping the pose increment of the end point of the laser radar scanning frame relative to the original point before and after transformation unchanged;
acquiring a second grid probability occupied by the end points of the laser radar scanning frame after transformation;
obtaining an updated grid probability corresponding to the end point of the laser radar scanning frame under the current pose according to the first grid probability and the second grid probability;
and a repositioning result determining step of determining the position corresponding to the updating grid probability which enables the correlation function to respond to the maximum as the repositioning result of the robot, wherein the correlation function is a nonlinear function of the updating grid probability corresponding to the end point of the laser radar scanning frame under any position traversed.
2. The method of claim 1, wherein obtaining pose increments of end points of a lidar scan frame of the robot relative to an origin comprises:
collecting a laser radar scanning frame by using a laser radar of the robot;
determining the number of endpoints included in the laser radar scanning frame;
and under the condition that the number of the end points of the laser radar scanning frame is determined to be 1, determining the pose increment of the end points of the laser radar scanning frame relative to the origin.
3. The method of claim 2, wherein obtaining the updated grid probability corresponding to the end point of the lidar scanning frame at the current pose according to the first grid probability and the second grid probability comprises:
calculating a first ratio probability corresponding to a first grid probability occupied by the end points of the laser radar scanning frame before transformation and a second ratio probability corresponding to a second grid probability occupied by the end points of the laser radar scanning frame after transformation;
obtaining an updated ratio probability according to the first ratio probability and the second ratio probability;
and obtaining the updated grid probability according to the updated ratio probability.
4. The method according to claim 3, wherein the relocation result determining step comprises:
respectively substituting the obtained updated grid probability corresponding to the end point of the laser radar scanning frame under each pose into a correlation function to obtain correlation function response corresponding to each pose;
determining the maximum correlation function response from the correlation function responses corresponding to all traversed poses;
and taking the pose corresponding to the maximum correlation function response as the repositioning result.
5. The method of claim 2, wherein obtaining pose increments of end points of a lidar scan frame of the robot relative to an origin further comprises:
and under the condition that the number of the endpoints of the laser radar scanning frame is determined to be more than 1, acquiring the pose increment of each endpoint relative to the origin for each endpoint in the laser radar scanning frame.
6. The method of claim 5, wherein the step of traversing comprises:
under a world coordinate system, traversing each pose in the known grid map, and respectively executing the following steps for each pose:
transforming the laser radar scanning frame from the radar coordinate system to the world coordinate system, enabling the current pose to be the origin pose of the laser radar scanning frame after transformation, and keeping the pose increment of the end point of the laser radar scanning frame relative to the origin before and after transformation unchanged;
respectively executing the following steps aiming at each end point of the converted laser radar scanning frame:
obtaining a second grid probability occupied by the endpoint after transformation;
judging whether the end point is matched with a known grid map or not after transformation according to the second grid probability;
if the matching is judged, the end point is reserved, and the updated grid probability corresponding to the end point under the current pose is obtained according to the first grid probability and the second grid probability occupied by the end point after transformation;
in the case where a mismatch is determined, the endpoint is deleted.
7. The method of claim 6, wherein determining whether the transformed endpoint matches a known grid map comprises:
judging whether the second grid probability occupied by the end point after transformation indicates that the end point and the known grid map have overlapped grids after transformation;
in the presence of overlapping grids, determining that the transformed endpoint matches a known grid map;
in the case where it is determined that there is no overlapping grid, it is determined that the end point after transformation does not match the known grid map.
8. The method according to claim 6, wherein the relocation result determining step comprises:
the following steps are respectively executed for each pose traversed:
for each reserved end point, substituting the updated grid probability corresponding to the end point in the current pose into a correlation function to obtain a correlation function response corresponding to the end point in the current pose;
accumulating the correlation function responses corresponding to all the reserved end points under the current pose, and taking the accumulation result as the correlation function response corresponding to the current pose;
determining the maximum correlation function response from the correlation function responses corresponding to all traversed poses;
and taking the pose corresponding to the maximum correlation function response as the repositioning result.
9. The method of claim 1, wherein the correlation function R isrs(xi) satisfies:
wherein p isξRepresenting the updated grid probability, odd (p), corresponding to the end point of the lidar scanning frame at any pose xi of traversalξ) Is pξThe ratio probability of (c).
10. A robot comprising a processor and a computer readable storage medium storing a computer program which, when executed by the processor, implements a repositioning method for a robot according to any of claims 1 to 9.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910446793.2A CN110174894B (en) | 2019-05-27 | 2019-05-27 | Robot and repositioning method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910446793.2A CN110174894B (en) | 2019-05-27 | 2019-05-27 | Robot and repositioning method thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110174894A CN110174894A (en) | 2019-08-27 |
| CN110174894B true CN110174894B (en) | 2022-03-15 |
Family
ID=67696221
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910446793.2A Active CN110174894B (en) | 2019-05-27 | 2019-05-27 | Robot and repositioning method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110174894B (en) |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110926485B (en) * | 2019-11-11 | 2021-10-08 | 华中科技大学 | Mobile robot positioning method and system based on linear features |
| CN111693053B (en) * | 2020-07-09 | 2022-05-06 | 上海大学 | Repositioning method and system based on mobile robot |
| CN111895990B (en) * | 2020-07-14 | 2022-04-15 | 武汉科技大学 | Method for detecting and repositioning mobile robot based on multi-index binding frame |
| CN111998846B (en) * | 2020-07-24 | 2023-05-05 | 中山大学 | Unmanned system rapid repositioning method based on environment geometry and topological characteristics |
| CN112116656A (en) * | 2020-08-03 | 2020-12-22 | 歌尔股份有限公司 | Incremental mapping method and device in synchronous positioning and mapping slam |
| CN112462758B (en) * | 2020-11-06 | 2022-05-06 | 深圳市优必选科技股份有限公司 | Drawing establishing method and device, computer readable storage medium and robot |
| CN112462769A (en) * | 2020-11-25 | 2021-03-09 | 深圳市优必选科技股份有限公司 | Robot positioning method and device, computer readable storage medium and robot |
| CN112986912B (en) * | 2021-03-19 | 2023-06-16 | 北京小狗吸尘器集团股份有限公司 | Floor sweeper repositioning method and device based on structured light sensor and electronic equipment |
| CN112904367B (en) * | 2021-03-19 | 2023-03-14 | 北京小狗吸尘器集团股份有限公司 | Position scoring method and device applied to relocation of sweeper and electronic equipment |
| CN112956961B (en) * | 2021-03-19 | 2022-08-23 | 北京小狗吸尘器集团股份有限公司 | Sweeping robot, repositioning method and repositioning device thereof, and storage medium |
| CN113510703B (en) * | 2021-06-25 | 2022-09-16 | 深圳市优必选科技股份有限公司 | Robot posture determining method and device, robot and storage medium |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| AU2010286429B2 (en) * | 2009-08-31 | 2013-11-28 | Vorwerk & Co. Interholding Gmbh | Method and apparatus for simultaneous localization and mapping of mobile robot environment |
| EP2776216B1 (en) * | 2011-11-11 | 2022-08-31 | iRobot Corporation | Robot apparautus and control method for resuming operation following a pause. |
| US9020637B2 (en) * | 2012-11-02 | 2015-04-28 | Irobot Corporation | Simultaneous localization and mapping for a mobile robot |
| US9233472B2 (en) * | 2013-01-18 | 2016-01-12 | Irobot Corporation | Mobile robot providing environmental mapping for household environmental control |
| CN105652871A (en) * | 2016-02-19 | 2016-06-08 | 深圳杉川科技有限公司 | Repositioning method for mobile robot |
| CN106052674B (en) * | 2016-05-20 | 2019-07-26 | 青岛克路德机器人有限公司 | A kind of SLAM method and system of Indoor Robot |
| CN107908185A (en) * | 2017-10-14 | 2018-04-13 | 北醒（北京）光子科技有限公司 | A kind of robot autonomous global method for relocating and robot |
| CN108024201A (en) * | 2017-11-30 | 2018-05-11 | 深圳市沃特沃德股份有限公司 | Sweeping robot and its method relocated |
| CN108344999B (en) * | 2018-01-09 | 2020-08-11 | 浙江大学 | Sonar map construction and repositioning method for underwater robot navigation |
| CN108759844B (en) * | 2018-06-07 | 2021-11-16 | 科沃斯商用机器人有限公司 | Robot repositioning and environment map constructing method, robot and storage medium |
| CN109556607A (en) * | 2018-10-24 | 2019-04-02 | 上海大学 | A method of quickly processing localization for Mobile Robot " kidnapping " problem |
-
2019
- 2019-05-27 CN CN201910446793.2A patent/CN110174894B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN110174894A (en) | 2019-08-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110174894B (en) | Robot and repositioning method thereof | |
| CN105865449B (en) | Hybrid positioning method of mobile robot based on laser and vision | |
| CN106796434B (en) | Map generation method, self-position estimation method, robot system, and robot | |
| US8209143B1 (en) | Accurate alignment of multiple laser scans using a template surface | |
| Borrmann et al. | Globally consistent 3D mapping with scan matching | |
| CN110858076B (en) | Equipment positioning and grid map construction method and mobile robot | |
| US8060254B2 (en) | Method, apparatus, and medium for building grid map in mobile robot and method, apparatus, and medium for cell decomposition that uses grid map | |
| MacKenzie et al. | Precise positioning using model-based maps | |
| CN110189366B (en) | Laser coarse registration method and device, mobile terminal and storage medium | |
| CN112068154A (en) | Laser mapping positioning method and device, storage medium and electronic equipment | |
| CN108332758B (en) | Corridor identification method and device for mobile robot | |
| Bülow et al. | Spectral 6dof registration of noisy 3d range data with partial overlap | |
| Bosse | ATLAS: a framework for large scale automated mapping and localization | |
| Tomono | 3-D localization and mapping using a single camera based on structure-from-motion with automatic baseline selection | |
| CN112444246B (en) | Laser fusion positioning method in high-precision digital twin scene | |
| CN110749901A (en) | Autonomous mobile robot, map splicing method and device thereof, and readable storage medium | |
| WO2022142744A1 (en) | Loopback detection method, apparatus and device, and computer readable storage medium | |
| CN111679664A (en) | Three-dimensional map construction method based on depth camera and sweeping robot | |
| TW202036030A (en) | Information processing device and mobile robot | |
| CN108520543B (en) | Method, equipment and storage medium for optimizing relative precision map | |
| CN111609853A (en) | Three-dimensional map construction method, sweeping robot and electronic equipment | |
| CN114565668A (en) | Instant positioning and mapping method and device | |
| WO2022111723A1 (en) | Road edge detection method and robot | |
| CN111113405B (en) | Method for robot to obtain position service and robot | |
| CN114463429B (en) | Robot, map creation method, positioning method, and medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP01 | Change in the name or title of a patent holder |
Address after: 7-605, 6th floor, building 1, yard a, Guanghua Road, Chaoyang District, Beijing 100026 Patentee after: Beijing dog vacuum cleaner Group Co.,Ltd. Address before: 7-605, 6th floor, building 1, yard a, Guanghua Road, Chaoyang District, Beijing 100026 Patentee before: PUPPY ELECTRONIC APPLIANCES INTERNET TECHNOLOGY (BEIJING) Co.,Ltd. |
|
| CP01 | Change in the name or title of a patent holder |