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 PDF

Info

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
Application number
CN201911403235.4A
Other languages
Chinese (zh)
Other versions
CN111142117A (en
Inventor
陈智君
伍永健
郝奇
高云峰
曹雏清
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhu Hit Robot Technology Research Institute Co Ltd
Original Assignee
Wuhu Hit Robot Technology Research Institute Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhu Hit Robot Technology Research Institute Co Ltd filed Critical Wuhu Hit Robot Technology Research Institute Co Ltd
Priority to CN201911403235.4A priority Critical patent/CN111142117B/en
Publication of CN111142117A publication Critical patent/CN111142117A/en
Application granted granted Critical
Publication of CN111142117B publication Critical patent/CN111142117B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

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

Hybrid navigation map construction method and system fusing corner plates and occupied grids
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 beam
Figure GDA0002436702020000031
The occupied grid is updated, and the slave is updated
Figure GDA0002436702020000032
To
Figure GDA0002436702020000033
Freely 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 segments
Figure GDA0002436702020000051
And
Figure GDA0002436702020000052
calculating angle θ using vector dot product:
Figure GDA0002436702020000053
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:
Figure GDA0002436702020000061
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:
Figure GDA0002436702020000071
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:
Figure GDA0002436702020000081
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 is
Figure GDA0002436702020000082
l 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:
Figure GDA0002436702020000083
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
Figure GDA0002436702020000084
S24, pair of beam end points
Figure GDA0002436702020000085
The occupied grid is updated, and the slave is updated
Figure GDA0002436702020000086
To
Figure GDA0002436702020000087
Freely updating the passed grids;
knowing the origin of a light beam
Figure GDA0002436702020000088
And an end point
Figure GDA0002436702020000089
k 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 pair
Figure GDA00024367020200000810
The occupied grid is updated, and the slave is updated
Figure GDA00024367020200000811
To
Figure GDA00024367020200000812
The 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:
Figure GDA0002436702020000091
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 is
Figure GDA0002436702020000092
l 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:
Figure GDA0002436702020000093
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
Figure GDA0002436702020000094
S31, end point of light beam
Figure GDA0002436702020000095
The occupied grid is updated, and the slave is updated
Figure GDA0002436702020000096
To
Figure GDA0002436702020000097
Freely updating the passed grids;
knowing the origin of a light beam
Figure GDA0002436702020000098
And an end point
Figure GDA0002436702020000099
k 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 pair
Figure GDA0002436702020000101
The occupied grid is updated, and the slave is updated
Figure GDA0002436702020000102
To
Figure GDA0002436702020000103
The 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 beam
Figure FDA0003196128560000031
The occupied grid is updated, and the slave is updated
Figure FDA0003196128560000032
To
Figure FDA0003196128560000033
Freely 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.
CN201911403235.4A 2019-12-31 2019-12-31 Hybrid navigation map construction method and system fusing corner plates and occupied grids Active CN111142117B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (5)

* Cited by examiner, † Cited by third party
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
CN108571971B (en) AGV visual positioning system and method
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
CN110031817B (en) Rapid matching method for laser radar reflector
CN103913162B (en) The mobile platform of enhancing positions
CN109613549B (en) Laser radar positioning method based on Kalman filtering
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
CN112070770B (en) High-precision three-dimensional map and two-dimensional grid map synchronous construction method
CN111260751B (en) Mapping method based on multi-sensor mobile robot
WO2021254019A1 (en) Method, device and system for cooperatively constructing point cloud map
CN103136789A (en) Traffic accident road base map information processing method based on topographic map and image
CN113743171A (en) Target detection method and device
CN111273304A (en) Natural positioning method and system for fusion reflecting column
CN112799096A (en) Map construction method based on low-cost vehicle-mounted two-dimensional laser radar
CN114549738A (en) Unmanned vehicle indoor real-time dense point cloud reconstruction method, system, equipment 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