Vision-and-laser-fused 2.5D map building method

Abstract

A vision-and-laser-fused 2.5D map building method includes: calculating inter-image frame transformation according to an RGB-D image sequence to establish a visual front-end odometer; taking a visual front-end initial estimation as an initial value of scanning matching, and performing laser front-end coarse-grained and fine-grained searches; performing loop closure detection, and performing back-end global optimization on a 2.5D map according to a detected closed loop; and performing incremental update on visual feature dimensions of the 2.5D map, and performing occupation probability update on grid dimensions. The 2.5D map is built using a method of fusing a laser grid and visual features, and compared with a pure laser map and a pure visual map, richness of dimensions is improved, and completeness of information expression is improved; the 2.5D map building method is not influenced by single sensor failure, and can still stably work in a scenario of sensor degradation.

Description

This application is the National Stage Application of PCT/CN2021/136356, filed on Dec. 8, 2021, which claims priority to Chinese Patent Application No. 202110735175.7, filed on Jun. 30, 2021, which is incorporated by reference for all purposes as if fully set forth herein.

FIELD OF THE DISCLOSURE

The present invention pertains to the field of autonomous map building of mobile robots, and particularly relates to a vision-and-laser-fused 2.5D map building method.

BACKGROUND OF THE DISCLOSURE

With a development of technologies, autonomous mobile robots are applied in more and more scenarios. The autonomous mobile robot is greatly required by industries, such as industrial handling, site inspection, or the like, mainly due to saving of a labor cost and more safety. Perception and adaptation to an environment are premises of autonomous intelligence of the robot, a simultaneous localization and mapping (SLAM) technology is considered as a core link for realizing autonomous navigation, and a series of SLAM technologies with a laser radar and a camera as cores are widely researched and applied. However, frequent handling of cargoes at an industrial site and an unknown environment of a patrolling site present significant challenges to the SLAM technology. In addition, as the robots surge to a service industry, more and more robots are required to work in home environments which are highly dynamic, and walks of persons and random movement of objects require the SLAM technology to be quite stable for stable work of the robots.

In a laser SLAM solution commonly used in an industrial community, scanning matching is performed dependent on accurate initial values in a structured scenario, and a wheel-type odometer usually has low precision, has an accumulated error, and furthermore is difficult to initialize; a visual SLAM solution usually does not have a navigation function and cannot be put into practical use. Sensor fusion is an effective solution for solving single sensor failure, and no solution for fusing stable open-source 2D laser and visual information exists at present.

2.5D maps are three-dimensional abstract descriptions of one or more aspects of reality or a part thereof to a scale based on three-dimensional electronic map databases.

SUMMARY OF THE DISCLOSURE

An object of the present invention is to provide a vision-and-laser-fused 2.5D map building method, which is used for solving a problem of insufficient expression dimensions of an existing map.

In order to achieve the above object, the following technical solution is adopted in the present invention.

A vision-and-laser-fused 2.5D map building method includes:

• • S1: calculating inter-image frame transformation according to an RGB-D image sequence to establish a visual front-end odometer;
  • S2: taking a visual front-end initial estimation as an initial value of scanning matching, and performing laser front-end coarse-grained and fine-grained searches;
  • S3: performing loop closure detection, and performing back-end global optimization on a 2.5D map according to a detected closed loop; and
  • S4: performing incremental update on visual feature dimensions of the 2.5D map, and performing occupation probability update on grid dimensions.

Preferably, in S1, the inter-frame transformation includes:

• • (1) extracting an ORB feature of each image frame on the time sequence, and establishing a corresponding feature matching relationship; and
  • (2) taking a previous frame as a reference frame, performing PnP calculation on a current frame and the reference frame, building a least square problem according to a minimized reprojection error, and performing iterative optimization solving to obtain inter-frame pose transformation.

Preferably, the least square problem is:

${\xi }^{*}=\arg \underset{\xi }{\min }\frac{1}{2}\sum _{i=1}^n{p_i-\frac{1}{s_i}K\exp \left({\xi }^{\bigwedge }\right)P_i}_2^2,$

• • wherein
  • ξ is the camera pose, and
  • K is a camera internal reference.

Preferably, in S2, the scanning matching includes:

• • (1) obtaining a pose initial estimation ξ₀ according to RGB-D VO; and
  • (2) after a search in a coarse-grained space, reducing a search range, and then searching in a fine-grained space to obtain an optimal pose ξ*.

Preferably, the optimal pose ξ* is:

${\xi }^{*}=\underset{\xi \in W}{\arg \max }\sum _{k=1}^{K}M\left(T_{\zeta }{h}_{\xi }\right),$

• • wherein
  • T_ξ is transformation of a scanning point into a map coordinate system,
  • h_k is a set of scanning points of a laser beam.

Preferably, in S3, the loop closure detection includes:

• • (1) performing a breadth optimization search within a certain range with a current node as a search center, so as to obtain nodes associated with the current node, generating a data chain using the nodes, performing matching on the current node, and if a response value reaches a set threshold, establishing a constraint ξ_ij between the current node and a node in the data chain closest to a centroid of the current node; and
  • (2) in a map building process, taking a certain number of accumulated laser data chains within a certain distance from the current node as the local map, the constraint ξ_im between the laser frame and the map being formed between the node and the current local map.

Preferably, in S3, an optimization formula of the back-end global optimization is:

$\underset{{\prod }_s,{\xi }_m}{\arg \min }\frac{1}{2}(\sum _{\mathrm{ij}}\rho (E^2\left({\xi }_i,{\xi }_j;{\sum }_{\mathrm{ij}},{\xi }_{\mathrm{ij}}\right)+\sum _i\rho \left(E^2\left({\xi }_i,{\xi }_m;{\sum }_{\mathrm{im}},{\xi }_{\mathrm{im}}\right)\right)$ $\mathrm{wherein}{E}^2\left({\xi }_i,{\xi }_j;{\sum }_{\mathrm{ij}},{\xi }_{\mathrm{ij}}\right)={e\left({\xi }_i,{\xi }_j;{\xi }_{\mathrm{ij}}\right)}^T{\sum }_{\mathrm{ij}}^{-1}e\left({\xi }_i,{\xi }_j;{\xi }_{\mathrm{ij}}\right),$ $e\left({\xi }_i,{\xi }_j;{\xi }_{\mathrm{ij}}\right)={\xi }_{\mathrm{ij}}-\left(\begin{array}{c}{R}_{{\xi }_i}^{-1}\left(t_{{\xi }_i}-t_{{\xi }_j}\right)\\ {\xi }_{i;\theta }-{\xi }_{j;\theta }\end{array}\right),$ $E^2\left({\xi }_i,{\xi }_m;{\sum }_{\mathrm{im}},{\xi }_{\mathrm{im}}\right)={e\left({\xi }_i,{\xi }_m;{\xi }_{\mathrm{im}}\right)}^T{\sum }_{\mathrm{im}}^{-1}e\left({\xi }_i,{\xi }_m;{\xi }_{\mathrm{im}}\right),$ $e\left({\xi }_i,{\xi }_m;{\xi }_{\mathrm{im}}\right)={\xi }_{\mathrm{im}}-\left(\begin{array}{c}{R}_{{\xi }_i}^{-1}\left(t_{{\xi }_i}-t_{{\xi }_m}\right)\\ {\xi }_{i;\theta }-{\xi }_{m;\theta }\end{array}\right),$ $\rho \left(e\right)=\{\begin{array}{cc}\frac{1}{2}{e}^2& ,❘e❘\le \delta \\ \delta \left(❘e❘-\frac{1}{2}\delta \right)& ,❘e❘>\delta \end{array}.$

Preferably, in S4, the 2.5D map is M={m_(x,y)}, the 2.5D map includes a laser grid map M_grid={m_g(x,y)} and a visual feature map M_feature={m_f(x,y)}, and the 2.5D map M={m_(x,y)} is:m_(x,y)={m_g(x,y),m_f(x,y)},m_f(x,y)={f(x,y,z₁),f(x,y,z₂), . . . ,f(x,y,z_n)},

• • wherein
  • f(x,y,z) is a feature at (x,y,z).

Preferably, an update form of the visual feature dimension is:M_feature^new={M_feature^old;f_new,ξ_f}.

Preferably, the grid dimension includes an unobserved grid and an observed grid, a laser hit probability p_hit or p_miss is directly given to the unobserved grid, and an update form of the observed grid is:

$\mathrm{odds}\left(p\right)=\frac{p}{1-p}$ $M_{\mathrm{grid}}^{\mathrm{new}}\left(x\right)={\mathrm{odds}}^{-1}\left(\mathrm{odds}\left(M_{\mathrm{grid}}^{\mathrm{old}}\left(x\right)\right)·\mathrm{odds}\left(p_{\mathrm{hit}/\mathrm{miss}}\right)\right).$

Due to application of the above technical solution, compared with a prior art, the present invention has the following advantages.

1. In the present invention, the 2.5D map is built using a method of fusing the laser grid and the visual features, and compared with a pure laser map and a pure visual map, richness of dimensions is improved, and completeness of information expression is improved.

2. The 2.5D map building method according to the present invention is not influenced by single sensor failure, and can still stably work in a scenario of sensor degradation.

3. The 2.5D map building method according to the present invention can avoid a laser initialization process of a positioning and repositioning system, and correct positioning can be quickly and accurately recovered in an error positioning scenario.

BRIEF DESCRIPTION OF DRAWINGS

FIG. 1 is a flow chart of a 2.5D map building method according to an embodiment;

FIG. 2 is a schematic diagram of an RGB-D front-end PnP calculation in the present embodiment;

FIG. 3 is a flow chart of front-end scanning matching in the present embodiment;

FIG. 4 is a flow chart of back-end global optimization in the present embodiment;

FIG. 5 is a schematic diagram of a 2.5D map structure in the present embodiment; and

FIG. 6 is a schematic diagram of a 2.5D map in the present embodiment.

DETAILED DESCRIPTION OF PREFERRED EMBODIMENTS

The technical solution of the present invention will be described clearly and completely below with reference to the accompanying drawings, and apparently, the described embodiments are some but not all of the embodiments of the present invention. All other embodiments obtained by a person of ordinary skill in the art based on the embodiments of the present invention without creative efforts fall within the protection scope of the present invention.

As shown in FIG. 1, a vision-and-laser-fused 2.5D map building method includes:

S1: calculating inter-image frame pose transformation according to an RGB-D image sequence to establish a visual front-end odometer, the calculating inter-image frame pose transformation specifically including:

(1) extracting an ORB feature of each image frame on the time sequence, and then establishing a corresponding feature matching relationship, the ORB feature being a feature with rotation invariance, and a feature extraction method being a method in an open-source framework ORB_SLAM; and

(2) taking a previous frame as a reference frame, performing perspective-n-point (PnP) calculation on a current frame and the reference frame, building a least square problem according to a minimized reprojection error, and performing iterative optimization solving to obtain inter-frame pose transformation.

In the present embodiment, the PnP problem is solved using a bundle adjustment (BA) method.

As shown in FIG. 2, p₁ and p₂ are a set of points obtained by feature matching, p₁ and p₂ are projections of a same spatial point P, there exists a distance between the projection {circumflex over (p)}₂ of P in an initial value and an actual value, an optimal camera pose is required to be found to minimize the error, and therefore, the least square problem is built according to the minimized reprojection error, and an optimization problem is iteratively solved using an LM algorithm; specifically, the least square problem is built according to the minimized reprojection error as:

${\xi }^{*}=\arg \underset{\xi }{\min }\frac{1}{2}\sum _{i=1}^n{p_i-\frac{1}{s_i}K\exp \left({\xi }^{\bigwedge }\right)P_i}_2^2,$

• • wherein
  • ξ is the camera pose, and
  • K is a camera internal reference.

S2: taking a visual front-end initial estimation as an initial value of scanning matching, and performing laser front-end coarse-grained and fine-grained searches, a scan-to-map matching mode being adopted in laser front-end scanning matching in the present embodiment.

Specifically, as shown in FIG. 3, in order to obtain an insertion pose between an input laser frame and a local map, the scanning matching includes:

• • (1) obtaining a pose initial estimation ξ₀ according to RGB-D VO; and
  • (2) after a search in a coarse-grained space, reducing a search range, and then searching in a fine-grained space to obtain an optimal pose ξ*, the optimal pose ξ* being:

${\xi }^{*}=\arg \underset{\xi \in W}{\max }\sum _{k=1}^{K}M\left(T_{\xi }{h}_{\xi }\right),$

• • wherein
  • T_ξ is transformation of a scanning point into a map coordinate system,
  • h_k is a set of scanning points of a laser beam.

A definition of the search space for the scanning matching is shown as follows:W={ξ₀+(rj_x,rj_y,δ_θj_θ):(j_x,j_y,j_θ)ϵW}

• • wherein

$\overline{W}=\left\{-w_x,\dots ,w_x\right\}\times \left\{-w_y,\dots ,w_y\right\}\times \left\{-w_{\theta },\dots ,w_{\theta }\right\},$ $w_x=\frac{W_x}{r},w_y=\frac{W_y}{r},w_{\theta }=\frac{W_{\theta }}{{\delta }_{\theta }},$

• • ξ₀ is obtained by an RGB-D odometer,
  • {W_x, W_y, W_θ} is a search upper and lower bound parameter,
  • {r,δ_θ} is a step parameter.

S3: performing loop closure detection, and performing back-end global optimization on a 2.5D map according to a detected closed loop, so as to solve a problem of an accumulated error caused by front-end scanning matching, and prevent a possible interleaving phenomenon of a map established only by a front end, specifically:

• • the loop closure detection includes a constraint between nodes and a constraint between the nodes and the local map, and as shown in FIG. 4, the loop closure detection includes:
  • (1) performing a breadth optimization search within a certain range with a current node as a search center, so as to obtain nodes associated with the current node, generating a data chain using the nodes, performing matching on the current node, and if a response value reaches a set threshold, establishing a constraint ξ_ij between the current node and a node in the data chain closest to a centroid of the current node; and
  • (2) in a map building process, taking a certain number of accumulated laser data chains within a certain distance from the current node as the local map, the constraint ξ_im between the laser frame and the map being formed between the node and the current local map.

An optimization formula of the back-end global optimization is:

$\underset{{\prod }_s,{\xi }_m}{\arg \min }\frac{1}{2}(\sum _{\mathrm{ij}}\rho (E^2\left({\xi }_i,{\xi }_j;{\sum }_{\mathrm{ij}},{\xi }_{\mathrm{ij}}\right)+\sum _i\rho \left(E^2\left({\xi }_i,{\xi }_m;{\sum }_{\mathrm{im}},{\xi }_{\mathrm{im}}\right)\right),$ $\mathrm{wherein}{E}^2\left({\xi }_i,{\xi }_j;{\sum }_{\mathrm{ij}},{\xi }_{\mathrm{ij}}\right)={e\left({\xi }_i,{\xi }_j;{\xi }_{\mathrm{ij}}\right)}^T{\sum }_{\mathrm{ij}}^{-1}e\left({\xi }_i,{\xi }_j;{\xi }_{\mathrm{ij}}\right),$ $e\left({\xi }_i,{\xi }_j;{\xi }_{\mathrm{ij}}\right)={\xi }_{\mathrm{ij}}-\left(\begin{array}{c}{R}_{{\xi }_i}^{-1}\left(t_{{\xi }_i}-t_{{\xi }_j}\right)\\ {\xi }_{i;\theta }-{\xi }_{j;\theta }\end{array}\right),$ $E^2\left({\xi }_i,{\xi }_m;{\sum }_{\mathrm{im}},{\xi }_{\mathrm{im}}\right)={e\left({\xi }_i,{\xi }_m;{\xi }_{\mathrm{im}}\right)}^T{\sum }_{\mathrm{im}}^{-1}e\left({\xi }_i,{\xi }_m;{\xi }_{\mathrm{im}}\right),$ $e\left({\xi }_i,{\xi }_m;{\xi }_{\mathrm{im}}\right)={\xi }_{\mathrm{im}}-\left(\begin{array}{c}{R}_{{\xi }_i}^{-1}\left(t_{{\xi }_i}-t_{{\xi }_m}\right)\\ {\xi }_{i;\theta }-{\xi }_{m;\theta }\end{array}\right),$ $\rho \left(e\right)=\{\begin{array}{cc}\frac{1}{2}{e}^2& ,❘e❘\le \delta \\ \delta \left(❘e❘-\frac{1}{2}\delta \right)& ,❘e❘>\delta \end{array},$

• • Π_s={ξ_i}_{i=1, . . . ,n} is a node set,
  • ξ_m is a pose of the local map,
  • ξ_ij is the constraint between the nodes, and
  • ξ_im is the constraint between the node and the local map.

S4: performing incremental update on visual feature dimensions of the 2.5D map, and performing occupation probability update on grid dimensions, specifically:

• • the 2.5D map is defined as M={m_(x,y)}, a structure of the 2.5D map is shown in FIG. 5, a 2.5D map space includes a two-dimensional space occupying a grid plane and a feature dimension space on a corresponding grid point, and therefore, the 2.5D map is divided into a laser grid map M_grid and a visual feature map M_feature from a perspective of dimension separation,M_grid={m_g(x,y)},M_feature={m_f(x,y)},m_(x,y)={m_g(x,y),m_f(x,y)},m_f(x,y)={f(x,y,z₁),f(x,y,z₂), . . . ,f(x,y,z_n)},
  • wherein
  • f(x,y,z) is a feature at (x,y,z).

In the present embodiment, as shown in FIG. 6, on the established 2.5D map, feature points are distributed above a two-dimensional grid map and share a coordinate system with a grid occupied by laser, a mobile robot model can perform a subsequent navigation control task according to the 2.5D map, an error recovery function can be achieved, and in the process of establishing the map, the map is updated with a gradual exploration of an environment by a mobile robot.

For the update of the visual feature dimension, a feature pose obtained by PnP optimization is incrementally inserted into the map at the front end, the pose of the robot is optimized again at the back end, and an update form of the visual feature dimension is:M_feature^new={M_feature^old;f_new,ξ_f}.

The grid dimension includes an unobserved grid and an observed grid, and for the update of the grid dimension, a laser hit probability p_hit or p_miss is directly given to the unobserved grid, and an update form of the observed grid is:

$\mathrm{odds}\left(p\right)=\frac{p}{1-p},$ $M_{\mathrm{grid}}^{\mathrm{new}}\left(x\right)={\mathrm{odds}}^{-1}\left(\mathrm{odds}\left(M_{\mathrm{grid}}^{\mathrm{old}}\left(x\right)\right)·\mathrm{odds}\left(p_{\mathrm{hit}/\mathrm{miss}}\right)\right).$

The above-mentioned embodiments are merely illustrative of the technical concepts and features of the present invention, and are intended to enable those skilled in the art to understand the contents of the present invention and implement the present invention, and not to limit the scope of the present invention. All equivalent changes or modifications made according to the spirit of the present invention are intended to be covered by the protection scope of the present invention.

Claims

1. A vision-and-laser-fused 2.5D map building method, comprising: S1: calculating inter-image frame transformation according to an RGB-D image sequence to establish a visual front-end odometer;S2: taking a visual front-end initial estimation as an initial value of scanning matching, and performing laser front-end coarse-grained and fine-grained searches;S3: performing loop closure detection, and performing back-end global optimization on a 2.5D map according to a detected closed loop; andS4: performing incremental update on visual feature dimensions of the 2.5D map, and performing occupation probability update on grid dimensions,wherein in S2, the matching comprises:(1) obtaining a pose initial estimation ξ0 according to RGB-D VO; and(2) after a search in a coarse-grained space, reducing a search range, and then searching in a fine-grained space to obtain an optimal pose ξ*; andwherein the search space for the scanning matching is defined as: W={ξ0+(rjx,rjy,δθjθ):(jx,jy,jθ)ϵW}, wherein

2. The vision-and-laser-fused 2.5D map building method according to claim 1, wherein in S1, the inter-frame transformation comprises: (1) extracting an ORB feature of each image frame on the time sequence, and establishing a corresponding feature matching relationship; and(2) taking a previous frame as a reference frame, performing PnP calculation on a current frame and the reference frame, building a least square problem according to a minimized reprojection error, and performing iterative optimization solving to obtain inter-frame pose transformation.

3. The vision-and-laser-fused 2.5D map building method according to claim 2, wherein the least square problem is:

4. The vision-and-laser-fused 2.5D map building method according to claim 2, wherein the ORB feature extraction method is an open-source framework ORB_SLAM method.

5. The vision-and-laser-fused 2.5D map building method according to claim 1, wherein in S2, a scan-to-map matching mode is adopted in laser front-end scanning matching.

6. The vision-and-laser-fused 2.5D map building method according to claim 1, wherein the optimal pose ξ* is:

7. The vision-and-laser-fused 2.5D map building method according to claim 1, wherein in S3, the loop closure detection comprises a constraint between nodes and a constraint between the nodes and a local map, and the loop closure detection comprises: (1) performing a breadth optimization search within a certain range with a current node as a search center, so as to obtain nodes associated with the current node, generating a data chain using the nodes, performing matching on the current node, and if a response value reaches a set threshold, establishing a constraint ξij between the current node and a node in the data chain closest to a centroid of the current node; and(2) in a map building process, taking a certain number of accumulated laser data chains within a certain distance from the current node as the local map, the constraint ξim between the laser frame and the map being formed between the node and the current local map.

8. The vision-and-laser-fused 2.5D map building method according to claim 1, wherein in S3, an optimization formula of the back-end global optimization is: arg⁢min∏s,ξm⁢12⁢(∑ijρ⁢(E2(ξi,ξj;∑ij,ξij)+∑iρ⁡(E2(ξi,ξm;∑im,ξim)),wherein

9. The vision-and-laser-fused 2.5D map building method according to claim 1, wherein in S4, the 2.5D map is M={m(x,y)}, the 2.5D map comprises a laser grid map Mgrid={mg(x,y)} and a visual feature map Mfeature={mf(x,y)}, and the 2.5D map M={m(x,y)} is: m(x,y)={mg(x,y),mf(x,y)},mf(x,y)={f(x,y,z1),f(x,y,z2), . . . ,f(x,y,zn)},whereinf(x,y,z) is a feature at (x,y,z).

10. The vision-and-laser-fused 2.5D map building method according to claim 1, wherein for the update of the visual feature dimension, a feature pose obtained by PnP optimization is incrementally inserted into the map at the front end, the pose of the robot is optimized again at the back end, and an update form of the visual feature dimension is: Mfeaturenew={Mfeatureold;fnew,ξf}.

11. The vision-and-laser-fused 2.5D map building method according to claim 1, wherein the grid dimension comprises an unobserved grid and an observed grid, a laser hit probability phit or pmiss is directly given to the unobserved grid, andan update form of the observed grid is:

12. A vision-and-laser-fused 2.5D map building method, comprising: S1: calculating inter-image frame pose transformation according to an RGB-D image sequence to establish a visual front-end odometer, the calculating inter-frame pose transformation comprising:(1) extracting an ORB feature of each image frame on the time sequence, and establishing a corresponding feature matching relationship; and(2) taking a previous frame as a reference frame, performing PnP calculation on a current frame and the reference frame, building a least square problem according to a minimized reprojection error, and performing iterative optimization solving to obtain inter-frame pose transformation, the least square problem being: