CN112815944A - Laser reflector positioning method based on corner joint characteristic structure - Google Patents

Abstract

The application discloses a laser reflector positioning method based on corner joint feature structures, which comprises the steps of processing laser reflector data, extracting and generating a corner feature structure set of each reflector, matching the reflectors in laser with the reflectors on a map one by one through a corner feature matching method, judging whether matching is successful according to a set matching threshold value, and sequencing matched point sets from top to bottom according to matching results. And then, carrying out pairing uniqueness detection on the sequencing result of the obtained reflector plate pairing point set, removing the pairing of the reflector plates which participate repeatedly, and taking the optimal front three groups of reflector plate pairing points as data for carrying out geometric matching next. And finally, accurately transforming and positioning the initial estimated pose of the laser sensor into an accurate pose on the map by using the three groups of acquired one-to-one corresponding laser reflectors and the map reference landmark two-dimensional coordinates through plane coordinate transformation, and finishing the accurate positioning of the reflectors.

Description

Laser reflector positioning method based on corner joint characteristic structure

Technical Field

The application relates to a positioning method, in particular to a laser reflector positioning method based on a corner joint characteristic structural body.

Background

With the continuous application of Automatic Guided Vehicles (AGVs) in the industries of automatic production, transportation, storage and logistics and the like of the whole factory, the navigation technology of the AGVs is more and more important. The navigation technology applied in the current AGV mainly comprises magnetic navigation, laser navigation, visual navigation and the like, wherein the laser navigation is taken as a navigation technical scheme which is rapidly popularized in recent years, and the AGV gradually becomes the AGV navigation mode which is currently and mainly popularized due to the characteristics of high precision and a plurality of advantages that the requirement on the transformation degree of the environment is low, the construction is more convenient than the magnetic navigation, and the stability and the precision reliability are more convenient than the current visual navigation.

The laser navigation technology mainly applied at present includes pure natural contour navigation (slam), laser reflector navigation and hybrid navigation (the pure natural slam and the reflector are used in a hybrid way). The vehicle position is positioned by constructing an environment map through pure natural navigation and using laser contour matching, the environment does not need to be transformed, the vehicle position is convenient, but the environment is easy to change or the position with a plurality of moving objects is easy to cause the unstable problem of positioning error increase or failure, and the laser reflector navigation mode adds an artificial fixed landmark through the mode of installing an artificial laser reflector in the working environment, so that the navigation stability and the positioning precision are improved when the environment is changed, and the vehicle position is widely adopted in the industrial manufacturing high-precision occasions at present.

Most of the current laser reflector navigation is based on field continuous search or based on side length characteristics between reflectors to realize data association between reflector data in laser scanning of a laser sensor and reflector data in a map, and then pose solving is carried out. However, reflector data association realized based on a continuous domain search mode needs to ensure accurate estimation of the initial pose of the AGV, and reflector data association based on reflector side length characteristics has high requirements on unequal constraint of side lengths arranged between reflectors, and in some occasions, incorrect matching is easily caused due to excessively uniform reflector arrangement, so that positioning navigation fails. Therefore, a laser reflector positioning method based on the corner joint feature structure is provided for solving the above problems.

Disclosure of Invention

A laser reflector positioning method based on corner joint characteristic structure bodies comprises the following steps;

step 1 Reflector data set Q on a known reference map_m{(x_m0，y_m0)，(x_m1，y_m1)，...，(x_mn，y_mn) And corresponding onesThe generated feature structure set F of the reflector_mReflector data set Q extracted by real-time scanning laser_r{(x_r0，y_r0)，(x_r1，y_r1)，...，(x_rk，y_rk) And for each reflector data, generating a reflector characteristic structure FeatureMark by combining the distance and mutual angle relationship among reflectors in the data set, and further obtaining a reflector characteristic structure set F corresponding to the laser scanning reflector data set_rThe sample laser data includes an approximate estimated position (x) of the laser sensor_ini，y_ini，theta_ini) And reflecting plate point set Q_r{(x_r0，y_r0)，(x_r1，y_r1)，(x_r2，y_r2)，(x_r3，y_r3) For each of the reflecting plates, reflecting plate r₀By using r₀Distance versus angle relationships to other reflectors in the same laser data can generate a plurality of corner edge feature cells, where edge1 is denoted by r₀To r₁Edge characteristics (including edge length and connection point ID) of (c), edge2 is denoted by r₀To r₂The angle represents the angle between two edges, and for a single reflector with n neighboring reflectors, n x (n-1)/2 edge-corner feature cells, such as r, can be generated₀Finally, the characteristic structure r of the reflector plate can be generated₀Including 3 corner edge feature units, for other reflectors (r) in the laser data₁，r₂，r₃) By performing the same operation, a reflector characteristic structure set F corresponding to the laser reflector data can be obtained_r{f_r0，f_r1，f_r2，f_r3}, baffle data Q_mThe characteristic structural body set F of the reflecting plate is obtained by the pretreatment of the process_m{f_m0，f_m1，f_m2，f_m3，f_m4}；

Step 2, processing the data of the laser reflector in the step 1 to obtain a characteristic structural body set F of the laser real-time scanning reflector_r{f_r0，f_r1，f_r2，f_r3And the parameterReflector feature structure set F obtained by examination map preprocessing_m{f_m0，f_m1，f_m2，f_m3，f_m4Performing matching operation of a feature structure FeatureMark, pushing a matching result into a matching pair set M of the feature structure of the reflector to be processed_mpThe matching between the reflector characteristic structural bodies is to compare the edge and corner characteristic units in the reflector characteristic structural bodies one by one, calculate the difference value, and count the number of qualified matched edge and corner characteristic units and the matching average error thereof. The matching calculation process of the edge and corner feature units is as follows:

firstly, calculating the angle difference of two corner edge characteristic units, and if the difference is at a set threshold value d_aAnd if the side length difference values of the two sides are within the range of the set threshold value eT (if the two side matching combinations are in accordance, the difference values are smaller), the characteristic units of the two corner sides are considered to be correspondingly equal, and a matching error is recorded. When two reflector feature structures are matched, edge and corner feature units contained in the two reflector feature structures are matched one by one according to the method, the successfully matched units are paired, and sequencing is carried out from small to large according to the average error. Because when matching, a situation that a single edge feature unit has a plurality of objects successfully matched occurs, at this time, we perform unique screening on an object group including a repeated matching unit according to the previous sequence from small arrival, retain an optimal result, and then calculate the number suc _ eAe _ sum of successfully matched edge feature units and the average error average _ error of matching of a plurality of edge feature units to count a reflector feature structure matching pair structure (the reflector feature structure matching pair structure is shown in fig. 4);

step 3, for the reflector plate characteristic structural body matching pair set M finally obtained in the step 2_mpAnd ranking the scores of the MatchPair from top to bottom according to each matching pair, wherein the scoring mode is that the score of the successfully matched edge feature unit number suc _ eAe _ sum in the matching pair is high, the score of the successfully matched edge feature unit number suc _ eAe _ sum is same as the score of the successfully matched edge feature unit number suc _ eAe _ sum, the average matching error average _ error is compared, the score is higher when the average _ error value is smaller, and the score is higher in the matching pair setIn the method, a plurality of other objects matched with the same reflector characteristic structural body may exist, at this time, the optimal unique matching pairs are screened and reserved according to the grading sorting, the matching pair sorting result of the sample laser scanning reflector data and the reference map landmark data is provided with two pairs of repeated pairs, and a set M containing four pairs of matched pairs is reserved after screening and deletion_mp{(f_r1，f_m2)，(f_r2，f_m4)，(f_r0，f_m1)，(f_r3，f_m0)}；

Step 4, through the acquired set M of matching pairs_mpReflector match point pairs with one-to-one correspondence of laser data to map landmarks, e.g., from M_mp{(f_r1，f_m2)，(f_r2，f_m4)，(f_r0，f_m1)，(f_r3，f_m0) Get the matching point pair of the sample in }

After the one-to-one corresponding accurate matching point pairs are obtained, considering that the position between the actually arranged map landmarks and the reflecting plate in the laser scanning data can be regarded as a connected and fixed rigid body structure, the angle difference of the attitude angle of the connecting vector formed between every two laser reflecting plate data in a laser coordinate system and the vector attitude angle formed by connecting the corresponding landmarks in the map coordinate system is the rotation angle of the two point sets, and the difference of the centroid coordinates of the point sets after rotation is the translation amount of the two point sets, so that the coordinate transformation relation between the two point sets can be obtained. There will be a set Q of (n +1) pairs of matching points_mpIs shown as

The rotation angle theta of the point set transformation can be obtained by formula (1), and then the translation amount is obtained

Can be obtained from formula (2) and formula (3);

step 5, the initial pose (x) of the laser sensor is determined_ini，y_ini，theta_ini) Through the obtained rotation translation amount between the laser scanning reflector data and the map landmark

Coordinate transformation is carried out according to the formula (4), and accurate map positioning pose (x) can be obtained_c，y_c，theta_c) The effect of the rotational translation is shown in fig. 6.

The beneficial effect of this application is: the invention provides a laser reflector positioning method based on corner joint characteristic structural bodies, which realizes one-to-one correspondence between a real-time laser scanning reflector and a map reference reflector through matching of the corner characteristic structural bodies, and further performs geometric matching to obtain an accurate current real-time pose of a robot. Compared with the original reflector matching method based on pure contour matching or only edge features, the method increases the angle features, has stronger stability and accuracy due to the multi-dimensional features, improves the reliability of reflector registration, can realize global positioning in a larger map range, and simultaneously strengthens the anti-interference performance on noise.

Drawings

In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings needed to be used in the description of the embodiments or the prior art will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art that other drawings can be obtained according to the drawings without inventive exercise.

FIG. 1 is a schematic diagram of a reflector feature structure extracted from laser scanning data according to the present application;

FIG. 2 is a schematic diagram of map data referenced in the present application;

FIG. 3 is a schematic diagram illustrating edge feature unit matching according to the present application;

FIG. 4 is a schematic diagram of a matching pair of characteristic structures of the reflecting plate of this application;

FIG. 5 is a schematic diagram illustrating sorting and screening of matching pairs of laser scanning reflector data and reference map landmark data according to the present application;

fig. 6 is a schematic diagram illustrating the matching effect of the laser reflector data and the map after the present rotational translation.

Detailed Description

In order to make the technical solutions better understood by those skilled in the art, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only partial embodiments of the present application, but not all embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.

It should be noted that the terms "first," "second," and the like in the description and claims of this application and in the drawings described above are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It should be understood that the data so used may be interchanged under appropriate circumstances such that embodiments of the application described herein may be used. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed, but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.

In this application, the terms "upper", "lower", "left", "right", "front", "rear", "top", "bottom", "inner", "outer", "middle", "vertical", "horizontal", "lateral", "longitudinal", and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings. These terms are used primarily to better describe the present application and its embodiments, and are not used to limit the indicated devices, elements or components to a particular orientation or to be constructed and operated in a particular orientation.

Moreover, some of the above terms may be used to indicate other meanings besides the orientation or positional relationship, for example, the term "on" may also be used to indicate some kind of attachment or connection relationship in some cases. The specific meaning of these terms in this application will be understood by those of ordinary skill in the art as appropriate.

Furthermore, the terms "mounted," "disposed," "provided," "connected," and "sleeved" are to be construed broadly. For example, it may be a fixed connection, a removable connection, or a unitary construction; can be a mechanical connection, or an electrical connection; may be directly connected, or indirectly connected through intervening media, or may be in internal communication between two devices, elements or components. The specific meaning of the above terms in the present application can be understood by those of ordinary skill in the art as appropriate.

It should be noted that the embodiments and features of the embodiments in the present application may be combined with each other without conflict. The present application will be described in detail below with reference to the embodiments with reference to the attached drawings.

Referring to fig. 1-6, a method for positioning a laser reflector based on a corner combining feature structure includes the following steps:

step 1 Reflector data set Q on a known reference map_m{(x_m0，y_m0)，(x_m1，y_m1)，...，(x_mn，y_mn) And a corresponding pre-generated feature structure set F of the reflector_mBy scanning laser in real timeLight extracted reflector plate dataset Q_r{(x_r0，y_r0)，(x_r1，y_r1)，...，(x_rk，y_rk) And for each reflector data, generating a reflector characteristic structure FeatureMark by combining the distance and mutual angle relationship among reflectors in the data set, and further obtaining a reflector characteristic structure set F corresponding to the laser scanning reflector data set_rAs shown in FIG. 1.a, the sample laser data contains the approximate estimated position (x) of the laser sensor_ini，y_ini，theta_ini) And reflecting plate point set Q_r{(x_r0，y_r0)，(x_r1，y_r1)，(x_r2，y_r2)，(x_r3，y_r3) For each of them, a reflector plate r as shown in FIG. 1.b₀By using r₀The distance and angular relationship to other reflectors in the same laser data can generate a plurality of corner-edge feature cells as shown in FIG. 1.c, where edge1 is denoted by r₀To r₁Edge characteristics (including edge length and connection point ID) of (c), edge2 is denoted by r₀To r₂The angle represents the angle between two edges, and for a single reflector with n neighboring reflectors, n x (n-1)/2 edge-corner feature cells, such as r, can be generated₀The final reflector feature f shown in FIG. 1.d can be produced₀Including 3 corner edge feature units, for other reflectors (r) in the laser data₁，r₂，r₃) By performing the same operation, a reflector characteristic structure set F corresponding to the laser reflector data shown in the figure can be obtained_r{f_r0，f_r1，f_r2，f_r3}, sample Reflector data Q of a reference map (shown in FIG. 2)_mThe characteristic structural body set F of the reflecting plate is obtained by the pretreatment of the process_m{f_m0，f_m1，f_m2，f_m3，f_m4}；

Step 2, processing the data of the laser reflector in the step 1 to obtain a characteristic structural body set F of the laser real-time scanning reflector_r{f_r0，f_r1，f_r2，f_r3Reflector feature structure set F obtained by preprocessing with reference map_m{f_m0，f_m1，f_m2，f_m3，f_m4Performing matching operation of a feature structure FeatureMark, pushing a matching result into a matching pair set M of the feature structure of the reflector to be processed_mpThe matching between the reflector characteristic structural bodies is to compare the edge and corner characteristic units in the reflector characteristic structural bodies one by one, calculate the difference value, and count the number of qualified matched edge and corner characteristic units and the matching average error thereof. The matching calculation flow of the edge and corner feature unit is shown in fig. 3:

firstly, calculating the angle difference of two corner edge characteristic units, and if the difference is at a set threshold value d_aAnd if the side length difference values of the two sides are within the range of the set threshold value eT (if the two side matching combinations are in accordance, the difference values are smaller), the characteristic units of the two corner sides are considered to be correspondingly equal, and a matching error is recorded. When two reflector feature structures are matched, edge and corner feature units contained in the two reflector feature structures are matched one by one according to the method, the successfully matched units are paired, and sequencing is carried out from small to large according to the average error. Because when matching, a situation that a single edge feature unit has a plurality of objects successfully matched occurs, at this time, we perform unique screening on an object group including a repeated matching unit according to the previous sequence from small arrival, retain an optimal result, and then calculate the number suc _ eAe _ sum of successfully matched edge feature units and the average error average _ error of matching of a plurality of edge feature units to count a reflector feature structure matching pair structure (the reflector feature structure matching pair structure is shown in fig. 4);

step 3, for the reflector plate characteristic structural body matching pair set M finally obtained in the step 2_mpAnd sorting the scores of the MatchPair from top to bottom according to each matching pair, wherein the scoring mode is that the score of the successfully matched edge feature unit number suc _ eAe _ sum in the matching pair is high, and if the suc _ eAe _ sum is the same, the average error average _ error value is compared, and the average error _ error value is comparedThe smaller the or value is, the higher the score is, the situation that a plurality of other objects matched with the same reflector characteristic structure body may exist in the matching pair set, at this time, the optimal unique matching pair is sorted, screened and reserved according to the score, for example, a matching pair sorting result of the laser scanning reflector data and the reference map landmark data in the example shown in fig. 5, wherein two pairs of repeated pairs exist, and a set M containing four pairs of matching pairs is reserved after screening and deletion_mp{(f_r1，f_m2)，(f_r2，f_m4)，(f_r0，f_m1)，(f_r3，f_m0)}；

Step 4, through the acquired set M of matching pairs_mpReflector match point pairs with one-to-one correspondence of laser data to map landmarks, e.g., from M_mp{(f_r1，f_m2)，(f_r2，f_m4)，(f_r0，f_m1)，(f_r3，f_m0) Get the matching point pair of the sample in }

After the one-to-one corresponding accurate matching point pairs are obtained, considering that the position between the actually arranged map landmarks and the reflecting plate in the laser scanning data can be regarded as a connected and fixed rigid body structure, the angle difference of the attitude angle of the connecting vector formed between every two laser reflecting plate data in a laser coordinate system and the vector attitude angle formed by connecting the corresponding landmarks in the map coordinate system is the rotation angle of the two point sets, and the difference of the centroid coordinates of the point sets after rotation is the translation amount of the two point sets, so that the coordinate transformation relation between the two point sets can be obtained. There will be a set Q of (n +1) pairs of matching points_mpIs shown as

The rotation angle theta of the point set transformation can be obtained by formula (1), and then the translation amount is obtained

Can be obtained from formula (2) and formula (3);

step 5, the initial pose (x) of the laser sensor is determined_ini，y_ini，theta_ini) Through the obtained rotation translation amount between the laser scanning reflector data and the map landmark

Coordinate transformation is carried out according to the formula (4), and accurate map positioning pose (x) can be obtained_c，y_c，theta_c) The effect of the rotational translation is shown in fig. 6.

The application has the advantages that:

according to the method, the corner structure body is generated for each reflecting plate by extracting the corner features between the reflecting plates in the laser measurement data, feature matching is carried out on the corner structure body and map reflecting plate data, corresponding data association is achieved, and then the real-time pose of the laser sensor is obtained through geometric relation matching.

The above description is only a preferred embodiment of the present application and is not intended to limit the present application, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, improvement and the like made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims (1)

1.A laser reflector positioning method based on corner joint feature structure body is characterized by comprising the following steps: the positioning method comprises the following steps:

step 1 Reflector data set Q on a known reference map_m{(x_m0，y_m0)，(x_m1，y_m1)，...，(x_mn，y_mn) And a corresponding pre-generated feature structure set F of the reflector_mReflector data set Q extracted by real-time scanning laser_r{(x_r0，y_r0)，(x_r1，y_r1)，...，(x_rk，y_rk) And for each reflector data, generating a reflector characteristic structure FeatureMark by combining the distance and mutual angle relationship among reflectors in the data set, and further obtaining a reflector characteristic structure set F corresponding to the laser scanning reflector data set_rThe sample laser data includes an approximate estimated position (x) of the laser sensor_ini，y_ini，theta_ini) And reflecting plate point set Q_r{(x_r0，y_r0)，(X_r1，y_r1)，(x_r2，y_r2)，(x_r3，y_r3) For each of the reflecting plates, reflecting plate r₀By using r₀Distance versus angle relationships to other reflectors in the same laser data can generate a plurality of corner edge feature cells, where edge1 is denoted by r₀To r₁Edge feature of (1), edge2 denoted by r₀To r₂The angle represents the angle between two edges, and for a single reflector with n neighboring reflectors, n x (n-1)/2 edge-corner feature cells, such as r, can be generated₀Finally, the characteristic structure f of the reflector plate can be generated₀Including 3 corner edge feature units, for other reflectors (r) in the laser data₁，r₂，r₃) By performing the same operation, a reflector characteristic structure set F corresponding to the laser reflector data can be obtained_r{f_r0，f_r1，f_r2，f_r3}, baffle data Q_mThe characteristic structural body set F of the reflecting plate is obtained by the pretreatment of the process_m{f_m0，f_m1，f_m2，f_m3，f_m4}；

Step 2, processing the data of the laser reflector in the step 1 to obtain the characteristic structure of the laser real-time scanning reflectorBody set F_r{f_r0，f_r1，f_r2，f_r3Reflector feature structure set F obtained by preprocessing with reference map_m{f_m0，f_m1，f_m2，f_m3，f_m4Performing matching operation of a feature structure FeatureMark, pushing a matching result into a matching pair set M of the feature structure of the reflector to be processed_mpThe matching between the reflector characteristic structural bodies is to compare the edge and corner characteristic units in the reflector characteristic structural bodies one by one, calculate the difference value, and count the number of qualified matched edge and corner characteristic units and the matching average error thereof. The matching calculation process of the edge and corner feature units is as follows:

firstly, calculating the angle difference of two corner edge characteristic units, and if the difference is at a set threshold value d_aAnd in the range, carrying out next step of side length difference matching, and if the side length differences of the two sides are within the range of the set threshold value eT, considering that the characteristic units of the two corner sides are correspondingly equal, and recording a matching error. When two reflector feature structures are matched, edge and corner feature units contained in the two reflector feature structures are matched one by one according to the method, the successfully matched units are paired, and sequencing is carried out from small to large according to the average error. When matching, a situation that a single edge feature unit has a plurality of objects successfully matched occurs, at this time, the object group comprising the repeated matching units is subjected to unique screening according to the previous sequence from small arrival, an optimal result is kept, and then the successfully matched edge feature unit number suc _ eAe _ sum and the average error of matching of the plurality of edge feature units are calculated to calculate the matching pair structure of the reflector feature structure;

step 3, for the reflector plate characteristic structural body matching pair set M finally obtained in the step 2_mpAnd sorting the scores of the MatchPair from top to bottom according to each matching pair, wherein the scoring mode is that the number of the edge characteristic units successfully matched in the matching pair suc _ eAe _ sum is high in score, the same number of suc _ eAe _ sum is obtained by comparing the values of the average matching error average _ error, the smaller the value of the average _ error is, the higher the score is, and the probability of the average matching error exists in the matching pair setUnder the condition that a plurality of other objects are matched on the same reflector characteristic structural body, at this time, the optimal unique matching pairs are screened and reserved according to the grading sorting, the sorting result of the matching pairs of the sample laser scanning reflector data and the reference map landmark data is provided, wherein two pairs of repeated pairs exist, and a set M containing four pairs of matching pairs is reserved after screening and deletion_mp{(f_r1，f_m2)，(f_r2，f_m4)，(f_r0，f_m1)，(f_r3，f_m0)}；

Step 4, through the acquired set M of matching pairs_mpReflector match point pairs with one-to-one correspondence of laser data to map landmarks, e.g., from M_mp{(f_r1，f_m2)，(f_r2，f_m4)，(f_r0，f_m1)，(f_r3，f_m0) Get the matching point pair of the sample in }

After the one-to-one corresponding accurate matching point pairs are obtained, considering that the position between the actually arranged map landmarks and the reflecting plate in the laser scanning data can be regarded as a connected and fixed rigid body structure, the angle difference of the attitude angle of the connecting vector formed between every two laser reflecting plate data in a laser coordinate system and the vector attitude angle formed by connecting the corresponding landmarks in the map coordinate system is the rotation angle of the two point sets, and the difference of the centroid coordinates of the point sets after rotation is the translation amount of the two point sets, so that the coordinate transformation relation between the two point sets can be obtained. There will be a set Q of (n +1) pairs of matching points_mpIs shown as

The rotation angle theta of the point set transformation can be obtained by formula (1), and then the translation amount is obtained

Can be obtained from formula (2) and formula (3);

step 5, the initial pose (x) of the laser sensor is determined_ini，y_ini，theta_ini) Through the obtained rotation translation amount between the laser scanning reflector data and the map landmark

Coordinate transformation is carried out according to the formula (4), and accurate map positioning pose (x) can be obtained_c，y_c，theta_c)。

Images