A shock-resistant scan matching method and system based on multi-sensor decision fusion

Through the integration of multi-sensor decision making, and the positioning is updated in combination with laser point cloud data, the positioning inaccuracy problem caused by lateral impact of the robot in complex environments is solved, and the robustness and accuracy of the robot positioning is improved.

CN113566828BActive Publication Date: 2024-12-03HARBIN INST OF TECH +1
View PDF 1 Cites 0 Cited by

Patent Information

Application Number
CN202110780758.1
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2021-07-09
Publication Date
2024-12-03
Estimated Expiration
2041-07-09

AI Technical Summary

Technical Problem

In complex dynamic environments, robots are susceptible to lateral impacts and cause side slippage, which makes the wheel encoder unable to accurately sense lateral displacement, affecting the accuracy of scan matching results.

Method used

Using the impact-resistant scanning matching method based on multi-sensor decision fusion, local decisions are generated by fusing information from wheel encoder, gyroscope and accelerometer, and global decisions are selected as the initial pose for scanning matching through decision fusion rules, combined with laser points Cloud data updates the final position to improve the robustness and accuracy of robot positioning.

Benefits of technology

It effectively improves the positioning accuracy of the robot in the case of lateral impact, reduces the risk of scan matching inaccuracy, and enhances the reliability of robot path planning and navigation.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN113566828B_ABST
    Figure CN113566828B_ABST
Patent Text Reader

Abstract

The present invention discloses a shock-resistant scan matching method and system based on multi-sensor decision fusion, wherein the algorithm comprises the following steps: step 1: generating a local first decision by fusing information of a wheel encoder with information of a gyroscope; step 2: generating a local second decision by fusing information of an accelerometer with information of a gyroscope; step 3: selecting one of the local first decision and the local second decision as a global decision according to a decision fusion rule; step 4: using the global decision as the initial posture of scan matching, performing scan matching through a search window centered on the initial posture, obtaining the posture with the highest probability of being occupied by a laser point cloud on the map as the final posture, updating the global decision to the final posture, and completing the scan matching of the global decision. The present invention provides a good initial posture estimation by fusing decisions, so that the scan matching algorithm has shock resistance, and improves the robustness and accuracy of the scan matching algorithm.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robot control algorithms, and in particular to a shock-resistant scanning matching method and system based on multi-sensor decision fusion. Background Art

[0002] Successful path planning and navigation of mobile robots in indoor environments requires a reliable estimate of the current robot position. One of the basic ideas to achieve this reliability is to fuse information from multiple sensors, which can improve the robustness and accuracy of robot positioning. Currently, the most popular mobile robot positioning solutions often combine lidar, wheel encoders and inertial sensors. However, in complex dynamic environments, robots are very susceptible to lateral impacts. When a robot moves on a low-friction surface, if the robot's tire adhesion is low, it will easily slip. Robot slip generally refers to the phenomenon of lateral movement of the robot's wheels. This phenomenon can be regarded as a robot kidnapping problem, that is, when the robot is instantly moved to another position without prior knowledge or sensor perception, the wheel encoder cannot perceive the lateral displacement caused by the impact, which will provide inaccurate prior knowledge to the scan matching algorithm, thereby affecting the scan matching results. Summary of the invention

[0003] To solve the above problems, the present invention provides a shock-resistant scan matching method and system based on multi-sensor decision fusion, which achieves better initial pose estimation through fusion decision, makes the scan matching algorithm shock-resistant, and improves the positioning accuracy of the robot based on the scan matching algorithm. The specific technical solutions of the present invention are as follows:

[0004] An anti-shock scan matching method based on multi-sensor decision fusion, the algorithm includes the following steps: step 1: generating a local first decision by fusing information of a wheel encoder with information of a gyroscope; step 2: generating a local second decision by fusing information of an accelerometer with information of a gyroscope; step 3: selecting one of the local first decision and the local second decision as a global decision according to a decision fusion rule; step 4: using the global decision as an initial position and posture of scan matching, performing scan matching in combination with a search window, obtaining a position with the highest probability of an occupied grid map being occupied by a laser point cloud as a final position and updating the global decision to the final position to complete the scan matching of the global decision.

[0005] Compared with the prior art, the present invention generates two local decisions based on the fusion of measurement results of multiple sensors to improve the robustness and accuracy of robot positioning. One of the decisions is selected as the global decision through the decision fusion rule to provide the scan matching algorithm with the initial posture for scan matching, so that the scan matching algorithm has impact resistance and avoids inaccurate scan matching when the robot is subjected to lateral impact. Compared with the traditional scan matching algorithm, the present invention makes the scan matching algorithm have impact resistance through multi-sensor decision fusion.

[0006] Furthermore, the anti-shock scanning matching method based on multi-sensor decision fusion also includes the following steps before executing step 1: step 00: caching raw data, and determining whether the number of cached frames of raw data has cached two frames or more, and if so, proceeding to step 01; step 01: preprocessing the cached raw data to obtain the preprocessing result of the raw data; step 02: determining whether the wheel encoder measurement value has changed, and if not, proceeding to step 03; if so, determining that the robot is in a non-stationary state, and proceeding to step 05; step 03: determining whether the standard deviation of the acceleration measurement value in the sliding window is less than a preset acceleration standard deviation threshold, and if so, determining that the robot is in a stationary state, and proceeding to step 04; if not, determining that the robot is in a non-stationary state, and proceeding to step 05; step 04: recording all acceleration measurement values ​​cached successively when the robot is in a stationary state as prior knowledge; step 05: the robot is in a non-stationary state, and the update of the offset b of the acceleration measurement value between two consecutive cached frames satisfies b k+1 =b k, wherein k is the serial number of the cached frame of the raw data; wherein the raw data includes wheel encoder measurement values, gyroscope measurement values, acceleration measurement values ​​and the acquisition time corresponding to each frame of each sensor measurement value; the preprocessing of the cached raw data includes: calculating the difference between two frames of wheel encoder measurement values ​​cached successively, calculating the posture change Δθ of two consecutive frames of robots, calculating the acquisition time difference Δt of two consecutive frames of sensor measurement values ​​cached successively, calculating the mean and standard deviation of the acceleration measurement values ​​in the sliding window, and storing the sensor measurement values ​​in sequence according to the collection time sequence to generate a sensor measurement value sequence; the posture change Δθ of two consecutive frames of robots, the acquisition time difference Δt of two consecutive frames of sensor measurement values ​​cached successively, the mean and standard deviation of the acceleration measurement values ​​in the sliding window and the sensor measurement value sequence are taken as the preprocessing results of the raw data; wherein the sensor measurement value sequence includes an acceleration measurement value sequence, a gyroscope measurement value sequence and a wheel encoder measurement value sequence; the acceleration measurement value in the sliding window refers to the acceleration measurement value of the latest cached preset frame number extracted from the acceleration measurement value sequence based on the sliding window technology. The present invention preprocesses the raw data, judges the robot's motion state based on the preprocessing result of the raw data, and decides whether to correct the sensor bias according to the robot's motion state, so as to make the sensor measurement value more accurate and reduce the error of the obtained local decision or global decision.

[0007] Furthermore, when it is detected that the robot is in a stationary state again after being started, the prior knowledge is updated to the acceleration measurement values ​​of all consecutive time frames cached when the robot is in a stationary state again. Considering the characteristics of the sensor such as temperature sensitivity, when the robot is in a stationary state again after being started, the prior knowledge is updated immediately to avoid the situation where the prior knowledge is not updated in time, resulting in inaccurate judgment results of whether the robot is subjected to lateral impact and affecting decision fusion.

[0008] Furthermore, the step 1 specifically includes: step 11: obtaining the robot posture change Δθ1 from the kth frame to the k+1th frame, obtaining the robot posture θ (k+1)1 ,θ (k+1)1 =θ k1 +Δθ1; Step 12: Define the first coordinate transformation matrix R of the robot in the kth frame on the robot's travel plane k1; Step 13: Based on the robot wheel diameter, the difference between the wheel encoder measurement value of the kth frame and the wheel encoder measurement value of the k+1th frame, obtain the robot's left wheel arc length ΔL and the robot's right wheel arc length ΔR; Step 14: Based on the robot's left wheel arc length ΔL and the robot's right wheel arc length ΔR obtained in step 13, obtain the robot's relative displacement ΔX1 in the X-axis direction from the kth frame to the k+1th frame and the robot's relative displacement ΔY1 in the Y-axis direction from the kth frame to the k+1th frame; Step 15: Obtain the robot position in the k+1th frame in, is the robot position of the kth frame; Step 16: The robot posture information (X (k+1)1 ,Y (k+1) ,θ (k+1)1 ) is generated as the local first decision; wherein, the arc length of the robot's left wheel refers to the arc length of a point on the robot's left wheel from the kth frame to the k+1th frame; the arc length of the robot's right wheel refers to the arc length of a point on the robot's right wheel from the kth frame to the k+1th frame; the frame number k is an integer greater than or equal to 1. As one of the options for the global decision, the local first decision is a decision generated based on the two internal sensing sensors, the wheel encoder and the gyroscope, sensing the position and posture changes of the robot.

[0009] Further, the step 14 specifically includes: calculating half of the sum of the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel, and confirming the calculation result as the length of the moving trajectory of the center of the robot from the kth frame to the k+1th frame; when the angle of the central angle of the robot's rotation from the kth frame to the k+1th frame is less than or equal to the preset central angle threshold, the length of the moving trajectory of the center of the robot from the kth frame to the k+1th frame is equal to the relative displacement ΔY1 of the robot in the Y-axis direction from the kth frame to the k+1th frame; calculating the absolute value of the difference between the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel; calculating the absolute value of the difference between the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel; and calculating the absolute value of the difference between the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel. The straight-line distance; the central angle of the robot's rotation from the kth frame to the k+1th frame is equal to the ratio of the absolute value of the difference between the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel to the straight-line distance between the robot's left wheel and the robot's right wheel; according to the Pythagorean theorem, the relative displacement ΔX1 of the robot in the X-axis direction from the kth frame to the k+1th frame is equal to the product of the relative displacement ΔY1 of the robot in the Y-axis direction from the kth frame to the k+1th frame and the tangent value of one-half of the central angle of the robot's rotation from the kth frame to the k+1th frame; wherein, the center of the robot refers to the midpoint of the line connecting the left wheel of the robot and the right wheel of the robot.

[0010] Further, the step 2 specifically includes: step 21: obtaining the posture change Δθ2 of the robot from the kth frame to the k+1th frame, obtaining the posture θ of the robot in the k+1th frame(k+1)2 ,θ (k+1)2 =θ k2 +Δθ2; Step 22: Define the robot Euler angles as follows: relative to the robot body coordinate system, the rotation around the Z axis is the yaw angle α, the rotation around the Y axis is the pitch angle β, and the rotation around the X axis is the roll angle γ. Based on the robot Euler angles, define the second coordinate transformation matrix R of the robot in the kth frame in the space where it moves k2 ; Step 23: According to the acceleration measurement value a of the kth frame in the original data k , the acceleration measurement value a of the k+1th frame k+1 , the acceleration measurement value bias b of the kth frame k , the acceleration measurement value bias b of the k+1th frame k+1 , the second coordinate transformation matrix R of the kth frame k2 and the second coordinate transformation matrix R of the k+1th frame (k+1)2 , get the midpoint acceleration a of the robot at the kth frame and the k+1th frame mid ; Step 24: Get the robot speed V of the k+1th frame k+1 , V k+1 =V k +a·Δt, where acceleration a is the midpoint acceleration a between the robot at the kth frame and the robot at the k+1th frame. mid and the sum of the gravitational acceleration g; Step 25: According to the robot velocity propagation equation of the k+1th frame obtained in step 24, integrate the robot velocity of the k+1th frame once to obtain the robot position P of the k+1th frame k+1 , Among them, P k+1 represents the robot's position in the k+1th frame (X (k+1)2 , Y (k+1)2 , Z (k+1)2 ), P k Represents the robot's position information in the kth frame (X k2 , Y k2 , Z k2 ); Step 26: The robot posture information (X (k+1)2 ,Y (k+1)2 ,θ (k+1)2 ) is generated as a local second decision. As one of the options for global decision, the local second decision is generated based on the external sensing sensor sensing the position change of the robot compared to the local first decision. After repeated experiments, the inventor found that using an external sensing sensor to assist an internal sensing sensor in the case of a lateral impact on the robot can obtain a more accurate posture estimation.

[0011] Furthermore, the step 23 specifically includes: calculating the robot's k-th frame acceleration measurement value ak The acceleration measurement value offset b of the robot at the kth frame k The difference is recorded as the robot's k-th frame acceleration correction measurement value; calculate the robot's k+1-th frame acceleration measurement value a k+1 The acceleration measurement value offset b of the robot at the k+1th frame k+1 The difference is recorded as the acceleration correction measurement value of the robot in the k+1th frame; the second coordinate transformation matrix R of the robot in the kth frame is calculated. k2 The product of the robot's k-th frame acceleration correction measurement value is recorded as the robot's k-th frame acceleration transformation measurement value; calculate the robot's k+1-th frame second coordinate transformation matrix R (k+1)2 The product of the acceleration correction measurement value of the robot's k+1th frame is recorded as the robot's k+1th frame acceleration transformation measurement value; the midpoint acceleration of the robot at the kth frame moment and the robot's k+1th frame moment is equal to half of the sum of the robot's kth frame acceleration transformation measurement value and the robot's k+1th frame acceleration transformation measurement value.

[0012] Furthermore, the step 3 specifically includes: step 31: obtaining a sensor measurement value sequence, and then entering step 32; step 32: taking the acceleration measurement values ​​of all consecutive time frames of the robot in the stationary state in the prior knowledge as the first population, extracting the latest cached acceleration measurement values ​​of a preset number of frames from the acceleration measurement value sequence based on a sliding window of a preset size as the first sample, calculating the Mahalanobis distance between the first sample and the first population, and judging whether the Mahalanobis distance between the first sample and the first population is greater than a preset Mahalanobis distance threshold, if so, entering step 33, if not, determining that the robot is not subjected to a lateral impact, and entering step 35; step 33: extracting the latest cached one frame of gyroscope measurement values ​​from the gyroscope measurement value sequence as the gyroscope measurement value at the current moment , according to the gyroscope measurement value at the current moment, the robot roll angle and the robot pitch angle at the current moment are obtained, the first Euclidean distance between the robot roll angle at the current moment and the preset robot roll angle threshold is calculated, and the second Euclidean distance between the robot pitch angle at the current moment and the preset robot pitch angle threshold is calculated, and it is determined whether the first Euclidean distance and the second Euclidean distance are both less than the preset Euclidean distance threshold. If so, it is determined that the robot is subjected to a lateral impact, and the process goes to step 34. If not, it is determined that the robot is not subjected to a lateral impact, and the process goes to step 35; Step 34: Select the local second decision as the global decision; Step 35: Select the local first decision as the global decision; wherein the specific size of the preset size of the sliding window is determined by the preset number of frames extracted from the acceleration measurement value. The present invention selects the Mahalanobis distance as the distance metric for measuring the similarity of acceleration, and selects the Euclidean distance as the distance metric for measuring the similarity of Euler angles. By judging the relationship between the Mahalanobis distance and the Euclidean distance and the corresponding threshold, it is determined whether the acceleration or posture of the robot has changed dramatically, so as to determine that the state of the robot corresponds to the selection of the corresponding local decision as the global decision.

[0013] Furthermore, in step 4, the global decision is used as the initial posture of the scan matching, and the scan matching is performed in combination with the search window to obtain the posture with the highest probability of being occupied by the laser point cloud in the occupancy grid map as the final posture, and the global decision is updated to the final posture to complete the scan matching of the global decision, which specifically includes: step 41: using the posture of the robot in the k+1th frame of the global decision as the initial posture of the scan matching; step 42: transforming the coordinates of the k+1th frame of the laser point cloud in the initial posture from the laser coordinate system to the map coordinate system; step 43: through the search window centered on the initial posture of the scan matching, obtaining the posture with the highest probability of being occupied by the k+1th frame of the occupancy grid map as the final posture, updating the posture of the k+1th frame of the global decision robot to the final posture, and completing the scan matching of the k+1th frame posture of the global decision robot. Compared with the traditional scanning matching algorithm, the present invention is based on the decision fusion rule. When the robot is subjected to lateral impact and slides sideways, the external sensing sensor is selected to assist the internal sensing sensor. This makes the scanning matching algorithm of the present invention more impact-resistant and has a better scanning matching effect, thereby effectively improving the positioning accuracy of the robot.

[0014] Further, the size of the search window used in step 43 is determined by the following steps: calculating the robot posture covariance matrix; obtaining the robot position error ellipse according to the robot posture covariance matrix; judging whether the main axis length of the robot position error ellipse is greater than the preset search window size initial value; if the main axis length of the robot position error ellipse is greater than the preset search window size initial value, the main axis length of the robot position error ellipse is used as the size of the search window; if the main axis length of the robot position error ellipse is less than or equal to the preset search window size initial value, the preset search window size initial value is used as the size of the search window. In the present invention, the size of the search window is adaptively adjusted according to the robot posture covariance matrix, so that the size of the search window is adaptively adjusted to the size that is most suitable for the current situation of the robot.

[0015] Furthermore, after a global decision scan match is completed, the robot pose covariance matrix is ​​reset to a preset robot pose covariance matrix initial value, and the size of the search window is reset to a preset search window size initial value. The present invention resets the size of the search window to an initial value after each global decision scan match to ensure that the results of the scan match of global decisions of different frames are not affected by changes in the search window size.

[0016] The present invention also discloses an anti-shock scanning matching system based on multi-sensor decision fusion, wherein the anti-shock scanning matching system based on multi-sensor decision fusion executes the anti-shock scanning matching method based on multi-sensor decision fusion as described above, and the anti-shock scanning matching system based on multi-sensor decision fusion includes: a wheel encoder for detecting the displacement information of the robot wheel; a three-axis gyroscope for detecting the angular velocity information of the robot; a three-axis accelerometer for detecting the acceleration information of the robot; a laser radar for acquiring laser point cloud data; a distributed detection system, including a decision maker one and a decision maker two, for generating one or more local decisions based on the measurement values ​​of multiple sensors and transmitting them to a decision fusion system; a decision fusion system for receiving one or more local decisions transmitted by the distributed detection system, and generating a global decision in combination with decision fusion rules; a scanning matching unit for scanning and matching in an occupancy grid map through a search window based on the global decision generated by the decision fusion system to obtain a scanning posture with the highest probability that the occupancy grid map is occupied by the laser point cloud. The invention discloses an impact-resistant scanning matching system based on multi-sensor decision fusion. A distributed detection system based on the measurement values ​​of external sensing sensors and internal sensing sensors generates multiple local decisions. The decision fusion system generates a global decision based on decision fusion rules and local decisions, thereby realizing the impact resistance of the scanning matching system and improving the accuracy of the scanning matching system. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a flowchart of the shock-resistant scan matching method based on multi-sensor decision fusion described in the first embodiment of the present invention.

[0018] Figure 2 It is a detailed flowchart of the shock-resistant scan matching method based on multi-sensor decision fusion described in the second embodiment of the present invention.

[0019] Figure 3 This is a schematic diagram of the structure of the kinematic model based on the wheel encoder according to the second embodiment of the present invention.

[0020] Figure 4 A schematic diagram of the structure of a shock-resistant scan matching system based on multi-sensor decision fusion according to an embodiment of the present invention. DETAILED DESCRIPTION

[0021] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention will be described and illustrated in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described below are only used to explain the present invention and are not used to limit the present invention. In addition, it can also be understood that for ordinary technicians in the field, some changes such as design, manufacturing or production on the technical content disclosed in the present invention are just conventional technical means, and should not be understood as the content disclosed in this application is insufficient.

[0022] Unless otherwise defined, the technical terms or scientific terms involved in the present invention should be understood by people with ordinary skills in the technical field to which this application belongs. The words "one", "a", "a kind of", "the" and the like involved in this application do not represent quantitative restrictions and may represent the singular or plural. The terms "include", "comprise", "have" and any of their variations involved in this application are intended to cover non-exclusive inclusions, such as: a process, method, system product or device comprising a series of steps or modules is not limited to the listed steps or units, but may also include steps or modules that are not listed, or may also include other steps or units inherent to these processes, methods, products or devices. The terms "first", "second", "third" and the like involved in this application are merely used to distinguish similar correspondences and do not represent a specific ordering of objects.

[0023] In a first embodiment of the present invention, a shock-resistant scan matching method based on multi-sensor decision fusion is provided. Figure 1 is a flow chart of the shock-resistant scan matching method based on multi-sensor decision fusion, such as Figure 1 As shown, the shock-resistant scan matching method based on multi-sensor decision fusion includes the following steps:

[0024] Step 1: Generate a local first decision by fusing the information of the wheel encoder and the information of the gyroscope; specifically, the information of the wheel encoder reflects the position change of the robot wheel, and the information of the gyroscope reflects the change of the robot posture; the local first decision refers to the decision generated by fusing the information of the wheel encoder and the information of the gyroscope to obtain the posture corresponding to the decision. This step obtains the decision by fusing the measurement values ​​of the internal sensing sensor. Since the wheel encoder cannot well detect the lateral displacement of the robot wheel, the local first decision has limitations when the robot is subjected to lateral impact, which reduces the accuracy of scan matching.

[0025] Step 2: Generate a local second decision by fusing the information of the accelerometer and the information of the gyroscope; specifically, the information of the accelerometer reflects the acceleration change of the robot. This step obtains the local second decision by fusing the measurement values ​​of the external sensing sensor and the gyroscope. After many experiments, it is found that the measurement value of the accelerometer changes dramatically when the robot is subjected to a lateral impact. By detecting the measurement value of the accelerometer, the problem of lateral impact on the robot wheel can be better detected, which solves the problem of limitations of the local first decision.

[0026] Step 3: Select one of the local first decision and the local second decision as the global decision according to the decision fusion rule; specifically, the decision fusion rule is a rule for selecting a local decision from the local first decision and the local second decision as the global decision according to the state of the robot; preferably, the decision fusion rule can be but is not limited to judging whether the robot is subjected to a lateral impact and then selecting a decision based on the judgment result. The global decision generated under this decision fusion rule makes the scan matching algorithm impact-resistant and reduces the impact of the lateral impact on the scan matching result caused by the robot being subjected to a lateral impact.

[0027] Step 4: Use the global decision as the initial pose of scan matching, perform scan matching in combination with the search window, obtain the pose with the highest probability of the occupancy grid map being occupied by the laser point cloud as the final pose, update the global decision to the final pose, and complete the scan matching of the global decision. The search window is centered on the initial pose of the scan matching.

[0028] Preferably, the step of selecting the global decision in step 3 specifically includes: obtaining a sensor measurement value sequence based on the preprocessing result of the original data; extracting one or more feature subsequences of a preset number of frames from the sensor measurement value sequence; classifying the extracted one or more feature subsequences of a preset number of frames according to features; selecting a local first decision or a local second decision as the global decision based on the classification result combined with a decision fusion rule; wherein the decision fusion rule may be, but is not limited to, a rule for selecting a local decision as the global decision based on determining whether the robot is subjected to a lateral impact; the number of frames of the feature subsequence may be adjusted and configured according to actual needs.

[0029] In a second embodiment of the present invention, another anti-shock scan matching method based on multi-sensor decision fusion is provided. Figure 2 This is a flowchart of the specific steps of the shock-resistant scan matching method based on multi-sensor decision fusion described in the second embodiment, as shown in FIG. Figure 2 As shown, the shock-resistant scan matching method based on multi-sensor decision fusion includes the following steps:

[0030] Step 101: Cache the original data and determine whether two or more frames of the original data have been cached. If so, proceed to step 102; specifically, the original data includes wheel encoder measurement values, gyroscope measurement values, acceleration measurement values ​​and time; the time refers to the collection moment corresponding to each frame of measurement values ​​of each sensor measurement value.

[0031] Step 102: preprocess the cached raw data, and then enter step 103; specifically, the preprocessing of the raw data includes: calculating the difference between the wheel encoder measurement values ​​of two frames cached successively, calculating the posture change Δθ of the robot of two frames cached successively, calculating the acquisition time difference Δt of the sensor measurement values ​​of two frames cached successively, calculating the mean and standard deviation of the acceleration measurement values ​​in the sliding window, and storing the sensor measurement values ​​in the order of the acquisition time to generate a sensor measurement value sequence; the sensor measurement value sequence includes an acceleration measurement value sequence, a gyroscope measurement value sequence and a wheel encoder measurement value sequence. The posture change Δθ of the robot of two frames cached successively, the acquisition time difference Δt of the sensor measurement values ​​of two frames cached successively, the mean and standard deviation of the acceleration measurement values ​​in the sliding window and the sensor measurement value sequence are taken as the preprocessing results of the raw data.

[0032] Step 103: Determine whether the wheel encoder measurement value has changed, if not, proceed to step 104, if yes, proceed to step 106; specifically, the determination of whether the wheel encoder measurement value has changed is based on the difference between two frames of wheel encoder measurement values ​​cached successively in the preprocessing results of the original data. If the difference between two consecutive frames of wheel encoder measurement values ​​is not zero, it means that the wheel encoder measurement value has changed between the two frames; if the difference between two consecutive frames of wheel encoder measurement values ​​is zero, it means that the wheel encoder measurement value has not changed between the two frames.

[0033] Step 104: Determine whether the standard deviation of the acceleration measurement value in the sliding window is less than a preset acceleration standard deviation threshold value. If so, proceed to step 105; if not, proceed to step 106; wherein the sliding window refers to a window for extracting the latest cached continuous preset number of sensor measurement values ​​based on the sliding window technology; the acceleration measurement value in the sliding window refers to the latest cached preset number of acceleration measurement values ​​extracted from the acceleration measurement value sequence based on the sliding window technology; the preset acceleration standard deviation threshold value is a value determined by the inventor after multiple experiments to limit the reasonable variation range of the acceleration measurement value; the reasonable variation range refers to the acceleration measurement value calculated by the acceleration measurement when the robot is in a stationary state. The amplitude of change reflected by the acceleration standard deviation threshold of the measurement value; specifically, this step is to judge whether the amplitude of change of the acceleration measurement value in the sliding window is within a reasonable range of change by comparing whether the standard deviation of the acceleration measurement value in the sliding window in the original data preprocessing result is less than the preset acceleration standard deviation threshold. If the standard deviation of the acceleration measurement value in the sliding window is less than the preset acceleration standard deviation threshold, it means that the amplitude of change of the acceleration measurement value of several consecutive frames in the sliding window is within a reasonable range of change. Conversely, if the standard deviation of the acceleration measurement value in the sliding window is greater than or equal to the preset acceleration standard deviation threshold, it means that the amplitude of change of the acceleration measurement value of several consecutive frames in the sliding window is not within a reasonable range of change.

[0034] Step 105: Determine that the robot is in a stationary state, update the bias of the acceleration measurement value to the average of the acceleration measurement values ​​in the sliding window, and record the acceleration measurement values ​​of all consecutive time frames cached successively when the robot is in a stationary state as prior knowledge; specifically, since the accelerometer has characteristics such as temperature sensitivity and the measurement cumulative error is large, the bias of the acceleration measurement value is updated to achieve correction of the acceleration measurement value and reduce the impact of the error; due to the spatial complexity of the algorithm and the differences between individual sensors, only the acceleration measurement value when the robot is in a stationary state is stored as prior knowledge.

[0035] Preferably, when it is detected that the robot is in a stationary state again after being started, the prior knowledge is updated to the acceleration measurement values ​​of all consecutive time frames that are cached successively when the robot is in a stationary state again.

[0036] Step 106: Determine that the robot is in a non-stationary state, and update the acceleration measurement value bias b to satisfy b k+1 =b k , where k is the serial number of the cache frame of the original data; it can be understood that when the robot is in a non-stationary state, the updated bias value b of the acceleration measurement value is k+1 and the bias value b before updating k same.

[0037] Step 201: Obtain the robot posture change Δθ1 from the kth frame to the k+1th frame, and obtain the robot posture θ of the k+1th frame (k+1)1 ,θ (k+1)1 =θ k1 +Δθ1; specifically, the posture change Δθ1 of the robot from the kth frame to the k+1th frame is a preprocessing result obtained by integrating the gyroscope measurement values ​​from the kth frame to the k+1th frame in the original data.

[0038] Step 202: According to the robot's k-th frame posture information θ k1 , defines the first coordinate transformation matrix R of the robot in the kth frame on the robot's moving plane k1 Specifically, the first coordinate transformation matrix R of the kth frame k1 The expression is

[0039] Step 203: Based on the wheel diameter of the robot and the difference in wheel encoder measurement values ​​between the kth frame and the k+1th frame, obtain the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel; specifically, Figure 3 This is a schematic diagram of the kinematic model structure based on the wheel encoder, such as Figure 3 As shown, the robot left wheel arc length ΔL refers to the arc length of a point on the robot left wheel from the kth frame to the k+1th frame; the robot right wheel arc length ΔR refers to the arc length of a point on the robot right wheel from the kth frame to the k+1th frame; specifically, in this step, the robot left and right wheel rotation arc lengths are calculated based on the robot wheel diameter and the robot wheel encoder measurement value, which belongs to the field of track calculation in robot positioning technology. The specific calculation method for obtaining the robot left wheel arc length ΔL and the robot right wheel arc length ΔR can be but is not limited to obtaining the robot left and right wheel rotation speeds based on the wheel encoder, combining the robot wheel diameter to obtain the robot travel speed, and then obtaining the robot left wheel arc length ΔL and the right wheel arc length ΔR based on the robot travel speed and the robot travel time.

[0040] Step 204: Based on the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel, obtain the robot's X-axis coordinate relative displacement ΔX1 from the kth frame to the k+1th frame and the robot's Y-axis relative displacement ΔY1 from the kth frame to the k+1th frame; specifically, Figure 3 As shown, ΔCenter refers to the length of the moving trajectory of the center of the robot (defined as the midpoint of the line connecting the left wheel and the right wheel of the robot) between the kth frame and the k+1th frame.

[0041] The step 204 specifically includes: calculating half of the sum of the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel, and confirming the calculation result as the length of the moving trajectory of the center of the robot from the kth frame to the k+1th frame; when the angle of the central angle of the robot's rotation from the kth frame to the k+1th frame is less than or equal to the preset central angle threshold, the length of the moving trajectory of the center of the robot from the kth frame to the k+1th frame is equal to the relative displacement ΔY1 of the robot in the Y-axis direction from the kth frame to the k+1th frame; calculating the absolute value of the difference between the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel; calculating the straight-line distance between the robot's left wheel and the robot's right wheel; the central angle of the robot's rotation from the kth frame to the k+1th frame is equal to the difference between the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel. The ratio of the absolute value to the straight-line distance between the left wheel and the right wheel of the robot; according to the Pythagorean theorem, the relative displacement ΔX1 of the robot in the X-axis direction from the kth frame to the k+1th frame is equal to the product of the relative displacement ΔY1 of the robot in the Y-axis direction from the kth frame to the k+1th frame and the tangent value of the central angle of half of the rotation of the robot from the kth frame to the k+1th frame; wherein, the center of the robot refers to the midpoint of the line connecting the left wheel of the robot and the right wheel of the robot; the preset central angle threshold is an angle set by the inventor based on the approximate conditions of the actual central angle. When the central angle of the robot's rotation is less than or equal to the preset central angle threshold, the central angle of the robot's rotation can be regarded as a minimum value, thereby determining that the central angle of the robot's rotation is equal to the relative displacement ΔY1 of the robot in the Y-axis direction. Figure 3 As shown, the center of the circle is a position at a preset distance from the center of the robot on the extension line of the line connecting the center of the left wheel of the robot and the center of the right wheel of the robot. The central angle of the circle refers to the angle formed by the line connecting the center of the robot's previous position and the center of the circle and the line connecting the center of the robot's current position and the center of the circle.

[0042] Step 205: According to the first coordinate transformation matrix R k1 , the X-axis relative displacement ΔX1 and the Y-axis relative displacement ΔY1, obtain the robot position of the k+1th frame in, is the robot position in the kth frame.

[0043] Step 206: The robot posture information (X (k+1)1 ,Y (k+1) ,θ (k+1)1 ) is generated as a local first decision; where k is an integer greater than or equal to 1.

[0044] Preferably, step 201 to step 206 are a specific process of generating a local first decision based on an internal sensor.

[0045] Step 301: Obtain the robot's posture change Δθ2 from the kth frame to the k+1th frame, and obtain the robot's posture θ in the k+1th frame (k+1)2 ,θ (k+1)2 =θ k2 +Δθ2; where θ k2 is the posture of the robot in the kth frame.

[0046] Step 302: Define the robot Euler angles as follows: relative to the robot body coordinate system, the rotation around the Z axis is the yaw angle α, the rotation around the Y axis is the pitch angle β, and the rotation around the X axis is the roll angle γ. Based on the robot Euler angles, define the second coordinate transformation matrix R of the robot in the kth frame in the indoor space where the robot moves. k2 Specifically, the second coordinate transformation matrix R of the kth frame k2 The expression is

[0047]

[0048] Step 303: Based on the acceleration measurement value a of the kth frame in the original data k , the acceleration measurement value a of the k+1th frame k+1 , the acceleration measurement value bias b of the kth frame k , the acceleration measurement value bias b of the k+1th frame k+1 , the second coordinate transformation matrix R of the kth frame k2 and the second coordinate transformation matrix R of the k+1th frame (k+1)2 , calculate the midpoint acceleration a of the robot at the kth frame and the k+1th frame mid Specifically, step 303 includes: calculating the robot's k-th frame acceleration measurement value a k The acceleration measurement value offset b of the robot at the kth frame k The difference is recorded as the robot's k-th frame acceleration correction measurement value; calculate the robot's k+1-th frame acceleration measurement value a k+1 The acceleration measurement value offset b of the robot at the k+1th frame k+1 The difference is recorded as the acceleration correction measurement value of the robot in the k+1th frame; the second coordinate transformation matrix R of the robot in the kth frame is calculated. k2 The product of the robot's k-th frame acceleration correction measurement value is recorded as the robot's k-th frame acceleration transformation measurement value; calculate the robot's k+1-th frame second coordinate transformation matrix R (k+1)2 The product of the acceleration correction measurement value of the robot's k+1th frame is recorded as the robot's k+1th frame acceleration transformation measurement value; the midpoint acceleration of the robot at the kth frame moment and the robot's k+1th frame moment is equal to half of the sum of the robot's kth frame acceleration transformation measurement value and the robot's k+1th frame acceleration transformation measurement value.

[0049] Step 304: Obtain the robot velocity V of the k+1th frame according to the kinematic velocity equation k+1 , V k+1 =V k +a·Δt, where V k is the robot speed at the kth frame, and acceleration a is the midpoint acceleration a between the robot at the kth frame and the robot at the k+1th frame. mid and the gravitational acceleration g, Δt is the difference in the acquisition time of the sensor measurement values ​​between the kth frame and the k+1th frame.

[0050] Step 305: Based on the robot velocity of the k+1th frame obtained in step 304, integrate the robot velocity of the k+1th frame once to obtain the robot position P of the k+1th frame k+1 , Among them, P k+1 represents the robot's position in the k+1th frame (X (k+1)2 , Y (k+1)2 , Z (k+1)2 ), P k Represents the robot's position information in the kth frame (X k2 , Y k2 , Z k2 ); Specifically, according to the knowledge of calculus, the robot position can be obtained by integrating the robot speed once. In this technical solution, the robot speed of the k+1th frame obtained in step 304 is integrated once to obtain the robot position of the k+1th frame.

[0051] Step 306: The robot posture information (X (k+1)2 ,Y (k+1)2 ,θ (k+1)2 ) is generated as a local second decision.

[0052] Preferably, steps 301 to 306 are specific processes for generating a local second decision based on an external sensing sensor assisting an internal sensing sensor. The specific process for generating a local first decision and the specific process for generating a local second decision may be, but are not limited to, two processes being performed simultaneously or one after the other; the order in which the two processes are performed may be to first perform the specific process for generating a local first decision, or to first perform the specific process for generating a local second decision.

[0053] Step 401: Acquire a sensor measurement value sequence; wherein the sensor measurement value sequence includes: a wheel encoder measurement value sequence, an acceleration measurement value sequence and a gyroscope measurement value sequence.

[0054] Step 402: The acceleration measurement values ​​of all continuous time frames of the robot in the static state in the prior knowledge are taken as the first population, and the acceleration measurement values ​​of the preset number of frames of the latest cache are extracted from the sensor measurement value sequence based on a sliding window of a preset size as the first sample, and the Mahalanobis distance between the first sample and the first population is calculated, and it is determined whether the Mahalanobis distance between the first sample and the first population is greater than the preset Mahalanobis distance threshold. If so, proceed to step 403, and if not, proceed to step 405; wherein the size of the sliding window can be regarded as the duration of the robot state, and as an adjustable parameter directly affects the result of decision fusion; the Mahalanobis distance was proposed by Indian statistician Mahalanobis, and is used to represent the covariance distance of data, which takes into account the connection between various characteristics. In this step, the Mahalanobis distance is used as a distance metric to measure the similarity of acceleration time series, which can be used to determine whether the acceleration of the robot changes dramatically. The inventors determined the preset size of the sliding window through multiple simulation results, and determined the Mahalanobis distance corresponding to the maximum amplitude of reasonable acceleration change when the robot is in a stationary state as the preset Mahalanobis distance threshold. The specific size of the preset size of the sliding window is determined by the preset number of frames that the inventors need to extract for the acceleration measurement value.

[0055] Step 403: extract the latest cached frame of gyroscope measurement values ​​from the gyroscope measurement value sequence as the gyroscope measurement value at the current moment, obtain the robot roll angle and the robot pitch angle at the current moment according to the gyroscope measurement value at the current moment, calculate the first Euclidean distance between the robot roll angle at the current moment and the preset robot roll angle threshold, calculate the second Euclidean distance between the robot pitch angle at the current moment and the preset robot pitch angle threshold, and determine whether the first Euclidean distance and the second Euclidean distance are both less than the preset Euclidean distance threshold. If so, proceed to step 404; if not, proceed to step 405; wherein the Euclidean distance refers to the Euclidean metric, which is used to represent the distance between two points; the gyroscope measurement value at the current moment includes the robot roll angle at the current moment, the robot pitch angle at the current moment, and the robot yaw angle at the current moment. In this step, the Euclidean distance is used as a distance metric to measure the similarity of the Euler angles, which can be used to determine whether the posture of the robot has changed drastically; the preset robot roll angle threshold and the preset robot pitch angle threshold are two angle thresholds determined by the inventor through multiple simulation results for determining whether the posture of the robot has changed significantly; the preset Euclidean distance threshold is a distance threshold determined by the inventor through multiple simulation results for determining whether the posture of the robot has changed drastically.

[0056] Step 404: Determine that the robot is subjected to a lateral impact, select the local second decision as the global decision, and then proceed to step 501;

[0057] Step 405: Determine that the robot is not subjected to lateral impact, select the local first decision as the global decision, and then go to step 501;

[0058] Specifically, step 401 to step 405 are the specific processes for the robot to select a global decision. This embodiment optimizes the local decision based on the decision fusion rule, determines the global decision according to the actual robot state, and provides a good initial pose for the scan matching algorithm to improve the positioning accuracy of the robot.

[0059] Step 501: The robot's position information of the k+1th frame of global decision is used as the initial position information of scan matching, and then proceed to step 502; specifically, according to the local decision information selected by the global decision, the robot's position information of the k+1th frame can be (X (k+1)1 ,Y (k+1) ,θ (k+1)1 ), or (X (k+1)2 ,Y (k+1)2 ,θ (k+1)2 ).

[0060] Step 502: According to the initial pose of the scan matching, the coordinates of the k+1th frame laser point cloud in the initial pose are transformed from the laser coordinate system to the map coordinate system; this step transforms the coordinates of the laser point cloud to facilitate the subsequent scan matching of the laser point cloud coordinates in the occupancy grid map.

[0061] Step 503: In the occupancy grid map, the posture with the highest probability of being occupied by the k+1th frame laser point cloud in the occupancy grid map is obtained as the final posture through a search window centered on the initial posture of the scan matching, and the posture of the k+1th frame of the robot of the global decision is updated to the final posture, and the scan matching of the k+1th frame posture of the robot of the global decision is completed; wherein, each grid point in the occupancy grid map has a one-to-one corresponding probability of being occupied; the scan matching in this step is mainly performed by projecting the laser point cloud onto the occupancy grid map, traversing all possible postures through a search window centered on the initial posture, and finding the posture with the highest degree of overlap between the laser point cloud and the map (i.e., the posture with the highest probability of being occupied), as the final posture of the initial posture, so as to achieve the purpose of improving the positioning accuracy of the robot after scan matching, and updating the posture of the k+1th frame of the robot of the global decision to the final posture, thereby improving the accuracy and credibility of the k+1th posture of the robot and reducing the positioning robustness of the robot.

[0062] Preferably, the size of the search window is adaptively adjustable. A third embodiment of the present invention provides a shock-resistant scan matching method based on multi-sensor decision fusion on the basis of the above-mentioned embodiment. The size of the search window used in the scan matching in the algorithm is adaptively adjusted by the following steps:

[0063] Step S1: Calculate the robot posture covariance matrix, and then enter step S2; wherein, the calculation method of the robot posture covariance matrix can be but is not limited to calculation based on the Jacobian matrix of the state equation with respect to the state vector, the Jacobian matrix of the state equation with respect to the control vector and the control vector noise covariance matrix.

[0064] Step S2: Obtain the robot position error ellipse according to the robot posture covariance matrix, and then proceed to step S3; specifically, the method for obtaining the robot position error ellipse can be but is not limited to obtaining eigenvectors and eigenvalues ​​based on the robot posture covariance transformation, determining the maximum posture propagation direction based on the eigenvector, and determining the propagation breadth based on the eigenvalue, wherein the major axis length of the robot position error ellipse is determined by the eigenvalue; the specific method for obtaining the robot position error ellipse based on the robot posture covariance matrix can also be to generate the robot position error ellipse by drawing based on the robot posture covariance matrix through programming language software such as C++ or Matlab.

[0065] Step S3: Determine whether the major axis length of the robot position error ellipse is greater than the preset search window size initial value, if so, proceed to step S41, if not, proceed to step S42; specifically, since the robot posture covariance matrix directly affects the size of the search window and indirectly affects the effect of the scan matching algorithm, the inventors change the initial value of the robot covariance matrix, and after multiple calculations of the scan matching results of the simulation experiments, determine the most suitable initial value of the robot posture covariance matrix, and then determine the most suitable preset search window size initial value as the preset search window size initial value.

[0066] Step S41: If the major axis length of the robot position error ellipse is greater than the preset search window initial value, the major axis length of the robot position error ellipse is used as the size of the search window;

[0067] Step S42: If the length of the major axis of the robot position error ellipse is less than or equal to the preset search window size initial value, the preset search window size initial value is used as the search window size.

[0068] In this embodiment, the search window is adaptively adjusted according to the robot posture covariance matrix, so that the size of the search window can change according to the actual robot posture, thereby improving the flexibility of adjusting the search window size.

[0069] Preferably, after a global decision scan matching is completed, the robot pose covariance matrix is ​​reset to a preset robot pose covariance matrix initial value, and the size of the search window is reset to a preset search window size initial value.

[0070] A fourth embodiment of the present invention provides a shock-resistant scan matching system based on multi-sensor decision fusion, which executes the shock-resistant scan matching method based on multi-sensor decision fusion as described in any one of the above embodiments, such as Figure 4 As shown, the system includes: a wheel encoder for detecting the rotation speed and displacement information of the robot wheel; a gyroscope for collecting the robot angular velocity information; an accelerometer for collecting the robot acceleration information; a laser radar for collecting laser point cloud data; a data processing unit for processing the measurement values ​​of sensors such as the wheel encoder, the gyroscope, and the accelerometer; a distributed detection unit, including a decision maker 1 and a decision maker 2, for generating one or more local decisions based on the data processing results of the data processing unit and transmitting them to a decision fusion system; a decision fusion unit for receiving one or more local decisions transmitted by the distributed detection unit, and generating a global decision in combination with a decision fusion rule; a scanning matching unit for scanning and matching the global decision generated by the decision fusion system in the occupancy grid map through a search window to obtain the posture with the highest probability of the occupancy grid map being occupied by the laser point cloud.

[0071] In the embodiments provided by the present invention, it should be understood that the disclosed systems and methods can be implemented in other ways. For example, the system embodiments described above are merely schematic, for example, the division of the units is merely a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. The methods described above are merely schematic, for example: the division of the steps can be otherwise divided in actual implementation, such as the order of execution of the steps can be adjusted according to actual conditions, or some steps can be ignored or not executed.

[0072] A person of ordinary skill in the art can understand that all or part of the steps carried by the method of the above-mentioned embodiment can be completed by instructing the relevant hardware through a program, and the program can be stored in a computer-readable storage medium, and the program, when executed, includes one or a combination of the steps of the method embodiment. In addition, each functional unit in each embodiment of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into one module. The above-mentioned integrated module can be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. The above-mentioned storage medium can be a read-only memory, a disk or an optical disk, etc.

[0073] The above embodiments only express several embodiments of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application.

Claims

1. A shock-resistant scan matching method based on multi-sensor decision fusion, characterized in that: The shock-resistant scan matching method based on multi-sensor decision fusion comprises the following steps: Step 1: Generate a local first decision by fusing the information of the wheel encoder and the information of the gyroscope; Step 2: Generate a local second decision by fusing the information of the accelerometer and the information of the gyroscope; Step 3: Select one of the local first decision and the local second decision as the global decision; Step 4: Use the global decision as the initial pose for scan matching, perform scan matching in combination with the search window, obtain the pose with the highest probability of the occupancy grid map being occupied by the laser point cloud as the final pose, update the global decision to the final pose, and complete the scan matching of the global decision; Among them, step 3 selects one of the local first decision and the local second decision as the global decision, including: determining whether the robot is subjected to a lateral impact, if it is determined that the robot is subjected to a lateral impact, then selecting the local second decision as the global decision; if it is determined that the robot is not subjected to a lateral impact, then selecting the local first decision as the global decision.

2. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 1 is characterized in that: The anti-shock scan matching method based on multi-sensor decision fusion further includes, before executing step 1: Step 00: Cache the original data and determine whether the cached frames of the original data have been cached for two frames or more. If so, proceed to step 01; Step 01: Preprocess the cached raw data to obtain the preprocessing result of the raw data; Step 02: Determine whether the wheel encoder measurement value has changed. If not, proceed to step 03. If yes, determine that the robot is in a non-stationary state and proceed to step 05. Step 03: Determine whether the standard deviation of the acceleration measurement value in the sliding window is less than the preset acceleration standard deviation threshold. If so, determine that the robot is in a stationary state and proceed to step 04. If not, determine that the robot is in a non-stationary state and proceed to step 05. Step 04: Update the bias of the acceleration measurement value to the average value of the acceleration measurement value in the sliding window, and record all the acceleration measurement values ​​cached successively when the robot is in a stationary state as prior knowledge; Step 05: The robot is in a non-stationary state, and the update of the offset b of the acceleration measurement value between two consecutive cached frames satisfies b k+1 =b k , where k is the sequence number of the cache frame of the original data; The raw data includes wheel encoder measurement values, gyroscope measurement values, acceleration measurement values ​​and the acquisition time corresponding to each frame of each sensor measurement value; the preprocessing of the cached raw data includes: calculating the difference between two frames of wheel encoder measurement values ​​cached successively, calculating the posture change Δθ of two consecutive frames of the robot, calculating the acquisition time difference Δt of two consecutive frames of sensor measurement values ​​cached successively, calculating the mean and standard deviation of the acceleration measurement values ​​in the sliding window, and storing the sensor measurement values ​​in sequence according to the collection time sequence to generate a sensor measurement value sequence; the posture change Δθ of two consecutive frames of the robot, the acquisition time difference Δt of two consecutive frames of sensor measurement values ​​cached successively, the mean and standard deviation of the acceleration measurement values ​​in the sliding window, and the sensor measurement value sequence are used as the preprocessing results of the raw data; the sensor measurement value sequence includes the acceleration measurement value sequence, the gyroscope measurement value sequence and the wheel encoder measurement value sequence; the acceleration measurement value in the sliding window refers to the latest cached acceleration measurement value of a preset number of frames extracted from the acceleration measurement value sequence based on the sliding window technology.

3. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 2 is characterized in that: When it is detected that the robot is in a stationary state again after being started, the prior knowledge is updated to the acceleration measurement values ​​of all consecutive time frames cached successively when the robot is in a stationary state again.

4. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 2 is characterized in that: The step 1 specifically includes: Step 11: Obtain the robot's posture change Δθ1 from the kth frame to the k+1th frame, and obtain the robot's posture θ of the k+1th frame (k+1)1 ,θ (k+1)1 =θ k1 +Δθ1; Step 12: Define the first coordinate transformation matrix R of the robot in the kth frame on the robot's travel plane k1 ; Step 13: Based on the wheel diameter of the robot and the difference between the wheel encoder measurement value of the kth frame and the wheel encoder measurement value of the k+1th frame, the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel are obtained; Step 14: Based on the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel obtained in step 13, obtain the relative displacement ΔX1 of the robot in the X-axis direction from the k-th frame to the k+1-th frame and the relative displacement ΔY1 of the robot in the Y-axis direction from the k-th frame to the k+1-th frame; Step 15: Get the robot position at the k+1th frame in, is the robot position in the kth frame; Step 16: The robot pose information (X (k+1)1 ,Y (k+1)1 ,θ (k+1)1 ) is generated as a local first decision; Among them, the arc length of the robot's left wheel refers to the arc length that a point on the robot's left wheel rotates from the kth frame to the k+1th frame; the arc length of the robot's right wheel refers to the arc length that a point on the robot's right wheel rotates from the kth frame to the k+1th frame; the frame number k is an integer greater than or equal to 1.

5. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 4 is characterized in that: The step 14 specifically includes: Calculate half of the sum of the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel, and confirm the calculation result as the moving trajectory length of the robot's center from the kth frame to the k+1th frame; When the angle of the center angle of the robot's rotation from the kth frame to the k+1th frame is less than or equal to the preset center angle threshold, the length of the moving trajectory of the robot's center from the kth frame to the k+1th frame is equal to the relative displacement ΔY1 of the robot in the Y-axis direction from the kth frame to the k+1th frame; Calculate the absolute value of the difference between the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel; Calculate the straight-line distance between the robot's left wheel and the robot's right wheel; The central angle of the robot's rotation from the kth frame to the k+1th frame is equal to the ratio of the absolute value of the difference between the arc length ΔL of the robot's left wheel and the arc length ΔR of the robot's right wheel to the straight-line distance between the robot's left wheel and the robot's right wheel; According to the Pythagorean theorem, the relative displacement ΔX1 of the robot in the X-axis direction from the kth frame to the k+1th frame is equal to the product of the relative displacement ΔY1 of the robot in the Y-axis direction from the kth frame to the k+1th frame and the tangent value of half of the central angle of the robot from the kth frame to the k+1th frame; The center of the robot refers to the midpoint of the line connecting the left wheel of the robot and the right wheel of the robot.

6. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 2 is characterized in that: The step 2 specifically includes: Step 21: Obtain the robot's posture change Δθ2 from the kth frame to the k+1th frame, and obtain the robot's posture θ at the k+1th frame (k+1)2 ,θ (k+1)2 =θ k2 +Δθ2; Step 22: Define the robot Euler angles as follows: relative to the robot body coordinate system, the rotation around the Z axis is the yaw angle α, the rotation around the Y axis is the pitch angle β, and the rotation around the X axis is the roll angle γ. Based on the robot Euler angles, define the second coordinate transformation matrix R of the robot in the kth frame in the space where it moves k2 ; Step 23: According to the acceleration measurement value a of the kth frame in the original data k , the acceleration measurement value a of the k+1th frame k+1 , the acceleration measurement value bias b of the kth frame k , the acceleration measurement value bias b of the k+1th frame k+1 , the second coordinate transformation matrix R of the kth frame k2 and the second coordinate transformation matrix R of the k+1th frame (k+1)2 , get the midpoint acceleration a of the robot at the kth frame and the k+1th frame mid ; Step 24: Get the robot speed V at the k+1th frame k+1 , V k+1 =V k +a·Δt, where acceleration a is the midpoint acceleration a between the robot at the kth frame and the robot at the k+1th frame. mid The sum of the gravitational acceleration g; Step 25: According to the robot velocity propagation equation of the k+1th frame obtained in step 24, integrate the robot velocity of the k+1th frame once to obtain the robot position P of the k+1th frame k+1 , Among them, P k+1 represents the robot's position in the k+1th frame (X (k+1)2 , Y (k+1)2 , Z (k+1)2 ), P k Represents the robot's position information in the kth frame (X k2 , Y k2 , Z k2 ); Step 26: The robot pose information (X (k+1)2 ,Y (k+1)2 ,θ (k+1)2 ) is generated as a local second decision.

7. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 6 is characterized in that: The step 23 specifically includes: Calculate the robot's acceleration measurement value a at the kth frame k The acceleration measurement value offset b of the robot at the kth frame k The difference is recorded as the robot's k-th frame acceleration correction measurement value; Calculate the robot's acceleration measurement value a at the k+1th frame k+1 The acceleration measurement value offset b of the robot at the k+1th frame k+1 The difference is recorded as the robot's acceleration correction measurement value for the k+1th frame; Calculate the second coordinate transformation matrix R of the robot in the kth frame k2 The product of the k-th frame acceleration correction measurement value of the robot is recorded as the k-th frame acceleration transformation measurement value of the robot; Calculate the second coordinate transformation matrix R of the robot in the k+1th frame (k+1)2 The product of the acceleration correction measurement value of the robot at the k+1th frame is recorded as the acceleration transformation measurement value of the robot at the k+1th frame; The midpoint acceleration of the robot at the kth frame moment and the robot at the k+1th frame moment is equal to half of the sum of the acceleration transformation measurement value of the robot at the kth frame and the acceleration transformation measurement value of the robot at the k+1th frame.

8. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 2 is characterized in that: The step 3 specifically includes: Step 31: Obtain the sensor measurement value sequence, and then proceed to step 32; Step 32: taking the acceleration measurement values ​​of all consecutive time frames when the robot is in a stationary state in the prior knowledge as the first population, extracting the acceleration measurement values ​​of the preset number of frames cached most recently from the acceleration measurement value sequence based on a sliding window of a preset size as the first sample, calculating the Mahalanobis distance between the first sample and the first population, and judging whether the Mahalanobis distance between the first sample and the first population is greater than a preset Mahalanobis distance threshold, if so, proceeding to step 33, if not, determining that the robot is not subjected to a lateral impact, and proceeding to step 35; Step 33: extract the latest cached frame of gyroscope measurement values ​​from the gyroscope measurement value sequence as the gyroscope measurement value at the current moment, obtain the robot roll angle and the robot pitch angle at the current moment according to the gyroscope measurement value at the current moment, calculate the first Euclidean distance between the robot roll angle at the current moment and the preset robot roll angle threshold, calculate the second Euclidean distance between the robot pitch angle at the current moment and the preset robot pitch angle threshold, and determine whether the first Euclidean distance and the second Euclidean distance are both less than the preset Euclidean distance threshold. If so, it is determined that the robot is subjected to a lateral impact, and the process goes to step 34. If not, it is determined that the robot is not subjected to a lateral impact, and the process goes to step 35. Step 34: Select the local second decision as the global decision; Step 35: Select the local first decision as the global decision; The preset size of the sliding window is determined by the preset number of frames extracted from the acceleration measurement values.

9. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 2 is characterized in that: In step 4, the global decision is used as the initial pose of the scan matching, and the scan matching is performed in combination with the search window to obtain the pose with the highest probability of being occupied by the laser point cloud in the occupancy grid map as the final pose, and the global decision is updated to the final pose to complete the scan matching of the global decision, which specifically includes: Step 41: The robot's position in the k+1th frame of global decision is used as the initial position of scan matching; Step 42: transform the coordinates of the k+1th frame laser point cloud in the initial pose from the laser coordinate system to the map coordinate system; Step 43: Through a search window centered on the initial pose of the scan matching, the pose with the highest probability of being occupied by the k+1th frame laser point cloud in the occupancy grid map is obtained as the final pose, and the pose of the k+1th frame of the global decision robot is updated to the final pose, completing the scan matching of the k+1th frame pose of the global decision robot.

10. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 9, characterized in that: The size of the search window used in step 43 is determined by the following steps: Calculate the robot pose covariance matrix; Obtaining a robot position error ellipse according to the robot posture covariance matrix; Determine whether the length of the major axis of the robot position error ellipse is greater than a preset search window size initial value; If the major axis length of the robot position error ellipse is greater than the preset search window initial value, the major axis length of the robot position error ellipse is used as the size of the search window; If the length of the major axis of the robot position error ellipse is less than or equal to the preset search window size initial value, the preset search window size initial value is used as the search window size.

11. The shock-resistant scan matching method based on multi-sensor decision fusion according to claim 10, characterized in that: When a global decision scan matching is completed, the robot pose covariance matrix is ​​reset to the preset robot pose covariance matrix initial value, and the search window size is reset to the preset search window size initial value.

12. A shock-resistant scan matching system based on multi-sensor decision fusion, characterized in that: The shock-resistant scan matching system based on multi-sensor decision fusion executes the shock-resistant scan matching method based on multi-sensor decision fusion according to any one of claims 1 to 11, and the shock-resistant scan matching system based on multi-sensor decision fusion comprises: Wheel encoder, used to detect the rotation speed and displacement information of the robot's wheels; Gyroscope, used to collect robot angular velocity information; Accelerometer, used to collect robot acceleration information; LiDAR, used to collect laser point cloud data; A data processing unit, used for processing the data measured by sensors such as wheel encoders, gyroscopes, and accelerometers; A distributed detection unit, including a decision maker 1 and a decision maker 2, for generating one or more local decisions based on the data processing result of the data processing unit and transmitting the local decisions to the decision fusion system; A decision fusion unit, used to receive one or more local decisions transmitted by the distributed detection units, and generate a global decision by combining the decision fusion rules; The scanning matching unit is used to scan and match the occupancy grid map through a search window based on the global decision generated by the decision fusion system to obtain the posture with the highest probability of the occupancy grid map being occupied by the laser point cloud.

Citation Information

Patent Citations

  • Pose matching method based on laser frame and probability map scanning

    CN112612034A