WO2019100354A1 - State sensing method and related apparatus - Google Patents

Abstract

A state sensing method and a related apparatus. The state sensing method comprises: acquiring point cloud data of a current frame (S101); extracting point cloud data of N historical frames from an historical point cloud library (S102), the acquisition time of each of the N historical frames being before the acquisition time of a current frame, wherein N is a positive integer greater than or equal to 1; performing point cloud registration on the point cloud data of the current frame and the point cloud data of the N historical frames to obtain N state estimate values of the point cloud data of the current frame (S103); and superimposing the N state estimate values to obtain a state estimation result of the point cloud data of the current frame (S104). The method and apparatus filter out irregular noise without requiring a loop motion, so as to obtain a more accurate state sensing result, and to improve accuracy when positioning and mapping a SLAM system.

Description

State aware method and related equipment Technical field

The present application relates to the field of positioning technologies, and in particular, to a state sensing method and related devices.

Background technique

Nowadays, with the development of sensor technology, AR/VR technology, intelligent mobile robots and driverless technology, the industry has more and more demand for positioning technology. Among them, Simultaneous Localization and Mapping (SLAM) Technology is a direction that people focus on. The problem that SLAM needs to solve can be described as that an autonomous vehicle or other autonomous mobile device starts moving from an unknown location in an unknown environment, and performs its own positioning according to the position estimation and the map during the movement, and builds on the basis of its own positioning. Quantitative maps for autonomous positioning and navigation of autonomous vehicles or other autonomous mobile devices.

For SLAM technology, error accumulation is a major technical problem. SLAM typically uses Kalman filtering and loopback detection to reduce error accumulation. Among them, Kalman filter is an algorithm that uses the linear system state equation to estimate the system state through the input and output of the system. The Kalman filter uses the probability to represent the system state. The error is positive. State distribution or other linear probability distribution. Loop detection is also called loop closure detection. When the autonomous vehicle or other autonomous mobile device performs loopback motion, that is, the starting point and the ending point are basically the same, then the Bayesian probability map can be optimized for the previously estimated path. To make the entire path estimate more accurate.

However, in practical applications, self-driving cars or other autonomous mobile devices may encounter potholes, sundries, etc. on uneven road surfaces, resulting in irregular noise in the positioning. These noises are high frequency, high energy, disorderly, and not normal. Distribution, which greatly affects SLAM's estimation of system state. The use of Kalman filter requires that the error is a normal distribution or other linear probability distribution, and it is difficult to filter irregular noise. For loop detection, the application range is limited. If the self-driving car or other autonomous mobile device does not perform loopback motion, loopback detection cannot be used to reduce the error. Because the error can not be eliminated, the error accumulates and accumulates continuously during the iterative process, which seriously affects the effect of positioning and mapping.

Summary of the invention

The embodiment of the invention provides a state sensing method and related equipment, which can filter irregular noise and improve the positioning precision of the SLAM system without requiring loopback motion.

In a first aspect, an embodiment of the present invention provides a state sensing method, where the method includes:

Collecting current frame point cloud data; extracting N historical frame point cloud data from the historical point cloud library, the collection time of the N historical frames is before the acquisition time of the current frame, where N is greater than or equal to 1 a positive integer; the current frame point cloud data and the N historical frame point cloud data are respectively subjected to point cloud registration to obtain N state estimation values of the current frame point cloud data; and the N states are The estimated values are superimposed to obtain a state estimation result of the current frame point cloud data.

When the vehicle is driving on an uneven road surface (such as encountering stones, obstacles, potholes, etc.), it causes strong jolting of the vehicle in a short period of time, which is reflected in the point cloud data that encounters irregular noise (referred to as noise). ), the noise is generally a high frequency, short time shock signal. In the embodiment of the present invention, the SLAM system disposed in the vehicle may be applied. The method for sensing the current state of the vehicle by the SLAM system is: after collecting the current frame point cloud data, the current point cloud data is compared with the previous frame point cloud data collected. The cloud is registered to obtain a plurality of state estimates, which are locations and/or poses corresponding to different historical frame point cloud data. Each state estimate from point cloud registration has a phase The reliability of the response should be low if the noise is affected, and the reliability is low if it is less affected by the noise. Then, a plurality of state estimation values are selected for superposition to obtain a current state estimation result, and the reliability of the state estimation result is relatively high, that is, the result is relatively accurate. For example, in the extracted number of historical frame point cloud data, the number of historical frames with high credibility is large, and the number of historical frames with low credibility is small. Therefore, the reliability of the result obtained after superposition is higher. . For another example, for different historical frame point cloud data, the greater the credibility is, the larger the weight value is, and the reliability of the result obtained after superposition is relatively high, that is, the noise can be greatly reduced or even eliminated. Affecting, obtaining the current accurate position and/or attitude results of the vehicle, then, it is understandable that the SLAM system will be able to perform more accurate mapping and positioning based on these results.

Based on the first aspect, in a specific implementation, the state estimation value includes an expected value and a weight value; wherein the weight value is determined by a coincidence degree value of the current frame point cloud data and the historical frame point cloud data. The weight value is represented by a linear distribution; superimposing the N state estimation values to obtain a state estimation result of the current frame point cloud data, comprising: basing the expected value of the N state estimation values based on And performing weighted superposition on the corresponding weight value to obtain a state estimation result of the current frame point cloud data.

In the embodiment of the present invention, in the point cloud registration process, the two frames of images have different degrees of coincidence based on whether there is noise and the intensity of the noise. If the two frames of images are completely accurate and have no noise, then the overlapping portions of the two frames of images are completely coincident after conventional processing such as translation, rotation, etc.; if the current frame point cloud data and/or historical frame point cloud data are noisy, Then the overlapping portions of the two frames of images cannot completely coincide. The degree of overlap of the overlapping parts of the two frames of images. Therefore, the degree of coincidence of the two frames of images will represent the credibility of the expected value of the state estimate. The higher the degree of coincidence, the higher the degree of credibility, the lower the degree of coincidence, and the more credible. low. In a specific implementation, the degree of coincidence can be quantized into a weighted value of a linear distribution such as a variance or a standard deviation or a covariance. That is, the magnitude of these variances or standard deviations or covariances determines the weight values of the corresponding state estimates. The larger the irregular noise, the smaller the weight value of the obtained state estimation value; the smaller the irregular noise, the larger the weight value of the obtained state estimation value. In this way, after the plurality of state estimation values are weighted and superimposed, the image frame affected by the noise has little influence on the final result, that is, the embodiment of the present invention can filter the noise in real time to obtain a more accurate state estimation result.

In the specific application scenario, assuming that the car encounters a bump in the ridge, the image acquired when the ridge is overlaid will be blurred, and the error of the result obtained by the point cloud registration based on these images will be large. The images acquired before the hurdle are clear, and the error of the result of point cloud registration based on these images will be small. If the current frame is a post-cannon image, then the image acquired after the ridge is taken out and the previous image of several frames are respectively subjected to point cloud registration, and the weight value of the result obtained by the blurred image is small, and the weight value of the clear image is large, then After the weighted superposition of the registration results, the obtained state estimation results are more accurate.

In a specific implementation, the weight value of each estimated state value in the superposition process may be further adjusted according to the type, size, and the like of the noise. The larger the noise, the smaller the corresponding weight value is adjusted, further eliminating noise, and improving the state estimation result. Precision. It can be understood that after the state estimation result is obtained, since the noise has been greatly reduced or even eliminated in the state estimation result, the reliability of the result is high, and accurate mapping and positioning can be further realized based on the state estimation result. After the current frame point cloud data is saved to the historical point cloud database as the historical frame point cloud data, the next time the point cloud data is collected, the above process is repeated and iteratively, and the effect of eliminating noise in real time can be achieved.

It should be noted that, in the embodiment of the present invention, the mathematical form of the state estimation value may be a probability distribution estimation, a variance, a covariance, a covariance matrix, a non-probability distribution estimation, etc., and then the state estimation values are superposed. Result The mathematical form remains the same. For example, the mathematical form of the state estimate is a normal distribution (including the expected value, and the variance as the weight value), the two normal distributions can be superimposed, and the result obtained after the superposition is still a normal distribution.

Based on the first aspect, in a possible implementation, before extracting N historical frame point cloud data from the historical point cloud database, the method includes: detecting random noise; extracting N history from the historical point cloud library The frame point cloud data includes: triggering extracting N historical frame point cloud data from the historical point cloud library in the case that the irregular noise is detected.

In a possible embodiment of the present invention, the system detects whether there is irregular noise in the current data frame currently collected, and if it detects that there is irregular noise in the current data frame, it may extract some historical frame point cloud data before the current data frame. . If no irregular noise is detected in the current data frame, the historical frame point cloud data in the previous frame of the current data frame may be extracted.

In another possible embodiment of the present invention, the system detects in real time whether there is a historical frame with noise in the historical point cloud library, and if a historical point cloud library is detected, there is a historical frame (normal data) that is not affected by noise and A historical frame affected by noise may extract a number of historical frame point cloud data preceding the current data frame. If no historical point cloud inventory is detected in the random noise, the historical frame point cloud data in the previous frame of the current data frame may be extracted.

Based on the first aspect, in a possible implementation, the extracting N historical frame point cloud data from the historical point cloud database includes: determining a sampling step size according to the random noise; and calculating a history based on the sampling step N historical frame point cloud data is extracted from the point cloud library.

In a possible embodiment of the present invention, the system determines a sampling step size according to the random noise when the random noise is detected, and extracts, before the point cloud data, from the historical point cloud database based on the sampling step size. Several historical frame point cloud data. For example, the system detects that there is random noise in the k+1th frame to the nth frame in the historical point cloud library, determines the sampling step size as n frames, ignores the history frame with noise, and extracts the history before the noise. A frame (such as the first frame to the kth frame in the illustration) is used as the point cloud data set. In a special application scenario, only one historical frame that is not affected by the random noise can be extracted based on the sampling step size to partially achieve the effect of filtering out noise of different frequencies in the embodiment of the present invention. For example, in a specific application scenario, assuming that the car has encountered a bump in the ridge, the image captured during the ridge will be blurred, if the two frames before and after the ridge are extracted, because the two frames The images are all clear, so the results of point cloud registration for the steps described below will also be accurate. In this way, the error caused by the random noise when the ridge is over, will not spread to the measurement after the ridge.

Based on the first aspect, in a possible implementation, the extracting the N historical frame point cloud data from the historical point cloud database includes: detecting that the current frame point cloud data collection order is in a preset order; Extracting the point cloud data set in the point cloud library before the current frame point cloud data, including: triggering from the historical point cloud database when detecting that the current frame point cloud data collection order is in a preset order Extract N historical frame point cloud data.

In a possible embodiment of the present invention, if the system detects that the order of the current frame point cloud data conforms to the preset order, extracting a plurality of historical frame point cloud data before the current frame point cloud data. If the system detects that the order of the current frame point cloud data does not conform to the preset order, the previous frame of the current frame point cloud data may be extracted as the point cloud data set. For example, if the acquisition order of the current frame is the 5th frame, the 10th frame, the 15th frame, etc., the first 4 frames of the historical frame point cloud data of the current frame point cloud data are extracted as the point cloud data set, and if If the current frame does not meet the above periodic order, the historical frame point cloud data of the previous frame of the current data frame may be extracted as the point cloud data set.

In a second aspect, an embodiment of the present invention provides a device for state awareness, where the device includes an acquisition module, an extraction module, a registration module, and a superposition module, where:

An acquisition module, configured to collect current frame point cloud data;

And an extraction module, configured to extract N historical frame point cloud data from the historical point cloud library, where the collection time of the N historical frames is before the acquisition time of the current frame, where N is greater than or equal to 1 Integer

a registration module, configured to perform point cloud registration on the current frame point cloud data and the N historical frame point cloud data respectively, to obtain a plurality of state estimation values of the current frame point cloud data;

And a superimposing module, configured to superimpose the N state estimation values to obtain a state estimation result of the current frame point cloud data.

In a specific embodiment, the acquisition module, the extraction module, the registration module, and the overlay module can be used to implement the method of the first aspect.

In a third aspect, an embodiment of the present invention provides a device for state awareness, the device comprising: a processor, and a sensor and a memory coupled to the processor, wherein the sensor is configured to acquire a current frame point Cloud data; the memory is for storing a historical point cloud library and program code; the processor is configured to invoke the program code to perform the method of the first aspect.

In a fourth aspect, the embodiment of the present invention provides a computer readable storage medium for storing an implementation code of the method of the first aspect.

In a fifth aspect, an embodiment of the present invention provides a computer software product, which when used in a computer, can be used to implement the method described in the first aspect.

It can be seen that, in the embodiment of the present invention, the object of the SLAM system performs point cloud registration through the current frame point cloud data and several historical frame point cloud data during normal driving, and obtains several states about the current position or posture. The estimated values are superimposed on these state estimates, and the irregular noise is eliminated by the algorithm during the superposition process. In autonomous driving, the sensor sampling frequency is usually high. In the traditional Kalman filter, if the sampling frequency is high, it is susceptible to random noise, resulting in large errors in the results. By implementing the embodiment of the present invention, even if the object of the SLAM system does not perform loopback motion, the irregular noise encountered can be eliminated, the accuracy of the state estimation result is greatly improved, and the accuracy improvement is real-time (while the traditional loopback motion) Accuracy improvement is not real-time), which can further improve the accuracy of drawing and positioning in subsequent applications, which is conducive to the practical use of autopilot products and enhance the user experience.

DRAWINGS

1 is a schematic structural diagram of a system of a SLAM system according to an embodiment of the present invention;

2 is a logic block diagram of a function implementation of a SLAM system according to an embodiment of the present invention;

3 is a schematic flowchart of a state sensing method according to an embodiment of the present invention;

4 is a logic block diagram of an application state sensing method scenario according to an embodiment of the present invention;

FIG. 5 is a schematic flowchart diagram of still another state sensing method according to an embodiment of the present disclosure;

FIG. 6 is a schematic diagram of a scenario for extracting a history frame according to an embodiment of the present invention;

FIG. 7 is a schematic diagram of another scenario for extracting a history frame according to an embodiment of the present invention; FIG.

FIG. 7b is a schematic diagram of another scenario for extracting a history frame according to an embodiment of the present disclosure;

FIG. 7c is a schematic diagram of another scenario for extracting a history frame according to an embodiment of the present disclosure;

FIG. 8 is a schematic diagram of another scenario for extracting a history frame according to an embodiment of the present disclosure;

FIG. 9 is a schematic diagram of another scenario for extracting a history frame according to an embodiment of the present disclosure;

FIG. 10 is a schematic diagram of a scenario in which state estimation values are superimposed according to an embodiment of the present invention; FIG.

Figure 11a is a schematic diagram showing the results obtained by a conventional scheme in a comparative experiment;

Figure 11b is a schematic view showing the results obtained by the embodiment of the present invention in a comparative test;

Figure 12a is a schematic diagram showing the results obtained by another conventional scheme in a comparative experiment;

Fig. 12b is a schematic view showing the results obtained by the embodiment of the present invention in a comparative test.

FIG. 13 is a schematic structural diagram of a SLAM terminal according to an embodiment of the present invention;

FIG. 14 is a schematic structural diagram of an apparatus according to an embodiment of the present invention.

Detailed ways

The technical solutions of the embodiments of the present invention are described below with reference to the related drawings.

First, the system framework of the embodiment of the present invention will be described. The embodiment of the invention provides a SLAM system, which can be applied to an automatic driving object such as a driverless car or a mobile robot. Referring to Figure 1, Figure 1 shows a scenario in which a SLAM system is applied to a driverless car. In hardware implementation, the SLAM system can include an inductor, a memory, and a processor. The sensor is used for data acquisition, the memory is used for data storage (such as storing data of the historical point cloud library), and the processor is used for data processing. Referring to FIG. 2, in the function implementation, the SLAM system may include several process modules, such as data acquisition, extended visual odometer, extended back-end optimization, and construction. The embodiment of the present invention mainly focuses on visual odometer and back-end optimization. These two modules have been extended and optimized. After the sensor completes the data acquisition, the processor is performing an extended visual odometer, extended backend optimization, and mapping. The specific instructions are as follows:

(1) Data collection.

In the embodiment of the present invention, the sensor is used for data acquisition, and the sensor may be, for example, a laser radar, a 3D camera, a GPS, a speedometer, an accelerometer, an odometer, etc., and the data is data related to the driving state. For example, the sensor is a 3D camera or a laser radar. In the car operation, the 3D camera or the laser radar monitors the surrounding environment in real time to obtain a stereoscopic image of one frame (ie, spatial point cloud data). For example, the sensor can also be a GPS, a speedometer, an accelerometer, an odometer, etc., and then the running state information (such as speed, acceleration, distance, etc.) of the car can be obtained according to the sensor.

(2) Extended visual odometer.

In the prior art, visual odometry (VO) is to perform two-frame images before and after processing, and perform point cloud registration on two images before and after, to estimate the relative motion of the automatic driving object at two moments, and determine the automatic driving. The position and/or posture of the object. In the embodiment of the present invention, the function of the visual odometer is expanded. In the extended visual odometer, the point cloud data of the current frame acquired in real time (hereinafter referred to as the current frame point cloud data) and the previously acquired history are obtained. The point cloud data of the frame (hereinafter referred to as historical frame point cloud data) is related. Here, the correlation means that part of the image content in the frame overlaps, and the movement of the automatic driving object in two frames can be estimated according to the overlapping portion of the image. Therefore, the current frame point cloud data and the plurality of historical frame point cloud data are respectively subjected to point cloud registration, thereby obtaining a plurality of state estimation values, wherein the state estimation values are obtained corresponding to different historical frame point cloud data. Position and / or posture. Among them, point cloud registration, also known as point cloud matching, refers to a technique in which two or more point cloud data are best fitted according to common features or markers.

(3) Extended backend optimization. In the prior art, Kalman filtering is usually used in the back-end optimization for error elimination. The embodiment of the invention expands the backend optimization, and obtains multiple state estimation values in the previous step visual odometer Then, in the backend optimization, the plurality of state estimation values are superimposed to obtain a total state estimation value corresponding to the current frame point cloud data.

In a specific embodiment, after the current frame point cloud data is respectively registered with the previous plurality of historical frame point cloud data, a plurality of independent state estimation values are obtained, and each state estimation value corresponds to a result reliability. . In the environment of uneven road surface (such as encountering stones, obstacles, potholes, etc.), the auto-driving object causes strong jolting of the vehicle in a short time, which is reflected in the point cloud data that encounters irregular noise (referred to as noise). ), random noise is generally a high frequency, short time shock signal. Based on the presence or absence of noise and the intensity of the noise, these state estimates will have different degrees of confidence. If the two frames of images are completely accurate and noise-free, then the overlapping portions of the two frames of image are processed after translation, rotation, etc. , is completely coincident; if the current frame point cloud data and/or historical frame point cloud data is noisy, the overlapping portions of the two frames of images cannot completely coincide. The degree of overlap of the overlapping portions of the two frames of images, so the degree of coincidence of the two frames of images will represent the credibility of the state estimate, the higher the degree of coincidence, the higher the degree of credibility, the lower the degree of coincidence, and the lower the credibility. In a specific implementation, the degree of coincidence can be quantized into a linear distribution such as variance or standard deviation or covariance. That is, the magnitude of these variances or standard deviations or covariances determines the weight values of the corresponding state estimates. The larger the irregular noise, the smaller the weight value of the obtained state estimation value; the smaller the irregular noise, the larger the weight value of the obtained state estimation value. In this way, after a plurality of state estimation values are superimposed, random noise can be filtered.

In a specific embodiment, the state estimation value includes an expected value and a weight value; wherein the weight value is determined by a coincidence degree value of the current frame point cloud data and the historical frame point cloud data; the weight value is linear And superimposing the N state estimation values to obtain a state estimation result of the current frame point cloud data, including: performing the expected value of the N state estimation values based on the corresponding weight value The weighted superposition obtains a state estimation result of the current frame point cloud data.

For example, the current car has just passed a hurdle, and there is a big bump, which causes the first five frames of the current frame to be affected by random noise. Then, after the current frame and the first five frames are respectively registered by the point cloud, the result is obtained. The state estimate has low confidence and the corresponding weight value is low. If the current frame is the point cloud data after the ridge, and the first frame of the current frame is the point cloud data before the ridge, the state estimation value obtained by the point cloud registration according to the two frames is highly reliable, and the corresponding weight is The value is high. By weighting the results of the plurality of state estimation values in this way, the effect of eliminating random noise can be achieved.

(4) Construction drawings.

After determining the final state estimation value of the current frame point cloud data, the state estimation value may be saved in the historical point cloud library, and the iterative calculation is continuously performed during the motion described later, and the embodiment of the present invention is repeated. In the above process, the current running state information can be sensed, and the mapping process can be performed in real time. Based on the image matching algorithm, the two frames of images are connected in turn, and finally the global stereo image is obtained, and the map built by SLAM is obtained. Can be geometric maps, raster maps, topological maps, 2D maps, 3D maps, and more. The embodiment of the present invention does not specifically limit the mapping process. It can be understood that in the latter application, the SLAM system can match the current frame obtained in real time with the built map described above, and can locate the position of the automatic driving object in the map.

Based on the described SLAM system, a state sensing method provided by an embodiment of the present invention is described in detail below. Referring to FIG. 3, the method includes but is not limited to the following steps:

Step S101: Collect current frame point cloud data.

In the embodiment of the present invention, the current frame point cloud data is the currently collected point cloud data. Specifically, through the sensor The point cloud data is collected; the sensor includes at least one of an optical sensor (such as a 3D camera), a laser sensor (such as a laser radar), a speedometer, an accelerometer, and an odometer. For example, a self-driving car periodically photographs the surrounding environment through a 3D camera or a laser radar to obtain point cloud data of one frame and one frame. The point cloud data refers to a set of scanning points in a three-dimensional coordinate system, the scanning points usually exist in the form of a set of vectors, which usually represent the monitored (scanned) objects in the form of three-dimensional coordinates. Geometric position information. The point cloud data may also represent information such as RGB color, gray value, depth, segmentation result, and the like of the scanning point.

Step S102: Extract a point cloud data set before the current frame point cloud data from the historical point cloud library.

Specifically, a historical point cloud library is set in the SLAM system, and the historical point cloud library is used to save a plurality of collected point cloud data, and such spotting data may be referred to as historical frame point cloud data. Referring to FIG. 4, FIG. 4 is a block diagram of a state sensing method according to an embodiment of the present invention. As shown in FIG. 4, after acquiring current frame point cloud data through a sensor in real time, on the one hand, the current frame point cloud may be Data is saved to the historical point cloud library; on the other hand, a point cloud data set before the current frame point cloud data is extracted from the historical point cloud library, the point cloud data set including one or more history Frame point cloud data.

It should be noted that, in a possible embodiment, the historical point cloud library dynamically maintains the saved point cloud data, that is, the historical point cloud library dynamically stores a plurality of recently obtained point cloud data, and will exceed The point cloud data of the deadline is deleted. The point cloud data set may be part or all of point cloud data in the historical point cloud library. For example, only the last 20 captured images are saved in the historical point cloud library, and the images before 20 frames are deleted, and the extracted point cloud data sets are 10 frames of images in the historical point cloud library.

S103. Perform current point cloud registration on the current frame point cloud data and each of the historical frame point cloud data to obtain multiple state estimation values of the current frame point cloud data.

Wherein the state estimate is used to describe the location or pose of the object in which the SLAM system is located, for example, the state estimate is an estimate of the current location of the self-driving car. In a specific implementation, the mathematical form of the state estimation value may be a linear distribution of probability distribution estimation (such as normal distribution), variance, covariance, covariance matrix, non-probability distribution estimation, and the like.

In a specific embodiment, the point cloud data and each of the historical frame point cloud data are respectively subjected to point cloud registration, and the obtained result includes: a plurality of current state estimation values of the object of the SLAM system obtained based on the point cloud matching. And determining, according to the coincidence degree value of the point cloud data and each of the historical frame point cloud data, a weight value of each of the state estimation values. The mathematical form of the weight value may be a linear distribution of probability distribution estimates (eg, normal distribution), variance, covariance, covariance matrices, non-probability distribution estimates, and the like.

Wherein, the point cloud registration is an operation of shifting, rotating, etc. of the image, so that the same scanning points on the two frames of images can be aligned one by one, and the state change of the object of the SLAM system during the two frames of images is determined, such as the current frame ratio. How many meters are advanced in the history frame, how many angles are turned, etc., to calculate state estimation values such as the current position or posture. As shown in FIG. 3, the extracted point cloud data set includes historical frame data 1, historical frame data 2, and historical frame data 3. The current frame point cloud data is respectively associated with historical frame data 1, historical frame data 2, and historical frame data. 3 Perform point cloud registration to obtain state estimation value 1 and weight value 1, state estimation value 2 and weight value 2, state estimation value 3 and weight value 3, respectively.

Among them, due to scanning angle, environmental noise, etc., there are always some features (features refer to points, lines, faces, etc. in the image) that will not be registered. Then, you need to find an optimal registration so that the features of the two frames of images can be registered as much as possible. The degree of confidence corresponding to the state estimate can be calculated based on the number of features that can be registered and the number of features on the unregistered. The fewer the number of features on the match, the lower the confidence of the state estimate, and the greater the number of features on the match. The higher the credibility of the state estimate. In a specific implementation, the credibility may be represented by a weight value, and the mathematical form of the weight value may be a variance or a standard deviation or a covariance, and may be a probability distribution, such as a normal distribution. It can be understood that, in the embodiment of the present invention, the registration result is capable of sensing noise. If the noise is large, then more registration of the features will be performed when the point cloud is registered, and the corresponding calculated weight value is also lower. .

S104. Superimpose the plurality of state estimation values to obtain a state estimation result of the current frame point cloud data.

Specifically, after obtaining the plurality of state estimation values, the plurality of state estimation values are superimposed together to obtain a total state estimation value about the object where the SLAM is located. Since the mathematical form of the state estimation value may be a probability distribution estimation, a variance, a covariance, a covariance matrix, a non-probability distribution estimation, etc., the mathematical form of the result obtained by superimposing the state estimation values is unchanged. For example, the mathematical form of the state estimate is a normal distribution, and the two normal distributions can be superimposed, and the result obtained after the superposition is still a normal distribution.

In a specific embodiment, the result of the point cloud registration includes a weight value in addition to the state estimate. Then, each state estimation value needs to be weighted and superimposed based on the corresponding weight value, thereby obtaining a state estimation result of the current frame point cloud data.

In a specific embodiment of the present invention, the point cloud registration process can sense random noise, and the weight value calculated based on different historical frame point cloud data is negatively correlated with the random noise, and after the superposition, the obtained state estimation result can be correspondingly Reduce the effects of random noise. In a specific implementation, the weight value of each estimated state value in the superposition process may also be adjusted according to the type, size, and the like of the noise. The larger the noise, the smaller the corresponding weight value is adjusted, and the noise is further eliminated, and the current frame can be guaranteed to be processed. When the cloud data is clicked, the resulting state estimation result is relatively accurate.

It can be understood that after the state estimation result is obtained, since the noise has been greatly reduced or even eliminated in the state estimation result, the reliability of the result is high, and accurate mapping and positioning can be further realized based on the state estimation result. After the current frame point cloud data is saved to the historical point cloud database as the historical frame point cloud data, the next time the point cloud data is collected, the above process is repeated and iteratively, and the effect of eliminating noise in real time can be achieved.

It can be seen that, in the embodiment of the present invention, the object of the SLAM system performs point cloud registration through the current frame point cloud data and several historical frame point cloud data during normal driving, and obtains several states about the current position or posture. The estimated values are superimposed on these state estimates, and the irregular noise is eliminated by the algorithm during the superposition process. In autonomous driving, the sensor sampling frequency is usually high. In the traditional Kalman filter, if the sampling frequency is high, it is susceptible to random noise, resulting in large errors in the results. By implementing the embodiment of the present invention, even if the object of the SLAM system does not perform loopback motion, the irregular noise encountered can be eliminated, the accuracy of the state estimation result is greatly improved, and the accuracy improvement is real-time (while the traditional loopback motion) Accuracy improvement is not real-time), which can further improve the accuracy of drawing and positioning in subsequent applications, which is conducive to the practical use of autopilot products and enhance the user experience.

Based on the described SLAM system, another state sensing method provided by the embodiment of the present invention is described in detail below. Referring to FIG. 5, the method includes but is not limited to the following steps:

Step S201: Collect current frame point cloud data. Specifically, the point cloud data may be collected by an inductor; the sensor includes at least one of an optical sensor (such as a 3D camera), a laser sensor (such as a laser radar), a speedometer, an accelerometer, and an odometer.

Step S202: Detect whether the current triggering policy is met, and if yes, trigger step S203.

In a specific embodiment, in order to improve work efficiency and achieve rapid noise elimination, it is not necessary to collect each time. The technical solution of the embodiment of the present invention is performed on the current frame point cloud data, and the technical solution of the embodiment of the present invention is executed only after the preset trigger policy is met.

In a possible embodiment, the triggering strategy is: in the case that detecting that there is random noise in the current frame point cloud data, triggering to perform the subsequent step S203. For example, when the current frame point cloud data is collected, the current frame point cloud data is first registered with the point cloud data of the previous frame point, and the degree of coincidence of the two frames is used to determine whether there is noise. If the degree of coincidence is low, There is random noise, thereby triggering the execution of the subsequent step S203.

In a further possible embodiment, the triggering strategy is: in the case that abnormal noise is detected in the historical point cloud library, triggering to perform the subsequent step S203. For example, as mentioned above, historical point cloud data is maintained in the historical point cloud library, and the historical frame point cloud data corresponds to the weight value, and the smaller weight value indicates that the reliability is low, that is, the history. Frame point cloud data is affected by noise. Therefore, each time the current frame history data is collected, the history point cloud database is first detected whether there is noise-affected historical frame point cloud data. If it exists, it indicates that random noise is detected, and the subsequent step S203 is triggered.

In a further possible embodiment, the triggering strategy is: triggering to perform the subsequent step S203 in the case that the environment is detected by the sensor to easily cause random noise. For example, when the sensor detects the state of motion of the vehicle, when the sensor detects that the vehicle is significantly bumpy on the uneven road surface, it can be determined that the environment is likely to cause irregular noise, and the subsequent step S203 is triggered.

In a further possible embodiment, the triggering policy is: when detecting that the order of the current frame point cloud data conforms to the preset order, triggering to perform the subsequent step S203. For example, each point cloud data corresponds to an order number. When the order of the current frame point cloud data satisfies the periodic order, if the current frame is the 5th frame, the 10th frame, the 15th frame, ... Step S203.

It should be noted that when the detection does not meet the preset triggering strategy, it indicates that there is no noise or the noise is weak at this time. In this case, the technical solution of the present invention may not be executed, and the current frame point cloud data is directly compared with the previous frame. Registration and completion of back-end graph optimization, mapping positioning and other work.

Step S203: Extract a point cloud data set before the point cloud data from the historical point cloud library.

A historical point cloud library is set in the SLAM system, and the historical point cloud library is used to save the point cloud data collected recently, and dynamically maintain the saved point cloud data.

Referring to FIG. 6 , in a possible embodiment, when it is required to extract a point cloud data set, k frame historical frame point cloud data before the current frame is extracted in the historical point cloud library as the point cloud data set. Where k>1, for example, k=10, that is, 10 frames of historical frame point cloud data before the current frame is fixedly extracted as the point cloud data set.

In a possible embodiment, the system detects in real time whether there is random noise in the current data frame currently collected. Referring to FIG. 7a, if it is detected that there is random noise in the current data frame, the k frame before the current data frame is extracted. Historical frame point cloud data is used as the set of point cloud data. Referring to FIG. 7c, if no irregular noise is detected in the current data frame, the previous frame historical frame point cloud data in the current data frame is extracted as the point cloud data set.

In a further possible embodiment, the system detects in real time whether there is a historical frame with noise in the historical point cloud library. Referring to FIG. 7b, if a historical point cloud library is detected, there is a historical frame that is not affected by noise (normal The data and the historical frame affected by the noise (the k+1th frame in the figure) extract the k-frame historical frame point cloud data before the current data frame as the point cloud data set. If no historical noise is detected in the historical point cloud inventory, the previous frame historical frame point cloud data in the current data frame is extracted as the point cloud data set (also shown in Figure 7c).

In a further possible embodiment, the system determines a sampling step size according to the random noise when the random noise is detected, and extracts the point cloud data from the historical point cloud database based on the sampling step size. Previous point cloud data collection. Referring to FIG. 8, the system detects that there is random noise in the k+1th frame to the nth frame in the historical point cloud library, determines the sampling step size as n frames, ignores the history frame with noise, and extracts before the noise. The history frame (such as the first frame to the kth frame in the figure) is used as the point cloud data set. In a special application scenario, only one historical frame that is not affected by the random noise can be extracted based on the sampling step size to partially achieve the effect of filtering out noise of different frequencies in the embodiment of the present invention. For example, in a specific application scenario, assuming that the car has encountered a bump in the ridge, the image captured during the ridge will be blurred, if the two frames before and after the ridge are extracted, because the two frames The images are all clear, so the results of point cloud registration for the steps described below will also be accurate. In this way, the error caused by the random noise when the ridge is over, will not spread to the measurement after the ridge.

In still another possible embodiment, if the system detects that the order of the current frame point cloud data conforms to a preset order, extracting k frames preceding the current frame point cloud data as a point cloud data set. If the system detects that the order of the current frame point cloud data does not conform to the preset order, extracting the previous frame of the current frame point cloud data as the point cloud data set. Referring to FIG. 9, if the order of the current frame is the 5th frame, the 10th frame, the 15th frame, etc., the first 4 frames of historical frame point cloud data of the current frame point cloud data are extracted as the point cloud data set, and If the current frame does not meet the above periodic order, the historical frame point cloud data of the previous frame of the current data frame is extracted as the point cloud data set.

It can be understood that, in the embodiment of the present invention, the manner of extracting the point cloud data set before the point cloud data from the historical point cloud database may have other implementation forms, and the foregoing examples are merely used to explain the embodiments of the present invention instead of limit.

S204: Perform point cloud registration on each of the current frame point cloud data and the point cloud data set in the point cloud data set to obtain multiple state estimation values of the current frame point cloud data, and each state. The weight value corresponding to the estimated value. For details, refer to the description of step S103 in the embodiment of FIG. 3, and details are not described herein again.

S205. Perform weighted superposition of the plurality of state estimation values to obtain a state estimation result of the current frame point cloud data.

In a specific embodiment, the state estimation value includes an expected value and a weight value; wherein the weight value is determined by a coincidence degree value of the current frame point cloud data and the historical frame point cloud data; the weight value is linear And superimposing the N state estimation values to obtain a state estimation result of the current frame point cloud data, including: performing the expected value of the N state estimation values based on the corresponding weight value The weighted superposition obtains a state estimation result of the current frame point cloud data. The mathematical form of the state estimate may be a probability distribution estimate (eg, a normal distribution), a variance, a covariance, a covariance matrix, a non-probability distribution estimate, and the like.

For example, referring to FIG. 10, it is assumed that the car runs through three positions A, B, and C, and the point cloud data collected at each point are A frame, B frame, and C frame, respectively. The state estimation value of the obtained A frame is represented by a mathematical expectation a, and the weight value is represented by a normal distribution of the variance σ ² . After the B frame is aligned with the A frame point cloud, the mathematical expectation of the B frame relative to the A frame whose position point advance distance is Δx ₁ and the normal deviation of the variance σ ₁ ² is calculated, and the state estimation value of the B frame is mathematical. It is expected that b = (a + Δx ₁ ) and the weight value is characterized by a normal distribution of variance (σ ² + σ ₁ ² ). For the C frame, the technical solution of the embodiment of the present invention is applied, and the C-frame and the A-frame and the B-frame respectively perform point cloud registration, and the mathematical expectation of calculating the advancement distance of the C frame relative to the B frame position is Δx ₂ and the variance is σ _{2 .} ^For a normal distribution of ² , a state estimate of the C frame is a mathematical expectation c ₁ = (a + Δx ₁ + Δx ₂ ), and a weight value is used by a variance σc ₁ ² = (σ ² + σ ₁ ² + σ ₂ ² ) Characterization of the normal distribution; the mathematical expectation of calculating the advancement distance of the C frame relative to the A frame position point is a normal distribution of Δx ₃ , variance σ ₃ ² , then another state estimation value of the C frame is the mathematical expectation c ₂ = ( a + Δx ₃ ), the weight value is characterized by a normal distribution of the variance σc ₂ ² = (σ ² + σ ₃ ² ). Then, the obtained two state evaluation values c ₁ and c ₂ of the C point and the corresponding weight values are weighted and superimposed, and the superposition process can be calculated by using the formula of the mean square error, thereby obtaining the total state estimation of the C point. The value is mathematically expected c=c ₁ +sqrt(σc ₁ ² /(σc ₁ ² +σc ₂ ² ))*(c ₂ -c ₁ ), and the weight value is variance σc ² =(1-sqrt(σc ₁ ² / (σc ₁ ² + σc ₂ ² ))) * σc ₁ ² Characterization.

It can be understood that after obtaining the state estimation result, the next time the point cloud data is collected, iteratively processing is performed with reference to the calculation process of the above position point C, and the car can be accurately evaluated at each point in the running process of the automobile. The position is achieved in real time to eliminate the effect of noise.

It can be seen that, in the embodiment of the present invention, the object of the SLAM system triggers the current frame point cloud data and the plurality of historical frame point cloud data to perform point cloud registration through a preset policy during normal driving, and obtains several related currents. The state estimation value and the weight value of the position or posture are weighted and superimposed, and the irregular noise is eliminated by the algorithm in the superimposition process. By implementing the embodiment of the present invention, even if the object of the SLAM system does not perform loopback motion, the irregular noise encountered can be eliminated, the accuracy of the state estimation result is greatly improved, and the accuracy improvement is real-time (and the accuracy of the conventional loopback motion) The improvement is not real-time), which can further improve the accuracy of drawing and positioning in subsequent applications, which is conducive to the practical use of automatic driving products and enhance the user experience.

In order to better understand the technical effects of the embodiments of the present invention, the test comparison results between the prior art solution and the technical solution of the present invention are briefly described below.

In a comparative test, see Fig. 11a and Fig. 11b, Fig. 11a shows the results obtained by constructing the trajectory of the vehicle under test on the uneven road surface using the conventional SLAM scheme, and the result is in two. The dimensional coordinate graph is presented, and the gray trajectory in the figure is the running trajectory of the obtained automobile. It can be seen that due to the serious influence of random noise, the running track is staggered and the lines are not continuous, and the result of the drawing is greatly errored. Figure 11b shows the results obtained by plotting the trajectory of the vehicle under test on an uneven road surface using the SLAM scheme provided by the embodiment of the present invention under equivalent test conditions. It can be seen that the two-dimensional coordinate graph shows that the running track line is smooth and flat, and the lines are continuous. That is to say, the embodiment of the present invention can greatly eliminate the influence of noise on subsequent frames, and the error of the construction result is small.

In yet another comparative test, referring to Figures 12a and 12b, Figure 12a shows the results of mapping the trajectory of the vehicle under test on a rough road using a conventional SLAM scheme. In the scheme, the artificially adjusted parameters are used to remove the Kalman filter image frame with noise. The obtained driving track has continuous lines, but the error between the starting point and the ending point in the illustrated result is about 250 meters. The result of the mapping is relatively large, and the reliability of the result is low, which is not conducive to the actual product application. Figure 12b shows the results obtained by constructing the trajectory of the vehicle under test on the uneven road surface using the SLAM scheme provided by the embodiment of the present invention under the same test conditions. It can be seen that two-dimensional The graph shows that the running track line is smooth and flat, the lines are continuous, and the error of the starting point and the ending point in the illustrated result is about 70 meters. That is, the embodiment of the present invention can greatly reduce the error of the drawing positioning compared with the conventional scheme. The accuracy of the car position evaluation is greatly improved, and the result is highly reliable. In addition, it should be noted that because random noise means that the system may encounter various frequencies or intensity noise, this means that the manual adjustment method is only suitable for the characteristic noise of a specific scene, and cannot use a set of universal parameters. All noise can be filtered out, and the effect of the system will be worse after the scene is changed. The embodiment of the present invention can automatically filter for various irregular noises, and is applicable to various application scenarios, and the actual product is more practical.

The foregoing describes the method flow of the embodiment of the present invention. The related device involved in the embodiment of the present invention is described below.

Referring to FIG. 13, FIG. 13 is a structural block diagram of an implementation manner of a state-aware SLAM terminal 300 according to an embodiment of the present invention. As shown in FIG. 13, the SLAM terminal 300 can include a chip 310, a memory 315 (one or more computer readable storage media), and a peripheral system 317. These components can communicate over one or more communication buses 314.

The peripheral system 317 is mainly used to implement the interaction function between the SLAM terminal 300 and the user/external environment. In specific implementation, the peripheral system 317 may include: a touch screen controller 318, a 3D camera controller 319, a lidar controller 320, and sensor management. Several components in module 321 . Each controller may be coupled to a corresponding peripheral device such as a touch screen 323, a 3D camera 324, a laser radar 325, and a sensor 326, etc., wherein the 3D camera controller 319, the lidar controller 320, and the sensor 326 are all sensor. In some embodiments, the sensor can be one or more of a speedometer, an accelerometer, an odometer GPS. It should be noted that the peripheral system 317 may also include other I/O peripherals.

The chip 310 can be integrated to include one or more processors 311, a clock module 312, and possibly a power management module 313. The clock module 312 integrated in the chip 310 is primarily used to generate the clocks required for data transfer and timing control for the processor 311. The power management module 313 integrated in the baseband chip 310 is primarily used to provide a stable, high accuracy voltage to the processor 311 and peripheral systems.

Memory 315 is coupled to processor 311 for storing data (such as a historical point cloud), various software programs, and/or sets of program instructions. In particular implementations, memory 315 can include high speed random access memory, and can also include non-volatile memory, such as one or more magnetic disk storage devices, flash memory devices, or other non-volatile solid state storage devices. The memory 315 can also store one or more applications. As shown in the figure, these applications may include: a map application, an image management application, and the like.

It should be understood that the SLAM terminal 300 is only an example provided by an embodiment of the present invention, and that the SLAM terminal 300 may have more or fewer components than the illustrated components, may combine two or more components, or may have Different configurations of components are implemented.

In a specific embodiment of the present invention, the 3D camera 324 or the laser radar 325 or the sensor 326 can be used to collect current frame point cloud data; the memory 312 is used to store a historical point cloud library; the processor 311 can be used to call program instructions in the memory. Perform the following program steps:

Extracting N historical frame point cloud data from the historical point cloud library, where the acquisition time of the N historical frames is before the acquisition time of the current frame, where N is a positive integer greater than or equal to 1;

Performing point cloud registration on the current frame point cloud data and the N historical frame point cloud data respectively to obtain N state estimation values of the current frame point cloud data;

And superimposing the N state estimation values to obtain a state estimation result of the current frame point cloud data.

In a specific embodiment, the state estimation value includes an expected value and a weight value; wherein the weight value is determined by a coincidence degree value of the current frame point cloud data and the historical frame point cloud data; the weight value is linear The processor 311 is configured to perform superimposing the plurality of state estimation values to obtain a state estimation result of the current frame point cloud data, where the processor 311 is configured to perform the N state estimation values. Superimposing, obtaining a state estimation result of the current frame point cloud data, comprising: performing weighted superposition on the expected value of the N state estimation values based on the corresponding weight value, to obtain a state of the current frame point cloud data Estimated results.

In a specific embodiment, before the processor 311 is configured to perform the extracting the N historical frame point cloud data from the historical point cloud library, the method further includes: detecting random noise; the historical point cloud library The N historical frame point cloud data is extracted, and the processor 311 is configured to: when the random noise is detected, trigger the extracting the N historical frame point cloud data from the historical point cloud library.

In a specific embodiment, before the processor 311 is configured to perform the extracting the N historical frame point cloud data from the historical point cloud library, the method further includes: the processor 311 is configured to perform the detecting the current frame point cloud data. The acquiring sequence is in a preset order; the processor 311 is configured to perform the extracting the N historical frame point cloud data from the historical point cloud library, where the processor 311 is configured to perform the detecting the current frame point cloud data. In the case that the acquisition order conforms to the preset order, the triggering extracts N historical frame point cloud data from the historical point cloud library.

In a specific embodiment, the processor 311 is configured to perform extracting a point cloud data set before the current frame point cloud data from the historical point cloud database, where the processor 311 is configured to perform determining the sampling step according to the random noise. Long; extracting a point cloud data set from the historical point cloud database before the current frame point cloud data based on the sampling step size.

The mathematical form of the state estimation value includes at least one of a probability distribution estimation, a variance, a covariance, a covariance matrix, and a non-probability distribution estimation.

It should be noted that, through the foregoing detailed description of the method embodiment of FIG. 3 or FIG. 5, the implementation methods of the components included in the SLAM terminal 300 can be clearly understood by those skilled in the art, and therefore, for brevity of the description, details are not described herein again.

Based on the same inventive concept, the embodiment of the present invention provides another apparatus 400 for state awareness. Referring to FIG. 14, the apparatus 400 includes: an acquisition module 401, an extraction module 402, a registration module 403, and a superposition module 404. The specific description is as follows:

The collecting module 401 is configured to collect current frame point cloud data;

The extraction module 402 is configured to extract N historical frame point cloud data from the historical point cloud library, where the collection time of the N historical frames is before the acquisition time of the current frame, where N is greater than or equal to 1. Positive integer

The registration module 403 is configured to perform point cloud registration on the current frame point cloud data and the N historical frame point cloud data respectively, to obtain a plurality of state estimation values of the current frame point cloud data;

The superimposing module 404 is configured to superimpose the N state estimation values to obtain a state estimation result of the current frame point cloud data.

In a specific embodiment, the state estimation value includes an expected value and a weight value; wherein the weight value is determined by a coincidence degree value of the current frame point cloud data and the historical frame point cloud data; the weight value is linear Distribution to indicate;

The superimposing module 404 is configured to superimpose the plurality of state estimation values to obtain a state estimation result of the current frame point cloud data, including: the superposition module 404 is configured to use the N state estimation values The expected value is weighted and superimposed based on the corresponding weight value to obtain a state estimation result of the current frame point cloud data.

In a specific embodiment, the device further includes a detecting module 405, and the detecting module 405 is configured to detect random noise;

The extracting module 402 is configured to extract N historical frame point cloud data from the historical point cloud library, including:

In the case that the detection module 405 detects the random noise, the extraction module 402 is triggered to extract N historical frame point cloud data from the historical point cloud library.

In a specific embodiment, the detecting module 405 is configured to detect whether the collection order of the current frame point cloud data conforms to a preset order;

The extracting module 402 is configured to extract N historical frame point cloud data from the historical point cloud database, including: when the detecting module 405 detects that the current frame point cloud data collection order meets a preset order, The extracting module 402 is triggered to extract N historical frame point cloud data from the historical point cloud library.

In the specific embodiment, the extracting module 405 extracts N historical frame point cloud data from the historical point cloud database, including: the extracting module 402 is configured to determine a sampling step according to the random noise; The sampling step size extracts N historical frame point cloud data from the historical point cloud library.

The mathematical form of the state estimation value includes at least one of a probability distribution estimation, a variance, a covariance, a covariance matrix, and a non-probability distribution estimation.

It should be noted that, through the foregoing detailed description of the method embodiment of FIG. 3 or FIG. 5, the implementation methods of the various functional modules included in the device 400 can be clearly understood by those skilled in the art, and therefore, for brevity of the description, no further details are provided herein.

In the above embodiments, it may be implemented in whole or in part by software, hardware, firmware, or any combination. When implemented in software, it may be implemented in whole or in part in the form of a computer program product. The computer program product comprises one or more computer instructions which, when loaded and executed on a computer, produce, in whole or in part, a process or function according to an embodiment of the invention. The computer can be a general purpose computer, a special purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer readable storage medium or transferred from one computer readable storage medium to another computer readable storage medium, for example, the computer instructions can be from a network site, computer, server or data center Transmission to another network site, computer, server, or data center via wired (eg, coaxial cable, fiber optic, digital subscriber line) or wireless (eg, infrared, microwave, etc.). The computer readable storage medium can be any available media that can be accessed by a computer, or can be a data storage device such as a server, data center, or the like that includes one or more available media. The usable medium may be a magnetic medium (such as a floppy disk, a hard disk, a magnetic tape, etc.), an optical medium (such as a DVD, etc.), or a semiconductor medium (such as a solid state hard disk) or the like.

In the above embodiments, the descriptions of the various embodiments are focused on, and the parts that are not detailed in a certain embodiment may be referred to the related descriptions of other embodiments.

The above is only a specific embodiment of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily think of changes or substitutions within the technical scope of the present invention. It should be covered by the scope of the present invention. Therefore, the scope of the invention should be determined by the scope of the appended claims.

Claims (14)

A state sensing method, comprising: Collecting current frame point cloud data; Extracting N historical frame point cloud data from the historical point cloud library, where the acquisition time of the N historical frames is before the acquisition time of the current frame, where N is a positive integer greater than or equal to 1; Performing point cloud registration on the current frame point cloud data and the N historical frame point cloud data respectively to obtain N state estimation values of the current frame point cloud data; And superimposing the N state estimation values to obtain a state estimation result of the current frame point cloud data. The method of claim 1 wherein The state estimation value includes an expected value and a weight value; wherein the weight value is determined by a coincidence degree value of the current frame point cloud data and the historical frame point cloud data; the weight value is represented by a linear distribution; And superimposing the N state estimation values to obtain a state estimation result of the current frame point cloud data, including: weighting and superimposing the expected values of the N state estimation values based on the corresponding weight values to obtain The state estimation result of the current frame point cloud data. The method according to claim 1 or 2, wherein before the extracting N historical frame point cloud data from the historical point cloud library, the method further comprises: detecting random noise; Extracting N historical frame point cloud data from the historical point cloud library, including: In the case that the irregular noise is detected, the N historical frame point cloud data is extracted from the historical point cloud library. The method according to claim 3, wherein the extracting N historical frame point cloud data from the historical point cloud library comprises: Determining a sampling step according to the random noise; N historical frame point cloud data is extracted from the historical point cloud library based on the sampling step. The method according to claim 1 or 2, wherein before the extracting the N historical frame point cloud data from the historical point cloud library, the method further comprises: detecting the current frame point cloud data collection The order is in accordance with the preset order; Extracting N historical frame point cloud data from the historical point cloud library, including: After detecting that the collection order of the current frame point cloud data conforms to a preset order, triggering extracting N historical frame point cloud data from the historical point cloud library. A device for state awareness, comprising: An acquisition module, configured to collect current frame point cloud data; And an extraction module, configured to extract N historical frame point cloud data from the historical point cloud library, where the collection time of the N historical frames is before the acquisition time of the current frame, where N is greater than or equal to 1 Integer a registration module, configured to perform point cloud registration on the current frame point cloud data and the N historical frame point cloud data respectively, to obtain a plurality of state estimation values of the current frame point cloud data; And a superimposing module, configured to superimpose the N state estimation values to obtain a state estimation result of the current frame point cloud data. The device according to claim 6 wherein: The state estimation value includes an expected value and a weight value; wherein the weight value is determined by a coincidence degree value of the current frame point cloud data and the historical frame point cloud data; the weight value is represented by a linear distribution; The superimposing module is configured to superimpose the N state estimation values to obtain a state estimation result of the current frame point cloud data, including: the superposition module is configured to use the expected value of the N state estimation values And performing weighted superposition based on the corresponding weight value to obtain a state estimation result of the current frame point cloud data. The device according to claim 6 or 7, wherein the device further comprises a detecting module, wherein the detecting module is configured to detect irregular noise; The extracting module is configured to extract N historical frame point cloud data from the historical point cloud library, including: And in the case that the detecting module detects the random noise, triggering the extracting module to extract N historical frame point cloud data from the historical point cloud library. The apparatus according to claim 8, wherein the extracting module extracts N historical frame point cloud data from the historical point cloud database, including: The extracting module is configured to determine a sampling step size according to the random noise; and extract N historical frame point cloud data from the historical point cloud library based on the sampling step. The device according to claim 6 or 7, wherein the device further comprises a detection module, wherein the detection module is configured to detect whether the collection order of the current frame point cloud data conforms to a preset order; The extracting module is configured to extract N historical frame point cloud data from the historical point cloud library, including: After the detecting module detects that the current frame point cloud data collection order meets a preset order, the extracting module is triggered to extract N historical frame point cloud data from the historical point cloud library. A device for state awareness, comprising: a processor, and a sensor and a memory coupled to the processor, wherein the sensor is configured to acquire current frame point cloud data; Storing a historical point cloud library; the processor is for: Extracting N historical frame point cloud data from the historical point cloud library, where the acquisition time of the N historical frames is before the acquisition time of the current frame, where N is a positive integer greater than or equal to 1; Performing point cloud registration on the current frame point cloud data and the N historical frame point cloud data respectively to obtain N state estimation values of the current frame point cloud data; And superimposing the N state estimation values to obtain a state estimation result of the current frame point cloud data. The device according to claim 11 wherein: The state estimation value includes an expected value and a weight value; wherein the weight value is determined by a coincidence degree value of the current frame point cloud data and the historical frame point cloud data; the weight value is represented by a linear distribution; And the processor is configured to superimpose the N state estimation values to obtain a state estimation result of the current frame point cloud data, including: calculating the expected value of the N state estimation values based on the corresponding weight value A weighted superposition is performed to obtain a state estimation result of the current frame point cloud data. A readable non-volatile storage medium storing computer instructions, comprising computer instructions executed to implement the method of any one of claims 1 to 5. A computer program product, characterized in that, when the computer program product runs on a computer, is executed to implement the method as claimed in any one of claims 1 to 5.

Images