CN111076733B - Robot indoor map building method and system based on vision and laser slam - Google Patents

Robot indoor map building method and system based on vision and laser slam Download PDF

Info

Publication number
CN111076733B
CN111076733B CN201911257094.XA CN201911257094A CN111076733B CN 111076733 B CN111076733 B CN 111076733B CN 201911257094 A CN201911257094 A CN 201911257094A CN 111076733 B CN111076733 B CN 111076733B
Authority
CN
China
Prior art keywords
pose
robot
laser
grid
map
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
CN201911257094.XA
Other languages
Chinese (zh)
Other versions
CN111076733A (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.)
Yijiahe Technology Co Ltd
Original Assignee
Yijiahe Technology 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 Yijiahe Technology Co Ltd filed Critical Yijiahe Technology Co Ltd
Priority to CN201911257094.XA priority Critical patent/CN111076733B/en
Publication of CN111076733A publication Critical patent/CN111076733A/en
Application granted granted Critical
Publication of CN111076733B publication Critical patent/CN111076733B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • 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

Abstract

The invention discloses a robot indoor map building method based on vision and laser slam, which comprises the following steps: a visual sensor acquires a left view and a right view to generate a feature point cloud; calculating a pose transformation estimate for the vision-based robot; obtaining laser data, calculating the confidence of pose transformation estimation and map grid updating of the robot based on laser; generating subgraphs by using continuous laser point cloud data by adopting a tile type mapping method; matching the obtained characteristic points with each sub-graph; and updating the pose of the robot in each sub-image through image optimization to form a final image construction result. The invention adopts a mapping method combining vision and laser, on one hand, the defect that a vision algorithm is greatly influenced by illumination is improved, on the other hand, closed loop is detected by using the descriptor attributes of the characteristic points, the problem that the success rate of laser closed loop detection is too low is solved, and the closed loop detection efficiency is improved.

Description

Robot indoor map building method and system based on vision and laser slam
Technical Field
The invention relates to the field of inspection robots, in particular to a robot indoor map building method and system based on vision and laser slam.
Background
The completion of various polling tasks of the robot is directly influenced by the quality of the indoor navigation environment. To complete the map creation, information about the surroundings is first obtained, which is done using sensors. The drawing research hot spot of the existing robot is mainly concentrated on two sensors, one is a laser sensor, the laser sensor has the advantages of high resolution, high distance measurement precision, strong active interference resistance, good detection performance and no influence of light rays, and the defects are that the obtained information does not have semantic information, the registration precision is not high, and closed loop detection failure often exists in the drawing establishing process. The other vision sensor can obtain abundant texture information and has the advantage of accurate matching, but has the defect of large influence by illumination change.
In order to overcome the defects of a laser sensor and a vision sensor, the invention provides a robot indoor map building method and system based on vision and laser slam.
Disclosure of Invention
In order to solve the above problems, the present invention provides a robot indoor map building method based on vision and laser slam, comprising the following steps:
the visual sensor acquires a left view and a right view, and generates a feature point cloud, which specifically comprises the following steps:
extracting feature points of the left view and the right view,
the obtained feature points are matched with each other,
calculating the space coordinates of the matched characteristic points;
calculating pose transformation estimation of the vision-based robot, namely performing secondary matching on the feature points matched with the left view and the right view at the moment and the feature points matched with the left view and the right view at the last moment at the moment, acquiring a pose transformation relation of the robot according to coordinate changes of the secondary matched feature points and main direction changes of the feature points, projecting descriptors of the feature points onto corresponding map grids, and defining feature point descriptor attributes of the grids;
obtaining laser data, calculating the confidence of pose transformation estimation and map grid updating of the robot based on laser;
generating subgraphs by using continuous laser point cloud data by adopting a tile type mapping method;
closed loop detection, namely matching descriptors of the obtained feature points with the attribute of the feature point descriptor of the grid corresponding to each grid of each sub-graph;
the graph optimization specifically comprises the following steps: the pose of the robot is represented by the nodes of the graph, the transformation relation between the poses of the robot is represented by the edges connecting the nodes, the error between each pose of the robot and the estimated pose transformed by the transformation matrix is calculated, the error objective function of the graph with a plurality of edges is calculated, the pose of the robot converging the objective function is obtained through multiple iterations, and the pose of the robot in each sub-graph is used for updating the pose of the robot in each sub-graph, so that the final graph building result can be formed.
Further, the extracting feature points of the left view and the right view comprises:
performing FAST corner detection on the left view and the right view by using an ORB algorithm, and extracting a characteristic point P;
establishing a coordinate system with the characteristic point P as an origin, calculating a centroid position C in the field S, and constructing a vector with the characteristic point P as a starting point and the centroid C as an end point
Figure GDA0003570351060000021
The moment of the domain S can be expressed as:
Figure GDA0003570351060000022
wherein Y (x, Y) is the gray value of the image, x, Y belongs to [ -r, r ], r is the radius of the neighborhood S, and p and q are 0 or 1, then the centroid position of the neighborhood is:
Figure GDA0003570351060000023
the principal direction θ of the feature point P is:
θ＝arctan(m01/m10)；
generating a characteristic point descriptor:
within the neighborhood S of the feature point P, a random set of n pairs of points is generated, represented by a2 × n matrix:
Figure GDA0003570351060000024
Figure GDA0003570351060000025
principal direction θ of feature points and corresponding rotation matrix RθCalculate the rotated 2 × n matrix:
Figure GDA0003570351060000026
wherein the content of the first and second substances,
Figure GDA0003570351060000027
the binary test criterion in the neighborhood S of the feature point P may be defined as:
Figure GDA0003570351060000028
wherein iθ＝i1θ，…，inθ，jθ＝j1θ，…，jnθAnd tau denotes a binary test criterion,
Figure GDA0003570351060000029
represents a point iθIs determined by the gray-scale value of (a),
Figure GDA00035703510600000210
represents a point jθThe gray value of (a);
a descriptor of the rotation of the feature point P is now available:
Figure GDA00035703510600000211
k' is 128, 256 or 512, and the size of the corresponding descriptor is 16 bytes, 32 bytes or 64 bytes, respectively, which is selected according to the real-time performance, storage space and recognition efficiency of the method.
Further, the matching of the obtained feature points specifically includes:
calculating the Hamming distance D of the descriptors of the feature points of the left view and the right view for matching:
Figure GDA0003570351060000031
wherein f isL，fRDescriptors of a certain characteristic point of the left view and the right view respectively; when two feature points with the Hamming distance smaller than the threshold exist in the two frames of images, the two feature points are successfully matched.
Further, the calculating the spatial coordinates of the matched feature points specifically includes:
Figure GDA0003570351060000032
wherein (x, y, z) is the coordinate of the characteristic point P in space, (u)L，vL) For the image point coordinates of the feature point P in the left camera, (u)R，vR) As coordinates of image points in the right camera, fFocal lengthIs the focal length of the binocular camera and d is the distance between the two cameras.
Further, the tile-type map building method, which generates sub-maps by using continuous laser point cloud data, specifically includes:
and generating sub-graphs by using continuous laser point cloud data, wherein each continuous Ts data generates one sub-graph, and the second sub-graph comprises the data in the last Ts of the previous sub-graph, wherein T is less than T.
Further, the graph optimization specifically includes:
the pose of the robot is represented as nodes in the graph, observation information is converted into a constraint relation between poses of the robots after being processed, and the constraint relation is represented by connecting edges between the nodes; each node of the graph depends on the description of the pose of the robot, and the state equation of all the nodes is
Figure GDA0003570351060000033
Wherein
Figure GDA0003570351060000034
Respectively the poses of the robot under the global coordinate system, and the positions of the robots are describedThe transformation relationship of (1); the observation equation between node Pi and node Pj can be expressed as:
zj＝xiTi，j
wherein x isiIs a node PiPose, x, of the corresponding robot in the global coordinate systemjIs a node PjPose, z, of the corresponding robot in the global coordinate systemjIs a node PjOf observed value of (a), i.e. xiEstimated pose after transformation by transformation matrix, Ti，jIs a node PiAnd node PjThe transformation relationship between the two;
robot pose xjAnd xiEstimated pose z after transformation by transformation matrixjThere is an error e (z) betweenj，xj) The calculation formula is as follows:
e(zj，xj)＝xiTi，j-xj
the error objective function e (x) of the graph with several edges is:
Figure GDA0003570351060000035
wherein the information matrix omegakIs the inverse of the covariance matrix, is a symmetric matrix; each element omega thereofi，jAs a coefficient of error, pair e (z)i，xi)、e(zj，xj) The error correlation of (a) is predicted;
let x bekAnd adding an increment delta X, finally obtaining a convergence value meeting the objective equation E (X) through multiple iterations, updating the state variable X by using the obtained delta X, and forming a final mapping result by using the pose of each node in the X.
A robot indoor map system based on vision and laser slam comprises a front end and a rear end, wherein the front end of the system acquires a left view and a right view of an environment by using a binocular vision sensor, extracts the left view and the right view by using an ORB algorithm, matches feature points of the left view and the right view, and further calculates coordinates of each feature point in a space; matching the feature points obtained at adjacent moments, further solving the pose transformation relation of the robot at the moments, projecting the feature points onto a map grid, and defining that the grid map with the feature point projection has descriptor attributes; the front end of the system obtains laser point cloud data of the environment through a laser sensor, de-noizes the laser point cloud data, obtains pose transformation estimation of the robot by obtaining a pose initial value of the robot, defining a scanning window and obtaining a confidence coefficient, projects the laser point cloud on a grid map, and updates the confidence coefficient of each map grid; and the back end of the system performs closed-loop detection and map optimization on the obtained map.
Compared with the prior art, the invention has the following beneficial effects:
by adopting the mapping method combining vision and laser, the defect that a vision algorithm is greatly influenced by illumination is overcome, the closed loop is detected by using the descriptor attributes of the characteristic points, the problem of low success rate of laser closed loop detection is solved, and the closed loop detection efficiency is improved.
Drawings
Fig. 1 is a robot indoor positioning and mapping process based on vision and laser slam.
FIG. 2 is a schematic diagram of the M16 template.
Detailed Description
The following describes a mobile robot indoor positioning and mapping method based on vision and laser slam in detail with reference to the accompanying drawings.
A mobile robot indoor positioning and mapping method based on vision and laser slam is disclosed, and the flow is shown in figure 1. Specifically, the indoor positioning and mapping system of the mobile robot based on vision and laser slam is divided into a front end and a rear end, the left view and the right view of the environment are obtained by the front end of the system through a binocular vision sensor, the left view and the right view are extracted through an ORB algorithm, feature points of the left view and the right view are matched, and then coordinates of the feature points in the space are obtained; and then matching the feature points obtained at adjacent moments, further solving the pose transformation relation of the robot at the moments, projecting the feature points onto a map grid, and defining that the grid projected by the feature points has descriptor attributes. Meanwhile, the front end of the system obtains laser point cloud data of the environment through a two-dimensional laser sensor, de-noizes the laser point cloud data, obtains pose transformation estimation of the robot through the modes of solving an initial pose value of the robot, defining a scanning window, solving a confidence coefficient and the like, projects the laser point cloud on a grid map on the basis, and updates the confidence coefficient of each map grid. And finally, in order to reduce the accumulated error of the sensor, the back end of the system performs closed-loop detection and map optimization on the obtained map.
First, system front end
1. ORB-based feature point cloud generation
(1) Extracting characteristic points: the binocular vision sensor can simultaneously obtain a left view I of the environmentLAnd right view IRUsing ORB (organized FAST and Rotated BRIEF, an algorithm for extracting and describing FAST feature points) algorithm to perform FAST (features from estimated Segment test) corner detection on left and right views, extracting feature points, and calculating the main direction of the feature points to generate an oFAST, the specific flow is as follows:
firstly, carrying out gray processing on the obtained left view and right view: y is 0.39 × R +0.5 × G +0.11 × B, Y is the gray level of a certain grayed pixel, and R, G, B is the three color components of red, green, and blue of the pixel.
Secondly, in the left view, 16 pixels (M10 template) are located in the circle with the radius rounding to 3 downwards and with the pixel point P as the center, as shown in fig. 2. If the gray value of N continuous (9 is more than or equal to N is less than or equal to 12) points in M16 is more than YP+ t or both less than YPT (wherein Y)PIs the gray value of the point P, t is the threshold), the point P is determined to be a feature point. (drawing a circle with a radius of 3 pixels around the center of P. if the gray value of n continuous pixel points on the circle is larger or smaller than that of the P point, the P point is considered as a characteristic point, and n is generally set to be 12.)
Establishing a coordinate system with the characteristic point P as an origin, calculating a centroid position C in the field S, and constructing a vector with the characteristic point P as a starting point and the centroid C as an end point
Figure GDA0003570351060000051
FIELDThe moment of S can be expressed as:
Figure GDA0003570351060000052
wherein Y (x, Y) is the gray value of the image, x, Y belongs to [ -r, r ], r is the radius of the neighborhood S, and p and q are 0 or 1, then the centroid position of the neighborhood is:
Figure GDA0003570351060000053
the principal direction θ of the feature point P is:
θ＝arctan(m01/m10)
(2) and (3) generating a feature point descriptor: within the neighborhood S of the feature point P, a random set of n pairs of points is generated, represented by a2 × n matrix:
Figure GDA0003570351060000054
according to the principal direction theta of the characteristic points obtained in the step (c) and the corresponding rotation matrix RθCalculate the rotated 2 × n matrix:
Figure GDA0003570351060000055
wherein the content of the first and second substances,
Figure GDA0003570351060000056
the binary test criterion in the neighborhood S of the feature point P may be defined as:
Figure GDA0003570351060000061
wherein iθ＝i1θ，…，inθ，jθ＝j1θ，…，jnθAnd tau denotes a binary test criterion,
Figure GDA0003570351060000065
represents a point iθIs determined by the gray-scale value of (a),
Figure GDA0003570351060000066
represents a point jθThe gray value of (a);
a descriptor of the rotation of the feature point P is now available:
Figure GDA0003570351060000062
k' may be 128, 256, or 512, and the corresponding descriptors may have sizes of 16 bytes, 32 bytes, and 64 bytes, respectively, selected according to the real-time performance, storage space, and recognition efficiency of the algorithm.
(3) Matching the characteristic points: after descriptors of feature points of two frames of images of a left view and a right view are obtained according to the scheme, the Hamming distance D of the two frames of images is calculated for matching:
Figure GDA0003570351060000063
wherein f isL，fRRespectively, a descriptor of a certain feature point of the left view and the right view. When two feature points with the Hamming distance smaller than the threshold exist in the two frames of images, the two feature points are successfully matched.
Calculating the space position of the feature point: suppose that the image point coordinate of the feature point P in the left camera is (u)L，vL) The image point coordinate in the right camera is (u)R，vR) Then, the coordinates (x, y, z) of the feature point P in space are calculated as follows:
Figure GDA0003570351060000064
wherein f isFocal lengthCalculating all matched feature point coordinates for the focal length of the binocular camera and d the distance between the two cameras, wherein the feature points have unique space coordinatesA unique descriptor.
2. Robot pose transform estimation
And processing the laser data to obtain laser point cloud data, and filtering the laser point cloud data, namely removing corresponding noise points (points with a short distance and points with a long distance) in each laser reflection point in the laser point cloud data, and taking the rest as effective points. And calculating the pose of the effective laser point cloud in the laser sensor coordinate system.
(1) Confidence coefficient of robot posture transformation estimation and map grid updating when observed value is laser
Robot pose initial value estimation
And setting the time point of obtaining the laser data of each frame as the time point of calculating the pose. Suppose that the pose of the robot at the time point t is P (t), the time point of the former pose calculation is t-1, the corresponding pose is P (t-1), the time point of the next pose calculation is t +1, and the corresponding pose is P (t + 1). And calculating the linear velocity V (x, y) and the angular velocity W of the robot motion through the position and posture difference (comprising displacement and rotation angle) between the time points t and t-1.
Linear velocity V (x, y) — (displacement between t and t-1)/time difference
Angular velocity W ═ angle of rotation between t and t-1)/time difference
Correcting the initial predicted pose value according to the latest received odometer data and the latest received inertial navigation data to obtain an estimated pose value (position _ estimated);
② positioning the scanning window
The displacement scanning parameters are used for limiting the displacement range of a positioning scanning window, and are respectively deviated from squares with the size of lcm up, down, left and right by taking the position and pose estimated value (position _ estimated) as the center. The angle scanning parameters are used for limiting the angle range of the positioning scanning window, and the angle of the upper part and the lower part deviating from the angle of w degrees is taken as the central angle by the estimated value of the pose.
And determining the position and pose of each scanning angle on each map grid in the positioning and scanning window according to the size of the positioning and scanning window, and taking the position and pose as all possible candidate position and pose _ candidates.
(iv) confidence estimation
According to the confidence degree of each map grid corresponding to each candidate pose (the confidence degree value of the map grid is related to the map building process, and is a determined value in the positioning process), calculating the confidence degree candidate _ probability of each candidate pose, and the formula is as follows:
Figure GDA0003570351060000071
wherein m is the total number of map grids in the discrete scanning data of the scanning angle corresponding to a certain candidate pose, and the map coordinate of the nth grid is (x)n，yn) The grid confidence is
Figure GDA0003570351060000073
The value range is 0.1-0.9.
Calculating confidence coefficient weight candidate _ weight corresponding to each candidate pose according to the pose difference between each candidate pose able _ candidate and the estimated value of the pose, position _ estimated, and the formula is as follows:
Figure GDA0003570351060000072
wherein x _ offset is the displacement along the x-axis between each candidate pose and the estimated value of pose, position _ estimated, y _ offset is the displacement along the y-axis between each candidate pose and the estimated value of pose, position _ estimated, transweightRotation is the rotation angle between candidate pose and pose estimate, position _ estimated, rotweightIs the rotation angle weight;
the product of the confidence level candidate _ probability and the confidence weight candidate _ weight of each candidate pose is used as the confidence level score of the current pose, the formula is as follows,
score＝candidate_probability*candidate_weight
the estimated value of the pose update pose with the highest confidence score, pos _ estimated, is selected as the optimal pose estimate P (t + 1). And (3) according to the obtained optimal pose transformation estimation P (T +1) and the current pose P (T) of the robot, solving a robot pose transformation C (comprising a rotation matrix and a displacement matrix), moving the robot to the pose of P (T +1) according to the transformation relation T, and projecting the current point cloud data into a map grid of a corresponding position on the basis. If a plurality of points repeatedly falling on the same map grid position exist, only the coordinates of the map grid corresponding to one point in the map coordinate system are taken, and the probability of the grid is updated according to the projection condition of the laser point cloud. After the first frame of laser points are projected on the map grids, the confidence coefficient after each grid is updated is as follows:
Figure GDA0003570351060000081
when the second frame of laser data falls on the two-dimensional grid plane, updating the confidence level of each grid according to the following formula:
Figure GDA0003570351060000082
the state update coefficients of the respective grids are:
Figure GDA0003570351060000083
when the nth frame of laser point cloud data falls into a two-dimensional plane grid, the confidence coefficient of each grid is as follows:
Figure GDA0003570351060000084
wherein, Pn-1(xi，yi) When the n-1 th frame of laser point cloud data falls into a two-dimensional plane grid, the coordinate is (x)i，yi) Confidence of the grid of (a);
(2) robot pose transformation estimation when observed values are feature points
Suppose that the matching feature point sequence obtained from the left and right views at time t-1 is S ═ S1，s2，…，sg]And the matched characteristic point sequence obtained from the left view and the right view at the moment t is P ═ P1，p2，…，ph]And matching the characteristic point sequence S, P at the time T-1 and the time T, and then obtaining a pose transformation relation T' of the robot according to the coordinate change of the matched characteristic points and the main direction change of the characteristic points, so that the descriptors of the characteristic points are projected onto the corresponding map grids on the basis, and at this time, a second attribute of the grids, namely the descriptor attribute of the characteristic points, is defined. At the moment, the confidence coefficient of the grid map has the double attributes of laser probability and characteristic points, and the first heavy attribute and the second heavy attribute are independent of each other.
4. Generating subgraphs
And generating subgraphs by using continuous laser point cloud data by using a tile type graph building method, wherein each continuous Ts data generates one subgraph, and the second subgraph comprises data in the last T (T is less than T) s of the last subgraph. Such as: the data in the 1s to 40s are registered to form a subgraph A1, the data in the 21s to 60s are registered to form a subgraph A2, and the data in the 41s to 80s are registered to form a subgraph A3.
Second, the back end of the system
1. Closed loop detection
And extracting feature points by utilizing an ORB algorithm according to the currently obtained left view and right view, and calculating descriptors of the feature points. Storing the obtained descriptors of the characteristic points into a set Q, Q ═ Q1，Q2，…，QNAnd matching each descriptor in the Q with a second-layer attribute, namely a grid feature point descriptor, corresponding to each grid in each subgraph, assuming that M subgraphs are generated at the time. And when the success rate of matching between the descriptor in the set Q and the descriptor of the grid feature point of a certain sub-image reaches over 90 percent, the closed-loop detection is successful.
2. Graph optimization
The graph optimization is to reduce the track distortion by reducing the drift in the track, and the pose of the robot is represented as a node in the graph in the process of the graph optimizationThe observation information is converted into a constraint relation between poses of the robots after being processed, and is represented by edges between the connecting nodes. Each node of the graph depends on the description of the pose of the robot, and the state equation of all the nodes is
Figure GDA0003570351060000091
Figure GDA0003570351060000092
Wherein
Figure GDA0003570351060000093
Respectively describing the transformation relation between the poses of the robot under the global coordinate system
Figure GDA0003570351060000094
The observation equation between node Pi and node Pj can be expressed as zj＝xiTi，jI.e. the robot passes through a movement Ti，jFrom Pi to Pj, where xiIs a node PiPose, x, of the robot in global coordinate systemjIs a node PjPose, z, of the corresponding robot in the global coordinate systemjIs a node PjAn observed value of (1), i.e. xiEstimated pose, T, transformed by a transformation matrixi，jIs a node PiAnd node PjThe transformation relationship between them.
In the optimization process, each node has an estimated value, and two vertexes connected by an edge have an estimated value respectively, and an error exists between the estimated values and the constraint of the edge. That is, ideally we have:
zj＝xiTi，j
robot pose xjAnd xiEstimated pose z after transformation by transformation matrixjThere is an error e (z) betweenj，xj)：
e(zj，xj)＝xiTi，j-xj
Since each edge produces a small point error, assuming a graph with K edges is created, the objective function can be written as:
Figure GDA0003570351060000095
wherein the information matrix omegakIs the inverse of the covariance matrix and is a symmetric matrix. Each element omega thereofi，jAs a coefficient of error, pair e (z)i，xi)、e(zj，xj) The error correlation of (c) is predicted. The simplest is to handle omegakAnd setting the matrix into a diagonal matrix, wherein the size of the elements of the diagonal matrix indicates the attention degree of corresponding errors of each element. Other covariance matrices may be used.
xkBy an increment Δ x, the error value is increased from e (z)k，xk) Become e (z)k，xk+ Δ x), the first order expansion of the error term:
Figure GDA0003570351060000101
Jkis e (z)k，xk) With respect to xkThe derivative of (c) is in the form of a jacobian matrix. For the objective function term of the kth edge, further developed are:
E(xk+Δx)＝e(zk，xk+Δx)TΩke(zk，xk+Δx)
≈[e(zk，xk)+JkΔx]TΩk[e(zk，xk)+JkΔx]
＝e(zk，xk)TΩke(zk，xk)+2e(zk，xk)TΩkJkΔx+ΔxTJk TΔx
＝Ck+2bkΔx+ΔxTHkΔx
sorting the terms independent of Δ x into constant terms CkCoefficient of first order of DeltaxkΔ x quadratic coefficient is written as Hk。
Rewriting the objective function:
Figure GDA0003570351060000102
wherein
Figure GDA0003570351060000103
xkAfter the increment occurs, the change in the (x) term of the objective function E is:
ΔE(x)＝2bΔx+ΔxTHΔx
to minimize Δ E (x), the derivative of Δ E (x) with respect to Δ x is made zero, having
Figure GDA0003570351060000104
And updating the state variable X by using the obtained delta X, finally obtaining a convergence value of the state variable X meeting the target equation through multiple iterations, optimizing the pose of the robot in the whole motion process, and forming a final mapping result by the pose of each node in the X.
The invention adopts a mapping method combining vision and laser, on one hand, the defect that a vision algorithm is greatly influenced by illumination is improved, on the other hand, closed loop is detected by using the descriptor attributes of the characteristic points, the problem of low success rate of laser closed loop detection is solved, and the closed loop detection efficiency is improved.
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, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.

Claims (6)

1. A robot indoor map building method based on vision and laser slam is characterized by comprising the following steps:
the visual sensor acquires a left view and a right view, and generates a feature point cloud, which specifically comprises the following steps:
extracting feature points of the left view and the right view,
the obtained feature points are matched with each other,
calculating the space coordinates of the matched characteristic points;
calculating pose transformation estimation of the vision-based robot, namely performing secondary matching on feature points matched with the left view and the right view at the moment and feature points matched with the left view and the right view at the last moment at the moment, obtaining a pose transformation relation of the robot according to coordinate change of the secondary matched feature points and main direction change of the feature points, projecting descriptors of the feature points onto corresponding map grids, and defining feature point descriptor attributes of the grids;
obtaining laser data, calculating the confidence of pose transformation estimation and map grid updating of the robot based on laser;
generating subgraphs by using continuous laser point cloud data by adopting a tile type mapping method;
closed loop detection, namely matching descriptors of the obtained feature points with the attribute of the feature point descriptor of the grid corresponding to each grid of each sub-graph;
the graph optimization specifically comprises the following steps: representing the pose of the robot through the nodes of the graph, representing the transformation relation among the poses of the robot through the edges connecting the nodes, calculating the error between each pose of the robot and the estimated pose transformed by the transformation matrix, calculating an error objective function of the graph with a plurality of edges, and obtaining the pose of the robot converging the objective function through multiple iterations, wherein the pose of the robot in each sub-graph is used for updating the pose of the robot in each sub-graph so as to form a final graph building result;
the extracting feature points of the left view and the right view comprises:
performing FAST corner detection on the left view and the right view by using an ORB algorithm, and extracting a characteristic point P;
establishing a coordinate system with the characteristic point P as an origin, calculating a centroid position C in the neighborhood S, and constructing a vector with the characteristic point P as a starting point and the centroid C as an end point
Figure FDA0003570351050000011
The moments of neighborhood S can be expressed as:
Figure FDA0003570351050000012
wherein Y (x, Y) is the gray value of the image, x, Y belongs to [ -r, r ], r is the radius of the neighborhood S, and p and q are 0 or 1, then the centroid position of the neighborhood is:
Figure FDA0003570351050000013
the principal direction θ of the feature point P is:
θ＝arctan(m01/m10)；
generating a characteristic point descriptor:
within the neighborhood S of the feature point P, a random set of n pairs of points is generated, represented by a2 × n matrix:
Figure FDA0003570351050000021
Figure FDA0003570351050000022
according to the principal direction theta of the characteristic point and the corresponding rotation matrix RθCalculate the rotated 2 xn matrix:
Figure FDA0003570351050000023
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003570351050000024
the binary test criterion in the neighborhood S of the feature point P may be defined as:
Figure FDA0003570351050000025
wherein iθ＝i1θ,…,inθ，jθ＝j1θ,…,jnθτ denotes a binary test criterion, YiθRepresents a point iθGray value of (2), YjθRepresents point jθThe gray value of (a);
a descriptor of the rotation of the feature point P is now available:
Figure FDA0003570351050000026
k' is 128, 256 or 512, the size of the corresponding descriptor is 16 bytes, 32 bytes or 64 bytes respectively, and the selection is carried out according to the real-time performance, the storage space and the identification efficiency of the method;
the method for obtaining laser data, calculating the confidence of the pose transformation estimation and the map grid updating of the laser-based robot specifically comprises the following steps:
correcting the initial predicted pose value according to the latest received odometer data and the latest received inertial navigation data to obtain an estimated value (position _ estimated) of the pose, wherein the initial predicted pose value is obtained by calculating laser data of the robot,
determining the pose of each scanning angle on each map grid in the positioning scanning window through the size of the positioning scanning window as all possible candidate pose positions,
calculating the confidence coefficient candidate _ probability of each candidate pose according to the confidence coefficient of each map grid corresponding to each candidate pose, wherein the formula is as follows:
Figure FDA0003570351050000027
wherein m is the total number of map grids in the discrete scanning data of the scanning angle corresponding to a certain candidate pose, and the map coordinate of the nth grid is (x)n,yn) The grid confidence is
Figure FDA0003570351050000029
The value range is 0.1-0.9,
calculating the confidence coefficient weight candidate _ weight corresponding to each candidate pose according to the pose difference between each candidate pose able _ candidate and the estimated value of the pose, position _ estimated, and the formula is as follows:
Figure FDA0003570351050000028
wherein x _ offset is the displacement along the x-axis between each candidate pose and the estimated value of pose, position _ estimated, y _ offset is the displacement along the y-axis between each candidate pose and the estimated value of pose, position _ estimated, transweightRotation is the rotation angle between candidate pose and pose estimate, position _ estimated, rotweightIs the weight of the angle of rotation,
the product of the confidence measure _ probability and the confidence weight measure _ weight of each candidate pose is used as the confidence score of the current pose, and the formula is as follows,
score＝candidate_probability*candidate_weight
selecting an estimated value (posesestimated) of the pose update pose with the highest confidence score (score) as an optimal pose estimate P (t +1), where t represents a time point at which the robot pose is calculated,
and (3) obtaining robot pose transformation C according to the obtained optimal pose transformation estimation P (T +1) and the current pose P (T) of the robot, moving the robot to the pose of P (T +1) according to the transformation relation T, projecting the current point cloud data into a map grid at a corresponding position on the basis, if a plurality of points repeatedly falling into the same map grid position exist, only taking the coordinate of the map grid corresponding to one point in a map coordinate system, updating the probability of the grid according to the projection condition of the laser point cloud, and after the first frame of laser points are projected onto the map grid, updating the confidence coefficient of each grid as follows:
Figure FDA0003570351050000031
when the second frame of laser data falls on the two-dimensional grid plane, updating the confidence level of each grid according to the following formula:
Figure FDA0003570351050000032
the state update coefficients of the respective grids are:
Figure FDA0003570351050000033
when the nth frame of laser point cloud data falls into a two-dimensional plane grid, the confidence coefficient of each grid is as follows:
Figure FDA0003570351050000034
wherein, Pn-1(xi,yi) When the n-1 th frame of laser point cloud data falls into a two-dimensional plane grid, the coordinate is (x)i,yi) The confidence level of the grid of (a),
and (3) estimating the robot pose transformation when the observed value is the characteristic point: suppose that the matching feature point sequence obtained from the left and right views at time t-1 is S ═ S1,s2,…,sg]And the matched characteristic point sequence obtained from the left view and the right view at the moment t is P ═ P1,p2,…,ph]Matching the characteristic point sequence S, P at the time T-1 and the time T, and then obtaining the pose transformation relation T' of the robot according to the coordinate change of the matched characteristic points and the main direction change of the characteristic points, thereby projecting the descriptors of the characteristic points to the corresponding map grids on the basis, and defining the second attribute of the grids, namely the descriptor attribute of the characteristic pointsAt the moment, the confidence coefficient of the grid map has the double attributes of the laser probability and the characteristic points, and the first double attribute and the second double attribute are independent of each other.
2. The robot indoor mapping method based on vision and laser slam according to claim 1, wherein the matching of the obtained feature points is specifically:
calculating the Hamming distance D of the descriptors of the feature points of the left view and the right view for matching:
Figure FDA0003570351050000041
wherein f isL,fRDescriptors of a certain characteristic point of the left view and the right view respectively; when two feature points with the Hamming distance smaller than the threshold exist in the two frames of images, the two feature points are successfully matched.
3. The robot built-in room map method based on vision and laser slam according to claim 2, characterized in that the calculating the spatial coordinates of the matched feature points is specifically:
Figure FDA0003570351050000042
wherein (x, y, z) is the coordinate of the characteristic point P in space, (u)L,vL) For the image point coordinates of the feature point P in the left camera, (u)R,vR) As coordinates of image points in the right camera, fFocal lengthIs the focal length of the binocular camera and d is the distance between the two cameras.
4. The robot indoor mapping method based on vision and laser slam according to claim 3, wherein the tile-type mapping method is adopted, and the generation of subgraphs by using continuous laser point cloud data specifically comprises:
and generating sub-graphs by using continuous laser point cloud data, wherein each continuous Ts data generates one sub-graph, and the second sub-graph comprises the data in the last Ts of the previous sub-graph, wherein T is less than T.
5. The robot indoor mapping method based on vision and laser slam according to claim 4, wherein the mapping optimization specifically comprises:
the pose of the robot is represented as nodes in the graph, observation information is converted into a constraint relation between poses of the robots after being processed, and the constraint relation is represented by connecting edges between the nodes; each node of the graph depends on the description of the pose of the robot, and the state equation of all the nodes is
Figure FDA0003570351050000051
Wherein
Figure FDA0003570351050000052
The positions and postures of the robot under the global coordinate system are respectively described, and the transformation relation among the positions and postures of the robot is described; the observation equation between node Pi and node Pj can be expressed as:
zj＝xiTi,j
wherein x isiIs a node PiPose, x, of the corresponding robot in the global coordinate systemjIs a node PjPose, z, of the corresponding robot in the global coordinate systemjIs a node PjOf observed value of (a), i.e. xiEstimated pose after transformation by transformation matrix, Ti,jIs a node PiAnd node PjThe transformation relationship between the two;
robot pose xjAnd xiEstimated pose z after transformation by transformation matrixjThere is an error e (z) betweenj,xj) The calculation formula is as follows:
e(zj,xj)＝xiTi,j-xj
the error objective function e (x) of the graph with several edges is:
Figure FDA0003570351050000053
wherein the information matrix omegakIs the inverse of the covariance matrix, is a symmetric matrix; each element omega thereofi,jAs a coefficient of error, pair e (z)i,xi)、e(zj,xj) The error correlation of (a) is predicted;
let x bekAnd adding an increment delta X, finally obtaining a convergence value meeting the objective equation E (X) through multiple iterations, updating the state variable X by using the obtained delta X, and forming a final mapping result by using the pose of each node in the X.
6. A robot indoor map system based on vision and laser slam is characterized in that the system is realized based on the method of any one of claims 1 to 5 and is divided into a front end and a rear end, the front end of the system acquires a left view and a right view of an environment by using a binocular vision sensor, extracts the left view and the right view by an ORB algorithm, matches feature points of the left view and the right view, and further finds coordinates of the feature points in a space; matching the feature points obtained at adjacent moments, further solving the pose transformation relation of the robot at the moments, projecting the feature points onto a map grid, and defining that the grid map with the feature point projection has descriptor attributes; the front end of the system obtains laser point cloud data of the environment through a laser sensor, de-noizes the laser point cloud data, obtains pose transformation estimation of the robot by obtaining a pose initial value of the robot, defining a scanning window and obtaining a confidence coefficient, projects the laser point cloud on a grid map, and updates the confidence coefficient of each map grid; and the back end of the system performs closed-loop detection and map optimization on the obtained map.
CN201911257094.XA 2019-12-10 2019-12-10 Robot indoor map building method and system based on vision and laser slam Active CN111076733B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911257094.XA CN111076733B (en) 2019-12-10 2019-12-10 Robot indoor map building method and system based on vision and laser slam

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911257094.XA CN111076733B (en) 2019-12-10 2019-12-10 Robot indoor map building method and system based on vision and laser slam

Publications (2)

Publication Number Publication Date
CN111076733A CN111076733A (en) 2020-04-28
CN111076733B true CN111076733B (en) 2022-06-14

Family

ID=70313690

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911257094.XA Active CN111076733B (en) 2019-12-10 2019-12-10 Robot indoor map building method and system based on vision and laser slam

Country Status (1)

Country Link
CN (1) CN111076733B (en)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111664843A (en) * 2020-05-22 2020-09-15 杭州电子科技大学 SLAM-based intelligent storage checking method
CN113835422B (en) * 2020-06-08 2023-09-29 杭州海康机器人股份有限公司 Visual map construction method and mobile robot
CN111795687B (en) * 2020-06-29 2022-08-05 深圳市优必选科技股份有限公司 Robot map updating method and device, readable storage medium and robot
CN111862162B (en) * 2020-07-31 2021-06-11 湖北亿咖通科技有限公司 Loop detection method and system, readable storage medium and electronic device
CN112116656A (en) * 2020-08-03 2020-12-22 歌尔股份有限公司 Incremental mapping method and device in synchronous positioning and mapping slam
CN112102400B (en) * 2020-09-15 2022-08-02 上海云绅智能科技有限公司 Distance-based closed loop detection method and device, electronic equipment and storage medium
CN112197773B (en) * 2020-09-30 2022-11-11 江苏集萃未来城市应用技术研究所有限公司 Visual and laser positioning mapping method based on plane information
CN113126621A (en) * 2020-10-14 2021-07-16 中国安全生产科学研究院 Automatic navigation method of subway carriage disinfection robot
CN112325872B (en) * 2020-10-27 2022-09-30 上海懒书智能科技有限公司 Positioning method of mobile equipment based on multi-sensor coupling
CN112665575B (en) * 2020-11-27 2023-12-29 重庆大学 SLAM loop detection method based on mobile robot
CN112650255B (en) * 2020-12-29 2022-12-02 杭州电子科技大学 Robot positioning navigation method based on visual and laser radar information fusion
CN113189613B (en) * 2021-01-25 2023-01-10 广东工业大学 Robot positioning method based on particle filtering
CN113112478B (en) * 2021-04-15 2023-12-15 深圳市优必选科技股份有限公司 Pose recognition method and terminal equipment
CN113624221B (en) * 2021-06-30 2023-11-28 同济人工智能研究院(苏州)有限公司 2.5D map construction method integrating vision and laser
CN115880364B (en) * 2023-02-09 2023-05-16 广东技术师范大学 Robot pose estimation method based on laser point cloud and visual SLAM

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106793086A (en) * 2017-03-15 2017-05-31 河北工业大学 A kind of indoor orientation method
CN109857123A (en) * 2019-03-21 2019-06-07 郑州大学 A kind of fusion method of view-based access control model perception and the indoor SLAM map of laser acquisition
CN109974712A (en) * 2019-04-22 2019-07-05 广东亿嘉和科技有限公司 It is a kind of that drawing method is built based on the Intelligent Mobile Robot for scheming optimization
CN110084272A (en) * 2019-03-26 2019-08-02 哈尔滨工业大学（深圳） A kind of cluster map creating method and based on cluster map and the matched method for relocating of location expression
CN110196044A (en) * 2019-05-28 2019-09-03 广东亿嘉和科技有限公司 It is a kind of based on GPS closed loop detection Intelligent Mobile Robot build drawing method
CN110361027A (en) * 2019-06-25 2019-10-22 马鞍山天邦开物智能商务管理有限公司 Robot path planning method based on single line laser radar Yu binocular camera data fusion
CN110533722A (en) * 2019-08-30 2019-12-03 的卢技术有限公司 A kind of the robot fast relocation method and system of view-based access control model dictionary

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2009021068A1 (en) * 2007-08-06 2009-02-12 Trx Systems, Inc. Locating, tracking, and/or monitoring personnel and/or assets both indoors and outdoors
WO2017076928A1 (en) * 2015-11-02 2017-05-11 Starship Technologies Oü Method, device and assembly for map generation
CN105953796A (en) * 2016-05-23 2016-09-21 北京暴风魔镜科技有限公司 Stable motion tracking method and stable motion tracking device based on integration of simple camera and IMU (inertial measurement unit) of smart cellphone
CN106909877B (en) * 2016-12-13 2020-04-14 浙江大学 Visual simultaneous mapping and positioning method based on dotted line comprehensive characteristics
SG10201700299QA (en) * 2017-01-13 2018-08-30 Otsaw Digital Pte Ltd Three-dimensional mapping of an environment
CN107356252B (en) * 2017-06-02 2020-06-16 青岛克路德机器人有限公司 Indoor robot positioning method integrating visual odometer and physical odometer
EP3707530A4 (en) * 2017-09-04 2021-09-22 Commonwealth Scientific and Industrial Research Organisation Method and system for use in performing localisation
CN109489660A (en) * 2018-10-09 2019-03-19 上海岚豹智能科技有限公司 Robot localization method and apparatus

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106793086A (en) * 2017-03-15 2017-05-31 河北工业大学 A kind of indoor orientation method
CN109857123A (en) * 2019-03-21 2019-06-07 郑州大学 A kind of fusion method of view-based access control model perception and the indoor SLAM map of laser acquisition
CN110084272A (en) * 2019-03-26 2019-08-02 哈尔滨工业大学（深圳） A kind of cluster map creating method and based on cluster map and the matched method for relocating of location expression
CN109974712A (en) * 2019-04-22 2019-07-05 广东亿嘉和科技有限公司 It is a kind of that drawing method is built based on the Intelligent Mobile Robot for scheming optimization
CN110196044A (en) * 2019-05-28 2019-09-03 广东亿嘉和科技有限公司 It is a kind of based on GPS closed loop detection Intelligent Mobile Robot build drawing method
CN110361027A (en) * 2019-06-25 2019-10-22 马鞍山天邦开物智能商务管理有限公司 Robot path planning method based on single line laser radar Yu binocular camera data fusion
CN110533722A (en) * 2019-08-30 2019-12-03 的卢技术有限公司 A kind of the robot fast relocation method and system of view-based access control model dictionary

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"基于双三次插值的巡检机器人初始位姿优化";林欢等;《机械设计与制造工程》;20180531;第47卷(第5期);56-60 *
"基于深度传感器的移动机器人视觉SLAM研究";李扬宇;《中国优秀硕士学位论文全文数据库 信息科技辑》;20180615(第6期);I140-264 *

Also Published As

Publication number Publication date
CN111076733A (en) 2020-04-28

Similar Documents

Publication Publication Date Title
CN111076733B (en) Robot indoor map building method and system based on vision and laser slam
Park et al. Elastic lidar fusion: Dense map-centric continuous-time slam
WO2021196294A1 (en) Cross-video person location tracking method and system, and device
CN108986037B (en) Monocular vision odometer positioning method and positioning system based on semi-direct method
CN112985416B (en) Robust positioning and mapping method and system based on laser and visual information fusion
CN113436260B (en) Mobile robot pose estimation method and system based on multi-sensor tight coupling
CN112258600A (en) Simultaneous positioning and map construction method based on vision and laser radar
CN110146099B (en) Synchronous positioning and map construction method based on deep learning
CN108332752B (en) Indoor robot positioning method and device
WO2019136613A1 (en) Indoor locating method and device for robot
Mu et al. Research on SLAM algorithm of mobile robot based on the fusion of 2D LiDAR and depth camera
CN114419147A (en) Rescue robot intelligent remote human-computer interaction control method and system
WO2023273169A1 (en) Vision and laser-fused 2.5d map construction method
CN111161334B (en) Semantic map construction method based on deep learning
CN112767546B (en) Binocular image-based visual map generation method for mobile robot
CN112752028A (en) Pose determination method, device and equipment of mobile platform and storage medium
CN111998862A (en) Dense binocular SLAM method based on BNN
CN112484746A (en) Monocular vision-assisted laser radar odometer method based on ground plane
CN112270698A (en) Non-rigid geometric registration method based on nearest curved surface
CN113781525B (en) Three-dimensional target tracking method based on original CAD model
Zhang LILO: A Novel Lidar–IMU SLAM System With Loop Optimization
CN116681733B (en) Near-distance real-time pose tracking method for space non-cooperative target
CN113674412A (en) Pose fusion optimization-based indoor map construction method and system and storage medium
CN115950414A (en) Adaptive multi-fusion SLAM method for different sensor data
CN115830070A (en) Infrared laser fusion positioning method for inspection robot of traction substation

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