WO2023131048A1 - Position and attitude information determining method and apparatus, electronic device, and storage medium - Google Patents

Abstract

A position and attitude information determining method. The method comprises: acquiring first particle positions and attitudes of N first particles corresponding to a movable device, wherein N is a positive integer greater than 1, and the first particle positions and attitudes are posterior positions and attitudes after longitudinal correction of corresponding first particles obtained at a previous moment (201); performing transverse positioning prediction on positions and attitudes of the first particles at a current moment on the basis of the first particle positions and attitudes of the first particles, respectively, to obtain first predicted positions and attitudes respectively corresponding to the first particles (202); determining a first estimated position and attitude of the movable device on the basis of the first predicted positions and attitudes respectively corresponding to the first particles (203); and using the first estimated position and attitude of the movable device as position and attitude information of the movable device at the current moment (204). The method allows for decoupling of transverse correction and longitudinal correction, and effectively improve the positioning accuracy. Also provided are a position and attitude information determining apparatus, an electronic device, and a storage medium.

Description

Method, device, electronic device and storage medium for determining pose information

This disclosure claims the priority of the Chinese patent application with application number 202210013207.7 and titled "Method and device for determining pose information, electronic equipment, and storage medium" filed on January 6, 2022, the entire contents of which are incorporated by reference incorporated in this disclosure.

technical field

The present disclosure relates to mobile device technology, in particular to a method, device, electronic device and storage medium for determining pose information.

Background technique

When performing high-precision positioning of mobile devices (such as vehicles) based on visual observations and high-precision maps, GNSS (Global Navigation Satellite System) is usually used to determine the global pose (including position) of mobile devices on the map. and attitude), and then perform precise position tracking based on the repeated process of prediction-correction-prediction-correction. In position tracking, the particle filter algorithm is usually used to achieve, but the existing pose determination method based on the particle filter algorithm has the coupling of horizontal and vertical observations, which easily leads to conflicts in the positioning of lane lines and arrow landmarks, resulting in low positioning accuracy .

Contents of the invention

In order to solve the technical problem of the above-mentioned coupling of lateral and longitudinal observations, the present disclosure is proposed. Embodiments of the present disclosure provide a method, device, electronic device, and storage medium for determining pose information.

According to an aspect of an embodiment of the present disclosure, a method for determining pose information is provided, including: acquiring the first particle poses of N first particles corresponding to the mobile device, where N is a positive integer greater than 1; the first The particle pose is the longitudinally corrected posterior pose corresponding to the first particle obtained at the previous moment; based on the first particle pose of each first particle, the pose of each first particle at the current moment is laterally positioned Forecasting, obtaining the first predicted pose corresponding to each first particle; determining the first estimated pose of the movable device based on the first predicted pose corresponding to each first particle; pose as the pose information of the mobile device at the current moment.

According to another aspect of the embodiments of the present disclosure, an apparatus for determining pose information is provided, including: a first acquisition module, configured to acquire the first particle poses of N first particles corresponding to the mobile device, where N is A positive integer greater than 1; the first particle pose is the longitudinally corrected posterior pose corresponding to the first particle obtained at the previous moment; the first processing module is used to respectively base on the first particle pose of each first particle , perform lateral positioning prediction on the pose of each first particle at the current moment, and obtain the first predicted pose corresponding to each first particle; the second processing module is used to obtain the first predicted pose corresponding to each first particle based on The pose is to determine the first estimated pose of the mobile device; the third processing module is configured to use the first estimated pose of the movable device as the pose information of the mobile device at the current moment.

According to still another aspect of the embodiments of the present disclosure, a computer-readable storage medium is provided, the storage medium stores a computer program, and the computer program is used to execute the method for determining pose information in any of the above-mentioned embodiments of the present disclosure.

According to yet another aspect of the embodiments of the present disclosure, an electronic device is provided, and the electronic device includes: a processor;

A memory for instructions; a processor, configured to read executable instructions from the memory, and execute the instructions to implement the method for determining pose information in any of the above-mentioned embodiments of the present disclosure.

Based on the method and device for determining pose information, electronic equipment, and storage media provided by the above-mentioned embodiments of the present disclosure, first perform lateral positioning prediction to obtain the predicted pose of the particle, and then estimate the pose of the mobile device based on the predicted pose of the particle. Then determine the vertical pose correction amount based on the estimated pose of the mobile device, which is used for vertical correction of the particles in the lateral positioning prediction. The influence of correction, because in the process of horizontal positioning prediction, the horizontal positioning prediction is performed based on the particles after the vertical correction at the previous moment, without introducing vertical noise, and avoiding the distribution of particles in the vertical direction, so that each particle is only different in the horizontal direction, and the vertical direction is different. The correction is carried out in the follow-up, realizing the decoupling of horizontal correction and vertical correction, which effectively solves the problem of low positioning accuracy due to the coupling of horizontal and vertical observations in the prior art.

The technical solution of the present disclosure will be described in further detail below with reference to the drawings and embodiments.

Description of drawings

The above and other objects, features and advantages of the present disclosure will become more apparent by describing the embodiments of the present disclosure in more detail with reference to the accompanying drawings. The accompanying drawings are used to provide a further understanding of the embodiments of the present disclosure, and constitute a part of the specification, and are used together with the embodiments of the present disclosure to explain the present disclosure, and do not constitute limitations to the present disclosure. In the drawings, the same reference numerals generally represent the same components or steps.

FIG. 1 is an exemplary application scenario of the method for determining pose information provided by the present disclosure;

Fig. 2 is a schematic flowchart of a method for determining pose information provided by an exemplary embodiment of the present disclosure;

Fig. 3 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure;

FIG. 4 is a schematic flowchart of step 301 provided by an exemplary embodiment of the present disclosure;

FIG. 5 is a schematic flowchart of step 203 provided by an exemplary embodiment of the present disclosure;

Fig. 6 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure;

Fig. 7 is a schematic diagram of a first grid coordinate area provided by an exemplary embodiment of the present disclosure;

Fig. 8 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure;

Fig. 9 is a schematic diagram of the principle of gridding-based particle swarm resampling provided by an exemplary embodiment of the present disclosure;

FIG. 10 is a schematic diagram of the principle of grid-based particle swarm resampling provided by another exemplary embodiment of the present disclosure;

FIG. 11 is a schematic flowchart of step 3011 provided by an exemplary embodiment of the present disclosure;

Fig. 12 is a schematic flowchart of step 3015 provided by an exemplary embodiment of the present disclosure;

FIG. 13 is a schematic flowchart of step 2031 provided by an exemplary embodiment of the present disclosure;

Fig. 14 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure;

Fig. 15 is a schematic diagram of the principle of horizontal and vertical correction of particle swarms provided by an exemplary embodiment of the present disclosure;

Fig. 16 is a schematic structural diagram of an apparatus for determining pose information provided by an exemplary embodiment of the present disclosure;

Fig. 17 is a schematic structural diagram of an apparatus for determining pose information provided by another exemplary embodiment of the present disclosure;

Fig. 18 is a schematic structural diagram of a first vertical processing module 505 provided by an exemplary embodiment of the present disclosure;

Fig. 19 is a schematic structural diagram of a second processing module 503 provided by an exemplary embodiment of the present disclosure;

Fig. 20 is a schematic structural diagram of a device for determining pose information provided by another exemplary embodiment of the present disclosure;

Fig. 21 is a schematic structural diagram of an application embodiment of the electronic device of the present disclosure.

Detailed ways

Hereinafter, exemplary embodiments according to the present disclosure will be described in detail with reference to the accompanying drawings. Apparently, the described embodiments are only some of the embodiments of the present disclosure, rather than all the embodiments of the present disclosure, and it should be understood that the present disclosure is not limited by the exemplary embodiments described here.

It should be noted that relative arrangements of components and steps, numerical expressions and numerical values set forth in these embodiments do not limit the scope of the present disclosure unless specifically stated otherwise.

Those skilled in the art can understand that terms such as "first" and "second" in the embodiments of the present disclosure are only used to distinguish different steps, devices or modules, etc. necessary logical sequence.

It should also be understood that in the embodiments of the present disclosure, "plurality" may refer to two or more than two, and "at least one" may refer to one, two or more than two.

It should also be understood that any component, data or structure mentioned in the embodiments of the present disclosure can generally be understood as one or more unless there is a clear limitation or a contrary suggestion is given in the context.

In addition, the term "and/or" in the present disclosure is only an association relationship describing associated objects, indicating that there may be three relationships, for example, A and/or B may indicate: A exists alone, and A and B exist at the same time , there are three cases of B alone. In addition, the character "/" in the present disclosure generally indicates that the contextual objects are an "or" relationship.

It should also be understood that the description of the various embodiments in the present disclosure emphasizes the differences between the various embodiments, and the same or similar points can be referred to each other, and for the sake of brevity, details are not repeated here.

At the same time, it should be understood that, for the convenience of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship.

The following description of at least one exemplary embodiment is merely illustrative in nature and in no way intended as any limitation of the disclosure, its application or uses.

Techniques, methods and devices known to those of ordinary skill in the relevant art may not be discussed in detail, but where appropriate, such techniques, methods and devices should be considered part of the description.

It should be noted that like numerals and letters denote like items in the following figures, therefore, once an item is defined in one figure, it does not require further discussion in subsequent figures.

Embodiments of the present disclosure may be applied to electronic devices such as terminal devices, computer systems, servers, etc., which may operate with numerous other general purpose or special purpose computing system environments or configurations. Examples of well known terminal devices, computing systems, environments and/or configurations suitable for use with electronic devices such as terminal devices, computer systems, servers include, but are not limited to: personal computer systems, server computer systems, thin clients, thick client Computers, handheld or laptop devices, microprocessor-based systems, set-top boxes, programmable consumer electronics, networked personal computers, minicomputer systems, mainframe computer systems, and distributed cloud computing technology environments including any of the foregoing, etc.

Electronic devices such as terminal devices, computer systems, servers, etc. may be described in the general context of computer system-executable instructions, such as program modules, being executed by the computer system. Generally, program modules may include routines, programs, objects, components, logic, data structures, etc., that perform particular tasks or implement particular abstract data types. The computer system/server can be practiced in distributed cloud computing environments where tasks are performed by remote processing devices that are linked through a communications network. In a distributed cloud computing environment, program modules may be located in both local and remote computing system storage media including storage devices.

Overview of the Disclosure

In the process of implementing the present disclosure, the inventors found that when a mobile device (such as a vehicle) is positioned with high precision based on visual observation and a high-precision map, the particle filter algorithm is usually used to determine the pose of the mobile device. However, In the existing pose determination method based on the particle filter algorithm, when predicting, the motion model based on it will introduce translation component noise and rotation noise to represent the cumulative error of motion. The translation component noise includes lateral noise and longitudinal noise, so that The coupling of horizontal and vertical observations can easily lead to conflicts in the positioning of lane lines and arrow road signs, resulting in low positioning accuracy.

Exemplary overview

Fig. 1 is an exemplary application scenario of the method for determining pose information provided by the present disclosure.

For high-precision positioning scenarios of mobile devices, taking vehicles as an example, when high-precision positioning of vehicles is required, it is necessary to first determine the initial pose of the vehicle in the navigation map, and then perform high-precision positioning in the subsequent movement of the vehicle based on the initial pose. Accurate positioning, during the movement process, using the determination method of the pose information provided by the embodiment of the present disclosure, the horizontal pose correction and the vertical pose correction can be decoupled, and at the current moment, based on the particles of the vertical correction at the previous moment Horizontal positioning prediction, based on the particle pose after horizontal correction, determine the current pose of the vehicle, and at the same time correct the particle pose after vertical correction at the current moment and use it as the particle at the next moment, so as to avoid horizontal and vertical Observe the coupling situation to improve the positioning accuracy of the pose. In Fig. 1, x and y represent the x-axis and y-axis of the vehicle's self-coordinate system respectively, the y direction is the horizontal direction, and the x direction is the longitudinal direction. When locating the vehicle, it is necessary to combine the observation results of lane lines and arrow landmarks. The observation results refer to the matching results of the lane lines and arrow landmarks recognized by the environment image collected by the camera and the lane lines and arrows in the map. Among them, the lane line mainly affects the horizontal correction, and the arrow mainly affects the vertical correction. In the embodiment of the present disclosure, the observation of the lane line and the observation of the arrow are used for the horizontal correction and the vertical correction respectively, thereby decoupling the horizontal observation and the vertical observation, avoiding For better observation of arrows, the observation of lane markings becomes worse, or for better observation of lane markings, the observation of arrows becomes worse. In the embodiment of the present disclosure, the pose includes three degrees of freedom including a horizontal coordinate component y, a vertical coordinate component x, and a heading angle θ.

exemplary method

Fig. 2 is a schematic flowchart of a method for determining pose information provided by an exemplary embodiment of the present disclosure. This embodiment can be applied to electronic equipment, specifically on a vehicle-mounted computing platform, as shown in Figure 2, including the following steps:

Step 201, obtain the first particle poses of the N first particles corresponding to the mobile device, where N is a positive integer greater than 1; the first particle poses are obtained at the previous moment after longitudinal correction of the corresponding first particles. Check pose.

Wherein, the mobile device may be any mobile device that needs to be positioned, such as a vehicle or a robot. The first particle poses of the N first particles may be the poses of the N first particles after vertical correction at the previous moment.

Step 202: Based on the first particle poses of the first particles, perform lateral positioning prediction on the poses of the first particles at the current moment, and obtain the first predicted poses corresponding to the first particles.

Wherein, the horizontal positioning prediction refers to the correction without considering the vertical direction, that is, the vertical noise of the translation component is not introduced into the motion model during the prediction process. The motion model may adopt an odometer-based motion model (Odometry Sample Motion Model) or other practicable motion models, which are not limited in the present disclosure.

Exemplarily, taking a first particle as an example, the first particle pose of the first particle is the original pose, based on the odometer information, the translation distance and rotation angle during the period from the previous moment to the current moment can be determined , inputting the original pose, the translation distance and the rotation angle into the motion model, the first predicted pose corresponding to the first particle can be output.

Step 203: Determine a first estimated pose of the mobile device based on the first predicted poses corresponding to the respective first particles.

After obtaining the first predicted poses corresponding to each first particle, the first estimated pose of the mobile device can be determined based on the first predicted poses corresponding to each first particle. The weights corresponding to the first particles are weighted and averaged for each of the first predicted poses as the first estimated pose of the mobile device, wherein the weights corresponding to the first particles used in the weighted average can be based on certain rules The updated weights.

In step 204, the first estimated pose of the mobile device is used as pose information of the mobile device at the current moment.

Since the horizontal correction is performed based on the particle swarm after the vertical correction at the previous moment, the correction at the current moment comprehensively considers the effects of vertical correction and horizontal correction, and the first estimated pose has a higher value in both horizontal and vertical directions. Using the first estimated pose as the pose information of the mobile device in the map at the current moment can achieve high-precision positioning of the mobile device.

In the embodiment of the present disclosure, the vertical correction at each moment is performed after the horizontal correction. It can be understood that the horizontal correction and the vertical correction are a cyclic and iterative process with the passage of time, and the above steps also exist at the previous moment. Operation, after the first estimated pose of the mobile device is obtained in step 203 at the previous moment, each particle is vertically corrected based on the first estimated pose, and the pose after vertical correction is obtained, as the above-mentioned N The first particle pose of the first particle.

The method for determining the pose information provided in this embodiment, at the current moment, based on the particle swarm corrected vertically at the previous moment, first performs lateral positioning prediction to obtain the predicted pose of the particles, and then estimates the position of the mobile device based on the predicted pose of the particles. For the vertical orientation, the current moment introduces the influence of the vertical correction at the previous moment. Since the horizontal positioning prediction process is based on the particles after the vertical correction at the previous moment, the horizontal positioning prediction is performed without introducing vertical noise, so as to avoid particles in the The distribution in the vertical direction makes each particle only different in the horizontal direction, and the vertical correction is carried out later, realizing the decoupling of the horizontal correction and the vertical correction, effectively solving the problem of low positioning accuracy due to the coupling of horizontal and vertical observations in the prior art question.

In an optional example, FIG. 3 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure. In this example, in step 203, based on the first predicted position corresponding to each first particle pose, after determining the first estimated pose of the mobile device, the disclosed method further includes:

Step 301, based on a first estimated pose of a mobile device, determine a first longitudinal pose correction amount.

Step 302: Perform longitudinal correction on the first predicted pose of each first particle based on the first longitudinal pose correction amount, and obtain the posterior pose and pose corresponding to each first particle at the current moment.

Specifically, after the horizontal correction is performed at the current moment, the first vertical pose correction amount used for vertical correction is determined based on the first estimated pose of the mobile device, so that the first predicted pose corresponding to each first particle Perform vertical correction, as the posterior pose of the current moment corresponding to each first particle, and use it as the basis of the processing flow at the next moment, or perform resampling as the basis of the processing flow at the next moment, so that the vertical direction at the current moment Corrective effects are introduced at the next moment.

The present disclosure determines the current pose of the mobile device based on the pose of the particle swarm after the horizontal correction at the current moment, and at the same time performs vertical correction on the particle swarm after the horizontal correction, so that the influence of the vertical correction will take effect at the next moment. In the case of horizontal and vertical decoupling, the vertical correction at the current moment is avoided to reduce the horizontal positioning accuracy, so as to ensure the horizontal positioning accuracy.

In an optional example, FIG. 4 is a schematic flowchart of step 301 provided by an exemplary embodiment of the present disclosure. In this example, step 301 may specifically include the following steps:

Step 3011, centering on the first estimated pose, sampling in the vertical direction to obtain the second particle poses of M second particles, where M is a positive integer greater than 1.

Specifically, a certain particle may be sampled forward and backward at fixed intervals in the vertical direction with the first estimated pose as the center, so as to obtain the second particle pose including M second particles in the center. The specific interval may be set according to actual requirements, which is not limited in this embodiment of the present disclosure.

Step 3012: Obtain prior weights corresponding to each second particle based on the Gaussian distribution.

Exemplarily, a Gaussian distribution is established centering on the first estimated pose to determine the prior weights corresponding to each second particle, as follows:

in,

Represents the prior weight of the i-th second particle sampled, i=-H,-(H-1),...,0,1,...,H, H is a positive integer, M=2H+1, x represents the The longitudinal distance between i second particles and the center, dx represents the sampling interval, mean represents the mean value, and σ represents the variance.

Optionally, the number of forward-sampled particles centered on the first estimated pose may also be different from the number of backward-sampled particles, for example, i=-S,-(S-1),...,0,1,... , H, S, and H are all positive integers, M=S+H+1, which can be set according to actual needs, and is not limited in this disclosure.

Step 3013, obtaining the arrow landmark observation results and pose measurement results corresponding to each second particle.

Wherein, the arrow landmark observation result corresponding to a second particle includes the observation probability of the arrow landmark (referred to as the arrow observation probability) under the second particle pose corresponding to the second particle; the pose measurement result corresponding to a second particle is included in The observation probability of the pose of the mobile device under the pose of the second particle corresponding to the second particle (referred to as the pose observation probability). Taking the observation result of an arrow road sign corresponding to a second particle as an example, the pose of the second particle corresponding to the second particle is used as the origin of the self-coordinate system of the mobile device, and the self-coordinate system of the mobile device obtained by image recognition is Transform the arrow landmarks of the map to the map coordinate system, or transform the arrow elements in the map to the self-coordinate system of the mobile device, match the observed arrow landmarks with the arrow elements of the map in the same coordinate system, and obtain For the observation result of the arrow landmark corresponding to the second particle, the better the matching degree, the higher the score, and the greater the observation probability of the arrow landmark. The specific score calculation function can be set according to actual needs, which is not limited in this disclosure. The pose measurement result corresponding to the second particle is determined based on the matching result between the GNSS pose of the mobile device and the second particle pose corresponding to the second particle, specifically based on the GNSS pose of the mobile device The distance from the pose of the second particle is determined. The larger the distance, the smaller the probability of pose observation included in the pose measurement result.

Step 3014: Based on the arrow landmark observation results and pose measurement results corresponding to the second particles, the prior weights corresponding to the second particles are updated to obtain the first weights corresponding to the second particles.

Optionally, for a second particle, the product of the prior weight of the second particle, the arrow observation probability, and the pose observation probability may be used as the first weight corresponding to the second particle.

Exemplary,

in,

That is, the first weight corresponding to the i-th second particle,

That is, it represents the prior weight of the i-th second particle sampled,

Indicates the pose observation probability corresponding to the i-th second particle,

Indicates the arrow observation probability corresponding to the i-th second particle, i=-H,-(H-1),...,0,1,...,H, H is a positive integer, M=2H+1.

Among them, ∝ means proportional, exp() means exponential function,

Indicates the GNSS pose observed at time t,

express will

Converted from UTM (Universal Transverse Mercator Grid System, Universal Transverse Mercator Grid System) to the map coordinate system, (x, y) represents the translation component of the pose X. In this example, X refers to the i-th second particle The corresponding second particle pose

means to calculate the matching score corresponding to the i-th second particle, f _ARROW () means to extract the arrow signpost from the semantically segmented image, π(,X) means to assume that the pose of the origin of the self-coordinate system of the movable device is X In the case of , project the arrow onto the map coordinate system. In this example, X is the pose of the second particle corresponding to the i-th second particle

d(I _t ) represents the semantic segmentation of the image collected at time t,

Represents an arrow element in the map.

Step 3015: Determine a first longitudinal pose correction amount based on the first weights corresponding to each second particle.

Optionally, based on the first weights respectively corresponding to the second particles, the first longitudinal pose correction amount may be obtained by means of density estimation.

This disclosure obtains the vertical pose correction amount by longitudinally sampling M second particles, combining arrow observation and pose measurement, acting on each first particle, and then using Positioning at the next moment, so that the influence of the vertical correction at the current moment will be applied to the next moment, effectively realizing the decoupling of the horizontal and vertical corrections, and improving the accuracy of horizontal and vertical positioning.

In an optional example, FIG. 5 is a schematic flowchart of step 203 provided by an exemplary embodiment of the present disclosure. In this example, step 203 may specifically include the following steps:

Step 2031, based on the first predicted pose corresponding to each first particle, update the second weight corresponding to each first particle respectively, and obtain the third weight corresponding to each first particle; The second weight is the prior weight of the first particle.

Wherein, the prior weight of the first particle may be the weight after vertical correction at the previous moment, which may be specifically set according to actual requirements. Taking the update of the second weight of a first particle as an example, it means that the first predicted pose of the first particle is used as the origin of the self-coordinate system of the mobile device to observe the lane line, and based on the observation result of the lane line, the The second weight of the first particle is updated to obtain the third weight corresponding to the first particle.

Step 2032: Determine a first estimated pose of the mobile device based on the first predicted pose and the third weight respectively corresponding to each first particle.

Specifically, based on the third weights respectively corresponding to the first particles, the first predicted poses are weighted and averaged, and the obtained results are used as the first estimated poses of the mobile device.

in,

That is, represents the first estimated pose of the mobile device at the current moment (time t),

Indicates the third weight of the jth first particle at time t,

Indicates the first predicted pose corresponding to the jth first particle.

Optionally, it is also possible to normalize the third weights respectively corresponding to the first particles first, and then weight the first predicted poses based on the normalized weights.

In an optional example, the determination of the first estimated pose of the mobile device may also be determined based on some of the first particles whose third weight is greater than a certain threshold among the N first particles, for example, there are a total of 20 first particles , where the weights of two first particles are very small and are smaller than the minimum threshold, these two particles are not considered when performing weighted averaging, and the specific minimum threshold can be set according to actual needs.

The present disclosure updates the weights of each first particle based on the predicted pose of lateral positioning prediction, and then performs weighted average of the first predicted poses of each first particle based on the updated weights to determine the pose of the mobile device. Since the particle swarm pose based on the horizontal positioning prediction is the vertically corrected pose at the previous moment, the horizontal prediction does not introduce vertical noise, so that the update of the particle swarm weights comprehensively considers the influence of vertical and horizontal noise, and there is no coupling between them. , so that the pose information of the mobile device obtained has high accuracy in both horizontal and vertical directions.

In an optional example, FIG. 6 is a schematic flowchart of a method for determining pose information provided in another exemplary embodiment of the present disclosure. In this example, after step 202, the method of the present disclosure further includes:

Step 4011, based on the pose information of the mobile device at the previous moment and the odometer information at the current moment, determine the second predicted pose of the movable device at the current moment.

Among them, the pose information of the mobile device at the previous moment refers to the estimated pose after lateral correction obtained by the processing flow at the previous moment, which is consistent with the first estimated pose at the current moment, and the odometer information includes the The translational distance and rotation angle during the time period from time to time, based on the pose information of the mobile device at the previous time and the odometer information at the current time, using the pre-acquired motion model, the current time Predict the pose of the mobile device to obtain the second predicted pose of the mobile device at the current moment. Wherein, the motion model may adopt an odometer-based motion model (Odometry Sample Motion Model). Consistent with particle prediction, in the motion model adopted, the vertical noise parameter is 0, that is, no vertical noise is introduced, and only horizontal positioning prediction is performed.

Step 4012, establish the first grid coordinate area based on the second predicted pose; the first coordinate axis of the first grid coordinate area is the lateral direction of the pose, and the second coordinate axis of the first grid coordinate area is the Heading direction.

Wherein, the first grid coordinate area includes N cells, N=N _y *N _θ , N _y and N _θ represent the number of cells in the first grid coordinate area in the lateral direction and the heading angle direction, respectively.

Wherein, taking the abscissa and heading angle of the second predicted pose as the center of the first grid coordinate area, the size of the first grid coordinate area can be based on the abscissa and The heading angle is determined based on the principle that, for example, the two-dimensional coordinate points composed of the abscissa and heading angle of most of the first particles are in the first grid coordinate area, which can be set according to actual needs.

Exemplarily, the cell step sizes in the y and θ directions are set as d _y and d _θ respectively to establish the first grid coordinate area, and each cell uses [m, n] to represent its position, including the following situations:

If both N _y and N _θ are even numbers, then

If both N _y and N _θ are odd numbers, then

If one of N _y and N _θ is an odd number and the other is an even number, then m and n are determined according to the above two situations, and details will not be repeated here.

Correspondingly, the coordinate range of each cell in the first grid coordinate area can be based on the specific conditions of the cell position [m, n], N _y and N _θ , and the y and θ directions of each cell The step size d _y and d _θ are determined, and details will not be repeated here.

For example, when N _y and N _θ are both even numbers, the cell [m, n]=[1,2], then the coordinate range of the cell [m, n] is:

y direction: [d _y *(m-1),d _y *m]=[0,d _y ]

θ direction: [d _θ *(n-1),d _θ *n]＝[d _θ ,2d _θ ]

Step 4013, based on the first predicted pose corresponding to each first particle, map each first particle to the first grid coordinate area, and obtain the cell to which each first particle belongs.

Specifically, the coordinates of each first particle in the cell are the transformation of the first predicted pose of the first particle relative to the prior pose of the first particle (that is, the pose after longitudinal correction at the previous moment) ,details as follows:

in,

That is, the coordinates of the jth first particle in the first grid coordinate area,

Indicates the prior pose of the mobile device at the current moment (that is, at time t), that is, the above-mentioned second predicted pose obtained by predicting the pose of the mobile device at the current moment based on the motion model,

Indicates the first predicted pose of the jth first particle at the current moment

make

definition

c[0]=(a[0]-b[0])*cos(b[2])+(a[1]-b[1])*sin(b[2])

c[1]=(a[1]-b[1])*cos(b[2])-(a[1]-b[1])*sin(b[2])

c[2]=a[2]-b[2]

It can be seen that the coordinates in the first grid coordinate area obtained by transformation also include three degrees of freedom, namely, the longitudinal coordinate c[0], the transverse coordinate c[1] and the heading angle coordinate c[2]. When mapping to the first grid coordinate area, it is enough to map based on the horizontal coordinates and the heading angle coordinates. When merging the particles in the cells, after the merging, the inverse transformation of the above transformation is required to transform back to the self-coordinate system , so the coordinates of the three degrees of freedom need to be combined.

Exemplarily, FIG. 7 is a schematic diagram of a first grid coordinate area provided by an exemplary embodiment of the present disclosure. Among them, y represents the lateral direction of the mobile device in the self-coordinate system, and θ represents the direction of the heading angle. Take N=24, N _y =6, N _θ =4 as an example, that is, the particle swarm includes 20 first particles (black dots in the figure), which are mapped to the first grid coordinate area, and the mapping result is, possibly One or more first particles fall into some cells, while no first particle falls into another part of the cells, and some first particles may fall outside the coordinate area of the first grid.

Step 4014, based on the cell to which each first particle belongs, determine the third particle corresponding to each cell and the third particle pose corresponding to each third particle.

Wherein, determining the third particle corresponding to each cell means, based on the cell to which each first particle belongs, each cell is represented by a third particle, regardless of whether there is a first particle in the cell, it is converted into a third particle. The specific conversion method can be set according to actual needs. For example, for a cell with multiple first particles, multiple first particles can be merged, and the merged result can be used as the third particle corresponding to the cell. For cells without first particles cell, a particle can be supplemented as the third particle corresponding to the cell according to the distance between the cell and the center.

Exemplarily, the third particle pose of the only third particle in the merged cell is:

The weight of the jth third particle obtained after merging is:

in,

Indicates the third particle pose of the corresponding third particle after the j-th cell is particleized, K represents the number of particles contained in the j-th cell,

Indicates the coordinates of the merged particles in the jth cell in the grid coordinate system,

Indicates the weight of the kth particle distributed in the jth cell (here, the normalized weight value),

Indicates the coordinates of the kth particle distributed in the jth cell in the grid coordinate system,

Indicates the second predicted pose of the mobile device at the current moment. If the kth particle is the first particle with the first predicted pose, then

Equal to the posterior weight of the kth first particle at the previous moment

is the coordinates of the first grid coordinate area after the above transformation, if the kth particle is a supplementary particle, then

is equal to the weight set for supplementary particles,

The horizontal coordinates and heading angle coordinates in are the center coordinates of the cell to which the particle belongs,

The vertical coordinate in is the initialization coordinate, and the specific initialization coordinate can be set according to actual needs, for example, it is set to 0, and there is no specific limitation.

Correspondingly, determining the first estimated pose of the mobile device based on the first predicted pose corresponding to each first particle in step 203 includes:

Step 4015: Determine the first estimated pose of the movable device based on the poses of the third particles corresponding to the third particles.

After obtaining the third particle pose of the third particle corresponding to each cell, it is equivalent to resampling the particle swarm of the first particle, so that the particle distribution of the particle swarm is stable, and then based on the corresponding The third particle pose determines the first estimated pose of the movable device.

In each prediction and observation process, the gathering and scattering of particles will be very different. In order to improve the stability of particle distribution, this disclosure achieves resampling by gridding the first particles, so that the particle swarm is stable. Distributed in the first grid coordinate area, it can effectively solve the problem of unstable particle distribution.

In an optional example, before step 4013, based on the first predicted pose corresponding to each first particle, each first particle is mapped to the first grid coordinate area, and the cell to which each first particle belongs is obtained , the method can also include:

For each first particle, multiple sampling is performed, for example, h times of sampling are performed, and finally h*N new first particles and new first predicted poses corresponding to each new first particle can be obtained. Correspondingly, step 4013 may specifically include mapping each new first particle to the first grid coordinate area based on the new first predicted pose corresponding to each new first particle, and obtaining the cell to which each new first particle belongs , the subsequent related steps are all processed based on the new first particle, and finally the third particle corresponding to each cell and the third particle pose corresponding to each third particle are obtained. The specific process is the same as the grid of the first particle above Similar to the above, the only difference is that the number of particles increases, so I won’t go into details here. Wherein, the multi-sampling method for any first particle may be: different h new first particles corresponding to the first particle are obtained by introducing different random noises in each sampling.

By sampling the N first particles multiple times, expanding the number of particles, and carrying out a grid process based on the expanded particle swarm, the positioning accuracy is improved on the basis of ensuring the stability of particle distribution.

In an optional example, FIG. 8 is a schematic flowchart of a method for determining pose information provided by yet another exemplary embodiment of the present disclosure. In this example, in step 4013, the first prediction based on each first particle respectively corresponds to pose, mapping each first particle to the first grid coordinate area, and after obtaining the cell to which each first particle belongs, the disclosed method further includes:

Step 4021: Delete the first particles mapped outside the coordinate area of the first grid to obtain the remaining first particles.

Step 4022, add a fourth particle to the center of each cell in the first grid coordinate area, and each fourth particle corresponds to a fourth weight.

Correspondingly, in step 4014, based on the cell to which each first particle belongs, determine the third particle corresponding to each cell and the third particle pose corresponding to each third particle, including:

Step 40141, based on the cell to which each remaining first particle belongs, the second weight corresponding to each remaining first particle, and the fourth weight corresponding to each fourth particle, merge the particles in each cell to obtain each The third particles corresponding to the cells and the poses of the third particles respectively corresponding to the third particles.

Specifically, since some first particles may be mapped outside the coordinate area of the first grid, and some cells may not fall into the first particle, in order to further improve the stability of particle distribution, at each moment, set The first particles mapped to outside the coordinate area of the first grid are deleted, and the remaining first particles are obtained. For the case of cells without first particles falling into them, a second grid is added to the center of each cell in the coordinate area of the first grid. Four particles, each fourth particle corresponds to a fourth weight, the fourth weight can be set according to actual needs, for example, a small weight can be set for it, such as 1/(N _y *N _θ )/100. The fourth particle pose corresponding to each fourth particle is the pose corresponding to the center of the corresponding cell. After the fourth particle is added, each cell has a particle distribution, and then based on the remaining first particles and the added fourth particles, the cells are particleized, and each cell is converted into a corresponding third particle, Thus, N third particles are obtained, which are stably distributed in each cell.

Exemplarily, FIG. 9 is a schematic diagram of the principle of gridding-based particle swarm resampling provided by an exemplary embodiment of the present disclosure. Wherein, each first particle is represented by a black dot, the added fourth particle is represented by a white dot, and each third particle obtained by cell particleization is represented by a gray dot.

In an optional example, it is also possible to add the fourth particle only in the cell where no first particle falls, and not increase the other cells. Correspondingly, in order to ensure the fairness of each cell, a certain amount of The weight can be set according to actual needs.

Exemplarily, FIG. 10 is a schematic diagram of the principle of gridding-based particle swarm resampling provided by another exemplary embodiment of the present disclosure. Among them, for each first particle, 3 new first particles corresponding to each first particle are obtained through 5 samples, so that 3*N new first particles are mapped to the first grid coordinate area, and N is still represented by 20 as an example, the new first particles are represented by black dots, the added fourth particles are represented by white dots, and the third particles obtained by cell particleization are represented by gray dots.

In an optional example, FIG. 11 is a schematic flowchart of step 3011 provided by an exemplary embodiment of the present disclosure. In this example, step 3011 may specifically include the following steps:

Step 30111, take the first estimated pose as the center particle, sample H fifth particles forward and backward according to the first interval in the longitudinal direction, and obtain 2H fifth particles, and each fifth particle and the first estimated position The second interval of posture, H is a positive integer.

Step 30112: Determine the pose of each fifth particle based on the first estimated pose and the second distance between each fifth particle and the first estimated pose.

Step 30113, using the central particle and each fifth particle as the second particle to obtain the second particle poses of M second particles, M=2H+1.

Exemplarily, the first estimated pose is expressed as

The first interval is denoted as d, then the corresponding pose increment is D=(d,0,0), then the pose of each fifth particle obtained by sampling is:

in,

That is, it represents the pose of the i-th fifth particle.

make

definition

c[0]=a[0]+b[0]*cos(a[2])-b[1]*sin(a[2])

c[1]=a[1]+b[0]*sin(a[2])+b[1]*cos(a[2])

c[2]=a[2]+b[2]

In an optional example, FIG. 12 is a schematic flowchart of step 3015 provided by an exemplary embodiment of the present disclosure. In this example, in step 3015, the first longitudinal position is determined based on the first weights corresponding to each second particle. Amount of posture correction, including:

Step 30151, based on the first weights corresponding to the second particles, determine the sum of the first weights of the second particles.

Step 30152: Determine the vertical correction amount based on the sum of the first weights of each second particle, the first weights corresponding to each second particle, and the second distance between each second particle and the first estimated pose.

Exemplarily, the vertical correction amount d _correct is expressed as:

in,

That is, the first weight corresponding to the i-th second particle,

represents the sum of the first weights of each second particle, d _i represents the second distance between the i-th second particle and the first estimated pose.

Step 30153: Determine the corresponding pose increment based on the longitudinal correction amount as the first longitudinal pose correction amount.

Exemplarily, the first longitudinal pose correction amount D _correct obtained based on the above-mentioned longitudinal correction amount d _correct is expressed as:

D _correct ＝(d _correct ,0,0)

After the first longitudinal pose correction amount is obtained, the first predicted pose of each first particle is longitudinally corrected based on the first longitudinal pose correction amount, and the posterior position corresponding to each first particle at the current moment is obtained posture

The specific expression is as follows:

in,

Indicates the first predicted pose corresponding to the jth first particle,

For the specific operation, see the above

I won't repeat them here.

In an optional example, FIG. 13 is a schematic flowchart of step 2031 provided by an exemplary embodiment of the present disclosure. In this example, in step 2031, based on the first predicted pose corresponding to each first particle, each first particle The second weights corresponding to each particle are updated to obtain the third weights corresponding to each first particle, including:

Step 20311, obtain the lane line observation results corresponding to each first particle.

Wherein, the lane line observation result corresponding to a first particle includes the lane line observation probability (lane line observation probability for short) at the first predicted pose corresponding to the first particle.

Specifically, take a first particle as an example, use the first predicted pose of the first particle as the origin of the self-coordinate system of the mobile device to observe the lane line, and compare the observed lane line with the lane in the map The line elements are transformed into the same coordinate system for matching. The higher the matching degree, the higher the score, the greater the probability of lane line observation, and the lower the matching degree, the lower the score, the lower the lane line observation probability.

Step 20312, based on the lane line observation results corresponding to the first particles, update the second weights corresponding to the first particles respectively, to obtain the third weights corresponding to the first particles.

Exemplary,

in,

That is to say, the third weight of the jth first particle at time t,

Indicates the second weight of the i-th first particle at time j, that is, the updated weight at time t-1 is taken as the prior weight at time t;

Indicates the lane line observation probability corresponding to the jth first particle, j=1,2,...,N.

Among them, the lane line observation probability corresponding to a first particle

Expressed as:

Among them, ω _LANE represents the preset weight of lane lines, max_dist represents the maximum distance threshold allowed for matching between observed lane lines and map lane line elements,

Indicates the Euclidean distance between the elements of the map lane line and the observed lane line at time t, and K (positive integer) indicates the number of lane lines. It can be seen that the larger the matching Euclidean distance, the lower the matching degree, and the smaller the probability of lane line observation , corresponding to the smaller the weight update amount of the first particle; the smaller the matching Euclidean distance, the higher the matching degree, the greater the lane line observation probability, and the larger the corresponding weight update amount of the first particle.

In an optional example, in step 202, based on the first particle poses of each first particle, the lateral positioning prediction is performed on the pose of each first particle at the current moment, and the first prediction corresponding to each first particle is obtained pose, including:

Based on the first particle pose of each first particle and the motion model based on the odometer, respectively, the first predicted pose corresponding to each first particle is obtained; the longitudinal noise parameter in the translation component in the motion model based on the odometer is 0.

The odometry-based motion model is a probabilistic robot motion model implemented using relative motion information measured by the odometry of the mobile device, more specifically, within the time interval (t-1,t), The movable device advances from the pose x _t-1 to the pose x _t , and the odometer feeds back the relative progress during this period. The motion model based on the odometer expresses the motion of the movable device with three series of basic motions, namely Rotation, linear motion (i.e. translation) and another rotation, the following equation expresses the reading from the odometer

(in,

T stands for transpose) computes two rotation values and one translation value:

Among them, δ _rot1 is a rotation value, δ _trans is a translation value, δ _rot2 is another rotation value, and atan2 is a function that returns an azimuth.

To model motion error, assume that the "true" values of rotation and translation are independent noise with mean zero and variance ^b2 subtracted from the measured values

get:

in,

is a noise variable with a mean of 0 and a variance of b ² , and parameters α ₁ -α ₄ are error parameters for a robot (referred to as a mobile device in this disclosure), which specify the cumulative error of motion.

In the embodiment of the present disclosure, when using the above-mentioned odometer-based motion model to predict the lateral positioning of each first particle at the current moment, the longitudinal noise parameter _α3 is set to 0, and only lateral positioning is performed during prediction. Forecast without introducing vertical noise, decoupling horizontal and vertical observations.

In an optional example, FIG. 14 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure. In this example, the method includes:

1. Obtain the first particle poses of the N first particles corresponding to the mobile device.

Among them, the pose of the first particle corresponding to the jth first particle is expressed as

That is, the posterior pose after longitudinal correction at the previous moment.

2. Based on the first particle pose of each first particle and the motion model based on the odometer, respectively, the first predicted pose corresponding to each first particle is obtained; the longitudinal noise in the translation component in the motion model based on the odometer The parameter is 0.

Among them, the first predicted pose corresponding to the jth first particle is expressed as

3. Based on the pose information of the mobile device at a previous moment and the odometer information at the current moment, determine a second predicted pose of the mobile device at the current moment.

Among them, in the prediction process, it is consistent with the above particle prediction based on the motion model of the odometer, where the longitudinal noise parameter in the translation component is 0, and the pose information of the mobile device at the previous moment is expressed as

The second predicted pose of the mobile device at the current moment is expressed as

4. Establish the first grid coordinate area based on the second predicted pose; the first coordinate axis of the first grid coordinate area is the lateral direction of the pose, and the second coordinate axis of the first grid coordinate area is the heading of the pose angular direction.

5. Based on the first predicted pose corresponding to each first particle, map each first particle to the first grid coordinate area, and obtain the cell to which each first particle belongs.

The coordinates of the jth first particle in the first grid coordinate area are expressed as

Specifically, according to the coordinates of each first particle in the first grid coordinate area

and the occupied range of each cell in the first grid coordinate area to map.

6. Delete the first particles mapped to outside the coordinate area of the first grid to obtain the remaining first particles.

7. A fourth particle is added to each cell center in the first grid coordinate area, and each fourth particle corresponds to a fourth weight.

8. Based on the cell to which each remaining first particle belongs, the second weight corresponding to each remaining first particle, and the fourth weight corresponding to each fourth particle, merge the particles in each cell to obtain each unit The third particles corresponding to the grids and the poses of the third particles respectively corresponding to the third particles.

The poses of the third particles corresponding to the jth third particles are expressed as

That is, through gridded resampling, the particle swarm of the first particle is transformed into the particle swarm of the third particle:

9. Based on the third particle poses corresponding to the third particles, the original weights corresponding to the third particles are updated to obtain the updated weights corresponding to the third particles.

in,

That is, it represents the updated weight of the jth third particle at time t,

Indicates the original weight of the jth third particle at time t;

Indicates the lane line observation probability corresponding to the jth third particle, j=1,2,...,N.

The weight update principle of this step is similar to the specific operation of the aforementioned step 2031 (specifically, steps 20311-20312), that is, the third particle pose corresponding to the third particle is used as the origin of the self-coordinate system of the mobile device to observe the lane line, based on The lane line observation results are used to update the weights, and the details will not be repeated. The original weights corresponding to the third particles are the sum of the weights of all particles in the cell.

10. Determine the first estimated pose of the mobile device based on the poses of the third particles corresponding to the third particles and the updated weights:

in,

That is, it represents the first estimated pose of the mobile device at the current moment (time t). For other symbols, refer to the preceding steps.

This step is consistent with the principle of the aforementioned step 2032, and will not be repeated here.

11. The first estimated pose of the mobile device is used as pose information of the mobile device at the current moment.

12. Take the first estimated pose as the center particle, sample H fifth particles forward and backward according to the first interval in the longitudinal direction, and obtain 2H fifth particles, and each fifth particle and the first estimated pose The second interval of , H is a positive integer.

13. Based on the first estimated pose and the second distance between each fifth particle and the first estimated pose, determine the pose of each fifth particle.

The first estimated pose is expressed as

The first interval is denoted as d, then the corresponding pose increment is D=(d,0,0), then the pose of each fifth particle obtained by sampling is:

and i∈[-H,H]

in,

That is, it represents the pose of the i-th fifth particle,

represents an integer space,

14. Using the central particle and each fifth particle as the second particle, obtain the second particle poses of the M second particles, where M=2H+1.

15. Obtain the prior weights corresponding to the second particles based on the Gaussian distribution.

16. Acquiring the arrow landmark observation results and pose measurement results corresponding to the second particles.

17. Based on the arrow landmark observation results and pose measurement results corresponding to each second particle, update the prior weights corresponding to each second particle, and obtain the first weights corresponding to each second particle.

in,

That is, the first weight corresponding to the i-th second particle,

That is, it represents the prior weight of the i-th second particle sampled,

Indicates the pose observation probability corresponding to the i-th second particle,

Indicates the arrow observation probability corresponding to the i-th second particle, i=-H,-(H-1),...,0,1,...,H, H is a positive integer, M=2H+1.

18. Based on the first weights respectively corresponding to the second particles, determine a first longitudinal pose correction amount.

Exemplarily, the vertical correction amount d _correct is expressed as:

in,

That is, the first weight corresponding to the i-th second particle,

represents the sum of the first weights of each second particle, d _i represents the second distance between the i-th second particle and the first estimated pose.

The first longitudinal pose correction D _correct obtained based on the above-mentioned longitudinal correction d _correct is expressed as:

D _correct ＝(d _correct ,0,0)

19. Perform longitudinal correction on the third particle pose of each third particle based on the first longitudinal pose correction amount, and obtain the posterior pose and pose corresponding to each third particle at the current moment.

The a posteriori pose at the current moment corresponding to each third particle is used as the a posteriori pose at the current moment corresponding to each first particle, and is applied to the next moment.

20. Go to the next moment, take the next moment as the current moment, and return to step 1 above.

The specific operations of the above steps 1-20 have been described in detail in the foregoing content, and will not be repeated here.

Exemplarily, FIG. 15 is a schematic diagram of the principle of horizontal and vertical correction of particle swarms provided by an exemplary embodiment of the present disclosure. Among them, the horizontal particle swarm is the particle swarm obtained by horizontal correction based on the particle swarm after the vertical correction at the previous moment. Here, the horizontal correction can include the above steps 2-9. The obtained horizontal particle swarm is the third particle formation after the weight update Particle swarms, the vertical observation to determine the vertical correction amount includes the above steps 12-18, and the vertical correction amount acts on the horizontal particle swarm includes the above step 19, which will not be described in detail.

Any method for determining pose information provided in the embodiments of the present disclosure may be executed by any appropriate device with data processing capabilities, including but not limited to: terminal devices, servers, and the like. Alternatively, the method for determining any pose information provided by the embodiments of the present disclosure may be executed by a processor, for example, the processor executes the determination of any pose information mentioned in the embodiments of the present disclosure by calling the corresponding instructions stored in the memory method. I won't go into details below.

Exemplary device

Fig. 16 is a schematic structural diagram of an apparatus for determining pose information provided by an exemplary embodiment of the present disclosure. The device of this embodiment can be used to implement the corresponding method embodiment of the present disclosure. The device shown in FIG.

The first acquisition module 501 is configured to acquire the first particle poses of the N first particles corresponding to the mobile device, where N is a positive integer greater than 1; the first particle poses are the corresponding first particles obtained at the previous moment The posterior pose after vertical correction; the first processing module 502 is used to perform horizontal processing on the pose of each first particle at the current moment based on the first particle pose of each first particle acquired by the first acquisition module 501, respectively. Positioning prediction, obtaining the first predicted poses corresponding to the first particles; the second processing module 503, configured to determine the movable device based on the first predicted poses corresponding to the first particles obtained by the first processing module 502 The first estimated pose of the mobile device; the third processing module 504 is configured to use the first estimated pose of the mobile device obtained by the second processing module 503 as the pose information of the mobile device at the current moment.

Fig. 17 is a schematic structural diagram of an apparatus for determining pose information provided by another exemplary embodiment of the present disclosure.

In an optional example, the apparatus of the present disclosure further includes: a first vertical processing module 505 and a second vertical processing module 506 . The first longitudinal processing module 505 is configured to determine the first longitudinal pose correction amount based on the first estimated pose of the mobile device obtained by the second processing module 503; the second longitudinal processing module 506 is configured to determine the first longitudinal pose correction amount based on the first longitudinal processing The first longitudinal pose correction amount obtained by module 505 performs longitudinal correction on the first predicted pose of each first particle, and obtains the posterior pose and pose corresponding to each first particle at the current moment.

Fig. 18 is a schematic structural diagram of a first vertical processing module 505 provided by an exemplary embodiment of the present disclosure.

In an optional example, the first vertical processing module 505 includes: a first sampling unit 5051 , a first acquiring unit 5052 , a second acquiring unit 5053 , a first weight updating unit 5054 and a first determining unit 5055 . The first sampling unit 5051 is configured to take the first estimated pose obtained by the second processing module 503 as the center and perform sampling in the longitudinal direction to obtain the second particle poses of M second particles, where M is a positive integer greater than 1 The first obtaining unit 5052 is used to obtain the prior weights corresponding to the second particles based on the Gaussian distribution; the second obtaining unit 5053 is used to obtain the arrow landmark observation results and the pose measurement results corresponding to the second particles respectively; The arrow landmark observation result corresponding to a second particle includes the observation probability of the arrow landmark under the second particle pose corresponding to the second particle; the pose measurement result corresponding to a second particle includes the second particle corresponding to the second particle The observation probability of the pose of the mobile device under the pose of the particle; the first weight update unit 5054, based on the arrow signpost observation results and the pose measurement results corresponding to each second particle, performs a priori weight corresponding to each second particle Update to obtain the first weights corresponding to the second particles; the first determination unit 5055 determines the first longitudinal pose correction amount based on the first weights corresponding to the second particles.

Fig. 19 is a schematic structural diagram of a second processing module 503 provided by an exemplary embodiment of the present disclosure.

In an optional example, the second processing module 503 includes: a second weight updating unit 5031 and a second determining unit 5032 . The second weight update unit 5031 is configured to update the second weights corresponding to the first particles based on the first predicted poses corresponding to the first particles obtained by the first processing module 502, and obtain the first particles respectively The corresponding third weight; the second weight corresponding to a first particle is the prior weight of the first particle; the second determination unit 5032 is configured to obtain the first particle corresponding to each first particle based on the first processing module 502 The predicted pose and second weight updating unit 5031 obtains the third weight corresponding to each first particle to determine the first estimated pose of the mobile device.

Fig. 20 is a schematic structural diagram of an apparatus for determining pose information provided by yet another exemplary embodiment of the present disclosure.

In an optional example, the apparatus of the present disclosure further includes: a first determining module 507 , a first establishing module 508 , a first mapping module 509 and a second determining module 510 . The first determination module 507 is used to determine the second predicted pose of the mobile device at the current moment based on the pose information of the movable device at the previous moment and the odometer information at the current moment; the first establishment module 508 is used to determine the second predicted pose of the movable device based on The second predicted pose obtained by the first determination module 507 establishes a first grid coordinate area; the first coordinate axis of the first grid coordinate area is the lateral direction of the pose, and the second coordinate axis of the first grid coordinate area is The heading angle direction of the pose; the first grid coordinate area includes N cells, N=N _y *N _θ , N _y and N _θ represent the cells of the first grid coordinate area in the lateral direction and the heading angle direction respectively Quantity; the first mapping module 509 is configured to map each first particle to the first grid coordinates established by the first establishment module 508 based on the first predicted pose corresponding to each first particle obtained by the first processing module 502 In the region, obtain the cell to which each first particle belongs; the second determination module 510 is configured to determine the third particle corresponding to each cell and the third particle corresponding to each third particle based on the cell to which each first particle belongs. The third particle pose; correspondingly, the second processing module 503 is specifically configured to determine the first estimated pose of the mobile device based on the third particle poses respectively corresponding to the third particles obtained by the second determining module 510 .

In an optional example, the apparatus of the present disclosure further includes: a first screening module 511 and a first supplementary module 512 . The first screening module 511 is used to delete the first particles mapped outside the first grid coordinate area to obtain the remaining first particles; the first supplementary module 512 is used for each cell in the first grid coordinate area A fourth particle is added to the center respectively, and each fourth particle corresponds to a fourth weight.

Correspondingly, the second determining module 510 is specifically configured to: based on the cell to which each remaining first particle belongs, the second weight corresponding to each remaining first particle, and the fourth weight corresponding to each fourth particle, divide each The particles in the cells are merged to obtain the third particles corresponding to each cell and the poses of the third particles respectively corresponding to the third particles.

In an optional example, the device of the present disclosure may further include: a resampling module 513, configured to perform multiple samplings for each first particle, for example, h times of sampling, and finally h*N new th particles can be obtained New first predicted poses corresponding to a particle and each new first particle respectively. Correspondingly, the first mapping module 509 is specifically configured to: map each new first particle to the first grid coordinate area based on the new first predicted pose corresponding to each new first particle, and obtain each new first particle The cell it belongs to. The second determination module 510 is specifically configured to determine the third particle corresponding to each cell and the third particle pose corresponding to each third particle based on the cell to which each new first particle belongs.

In an optional example, the first sampling unit 5051 is specifically configured to: take the first estimated pose as the center particle, sample H fifth particles forward and backward according to the first interval in the longitudinal direction, and obtain 2H The fifth particle, and the second interval between each fifth particle and the first estimated pose, H is a positive integer; based on the first estimated pose, and the second interval between each fifth particle and the first estimated pose, each The pose of the fifth particle: the central particle and each fifth particle are used as the second particles to obtain the second particle poses of the M second particles, M=2H+1.

In an optional example, the first determining unit 5055 is specifically configured to: determine the sum of the first weights of each second particle based on the first weights corresponding to each second particle; The sum, the first weight corresponding to each second particle, and the second interval between each second particle and the first estimated pose determine the vertical correction amount; determine the corresponding pose increment based on the vertical correction amount as the first Amount of vertical pose correction.

In an optional example, the second weight updating unit 5031 is specifically configured to: acquire the lane line observation results corresponding to each first particle; the lane line observation result corresponding to a first particle is included in the first particle corresponding to the first particle Predicting the observation probability of the lane line in the pose; based on the lane line observation results corresponding to the first particles, updating the second weights corresponding to the first particles respectively to obtain the third weights corresponding to the first particles.

In an optional example, the first processing module 502 is specifically configured to: obtain the first predicted position corresponding to each first particle respectively based on the first particle pose of each first particle and the motion model based on the odometer attitude; the longitudinal noise parameter in the translation component in the odometry-based motion model is 0.

Exemplary electronic device

An embodiment of the present disclosure also provides an electronic device, including: a memory configured to store a computer program;

The processor is configured to execute the computer program stored in the memory, and when the computer program is executed, implement the method for determining pose information described in any one of the above-mentioned embodiments of the present disclosure.

Fig. 21 is a schematic structural diagram of an application embodiment of the electronic device of the present disclosure. In this embodiment, the electronic device 10 includes one or more processors 11 and memory 12 .

Processor 11 may be a central processing unit (CPU) or other form of processing unit having data processing capabilities and/or instruction execution capabilities, and may control other components in electronic device 10 to perform desired functions.

Memory 12 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and/or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and/or cache memory (cache). The non-volatile memory may include, for example, a read-only memory (ROM), a hard disk, a flash memory, and the like. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 11 may execute the program instructions to implement the methods for determining pose information and the methods for determining pose information in various embodiments of the present disclosure described above. / or other desired functionality. Various contents such as input signal, signal component, noise component, etc. may also be stored in the computer-readable storage medium.

In one example, the electronic device 10 may further include: an input device 13 and an output device 14, and these components are interconnected through a bus system and/or other forms of connection mechanisms (not shown).

For example, the input device 13 may be the above-mentioned microphone or microphone array, which is used to capture the input signal of the sound source.

In addition, the input device 13 may also include, for example, a keyboard, a mouse, and the like.

The output device 14 can output various information to the outside, including determined distance information, direction information, and the like. The output device 14 may include, for example, a display, a speaker, a printer, a communication network and its connected remote output devices, and the like.

Of course, for simplicity, only some of the components related to the present disclosure in the electronic device 10 are shown in FIG. 21 , and components such as bus, input/output interface, etc. are omitted. In addition, according to specific application conditions, the electronic device 10 may also include any other suitable components.

Exemplary computer program product and computer readable storage medium

In addition to the above-mentioned methods and devices, embodiments of the present disclosure may also be computer program products, which include computer program instructions that, when executed by a processor, cause the processor to perform the above-mentioned "exemplary method" of this specification. Steps in methods according to various embodiments of the present disclosure described in section.

The computer program product can be written in any combination of one or more programming languages to execute the program codes for performing the operations of the embodiments of the present disclosure, and the programming languages include object-oriented programming languages, such as Java, C++, etc. , also includes conventional procedural programming languages, such as the "C" language or similar programming languages. The program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device, or entirely on the remote computing device or server to execute.

In addition, the embodiments of the present disclosure may also be a computer-readable storage medium, on which computer program instructions are stored, and the computer program instructions, when executed by a processor, cause the processor to execute the above-mentioned "Exemplary Method" section of this specification. Steps in methods according to various embodiments of the present disclosure described in .

The computer readable storage medium may employ any combination of one or more readable media. The readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may include, but is not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or device, or any combination thereof, for example. More specific examples (non-exhaustive list) of readable storage media include: electrical connection with one or more conductors, portable disk, hard disk, random access memory (RAM), read only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

The basic principles of the present disclosure have been described above in conjunction with specific embodiments, but it should be pointed out that the advantages, advantages, effects, etc. mentioned in the present disclosure are only examples rather than limitations, and these advantages, advantages, effects, etc. Various embodiments of the present disclosure must have. In addition, the specific details disclosed above are only for the purpose of illustration and understanding, rather than limitation, and the above details do not limit the present disclosure to be implemented by using the above specific details.

Each embodiment in this specification is described in a progressive manner, each embodiment focuses on the difference from other embodiments, and the same or similar parts of each embodiment can be referred to each other. As for the system embodiment, since it basically corresponds to the method embodiment, the description is relatively simple, and for the related parts, please refer to the part of the description of the method embodiment.

The block diagrams of devices, devices, devices, and systems involved in the present disclosure are only illustrative examples and are not intended to require or imply that they must be connected, arranged, and configured in the manner shown in the block diagrams. As will be appreciated by those skilled in the art, these devices, devices, devices, systems may be connected, arranged, configured in any manner. Words such as "including", "comprising", "having" and the like are open-ended words meaning "including but not limited to" and may be used interchangeably therewith. As used herein, the words "or" and "and" refer to the word "and/or" and are used interchangeably therewith, unless the context clearly dictates otherwise. As used herein, the word "such as" refers to the phrase "such as but not limited to" and can be used interchangeably therewith.

The methods and apparatus of the present disclosure may be implemented in many ways. For example, the methods and apparatuses of the present disclosure may be implemented by software, hardware, firmware or any combination of software, hardware, and firmware. The above sequence of steps for the method is for illustration only, and the steps of the method of the present disclosure are not limited to the sequence specifically described above unless specifically stated otherwise. Furthermore, in some embodiments, the present disclosure can also be implemented as programs recorded in recording media, the programs including machine-readable instructions for realizing the method according to the present disclosure. Thus, the present disclosure also covers a recording medium storing a program for executing the method according to the present disclosure.

It should also be pointed out that, in the devices, equipment and methods of the present disclosure, each component or each step can be decomposed and/or reassembled. These decompositions and/or recombinations should be considered equivalents of the present disclosure.

The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use the present disclosure. Various modifications to these aspects will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other aspects without departing from the scope of the present disclosure. Thus, the present disclosure is not intended to be limited to the aspects shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

The foregoing description has been presented for purposes of illustration and description. Furthermore, this description is not intended to limit the disclosed embodiments to the forms disclosed herein. Although a number of example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, changes, additions and sub-combinations thereof.

Claims (13)

1. A method for determining pose information, comprising:

   Obtain the first particle poses of the N first particles corresponding to the mobile device, where N is a positive integer greater than 1; the first particle poses are the longitudinally corrected posteriors of the corresponding first particles obtained at the previous moment pose;

   Based on the first particle pose of each of the first particles, perform lateral positioning prediction on the pose of each of the first particles at the current moment, and obtain a first predicted pose corresponding to each of the first particles;

   determining a first estimated pose of the mobile device based on a first predicted pose corresponding to each of the first particles;

   The first estimated pose of the mobile device is used as the pose information of the mobile device at the current moment.
2. The method according to claim 1, wherein, after determining the first estimated pose of the mobile device based on the first predicted pose corresponding to each of the first particles, further comprising:

   determining a first longitudinal pose correction amount based on a first estimated pose of the mobile device;

   The first predicted pose of each of the first particles is longitudinally corrected based on the first longitudinal pose correction amount, so as to obtain the posterior pose and pose corresponding to each of the first particles at the current moment.
3. The method according to claim 2, wherein said determining a first longitudinal pose correction amount based on the first estimated pose of the mobile device comprises:

   Taking the first estimated pose as the center, sampling in the vertical direction to obtain the second particle poses of M second particles, where M is a positive integer greater than 1;

   Acquiring prior weights corresponding to each of the second particles based on a Gaussian distribution;

   Obtain the arrow landmark observation results and pose measurement results corresponding to each of the second particles; one arrow landmark observation result corresponding to the second particle includes the observation of the arrow landmark under the second particle pose corresponding to the second particle Probability; a pose measurement result corresponding to the second particle includes an observation probability of the pose of the mobile device under the pose of the second particle corresponding to the second particle;

   Based on the arrow landmark observation results and pose measurement results corresponding to the second particles, updating the prior weights corresponding to the second particles respectively, to obtain the first weights corresponding to the second particles;

   The first longitudinal pose correction amount is determined based on the first weights respectively corresponding to the second particles.
4. The method according to claim 1, wherein said determining the first estimated pose of the mobile device based on the first predicted poses respectively corresponding to the first particles comprises:

   Based on the first predicted poses corresponding to each of the first particles, the second weights corresponding to each of the first particles are updated to obtain the third weights corresponding to each of the first particles; one first particle The corresponding second weight is the prior weight of the first particle;

   A first estimated pose of the mobile device is determined based on the first predicted pose and the third weight respectively corresponding to each of the first particles.
5. The method according to claim 4, wherein, based on the first particle poses of each of the first particles, the pose of each of the first particles at the current moment is predicted for lateral positioning, and each of the first particles is obtained. After the first predicted pose corresponding to a particle, it also includes:

   Based on the pose information of the movable device at a previous moment and the odometer information at the current moment, determine a second predicted pose of the movable device at the current moment;

   Establish a first grid coordinate area based on the second predicted pose; the first coordinate axis of the first grid coordinate area is the lateral direction of the pose, and the second coordinate axis of the first grid coordinate area is The heading angle direction of the pose; the first grid coordinate area includes N cells, N=N _y *N _θ , N _y and N _θ respectively represent the lateral direction and heading angle of the first grid coordinate area the number of cells in the direction;

   Map each of the first particles to the first grid coordinate area based on the first predicted pose corresponding to each of the first particles, and obtain the cell to which each of the first particles belongs;

   Based on the cell to which each of the first particles belongs, determine a third particle corresponding to each of the cells, and a third particle pose corresponding to each of the third particles;

   The determining the first estimated pose of the mobile device based on the first predicted pose corresponding to each of the first particles includes:

   Based on the third particle poses corresponding to the third particles, a first estimated pose of the movable device is determined.
6. The method according to claim 5, wherein, in the first predicted poses corresponding to each of the first particles, each of the first particles is mapped to the first grid coordinate area to obtain After each cell to which the first particle belongs, it also includes:

   Deleting the first particles mapped to outside the first grid coordinate area to obtain the remaining first particles;

   A fourth particle is respectively added to the center of each cell in the first grid coordinate area, and each fourth particle corresponds to a fourth weight;

   The determining the third particle corresponding to each of the cells and the pose of the third particle corresponding to each of the third particles based on the cells to which each of the first particles belong includes:

   Based on the cell to which each of the remaining first particles belongs, the second weight corresponding to each of the remaining first particles, and the fourth weight corresponding to each of the fourth particles, the particles in each of the cells Merging is performed to obtain the third particle corresponding to each of the cells and the pose of the third particle respectively corresponding to each of the third particles.
7. The method according to claim 3, wherein, taking the first estimated pose as the center, sampling in the longitudinal direction to obtain the second particle poses of the M second particles comprises:

   Taking the first estimated pose as the center particle, sampling H fifth particles forward and backward according to the first interval in the longitudinal direction, to obtain 2H fifth particles, and the relationship between each fifth particle and the first A second interval of the estimated pose, H is a positive integer;

   determining a pose of each fifth particle based on the first estimated pose and a second distance between each of the fifth particles and the first estimated pose;

   Using the central particle and each of the fifth particles as second particles, the second particle poses of the M second particles are obtained, where M=2H+1.
8. The method according to claim 7, wherein the determining the first longitudinal pose correction amount based on the first weights respectively corresponding to the second particles comprises:

   determining the sum of the first weights of the second particles based on the first weights corresponding to the second particles;

   Determine the vertical correction based on the sum of the first weights of each second particle, the first weight corresponding to each second particle, and the second distance between each second particle and the first estimated pose quantity;

   A corresponding pose increment is determined based on the longitudinal correction amount as the first longitudinal pose correction amount.
9. The method according to claim 4, wherein, based on the first predicted poses corresponding to each of the first particles, the second weights corresponding to each of the first particles are updated to obtain each of the first particles. The third weight corresponding to each particle includes:

   Acquiring lane line observation results corresponding to each of the first particles; one lane line observation result corresponding to the first particle includes the observation probability of the lane line in the first predicted pose corresponding to the first particle;

   Based on the lane line observation results corresponding to the first particles, the second weights corresponding to the first particles are updated to obtain the third weights corresponding to the first particles.
10. The method according to any one of claims 1-9, wherein the lateral positioning prediction is performed on the pose of each of the first particles at the current moment based on the first particle pose of each of the first particles, Obtaining the first predicted pose corresponding to each of the first particles, including:

    Based on the first particle pose of each of the first particles and the motion model based on the odometer, respectively, the first predicted pose corresponding to each of the first particles is obtained; the translation component in the motion model based on the odometer The longitudinal noise parameter in is 0.
11. A device for determining pose information, comprising:

    The first acquisition module is used to acquire the first particle poses of N first particles corresponding to the mobile device, where N is a positive integer greater than 1; the first particle poses are the corresponding first particles obtained at the previous moment The longitudinally corrected posterior pose of ;

    The first processing module is configured to perform lateral positioning prediction on the pose of each of the first particles at the current moment based on the first particle pose of each of the first particles, and obtain the corresponding the first predicted pose;

    A second processing module, configured to determine a first estimated pose of the mobile device based on a first predicted pose corresponding to each of the first particles;

    The third processing module is configured to use the first estimated pose of the movable device as pose information of the movable device at the current moment.
12. A computer-readable storage medium, the storage medium stores a computer program, and the computer program is used to execute the method for determining pose information according to any one of claims 1-10.
13. An electronic device comprising:

    processor;

    memory for storing said processor-executable instructions;

    The processor is configured to read the executable instructions from the memory, and execute the instructions to implement the method for determining pose information according to any one of claims 1-10.

Images