1. A mobile robot positioning method based on dotted line feature scanning matching is characterized by comprising the following steps:

step 1: the mobile robot scans the surrounding environment through the carried laser radar sensor, acquires two-dimensional point cloud data from the laser radar, and performs coordinate conversion and filtering pretreatment:

step 1.1: the acquired two-dimensional point cloud data is represented asWhereinThe observed quantity of the ith laser beam of the laser radar at the time t is measured, d is the measured distance of the obstacle, and eta is the scanning resolution of the laser radar; coordinate conversion is carried out through a formula (1), and the obtained original point cloud data is converted from polar coordinates to rectangular coordinates to obtain

Step 1.2: carrying out filtering processing on the two-dimensional point cloud data: when the distance from one scanning point to two adjacent scanning points is greater than a threshold value gamma, calculating the distance from the scanning point to a line segment connected with the two adjacent scanning points, when the distance is greater than the threshold value lambda, judging the scanning point as an outlier, and removing the scanning point;

step 1.3: acquiring a frame of point cloud data at the time t, and performing coordinate conversion and filtering to obtain a point cloudData ofWhereinn is the number of scanning points contained in the frame point cloud data;

step 2: point-to-point cloud data P_tSequentially taking m scanning points from the scanning sequence to form alternative seed segments, performing Singular Value Decomposition (SVD) on each alternative seed segment to obtain n-m +1 fitting residual square sums, and performing sequencing and screening:

step 2.1: sequentially taking m scanning points from the ith scanning point to form alternative seed subsectionsPerforming Singular Value Decomposition (SVD) according to formula (2):

wherein u is_iIs a left singular matrix; s_iIs a matrix of singular values and is,and isv_iIn the form of a right singular matrix, is composed ofMean value of (i)

Step 2.2: when m scanning points are subjected to linear fitting, n-m +1 fitting residual square sums are obtainedSequencing in an ascending order; setting a threshold epsilon, discarding E_iAlternative seed segments > epsilon, remaining seed segmentsFor the region growing algorithm, where N is the number of seed segments;

and step 3: fitting a straight line to the Seed segments in the Seed, growing and extending to obtain straight line characteristics, and carrying out merging operation:

step 3.1: for seed sectionFitting a straight line by using a total least square method, and calculating a straight line equation y as a by formula (3)_ix+b_i：

Wherein a is_iAnd b_iRespectively representing the slope and intercept of the straight line,andis taken from

Step 3.2: the growth conditions for the region growing algorithm are set as: the distance from the nearest scanning point on either side of the seed section to the fitted straight line is less than a threshold value delta;

the termination conditions were set as: the distance from the nearest scanning points on the two sides of the seed section to the fitting straight line is larger than a threshold value delta, or the distance reaches the edge of the point cloud;

and (3) taking the growth condition and the termination condition as judgment conditions to carry out growth extension on the seed sections:

step 3.2.1: the seed section extends backwards;

at the seed section S_iGet the nearest neighbor of the back scan point in the neighborhood of (2)Calculating the scanning point to linear equation y ═ a_ix+b_iLinear distance D of_b：

If the condition D is satisfied_b<δ, then scanning the spotIncluding the seed section, expanding the seed section backwards; then, the current nearest rear side scanning point is calculatedTo the linear equation y ═ a_ix+b_iLinear distance D of_bIf the condition D is satisfied_b<Delta then willAn inclusion seed segment; the execution is repeated until the termination condition is met: the current nearest neighbor back side scan point to line equation y ═ a_ix+b_iThe linear distance of the seed segment is greater than a threshold value delta, or the scanning point is a point cloud edge point, and the seed segment stops extending backwards;

step 3.2.2: the seed section extends forwards;

at the seed section S_iRear side scan of the nearest neighbor in the neighborhood of (1)DotCalculating the scanning point to linear equation y ═ a_ix+b_iLinear distance D of_f：

If the condition D is satisfied_f<δ, then scanning the spotIncluding the seed section, expanding the seed section forwards; then, the current nearest front side scanning point is calculatedTo the linear equation y ═ a_ix+b_iLinear distance D of_fSatisfies the condition D_f<Delta then willAn inclusion seed segment; the execution is repeated until the termination condition is met: the current nearest neighbor anterior scan point to line equation y ═ a_ix+b_iThe linear distance of the seed segment is greater than a threshold value delta, or the scanning point is a point cloud edge point, and the seed segment stops extending forwards;

when both sides stop growing, a complete seed section is obtainedWherein s is_iAnd e_iRespectively represent the seed segments S_iHead and tail scanning point in P_tThe subscript of the sequence number (C); to S_iFitting the line by using a total least square method according to formula (3), and determining the slope a of the fitted line_iAnd intercept b_iUpdating is carried out;

step 3.3: determining end points on two sides of the straight line feature;

for seed of a speciesSub-segment S_iTaking the head and tail scanning pointsAndto a straight line y ═ a_ix+b_iMaking a perpendicular line intersecting the straight lineAndtwo points, thenAndthe end points of the straight line feature on the head and the tail are calculated by the formulas (6) and (7)Andthe coordinates of (a):

then by the seed section S_iThe obtained straight line features are expressed asWhereinIs the center of gravity of the straight line feature, which is also the center,l_iindicating the length of a straight line feature, i.e.Straight line characteristic L from origin of coordinate system_iAs an auxiliary vertical line, r_iAnd theta_iRespectively representing the length of the auxiliary vertical line under the current laser radar coordinate system and an included angle between the auxiliary vertical line and the x axis of the transverse axis;

step 3.4: after the linear features of all the seed sections are extracted, reordering all the linear features according to the acquisition sequence of the scanning points contained in the seed sections, and then judging and combining:

when two straight line characteristics are same approximate straight line after present, supplementary plumb line between them also can approximate coincidence, judge two straight line characteristics through the geometric position of two supplementary plumb lines of comparison and whether can merge to avoid the too big condition that leads to the disconnection of straight line characteristic of individual scanning point noise, specifically as follows:

get L_iAnd L_i+1I ∈ 1,2, …, N-1, making a conditional judgment:

step 3.4.1: judging conditions: | r_i-r_i+1|<Δ r, where Δ r is the maximum length difference between the secondary perpendicular lines of the two straight line features, r_i+1Feature L representing straight line passing through origin of coordinate system_i+1The length of the auxiliary vertical line under the current laser radar coordinate system and the transverse axis_xThe included angle of the axes;

step 3.4.2: judging conditions: [ theta ]_i-θ_i+1|<Δ θ, where Δ θ is the maximum angular difference between the secondary perpendicular lines of the two straight line features, θ_i+1Feature L representing straight line passing through origin of coordinate system_i+1Making an included angle between an auxiliary vertical line and the x axis of the transverse axis under the current laser radar coordinate system;

when the two conditions are satisfied simultaneously, the straight line characteristic L is determined_iAnd L_i+1Are combined to form L_iAnd to L_iThe parameters are updated and obtained after the combination is finishedWherein N is₁Number of straight features after merging, N₁≤N；

And 4, step 4: extracting angular point features;

step 4.1: fromSequentially taking two adjacent straight line characteristics L_iAnd L_i+1Judging whether the conditions of the corner feature are met:

step 4.1.1: and (3) condition judgment: [ theta ]_i-θ_i+1|∈[90°-σ,90°+σ]Where σ is an uncertainty parameter for determining the corner;

step 4.1.2: and (3) condition judgment:wherein D_maxIs the farthest distance of the adjacent end points of the linear features forming the angular point features;

step 4.2, if the conditions of the step 4.1.1 and the step 4.1.2 are simultaneously met, the straight line characteristic L is considered_iAnd L_i+1Can extend and intersect to form an angular point feature C_jAnd calculating the coordinates of the corner feature by using the formula (8):

wherein a is_i+1And b_i+1Respectively represent the seed segments S_i+1Fitting the slope and intercept of the straight line using a total least squares method; then by the straight line feature L_iAnd L_i+1The formed corner feature is expressed asWhereinPosition coordinates of the angular point features;andis the angle between two right-angle sides of the angular point feature and the x axis of the cross axis under the current laser radar coordinate system, namelyJudging and calculating all linear features to obtain an angular point feature set of the frame point cloudWherein N is₂The number of extracted corner features;

and 5: reading the laser radar point cloud data at the time of t +1 in the motion process of the mobile robot, and performing the same processing operation from step 1 to step 4 to extract point-line features, thereby obtaining a linear feature set in the point cloud data at the time of t +1And corner feature setCalculating the matching degree according to the following steps to find the corresponding relation between two frames of point clouds; the matching degree is divided into a straight matching degree and an angular point matching degree;

step 5.1: calculating the matching degree of the straight lines;

setting a sliding window with a fixed size for linear features Line and Line' of front and back two frames of point clouds to determine a linear feature pair needing matching calculation;

for straight line feature L_ie.Line and L_j'epsilon' and the straight Line matching degree is as follows:

whereinAndrespectively a length matching degree, a position matching degree and an angle matching degree, and

wherein DL_maxThe maximum gravity center distance difference of two matched straight line features is obtained; delta theta_maxThe maximum included angle difference of two straight line characteristics which can be matched;

variables in the formulas in the step 5 and the step 6 with the top right corner mark' both represent the laser radar point cloud data at the time t +1, and the variable definitions are the same as those at the time t;

step 5.2: calculating the angular point matching degree;

determining the corner feature pair needing matching calculation by using the same sliding window for the corner features CP and CP' of the front and back frame point clouds, and taking C_iE.g. CP and C_j'belongs to CP', the corner matching degree of the two-corner feature is as follows:

whereinIn order to achieve a degree of positional matching,andis the angle matching degree of two right-angle sides of the angular point characteristics, and

wherein DL_maxAnd Δ θ_maxThe parameters are the same as the parameters in the formulas (11) and (12), and are respectively the maximum coordinate distance difference and the maximum included angle difference of the two matched linear features;

step 6: estimating the motion parameters of the mobile robot by using the linear matching degree and the angular point matching degree obtained in the step 5, and calculating the pose transformation of the mobile robot between the front frame point cloud and the back frame point cloud;

the motion of the mobile robot is divided into translation and rotation, the translation parameter (x)_t,y_t) And a rotation parameter theta_tThe calculation process is expressed as:

wherein n is₁，n₂Respectively the number of the linear feature matching pairs and the angular point feature matching pairs with the matching degree not being 0;representing a linear feature matching pair (L)_i,L_j') the weight to be taken up in the calculation of the parameters, matching pairs of feature representing corner points (C)_i,C_j') the weight to be taken up in the calculation of the parameters,

the rotation matrix of the pose transformation of the mobile robot between the front frame point cloud and the back frame point cloud, namely from the time t to the time t +1 isThe translation matrix is

2. The method as claimed in claim 1, wherein the step 1.2 is to determine the position of the mobile robot based on the point-line feature scan matchingStep of outlierThe method comprises the following steps:

step 1.2.1: judging conditions:wherein gamma is the maximum distance threshold from a scanning point to an adjacent scanning point;

step 1.2.2: judging conditions:

step 1.2.3: judging conditions:whereinAre respectively a scanning pointAndthe slope and intercept of the line segment formed by connection; λ is the maximum distance threshold of a line segment formed by connecting the scanning point to the adjacent scanning point;

if the above three conditions are satisfied simultaneously, the judgment is made thatTo outlier, remove

Background

With the application of mobile robots in the fields of industry, civilian use And the like, Simultaneous Localization And Mapping (SLAM) technology has been developed greatly, And is a research hotspot And difficulty in the field of mobile robots. Positioning is a key link in SLAM, and the scanning matching method is used as the front end of the laser SLAM and is responsible for realizing data association and estimating the position and attitude change of the robot for subsequent positioning. The scanning matching method is the core foundation of the laser SLAM, and the pose estimation accuracy or not directly influences the mapping effect of the laser SLAM. Present scan matching methods can be classified into methods based on point matching, methods based on mathematical property matching, and methods based on feature matching. Compared with the methods based on points and mathematical characteristics, the feature-based matching method does not need initial values in the process of calculating the transformation parameters because the feature descriptors are used for matching calculation, can process continuous scanning point clouds with partial overlapping and large offset, and is widely used.

Currently, a laser Point cloud linear feature extraction algorithm represented by Iterative End Point Fit (IEPF) and Split-Merge algorithm (Split-Merge) uses a recursive method to separate line segments, so that efficiency is low, and sparsity of scanning Point clouds has a large influence on algorithm performance. Most of laser point cloud corner Feature extraction algorithms represented by a Congreue Transformation innovation Feature (CIF) and a bivariate normal probability density mapping method select corner Feature screening from point clouds obtained by a laser radar directly, are limited by the influence of laser radar noise, and introduce unnecessary errors in the subsequent pose Transformation parameter matching calculation process.

Disclosure of Invention

In order to overcome the defects of the prior art, the invention provides a mobile robot positioning method based on point-line feature scanning matching. The method is characterized in that the corresponding relation between two frames of laser point clouds is found by calculating the matching degree between the linear features and the angular point features of the two frames of laser point clouds before and after, so that the pose transformation relation of the mobile robot during the two frames of laser point clouds is calculated, the motion state of the robot is estimated more accurately, the positioning precision is improved, and the mapping effect of the laser SLAM is improved.

The technical scheme adopted by the invention for solving the technical problem comprises the following steps:

step 1: the mobile robot scans the surrounding environment through the carried laser radar sensor, acquires two-dimensional point cloud data from the laser radar, and performs coordinate conversion and filtering pretreatment:

step 1.1: the acquired two-dimensional point cloud data is represented asWhereinThe observed quantity of the ith laser beam of the laser radar at the time t is measured, d is the measured distance of the obstacle, and eta is the scanning resolution of the laser radar; coordinate conversion is carried out through a formula (1), and the obtained original point cloud data is converted from polar coordinates to rectangular coordinates to obtain

Step 1.2: carrying out filtering processing on the two-dimensional point cloud data: when the distance from one scanning point to two adjacent scanning points is greater than a threshold value gamma, calculating the distance from the scanning point to a line segment connected with the two adjacent scanning points, when the distance is greater than the threshold value lambda, judging the scanning point as an outlier, and removing the scanning point;

step 1.3: obtainingCarrying out coordinate conversion and filtering processing on a frame of point cloud data at the time t to obtain point cloud dataWhereinn is the number of scanning points contained in the frame point cloud data;

step 2: point-to-point cloud data P_tSequentially taking m scanning points from the scanning sequence to form alternative seed segments, performing Singular Value Decomposition (SVD) on each alternative seed segment to obtain n-m +1 fitting residual square sums, and performing sequencing and screening:

step 2.1: sequentially taking m scanning points from the ith scanning point to form alternative seed subsectionsPerforming Singular Value Decomposition (SVD) according to formula (2):

wherein u is_iIs a left singular matrix; s_iIs a matrix of singular values and is,and isv_iIn the form of a right singular matrix, is composed ofMean value of (i)

Step 2.2: when m scanning points are subjected to linear fitting, n-m +1 fitting residual square sums are obtainedSequencing in an ascending order; setting a threshold epsilon, discarding E_iAlternative seed segments > epsilon, remaining seed segmentsFor the region growing algorithm, where N is the number of seed segments;

and step 3: fitting a straight line to the Seed segments in the Seed, growing and extending to obtain straight line characteristics, and carrying out merging operation:

step 3.1: for seed sectionFitting a straight line by using a total least square method, and calculating a straight line equation y as a by formula (3)_ix+b_i：

Wherein a is_iAnd b_iRespectively representing the slope and intercept of the straight line,andis taken from

Step 3.2: the growth conditions for the region growing algorithm are set as: the distance from the nearest scanning point on either side of the seed section to the fitted straight line is less than a threshold value delta;

the termination conditions were set as: the distance from the nearest scanning points on the two sides of the seed section to the fitting straight line is larger than a threshold value delta, or the distance reaches the edge of the point cloud;

and (3) taking the growth condition and the termination condition as judgment conditions to carry out growth extension on the seed sections:

step 3.2.1: the seed section extends backwards;

at the seed section S_iGet the nearest neighbor of the back scan point in the neighborhood of (2)Calculating the scanning point to linear equation y ═ a_ix+b_iLinear distance D of_b：

If the condition D is satisfied_b<δ, then scanning the spotIncluding the seed section, expanding the seed section backwards; then, the current nearest rear side scanning point is calculatedTo the linear equation y ═ a_ix+b_iLinear distance D of_bIf the condition D is satisfied_b<Delta then willAn inclusion seed segment; the execution is repeated until the termination condition is met: the current nearest neighbor back side scan point to line equation y ═ a_ix+b_iThe linear distance of the seed segment is greater than a threshold value delta, or the scanning point is a point cloud edge point, and the seed segment stops extending backwards;

step 3.2.2: the seed section extends forwards;

at the seed section S_iGet the nearest neighbor of the back scan point in the neighborhood of (2)Calculating the scanning point to linear equation y ═ a_ix+b_iLinear distance D of_f：

If the condition D is satisfied_f<δ, then scanning the spotIncluding the seed section, expanding the seed section forwards; then, the current nearest front side scanning point is calculatedTo the linear equation y ═ a_ix+b_iLinear distance D of_fSatisfies the condition D_f<Delta then willAn inclusion seed segment; the execution is repeated until the termination condition is met: the current nearest neighbor anterior scan point to line equation y ═ a_ix+b_iThe linear distance of the seed segment is greater than a threshold value delta, or the scanning point is a point cloud edge point, and the seed segment stops extending forwards;

when both sides stop growing, a complete seed section is obtainedWherein s is_iAnd e_iRespectively represent the seed segments S_iHead and tail scanning point in P_tThe subscript of the sequence number (C); to S_iFitting the line by using a total least square method according to formula (3), and determining the slope a of the fitted line_iAnd intercept b_iUpdating is carried out;

step 3.3: determining end points on two sides of the straight line feature;

for seed segment S_iTaking the head and tail scanning pointsAndto a straight line y ═ a_ix+b_iMaking a perpendicular line intersecting the straight lineAndtwo points, thenAndthe end points of the straight line feature on the head and the tail are calculated by the formulas (6) and (7)Andthe coordinates of (a):

then by the seed section S_iThe obtained straight line features are expressed asWhereinIs the center of gravity of the straight line feature, which is also the center,l_iindicating the length of a straight line feature, i.e.Straight line characteristic L from origin of coordinate system_iAs an auxiliary vertical line, r_iAnd theta_iRespectively representing the length of the auxiliary vertical line under the current laser radar coordinate system and the horizontal axis_xThe included angle of the axes;

step 3.4: after the linear features of all the seed sections are extracted, reordering all the linear features according to the acquisition sequence of the scanning points contained in the seed sections, and then judging and combining:

when two straight line characteristics are same approximate straight line after present, supplementary plumb line between them also can approximate coincidence, judge two straight line characteristics through the geometric position of two supplementary plumb lines of comparison and whether can merge to avoid the too big condition that leads to the disconnection of straight line characteristic of individual scanning point noise, specifically as follows:

get L_iAnd L_i+1I ∈ 1,2, …, N-1, making a conditional judgment:

step 3.4.1: judging conditions: | r_i-r_i+1|<Δ r, where Δ r is the maximum length difference between the secondary perpendicular lines of the two straight line features, r_i+1Feature L representing straight line passing through origin of coordinate system_i+1The length of the auxiliary vertical line under the current laser radar coordinate system and the included angle between the auxiliary vertical line and the x axis of the transverse axis are made;

step 3.4.2: judging conditions: [ theta ]_i-θ_i+1|<Δ θ, where Δ θ is the maximum angular difference between the secondary perpendicular lines of the two straight line features, θ_i+1Feature L representing straight line passing through origin of coordinate system_i+1Making an included angle between an auxiliary vertical line and the x axis of the transverse axis under the current laser radar coordinate system;

when the two conditions are satisfied simultaneously, the straight line characteristic L is determined_iAnd L_i+1Are combined to form L_iAnd to L_iThe parameters are updated and obtained after the combination is finishedWherein N is₁Number of straight features after merging, N₁≤N；

And 4, step 4: extracting angular point features;

step 4.1: fromSequentially taking two adjacent straight line characteristics L_iAnd L_i+1Judging whether the conditions of the corner feature are met:

step 4.1.1: and (3) condition judgment: [ theta ]_i-θ_i+1|∈[90°-σ,90°+σ]Where σ is an uncertainty parameter for determining the corner;

step 4.1.2: and (3) condition judgment:wherein D_maxIs the farthest distance of the adjacent end points of the linear features forming the angular point features;

step 4.2, if the conditions of the step 4.1.1 and the step 4.1.2 are simultaneously met, the straight line characteristic L is considered_iAnd L_i+1Can extend and intersect to form an angular point feature C_jAnd calculating the coordinates of the corner feature by using the formula (8):

wherein a is_i+1And b_i+1Respectively represent the seed segments S_i+1Fitting the slope and intercept of the straight line using a total least squares method; then by the straight line feature L_iAnd L_i+1The formed corner feature is expressed asWhereinPosition coordinates of the angular point features;andis the angle between two right-angle sides of the angular point feature and the x axis of the cross axis under the current laser radar coordinate system, namelyJudging and calculating all linear features to obtain an angular point feature set of the frame point cloudWherein N is₂The number of extracted corner features;

and 5: reading the laser radar point cloud data at the time of t +1 in the motion process of the mobile robot, and performing the same processing operation from step 1 to step 4 to extract point-line features, thereby obtaining a linear feature set in the point cloud data at the time of t +1And corner feature setCalculating the matching degree according to the following steps to find the corresponding relation between two frames of point clouds; the matching degree is divided into a straight matching degree and an angular point matching degree;

step 5.1: calculating the matching degree of the straight lines;

setting a sliding window with a fixed size for linear features Line and Line' of front and back two frames of point clouds to determine a linear feature pair needing matching calculation;

for straight line feature L_ie.Line and L_j'epsilon' and the straight Line matching degree is as follows:

whereinAndrespectively a length matching degree, a position matching degree and an angle matching degree, and

wherein DL_maxThe maximum gravity center distance difference of two matched straight line features is obtained; delta theta_maxThe maximum included angle difference of two straight line characteristics which can be matched;

variables in the formulas in the step 5 and the step 6 with the top right corner mark' both represent the laser radar point cloud data at the time t +1, and the variable definitions are the same as those at the time t;

step 5.2: calculating the angular point matching degree;

determining the corner feature pair needing matching calculation by using the same sliding window for the corner features CP and CP' of the front and back frame point clouds, and taking C_iE.g. CP and C_j'belongs to CP', the corner matching degree of the two-corner feature is as follows:

whereinIn order to achieve a degree of positional matching,andangle matching of two right-angle sides for angular point featuresDegree of and

wherein DL_maxAnd Δ θ_maxThe parameters are the same as the parameters in the formulas (11) and (12), and are respectively the maximum coordinate distance difference and the maximum included angle difference of the two matched linear features;

step 6: estimating the motion parameters of the mobile robot by using the linear matching degree and the angular point matching degree obtained in the step 5, and calculating the pose transformation of the mobile robot between the front frame point cloud and the back frame point cloud;

the motion of the mobile robot is divided into translation and rotation, the translation parameter (x)_t,y_t) And a rotation parameter theta_tThe calculation process is expressed as:

wherein n is₁，n₂Respectively the number of the linear feature matching pairs and the angular point feature matching pairs with the matching degree not being 0;representing a linear feature matching pair (L)_i,L_j') the weight to be taken up in the calculation of the parameters, matching pairs of feature representing corner points (C)_i,C_j') the weight to be taken up in the calculation of the parameters,

the rotation matrix of the pose transformation of the mobile robot between the front frame point cloud and the back frame point cloud, namely from the time t to the time t +1 isThe translation matrix is

Preferably, the determination in step 1.2The steps for outliers are as follows:

step 1.2.1: judging conditions:wherein gamma is the maximum distance threshold from a scanning point to an adjacent scanning point;

step 1.2.2: judging conditions:

step 1.2.3: judging conditions:whereinAre respectively a scanning pointAndthe slope and intercept of the line segment formed by connection; λ is the maximum distance threshold of a line segment formed by connecting the scanning point to the adjacent scanning point;

if the above three conditions are satisfied simultaneously, the judgment is made thatTo outlier, remove

The invention has the following beneficial effects:

1. the invention uses the region growing algorithm to extract the linear characteristics, and uses the linear fitting residual square sum as the screening condition of the seed segment, thereby ensuring the accuracy of the seed segment;

2. in the growth process of the seed section, the seed section extends and expands towards two sides simultaneously, and is operated in an iteration mode, compared with a recursion cycle mode of the existing linear feature extraction algorithm, the operation speed is higher, and the operation efficiency of the scanning matching algorithm is improved;

3. the invention uses the total least square method to carry out straight line fitting, simultaneously considers the errors in the directions of the x axis and the y axis, and the common least square method only calculates the errors in the direction of the y axis, so the fitting precision is higher, and the obtained straight line characteristics are more accurate;

4. the corner features obtained by the method are virtual scanning points, and the influence of the noise of the laser radar sensor can be avoided.

5. The invention uses the combination characteristics of the linear characteristics and the angular point characteristics to carry out pose transformation calculation of the mobile robot, and compared with a method only extracting the linear characteristics or the angular point characteristics, the invention can obtain higher positioning precision and increase the robustness of the laser SLAM algorithm.

Drawings

FIG. 1 is a flow chart of the method of the present invention.

FIG. 2 is a diagram illustrating outlier rejection by filtering according to the present invention.

FIG. 3 is a schematic diagram of the region growing algorithm for extracting linear features.

FIG. 4 is a schematic diagram of the linear feature endpoint determination of the present invention.

FIG. 5 is a diagram illustrating various parameters of the linear feature of the present invention.

FIG. 6 is a schematic view of the combination of two linear features of the present invention.

Fig. 7 is a schematic diagram of corner feature extraction according to the present invention.

Fig. 8 is a schematic diagram of determining a linear feature matching pair by the sliding window method of the present invention.

Detailed Description

The invention is further illustrated with reference to the following figures and examples.

As shown in fig. 1, a mobile robot positioning method based on dotted line feature scan matching includes the following steps:

step 1: the mobile robot scans the surrounding environment through the carried laser radar sensor, acquires two-dimensional point cloud data from the laser radar, and performs coordinate conversion and filtering pretreatment:

step 1.1: the acquired two-dimensional point cloud data is represented asWhereinThe observed quantity of the ith laser beam of the laser radar at the time t is measured, d is the measured distance of the obstacle, and eta is the scanning resolution of the laser radar; the lidar model used in this example was a check LMS200 with a scanning frequency of 5Hz, a scanning range of 10m, an angular resolution of 1 ° and a scanning angle of 180 °. Converting the obtained original point cloud data from polar coordinates to rectangular coordinates by using the formula (1) to obtainWhere i ∈ 1,2, …, 180:

step 1.2: carrying out filtering processing on the two-dimensional point cloud data: when the distance from one scanning point to two adjacent scanning points is greater than a threshold value gamma, calculating the distance from the scanning point to a line segment formed by connecting the two adjacent scanning points to eliminate the condition that the three scanning points are positioned at approximate straight line positions, when the distance is greater than the threshold value lambda, judging the scanning point as an outlier, and removing the scanning point;

step 1.2.1: judging conditions:wherein gamma is the maximum distance threshold from a scanning point to an adjacent scanning point;

step 1.2.2: judging conditions:

step 1.2.3: judging conditions:whereinAre respectively a scanning pointAndthe slope and intercept of the line segment formed by connection; λ is the maximum distance threshold of a line segment formed by connecting the scanning point to the adjacent scanning point;

if the above three conditions are satisfied simultaneously, the judgment is made thatTo outlier, remove

Step 1.3: acquiring a frame of point cloud data at the time t, and performing coordinate conversion and filtering to obtain a point cloudData ofWhereinn is the number of scanning points contained in the frame point cloud data;

step 2: point-to-point cloud data P_tSequentially taking m scanning points from the scanning sequence to form alternative seed segments, performing Singular Value Decomposition (SVD) on each alternative seed segment to obtain n-m +1 fitting residual square sums, and performing sequencing and screening; the parameter m is the number of scanning points included in one subsection, and is determined according to the number of scanning points of one frame of point cloud of the laser radar, and if 180 scanning points are included in one frame of point cloud in the example, m is 5;

step 2.1: sequentially taking m scanning points from the ith scanning point to form alternative seed subsectionsPerforming Singular Value Decomposition (SVD) according to formula (2):

wherein u is_iIs a left singular matrix; s_iIs a matrix of singular values and is,and isv_iIn the form of a right singular matrix, is composed ofMean value of (i)

Step 2.2: when m scanning points are subjected to linear fitting, n-m +1 fitting residual square sums are obtainedSequencing in an ascending order; setting a threshold epsilon, discarding E_iAlternative seed segments > epsilon, remaining seed segmentsFor the region growing algorithm, where N is the number of seed segments;

and step 3: fitting a straight line to the Seed segments in the Seed, growing and extending to obtain straight line characteristics, and carrying out merging operation:

step 3.1: for seed sectionFitting a straight line by using a total least square method, and calculating a straight line equation y as a by formula (3)_ix+b_i：

Wherein a is_iAnd b_iRespectively representing the slope and intercept of the straight line,andis taken from

Step 3.2: the growth conditions for the region growing algorithm are set as: the distance from the nearest scanning point on either side of the seed section to the fitted straight line is less than a threshold value delta;

the termination conditions were set as: the distance from the nearest scanning points on the two sides of the seed section to the fitting straight line is larger than a threshold value delta, or the distance reaches the edge of the point cloud;

and (3) taking the growth condition and the termination condition as judgment conditions to carry out growth extension on the seed sections:

step 3.2.1: the seed section extends backwards;

at the seed section S_iGet the nearest neighbor of the back scan point in the neighborhood of (2)Calculating the scanning point to linear equation y ═ a_ix+b_iLinear distance D of_b：

If the condition D is satisfied_b<δ, then scanning the spotIncluding the seed section, expanding the seed section backwards; then, the current nearest rear side scanning point is calculatedTo the linear equation y ═ a_ix+b_iLinear distance D of_bIf the condition D is satisfied_b<Delta then willAn inclusion seed segment; the execution is repeated until the termination condition is met: the current nearest neighbor back side scan point to line equation y ═ a_ix+b_iThe linear distance of the seed segment is greater than a threshold value delta, or the scanning point is a point cloud edge point, and the seed segment stops extending backwards;

step 3.2.2: the seed section extends forwards;

at the seed section S_iGet the nearest neighbor of the back scan point in the neighborhood of (2)Calculating the scanning point to linear equation y ═ a_ix+b_iLinear distance D of_f：

If the condition D is satisfied_f<δ, then scanning the spotIncluding the seed section, expanding the seed section forwards; then, the current nearest front side scanning point is calculatedTo the linear equation y ═ a_ix+b_iLinear distance D of_fSatisfies the condition D_f<Delta then willAn inclusion seed segment; the execution is repeated until the termination condition is met: the current nearest neighbor anterior scan point to line equation y ═ a_ix+b_iThe linear distance of the seed segment is greater than a threshold value delta, or the scanning point is a point cloud edge point, and the seed segment stops extending forwards;

when both sides stop growing, a complete seed section is obtainedWherein s is_iAnd e_iRespectively represent the seed segments S_iHead and tail scanning point in P_tThe subscript of the sequence number (C); to S_iFitting the line by using a total least square method according to formula (3), and determining the slope a of the fitted line_iAnd intercept b_iUpdating is carried out;

step 3.3: determining end points on two sides of the straight line feature;

for seed segment S_iTaking the head and tail scanning pointsAndto a straight line y ═ a_ix+b_iMaking a perpendicular line intersecting the straight lineAndtwo points, thenAndthe end points of the straight line feature on the head and the tail are calculated by the formulas (6) and (7)Andthe coordinates of (a):

then by the seed section S_iThe obtained straight line features are expressed asWhereinIs the center of gravity of the straight line feature, which is also the center,l_iindicating the length of a straight line feature, i.e.Straight line characteristic L from origin of coordinate system_iAs an auxiliary vertical line, r_iAnd theta_iRespectively representing the length of the auxiliary vertical line under the current laser radar coordinate system and an included angle between the auxiliary vertical line and the x axis of the transverse axis;

step 3.4: after the linear features of all the seed sections are extracted, reordering all the linear features according to the acquisition sequence of the scanning points contained in the seed sections, and then judging and combining:

when two straight line characteristics are same approximate straight line after present, supplementary plumb line between them also can approximate coincidence, judge two straight line characteristics through the geometric position of two supplementary plumb lines of comparison and whether can merge to avoid the too big condition that leads to the disconnection of straight line characteristic of individual scanning point noise, specifically as follows:

get L_iAnd L_i+1I ∈ 1,2, …, N-1, making a conditional judgment:

step 3.4.1: judging conditions: | r_i-r_i+1|<Δ r, where Δ r is the maximum length difference between the secondary perpendicular lines of the two straight line features, r_i+1Feature L representing straight line passing through origin of coordinate system_i+1The length of the auxiliary vertical line under the current laser radar coordinate system and the transverse axis_xThe included angle of the axes;

step 3.4.2: judging conditions: [ theta ]_i-θ_i+1|<Δ θ, where Δ θ is the maximum angular difference between the secondary perpendicular lines of the two straight line features, θ_i+1Feature L representing straight line passing through origin of coordinate system_i+1Making an included angle between an auxiliary vertical line and the x axis of the transverse axis under the current laser radar coordinate system;

when the two conditions are satisfied simultaneously, the straight line characteristic L is determined_iAnd L_i+1Are combined to form L_iAnd to L_iThe parameters are updated and obtained after the combination is finishedWherein N is₁Number of straight features after merging, N₁≤N；

And 4, step 4: extracting angular point features;

step 4.1: fromSequentially taking two adjacent straight line characteristics L_iAnd L_i+1Judging whether the conditions of the corner feature are met:

step 4.1.1: and (3) condition judgment: [ theta ]_i-θ_i+1|∈[90°-σ,90°+σ]Where σ is an uncertainty parameter for determining the corner;

step 4.1.2: and (3) condition judgment:wherein D_maxIs the farthest distance of the adjacent end points of the linear features forming the angular point features;

step 4.2, if the conditions of the step 4.1.1 and the step 4.1.2 are simultaneously met, the straight line characteristic L is considered_iAnd L_i+1Can extend and intersect to form an angular point feature C_jAnd calculating the coordinates of the corner feature by using the formula (8):

wherein a is_i+1And b_i+1Respectively represent the seed segments S_i+1Fitting the slope and intercept of the straight line using a total least squares method; then by the straight line feature L_iAnd L_i+1The formed corner feature is expressed asWhereinPosition coordinates of the angular point features;andtwo right-angle sides which are characterized by angular points are connected with a transverse shaft under the current laser radar coordinate system_xAngle of axes, i.e.Judging and calculating all linear features to obtain an angular point feature set of the frame point cloudWherein N is₂The number of extracted corner features;

and 5: reading the laser radar point cloud data at the time of t +1 in the motion process of the mobile robot, and performing the same processing operation from step 1 to step 4 to extract point-line features, thereby obtaining a linear feature set in the point cloud data at the time of t +1And corner feature setCalculating the matching degree according to the following steps to find the corresponding relation between two frames of point clouds; the matching degree is divided into a straight matching degree and an angular point matching degree;

step 5.1: calculating the matching degree of the straight lines;

setting a sliding window with a fixed size for the linear features Line and Line' of the front and back frame point clouds to determine the linear feature pair needing matching calculation, and if the window size is 5, setting the linear feature L₇The straight line feature in the next frame of point cloud data to be matched is L₅'、L₆'、L₇'、L₈' and L₉'；

For straight line feature L_ie.Line and L_j'epsilon' and the straight Line matching degree is as follows:

whereinAndrespectively a length matching degree, a position matching degree and an angle matching degree, and

wherein DL_maxThe maximum gravity center distance difference of two matched straight line features is obtained; delta theta_maxThe maximum included angle difference of two straight line characteristics which can be matched;

variables in the formulas in the step 5 and the step 6 with the top right corner mark' both represent the laser radar point cloud data at the time t +1, and the variable definitions are the same as those at the time t;

step 5.2: calculating the angular point matching degree;

determining the corner feature pair needing matching calculation by using the same sliding window for the corner features CP and CP' of the front and back frame point clouds, and taking C_iE.g. CP and C_j'belongs to CP', the corner matching degree of the two-corner feature is as follows:

whereinIn order to achieve a degree of positional matching,andis the angle matching degree of two right-angle sides of the angular point characteristics, and

wherein DL_maxAnd Δ θ_maxThe parameters are the same as the parameters in the formulas (11) and (12), and are respectively the maximum coordinate distance difference and the maximum included angle difference of the two matched linear features;

degree of linear matching ML_ijDegree of matching with angular point MC_ijRespectively reflect two straight line characteristics L_iAnd L_j', and two-corner feature C_iAnd C_j' degree of matching of distribution in geometric space. By definition, ML_ij,MC_ij∈[0,1]And ML_ijThe larger the straight line is, the higher the matching degree is, the L is shown_iAnd L_j' the greater the probability of corresponding to the same object in the environment, while the angular feature is formed by the extension and intersection of straight line features, and does not necessarily correspond to the actual object in the environment, MC_ijThe larger the line characteristic is, the higher the accuracy of the corresponding relation of the line characteristic is reflected from the side surface;

step 6: estimating the motion parameters of the mobile robot by using the linear matching degree and the angular point matching degree obtained in the step 5, and calculating the pose transformation of the mobile robot between the front frame point cloud and the back frame point cloud;

the motion of the mobile robot is divided into translation and rotation, and translationShift parameter (x)_t,y_t) And a rotation parameter theta_tThe calculation process is expressed as:

wherein n is₁，n₂Respectively the number of the linear feature matching pairs and the angular point feature matching pairs with the matching degree not being 0;representing a linear feature matching pair (L)_i,L_j') the weight to be taken up in the calculation of the parameters, matching pairs of feature representing corner points (C)_i,C_j') the weight to be taken up in the calculation of the parameters,

translation parameter (x)_t,y_t) And a rotation parameter theta_tThe calculation can be divided into a front part and a rear part, wherein the front half part is obtained by weighting and summing the position or angle parameters according to the self linear matching degree of all linear matching pairs, and the rear half part is obtained by weighting and summing the position or angle parameters according to the self angular point matching degree of all angular point matching pairs. The two parts are summed in proportion according to the quantity relation of the linear characteristic matching pairs and the angular point characteristic matching pairs to obtain translation parameters (x)_t,y_t) And a rotation parameter theta_tThen the rotation matrix of the pose transformation of the mobile robot between the front and back two frames of point clouds, namely from the time t to the time t +1 isThe translation matrix is