Multi-sensor fusion positioning uncertainty expression method and device, and electronic equipment

Abstract

The present application provides a kind of multi-sensor fusion positioning uncertainty expression method, device and electronic equipment, the method includes: based on the measurement result of the multiple sensors corresponding to target object, the probability density function of the pose of target object itself is constructed, the probability density function is converted into the weighted particle set expression of Monte Carlo positioning;The position distribution of different weighted particles is determined based on the weighted particle set expression, a selection box is determined to cover the weighted particles of the preset proportion based on the selection box, and the selection box is divided into multiple equidistant grids;Based on the position information of each weighted particle, the weighted particle is put into the multiple equidistant grids;The weight sum of all weighted particles in each equidistant grid is counted, and based on the weight sum, the probability that the target object is in the corresponding equidistant grid is determined.The present application can solve the technical problem that it is difficult to obtain the positioning uncertainty of multi-sensor fusion in the prior art in dynamic scene.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of sensor fusion, and in particular to an uncertainty expression method and device for multi-sensor fusion positioning and electronic equipment. BACKGROUND

[0002] Multi-sensor fusion positioning technology is to use the complementary properties of multiple sensors to quickly and accurately realize target tracking or robot self-positioning, but the positioning result still has certain uncertainty. Only better understanding of the positioning uncertainty can better formulate subsequent planning or control strategies. In recent years, through the Cramer-Rao Bound theory, the Fisher Information Matrix (FIM) is a useful index to evaluate the environmental positionability, which can represent the theoretical accuracy of a vehicle at a certain pose in a pre-built map. However, in the actual path planning process, it is difficult to estimate the uncertainty of the Fisher Information Matrix. SUMMARY

[0003] Therefore, it is necessary to provide an uncertainty expression method and device for multi-sensor fusion positioning and electronic equipment to solve the technical problem that it is difficult to obtain the positioning uncertainty after multi-sensor fusion in a dynamic scene in the prior art.

[0004] In order to achieve the above purpose, the present application provides an uncertainty expression method for multi-sensor fusion positioning, comprising:

[0005] Based on the measurement results of the multiple sensors corresponding to the target object, a probability density function of the pose of the target object itself is constructed, and the probability density function is converted into a weighted particle set expression of Monte Carlo positioning;

[0006] Based on the weighted particle set expression, the position distribution of different weighted particles is determined, a selection frame is determined to cover a preset proportion of weighted particles based on the selection frame, and the selection frame is divided into multiple equidistant grids;

[0007] Based on the position information of each weighted particle, the weighted particles are put into the multiple equidistant grids;

[0008] The total weight of all weighted particles in each equidistant grid is counted, and based on the total weight, the probability that the target object is in the corresponding equidistant grid is determined.

[0009] Further, the probability density function is:

[0010]

[0011] where x t represents the pose of the target object itself at time t; z 1:tThis represents the observations from the lidar from time 1 to t; u 1:t It is a preset set of control variables; This represents a preset grid diagram, m n This refers to the probability of occupying the nth grid.

[0012] Further, the conversion of the probability density function into a weighted particle set expression for Monte Carlo localization includes:

[0013] Based on Bayesian rules and Markov assumptions, the probability density function is transformed into a rewritten expression;

[0014] The rewritten expression is approximated based on a preset weighted particle set to obtain the weighted particle set expression for Monte Carlo localization.

[0015] The rewritten expression is: κ is the normalization coefficient, P(x) t |u 1:t ,x t-1 P(z) is the motion model. t |x t ) is the observation model.

[0016] Furthermore, the weighted particle set expression for Monte Carlo localization is:

[0017] in, It is a particle that detects the pose of the target object itself, N p It is the number of particles. It is the Nth p The weight of the particle, δ, is a Dirac trigonometric function.

[0018] Furthermore, the motion model is as follows:

[0019]

[0020] Where, Δu x ,Δu y ,Δu θ It is a random variable that satisfies the following distribution;

[0021]

[0022] in For odometer parameters; (p i,x ,p i,y ) and p i,θ For the i-th path point p in the future path point set i Position and orientation, pi ,θ By p i-1 and p iicalculated; restrict(p i，θ , p i-1，θ ) makes the angle difference between p i,θ and p i-1,θ belong to [-pi, pi); and detect the translation and rotation variation when the target object moves and rotates by itself.

[0023] Further, the observation model is used to:

[0024] obtain a temporary map containing a plurality of grids, and a projected point cloud of the lidar point cloud in the map coordinate system corresponding to the temporary map;

[0025] in the case of determining that there is no obstacle around the projected point cloud, setting the same occupancy probability for the projected point cloud and the grid around the projected point cloud to update the temporary map in real time;

[0026] performing measurement prediction of the lidar based on the updated temporary map and the future path point set.

[0027] Further, the measurement prediction of the lidar based on the updated temporary map and the future path point set comprises:

[0028] performing a ray casting algorithm on each future path point in the future path point set to generate a plurality of simulated lidar point clouds from the updated temporary map, and performing measurement prediction of the lidar based on the plurality of simulated lidar points.

[0029] The application also provides an uncertainty expression device for multi-sensor fusion positioning, comprising:

[0030] a construction module configured to construct a probability density function of the target object's own pose based on the measurement results of a plurality of sensors corresponding to the target object, and convert the probability density function into a weighted particle set expression of Monte Carlo positioning;

[0031] a segmentation module configured to determine the position distribution of different weighted particles based on the weighted particle set expression, determine a selection frame to cover a preset proportion of weighted particles based on the selection frame, and segment the selection frame into a plurality of equidistant grids;

[0032] a release module configured to release weighted particles into the plurality of equidistant grids based on the position information of each weighted particle;

[0033] a determination module configured to count the total weight of all weighted particles in each equidistant grid, and determine the probability that the target object is in the corresponding equidistant grid based on the total weight.

[0034] The application further provides an electronic device comprising a memory and a processor, wherein,

[0035] The memory is configured to store a program.

[0036] The processor is coupled to the memory and is configured to execute the program stored in the memory to implement the steps in the method for expressing uncertainty of multi-sensor fusion positioning according to any one of the preceding embodiments.

[0037] The application further provides a non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the method for expressing uncertainty of multi-sensor fusion positioning according to any one of the preceding embodiments.

[0038] The method for expressing uncertainty of multi-sensor fusion positioning, the device and the electronic device provided by the application have the following beneficial effects: the method for expressing uncertainty of multi-sensor fusion positioning, the device and the electronic device provided by the application construct a weighted particle set expression of Monte Carlo positioning through measurement results of multiple sensors, can obtain positioning uncertainty of multi-sensor fusion in a dynamic scene based on the weighted particle set expression of Monte Carlo positioning, put the weighted particles into multiple equidistant grids based on position information of each weighted particle, count a total weight of all weighted particles in each equidistant grid, determine a probability that the target object is in a corresponding equidistant grid based on the total weight, and solve the technical problem that it is difficult to obtain positioning uncertainty of multi-sensor fusion in a dynamic scene in the prior art. BRIEF DESCRIPTION OF DRAWINGS

[0039] In order to more clearly illustrate the technical solutions in the embodiments of the application, the following will briefly introduce the drawings needed in the embodiment description. Obviously, the drawings in the following description are only some embodiments of the application, and other drawings can be obtained by those skilled in the art without creative effort.

[0040] Figure 1 A flowchart of an embodiment of the method for expressing uncertainty of multi-sensor fusion positioning provided by the application;

[0041] Figure 2 A flowchart of another embodiment of the method for expressing uncertainty of multi-sensor fusion positioning provided by the application;

[0042] Figure 3 A diagram for expressing positioning uncertainty provided by the application;

[0043] Figure 4 A diagram for expressing positioning uncertainty in static and dynamic scenes provided by the application;

[0044] Figure 5 FIG. 1 is a structural schematic diagram of an embodiment of the uncertainty expression device for multi-sensor fusion positioning provided by the present application;

[0045] Figure 6 FIG. 2 is a structural schematic diagram of an embodiment of the electronic device provided by the present application. DETAILED DESCRIPTION

[0046] The technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the drawings of the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by a person skilled in the art without creative work fall within the protection scope of the present application.

[0047] In the description of the embodiments of the present application, the meaning of "a plurality of" is two or more, unless otherwise specified.

[0048] In the embodiments of the present application, the terms "comprising" and "having" and any variations thereof are intended to cover the inclusions that are not exclusive, for example, the processes, methods, devices, products or equipment comprising a series of steps or modules do not have to be limited to the clearly listed steps or modules, but can include other steps or modules that are not clearly listed or inherent to these processes, methods, products or equipment.

[0049] The naming or numbering of the steps appearing in the embodiments of the present application does not mean that the steps in the method flow must be executed in the time / logical order indicated by the naming or numbering. The flow steps that have been named or numbered can change the execution order according to the technical purpose to be achieved, as long as the same or similar technical effects can be achieved.

[0050] Reference to "an embodiment" in this document means that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the present application. The phrase appears at various places in the specification does not necessarily all refer to the same embodiment, nor is it necessarily mutually exclusive of other embodiments. It is explicitly and implicitly understood that the embodiments described herein are capable of combination.

[0051] As shown in FIG. 1, the present application provides a multi-sensor fusion positioning uncertainty expression method, device and electronic equipment, which are described below respectively. Figure 1 As shown in FIG. 1, the present application provides a multi-sensor fusion positioning uncertainty expression method, device and electronic equipment, which are described below respectively.

[0052] The multi-sensor fusion positioning uncertainty expression method provided by the present application comprises:

[0053] Step 110, based on the measurement results of the plurality of sensors corresponding to the target object, constructing a probability density function of the pose of the target object itself, and converting the probability density function into a weighted particle set expression of Monte Carlo localization.

[0054] It can be understood that the probability density function is:

[0055]

[0056] Wherein, x t represents the pose of the target object itself at time t, the target object can be a robot, a dynamic obstacle or other external targets such as vehicles, x t ~(x t ,y t ,θ t ), (x t ,y t ) and θ k respectively represent the position and direction of the detected target object itself; z 1:t represents the observation value of the lidar from time 1 to t, represents a set of distances ; u 1:t is a set of preset control quantities, which can be obtained by an encoder sensor; represents a preset grid map, m n represents the occupancy probability of the nth grid, and when the occupancy probability is less than 0.5, it represents that there is no obstacle in the area represented by the grid.

[0057] Step 120, determining the position distribution of different weighted particles based on the weighted particle set expression, determining a selection frame to cover a preset proportion of weighted particles based on the selection frame, and dividing the selection frame into a plurality of equidistant grids.

[0058] It can be understood that according to the position distribution of different particles, a square frame of A*A is determined to ensure that 90% of the particles can fall into the square frame, and the square frame is divided into equidistant small grids at a certain interval.

[0059] Step 130, based on the position information of each weighted particle, the weighted particle is put into the plurality of equidistant grids.

[0060] It can be understood that different sizes of black circles represent different particle weights. According to the position information of each particle, the weighted particle is put into different equidistant small grids.

[0061] Step 140, counting the total weight of all weighted particles in each equidistant grid, and determining the probability that the target object is in the corresponding equidistant grid based on the total weight.

[0062] It can be understood that the probability of the target object being in the corresponding equidistant grid, that is, the uncertainty expression result of the final multi-sensor fusion positioning. The probability of the target object being in the grid can be obtained by counting the total weight sum of all particles in each equidistant small grid, and all equidistant small grids with different probabilities constitute a positioning uncertainty expression map.

[0063] In some embodiments, the flow of the multi-sensor fusion positioning uncertainty expression method provided by the present application is summarized as Figure 2 As shown in the figure, the obtained positioning uncertainty expression map is as shown in the figure Figure 3 As shown in the figure, Figure 3 In the P i The positioning uncertainty corresponds to the distribution of the weight particles in the map, so that the distribution of the detected target object is concentrated in the middle and the surrounding is a low probability area, and the robot is probably distributed in the high probability area. And Figure 3 In the P i+1 The particle distribution in the map corresponds to the positioning uncertainty, which leads to a large positioning uncertainty, which is reflected in that the robot can be distributed in a larger area.

[0064] In some embodiments, the conversion of the probability density function into a Monte Carlo positioning weighted particle set expression includes:

[0065] Based on the Bayes rule and the Markov assumption, the probability density function is converted into a rewritten expression;

[0066] Based on the preset weighted particles, the rewritten expression is approximately processed to obtain a Monte Carlo positioning weighted particle set expression;

[0067] Wherein, the rewritten expression is κ is a normalization coefficient, P(x t |u 1:t ,x t-1 ) is a motion model, and P(z t |x t ) is an observation model.

[0068] The Monte Carlo positioning weighted particle set expression is

[0069] Wherein, is a particle detecting the pose of the target object itself, N p is the number of particles, is the weight of the N p th particle, and δ is the Dirac delta function.

[0070] It can be appreciated that, in order to avoid complex integral calculation, the Monte Carlo localization can be used to approximate the expression of the rewritten probability density function by using a set of weighted particles .

[0071] In some embodiments, the motion model is:

[0072]

[0073] where Δu x , Δu y , Δu θ are random variables satisfying the following distribution:

[0074]

[0075] where is the odometry parameter; (p i,x , p i,y ) and p i,θ are the position and direction of the i-th path point p i in the future path point set, p i,θ is calculated from p i-1 and p ii ; restrict(p i，θ , p i-1，θ ) makes the angle difference between p i,θ and p i-1,θ belong to [-π, π); and are the translation and rotation variances when detecting the target object moving and rotating .

[0076] It can be appreciated that the motion model is a path point-based probabilistic motion model, which uses the odometry (x odo , y odo , θ odo ) T calculated from the encoder or the inertial measurement unit (IMU) of the vehicle to generate u t = (Δu x , Δu y , Δu θ ) T , so as to obtain the transition of the detected target object state. Once the path is generated, a series of path points can be obtained. For this purpose, the present application can obtain the odometry information with the help of the path points.

[0077] The future path point set is obtained based on the A* path planning algorithm, taking the current pose as the path planning starting point p0, randomly sampling a point in the blank area of the map within a range of x meters, and taking the point as the path planning path point pn Then, the A* path planning algorithm is used to obtain n+1 path points between p0 and p n and used for the calculation of the probabilistic motion model based on path points.

[0078] In some embodiments, the observation model is used for:

[0079] obtaining a temporary map containing a plurality of grids, and a projected point cloud of the lidar point cloud in a map coordinate system corresponding to the temporary map;

[0080] in the case of determining that there is no obstacle around the projected point cloud, setting the same occupancy probability for the projected point cloud and the grids around the projected point cloud to update the temporary map in real time;

[0081] performing measurement prediction of the lidar based on the updated temporary map and the future path point set.

[0082] It can be understood that the observation model is applied to describe the relationship between the detected target object itself and the sensor measurement value, and the construction form is mainly divided into a beam model and a likelihood field model. The likelihood field model calculates the matching score between z t and M, which is more robust and can be calculated faster by looking up the table from the pre-established likelihood field map. However, it is still a challenge to obtain z t at a future time. Therefore, the present application proposes a prophetic observation model using ray projection, which includes temporary map updating and measurement prediction. The temporary map updating can describe the dynamic environment at the current time, and the measurement prediction will generate a simulated point cloud at a future time based on the temporary map.

[0083] The implementation process of the temporary map updating includes point cloud projection and occupancy probability updating. The point cloud projection is used to obtain the projection of the lidar point cloud in the map coordinate system O M , and then the temporary map is updated in real time according to the occupancy probability of the grid map near the projected point cloud. The position of the lidar in the robot coordinate system is (ξ x ,ξ y ) T , which can be accurately obtained through calibration. The rotation angle of the n L -th lidar beam relative to the direction of the detected target or the robot itself is determined by the beam index number and the angle resolution. At this point, the position of the projected point cloud in O M can be expressed as:

[0084]

[0085] wherein, represents the xt Time and the conversion relationship.

[0086] updating the temporary map M I The method is to determine all the projected point clouds grid as occupied, but this can distort M I and make it have slight positioning fluctuations.

[0087] The robust temporary map updating algorithm based on the projected point cloud is as follows:

[0088] Initialization is performed;

[0089] Determine whether the grid occupancy probability corresponding to each point cloud needs to be updated;

[0090] Determine whether there is an occupied grid within the search range SR from the projected point. Only when there is no obstacle near the projected point, it means that the projected point is definitely caused by a dynamic obstacle, and the grid where the projected point is located can be updated;

[0091] In order to make the map updating more efficient, those close to the projected point are given the same occupancy probability to avoid repeated judgment; wherein, only the idle grid can be modified to the occupied grid, so as to avoid the map updating to modify the original map incorrectly.

[0092] In some embodiments, the measurement prediction of the lidar is performed based on the updated temporary map and the future path point set, comprising:

[0093] Performing a ray casting algorithm on each future path point in the future path point set to generate a plurality of simulated lidar point clouds from the updated temporary map, and performing measurement prediction of the lidar based on the plurality of simulated lidar points.

[0094] It can be understood that the measurement prediction is performed according to the future path point set and M I , that is, the ray casting algorithm is performed on each future path point, and a plurality of simulated lidar point clouds can be generated from M I , so as to realize the measurement prediction.

[0095] The implementation process of the ray casting algorithm is as follows. Assuming that the detection target or the robot itself is at the path point p i+n =(p i+n,x, p i+n,y , p i+n,θ ) T , the initial emission angle E0 of the lidar beam is p i+n,θ , the n L -th emission angle can be written as:

[0096]

[0097] where E r is the angular resolution of the lidar; N E is the number of current beams; denotes the angular measurement error based on the lidar measured angular variance σ angle .

[0098] Each beam can travel along the way until it hits an obstacle or exceeds the maximum distance, the simulated lidar point is represented as:

[0099]

[0100]

[0101] where s refers to the search step; r max refers to the maximum distance of the ray depending on the maximum range of the lidar; is the distance measurement error based on the lidar measured distance variance σ distance ; g ~ U(0, r) refers to the influence of the grid resolution on .

[0102] After traversing all possible emission angles at each selected path point, a set of simulated point clouds at future time instants can be generated. To this end, a path point based probabilistic motion model design and a prediction observation model can be used to achieve an iterative estimation of the detected target's own pose distribution.

[0103] To show the change of localization uncertainty in different environments, the present application respectively carried out 100 times of localization experiments in dynamic and static environments at three positions (position 1 is the current time, and positions 2 and 3 are future time positions), Figure 4 show the quantitative description results of the proposed localization uncertainty. First, as shown in the part of the change of localization uncertainty in the static scene in Figure 4 , from position 1 to position 3, the localization uncertainty evaluation at future time instants is described. Because, the distribution probability of the detected target or the robot's own state is quite concentrated, it exceeds 0.6 in a specific and limited area, which means that the detected target or the robot itself is certain about its pose, resulting in a very small localization error. On the contrary, Figure 4As shown in the positioning uncertainty part in the dynamic scene, the positioning uncertainty in the dynamic scene is high, and the maximum value of the distribution probability is not more than 0.04, which is consistent with the phenomenon that the positioning error is large in the dynamic environment. Therefore, the positioning uncertainty estimation method provided by the present application can provide reliable positioning constraints for subsequent path planning or control.

[0104] In summary, the uncertainty expression method of multi-sensor fusion positioning provided by the present application comprises: constructing a probability density function of the target object itself pose based on the measurement results of the plurality of sensors corresponding to the target object, converting the probability density function into a weighted particle set expression of Monte Carlo positioning; determining the position distribution of different weighted particles based on the weighted particle set expression, determining a selection frame to cover a preset proportion of weighted particles based on the selection frame, and dividing the selection frame into a plurality of equidistant grids; based on the position information of each weighted particle, the weighted particles are put into the plurality of equidistant grids; the weight sum of all weighted particles in each equidistant grid is counted, and the probability that the target object is in the corresponding equidistant grid is determined based on the weight sum.

[0105] In the uncertainty expression method of multi-sensor fusion positioning provided by the present application, the weighted particle set expression of Monte Carlo positioning is constructed through the measurement results of the plurality of sensors, the positioning uncertainty after multi-sensor fusion can be obtained in the dynamic scene based on the weighted particle set expression of Monte Carlo positioning, the weighted particles are put into the plurality of equidistant grids based on the position information of each weighted particle, the weight sum of all weighted particles in each equidistant grid is counted, and the probability that the target object is in the corresponding equidistant grid is determined based on the weight sum, thereby solving the technical problem that the positioning uncertainty after multi-sensor fusion is difficult to be obtained in the dynamic scene in the prior art.

[0106] Further, compared with the traditional method which can only evaluate the positioning uncertainty at the current time, the present application proposes a positioning uncertainty expression method which fuses the probability motion model design based on path points and the prediction observation model, which can be used to estimate the positioning uncertainty at the future time, and provides guidance for subsequent planning and control.

[0107] Compared with the method of using only the maximum particle weight, entropy and variance and other single indicators in Monte Carlo positioning to evaluate the positioning uncertainty, the present application proposes a positioning uncertainty expression method using particle distribution probability graph, which is more robust and accurate in presenting the positioning uncertainty.

[0108] As Figure 5 shown, the present application further provides a multi-sensor fusion positioning uncertainty expression device 500, comprising:

[0109] The constructing module 510 is configured to construct a probability density function of a pose of the target object based on measurement results of a plurality of sensors corresponding to the target object, and convert the probability density function into a weighted particle set expression of Monte Carlo localization.

[0110] The dividing module 520 is configured to determine a position distribution of different weighted particles based on the weighted particle set expression, determine a selection frame based on the selection frame covering a preset proportion of the weighted particles, and divide the selection frame into a plurality of equidistant grids.

[0111] The dropping module 530 is configured to drop the weighted particles into the plurality of equidistant grids based on position information of each weighted particle.

[0112] The determining module 540 is configured to count a total weight of all weighted particles in each equidistant grid, and determine a probability that the target object is in a corresponding equidistant grid based on the total weight.

[0113] The above embodiment provides a multi-sensor fusion positioning uncertainty expression device, which can realize the technical solutions described in the multi-sensor fusion positioning uncertainty expression method embodiment. The principles of the specific implementation of the above modules or units can be referred to the corresponding content in the multi-sensor fusion positioning uncertainty expression method embodiment, which will not be described here.

[0114] As shown in FIG. 6, the present application also provides an electronic device 600. Figure 6 The electronic device 600 includes a processor 601, a memory 602, and a display 603. Figure 6 Only some components of the electronic device 600 are shown, but it should be understood that all the components shown are not required, and more or less components can be implemented instead.

[0115] The memory 602 can be an internal storage unit of the electronic device 600 in some embodiments, such as a hard disk or memory of the electronic device 600. The memory 602 can also be an external storage device of the electronic device 600 in other embodiments, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc.

[0116] Further, the memory 602 can include both an internal storage unit and an external storage device of the electronic device 600. The memory 602 is used to store application software and various data installed in the electronic device 600.

[0117] The processor 601 may, in some embodiments, be a central processing unit (CPU), a microprocessor, or other data processing chip, for running program codes stored in the memory 602 or processing data, such as the uncertainty expression method of multi-sensor fusion positioning in the present application.

[0118] The display 603 may, in some embodiments, be an LED display, a liquid crystal display, a touch liquid crystal display, an OLED (Organic Light-Emitting Diode) touch, and the like. The display 603 is used to display information of the electronic device 600 and to display a visualized user interface. The components 601-603 of the electronic device 600 communicate with each other through a system bus.

[0119] In some embodiments of the present application, when the processor 601 executes the multi-sensor fusion positioning uncertainty expression program in the memory 602, the following steps can be implemented:

[0120] Based on the measurement results of the plurality of sensors corresponding to the target object, a probability density function of the pose of the target object itself is constructed, and the probability density function is converted into a weighted particle set expression of Monte Carlo localization;

[0121] Based on the weighted particle set expression, the position distribution of different weighted particles is determined, a selection box is determined to cover a preset proportion of weighted particles based on the selection box, and the selection box is divided into a plurality of equidistant grids;

[0122] Based on the position information of each weighted particle, the weighted particles are put into the plurality of equidistant grids;

[0123] The total weight of all weighted particles in each equidistant grid is counted, and based on the total weight, the probability that the target object is in the corresponding equidistant grid is determined.

[0124] It should be understood that, in addition to the above functions, the processor 601 may, when executing the multi-sensor fusion positioning uncertainty expression program in the memory 602, also implement other functions, which can be referred to the description of the corresponding method embodiments above.

[0125] Further, the embodiments of the present application do not make specific limitation on the type of the electronic device 600 mentioned above, and the electronic device 600 can be a mobile phone, a tablet computer, a personal digital assistant (PDA), a wearable device, a laptop computer, or the like. Exemplary embodiments of the portable electronic device include, but are not limited to, a portable electronic device running an IOS, an android, a microsoft, or other operating system. The portable electronic device mentioned above can also be other portable electronic devices, such as a laptop computer having a touch-sensitive surface (e.g., a touch panel), and the like. It should also be understood that in some other embodiments of the present application, the electronic device 600 can also not be a portable electronic device, but a desktop computer having a touch-sensitive surface (e.g., a touch panel).

[0126] In yet another aspect, the present application also provides a non-transitory computer readable storage medium having stored thereon a computer program, which, when executed by a processor, implements the method for expressing uncertainty of multi-sensor fusion positioning provided by the above-mentioned methods, and the method comprises:

[0127] Based on the measurement results of the plurality of sensors corresponding to the target object, a probability density function of the pose of the target object itself is constructed, and the probability density function is converted into a weighted particle set expression of Monte Carlo localization;

[0128] Based on the weighted particle set expression, the position distribution of different weighted particles is determined, a selection box is determined to cover a preset proportion of weighted particles based on the selection box, and the selection box is divided into a plurality of equidistant grids;

[0129] Based on the position information of each weighted particle, the weighted particles are put into the plurality of equidistant grids;

[0130] The total weight of all weighted particles in each equidistant grid is counted, and based on the total weight, the probability that the target object is in the corresponding equidistant grid is determined.

[0131] Those skilled in the art can understand that all or part of the processes of the above-mentioned embodiments can be completed by a computer program instructing related hardware, and the program can be stored in a computer readable storage medium. The computer readable storage medium includes a magnetic disk, an optical disk, a read-only memory, a random access memory, and the like.

[0132] The uncertainty expression method, the device and the electronic equipment for the multi-sensor fusion positioning provided by the application are described in detail above, the principle and the implementation mode of the application are described by applying specific examples in this paper, and the above example is only used to help understand the method of the application and the core idea; at the same time, for the skilled in the art, according to the idea of the application, the specific implementation mode and the application range will be changed, and the above description should not be understood as the limitation of the application.

Claims

1. A method for expressing uncertainty in multi-sensor fusion localization, characterized in that, include: Based on the measurement results of multiple sensors corresponding to the target object, a probability density function of the target object's own pose is constructed, and the probability density function is converted into a weighted particle set expression for Monte Carlo localization. Based on the weighted particle set expression, the position distribution of different weighted particles is determined, a selection box is determined, and the selection box covers a preset proportion of weighted particles, and the selection box is divided into multiple equidistant grids. Based on the position information of each weighted particle, the weighted particles are projected into the plurality of equidistant grids; The weights of all weighted particles in each of the equidistant grids are summed, and the probability that the target object is located in the corresponding equidistant grid is determined based on the sum of the weights. The probability density function is: in, x t This indicates that the target object itself is detected in time. The position above; Indicates the lidar from time 1 to Observed values; It is a preset set of control variables; This represents a preset grid diagram. This refers to the probability of occupying the nth grid. The step of converting the probability density function into a weighted particle set expression for Monte Carlo localization includes: Based on Bayesian rules and Markov assumptions, the probability density function is transformed into a rewritten expression; The rewritten expression is approximated based on a preset weighted particle set to obtain the weighted particle set expression for Monte Carlo localization. The rewritten expression is: , The normalization coefficient is... For motion model, For observation models; The motion model is as follows: in, , , It is a random variable that satisfies the following distribution; in , , , , , These are the parameters for the odometer; and For the i-th path point in the future path point set p i Position and direction Depend on p i-1 and p i Calculated; make p i,θ and p i-1,θ Angular difference belongs to ; and To detect the movement of the target object itself and rotation Translation and rotation variations over time.

2. The uncertainty expression method for multi-sensor fusion positioning according to claim 1, characterized in that, The weighted particle set expression for Monte Carlo localization is: , ; in, It is a particle that detects the pose of the target object itself. N p It is the number of particles. It is the first N p Particle weights It is a Dirac trigonometric function.

3. The uncertainty expression method for multi-sensor fusion positioning according to claim 1, characterized in that, The observation model is used for: Obtain a temporary map containing multiple grids, and the projection point cloud of the LiDAR point cloud into the map coordinate system corresponding to the temporary map; If it is determined that there are no obstacles in the grid around the projected point cloud, the same occupancy probability is set for the projected point cloud and the grid around the projected point cloud to update the temporary map in real time. LiDAR measurement predictions are performed based on the updated temporary map and the set of future path points.

4. The uncertainty expression method for multi-sensor fusion positioning according to claim 3, characterized in that, The process of performing lidar measurement predictions based on the updated temporary map and the future path point set includes: A ray casting algorithm is executed at each future path point in the set of future path points to generate multiple simulated lidar point clouds from the updated temporary map, and lidar measurement prediction is performed based on the multiple simulated lidar points.

5. A device for expressing uncertainty in multi-sensor fusion positioning, characterized in that, include: The construction module is used to construct the probability density function of the target object's own pose based on the measurement results of multiple sensors corresponding to the target object, and convert the probability density function into a weighted particle set expression for Monte Carlo localization; The segmentation module is used to determine the position distribution of different weighted particles based on the weighted particle set expression, determine a selection box, cover a preset proportion of weighted particles based on the selection box, and segment the selection box into multiple equidistant grids. The delivery module is used to deliver weighted particles into the plurality of equidistant grids based on the position information of each weighted particle; The determination module is used to calculate the total weight of all weighted particles in each of the equidistant grids, and based on the total weight, determine the probability that the target object is located in the corresponding equidistant grid. The probability density function is: in, x t This indicates that the target object itself is detected in time. The position above; Indicates the lidar from time 1 to Observed values; It is a preset set of control variables; This represents a preset grid diagram. This refers to the probability of occupying the nth grid. The step of converting the probability density function into a weighted particle set expression for Monte Carlo localization includes: Based on Bayesian rules and Markov assumptions, the probability density function is transformed into a rewritten expression; The rewritten expression is approximated based on a preset weighted particle set to obtain the weighted particle set expression for Monte Carlo localization. The rewritten expression is: , The normalization coefficient is... For motion model, For observation models; The motion model is as follows: in, , , It is a random variable that satisfies the following distribution; in , , , , , These are the parameters for the odometer; and For the i-th path point in the future path point set p i Position and direction Depend on p i-1 and p i Calculated; make p i,θ and p i-1,θ Angular difference belongs to ; and To detect the movement of the target object itself and rotation Translation and rotation variations over time.

6. An electronic device, characterized in that, Including memory and processor, among which, The memory is used to store programs; The processor, coupled to the memory, is used to execute the program stored in the memory to implement the steps in the uncertainty expression method for multi-sensor fusion localization as described in any one of claims 1 to 4.

7. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the uncertainty expression method for multi-sensor fusion positioning as described in any one of claims 1 to 4.

Images