CN112815944B - 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 navigation mode gradually becomes the current mainstream popularization AGV navigation mode due to the advantages of low high-precision characteristics, low requirement on the transformation degree of the environment, convenience in construction compared with the magnetic navigation, stability and precision reliability compared with the current visual navigation and the like.

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 to solve 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 the corresponding pre-generated feature set F of the baffle_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_rSample 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 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₁Edge2 (containing the edge length and the connection point ID), 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_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 the matching result into a reflecting plate feature structure matching pair set M to be processed_mpThe matching between the reflecting plate characteristic structural bodies is to compare the edge and corner characteristic units in the reflecting plate characteristic structural bodies one by one, calculate the difference value, and count the number of qualified matched edge and corner characteristic units and the average matching 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_aWithin the range, carrying out next step of matching side length difference values, and if the side length difference values existAnd (3) under the condition that the side length difference values of the two sides are within the range of the set threshold value eT (if the two sides are matched and combined, the difference values are small), the characteristic units of the two sides and the corners are considered to be correspondingly equal, and the 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_mpSorting MatchPair scores from top to bottom according to each matching pair, wherein the scoring mode is that the number of edge characteristic units successfully matched in a matching pair is large with high score suc _ eAe _ sum, the number of suc _ eAe _ sum is the same, the value of an average matching error average _ error is compared, the smaller the average _ error value is, the higher the score is, the situation that a plurality of other objects matched with the same reflector characteristic structure body possibly exist in a matching pair set, at this time, an optimal unique matching pair is sorted and reserved according to the score sorting, the sorting result of the matching pairs of sample laser scanning reflector data and reference map landmark data is obtained, two pairs of repeated pairs exist, and a set M comprising 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 beneficial effect of this application is: the invention provides a laser reflector positioning method based on a corner joint characteristic structural body, which realizes one-to-one correspondence between a real-time laser scanning reflector and a map reference reflector through matching of 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 the case may be.

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 accompanying drawings in conjunction with embodiments.

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_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_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 reflection thereinPlates, e.g. reflecting plates r, 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 the 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 the matching result into a reflecting plate feature structure matching pair set M 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_aWithin the range, then goAnd performing next step of matching side length difference values, and if the side length difference values of the two sides are within the range of the set threshold value eT (if the matching combination of the two sides is in accordance, taking a smaller group of difference values), considering that the characteristic units of the two sides and the corner 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. Because a single edge feature unit has a plurality of successfully matched objects during matching, at this time, we uniquely screen an object group including a repeated matching unit according to the previous sequence from the smallest arrival, and keep the optimal result, and then calculate the number of successfully matched edge feature units suc _ eAe _ sum and the average error of matching of a plurality of edge feature units, and count the structure of a matched pair of reflector feature structures (the structure of a matched pair of reflector feature structures is shown in fig. 4);

step 3, for the reflector plate characteristic structural body matching pair set M finally obtained in the step 2_mpSorting MatchPair scores from top to bottom according to each matching pair, wherein the scoring mode is that the number of edge characteristic units successfully matched in a matching pair is large with high score suc _ eAe _ sum, the number of suc _ eAe _ sum is the same, the value of an average matching error average _ error is compared, the smaller the average _ error value is, the higher the score is, the situation that a plurality of other objects matched with the same reflector characteristic structure body possibly exist in a matching pair set, and at this time, an optimal unique matching pair is sorted and reserved according to the scoring sorting, for example, the sorting result of the matching pairs of the laser scanning reflector data and the reference map landmark data in the example shown in FIG. 5 is obtained, 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 pairs of the sample in the (b) } step

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 to the present application 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 estimated position (x) sensed by 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₀The distance and angle relationships to other reflectors in the same laser data generate a plurality of corner edge feature cells, edge1 denoted by r₀To r₁Edge feature of (1), edge2 denoted by r₀To r₂For a single reflector with n neighboring reflectors, a total of n x (n-1)/2 corner-edge feature cells, e.g., r, are generated₀Finally, the characteristic structure f like a reflecting plate is generated₀Including 3 corner edge feature units for other reflectors (r) in the laser data₁，r₂，r₃) The same operation is carried out to obtain a reflector characteristic structure body set F corresponding to the laser reflector data_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_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 among the reflector characteristic structural bodies is to compare edge and corner characteristic units in the reflector characteristic structural bodies one by one, calculate the difference value of the edge and corner characteristic units and count the number of matched edge and corner characteristic units and the matching average error of the matched edge and corner characteristic units; 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_aIf the side length difference values of the two sides are within the range of the set threshold value eT, the characteristic units of the two corner sides are considered to be correspondingly equal, and a matching error is recorded; when two reflector plate feature structures are matched, the corner edge feature units contained in the two reflector plate feature structures are matched one by one according to the method, the successfully matched units are matched, and the units are sorted from small to large according to the average error; when matching is carried out, the condition that a single edge characteristic unit has a plurality of objects successfully matched occurs, at the moment, according to the previous sequence from small to large, object groups containing repeated matching units are subjected to unique screening, the optimal result is kept, and then the successfully matched edge characteristic unit number suc _ eAe _ sum and the average error of matching of the plurality of edge characteristic units are calculated to calculate the matched pair structure of the reflector plate characteristic structure;

step 3, matching pair set M of the characteristic structural body of the reflecting plate finally obtained in the step 2_mpSorting the scores of MatchPair from top to bottom according to the scores of each matching pair, wherein the scoring mode is that the number of edge characteristic units successfully matched in the matching pairs is large in suc _ eAe _ sum, the score is high, the suc _ eAe _ sum is the same, the value of an average matching error average _ error is compared, the smaller the value of the average _ error is, the higher the score is, the condition that a plurality of other objects matched with the same reflector characteristic structure exist in the matching pair set, the optimal unique matching pair is sorted and reserved according to the score sorting at this time, the matching pair sorting result of the sample laser scanning reflector data and the reference map landmark data is obtained, two pairs of repeated pairs are provided, and a set M comprising 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_mpObtaining reflector matching point pairs corresponding to the laser data and the map landmarks one by one, such as 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 one-to-one corresponding accurate matching point pairs are obtained, considering that the position between a map landmark and a reflector in laser scanning data which are actually arranged is a connected and fixed rigid body structure, the angle difference of the attitude angle of a connecting vector formed between every two laser reflectors 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 is 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 is obtained through the formula (1), and then the translation amount is obtained

Obtained by 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 the accurate map positioning pose (x) is obtained_c，y_c，theta_c)。

Images