CN100449444C - A Method for Simultaneous Localization and Map Construction of Mobile Robots in Unknown Environments - Google Patents

Abstract

The invention discloses a method for simultaneous positioning and map construction of a mobile robot in an unknown environment. The method includes the following steps: 1) Construct the currently observed local occupancy grid map and local line feature map according to the data obtained by the ranging sensor installed on the mobile robot; 2) According to the ranging sensor data, the dead reckoning sensor data, the upper Estimating the pose of the mobile robot in one cycle and the global line segment feature map constructed in the previous stage, and estimating the pose of the mobile robot in the current cycle; 3) According to the pose estimation and construction of the mobile robot in the current cycle Occupancy grid map, update the global occupancy grid map and the global line segment feature map. The line segment feature map and occupied grid map constructed by the invention can be used by various application robots for path planning and navigation, eliminating the need to manually input the environment map for the mobile robot, and effectively improving the cognition ability of various robots to unknown environments.

Description

A Method for Simultaneous Localization and Map Construction of Mobile Robots in Unknown Environments

technical field

The present invention relates to a method and system for simultaneous positioning and map construction of a mobile robot in an unknown environment, which can be used for a mobile robot to start at any point in an unknown environment, and estimate the pose of the robot based on the perception data of a ranging sensor and a dead reckoning sensor and build a map of the environment.

Background technique

With the gradual expansion of the application range of mobile robots, how to make robots autonomously recognize unknown environments has become a research hotspot in the field of robotics and artificial intelligence in recent years. One of the ways for the robot to autonomously recognize the unknown environment is to build a model or map of the unknown environment, that is, the robot autonomously builds a spatial model or map of its environment based on sensor measurement information (see "S. Thrun. Robotic mapping: Asurvey. In G .Lakemeyer and B.Nebel, editors, Exploring Artificial Intelligence in the New Millenium. Morgan Kaufmann, 2002." and "Chen Weidong, Zhang Fei. Synchronous self-localization and map creation research progress of mobile robots. Control Theory and Applications, 22(3) : 455-460, 2005."). By constructing the obtained environmental map, the robot can perform task planning, path planning, and perform various tasks.

The early construction of unknown environment maps assumed that all poses during the movement of the robot could be accurately obtained by dead reckoning sensors, and thus proposed methods for constructing various environmental maps based on ranging sensor information, including building occupancy maps based on Boolean Bayesian filtering. The method of raster map (see "A.Elfes.Sonar-basedreal-world mapping and navigation.IEEE Journal of Robotics and Automation, RA-3(3):249-265, 1987.") and constructing features based on Kalman filter Mapping methods (see "K.S. Chong, L. Kleeman. Mobile-robot map building from an advanced sonar array and accurate odometry. International Journal of Robotics Research, 18(1):20-36, 1999.").

However, the pose information of the robot obtained by the dead reckoning sensor is not deterministic, on the contrary, it has a statistically related measurement error, that is, the error will accumulate over time. However, traditional robot pose estimation, such as Markov localization method and Monte Carlo localization method, always assumes that the map is known (see "D. Fox, W. Burgard, and S. Thrun. Markov localization for mobile robots in dynamic environments. Journal of Artificial Intelligence Research, 11:391-427, 1999.” and “S. Thrun, D. Fox, W. Burgard and F. Dellaert. Robust Monte Carlo localization for mobile robots. Artificial Intelligence, 101: 99-141, 2000.” ).

It can be seen that in the construction of the unknown environment map, due to the unavoidable measurement noise in the sensor measurement data used by the robot to perceive the external environment information and its own movement information, there are uncertainties in both the robot positioning and the construction of the environmental map, and These uncertainties are interrelated and affect each other. When the currently observed information is added to the constructed map according to the robot pose, the uncertainty of robot positioning will be brought into the constructed map, which will make the constructed map deviate; based on the current observation and When the matching relationship of the constructed map estimates the pose of the robot, the uncertainty in the current observation and the constructed map will make the robot positioning bias. Therefore, the unknown environment map construction must solve the problem of robot positioning and map construction at the same time.

Existing simultaneous localization and map construction methods include extended Kalman filter method, expected value maximization method, and particle filter method. Simultaneous localization and map building method based on extended Kalman filtering (see "J.A. Castellanos, et al. The SPmap: A probabilistic framework for simultaneous localization and map building. IEEE Transactions on Robotics and Automation, 15(5): 948-953, 1999 .”) The map consists of a series of landmarks, and the algorithm maintains the robot pose and the posterior probabilities of all landmark locations. This type of method can realize online map construction, but due to the assumption of Gaussian noise, it requires that the landmarks can be fully distinguished from each other, and the lack of landmarks, landmark recognition errors and matching errors will lead to the failure of the algorithm. Simultaneous positioning and map construction method based on expected value maximization (see "W. Burgard, et al. Sonar-based mapping of large-scale mobile robotenvironments using EM. in Proc. of the International Conference on Machine Learning, Slovenia, 1999, pp. 67-76.") to gradually improve the accuracy of the map by repeating two steps, one is to calculate the posterior probability of the robot pose according to the given map, and the other is to calculate the most likely map according to the expected value of the pose . This method can avoid map construction failures due to landmark recognition errors and matching errors, but it cannot achieve online map construction because it performs hill-climbing in all map spaces. Simultaneous localization and map construction method based on particle filter (see "M.Montemerlo and S.Thrun.Simultaneous localization and mapping with unknown data association using FastSLAM.in Proceedings of the 2003 IEEE International Conference on Robotics & Automation, Taipei, Taiunn, 2003, pp.1985 -1991.") is a combination of particle filter and extended Kalman filter, which uses particle filter to estimate the pose of the robot, each particle represents a possible robot path, and maintains a map determined by the path, in the map The position of the landmarks is estimated using the extended Kalman filter method. This method solves the problem of mutual influence between localization and map construction uncertainty through multiple assumptions, but maintains a corresponding map for each particle, which consumes a large amount of storage space and computing resources.

Chinese Invention Patent No. 00816243.3 proposes a multi-robot collaboration system, in which one or more navigation robots complete the drawing of the environment, self-positioning, functional robot positioning and functional robot task assignment, in which the navigation robot uses landmark recognition when positioning itself and calculation algorithms. Errors in the reckoning algorithm accumulate over time due to factors such as wheel slippage or misalignment. To compensate for this error, the method incorporates landmark recognition when positioning. However, since it uses the functional robot in the system as a landmark, this method is not suitable for a single robot working in an unknown environment, especially when there is no landmark in the current observation, the robot positioning will have great uncertainty .

The environment recognition equipment and method proposed in Chinese Invention Patent No. 200410100518.9 are mainly for multi-plane environments (ie stairs), and do not consider the uncertainty of robot positioning.

Therefore, it is necessary to develop a robot that can not only realize accurate robot pose estimation and map construction, but also run online in real time, require less storage space and computing resources, and the constructed map can provide detailed environmental information, which is suitable for robots. Simultaneous localization and map construction methods for navigation and other tasks.

Contents of the invention

The purpose of the present invention is to provide a method for simultaneous localization and map construction of a mobile robot in an unknown environment.

The method for simultaneous localization and map construction of a mobile robot in an unknown environment comprises the following steps:

1) According to the data obtained by the ranging sensor installed on the mobile robot, construct the currently observed local occupancy grid map and local line segment feature map;

2) Estimate the pose of the mobile robot in the current cycle based on the ranging sensor data, the dead reckoning sensor data, the pose estimation of the mobile robot in the previous cycle, and the global line segment feature map constructed in the previous stage;

3) Update the global occupancy grid map and the global line segment feature map based on the estimated and constructed local line segment feature map and local occupancy grid map of the current cycle mobile robot.

The data obtained by the ranging sensor is the distance and angle of each point on the obstacle in the environment relative to the mobile robot obtained by scanning the ranging sensor on the ranging height plane.

The local occupancy grid map and the local line segment feature map adopt a coordinate system with the current cycle mobile robot position as the origin and the current cycle mobile robot positive direction as the X axis.

The global occupancy grid map and the global line segment feature map adopt a coordinate system with the position of the mobile robot in the first period as the origin and the positive direction of the mobile robot in the first period as the X-axis.

According to the data obtained by the ranging sensor installed on the mobile robot, construct the currently observed local occupancy grid map and local line segment feature map: it includes the construction of the local occupancy grid map and the construction of the local line segment feature map, where the local occupancy grid The grid map construction adopts the Boolean Bayesian filtering method, and the steps of local line segment feature map construction are as follows:

1) For the ranging sensor data, the Hough transform straight line fitting method is used to obtain a preliminary fitting line segment;

2) Carry out isotropic judgment, collinearity judgment and coincidence judgment on the preliminary fitting line segment, and merge the fitting point set of the line segment coincident with the collinear line roughly in the same direction;

3) For the merged line segment fitting point set, use the least squares fitting method to obtain an accurate fitting line segment;

According to the ranging sensor data, the dead reckoning sensor data, the pose estimation of the mobile robot in the previous cycle and the global line segment feature map constructed in the previous period, the steps to estimate the pose of the mobile robot in the current cycle are as follows:

1) According to the dead reckoning sensor data and the pose estimation of the mobile robot in the previous cycle, use the motion model of the mobile robot to estimate the pose of the mobile robot in the current cycle;

2) According to the estimated pose, extract the features that fall within the scanning range of the ranging sensor from the global line segment feature map, and obtain the estimated currently visible line segment features;

3) Transform the currently visible line segment features from the global map coordinate system to the local map coordinate system;

4) Calculate the pose correction offset by finding the best match between the ranging sensor data and the expected current visible line segment features;

5) According to the estimated pose and pose correction offset of the mobile robot in the current period, calculate the pose estimation of the mobile robot in the current period.

According to the dead reckoning sensor data and the pose estimation of the mobile robot in the previous cycle, the motion model of the mobile robot is used to estimate the pose of the mobile robot in the current cycle: including the following steps,

1) According to the dead reckoning sensor data of the current cycle and the dead reckoning sensor data of the previous cycle, calculate the approximate moving speed of the mobile robot during this period;

2) According to the calculated approximate moving speed of the mobile robot and the estimated pose of the mobile robot in the previous cycle, calculate the estimated pose of the mobile robot in the current cycle.

Calculate the pose correction offset by finding the best match between the ranging sensor data and the expected current visible line segment features: including the following steps,

1) Use the matching method with the shortest feature distance from the point to the line segment to find the matching relationship between the ranging sensor data and the expected current visible line segment feature;

2) Start with the data point relationship matching the same line segment feature, judge the above matching relationship, and delete the inappropriate match;

3) Proportional quantification is defined from five aspects: the generation time of the matching line segment feature, the reliability of the matching line segment feature, whether the matching line segment is a corner composition line segment, the length of the matching line segment, and whether the matching data is a fitting point of a local line segment feature matching weight;

4) According to the condition that the best match is that the characteristic distance from the data point to the matching line segment is zero, the parameter model structure of the best match of the matching part is constructed, and the weighted least squares parameter estimation method is used to correct the offset;

5) Translate and rotate the ranging sensor data according to the pose correction offset;

6) Steps 1)-5) are repeated until the residual error of weighted least squares parameter estimation is less than a certain value or the convergence condition is stable.

Update the global occupancy grid map and the global line segment feature map based on the pose estimation of the current cycle mobile robot, the constructed local line segment feature map and the local occupancy grid map: including the global occupancy grid map update and the global line segment feature map update , where the global occupancy grid map is updated using the Boolean Bayesian filtering method, and the global line segment feature map update includes the following steps:

1) For the line segment feature in the local line segment feature map, find whether there is a global line segment feature and the local line segment feature in the global line segment feature map to meet the requirements of isotropy, collinearity and coincidence;

2) If it exists, merge the line segment fitting points, use the least squares straight line fitting method to re-fit the line segment, and replace the original global line segment feature;

3) If it does not exist, add the local line feature to the global line feature map.

4) Using the global occupancy grid map to calculate the credibility of the line feature in the global line feature map.

The method for simultaneous positioning and map construction of a mobile robot in an unknown environment proposed by the present invention solves the problem that the uncertainty of the pose of the robot and the uncertainty of map construction in the construction of the map of the unknown environment are interrelated and affect each other, and the storage space occupied And with less computing resources, online unknown environment map construction can be realized. The constructed global line segment feature map and global occupancy grid map are suitable for path planning and navigation of various application robots.

Description of drawings

Figure 1 is a software flow chart for implementing simultaneous localization and map construction of mobile robots in an unknown environment;

Fig. 2 is a cone diagram constructed for each obstacle point data in the local occupancy grid map construction;

Fig. 3 is the operation flowchart of estimating the pose of the mobile robot in the current cycle in the method of the present invention;

Fig. 4 is the operation flowchart of calculating pose correction offset in the method of the present invention;

Fig. 5 is an example of actually drawing a local occupancy grid map and a local line segment feature map by utilizing the local occupancy grid map construction method and the local line segment feature map construction method in the method of the present invention;

Figure 5(a) is an example of the obstacle point data obtained by the ranging sensor;

Fig. 5 (b) is a local occupancy grid map obtained by constructing the data instance in Fig. 5 (a) by using the local occupancy grid map construction method in the method of the present invention, wherein black represents the occupied area, and white represents Free area, gray indicates uncertain area;

Fig. 5 (c) utilizes the local line segment feature map construction method in the method of the present invention to construct a local line segment feature map obtained from the data instance in Fig. 5 (a);

Fig. 6 is another example of actually drawing a local occupancy grid map and a local line segment feature map by using the local occupancy grid map construction method and the local line segment feature map construction method in the method of the present invention;

Fig. 6 (a) is another example of the obstacle point data obtained by the ranging sensor;

Fig. 6 (b) is a local occupancy grid map obtained by constructing the data instance in Fig. 6 (a) by using the local occupancy grid map construction method in the method of the present invention, wherein black represents the occupied area, and white represents Free area, gray indicates uncertain area;

Fig. 6 (c) utilizes the local line segment feature map construction method in the method of the present invention to construct a local line segment feature map obtained from the data instance in Fig. 6 (a);

Fig. 7 is an example of calculating the current cycle mobile robot pose in the method of the present invention;

Figure 7(a) is an example of the matching situation between the obstacle point data obtained by the current period ranging sensor and the estimated current visible line segment features obtained according to the current period dead reckoning sensor data, where the black point is the current period ranging sensor Observed obstacle data points, the black line is the expected current visible line segment feature obtained according to the current cycle dead reckoning sensor data;

Figure 7(b) is an example of the matching situation of the obstacle point data acquired by the ranging sensor in the current period and the estimated current visible line segment features obtained by the estimated pose of the robot in the current period calculated according to the method of the present invention, wherein the black dots is the obstacle data point observed by the current period ranging sensor, and the black line is the estimated current visible line segment feature obtained by the estimated pose of the robot in the current period calculated according to the method of the present invention;

Figure 7(c) is an example of the matching situation between the obstacle point data acquired by the current ranging sensor and the estimated current visible line segment features obtained by the current period robot pose calculated according to the method of the present invention, wherein the black point is the current period The obstacle data points observed by the ranging sensor, the black line is the estimated current visible line segment feature obtained by the current period robot pose calculated according to the method of the present invention;

Fig. 8 is a global occupancy grid map and a global line segment feature map example drawn by the method of the present invention;

Fig. 8(a) is an example of a global occupancy grid map drawn by the method of the present invention for a room with a length of 16.4m and a width of 7.9m. Wherein the gray line is the connection line of the robot position obtained by the dead reckoning sensor, and the black line is the connection line of the robot position calculated by the method of the present invention;

Fig. 8(b) is an example of a global line segment feature map drawn by using the method of the present invention for the room described in Fig. 8(a).

Detailed ways

An implementation example of simultaneous localization and map construction of a robot in an unknown environment according to the present invention will be described in detail below with reference to the accompanying drawings. In this implementation example, the robot can move independently, and the distance measuring sensor equipped is a laser range finder, or a sonar range finder, or a stereo vision system, or a combination thereof, and the dead reckoning sensor equipped is an odometer .

Fig. 1 is the software flowchart of implementing the method of the present invention. The mobile robot starts from any position in the unknown environment and in any direction. First, collect sensor data (step S1), construct a local occupancy grid map and a local line segment feature map (step S2), and convert the local occupancy grid map data and local line segment feature map data into global map coordinates through coordinate system conversion system, thereby obtaining an initial global occupancy grid map and a global line segment feature map (step S3). Then the robot starts to move (step S4), and stops at intervals for a certain period of time to collect sensor data (step S5), complete simultaneous positioning and map construction (step S6), and then continue to move, so that the cycle repeats, when the robot no longer moves, exit the loop. The simultaneous localization and map construction include steps: construction of local occupancy grid map and local line segment feature map; robot pose estimation and correction; update of global occupancy grid map and global line segment feature map.

In an implementation example, the ranging sensor obtains a set of distances and angles of each point on the obstacle in the environment on the ranging height plane relative to the robot. The local map coordinate system is constructed with the current mobile robot position as the origin and the positive direction of the current mobile robot as the X axis, then the detected obstacle point data is the number pair (r _i , α _i ) in the local map coordinate system , i=1,...,n, n represents the number of points, r _i represents the distance from the point to the origin of the coordinate system, α _i represents the angle between the line from the origin of the coordinate system to the point and the X axis. The Cartesian coordinates of each obstacle point are (x _i , y _i ), x _i = r _i cos α _i , y _i = r _i sin α _i .

Local occupancy grid maps are constructed using Boolean Bayesian filtering. In this implementation example, for each obstacle point data (r _i , α _i ), first construct a cone-shaped area as shown in Figure 2, where d ₁ =r _i -d, d ₂ =r _i +d, θ It is 1/2 of the angle between the adjacent obstacle point and the coordinate origin. For this obstacle point, the calculation method of the occupation probability of the grid in area 1 is

f=1-E _r E _α , (1)

Where E _r =1-k _r ·(l/d ₁ ) ² , E _α =1-k _α ·(β/θ) ² , k _r , k _α is a constant coefficient, l is the Distance, β is the angle between the line connecting the grid to the apex of the cone and the center line of the cone. The calculation method of the occupancy probability of the grid in area 2 is

f=O _r O _α , (2)

where O _r ＝1-k _r ·((lr _i )/d) ² , O _α =1-k _α ·(β/θ) ² . Using coordinate transformation, it is determined that each grid in the cone-shaped area occupies the local grid The corresponding position in the grid map, using the formula

$\mathrm{<span\; class="notranslate">\; log</span>}\frac{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; log</span>}\frac{{\mathrm{<span\; class="notranslate">\; p</span>}}^{<span\; class="notranslate">\; \&prime;</span>}}{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; p</span>}}^{<span\; class="notranslate">\; \&prime;</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; log</span>}\frac{\mathrm{<span\; class="notranslate">\; f</span>}}{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; f</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

Calculate the occupancy probability of the corresponding grid in the local occupancy grid map, where p' is the original occupancy probability of the grid, and p is the obtained updated occupancy probability.

In this implementation example, each line segment feature is described by six parameters [c, θ, l, P, gt, conf, FLine], denoted as L. Among them, c and θ are the parameters of the straight line where the line segment is located, and the equation of the straight line is

xcosθ+ysinθ+c＝0. (4)

c is the distance from the origin to the straight line, and θ is the angle between the normal of the straight line and the x-axis of the coordinate system. l is the length of the line segment, P=(x _c , y _c ) ^T is the position of the center point of the line segment (T means transpose). gt is a timestamp attribute, which records the generation time of the line segment. conf is the credibility attribute of the line segment. FLine is a Boolean quantity, used to indicate whether the line segment is a line segment that constitutes a corner, if it is, it is true, otherwise it is false.

In the construction of the local line segment feature map in this implementation example, firstly, the Hough transform straight line fitting method is applied to the obstacle point data set obtained by the ranging sensor to preliminarily determine which line segment those point data belong to. Specifically, a two-dimensional voting grid is constructed according to a certain degree of dispersion, in which one dimension is the discrete value of the line parameter c, and the other is the discrete value of the line parameter θ. For each obstacle point (x _i , y _i ), a set of straight lines with parameters (c _j , θ _j ) pass through the point, thereby voting for the corresponding grid. Through vote counting, it is possible to determine which line segment each obstacle point belongs to and the parameter description of the line segment.

However, due to measurement errors, the obstacle points measured by the ranging sensor are not exactly collinear, and it is difficult to choose an appropriate discrete grid size in the Hough change. For a group of points that should be collinear, it is often generated More than one line segment to fit. Therefore, in the construction of the local line segment feature map of the implementation example, after the Hough transform straight line fitting, pairwise collinearity judgments are performed on the line segments in the obtained line segment set. When the line segment L ₁ and the line segment L ₂ satisfy the following synlinearity judgment rules, the two line segments are considered to be co-linear, and the point sets on the line segments are merged.

1) Isotropy, requiring |θ ₁ -θ ₂ |≤Δθ, where Δθ is the threshold;

2) Collinearity requires ‖c ₁ |-|c ₂ ‖≤ΔC, $d=|x_{{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="C20061005369000221.png"\; state="\{\{state\}\}">c</figure-callout>}}_1}\cos {\mathrm{\&theta;}}_2+{\mathrm{the\; <figure-callout\; id="1"\; label="y\; c"\; filenames="C20061005369000221.png"\; state="\{\{state\}\}">y</figure-callout>}}_{c_{}}\sin {\mathrm{\&theta;}}_2+c_2|\&le;\mathrm{\&Delta;D}$ , where ΔC, ΔD are threshold values. That is, the distance difference from the origin to the straight line where the line segment is located is less than the threshold ΔC, and at the same time, the vertical distance from the center point of one line segment to another line segment is less than the threshold ΔD.

3) Reconciliation, requirements ${(x_{c_1}-x_{c_2})}^2+{({\mathrm{the\; y}}_{c_1}-{\mathrm{the\; y}}_{c_2})}^2\&le;{({\mathrm{<figure-callout\; id="1"\; label="l"\; filenames="C20061005369000221.png"\; state="\{\{state\}\}">l</figure-callout>}}_1/2+l_2/2)}^2$ , that is, the Euclidean distance between the center points of the line segments is less than or equal to the sum of the half-fields of the line segments.

After merging roughly coincident line segment point sets through synlinearity judgment, the least squares straight line fitting method is used to obtain more accurate line segment parameters c, θ, l and P. The parameter gt is the current time, and the parameter conf=1. FLine is set to true when the angle between a line segment and another line segment is around 90 degrees and the intersection point is on the line segment.

In this implementation example, based on the ranging sensor data, dead reckoning sensor data, the pose estimation of the mobile robot in the previous period, the global line segment feature map and the global occupancy grid map obtained in the previous period, the mobile robot’s position in the current period is estimated. pose, the steps are shown in Figure 3. In this implementation example, the global map coordinate system is constructed with the initial pose of the mobile robot as the origin and the initial direction of the mobile robot as the X-axis. Both dead reckoning sensor data and mobile robot pose estimation are data in the global map coordinate system. Therefore, the initial pose of the robot in the global occupancy grid map and the global line segment feature map is (0, 0, 0).

In step S61, according to the dead reckoning sensor data and the pose estimation of the mobile robot in the last cycle, the pose of the mobile robot in the current cycle is estimated by using the motion model of the mobile robot. Record the current period as t, and the reading of the dead reckoning sensor in the t-1 period is $o_{t-1}={(x_{o_{t-1}},{\mathrm{the\; y}}_{o_{t-1}},z_{o_{t-1}})}^T$ , the t-period dead reckoning sensor reading is $o_t={(x_{o_t},{\mathrm{the\; y}}_{o_t},z_{o_t})}^T,$ The pose estimation of the t-1 period mobile robot is X _t-1 =(x _t-1 , y _t-1 , z _t-1 ) ^T . It can be assumed that the motion of the robot during the t-1 period to the t period is approximately a uniform motion, and its motion speed is D=(dx, dy, dθ) ^T , where dx, dy are the linear velocity in the x and y directions respectively, and dθ is the angular velocity , its calculation formula is as follows:

$\left\{\begin{array}{c}\mathrm{<span\; class="notranslate">\; dx</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\\ \mathrm{<span\; class="notranslate">\; dy</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; .</span>\\ \mathrm{<span\; class="notranslate">\; d\&theta;</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; o</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

According to the moving speed and the pose estimation of the t-1 cycle mobile robot, the estimated pose of the t cycle mobile robot can be obtained $x_{t_0}={(x_{t_0},{\mathrm{the\; y}}_{t_0},z_{t_0})}^T,$ Its calculation formula is as follows:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; dx</span>}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; dy</span>}\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; dx</span>}\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; dy</span>}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; .</span>\\ {\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; d\&theta;</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

In step S62, according to the estimated pose $x_{t_0}={(x_{t_0},{\mathrm{the\; y}}_{t_0},z_{t_0})}^T$ The line segment features that fall within the scanning range of the ranging sensor are taken out from the global line segment feature map, and the currently visible line segment features are obtained. In this implementation example, the line segment features whose endpoints fall within the measurement range of the range finder are the currently visible line segment features, and all the currently visible line segment features form a set Γ.

In this implementation example, the local map coordinate system is selected as the matching coordinate system, so in step S63, the currently visible line segment features are converted from the global map coordinate system to the local map coordinate system, that is, the line segment features in the set Γ are converted to in the local map coordinate system. Note that the line segment in the set Γ is L _W : {c _W , θ _W , l _W , P _W , gt _W , conf _W }, and the line segment transformed into the local map coordinate system is L _R : {c _R , θ _R , l _R , P _R , gt _R , conf _R }, the conversion equation is

$\left\{\begin{array}{c}\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& \mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}& \mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\end{array}\right]\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; W</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; W</span>}}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\end{array}\right]\\ {\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; W</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}\mathrm{<span\; class="notranslate">\; 0</span>}}\\ {\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}}\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; )</span>\\ {\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; W</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; gt</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; gt</span>}}_{\mathrm{<span\; class="notranslate">\; W</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; con</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; con</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; W</span>}}\end{array}\right.$

The converted line segment features constitute a set Γ′.

In step S64, the pose correction offset is calculated by finding the best match between the ranging sensor data and the predicted current visible line segment features. Figure 4 shows the implementation steps.

In step S641, for each obstacle point data ( _xi , _yi ) measured by the ranging sensor, the feature of the line segment with the closest distance is searched in Γ'. In this implementation example, the distance between the obstacle data point and the nearest line segment feature is required to be less than a certain threshold, and if the condition is satisfied, the line segment feature is called the matching line segment of the data point, and the data point is called the matching point. All matching points form a set V.

The matching method in step S641 is likely to cause improper matching, which will affect the accuracy of the calculated pose correction offset and needs to be removed. In step S642, the improper matching is judged, and the points judged to be improper matching are deleted from the set V. An implementation example of improper matching judgment is: record a certain matching line segment as L _p , connect the matching points in the set V with L _p as the matching line segment clockwise or counterclockwise, and record it as set Z, Z={v _i , i=1,...,k}, v _i =(xi _, y _i ) ^T , k is the number of matching points. Use the least squares straight line fitting method to fit the line segment to the points in Z, and denote the fitted line segment as L _q . Calculate the distance and d of the point to the fitted line segment,

$\mathrm{<span\; class="notranslate">\; d</span>}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; d</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; c</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

If |θ _p -θ _q |>threshold value, that is, when the angle difference between the line segment obtained by matching the matching points and the matching line segment is large, it is considered that the matching is wrong, and all points in the set Z are deleted in the set V. If |θ _p -θ _q |≤threshold value, and d>threshold value, it is considered that some points in Z have mismatching. In order to remove these mismatches, each point in Z is judged as follows:

1) If v _i is the fitting point of a line segment L _o in the current local line segment feature map, then go to (3), otherwise go to (2);

2) Take the previous matching point v _i-1 and the next matching point v _i+1 of v _i in Z, and calculate the length and angle of the connecting line between v _i and these two points, θ ₂ =∠v _i v _i+1 , Δθ ₁ = |θ _p -θ ₁ |, Δθ ₂ = |θ _p -θ ₂ |. If d ₁ or d ₂ is greater than a certain value, and both Δθ ₁ and Δθ ₁ If it is less than the fixed value, it is a correct match, otherwise it is a wrong match.

3) If the local line segment L _o and the matching line segment L _p meet the requirement of synteny, it is a correct match, otherwise it is a wrong match.

In the set V, delete the points that are judged as wrong matches. The matching point set V' is obtained by step S42.

In step S643, a matching weight w _i is defined for each matching point v _i =( _xi , y _i ) ^T in the matching point set V'. The weight calculation formula is

${\mathrm{<span\; class="notranslate">\; w</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; gt</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; /</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}}{\mathrm{<span\; class="notranslate">\; min</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; gt</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; Fline</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; conf</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; LineDot</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; min</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; o</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$

In the formula, L _p is the matching line segment feature corresponding to v _i , and LineDot is a Boolean quantity, which is used to indicate whether the point v _i is a fitting point of a line segment feature in the current local line segment feature map. If so, it is true, and the local The line segment is characterized by L _o . The first item in the formula indicates that the earlier the matching line segment is generated, the higher the matching weight, because the feature of the line segment is constantly corrected as the map is updated; the second item indicates the matching weight when the matching line segment is a corner segment higher; the third item indicates that the higher the reliability of the matching line, the higher the matching weight; the fourth item indicates that when the matching point is a fitting point of a local line segment, its matching weight is higher than that of failing to fit any The matching weight of the matching point in the local line segment, and when the length of the local line segment is less than or equal to the length of the matching line segment, its weight is higher than the weight of the local line segment length greater than the length of the matching line segment, because the latter indicates that the matching line segment will be corrected.

Step S644 calculates the pose correction offset Δ, Δ=(Δ _x , Δ _y , Δ _θ ) ^T . In the implementation example, the parameter model structure is first constructed according to the best matching. The best matching situation is that after rotating and translating each matching point v _i in V′ according to the pose correction offset Δ, the distance from each matching point to the corresponding matching line is 0. Denote v _i ' as the point obtained after v _i is rotated and translated according to Δ.

${{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}}^{<span\; class="notranslate">\; \&prime;</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}\\ \mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}& \mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}\end{array}\right]<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; +</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\end{array}\right]<span\; class="notranslate">\; \&ap;</span>{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; v</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; v</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; +</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; \&Delta;</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\end{array}\right]<span\; class="notranslate">\; .</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 10</span>}<span\; class="notranslate">\; )</span>$

Where v is the center of gravity of all points in the matching set V'. The distance from v _i ′ to v _i corresponding to the matching line L _p is d _i = r _i -u _i ·v _i ′, u _i is the unit normal vector of L _p , and _{ri is} the distance between any point on the matching line and u _i Dot multiplication. From the linear equation, u _i =[u _i1 , u _i2 ]=[cosθ _p , sinθ _p ], r _i =-c _p . Then from the best matching condition d _i =0, we can get

in

There are m points in the set V′, we can get

Y＝ΦΔ. (13)

组成的m×3矩阵。利用加权最小二乘估计可计算得到位姿校正偏移量Δ，where Y is an m×1 column vector consisting of y _i and Φ is

Composed of m×3 matrices. The weighted least squares estimation can be used to calculate the pose correction offset Δ,

Δ＝(Φ ^T WΦ) ^-1 Φ ^T WY. (14)

Where W is an m×m diagonal positive definite weight matrix, and W[i, i] is the matching weight w _i of the matching point v _i . At this time, the matching part will achieve the best match, that is, the matching error covariance J _W =(Y-ΦΔ) ^T W(Y-ΦΔ) reaches the minimum value.

Step S645 translates and rotates each obstacle data point according to the pose correction offset. Then step S41 is executed again, and after the execution of step S41 is completed, a convergence judgment is performed. In this implementation example, the convergence condition is that the residual error of the weighted least squares estimation is less than a certain value or is stable at a certain value. When the convergence condition is satisfied, the obtained pose correction offset is the desired pose correction offset; when the convergence condition is not satisfied, return to step S42, and so on until the convergence condition is satisfied.

Then execute step S65, according to the estimated pose of the mobile robot in the current cycle $x_{t_0}={(x_{t_0},{\mathrm{the\; y}}_{t_0},z_{t_0})}^T$ And pose correction offset Δ=(Δ _x , Δ _y , Δ _θ ) ^T , calculate the pose estimation X _t of the mobile robot in the current period = (x _t , y _t , z _t ) ^T , the calculation formula is:

$\left\{\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; x</span>}\mathrm{<span\; class="notranslate">\; sin</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Delta;</span>}\mathrm{<span\; class="notranslate">\; the\; y</span>}\mathrm{<span\; class="notranslate">\; cos</span>}{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; .</span>\\ {\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; t</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&Delta;\&theta;</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$

According to the obtained robot pose estimation, local line feature map and local occupancy grid map, the global line feature map and global occupancy grid map can be updated. According to the estimated pose of the robot, calculate the corresponding position of each grid in the local occupancy grid map in the global occupancy grid map, and use the formula (3) to update the occupancy probability of the grid. Similarly, according to the estimated robot pose, each line feature in the local line feature map is transformed into the global line feature map, and whether there is a global line feature in the global line feature map that satisfies the same direction as the local line feature sex, collinearity and coincidence requirements. If it exists, merge the line segment fitting points, use the least squares line fitting method to recalculate the line segment feature attributes, and replace the original global line segment feature, if not, add the line segment feature to the global line segment feature map. Finally, the credibility of each line segment feature is calculated by using the global occupancy grid map. The calculation method is to divide the length of the line segment feature located in the occupied grid by the total length of the line segment feature. When the confidence is lower than a certain value, the line feature is deleted in the global line feature map.

Claims (9)

1. A method for mobile robot positioning and map construction in an unknown environment, characterized in that, comprising the steps: 1) According to the data obtained by the ranging sensor installed on the mobile robot, the local occupancy grid map and the local line segment feature map are constructed, and the local occupancy grid map data and the local line segment feature map data are converted to the global map through coordinate system conversion In the coordinate system, the initial global occupancy grid map and the global line segment feature map are obtained; 2) According to the data obtained by the ranging sensor installed on the mobile robot, construct the currently observed local occupancy grid map and local line segment feature map; 3) Estimate the pose of the mobile robot in the current cycle according to the ranging sensor data, the dead reckoning sensor data, the pose estimation of the mobile robot in the previous cycle, and the global line segment feature map constructed in the previous stage; 4) Update the global occupancy grid map and the global line segment feature map according to the local line segment feature map and the local occupancy grid map obtained by estimating and constructing the pose of the mobile robot in the current cycle; 5) Repeat steps 2) to 4) to obtain the global occupancy grid map and the global line segment feature map. 2. The method for simultaneous positioning and map construction of a mobile robot in an unknown environment according to claim 1, wherein the data obtained by the ranging sensor is obtained by scanning the ranging sensor on the ranging height plane The distance and angle of each point on the obstacle in the environment relative to the mobile robot. 3. The method for simultaneous positioning and map construction of a mobile robot in an unknown environment according to claim 1, wherein the local occupancy grid map and the local line segment feature map are based on the position of the current cycle mobile robot as The origin, the coordinate system with the positive direction of the mobile robot in the current cycle as the X-axis. 4. A method for simultaneous positioning and map construction of a mobile robot in an unknown environment according to claim 1, characterized in that, said global occupancy grid map and global line segment feature map use the first cycle to move the robot position is the origin, and the positive direction of the mobile robot in the first period is the coordinate system of the X-axis. 5. The method for simultaneous positioning and map construction of a mobile robot in an unknown environment according to claim 1, characterized in that, according to the data obtained by the rangefinding sensor installed on the mobile robot, the currently observed Local occupancy grid map and local line segment feature map: it includes the construction of local occupancy grid map and local line segment feature map, where the local occupancy grid map is constructed using Boolean Bayesian filtering method, and the steps of local line segment feature map construction for: 1) For the ranging sensor data, the Hough transform straight line fitting method is used to obtain a preliminary fitting line segment; 2) Carry out isotropic judgment, collinearity judgment and coincidence judgment on the preliminary fitting line segment, and merge the fitting point set of the line segment coincident with the collinear line roughly in the same direction; 3) For the merged line segment fitting point set, use the least squares fitting method to obtain an accurate fitting line segment

. 6. The method for simultaneously positioning and map construction of a mobile robot in an unknown environment according to claim 1, characterized in that, according to the distance measurement sensor data, the dead reckoning sensor data, and the previous period to the mobile robot The pose estimation and the global line segment feature map obtained in the previous construction, the steps to estimate the pose of the mobile robot in the current cycle are: 1) According to the dead reckoning sensor data and the pose estimation of the mobile robot in the previous cycle, use the motion model of the mobile robot to estimate the pose of the mobile robot in the current cycle; 2) According to the estimated pose, extract the features that fall within the scanning range of the ranging sensor from the global line segment feature map, and obtain the estimated currently visible line segment features; 3) Transform the currently visible line segment features from the global map coordinate system to the local map coordinate system; 4) Calculate the pose correction offset by finding the best match between the ranging sensor data and the expected current visible line segment features; 5) According to the estimated pose and pose correction offset of the mobile robot in the current period, calculate the pose estimation of the mobile robot in the current period. 7. The method for simultaneous positioning and map construction of a mobile robot in an unknown environment according to claim 6, wherein said estimation of the pose of the mobile robot according to the dead reckoning sensor data and the last cycle, utilizes The mobile robot motion model estimates the pose of the mobile robot in the current cycle: including the following steps, 1) According to the dead reckoning sensor data of the current cycle and the dead reckoning sensor data of the previous cycle, calculate the approximate moving speed of the mobile robot during this period; 2) According to the calculated approximate moving speed of the mobile robot and the estimated pose of the mobile robot in the previous cycle, calculate the estimated pose of the mobile robot in the current cycle. 8. A method for simultaneous positioning and map construction of a mobile robot in an unknown environment according to claim 6, characterized in that, calculating the position by finding the best match between the ranging sensor data and the expected current visible line segment features Attitude correction offset: includes the following steps, 1) Use the matching method with the shortest feature distance from the point to the line segment to find the matching relationship between the ranging sensor data and the expected current visible line segment feature; 2) Start with the data point relationship matching the same line segment feature, judge the above matching relationship, and delete the inappropriate match; 3) Proportional quantification is defined from five aspects: the generation time of the matching line segment feature, the reliability of the matching line segment feature, whether the matching line segment is a corner composition line segment, the length of the matching line segment, and whether the matching data is a fitting point of a local line segment feature matching weight; 4) According to the condition that the best matching is that the characteristic distance from the data point to the matching line segment is zero, the parameter model structure of the matching part is best matched, and the weighted least squares parameter estimation method is used to calculate the pose correction offset; 5) Translate and rotate the ranging sensor data according to the pose correction offset; 6) Steps 1)-5) are repeated until the residual error of weighted least squares parameter estimation is less than a certain value or the convergence condition is stable. 9. The method for simultaneous positioning and map construction of a mobile robot in an unknown environment according to claim 1, wherein the local line segment feature map obtained according to the pose estimation and construction of the mobile robot in the current cycle is described and local occupancy grid map, update global occupancy grid map and global line segment feature map: including global occupancy grid map update and global line segment feature map update, in which global occupancy grid map update adopts Boolean Bayesian filter method, global line segment Feature map update includes the following steps: 1) For the line segment feature in the local line segment feature map, find whether there is a global line segment feature and the local line segment feature in the global line segment feature map to meet the requirements of isotropy, collinearity and coincidence; 2) If it exists, merge the line segment fitting points, use the least squares straight line fitting method to re-fit the line segment, and replace the original global line segment feature; 3) If it does not exist, add the local line feature to the global line feature map; 4) Using the global occupancy grid map to calculate the credibility of the line feature in the global line feature map.

Images