CN101944240A - Fusion method of multi-robot three-dimensional geometrical map - Google Patents

Abstract

The invention discloses a fusion method of a multi-robot three-dimensional geometrical map, comprising the steps of projecting a three-dimensional geometrical map to be fused into two-dimensional grid maps; fusing the grid maps by adopting an image matching algorithm to acquire rotation parameters among the two-dimensional grid maps; and rotating and transforming the three-dimensional map and fusing the three-dimensional geometrical map by adopting a three-dimensional point set registration algorithm. The fusion method of the multi-robot three-dimensional geometrical map integrates the two-dimensional map fusion and the three-dimensional point set registration algorithm through considering geometrical characteristic information in the three-dimensional map to finish the fusion of the multi-robot three-dimensional geometrical map and obviously improve the fusion accuracy of the three-dimensional geometrical map. The fusion method of the multi-robot three-dimensional geometrical map is mainly applied to simultaneously positioning a plurality of robots and reestablishing the map and fusing partial map created by a single robot to form a global map.

Description

The fusion method of multirobot three-dimensional geometry map

Technical field

The present invention relates to robot and locate simultaneously and map reconstruction field, is the method that a kind of how much maps that multirobot is created merge specifically.

Background technology

Along with the continuous expansion of application, the location can not satisfy the demands in many instances with the map reconstruction algorithm in the time of based on unit device robot system; And multi-robot system because have concurrent job, have complementary functions, advantage such as information sharing, be subjected to people and more and more paid close attention to.The mobile robot locate simultaneously with map reconstruction problem in, map is according to the current pose information creating of robot, therefore, if can know the initial posture information and the corresponding equation of motion thereof of each robot, create when so just can realize map by the mode of multirobot associating filtering.Yet, in many instances, this condition can not be met, each robot all must independently finish the building work of local map, did thereby face a following problem with regard to needs: when each robot had all safeguarded a map about local environment, how this utilize these local maps to obtain the global information of environment? the map of Here it is multirobot merges problem (as shown in Figure 1).

Concern that at the robot mutual alignment map under the unknown situation merges problem, a kind of method is to estimate transformation relation between map by the mutual measurement between the robot, then the feature in the map is carried out association, finally realizes the fusion of map; Or robot is by mutual exchange metrical information, adopts a kind of self-adaptation particle filter algorithm to estimate relative pose information between the robot then, and decides the multirobot cooperation to explore the strategy of environment thus.At the fusion problem of multirobot grating map, can regard it as find the solution optimal transformation process, or solve by the mode of image registration.Yet, present method mostly at be the fusion problem of two-dimensional map.To the fusion problem of three-dimensional geometry map, directly adopt three-dimensional point set registration Algorithm to solve usually, but three-dimensional point set registration Algorithm does not consider that map can reflect this situation of geometric properties in the environment.

Summary of the invention

The invention provides a kind of fusion method of multirobot three-dimensional geometry map, by considering the geometric properties information in the three-dimensional map, merge and three-dimensional point set registration Algorithm in conjunction with two-dimensional map, finish the fusion of multirobot three-dimensional geometry map, obvious improve the accuracy that the three-dimensional geometry map merges.

A kind of fusion method of multirobot three-dimensional geometry map may further comprise the steps:

(1) two width of cloth three-dimensional geometry map M that will be to be merged ₁(x, y, z) and M ₂(x, y, z) project to that dimensionality reduction obtains two-dimensional map on the x-y plane, with fixing size two-dimensional map is carried out rasterizing then and obtain two width of cloth grating maps to be merged, wherein, the grid that the feature existence is arranged is for occupying grid, and the grid that no feature exists then is idle grid, and two kinds of different grids are given different numerical value;

(2) two width of cloth grating maps for the treatment of fusion carry out Fourier-Mellin transform, calculate relative rotation angle θ between the two;

(3) the relative rotation angle θ that utilizes step (2) to obtain is to three-dimensional geometry map M ₂(x, y z) are rotated conversion, obtain the three-dimensional geometry map M ' after the rotational transform ₂(x, y, z);

(4) adopt the most contiguous iterative algorithm, to M ₁(x, y, z) and M ' ₂(x, y z) merge.

Further, described step (4) is:

(4.1) calculate M ' ₂(z) middle every bit is to M for x, y ₁(closest approach z) utilizes the hypercomplex number algorithm for x, y, finds the solution to make M ' ₂(x, y, z) every bit in and M ₁(x, y, z) Euler's square distance between the closest approach of middle correspondence and minimum transformation matrix K;

(4.2) with described transformation matrix matrix K to M ' ₂(x, y z) carry out conversion, the M ' that obtains upgrading ₂(x, y z), and calculate M ₁(x, y is z) with the M ' that upgrades ₂(z) distance between is the result of an iteration for x, y;

(4.3) iteration is carried out in repeating step (4.1) and (4.2), calculates the error of twice iteration, if error greater than ε, continues iterative process; If error is less than ε, iteration finishes.ε is according to fusion accuracy needs value.

The fusion method of multirobot three-dimensional geometry map of the present invention, by considering the geometric properties information in the three-dimensional map, merge and three-dimensional point set registration Algorithm in conjunction with two-dimensional map, finish the fusion of multirobot three-dimensional geometry map, the obvious accuracy of improving the fusion of three-dimensional geometry map.The fusion method of multirobot three-dimensional geometry map of the present invention be mainly used in multirobot locate simultaneously with the map reconstruction application in, the local map that the individual machine people is created merges to form global map.

Description of drawings

Fig. 1 is the synoptic diagram that multirobot three-dimensional geometry map merges problem;

Fig. 2 is the process flow diagram of the fusion method of multirobot three-dimensional geometry map of the present invention;

Fig. 3 is two width of cloth three-dimensional geometry map to be merged as the test input;

Fig. 4 is the projection on two dimensional surface before the test input of Fig. 3 is not merged;

Fig. 5 is the projection of test result on two dimensional surface after test input employing the inventive method shown in Figure 3 is merged;

Fig. 6 is the projection of test result on two dimensional surface after test input employing method of the prior art shown in Figure 3 is merged.

Embodiment

Describe the present invention in detail below in conjunction with embodiment and accompanying drawing, but the present invention is not limited to this.

As shown in Figure 2, a kind of fusion method of multirobot three-dimensional geometry map, with vision sensor perception environment, and how much maps of partial 3 d of creating separately about environment merge to a plurality of robots, and its flow process may further comprise the steps:

Consider that robot moves on the x-y plane, definition dimensionality reduction mapping (being projection model)

f：(x，y，z)→(x，y)

Wherein (x, y z) are the volume coordinate of geometric properties, and (x y) is the planimetric coordinates of geometric properties.

With two width of cloth three-dimensional geometry map M to be merged ₁(x, y, z) and M ₂(x, y z) project to dimensionality reduction on the x-y plane, carry out rasterizing and obtain two width of cloth grating map m to be merged projecting to map on the x-y plane with fixing size then ₁(x, y) and m ₂(x, y).Grating map is made up of the grid cell of several same size, a sub regions on the corresponding x-y of the grid cell plane, and the size of subregion can be adjusted according to actual conditions.In the grating map, the grid that the feature existence is arranged is for occupying grid, and the grid that no feature exists then is idle grid, and two kinds of different grids are given different numerical value.

If two width of cloth grating map m to be merged ₁(x, y) and m ₂(x, y) in, m ₂(x is by to m y) ₁(x y) carries out obtaining behind translation, rotation and the scale transformation, then m ₁(x, y) and m ₂(x, y) existence is following suc as formula the transformation relation shown in (I):

m ₂(x，y)＝m ₁(s(xcosα+ysinα)-x ₀，s(-xsinα+ycosσ)-y ₀) (I)

In the formula (I), s is a zoom factor, and α is the anglec of rotation, x ₀And y ₀Be translational movement;

M then ₁(x, y) and m ₂(x, y) Dui Ying Fourier transform amplitude F ₁(ξ, ζ) and F ₂(ξ satisfies formula (II) between ζ), and is as follows:

$\left|F_2(\mathrm{\&xi;},\mathrm{\&zeta;})\right|=\frac{1}{\left|{\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="HSA00000242664300022.png"\; state="\{\{state\}\}">s</figure-callout>}}^2\right|}\left|F_1\right[(\frac{\mathrm{\&xi;}}{s}\cos \mathrm{\&alpha;}+\frac{\mathrm{\&zeta;}}{s}\sin \mathrm{\&alpha;}),(-\frac{\mathrm{\&xi;}}{s}\sin \mathrm{\&alpha;}+\frac{\mathrm{\&zeta;}}{s}\cos \mathrm{\&alpha;})\left]\right|---\left(\mathrm{II}\right)$

From formula (II) as can be seen, the Fourier transform amplitude F of two width of cloth grating maps to be merged ₁(ξ, ζ) and F ₂(ξ, ζ) relation between is only relevant with zoom factor s and anglec of rotation α, and with translational movement x ₀And y ₀Irrelevant.

Therefore, grating map m ₁(x, y) and m ₂(x, y) fusion is carried out in two steps: ask zoom factor s and rotation angle α by amplitude spectrum earlier, and then find the solution translation parameters x ₀And y ₀

To formula (II) introduce polar coordinates represent (ξ ζ), can obtain formula (III), and is as follows:

$\left|F_2(\mathrm{\&rho;},\mathrm{\&theta;})\right|=s^{-2}\left|F_1(\frac{\mathrm{\&rho;}}{s},\mathrm{\&theta;}-\mathrm{\&alpha;})\right|---\left(\mathrm{III}\right)$

In the formula (III), θ is relative rotation angle, and ρ is a polar radius;

Further adopt the representation of logarithmic coordinate, and ignore coefficient s ^-2, can obtain formula (IV), as follows:

|F ₂(logρ，θ)|＝|F ₁(logρ-logs，θ-α)| (IV)

Formula (IV) can be used Fourier's displacement theory and solve:

If two width of cloth grating map m to be merged ₁(x, y) and m ₂(x, y) middle two width of cloth image I that exist ₁(x, y) and I ₂(x, y) in, image I ₂(x is y) by image I ₁(x y) distinguishes translation x in the x and y direction ₀And y ₀Obtain, then I ₁(x, y) and I ₂(x y) exists suc as formula the transformation relation shown in (V):

I ₂(x，y)＝I ₁(x-x ₀，y-y ₀) (V)

And establish F ₁(ξ, ζ) and F ₂(ξ ζ) is respectively I ₁(x, y) and I ₂(x, y) Dui Ying Fourier transform, then F ₁(ξ, ζ) and F ₂(ξ, satisfy suc as formula the relation shown in (VI) between ζ):

$F_2(\mathrm{\&xi;},\mathrm{\&zeta;})=F_1(\mathrm{\&xi;},\mathrm{\&zeta;})e^{-j(\mathrm{\&xi;}{x}_0+\mathrm{\&zeta;}{y}_0)}---\left(\mathrm{VI}\right)$

The definition image I ₁(x, y) and I ₂(x, cross-power spectrum y) is

$\left|\frac{F_1(\mathrm{\&xi;},\mathrm{\&zeta;}){\mathrm{<figure-callout\; id="2"\; label="F"\; filenames="HSA00000242664300022.png"\; state="\{\{state\}\}">F</figure-callout>}}_2^{*}(\mathrm{\&xi;},\mathrm{\&zeta;})}{F_1(\mathrm{\&xi;},\mathrm{\&zeta;})F_2(\mathrm{\&xi;},\mathrm{\&zeta;})}\right|=e^{-j(\mathrm{\&xi;}{x}_0+\mathrm{\&zeta;}{y}_0)}---\left(\mathrm{VII}\right)$

Wherein

Be F ₂Complex conjugate.(VII) carries out Fourier inversion to formula, (the x on image space ₀, y ₀) locate to obtain an impulse function, the position of pulse is two width of cloth image I ₁(x, y) and I ₂(x, y) the relative translation amount x between ₀And y ₀

After obtaining zoom factor s and rotation angle α thereof, to map m ₂(x y) carries out convergent-divergent and rotation, the map m ' after the generation conversion ₂(x, y), m ' then ₂(x, y) and m ₁(y) only there is translation relation in x in the x and y direction

m′ ₂(x，y)＝m ₁(x-x ₀，y-y ₀) (VIII)

Use Fourier's displacement theory can obtain two width of cloth map m ' once more to formula (VIII) ₂(x, y) and m ₁(x, y) the relative translation amount x between ₀And y ₀

Because three-dimensional geometry map M ₁(x, y, z) and M ₂(x, y, z) projection m in the plane ₁(x, y) and m ₂(x, y) the relative rotation angle θ between can find the solution by the fusion of grating map, therefore, finds the solution the relative rotation angle θ that obtains according to the fusion of grating map, can obtain three-dimensional geometry map M ₁(x, y, z) and M ₂(x, y, relative rotation angle θ z), and with this to three-dimensional geometry map M ₂(x, y z) obtain M ' after being rotated ₂(x, y z), adopt the most contiguous iterative algorithm to carry out the fusion of map then.

The iterative process the most each time of contiguous iterative algorithm is all carried out in two steps.The first step: try to achieve M ' earlier ₂(z) middle every bit is to M for x, y ₁(closest approach z) utilizes the hypercomplex number algorithm to find the solution to make M ' then for x, y ₂(x, y, z) every bit in and M ₁(x, y, z) Euler's square distance between the closest approach of middle correspondence and minimum transformation matrix K; Second step; With transformation matrix K to M ' ₂(x, y z) carry out the M ' that conversion obtains upgrading ₂(x, y z), calculate M then ₁(x, y, z) and M ' ₂(x, y, z) distance between.Repeat above-mentioned iterative process, calculate the error of twice iteration, if error greater than ε, continues iterative process; If error is less than ε, iteration finishes.ε is according to fusion accuracy needs value.

Use the Hausdroff distance to weigh M ₁(x, y, z) and M ' ₂(x, y, z) distance between.Definition three-dimensional geometry map M ₁(x, y, z) and M ₂(x, y, z) the Hausdroff distance D (M between ₁, M ₂) be:

D(M ₁，M ₂)＝max(d(M ₁，M ₂)，d(M ₂，M ₁))

Wherein,

d(M ₁，M ₂)＝||p-q||，p＝max(M ₁)，q＝min(M ₂)

d(M ₂，M ₁)＝||p-q||，p＝max(M ₂)，q＝min(M ₁)

Import as test with two width of cloth three-dimensional geometry maps as shown in Figure 3, projection before it does not merge on two dimensional surface as shown in Figure 4, adopt method of the present invention to merge the projection of back on two dimensional surface as shown in Figure 5, and adopt method of the prior art to merge the projection of back on two dimensional surface as shown in Figure 6.According to the comparison of Fig. 4～6, can draw: use method of the present invention can obtain more objective syncretizing effect.

Claims (2)

1. the fusion method of a multirobot three-dimensional geometry map is characterized in that, may further comprise the steps:

(1) two width of cloth three-dimensional geometry map M that will be to be merged ₁(x, y, z), M ₂(x, y, z) project to that dimensionality reduction obtains two-dimensional map on the x-y plane, with fixing size two-dimensional map is carried out rasterizing then and obtain two width of cloth grating maps to be merged, wherein, the grid that the feature existence is arranged is for occupying grid, and the grid that no feature exists then is idle grid, and two kinds of different grids are given different numerical value;

(2) two width of cloth grating maps for the treatment of fusion carry out Fourier-Mellin transform, calculate the relative rotation angle θ between two width of cloth grating maps;

(3) the relative rotation angle θ that utilizes step (2) to obtain is to three-dimensional geometry map M ₂(x, y z) are rotated conversion, obtain the three-dimensional geometry map M ' after the conversion ₂(x, y, z);

(4) adopt the most contiguous iterative algorithm, to M ₁(x, y, z) and M ' ₂(x, y z) merge.

2. fusion method as claimed in claim 1 is characterized in that, in the described step (4), to M ₁(x, y, z) and M ' ₂(process that z) merges is as follows for x, y:

(4.1) calculate M ' ₂(z) middle every bit is to M for x, y ₁(closest approach z) utilizes the hypercomplex number algorithm for x, y, finds the solution to make M ' ₂(x, y, z) every bit in and M ₁(x, y, z) Euler's square distance between the closest approach of middle correspondence and minimum transformation matrix K;

(4.2) with described transformation matrix matrix K to M ' ₂(x, y z) carry out conversion, the M ' that obtains upgrading ₂(x, y z), and calculate M ₁(x, y is z) with the M ' that upgrades ₂(z) distance between is the result of an iteration for x, y;

(4.3) error of twice iteration is calculated, if error greater than ε, then continues to repeat above-mentioned step in repeating step (4.1) and (4.2); If error is less than ε, then iteration finishes.

Images