A laser scanning matching positioning method and system suitable for complex environments in a station

Abstract

The application discloses a kind of laser scanning matching positioning method and system suitable for complex environment in station, including the local subgraph of surrounding map information is constructed, and the type of local subgraph is grid map;The correlation scanning of multiple poses is carried out to local subgraph, and the best pose with the highest correlation degree is found out;The pose error of the time when the inspection robot is located in the best pose is solved by using nonlinear optimization, and the correlation degree score error is the minimum;Based on pose error, the optimal pose point of inspection robot is determined;Based on optimal pose point, global map is updated.The application proposes a scanning matching scheme in laser SLAM mapping, with inertial wheeled odometry pose as priori pose, in the process of scanning matching, first, CSM scanning matching is used as coarse matching, and then nonlinear optimization is used for fine matching based on coarse matching.This scheme has the characteristics of high real-time, low positioning error rate and the like in the face of the complex indoor environment such as multiple corridors in station building, irregularly distributed equipment in electronic workshop and multi-corner structure.

Description

TECHNICAL FIELD

[0001] The present application relates to laser scanning matching positioning technology, in particular to a laser scanning matching positioning method and system suitable for complex environment in a station. BACKGROUND

[0002] The current scanning matching method for station positioning of the inspection robot can be generally classified into Scan-to-Scan matching and Scan-to-Submap matching. The Scan-to-Scan matching method based on ICP and point-line feature matching is a rigid transformation relationship solving method between two point cloud sets in a three-dimensional space. The core of this method is to accurately extract the point-line features in the adjacent two frames of radar information, and to solve the relative pose between the frames by ICP according to the feature point set. The Scan-to-Submap matching method includes non-linear optimization based scanning matching and Correlative Scan Matching (CSM). The non-linear optimization based scanning matching converts the scanning matching problem into a non-linear problem, and completes the matching by constantly approaching the extreme value through gradient descent. The CSM method constructs sub-maps of different resolutions and performs global search to obtain the matching result.

[0003] However, the Scan-to-Scan matching method based on ICP can solve the relatively accurate inter-frame pose and complete inter-frame matching when there is an accurate prior pose. However, since it is processed based on the point cloud information collected by the radar, it is impossible to establish a sub-map, which will lead to a loose pose structure in the back end, and the accumulated error will be generated due to long-time movement, resulting in deviation of the map. The Scan-to-Submap matching method based on non-linear optimization is for local pose, and in the case of local extreme value, it is easy to fall into the local optimal solution, which leads to the failure to match the optimal solution. The Scan-to-Submap matching method based on CSM is a kind of exhaustive matching, and the matching accuracy depends on the resolution of the map. Meanwhile, the calculation amount of CSM is large under high resolution, and the matching efficiency is low. SUMMARY

[0004] The purpose of the present application is to provide a laser scanning matching positioning method and system with small calculation amount, high accuracy and suitable for complex environment in a station.

[0005] Technical scheme: To achieve the above-mentioned purpose, the laser scanning matching positioning method suitable for complex environment in a station comprises the following steps:

[0006] Step 1: Construct a local sub-map containing surrounding map information, and the type of the local sub-map is a grid map;

[0007] Step 2: correlation scanning of the local subgraph in multiple poses to find the best pose with the highest correlation degree;

[0008] Step 3: using nonlinear optimization to solve the pose error of the inspection robot when it is in the best pose, and the error of the correlation degree score is the smallest;

[0009] Step 4: determining the optimal pose point of the inspection robot based on the pose error;

[0010] Step 5: updating the global map based on the optimal pose point.

[0011] The local method for constructing a local map containing surrounding map information in step 1 is: fusing the collected inspection robot running mileage and acceleration information through Kalman filtering as the prior pose of the inspection robot; the inspection robot collects laser point cloud data through a laser radar, and constructs a local subgraph containing surrounding map information based on the laser point cloud data and the prior pose.

[0012] Wherein, the inspection robot is controlled to move through a keyboard, and laser point cloud data is collected during the movement to construct a local subgraph.

[0013] The method for scanning the correlation of the local subgraph in multiple poses to find the best pose with the highest correlation degree in step 2 is:

[0014] A search space (x, y, θ) containing three dimensions is established at the prior pose of the local subgraph, wherein the angle search space is dθ∈[-Δθ, Δθ], and the lateral translation search space dx∈[-Δx, Δx] and the longitudinal translation search space dy∈[-Δy, Δy] are defined within the angle search space;

[0015] The angle, the position of the inspection robot on the X axis, and the position of the inspection robot on the Y axis are traversed within the search space (x, y, θ) to cover all possible poses;

[0016] According to each possible pose in the search space (x, y, θ), the local subgraph is spliced with the global map that has been built. The laser point cloud data is mapped to the global map coordinate system after splicing, the probability of each point in the laser point cloud data falling into the global map is calculated, the probability of each laser point is taken logarithm, and the logarithm probabilities of all laser points are added to obtain the sum of the logarithm probabilities of the current pose falling into the global map, and the sum is defined as the correlation degree score of the current pose;

[0017] The correlation degree score of all poses in the search space (x, y, theta) is calculated, and the pose with the highest correlation degree score is taken as the best pose, at which the matching degree of the collected laser point cloud data and the global map is the highest.

[0018] The search step length of the lateral translation search space and the longitudinal translation search space is not less than the resolution of the grid map.

[0019] The multi-pose correlation scanning of the local sub-map is accelerated by the pruning acceleration method, and the search speed is accelerated, and the method is specifically as follows:

[0020] First, a low-resolution grid map with a resolution lower than that of the local sub-map is established based on each angle search space d theta E [-delta theta, delta theta];

[0021] The angle, the position of the inspection robot on the X axis, and the position of the inspection robot on the Y axis of the low-resolution grid map under all angle search spaces are traversed respectively, the correlation degree score of the pose under all angle search spaces is calculated, and the low-resolution grid map with the highest correlation degree score is found out.

[0022] The resolution of the low-resolution grid map with the highest correlation degree score is improved to obtain a high-resolution grid map, the angle, the position of the inspection robot on the X axis, and the position of the inspection robot on the Y axis of the high-resolution grid map are traversed respectively, the correlation degree score of the pose under the current angle search space is calculated, and the pose with the highest correlation degree score is taken as the best pose.

[0023] The resolution ratio coefficient is valued at 2-64.

[0024] The method for solving the pose error of the inspection robot at the best pose when the correlation degree score error is the smallest by using the nonlinear optimization in step 3 is as follows:

[0025] Step 301: Based on the pose T = (T x , T y , T θ ) T of the inspection robot in the search space, the correlation degree of the current laser point cloud data in the local sub-map is established in the form of nonlinear least squares, as shown in the following formula:

[0026]

[0027] Wherein, L i (T) is the coordinates of the signal collection point at time i as shown in the following formula:

[0028]

[0029] M(L i (T) is the normalized score of the signal collection point at time i;

[0030] Step 302: Construct the function f(T)=[1-M(L i (T)), and use the Gauss-Newton iteration method to expand the nonlinear function f(T)=[1-M(L i (T)) according to the first-order Taylor expansion as shown in the following formula:

[0031]

[0032] Wherein, Indicates the grid gradient;

[0033]

[0034] p ix , p iy Respectively, the gradients in the X and Y axis directions;

[0035] The first-order Taylor expansion formula of the construction function is brought into formula And the following can be obtained:

[0036]

[0037] Step 303: Partially derive ΔT and let it be 0:

[0038]

[0039] Expand :

[0040]

[0041] Let H represent

[0042]

[0043] Multiply H On both sides of the equation -1 , and the following can be obtained: E(T+ΔT) obtains extreme value, that is, the minimum error value when ΔT:

[0044]

[0045] Wherein, the optimal pose point described in step 4 is T+△T.

[0046] The laser scanning matching positioning system suitable for complex environments in the station comprises the following modules:

[0047] The local subgraph construction module is used for constructing a local subgraph containing surrounding map information, and the type of the local subgraph is a grid map;

[0048] Local subgraph correlation scanning module: used for correlation scanning of local subgraph in multiple poses, and finding the best pose with the highest correlation degree;

[0049] Nonlinear optimization module: used for solving the pose error of the inspection robot when the best pose is located by using nonlinear optimization, and the correlation degree score error is the smallest;

[0050] Optimal pose point determination module: used for determining the optimal pose point of the inspection robot based on the pose error;

[0051] Global map updating module: used for global map updating based on the optimal pose point.

[0052] Advantages: the present application has the following advantages: 1, compared with the prior art, the present application first performs correlation scanning coarse matching, and the pose matching result is positioned near the optimal pose, solving the problem of easy falling into local optimum; 2, the present application proposes a low-resolution pruning acceleration method to reduce the amount of calculation for the problem of large amount of calculation in high-resolution map traversal in correlation scanning matching; 3, the present application uses the data fusion measurement of the inertial measurement unit and the wheeled odometry to calculate the pose as the prior pose, enhancing the real-time and accuracy of positioning. BRIEF DESCRIPTION OF DRAWINGS

[0053] Figure 1 It is a mapping flowchart for an inspection robot;

[0054] Figure 2 It is a correlation scanning registration schematic diagram;

[0055] Figure 3 It is a map search tree schematic diagram;

[0056] Figure 4 It is a schematic diagram of the structure of an inspection robot system;

[0057] Figure 5 It is an inter-frame matching pose estimation experiment. DETAILED DESCRIPTION

[0058] The technical solutions of the present application will be described in detail below in combination with the embodiments and the drawings.

[0059] Example 1

[0060] The present application proposes a scanning matching scheme in laser SLAM mapping, which takes the inertial wheeled odometry pose as the prior pose. In the scanning matching process, CSM scanning matching is used as coarse matching, and nonlinear optimization is used for fine matching based on coarse matching. This scheme has the characteristics of high real-time and low positioning error rate in the face of indoor complex environments such as multiple corridors in station buildings, irregularly distributed equipment in electronic workshops and multi-corner structures, and is recommended for use in similar complex environments.

[0061] like Figure 1 The diagram illustrates the laser SLAM mapping process for an inspection robot. The inspection robot is equipped with multiple sensor systems, including a wheeled odometer, an inertial measurement unit (IMU), and a lidar system, which collect odometer information, inertial acceleration information, and lidar information, respectively. To complete indoor positioning, the inspection robot first collects its mileage and acceleration information using the wheeled odometer and IMU, fusing these data to obtain the inertial wheeled odometer information as the prior pose. When constructing the environmental map, upon acquiring new laser point cloud data, multiple local sub-maps containing surrounding map information are created. When a sufficient number of local sub-maps exist, a coarse matching of the sub-maps is first performed using correlation scanning, followed by fine matching using nonlinear optimization, thus constructing the global map.

[0062] like Figure 2 The diagram illustrates correlation scan matching. Correlative Scan Matching (CSM) is a search method for finding the globally optimal pose. First, a three-dimensional search space (x, y, θ) is established near the prior pose, where the angle search space is dθ∈[-Δθ, θΔ]. Within the angle search space, a lateral translation search space dx∈[-Δx, Δx] and a longitudinal translation search space dy∈[-Δy, Δy] are defined. The system iterates through θ (angle), x (robot's position on the X-axis), and y (robot's position on the Y-axis) to cover all possible poses. Each laser point is compared with the constructed map, and the sum of the log probabilities is used as the correlation score. The pose with the highest correlation score is selected as the optimal pose.

[0063] To reduce the computational load, a multi-resolution map is used to accelerate the global scan when establishing the angle search space, such as... Figure 3 As shown. First, a low-resolution raster map with a lower resolution than the original is created for each angle layer, with a scale factor typically between 2 and 64. The low-resolution raster is searched to obtain the highest score. Then, a raster search is performed on the high-resolution raster map corresponding to the low-resolution map with the highest score to obtain the highest score. The remaining low-resolution maps are traversed for pruning and acceleration. If the relevance score of a low-resolution map is higher than the previously obtained highest score, the corresponding high-resolution map is traversed to update the highest score; otherwise, it is excluded, completing the pruning and acceleration process. The pruning process has a probability of excluding the optimal pose, but it ensures that the algorithm obtains a solution near the optimal pose.

[0064] After the correlation scanning registration is completed, a non-linear optimization fine registration is introduced, and a state estimation model is first constructed. The state estimation model includes a motion equation representing the state change rule of the inspection robot pose at time k and k-1, and an observation equation representing the change of the pose estimated by the laser radar at time k and the real state of the system, as shown in formula 1-1:

[0065]

[0066] wherein x k is the actual pose of the inspection robot at time k; u k is the pose information provided by the inertial wheeled odometer, including horizontal coordinates, vertical coordinates and angle; y j is the distribution information of the obstacles in the environment collected; z k，j is the y k laser radar information collected by the laser radar of the inspection robot at the pose x j ; ω k is the noise generated in the motion, and v k，j is the noise in the laser radar collected signal.

[0067] The observation equation satisfies the Gaussian normal distribution as shown in formula 1-2:

[0068]

[0069] wherein h(y j , x k ) is the mean value, Q k，j is the variance;

[0070] The probability density expansion form of formula 1-2 is shown in formula 1-3:

[0071]

[0072] Formula 1-3 is converted into a negative logarithmic form as shown in formula 1-4:

[0073]

[0074] The second term on the right side of the equation is a matrix quadratic form, and the extreme value can be obtained by using non-linear optimization.

[0075] From the above reasoning, for the scan matching problem of one observation, it can be considered that in the local subgraph, the pose is found which makes the current frame point cloud after the rotation matrix T transformation, and the matching degree reaches the optimal pose, wherein the correlation degree is measured by using the logarithmic probability score.

[0076] First, the same correlation degree calculation method as CSM matching is used, and the correlation degree of the current laser data in the local subgraph is used to establish the form of non-linear least squares, as shown in formula 1-5:

[0077]

[0078] where T = (T x , T y , T θ ) T is the actual pose of the robot, i.e., x k ; L i (T) is the coordinate of the signal collection point at time i as shown in equation 1-6:

[0079]

[0080] M(L i (T)) is the normalized score of the signal collection point at time i.

[0081] The constructor f(T) = [1 - M(L i (T))] is expanded in first order Taylor as shown in equation 1-7:

[0082]

[0083] where, represents the grid gradient. Substituting equation 1-7 into equation 1-5 gives:

[0084]

[0085] Take the partial derivative of AT:

[0086]

[0087] Let equation (1-9) equal 0 and expand as equation 1-10:

[0088]

[0089] Let H represent:

[0090]

[0091] Multiply both sides of the equation by H -1 to get AT when E(T + AT) takes the extreme value (minimum error value), as

[0092] Equation 1-12:

[0093]

[0094] Take the partial derivative of T with respect to the function L i (T), as shown in equation (1-13):

[0095]

[0096] p i of the gradient Interpolation is needed, which can be estimated by bilinear interpolation of the values of the surrounding four adjacent grids.

[0097] The optimal pose calculated by non-linearization is T+△T.

[0098] Example two

[0099] As Figure 4 The structure of the inspection robot system is shown in the figure, in which the Jetson Nano industrial computer is used as the main control board of the inspection robot system structure, the STM32 is used as the chassis controller, and sensors such as laser radar, wheeled odometer and inertial unit are carried, and the corresponding interface layer, logic layer and device layer are designed.

[0100] Interface layer: realize the information interaction between user end and robot bottom layer. The user end can obtain the pose information of the robot and the environment information of the device workshop through the Rviz visualization interface, and issue corresponding inspection instructions to realize the control of the inspection robot by the user.

[0101] Logic layer: realize the processing work of the sensor data of the system, and control the running of the inspection process, to ensure that the system can complete the inspection work according to the demand. The information collected by the laser radar, the wheeled odometer and the inertial unit is stored in the logic layer, and the indoor positioning scheme combining the inertial wheeled odometer pose estimation as the prior pose, the CSM correlation scanning coarse matching and the non-linear optimization fine matching is used in the logic layer to realize information fusion, simultaneous localization and mapping.

[0102] Device layer: realize the sensor module control, environment data acquisition and inter-module communication work. The device layer includes laser radar, inertial measurement unit and encoder. The pose state of the robot itself and the laboratory environment information are collected and interacted with the logic layer to complete data transmission.

[0103] The above inspection robot uses wheeled odometer, inertial measurement unit and laser radar and other multi-sensor cooperation to collect indoor environment and complete indoor positioning in the laboratory, effectively improving the positioning accuracy.

[0104] The inter-frame matching pose tracking experiment was carried out in the experimental environment with a trajectory of 200m with two right-angle turns, and 20 repeated test experiments were carried out. As Figure 5 shown, the absolute position average error of the end position obtained by the laser scanning registration and non-linear pose optimization pose tracking method is 0.53m, which is reduced by 87% compared with the pose estimation relying only on the inertial wheeled odometer, and the positioning error ratio is 0.25%.

Claims

1. A laser scanning matching positioning method suitable for complex environments in a station, characterized by, The method comprises the following steps: Step 1: constructing a local subgraph containing surrounding map information, the type of the local subgraph being a grid map, specifically, fusing odometer and acceleration sensor information as a prior pose of the robot, and constructing the local subgraph containing surrounding map information by the laser radar at the prior pose; Step 2: Correlation scanning of local subgraph in multiple poses to find the best pose with the highest correlation, which is based on the prior pose to establish a search space The local subgraph is spliced according to each possible pose in the search space and the global map, and the best pose and the best global map are obtained according to the probability of the laser frame falling into the spliced global map; Step 3: Solve the pose error of the inspection robot when the correlation score error is the smallest by using nonlinear optimization, which is based on the robot pose The corresponding collection point coordinates in the global map Normalize the score of the collection point coordinates to construct a function Based on the function Solve the minimum pose error △T by using the partial derivative of the grid gradient and the first-order Taylor series. Step 4: determining an optimal pose point T+△T of the inspection robot based on the pose error; Step 5: updating the global map based on the optimal pose point.

2. The laser scanning matching positioning method suitable for complex environment in a station according to claim 1, characterized in that, The method for constructing the local subgraph containing surrounding map information in step 1 is as follows: fusing the collected running odometer and acceleration information of the inspection robot through Kalman filtering as the prior pose of the inspection robot; collecting laser point cloud data by the laser radar of the inspection robot, and constructing the local subgraph containing surrounding map information based on the laser point cloud data and the prior pose.

3. The laser scanning matching positioning method suitable for complex environment in a station according to claim 1, characterized in that, The inspection robot is controlled to move through a keyboard, and laser point cloud data is collected during the movement to construct the local subgraph.

4. The laser scanning matching positioning method suitable for complex environment in a station according to claim 1, characterized in that, The method for scanning the correlation of the local subgraph at multiple poses in step 2 to find the best pose with the highest correlation degree and updating the global map based on the best pose is as follows: establish a search space of three dimensions at the prior pose of the local submap where the angular search space is define a lateral translation search space within the angular search space and a longitudinal translation search space ; In the search space The angle, the position of the inspection robot on the X axis, and the position of the inspection robot on the Y axis are traversed respectively to cover all candidate poses. According to each candidate pose in the search space , the local subgraph is spliced with the built global map; the laser point cloud data is mapped to the spliced global map coordinate system, the probability of each point in the laser point cloud data falling into the global map is calculated, the logarithm of the probability of each laser point is taken, and the logarithm probabilities of all laser points are added to obtain the sum of the logarithm probabilities of the current pose falling into the global map, and the sum is defined as the relevance score of the current pose; Computing search space The correlation scores of all poses are calculated, and the pose with the highest correlation score is taken as the best pose, at which the matching degree of the collected laser point cloud data and the global map is the highest. The local sub-map is spliced with the global map using the best pose, and the global map is updated.

5. The laser scanning matching positioning method suitable for complex environment in a station according to claim 4, characterized in that, The search step length of the lateral translation search space and the longitudinal translation search space is not less than the resolution of the grid map.

6. The laser scanning matching positioning method suitable for complex environment in a station according to claim 4, characterized in that, The correlation of the local subgraph at multiple poses is scanned through a pruning acceleration method to accelerate the search speed, specifically as follows: First, search space for each angle is searched , a low resolution grid map is established with lower resolution than the local submap all the low-resolution grid maps under the angle search space are traversed in terms of angle, position of the inspection robot on the X-axis and position of the inspection robot on the Y-axis, the correlation degree scores of the poses under all the angle search spaces are calculated, and the low-resolution grid map with the highest correlation degree score is found; the resolution of the low-resolution grid map with the highest correlation degree score is improved to obtain a high-resolution grid map, the high-resolution grid map is traversed in terms of angle, position of the inspection robot on the X-axis and position of the inspection robot on the Y-axis, the correlation degree score of the pose under the current angle search space is calculated, and the pose with the highest correlation degree score is found as the best pose.

7. The laser scanning matching positioning method suitable for complex environment in a station according to claim 6, characterized in that, The resolution ratio coefficient is 2-64.

8. The laser scanning matching positioning method suitable for complex environment in a station according to claim 1, characterized in that, The method for solving the pose error when the inspection robot is at the best pose with the minimum correlation degree score error through nonlinear optimization in step 3 is as follows: Step 301: Based on the patrol robot pose in the search space According to the correlation degree of the current laser point cloud data in the local subgraph, a nonlinear least square form is established, as shown in the following formula: ； wherein is The coordinates of the time signal collection point are given by the following equation: ； To normalize the scores of the time signal collection points; Step 302: constructor , using a Gauss-Newton iterative method, to find the non-linear function According to a first order Taylor expansion, as shown in the following equation: ； wherein represents a grid gradient; ； , respectively the gradients in the X, Y axis directions; Bringing the first order Taylor expansion of the constructor into the equation This gives: ； Step 303: Take the partial derivative of and set it to zero: ； To unfold: ； Let is represented as: ； Both sides are multiplied by Both sides are multiplied by , we get The extreme value, that is, the minimum error value : 。 9. The laser scanning matching positioning method suitable for complex environment in a station according to claim 8, characterized in that, The optimal pose point in step 4 is T+△T.

10. A system suitable for the laser scanning matching positioning method according to claim 1, suitable for complex environments in a station, characterized in that, The method comprises the following modules: A local subgraph construction module for constructing a local subgraph containing surrounding map information, the type of the local subgraph being a grid map; A local subgraph correlation scanning module for scanning the correlation of the local subgraph at multiple poses to find the best pose with the highest correlation degree; A nonlinear optimization module for solving the pose error when the inspection robot is at the best pose with the minimum correlation degree score error through nonlinear optimization; An optimal pose point determination module for determining an optimal pose point of the inspection robot based on the pose error; A global map updating module for updating the global map based on the optimal pose point.

Images