CN111142117B - Hybrid navigation map construction method and system fusing corner plates and occupied grids - Google Patents
Hybrid navigation map construction method and system fusing corner plates and occupied grids Download PDFInfo
- Publication number
- CN111142117B CN111142117B CN201911403235.4A CN201911403235A CN111142117B CN 111142117 B CN111142117 B CN 111142117B CN 201911403235 A CN201911403235 A CN 201911403235A CN 111142117 B CN111142117 B CN 111142117B
- Authority
- CN
- China
- Prior art keywords
- map
- corner
- grid
- laser radar
- line segment
- 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
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/50—Systems of measurement based on relative movement of target
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention is suitable for the technical field of map construction, and provides a hybrid navigation map construction method fusing a folding plate and an occupying grid, which comprises the following specific steps: s1, detecting whether a corner plate exists in the current frame, if so, executing a step S2, and if not, executing a step S3; s2, calculating the current pose of the laser radar in the map coordinate system based on the corner plates, and updating the grid map and the corner plate map based on the current pose of the laser radar in the map coordinate system; and S3, matching the scanning data of the current frame with the grid map, acquiring the current position and posture of the laser radar in the map coordinate system, and updating the grid map based on the position and posture of the laser radar in the map coordinate system. The grid map is constructed in the area where the angle plates are required to be accurately positioned, the map with higher precision is constructed, high-precision pose information is convenient to obtain, the angle plates are inconvenient to arrange or the grid map is constructed in the area where the accurate positioning is not required, the positioning is carried out based on the grid map, and the universality is higher.
Description
Technical Field
The invention belongs to the technical field of map construction, and provides a hybrid navigation map construction method and system fusing a folding plate and an occupying grid.
Background
With the development of society and the advancement of technology, mobile robots are increasingly involved in human daily lives, such as cleaning robots in homes, transfer robots in factories, and meal delivery robots in restaurants. The mobile robot must accurately know the position of the mobile robot to realize the functions, namely real-time positioning. The prerequisite for real-time positioning of mobile robots is the establishment of maps, which is the key to robot navigation and other intelligent behaviors. The laser radar sensor is widely applied to positioning and navigation of the mobile robot because of the advantages of high measurement precision, no influence of illumination conditions and the like. The positioning and navigation method based on the laser radar can be divided into a positioning and navigation method based on a reflective marker and a positioning and navigation method based on a contour, wherein the positioning and navigation method based on the reflective marker has the advantages of high precision, good stability and the like, but some scenes may be inconvenient to lay the reflective marker, and in addition, the cost is increased by laying a large number of reflective markers.
Disclosure of Invention
The embodiment of the invention provides a hybrid navigation map construction method fusing a folding plate and occupying grids, which is used for constructing a grid map in an area where a light-reflecting mark is inconvenient to arrange, positioning is carried out based on the grid map, and universality is improved.
The invention is realized in such a way that a hybrid navigation map construction method fusing a folding plate and an occupying grid is characterized by comprising the following steps:
s1, detecting whether a corner plate exists in the current frame, if so, executing a step S2, and if not, executing a step S3;
s2, calculating the current pose of the laser radar in the map coordinate system based on the corner plates, and updating the grid map and the corner plate map based on the current pose of the laser radar in the map coordinate system;
and S3, matching the scanning data of the current frame with the grid map, acquiring the current position and posture of the laser radar in the map coordinate system, and updating the grid map based on the position and posture of the laser radar in the map coordinate system.
Further, the detection method of the folded angle plate comprises the following steps:
s11, outputting the scanning data of the current frame from the minimum scanning angle to the maximum scanning angle in sequence, and sequentially extracting N line segments meeting the length threshold:
s12, traversing N line segments, calculating the length of any two adjacent line segments and the head-to-tail distance of the two line segments, wherein the head-to-tail distance of the two line segments refers to the distance Dist from the tail of a first line segment to the head of a second line segment, and executing a step S13 on a point set of the two adjacent line segments which meet the condition that L1-L2 are smaller than a threshold value and Dist is smaller than a preset threshold value, wherein L1 and L2 refer to the distance between the first line segment and the second line segment respectively.
S13, calculating the included angle theta of two adjacent line segments and the difference value between the included angle theta and the included angle of the folded plate, and if the difference value is within the allowable deviation range, determining that the adjacent line segment is the projection line segment of the two panels of the folded plate;
s14, calculating a rejection proportionality coefficient div of a first line segment in each corner folding plate, rejecting a point set belonging to a second line segment on the basis of the rejection proportionality coefficient div at the tail of the first point segment, and executing the step S15;
s15, respectively extracting RANSAC interior points of two adjacent line segments of each folded plate, finding two straight lines with the most interior points, performing straight line fitting on the interior points to form projected line segments of the two panels of the folded plate, and calculating the intersection point p ' (x ', y ') of the projected line segments of the two panels of each folded plate and the vector v of the projected line segments of the two panels of the folded plate1′(x1′,y1′),v2′(x2′,y2′)。
Further, the line segment extraction method in S11 is specifically as follows
S111, calculating a line segment L based on the first two points of the current frame;
s112, detecting whether the distance from the next point to the line segment L is smaller than a distance threshold value T, if so, executing a step S113, and if not, executing a step S114;
s113, placing the next point into a point set of the line segment, performing linear least square fitting on the point set, updating the line segment L, and executing the step S112;
s114, calculating the length D of the line segment LLIf legth _ min is less than or equal to DLAnd (4) updating the line segment L if the length _ max is less than or equal to the length _ max, storing the point set into the line set, taking the two points behind the last point in the point set as the starting points of the line segment of the next line segment, calculating the line segment L, and executing the step S112 until all the points in the scanning data of the current frame are traversed。
Further, in step S2, the method for calculating the current pose of the laser radar in the map coordinate system includes the following steps:
s21, extracting the unmarked corner plate b '(p' (x ', y'), v) nearest to the laser radar within a specified distance1′(x1′,y1′),v2′(x2′,y2')) of the folded angle plate b', wherein p '(x', y ') is a two-dimensional projection point of the intersection line of two single planes of the folded angle plate b', v1′(x1′,y1′),v2′(x2′,y2') two-dimensional projection of two single planes of the angle folding plate respectively;
s22, projecting the intersection line projection point p ' (x ', y ') of the angle plate b ' to the grid map based on the pose of the laser radar in the previous frame, and extracting the angle plate b (p (x, y), v) nearest to the intersection line projection point p ' (x ', y ') from the grid map1(x1,y1),v2(x2,y2) If the distance from the intersection line projection point p ' (x ', y ') to p (x, y) is less than the distance threshold, the gusset b is the coordinate of the gusset b ' in the grid map, i.e. the gusset b ' is a known gusset, if the distance from the intersection line projection point p ' (x ', y ') to p (x, y) is greater than the distance threshold, the gusset b ' is an unknown gusset, and the process returns to step S21;
s23, folding board b '(p' (x ', y'), v) in current frame1′(x1′,y1′),v2′(x2′,y2') and matching gussets b (p (x, y), v) in the grid map1(x1,y1),v2(x2,y2) Position) to calculate the robot's current pose in the map coordinate system.
Further, the updating method of the corner board map comprises the following specific steps:
calculating a rotation matrix R and a translational vector T of the laser radar relative to a map coordinate system at present based on the pose of the laser radar in the map coordinate system at present;
and calculating the coordinates of the unknown corner folding plate in the coordinate system of the laser radar based on the rotation matrix R and the translation vector T of the laser radar relative to the coordinate system of the map, and updating the corner folding plate map.
Further, the grid map updating method specifically includes:
for the end point of light beamThe occupied grid is updated, and the slave is updatedToFreely updating the passed grids;
and calculating the occupation probability of the occupied updating grid and the free updating grid, and updating the grid map.
The invention is realized in such a way that a hybrid navigation map construction system based on corner boards and occupied grids comprises:
a corner board arranged in the driving area with accurate positioning, the corner board is composed of two planes with certain angle, the corner board is arranged perpendicular to the ground,
the system comprises a laser radar, a processor and a grid map or a folding plate map, wherein the laser radar is arranged on a mobile machine in a horizontal mode and is connected with the processor, the laser radar is used for collecting environmental data in a driving area and sending scanning data to the processor, and the processor is used for constructing the grid map or the folding plate map based on the hybrid navigation map construction method for fusing folding plates and occupying grids; the processor stores therein environment data of the traveling region.
The invention provides a hybrid navigation map construction method for fusing corner plates and occupying grids, which comprises the following steps: the grid map is constructed in the area where the angle plates are required to be accurately positioned, so that the map with higher precision is constructed, high-precision pose information is convenient to obtain, the grid map is constructed in the area where the angle plates are inconvenient to arrange or the area where the accurate positioning is not required, the positioning can be carried out based on the grid map, and the universality is higher.
Drawings
FIG. 1 is a flow chart of a method for constructing a hybrid navigation map with fused gussets and occupied grids according to an embodiment of the present invention
FIG. 2 is a schematic structural view of a gusset provided in an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Fig. 1 is a flowchart of a method for constructing a hybrid navigation map with fused corner panels and occupied grids according to an embodiment of the present invention, the method including the following steps:
s1, detecting whether a corner plate exists in the current frame, if so, executing a step S2, and if not, executing a step S3;
in the embodiment of the invention, the angle folding plate is composed of two single planes forming a certain angle, the structural schematic diagram of the angle folding plate is shown in fig. 2, and the detection method of the angle folding plate comprises the following steps:
s11, sequentially outputting the scanning data of the current frame from the minimum scanning angle to the maximum scanning angle, and sequentially extracting N line segments meeting the length threshold, wherein the line segment extraction method specifically comprises the following steps:
s111, calculating a line segment L based on the first two points of the current frame;
s112, detecting whether the distance from the next point to the line segment L is smaller than a distance threshold value T, if so, executing a step S113, and if not, executing a step S114;
s113, placing the next point into a point set of the line segment, performing linear least square fitting on the point set, updating the line segment L, and executing the step S112;
s114, calculating the length D of the line segment LLIf legth _ min is less than or equal to DLAnd (4) updating the line segment L if the length _ max is less than or equal to the length _ max, storing the point set into the line set, taking the two points behind the last point in the point set as the starting points of the line segment of the next line segment, calculating the line segment L, and executing the step S112 until the line segment L is up to the point setTraversing all points in the scanning data of the current frame, extracting N line segment point sets meeting the requirements of the length threshold of the line segment from the line set, setting the minimum length of the length threshold as length _ min, and setting the maximum length of the length threshold as length _ max.
212. Traversing N line segments, calculating the lengths of any two adjacent line segments and the head-to-tail distances of the two line segments, wherein the head-to-tail distances of the two line segments refer to the distance Dist from the tail of the first line segment to the head of the second line segment, and executing step S213 on two adjacent line segment point sets which meet the condition that L1-L2 are smaller than a threshold value and the Dist is smaller than a preset threshold value, wherein L1 and L2 refer to the distances of the first line segment and the second line segment respectively.
S13, calculating the included angle theta of two adjacent line segments and the difference value between the included angle theta and the included angle of the folded plate, and if the difference value is within the allowable deviation range, determining that the adjacent line segment is the projection line segment of the two panels of the folded plate;
calculating the intersection point of two adjacent line segments, and respectively constructing two unit vectors by the intersection point and two data points in the point set of the two adjacent line segmentsAndcalculating angle θ using vector dot product:
s14, calculating a rejection proportionality coefficient div of a first line segment in each corner folding plate, rejecting a point set belonging to a second line segment on the basis of the rejection proportionality coefficient div at the tail of the first point segment, and executing the step S15;
screening an original point set of a first line segment of the angle folding plate, rejecting a point set originally belonging to a second line segment, and rejecting a proportionality coefficient div to obtain the following result from an included angle theta of the angle folding plate and a distance threshold T:
wherein a is the actual distance of the single panel of the folded angle plate in the projection direction.
S15, respectively extracting RANSAC interior points of two adjacent line segments of each folded plate, finding two straight lines with the most interior points, performing straight line fitting on the interior points to form projected line segments of the two panels of the folded plate, and calculating the intersection point p ' (x ', y ') of the projected line segments of the two panels of each folded plate and the vector v of the projected line segments of the two panels of the folded plate1′(x1′,y1′),v2′(x2′,y2′);
Respectively extracting RANSAC interior points of two line segment point sets, specifically setting a RANSAC linear interior point judgment threshold value, setting an iteration stop time threshold value, finally finding the maximum number of the interior points of the straight line where two points are located in the original point set of the line segment, storing the interior point set, performing linear least square fitting again to obtain two high-precision line segments of the folded plate, calculating the intersection point and the vector of the two line segments, the intersection point p '(x', y ') of the two panel projection line segments of the folded plate and the vector v of the two panel projection line segments of the folded plate, and calculating the intersection point p' (x ', y') of the two panel projection line segments of the folded plate1′(x1′,y1′),v2′(x2′,y2′)。
S2, calculating the current pose of the laser radar in the map coordinate system based on the corner plates, and updating the grid map and the corner plate map based on the current pose of the laser radar in the map coordinate system;
in the embodiment of the invention, the method for calculating the current pose of the laser radar in the map coordinate system specifically comprises the following steps:
s21, extracting the unmarked corner plate b '(p' (x ', y'), v) nearest to the laser radar within a specified distance1′(x1′,y1′),v2′(x2′,y2')) of the folded angle plate b', wherein p '(x', y ') is a two-dimensional projection point of the intersection line of two single planes of the folded angle plate b', v1′(x1′,y1′),v2′(x2′,y2') two-dimensional projection of two single planes of the angle folding plate respectively;
s22, projecting the intersection line projection point p ' (x ', y ') of the angle plate b ' to the grid map based on the pose of the laser radar in the previous frame, and extracting the angle plate b (p (x, y), v) nearest to the intersection line projection point p ' (x ', y ') from the grid map1(x1,y1),v2(x2,y2) If the distance from the intersection line projection point p ' (x ', y ') to p (x, y) is less than the distance threshold, the folded corner plate b is the coordinate of the folded corner plate b ' in the grid map, that is, the folded corner plate b ' is a known folded corner plate, step S23 is executed, if the distance from the intersection line projection point p ' (x ', y ') to p (x, y) is greater than the distance threshold, the folded corner plate b ' is an unknown folded corner plate, that is, there is no matched folded corner plate in the grid map, and the process returns to step S21;
in the embodiment of the invention, the map coordinate system comprises a grid map and a rectangular map, the corner plates already included in the map coordinate system are known corner plates, the corner plates not included in the map coordinate system are unknown corner plates, and the construction process of the hybrid map is to continuously include the unknown corner plates in the map coordinate system.
S23, folding board b '(p' (x ', y'), v) in current frame1′(x1′,y1′),v2′(x2′,y2') and matching gussets b (p (x, y), v) in the grid map1(x1,y1),v2(x2,y2) The current pose of the robot in the map coordinate system, that is, the current pose P (x, y, θ) of the laser radar in the map coordinate system, is calculated according to the following formula:
r, T are respectively a rotation matrix and a translation vector from a map coordinate system to a radar coordinate system, and the pose P (x, y, theta) of the robot is calculated based on R, T:
pos=-R-1*T
θ=arctan(R1,0/R0,0)
wherein,R1,0/R0,0The second row and the first column of the rotation matrix R are divided by the first row and the first column of the rotation matrix, pos is the position coordinate of the mobile robot, theta is the attitude angle of the mobile robot, and the position coordinate and the attitude angle of the mobile robot constitute the pose of the robot.
Calculating the coordinate of the unknown corner plate in the map coordinate system based on the current pose of the laser radar in the map coordinate system, wherein the calculation method comprises the following specific steps:
calculating a rotation matrix R and a translational vector T of the laser radar relative to a map coordinate system based on the current pose of the laser radar in the map coordinate system;
suppose the coordinates b '(p' (x ', y'), v) of the unknown corner plate in the lidar coordinate system1′(x1′,y1′),v2′(x2′,y2') of the corner panel is known at map coordinates b (p (x, y), v) of the map1(x1,y1),v2(x2,y2) The calculation method is as follows:
in order to improve the accuracy of calculating the position of the corner plate, the coordinate average value of 50 times of calculation can be taken as the final corner plate coordinate.
Updating the grid map based on the current pose of the laser radar in a map coordinate system, wherein the updating method specifically comprises the following steps:
knowing the pose P (x, y, theta) of the laser radar in the map coordinate system, the position of the laser radar in the grid map isl is the actual size of the side length of a single grid, the conversion matrix from the scanning point in the laser radar coordinate system to the grid map coordinate system is M, and the expression of M is as follows:
wherein P ismI.e. the starting point of the laser radar beam, and the homogeneous coordinate of the ith laser scanning point in the laser radar coordinate system is known as pi(xi,yi1), the coordinates of the end point of the ith light beam in the grid map are
S24, pair of beam end pointsThe occupied grid is updated, and the slave is updatedToFreely updating the passed grids;
knowing the origin of a light beamAnd an end pointk is the number of grids the beam passes through, and the probability of the beam passing through the grids can be updated by using the log-of-superiority integration method. To pairThe occupied grid is updated, and the slave is updatedToThe grids passed by are freely updated. The freely updated value oddsFree is set to 0.4 and the updated value oddsoc is taken to 0.6.
And S25, calculating the occupation probability of the occupied updating grid and the free updating grid, and updating the grid map.
Assuming that the occupancy probability of the grid before update is p, the odds value of the grid before update is odds ═ log (p/(1-p)), if the current grid is updated freely, odds ═ odds + oddsFree, if the current grid is updated, odds ═ odds + oddsoc, and after update, the occupancy probability of the grid is p ═ exp (odds))/(exp (odds)) + 1).
And S3, acquiring the current pose of the laser radar in the map coordinate system based on the matching of the scanning data and the grid map, and updating the grid map based on the pose of the laser radar in the map coordinate system.
Mapping each scanning point to a grid map according to the pose of the laser radar at the previous moment, and constructing the following objective function based on all the scanning points on the grid map:
Si(xi) denotes the transformation of the ith laser point at time t into the grid map coordinate system, M (S)i(ξ)) represents the probability that the ith laser point's position on the grid map is occupied, thus transforming the pose calculation into a least squares solution problem; computing M (S) using bilinear differencesi(xi)), and for M (S)i(ξ)), converting the least square solution of the pose calculation into an iterative pose increment calculation problem, and determining the positioning of the laser radar current frame in the grid map based on the pose increment.
Updating the grid map based on the position of the laser radar in the map coordinate system at present:
knowing the pose P (x, y, theta) of the laser radar in the map coordinate system, the position of the laser radar in the grid map isl is the actual size of the side length of a single grid, the conversion matrix from the scanning point in the laser radar coordinate system to the grid map coordinate system is M, and the expression of M is specifically shown asThe following:
wherein P ismI.e. the starting point of the laser radar beam, and the homogeneous coordinate of the ith laser scanning point in the laser radar coordinate system is known as pi(xi,yi1), the coordinates of the end point of the ith light beam in the grid map are
S31, end point of light beamThe occupied grid is updated, and the slave is updatedToFreely updating the passed grids;
knowing the origin of a light beamAnd an end pointk is the number of grids the beam passes through, and the probability of the beam passing through the grids can be updated by using the log-of-superiority integration method. To pairThe occupied grid is updated, and the slave is updatedToThe passed grid is freeAnd (6) updating. The freely updated value oddsFree is set to 0.4 and the updated value oddsoc is taken to 0.6.
And S32, calculating the occupation probability of the occupied updating grid and the free updating grid, and updating the grid map.
Assuming that the occupancy probability of the grid before update is p, the odds value of the grid before update is odds ═ log (p/(1-p)), if the current grid is updated freely, odds ═ odds + oddsFree, if the current grid is updated, odds ═ odds + oddsoc, and after update, the occupancy probability of the grid is p ═ exp (odds))/(exp (odds)) + 1).
The invention also provides a hybrid navigation map construction system based on the angle folding plate and the occupied grid, which comprises the following components:
the system comprises a folding plate, a processor and a navigation system, wherein the folding plate is arranged in a driving area with accurate positioning and is composed of two planes with certain angles, the folding plate is arranged perpendicular to the ground and is arranged on a laser radar which is arranged on a mobile machine in a horizontal mode, the laser radar is connected with the processor, the laser radar collects environmental data in the driving area and sends scanning data to the processor, and the processor constructs a grid map or a folding plate map based on the hybrid navigation map construction method which integrates the folding plate and occupies grids; the processor stores therein environment data of the traveling region.
The invention provides a hybrid navigation map construction method for fusing corner plates and occupying grids, which comprises the following steps: the grid map is constructed in the area where the angle plates are required to be accurately positioned, so that the map with higher precision is constructed, high-precision pose information is convenient to obtain, the grid map is constructed in the area where the angle plates are inconvenient to arrange or the area where the accurate positioning is not required, the positioning can be carried out based on the grid map, and the universality is higher.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents and improvements made within the spirit and principle of the present invention are intended to be included within the scope of the present invention.
Claims (6)
1. A hybrid navigation map construction method fusing corner panels with occupied grids, characterized by comprising the steps of:
s1, detecting whether a corner plate exists in the current frame, if so, executing a step S2, and if not, executing a step S3;
s2, calculating the current pose of the laser radar in the map coordinate system based on the corner plates, and updating the grid map and the corner plate map based on the current pose of the laser radar in the map coordinate system;
s3, matching the scanning data of the current frame with the grid map, acquiring the current pose of the laser radar in a map coordinate system, and updating the grid map based on the pose of the laser radar in the map coordinate system;
the detection method of the folded angle plate comprises the following steps:
s11, outputting the scanning data of the current frame from the minimum scanning angle to the maximum scanning angle in sequence, and sequentially extracting N line segments meeting the length threshold:
s12, traversing N line segments, calculating the length of any two adjacent line segments and the head-to-tail distance of the two line segments, wherein the head-to-tail distance of the two line segments is the distance Dist from the tail of a first line segment to the head of a second line segment, and executing a step S13 on a point set of the two adjacent line segments which meet the condition that L1-L2 are smaller than a threshold value and Dist is smaller than a preset threshold value, wherein L1 and L2 are the distances of the first line segment and the second line segment respectively;
s13, calculating the included angle theta of two adjacent line segments and the difference value between the included angle theta and the included angle of the folded plate, and if the difference value is within the allowable deviation range, determining that the adjacent line segment is the projection line segment of the two panels of the folded plate;
s14, calculating a rejection proportionality coefficient div of a first line segment in each corner folding plate, rejecting a point set belonging to a second line segment on the basis of the rejection proportionality coefficient div at the tail of the first point segment, and executing the step S15;
s15, respectively extracting RANSAC interior points of two adjacent line segments of each folded plate, finding two straight lines with the most interior points, performing straight line fitting on the interior points to form projected line segments of the two panels of the folded plate, and calculating the intersection point p ' (x ', y ') of the projected line segments of the two panels of each folded plate and the vector v of the projected line segments of the two panels of the folded plate1′(x1′,y1′),v2′(x2′,y2′)。
2. The method for constructing a hybrid navigation map by fusing a gusset and an occupancy grid according to claim 1, wherein the line segment extraction method in S11 is specifically as follows:
s111, calculating a line segment L based on the first two points of the current frame;
s112, detecting whether the distance from the next point to the line segment L is smaller than a distance threshold value T, if so, executing a step S113, and if not, executing a step S114;
s113, placing the next point into a point set of the line segment, performing linear least square fitting on the point set, updating the line segment L, and executing the step S112;
s114, calculating the length D of the line segment LLIf legth _ min is less than or equal to DLAnd (4) updating the line segment L if the length _ max is less than or equal to the length _ max, storing the point set into the line set, taking the two points behind the last point in the point set as the starting points of the line segment of the next line segment, calculating the line segment L, and executing the step S112 until all the points in the scanning data of the current frame are traversed.
3. The hybrid navigation map construction method for fusing the folding plates and the occupying grids according to any one of claims 1 to 2, wherein the method for calculating the current pose of the laser radar in the map coordinate system in the step S2 is as follows:
s21, extracting the unmarked corner plate b '(p' (x ', y'), v) nearest to the laser radar within a specified distance1′(x1′,y1′),v2′(x2′,y2')) of the folded angle plate b', wherein p '(x', y ') is a two-dimensional projection point of the intersection line of two single planes of the folded angle plate b', v1′(x1′,y1′),v2′(x2′,y2') two-dimensional projection of two single planes of the angle folding plate respectively;
s22 projecting the intersection line of the angle folding plate b' based on the pose in the last frame of the laser radarProjecting the point p '(x', y ') into the grid map, and extracting the corner plate b (p (x, y), v) nearest to the intersection line projection point p' (x ', y') from the grid map1(x1,y1),v2(x2,y2) If the distance from the intersection line projection point p ' (x ', y ') to p (x, y) is less than the distance threshold, the gusset b is the coordinate of the gusset b ' in the grid map, i.e. the gusset b ' is a known gusset, if the distance from the intersection line projection point p ' (x ', y ') to p (x, y) is greater than the distance threshold, the gusset b ' is an unknown gusset, and the process returns to step S21;
s23, folding board b '(p' (x ', y'), v) in current frame1′(x1′,y1′),v2′(x2′,y2') and matching gussets b (p (x, y), v) in the grid map1(x1,y1),v2(x2,y2) Position) to calculate the robot's current pose in the map coordinate system.
4. The method for constructing a hybrid navigation map by fusing corner plates and occupying grids according to claim 3, wherein the updating method of the corner plate map is as follows:
calculating a rotation matrix R and a translational vector T of the laser radar relative to a map coordinate system at present based on the pose of the laser radar in the map coordinate system at present;
and calculating the coordinates of the unknown corner folding plate in the coordinate system of the laser radar based on the rotation matrix R and the translation vector T of the laser radar relative to the coordinate system of the map, and updating the corner folding plate map.
5. The method for constructing a hybrid navigation map by fusing corner plates and occupying grids according to claim 3, wherein the grid map is updated by the following method:
for the end point of light beamThe occupied grid is updated, and the slave is updatedToFreely updating the passed grids;
and calculating the occupation probability of the occupied updating grid and the free updating grid, and updating the grid map.
6. A hybrid navigation map construction system based on corner panels and occupancy grids, the system comprising:
a corner board arranged in the driving area with accurate positioning, the corner board is composed of two planes with certain angle, the corner board is arranged perpendicular to the ground,
the system comprises a laser radar, a processor and a grid map or a corner plate map, wherein the laser radar is arranged on a mobile machine in a horizontal mode and is connected with the processor, the laser radar collects environmental data in a driving area and sends scanning data to the processor, and the processor constructs the grid map or the corner plate map based on the hybrid navigation map construction method for fusing corner plates and occupying grids according to any one of claims 1 to 5; the processor stores therein environment data of the traveling region.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911403235.4A CN111142117B (en) | 2019-12-31 | 2019-12-31 | Hybrid navigation map construction method and system fusing corner plates and occupied grids |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911403235.4A CN111142117B (en) | 2019-12-31 | 2019-12-31 | Hybrid navigation map construction method and system fusing corner plates and occupied grids |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111142117A CN111142117A (en) | 2020-05-12 |
CN111142117B true CN111142117B (en) | 2021-11-05 |
Family
ID=70522353
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911403235.4A Active CN111142117B (en) | 2019-12-31 | 2019-12-31 | Hybrid navigation map construction method and system fusing corner plates and occupied grids |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111142117B (en) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2022067534A1 (en) * | 2020-09-29 | 2022-04-07 | 华为技术有限公司 | Occupancy grid map generation method and device |
CN115561730B (en) * | 2022-11-11 | 2023-03-17 | 湖北工业大学 | Positioning navigation method based on laser radar feature recognition |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106969768A (en) * | 2017-04-22 | 2017-07-21 | 深圳力子机器人有限公司 | A kind of trackless navigation AGV's is accurately positioned and parking method |
CN109000649A (en) * | 2018-05-29 | 2018-12-14 | 重庆大学 | A kind of all directionally movable robot pose calibration method based on right angle bend feature |
CN109613547A (en) * | 2018-12-28 | 2019-04-12 | 芜湖哈特机器人产业技术研究院有限公司 | It is a kind of that grating map construction method is occupied based on reflector |
CN109631919A (en) * | 2018-12-28 | 2019-04-16 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of hybrid navigation map constructing method for merging reflector and occupying grid |
CN110361010A (en) * | 2019-08-13 | 2019-10-22 | 中山大学 | It is a kind of based on occupy grating map and combine imu method for positioning mobile robot |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9919425B2 (en) * | 2015-07-01 | 2018-03-20 | Irobot Corporation | Robot navigational sensor system |
CN109064506B (en) * | 2018-07-04 | 2020-03-13 | 百度在线网络技术(北京)有限公司 | High-precision map generation method and device and storage medium |
-
2019
- 2019-12-31 CN CN201911403235.4A patent/CN111142117B/en active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106969768A (en) * | 2017-04-22 | 2017-07-21 | 深圳力子机器人有限公司 | A kind of trackless navigation AGV's is accurately positioned and parking method |
CN109000649A (en) * | 2018-05-29 | 2018-12-14 | 重庆大学 | A kind of all directionally movable robot pose calibration method based on right angle bend feature |
CN109613547A (en) * | 2018-12-28 | 2019-04-12 | 芜湖哈特机器人产业技术研究院有限公司 | It is a kind of that grating map construction method is occupied based on reflector |
CN109631919A (en) * | 2018-12-28 | 2019-04-16 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of hybrid navigation map constructing method for merging reflector and occupying grid |
CN110361010A (en) * | 2019-08-13 | 2019-10-22 | 中山大学 | It is a kind of based on occupy grating map and combine imu method for positioning mobile robot |
Also Published As
Publication number | Publication date |
---|---|
CN111142117A (en) | 2020-05-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109631919B (en) | Hybrid navigation map construction method integrating reflector and occupied grid | |
CN109613547B (en) | Method for constructing occupied grid map based on reflector | |
CN108012325B (en) | Navigation positioning method based on UWB and binocular vision | |
CN109725327B (en) | Method and system for building map by multiple machines | |
CN109613550B (en) | Laser radar map construction and positioning method based on reflector | |
CN112764053B (en) | Fusion positioning method, device, equipment and computer readable storage medium | |
CN109613548B (en) | Laser radar road sign map construction method based on graph optimization | |
JP5588812B2 (en) | Image processing apparatus and imaging apparatus using the same | |
CN103257342B (en) | Three-dimension laser sensor and two-dimension laser sensor combined calibration method | |
CN108180917B (en) | Top map construction method based on pose graph optimization | |
US7446766B2 (en) | Multidimensional evidence grids and system and methods for applying same | |
CN103913162B (en) | The mobile platform of enhancing positions | |
CN112070770B (en) | High-precision three-dimensional map and two-dimensional grid map synchronous construction method | |
CN109613549B (en) | Laser radar positioning method based on Kalman filtering | |
CN108921947A (en) | Generate method, apparatus, equipment, storage medium and the acquisition entity of electronic map | |
CN103499337B (en) | Vehicle-mounted monocular camera distance and height measuring device based on vertical target | |
CN110243380A (en) | A kind of map-matching method based on multi-sensor data and angle character identification | |
CN104914865A (en) | Transformer station inspection tour robot positioning navigation system and method | |
CN111142117B (en) | Hybrid navigation map construction method and system fusing corner plates and occupied grids | |
Bosse | ATLAS: a framework for large scale automated mapping and localization | |
CN111260751B (en) | Mapping method based on multi-sensor mobile robot | |
CN111273304A (en) | Natural positioning method and system for fusion reflecting column | |
CN114549738A (en) | Unmanned vehicle indoor real-time dense point cloud reconstruction method, system, equipment and medium | |
Khurana et al. | Extrinsic calibration methods for laser range finder and camera: A systematic review | |
CN113028990B (en) | Laser tracking attitude measurement system and method based on weighted least square |
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 |