CN116805047A - Uncertainty expression method, device and electronic equipment for multi-sensor fusion positioning - Google Patents

Abstract

The application provides an uncertainty expression method and device for multi-sensor fusion positioning and electronic equipment, wherein the method comprises the following steps: based on measurement results of a plurality of sensors corresponding to the target object, constructing a probability density function of the pose of the target object, and converting the probability density function into a weighted particle set expression of Monte Carlo positioning; determining position distribution of different weighted particles based on the weighted particle group 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, putting the weighted particles into the plurality of equidistant grids; and counting the weight sum 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 weight sum. The method and the device can solve the technical problem that positioning uncertainty after multi-sensor fusion is difficult to obtain in a dynamic scene in the prior art.

Description

Uncertainty expression method, device and electronic equipment for multi-sensor fusion positioning

Technical field

The invention relates to the field of sensor fusion technology, and in particular to an uncertainty expression method, device and electronic equipment for multi-sensor fusion positioning.

Background technique

Multi-sensor fusion positioning technology uses the complementary attributes of multiple sensors to quickly and accurately achieve detection target tracking or robot self-positioning. However, there is still a certain uncertainty in the positioning results. Only by better understanding the positioning uncertainty can we achieve better results. Develop follow-up planning or control strategies. In recent years, through the Cramér-Rao Bound theory, the Fisher Information Matrix (FIM) has become a useful indicator for evaluating the localizability of the environment. It can represent the theoretical accuracy of a vehicle in a certain pose in a pre-built map. However, in the actual path planning process, the Fisher information matrix is difficult to estimate uncertainty.

Contents of the invention

In view of this, it is necessary to provide an uncertainty expression method, device and electronic equipment for multi-sensor fusion positioning to solve the technical problem in the existing technology that it is difficult to obtain positioning uncertainty after multi-sensor fusion in dynamic scenes. .

In order to achieve the above objectives, the present invention provides an uncertainty expression method for multi-sensor fusion positioning, including:

Based on the measurement results of multiple sensors corresponding to the target object, construct a probability density function of the target object's own pose, and convert the probability density function into a weighted particle set expression for Monte Carlo positioning;

Determine the position distribution of different weighted particles based on the weighted particle set expression, determine a selection box to cover a preset proportion of weighted particles based on the selection box, and divide the selection box into a plurality of equidistant grids;

Based on the position information of each weighted particle, place the weighted particles into the plurality of equidistant grids;

The sum of the weights of all weighted particles in each of the equidistant grids is counted, and based on the sum of the weights, the probability that the target object is in the corresponding equidistant grid is determined.

Further, the probability density function is:

Among them, x _t represents the detected position of the target object itself at time t; z _1:t represents the observation value of the lidar from time 1 to t; u _1:t is a set of preset control quantities; Represents the preset grid map, m _n refers to the occupancy probability of the nth grid.

Further, converting the probability density function into a weighted particle set expression of Monte Carlo positioning includes:

Convert the probability density function into a rewritten expression based on Bayes' rule and Markov hypothesis;

Approximate the rewritten expression based on preset weighted particles to obtain a weighted particle set expression for Monte Carlo positioning;

Among them, the rewritten expression is κ is the normalization coefficient, P(x _t |u _1:t ,x _t-1 ) is the motion model, and P(z _t |x _t ) is the observation model.

Further, the weighted particle set expression of the Monte Carlo positioning is

in, is a particle that detects the pose of the target object itself, N _p is the number of particles, /> is the weight of the Np _-th particle, and δ is the Dirac trigonometric function.

Further, the motion model is:

Among them, Δu _x , Δu _y , Δu _θ are random variables that satisfy the following distribution;

in are the odometer parameters; (pi _,x ,pi _,y ) and p _i,θ are the position and direction of the i-th way point p _i in the future way point set, and pi _,θ are calculated by p _i-1 and p _ii It follows that; restrict(p _{i, θ} , p _{i-1, θ} ) makes the angle difference between p _{i, θ} and p _{i-1, θ} belong to [-π, π);/> and/> In order to detect the movement of the target object/> and rotate/> translational and rotational variations.

Further, the observation model is used to:

Obtain a temporary map containing multiple grids, and the projection point cloud of the lidar point cloud in the map coordinate system corresponding to the temporary map;

When 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 waypoints.

Further, performing lidar measurement prediction based on the updated temporary map and the future way point set includes:

Perform a ray casting algorithm on each future waypoint in the set of future waypoints, generate a plurality of simulated lidar point clouds from the updated temporary map, and perform a lidar calculation based on the plurality of simulated lidar points. Measure predictions.

The invention also provides an uncertainty expression device for multi-sensor fusion positioning, which includes:

A building module for constructing a probability density function of the target's own pose based on the measurement results of multiple sensors corresponding to the target, and converting the probability density function into a weighted particle set expression for Monte Carlo positioning;

A segmentation module for determining the position distribution of different weighted particles based on the weighted particle set expression, determining a selection box to cover a preset proportion of weighted particles based on the selection box, and dividing the selection box into multiple equal parts. distance grid;

A delivery module, configured to deliver weighted particles into the plurality of equidistant grids based on the position information of each weighted particle;

A determination module, configured to count the sum of weights of all weighted particles in each of the equidistant grids, and determine the probability that the target object is in the corresponding equidistant grid based on the sum of weights.

The invention also provides an electronic device, including a memory and a processor, wherein,

The memory is used to store programs;

The processor is coupled to the memory and is configured to execute the program stored in the memory to implement the steps in the uncertainty expression method for multi-sensor fusion positioning as described in any one of the above.

The present invention also provides a non-transitory computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the uncertainty expression method for multi-sensor fusion positioning as described in any one of the above is implemented. .

The beneficial effects of adopting the above implementation method are: the uncertainty expression method, device and electronic equipment for multi-sensor fusion positioning provided by the present invention can construct a weighted particle set expression for Monte Carlo positioning based on the measurement results of multiple sensors. The weighted particle set expression of Monte Carlo positioning can obtain the positioning uncertainty after multi-sensor fusion in dynamic scenes. Based on the position information of each weighted particle, the weighted particles are placed into multiple equidistant grids, and the statistics of each The sum of the weights of all weighted particles in an equidistant grid, based on the sum of weights, determines the probability that the target object is in the corresponding equidistant grid, solving the difficulty in obtaining positioning after multi-sensor fusion in dynamic scenes in the existing technology. Definitive technical issues.

Description of the drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present invention. For those skilled in the art, other drawings can also be obtained based on these drawings without exerting creative efforts.

Figure 1 is a schematic flow chart of an embodiment of the uncertainty expression method for multi-sensor fusion positioning provided by the present invention;

Figure 2 is a schematic flow chart of another embodiment of the uncertainty expression method for multi-sensor fusion positioning provided by the present invention;

Figure 3 is a schematic diagram of positioning uncertainty expression provided by the present invention;

Figure 4 is a schematic diagram of positioning uncertainty changes in static and dynamic scenarios provided by the present invention;

Figure 5 is a schematic structural diagram of an embodiment of the uncertainty expression device for multi-sensor fusion positioning provided by the present invention;

Figure 6 is a schematic structural diagram of an embodiment of the electronic device provided by the present invention.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative efforts fall within the scope of protection of the present invention.

In the description of the embodiments of this application, unless otherwise specified, "plurality" means two or more.

The terms "comprising" and "having" and any variations thereof in the embodiments of the present invention are intended to cover non-exclusive inclusion, for example, a process, method, device, product or equipment that includes a series of steps or modules and need not be limited to the clear Those steps or modules listed may instead include other steps or modules not expressly listed or inherent to the process, method, product or apparatus.

The naming or numbering of steps in the embodiments of the present invention does not mean that the steps in the method flow must be executed in the time/logical sequence indicated by the naming or numbering. The named or numbered process steps can be executed as needed. The order of execution can be changed to achieve the technical purpose, as long as the same or similar technical effect can be achieved.

Reference herein to "an embodiment" means that a particular feature, structure or characteristic described in connection with the embodiment can be included in at least one embodiment of the invention. The appearances of this phrase in various places in the specification are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments. Those skilled in the art understand, both explicitly and implicitly, that the embodiments described herein may be combined with other embodiments.

As shown in Figure 1, the present invention provides an uncertainty expression method, device and electronic equipment for multi-sensor fusion positioning, which will be described separately below.

The uncertainty expression method for multi-sensor fusion positioning provided by the present invention includes:

Step 110: Construct a probability density function of the target's own pose based on the measurement results of multiple sensors corresponding to the target, and convert the probability density function into a weighted particle set expression for Monte Carlo positioning.

It can be understood that the probability density function is:

Among them, x _t represents detecting the pose of the target object itself at time t. The target object can be an external target such as a robot, a dynamic obstacle or other vehicle, x _t ~ (x _t ,y _t ,θ _t ), ( x _t , y _t ) and θ _k are respectively the position and direction of the detected target itself; z _1:t represents the observation value of the lidar from time 1 to t, Represents a set of distances/> A set of; u _1:t is a preset set of control quantities, which can be obtained by the encoder sensor;/> Represents the preset grid map, m _n refers to the occupancy probability of the nth grid. When the occupancy probability is less than 0.5, it means that there are no obstacles in the area represented by the grid.

Step 120: Determine the position distribution of different weighted particles based on the weighted particle set expression, determine a selection box to cover a preset proportion of weighted particles based on the selection box, and divide the selection box into multiple equidistant grids. grid.

It can be understood that based on the position distribution of different particles, determine a square box of A*A to ensure that 90% of the particles can fall in the square box, and divide the square box into equal intervals at a certain distance. Small grid.

Step 130: Based on the position information of each weighted particle, place the weighted particles into the plurality of equidistant grids.

Understandably, black circles of different sizes represent different particle weights. Based on the position information of each particle, weighted particles are placed into different equidistant small grids.

Step 140: Calculate the sum of weights 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 sum of weights.

It can be understood that the probability that the target is in the corresponding equidistant grid is the uncertainty expression result of the final multi-sensor fusion positioning. By counting the sum of the weights of all particles in each small equidistant grid, the probability that the detection target is located in the grid can be obtained. All the small equidistant grids with different probabilities form a positioning uncertainty expression map.

In some embodiments, the process of the uncertainty expression method for multi-sensor fusion positioning provided by the present invention is summarized as shown in Figure 2. The obtained positioning uncertainty expression schematic diagram is shown in Figure 3. The position at P _i in Figure 3 The distribution of the weight particles in the figure corresponding to the uncertainty is concentrated, so that the distribution of the detection target is concentrated in the middle and surrounded by low probability areas, and the robot is distributed in the high probability area with a high probability. In the figure corresponding to the positioning uncertainty at P _i+1 in Figure 3, the scattered particle distribution leads to greater positioning uncertainty, which is reflected in the fact that the robot may be distributed in a larger area.

In some embodiments, converting the probability density function into a Monte Carlo positioning weighted particle set expression includes:

Convert the probability density function into a rewritten expression based on Bayes' rule and Markov hypothesis;

Approximate the rewritten expression based on preset weighted particles to obtain a weighted particle set expression for Monte Carlo positioning;

Among them, the rewritten expression is κ is the normalization coefficient, P(x _t |u _1:t ,x _t-1 ) is the motion model, and P(z _t |x _t ) is the observation model.

The expression of the weighted particle set of the Monte Carlo positioning is

in, is a particle that detects the pose of the target object itself, N _p is the number of particles, /> is the weight of the Np _-th particle, and δ is the Dirac trigonometric function.

Understandably, in order to avoid complex integral calculations, Monte Carlo localization can be achieved by using a set of weighted particles to approximate the rewritten expression of the probability density function.

In some embodiments, the motion model is:

Among them, Δu _x , Δu _y , Δu _θ are random variables that satisfy the following distribution;

in is the odometer parameter; (pi _,x ,pi _,y ) and p _i,θ are the position and direction of the i-th way point p _i in the future way point set, p _i,θ is determined by p _i-1 and p _ii Calculated; restrict(p _{i, θ} , p _{i-1, θ} ) makes the angle difference between p _{i, θ} and p _{i-1, θ} belong to [-π, π);/> and/> In order to detect the movement of the target object/> and rotate/> translational and rotational variations.

It can be understood that the motion model is a probabilistic motion model based on way points, which uses the mileage (x _odo , y _odo , θ _odo ) ^T calculated from the encoder or the vehicle inertial measurement unit (IMU). Generate _ut = (Δu _x , Δu _y , Δu _θ ) ^T , thereby obtaining the conversion of the state of the detection target itself. Once a path is generated, a series of waypoints can be obtained. For this purpose, the present invention can obtain odometer information with the help of waypoints.

The future path point set is obtained based on the A* path planning algorithm. Taking the current pose as the starting point p ₀ of the path planning, randomly sampling points in the blank space of the map outside the range of x meters in the future, and using this point as the path planning path point p _n , and then use the A* path planning algorithm to obtain n+1 path points between p ₀ and p _n , and use them for the calculation of the probabilistic motion model based on the path points.

In some embodiments, the observation model is used to:

Obtain a temporary map containing multiple grids, and the projection point cloud of the lidar point cloud in the map coordinate system corresponding to the temporary map;

When 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 waypoints.

It can be understood that the observation model is used to describe the relationship between the detected target's own posture and the sensor measurement value, and its construction form is mainly divided into beam model and 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 graph. However, getting _zt will still be a challenge in the coming moments. To this end, the present invention proposes a predictive observation model using ray casting, which includes ad hoc map updates and measurement predictions. Temporary map updates can describe the dynamic environment at the current time, while measurement predictions will rely on the temporary map to generate simulated point clouds at future moments.

The implementation process of ad hoc map updates includes point cloud projection and occupancy probability updates. 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's coordinate system is (ξ _x ,ξ _y ) ^T , which can be accurately obtained through calibration. n _L -th rotation angle of the lidar beam relative to the detection target or the robot's own direction Determined by beam index number and angular resolution. At this point, project the point cloud/> The position in O _M can be expressed as:

in, means that when x _t is given/> and/> conversion relationship.

The way to update the temporary map M _I is to convert all projected point clouds The grid is determined to be occupied, but this may distort M _I and cause it to have slight positioning fluctuations.

The robust temporary map update algorithm based on projected point clouds is as follows:

Initialize;

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

Determine whether there is an occupied grid within the search range SR from the projection point. Only when there are no obstacles in the grid near the projection point, it means that the projection point must be caused by a dynamic obstacle. At this time, the location of the projection point can be updated. grid;

In order to make the map update more efficient, those projection points that are close to each other are given the same occupancy probability to avoid repeated judgments; among them, it is ensured that only free grids can be modified into occupied grids to avoid incorrect map updates. Modify the original map.

In some embodiments, performing lidar measurement prediction based on the updated temporary map and the future waypoint set includes:

Perform a ray casting algorithm on each future waypoint in the set of future waypoints, generate a plurality of simulated lidar point clouds from the updated temporary map, and perform a lidar calculation based on the plurality of simulated lidar points. Measure predictions.

It can be understood that by performing measurement prediction based on the future waypoint set and _MI , that is, executing a ray casting algorithm on each future waypoint, multiple simulated lidar point clouds can be generated from _MI , thereby achieving Measure predictions.

The implementation process of the ray casting algorithm is as follows. Assume that the detection target or the robot itself is on the path point p _i+n = (p _i+n,x, p _i+n,y , p _i+n,θ ) ^T , the initial emission angle E ₀ of the lidar beam is p _i+n,θ ,n _L -th emission angle can be written as:

Among them, E _r is the angular resolution of the lidar; N _E is the number of current beams; Represents the angle measurement error based on the angular difference σ _angle measured by lidar.

Each beam can be Travel until you hit an obstacle or exceed the maximum distance, simulated lidar point/> Expressed as:

Among them, s refers to the search step; r _max refers to the maximum distance of the ray that depends on the maximum range of the lidar; It is the distance measurement error based on the distance variance σ _distance measured by lidar; g~U(0,r) refers to the grid resolution pair/> Impact.

After each selected waypoint crosses all possible launch angles, a set of simulated point clouds at different times in the future can be generated. To this end, the probabilistic motion model design and predictive observation model based on path points can be used to achieve iterative estimation of the posture distribution of the detected target itself.

In order to show the changes in positioning uncertainty in different environments, the present invention conducted 100 positioning experiments at three positions in dynamic and static environments (position 1 is the current moment, positions 2 and 3 are future moments). Figure 4 shows The quantitative description results of the proposed positioning uncertainty are presented. First, as shown in the positioning uncertainty change part under the static scene in Figure 4, from position 1 to position 3, the positioning uncertainty evaluation at future moments is described. Because the distribution probability of the detection target or the robot's own state is quite concentrated, exceeding 0.6 in a specific and limited area, this means that the detection target or the robot's own posture is certain, resulting in a small positioning error. On the contrary, as shown in the positioning uncertainty part of the dynamic scene in Figure 4, the positioning uncertainty in the dynamic scene is relatively high, and the maximum value of the distribution probability does not exceed 0.04, which is consistent with the phenomenon of large positioning errors in dynamic environments. consistent. Therefore, the positioning uncertainty estimation method proposed by the present invention can provide reliable positioning constraints for subsequent path planning or control.

In summary, the uncertainty expression method for multi-sensor fusion positioning provided by the present invention includes: based on the measurement results of multiple sensors corresponding to the target object, constructing a probability density function of the target object's own pose, and converting the The probability density function is converted into a weighted particle set expression for Monte Carlo positioning; the position distribution of different weighted particles is determined based on the weighted particle set expression, and a selection box is determined to cover a preset proportion of weighted particles based on the selection box, And divide the selection box into multiple equidistant grids; based on the position information of each weighted particle, place the weighted particles into the multiple equidistant grids; count all the particles in each equidistant grid. The sum of the weights of weighted particles, based on the sum of weights, determines the probability that the target object is located in the corresponding equidistant grid.

In the uncertainty expression method of multi-sensor fusion positioning provided by the present invention, a weighted particle set expression for Monte Carlo positioning is constructed through the measurement results of multiple sensors. The weighted particle set expression based on Monte Carlo positioning can be The positioning uncertainty after multi-sensor fusion is obtained in dynamic scenes. Based on the position information of each weighted particle, the weighted particles are placed into multiple equidistant grids, and the sum of the weights of all weighted particles in each equidistant grid is calculated. , based on the sum of weights, determine the probability that the target object is in the corresponding equidistant grid, solving the technical problem in the existing technology that it is difficult to obtain positioning uncertainty after multi-sensor fusion in dynamic scenes.

Furthermore, compared with traditional methods that can only evaluate positioning uncertainty at the current moment, the present invention proposes a positioning uncertainty expression method that integrates path point-based probabilistic motion model design and predictive observation models, which can be used to estimate future Momentary positioning uncertainty estimation provides guidance for subsequent planning and control.

Compared with the method that only uses single indicators such as the maximum particle weight, entropy and variance in Monte Carlo positioning to evaluate positioning uncertainty, the present invention proposes a positioning uncertainty expression method that uses particle distribution probability maps, which is more Robustly and accurately present positioning uncertainty.

As shown in Figure 5, the present invention also provides an uncertainty expression device 500 for multi-sensor fusion positioning, including:

The construction module 510 is used to construct a probability density function of the target's own pose based on the measurement results of multiple sensors corresponding to the target, and convert the probability density function into a weighted particle set expression for Monte Carlo positioning;

Segmentation module 520, configured to determine the position distribution of different weighted particles based on the weighted particle set expression, determine a selection box to cover a preset proportion of weighted particles based on the selection box, and divide the selection box into multiple Equispaced grid;

The placement module 530 is used to place weighted particles into the plurality of equidistant grids based on the position information of each weighted particle;

The determination module 540 is configured to count the sum of weights of all weighted particles in each of the equidistant grids, and determine the probability that the target object is in the corresponding equidistant grid based on the sum of weights.

The uncertainty expression device for multi-sensor fusion positioning provided by the above embodiment can realize the technical solution described in the embodiment of the uncertainty expression method for multi-sensor fusion positioning. The specific implementation principles of each module or unit mentioned above can be found in the above-mentioned multi-sensor. The corresponding contents in the embodiments of the uncertainty expression method for fusion positioning will not be described again here.

As shown in Figure 6, the present invention also provides an electronic device 600. The electronic device 600 includes a processor 601, a memory 602 and a display 603. FIG. 6 shows only some components of the electronic device 600, but it should be understood that implementation of all illustrated components is not required, and more or fewer components may be implemented instead.

The memory 602 may 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 . In other embodiments, the memory 602 may also be an external storage device of the electronic device 600, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), or a secure digital (SD) equipped on the electronic device 600. card, flash card, etc.

Further, the memory 602 may also include both an internal storage unit of the electronic device 600 and an external storage device. The memory 602 is used to store application software and various types of data installed on the electronic device 600 .

In some embodiments, the processor 601 may be a central processing unit (CPU), a microprocessor or other data processing chip, used to run program codes or process data stored in the memory 602, such as in the present invention. Uncertainty expression method for multi-sensor fusion positioning.

In some embodiments, the display 603 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, an OLED (Organic Light-Emitting Diode, organic light-emitting diode) touch device, etc. The display 603 is used to display information on the electronic device 600 and to display a visual user interface. Components 601-603 of electronic device 600 communicate with each other via a system bus.

In some embodiments of the present invention, when the processor 601 executes the uncertainty expression program of multi-sensor fusion positioning in the memory 602, the following steps may be implemented:

Based on the measurement results of multiple sensors corresponding to the target object, construct a probability density function of the target object's own pose, and convert the probability density function into a weighted particle set expression for Monte Carlo positioning;

Determine the position distribution of different weighted particles based on the weighted particle set expression, determine a selection box to cover a preset proportion of weighted particles based on the selection box, and divide the selection box into a plurality of equidistant grids;

Based on the position information of each weighted particle, place the weighted particles into the plurality of equidistant grids;

The sum of the weights of all weighted particles in each of the equidistant grids is counted, and based on the sum of the weights, the probability that the target object is in the corresponding equidistant grid is determined.

It should be understood that when the processor 601 executes the multi-sensor fusion positioning uncertainty expression program in the memory 602, in addition to the above functions, it can also implement other functions. For details, please refer to the previous description of the corresponding method embodiment.

Further, the embodiment of the present invention does not specifically limit the type of electronic device 600 mentioned. The electronic device 600 can be a mobile phone, a tablet computer, a personal digital assistant (personal digital assistant, PDA), a wearable device, a laptop computer ( laptop) and other portable electronic devices. Exemplary embodiments of portable electronic devices include, but are not limited to, portable electronic devices equipped with IOS, Android, Microsoft, or other operating systems. The above-mentioned portable electronic device may also be other portable electronic devices, such as a laptop computer (laptop) with a touch-sensitive surface (such as a touch panel). It should also be understood that in some other embodiments of the present invention, the electronic device 600 may not be a portable electronic device, but a desktop computer having a touch-sensitive surface (eg, a touch panel).

In another aspect, the present invention also provides a non-transitory computer-readable storage medium on which a computer program is stored. The computer program is implemented when executed by a processor to perform the uncertainty of multi-sensor fusion positioning provided by the above methods. Expression method, which includes:

Based on the measurement results of multiple sensors corresponding to the target object, construct a probability density function of the target object's own pose, and convert the probability density function into a weighted particle set expression for Monte Carlo positioning;

Determine the position distribution of different weighted particles based on the weighted particle set expression, determine a selection box to cover a preset proportion of weighted particles based on the selection box, and divide the selection box into a plurality of equidistant grids;

Based on the position information of each weighted particle, place the weighted particles into the plurality of equidistant grids;

The sum of the weights of all weighted particles in each of the equidistant grids is counted, and based on the sum of the weights, the probability that the target object is in the corresponding equidistant grid is determined.

Those skilled in the art can understand that all or part of the process of implementing the method of the above embodiments can be completed by instructing relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium. Among them, the computer-readable storage media is a magnetic disk, an optical disk, a read-only memory or a random access memory, etc.

The uncertainty expression method, device and electronic equipment for multi-sensor fusion positioning provided by the present invention have been introduced in detail above. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only It is used to help understand the method and its core idea of the present invention; at the same time, for those skilled in the art, there will be changes in the specific implementation and application scope according to the idea of the present invention. In summary, this specification The contents should not be construed as limitations of the invention.

Claims (10)

1. An uncertainty expression method for multi-sensor fusion positioning, which is characterized by including: Based on the measurement results of multiple sensors corresponding to the target object, construct a probability density function of the target object's own pose, and convert the probability density function into a weighted particle set expression for Monte Carlo positioning; Determine the position distribution of different weighted particles based on the weighted particle set expression, determine a selection box to cover a preset proportion of weighted particles based on the selection box, and divide the selection box into a plurality of equidistant grids; Based on the position information of each weighted particle, place the weighted particles into the plurality of equidistant grids; The sum of the weights of all weighted particles in each of the equidistant grids is counted, and based on the sum of the weights, the probability that the target object is in the corresponding equidistant grid is determined. 2. The uncertainty expression method of multi-sensor fusion positioning according to claim 1, characterized in that the probability density function is: Among them, x _t represents the detected position of the target object itself at time t; z _1:t represents the observation value of the lidar from time 1 to t; u _1:t is a set of preset control quantities; Represents the preset grid map, m _n refers to the occupancy probability of the nth grid. 3. The uncertainty expression method of multi-sensor fusion positioning according to claim 2, characterized in that said converting the probability density function into a weighted particle set expression of Monte Carlo positioning includes: Convert the probability density function into a rewritten expression based on Bayes' rule and Markov hypothesis; Approximate the rewritten expression based on preset weighted particles to obtain a weighted particle set expression for Monte Carlo positioning; Among them, the rewritten expression is κ is the normalization coefficient, P(x _t |u _1:t ,x _t-1 ) is the motion model, and P(z _t |x _t ) is the observation model. 4. The uncertainty expression method of multi-sensor fusion positioning according to claim 3, characterized in that the weighted particle set expression of the Monte Carlo positioning is in, is a particle that detects the pose of the target object itself, N _p is the number of particles, /> is the weight of the Np _-th particle, and δ is the Dirac trigonometric function. 5. The uncertainty expression method of multi-sensor fusion positioning according to claim 3, characterized in that the motion model is: Among them, Δu _x , Δu _y , Δu _θ are random variables that satisfy the following distribution; in is the odometer parameter; (pi _,x ,pi _,y ) and p _i,θ are the position and direction of the i-th way point p _i in the future way point set, p _i,θ is determined by p _i-1 and p _ii Calculated; restrict(p _{i, θ} , p _{i-1, θ} ) makes the angle difference between p _{i, θ} and p _{i-1, θ} belong to [-π, π);/> and/> In order to detect the movement of the target object/> and rotate/> translational and rotational variations. 6. The uncertainty expression method of multi-sensor fusion positioning according to claim 5, 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 in the map coordinate system corresponding to the temporary map; When 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 waypoints. 7. The uncertainty expression method of multi-sensor fusion positioning according to claim 6, characterized in that the measurement prediction of lidar based on the updated temporary map and the future waypoint set includes: Perform a ray casting algorithm on each future waypoint in the set of future waypoints, generate a plurality of simulated lidar point clouds from the updated temporary map, and perform a lidar calculation based on the plurality of simulated lidar points. Measure predictions. 8. An uncertainty expression device for multi-sensor fusion positioning, characterized by including: A building module for constructing a probability density function of the target's own pose based on the measurement results of multiple sensors corresponding to the target, and converting the probability density function into a weighted particle set expression for Monte Carlo positioning; A segmentation module for determining the position distribution of different weighted particles based on the weighted particle set expression, determining a selection box to cover a preset proportion of weighted particles based on the selection box, and dividing the selection box into multiple equal parts. distance grid; A delivery module, configured to deliver weighted particles into the plurality of equidistant grids based on the position information of each weighted particle; A determination module, configured to count the sum of weights of all weighted particles in each of the equidistant grids, and determine the probability that the target object is in the corresponding equidistant grid based on the sum of weights. 9. An electronic device, characterized by comprising a memory and a processor, wherein, 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 uncertainty expression method of multi-sensor fusion positioning as described in any one of claims 1 to 7. steps in. 10. A non-transitory computer-readable storage medium with a computer program stored thereon, characterized in that when the computer program is executed by a processor, the multi-sensor fusion positioning as claimed in any one of claims 1 to 7 is achieved. method of expressing uncertainty.