CN114119673B

CN114119673B - Method and device for determining initial pose, electronic equipment and storage medium

Info

Publication number: CN114119673B
Application number: CN202210082899.0A
Authority: CN
Inventors: 尤越; 胡芬
Original assignee: Beijing Horizon Robotics Technology Research and Development Co Ltd
Current assignee: Beijing Horizon Robotics Technology Research and Development Co Ltd
Priority date: 2022-01-25
Filing date: 2022-01-25
Publication date: 2022-04-22
Anticipated expiration: 2042-01-25
Also published as: WO2023142864A1; CN114119673A

Abstract

The embodiment of the disclosure discloses a method and a device for determining an initial pose, an electronic device and a storage medium, wherein the method comprises the following steps: at the t-th moment, predicting based on the t-1-th particle pose respectively corresponding to each first particle to obtain the t-th particle pose respectively corresponding to each first particle, wherein the initial particle pose of each first particle is obtained by performing transverse sampling in a first preset range based on the first GNSS pose; updating the t-1 weight of each first particle based on the t-th road edge matching result and/or the t-th lane line matching result corresponding to each t-th particle pose to obtain the t-th weight corresponding to each first particle; determining the tth positioning pose of the movable equipment according to each tth weight; and if the first particles are converged, taking the tth positioning pose as the initial pose of the movable equipment. Effectively reduces the number of particles and improves the initialization efficiency.

Description

Method and device for determining initial pose, electronic equipment and storage medium

Technical Field

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

Background

In the visual positioning based on the high-precision map, when the mobile device is just started or just enters the map range, the initial pose of the mobile device in the map needs to be determined, that is, the mobile device needs to be initialized.

Disclosure of Invention

The disclosure is provided for solving the technical problems of more computing resources and time required by the particle filter algorithm. The embodiment of the disclosure provides a method and a device for determining an initial pose, an electronic device and a storage medium.

According to an aspect of the embodiments of the present disclosure, there is provided a method for determining an initial pose, including: at the t moment, acquiring t-1 particle poses respectively corresponding to N first particles at the t-1 moment; t is a positive integer, and the initial particle poses corresponding to the N first particles at the initial moment are obtained by performing transverse sampling in a first preset range on the basis of the first GNSS pose; the first GNSS pose is the GNSS pose of the movable equipment at the initial moment; predicting the pose of each first particle at the t moment based on the t-1 particle pose corresponding to each first particle to obtain the t particle pose corresponding to each first particle; acquiring a tth matching result corresponding to each tth particle pose based on the tth particle pose corresponding to each first particle; the tth matching result comprises at least one of a tth road edge matching result and a tth lane line matching result; updating the t-1 weight corresponding to each first particle based on the t matching result corresponding to each t particle pose to obtain the t weight corresponding to each first particle; determining a tth positioning pose of the movable equipment according to the tth weight corresponding to each first particle; and if the first particles are converged, taking the tth positioning pose as the initial pose of the movable equipment.

According to another aspect of the embodiments of the present disclosure, there is provided an initial pose determination apparatus including: the first acquisition module is used for acquiring t-1 particle poses respectively corresponding to the N first particles at the t-1 moment; t is a positive integer, and the initial particle poses corresponding to the N first particles at the initial moment are obtained by performing transverse sampling in a first preset range on the basis of the first GNSS pose; the first GNSS pose is the GNSS pose of the movable equipment at the initial moment; the first processing module is used for predicting the pose of each first particle at the t-th moment based on the t-1-th particle pose corresponding to each first particle to obtain the t-th particle pose corresponding to each first particle; the second processing module is used for acquiring a tth matching result corresponding to each tth particle pose based on the tth particle pose corresponding to each first particle; the tth matching result comprises at least one of a tth road edge matching result and a tth lane line matching result; a third processing module, configured to update a t-1 th weight corresponding to each first particle based on a t-th matching result corresponding to each t-th particle pose, so as to obtain a t-th weight corresponding to each first particle; the fourth processing module is used for determining the tth positioning pose of the movable equipment according to the tth weight corresponding to each first particle; and the fifth processing module is used for taking the tth positioning pose as the initial pose of the movable equipment if each first particle is converged.

According to still another aspect of the embodiments of the present disclosure, there is provided a computer-readable storage medium storing a computer program for executing the method for determining an initial pose according to any one of the above-mentioned embodiments of the present disclosure.

According to still another aspect of the embodiments of the present disclosure, there is provided an electronic apparatus including: a processor; a memory for storing the 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 the initial pose according to any of the above embodiments of the present disclosure.

Based on the method and the device for determining the initial pose, the electronic device and the storage medium provided by the embodiment of the disclosure, a small number of particles are obtained by performing transverse sampling in a certain range based on the GNSS pose at the initial moment, and then the weights of the particles are iteratively updated by combining the observation of the road edge and the lane line, so that the initial pose of the movable device is obtained after the particles are converged.

The technical solution of the present disclosure is further described in detail by the accompanying drawings and examples.

Drawings

The above and other objects, features and advantages of the present disclosure will become more apparent by describing in more detail embodiments of the present disclosure with reference to the attached drawings. The accompanying drawings are included to provide a further understanding of the embodiments of the disclosure and are incorporated in and constitute a part of this specification, illustrate embodiments of the disclosure and together with the description serve to explain the principles of the disclosure and not to limit the disclosure. In the drawings, like reference numbers generally represent like parts or steps.

Fig. 1 is an exemplary application scenario of the determination method of the initial pose provided by the present disclosure;

fig. 2 is a flowchart illustrating a method for determining an initial pose according to an exemplary embodiment of the present disclosure;

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

FIG. 4 is a flowchart of step 203 provided by another exemplary embodiment of the present disclosure;

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

FIG. 6 is a schematic flow chart diagram illustrating step 2041 provided by an exemplary embodiment of the present disclosure;

fig. 7 is a flowchart illustrating a method for determining an initial pose according to another exemplary embodiment of the present disclosure;

fig. 8 is a schematic processing flow diagram at t =1 provided by an exemplary embodiment of the present disclosure;

fig. 9 is a flowchart illustrating a method for determining an initial pose according to still another exemplary embodiment of the present disclosure;

fig. 10 is an overall flowchart of a method for determining an initial pose according to an exemplary embodiment of the present disclosure;

FIG. 11 is a schematic diagram of a particle convergence process provided by an exemplary embodiment of the present disclosure;

FIG. 12 is a schematic diagram of a curb and lane line matching principle provided by an exemplary embodiment of the present disclosure;

fig. 13 is a schematic structural diagram of an initial pose determination apparatus provided by an exemplary embodiment of the present disclosure;

fig. 14 is a schematic structural diagram of a second processing module 503 according to an exemplary embodiment of the disclosure;

fig. 15 is a schematic structural diagram of a third processing module 504 provided in an exemplary embodiment of the present disclosure;

fig. 16 is a schematic structural diagram of an initial pose determination apparatus provided by another exemplary embodiment of the present disclosure;

fig. 17 is a schematic structural diagram of an application embodiment of the electronic device of the present disclosure.

Detailed Description

Hereinafter, example embodiments according to the present disclosure will be described in detail with reference to the accompanying drawings. It is to be understood that the described embodiments are merely a subset of the embodiments of the present disclosure and not all embodiments of the present disclosure, with the understanding that the present disclosure is not limited to the example embodiments described herein.

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

It will be understood by those of skill in the art that the terms "first," "second," and the like in the embodiments of the present disclosure are used merely to distinguish one element from another, and are not intended to imply any particular technical meaning, nor is the necessary logical order between them.

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

It is also to be understood that any reference to any component, data, or structure in the embodiments of the disclosure, may be generally understood as one or more, unless explicitly defined otherwise or stated otherwise.

In addition, the term "and/or" in the present disclosure is only one kind of association relationship describing an associated object, and means that three kinds of relationships may exist, for example, a and/or B may mean: a exists alone, A and B exist simultaneously, and B exists alone. In addition, the character "/" in the present disclosure generally indicates that the former and latter associated objects are in an "or" relationship.

It should also be understood that the description of the various embodiments of the present disclosure emphasizes the differences between the various embodiments, and the same or similar parts may be referred to each other, so that the descriptions thereof are omitted for brevity.

Meanwhile, it should be understood that the sizes of the respective portions shown in the drawings are not drawn in an actual proportional relationship for the convenience of description.

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

Techniques, methods, and apparatus known to those of ordinary skill in the relevant art may not be discussed in detail but are intended to be part of the specification where appropriate.

It should be noted that: like reference numbers and letters refer to like items in the following figures, and thus, once an item is defined in one figure, further discussion thereof is not required in subsequent figures.

The disclosed embodiments may be applied to electronic devices such as terminal devices, computer systems, servers, etc., which are operational 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 that may be suitable for use with electronic devices, such as terminal devices, computer systems, servers, and the like, include, but are not limited to: personal computer systems, server computer systems, thin clients, thick clients, hand-held or laptop devices, microprocessor-based systems, set-top boxes, programmable consumer electronics, networked personal computers, minicomputer systems, mainframe computer systems, distributed cloud computing environments that include any of the above, and the like.

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 a 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 may 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 computer system storage media including memory storage devices.

Summary of the disclosure

In the process of implementing the present disclosure, the inventor finds that, when a mobile device is just started or just enters a map range, an initial pose of the mobile device in the map needs to be determined, that is, the mobile device needs to be initialized, in the prior art, initialization is usually performed based on a particle filtering algorithm, however, the particle filtering algorithm needs to sample in the vicinity of a GNSS position according to a certain distribution to generate a large number of particles, at least several hundred particles are needed, and each particle is iterated, which requires more computing resources and time, resulting in lower computing efficiency.

Brief description of the drawings

Fig. 1 is an exemplary application scenario of the determination method of the initial pose provided by the present disclosure.

For a high-precision positioning scene of a movable device, taking a vehicle as an example, when the vehicle needs to be positioned at high precision, an initial pose of the vehicle in a navigation map needs to be determined first, and then high-precision positioning is performed in subsequent motion of the vehicle based on the initial pose. When the initial pose of the vehicle is determined, by using the initial pose determining method disclosed by the invention, at the initial moment, transverse sampling is carried out in a certain range based on the GNSS pose to obtain N first particles and the initial particle pose of each first particle, wherein the transverse direction refers to the transverse direction of a vehicle body, namely the y direction in the figure.

Exemplary method

Fig. 2 is a flowchart illustrating a method for determining an initial pose according to an exemplary embodiment of the present disclosure. The embodiment can be applied to an electronic device, specifically, for example, a vehicle-mounted computing platform, as shown in fig. 2, and includes the following steps:

step 201, acquiring t-1 particle poses respectively corresponding to N first particles at the t-1 moment; t is a positive integer, and the initial particle poses corresponding to the N first particles at the initial moment are obtained by performing transverse sampling in a first preset range on the basis of the first GNSS pose; the first GNSS pose is the GNSS pose of the movable device at the initial time.

The first GNSS pose is a pose under a map coordinate system, and specifically, the original GNSS pose at the initial time is subjected to coordinate conversion, and is converted from an wgs84 coordinate system to the map coordinate system, so that the first GNSS pose is obtained. Lateral sampling refers to sampling in the lateral direction of the mobile device, taking the vehicle as an example, the width direction of the vehicle, i.e., the direction indicated by the y-axis in fig. 1 above. The first preset range may be set according to actual requirements, for example, the range is 4m (meters) in the lateral direction and the left and right ranges with the first GNSS pose as a center, the first sampling interval is 0.25 meter, N (N =16 × 2+1= 33) first particles are obtained by sampling, and the initial particle poses respectively corresponding to the first particles are obtained. The initial weights corresponding to the first particles may be determined, and the setting of the initial weights may be set according to actual requirements, for example, the initial weight of each first particle may be set to 1/N, or the initial weights corresponding to the first particles are obtained according to a one-dimensional gaussian distribution, which is not limited specifically.

Illustratively, a gaussian distribution is established with the first GNSS pose as a center to determine the initial weight corresponding to each first particle, specifically as follows:

wherein the content of the first and second substances,

represents the first of a sample

The initial weight of the first particle is,

，

is a positive integer and is a non-zero integer,

，

is shown as

The lateral distance of the first particle from the center,

which represents the interval of sampling,

the mean value is represented by the average value,

the variance is indicated.

When t is greater than 1, the t-1 st particle poses respectively corresponding to the N first particles at the t-1 st moment are posterior poses of each first particle obtained at the t-1 st moment, or a combination of posterior poses of a part of first particles screened based on the matching scores of each first particle and poses of a part of particles subjected to resampling and supplementing, and the t-1 st particle poses can be set according to actual requirements.

And 202, predicting the pose of each first particle at the t moment based on the t-1 th particle pose respectively corresponding to each first particle to obtain the t-th particle pose respectively corresponding to each first particle.

The prediction of the particle pose may be implemented based on a Motion Model obtained in advance, and the Motion Model may be set according to actual requirements, for example, an odometer-based Motion Model (odometer Sample Motion Model) or other implementable Motion models are adopted, which is not limited in this disclosure.

Step 203, acquiring a tth matching result corresponding to each tth particle pose based on the tth particle pose corresponding to each first particle; the tth matching result includes at least one of a tth road edge matching result and a tth lane line matching result.

And for the matching result of the t-th road edge corresponding to one first particle, determining the coordinate axis direction of the self-coordinate system according to the course angle of the first particle by taking the position of the first particle as the origin of the self-coordinate system of the movable equipment, observing the road edge, and matching the observed road edge with the road edge element in the map to obtain the matching result. Similarly, the t-th lane line matching result is obtained by matching the observed lane line with the lane line elements in the map.

And 204, updating the t-1 weight corresponding to each first particle based on the t matching result corresponding to each t particle pose to obtain the t weight corresponding to each first particle.

For the tth particle pose of a first particle, the corresponding road edge matching degree and lane line matching degree represent the accuracy of taking the tth particle pose as the pose of the movable device, and when the matching degree is higher, the tth particle pose is closer to the real pose of the movable device, so that the weight change amount of the first particle with higher matching degree is relatively larger, and the weight change amount of the first particle with lower matching degree is relatively smaller. Therefore, the t-1 th weight corresponding to each first particle is updated based on the t-th matching result corresponding to each t-th particle pose, and the weight of each first particle is adjusted to promote convergence of each first particle.

And step 205, determining the tth positioning pose of the movable equipment according to the tth weight corresponding to each first particle.

In particular, a tth positioning pose of the movable device may be determined based on the density estimate.

Optionally, after the first particle weights are updated, weighted averaging is performed on the tth particle poses respectively corresponding to the first particles based on the updated tth weights, so as to obtain the tth positioning pose of the mobile device.

Optionally, the tth weight corresponding to each first particle may be normalized, and the weighted sum of the poses of each tth particle may be performed based on the normalized weight, which may be specifically set according to actual requirements.

And step 206, if the first particles are converged, taking the tth positioning pose as the initial pose of the movable equipment.

The specific convergence condition for determining the convergence of each first particle may be set according to an actual requirement, for example, the convergence of each first particle may mean that the road edge matching and the lane line matching corresponding to each first particle both satisfy a certain condition, and the number of times that the condition is satisfied is greater than a certain number of times.

According to the method for determining the initial pose, a small number of particles are obtained by performing transverse sampling in a certain range at an initial moment based on a GNSS pose, and then iterative updating is performed on the weight of each particle by combining with observation of a road edge and a lane line, so that the initial pose of the movable equipment is obtained after the particles are converged.

In an optional example, fig. 3 is a schematic flowchart of step 203 provided in an exemplary embodiment of the present disclosure, and in this example, step 203 may specifically include the following steps:

step 2031, respectively using each tth particle pose as a target particle pose, determining a self-coordinate system of the movable device based on the target particle pose, and acquiring the observed first path information and first lane line information under the self-coordinate system.

The origin of the mobile device from the coordinate system can be set according to actual requirements, for example, the origin of the vehicle from the coordinate system is set as the center of the rear axle of the vehicle. The observed first path information and the first lane line information are obtained in the following manner: the method comprises the steps of taking the pose of a target particle as the pose of the movable equipment, establishing a self-coordinate system of the movable equipment, namely, taking a position component in the pose of the target particle as an origin point of the self-coordinate system of the movable equipment, determining the directions of an x axis and a y axis of the self-coordinate system according to a course angle (a posture component) in the pose of the target particle, acquiring a visually perceived environment image, converting the environment image into the self-coordinate system, and identifying a road edge and a lane line in the environment image through target detection, so that the observed first road edge information and first lane line information in the self-coordinate system can be acquired. The specific observation principle is not described in detail.

Step 2032, obtaining the original road edge elements and the original lane line elements in the map coordinate system.

The original road edge elements and the original lane line elements in the map are known data with determined positions, and the data are extracted from the map, and the specific extraction mode is not repeated.

Step 2033, mapping the original road edge element and the original lane line element to the self-coordinate system, and obtaining a corresponding first road edge element and a corresponding first lane line element in the self-coordinate system.

Mapping is achieved based on the target particle pose under the self-coordinate system, namely the target particle pose is used as the pose of the movable equipment, and the specific coordinate system transformation principle is not repeated.

Step 2034, matching the observed first edge information with the first edge element to obtain the tth road edge matching result; and matching the observed first lane line information with the first lane line elements to obtain a tth lane line matching result.

The third matching distance is the distance between the position of the road edge included by the first road edge information and the position of the first road edge element, and similarly, the fourth matching distance is the distance between the position of the first road edge information and the position of the first road edge element.

In the actual matching process, taking lane lines as an example (along the same road), the observed first lane line information may include one lane line or multiple lane lines, each lane line usually needs to calculate a matching distance with multiple lane line elements in the map, and whether matching is successful is determined based on the lane line element with the closest matching distance.

Step 2035, taking the matching result of the t-th road edge and the matching result of the t-th lane line as the matching result of the t-th corresponding to the pose of the target particle.

According to the method and the device, the road edge elements and the lane line elements in the map are mapped to the self-coordinate system of the movable equipment and are matched with the observed road edge information and lane line information, so that the t-th matching results corresponding to the t-th particle poses are determined, the condition that the t-th particle poses are close to the movable equipment can be effectively judged, and the convergence condition of the first particles can be further determined.

In an alternative example, fig. 4 is a flowchart of step 203 provided by another exemplary embodiment of the present disclosure, in this example, before acquiring the original road-edge element and the original lane-line element in the map coordinate system in the map in step 2032, step 203 further includes:

step 20321, based on the first GNSS pose and the preset range parameter, a first area map in the map is obtained.

Correspondingly, step 2032 specifically comprises:

step 20322, obtain the original road edge element and the original lane line element in the first area map.

Specifically, in order to effectively reduce the matching calculation amount and improve the data processing efficiency, a partial map segment around the first GNSS pose may be extracted from the map based on the first GNSS pose and the preset range parameter, and the partial map segment is referred to as a first area map. When the first road edge information and the first lane line information are matched, the first road edge information and the first lane line information only need to be matched with the first road edge element and the first lane line element in the first area map respectively.

According to the method and the device, the required partial map segments are extracted for matching, so that the matching calculation amount can be effectively reduced, the data processing efficiency is improved, and the initialization speed can be further improved.

In an optional example, fig. 5 is a schematic flowchart of step 204 provided in an exemplary embodiment of the present disclosure, in this example, the updating, based on the tth matching result corresponding to each tth particle pose and the t-1 weight corresponding to each first particle in step 204, the obtaining the tth weight corresponding to each first particle includes:

2041, determining the tth matching score corresponding to each first particle according to the tth matching result corresponding to each tth particle pose.

Optionally, the tth matching result includes at least one of a tth road edge matching result and a tth lane line matching result, and further, the tth matching result includes a tth road edge matching result and a tth lane line matching result. The tth road edge matching result comprises a third matching distance between the observed first road edge information and the first road edge element in the map, and the tth lane line matching result comprises a fourth matching distance between the first lane line information and the first lane line element. The larger the matching distance is, the worse the matching degree is, the lower the corresponding matching score is, the smaller the matching distance is, the better the matching degree is, and the higher the corresponding matching score is. The mapping relationship between the specific matching score and the matching result can be set according to actual requirements.

Optionally, for the road edge matching, it may also be determined whether the road edge matching is successful based on a third matching distance of the road edge, and when the road edge matching is successful, the road edge matching score is set to a larger fixed value.

Step 2042, updating the t-1 th weight corresponding to each first particle based on the t-th matching score corresponding to each first particle, to obtain the t-th weight corresponding to each first particle.

The t-1 th weight corresponding to each first particle is the weight updated at the t-1 th time. The higher the matching score, the closer the corresponding first particle is to the true position of the movable device and therefore the larger the update amount, and conversely, the lower the matching score, the smaller the weight update amount. The specific update rule can be set according to actual requirements.

In an optional example, fig. 6 is a schematic flowchart of step 2041 provided in an exemplary embodiment of the present disclosure, and in this example, determining, according to the tth matching result corresponding to each tth particle pose in step 2041, a tth matching score corresponding to each first particle may specifically include:

20411, determining the tth road edge matching score corresponding to each tth particle pose based on the tth road edge matching result corresponding to each tth particle pose and the first score rule.

The first score rule is determined based on the principle that the matching degree of the first edge information and the first edge element is lower when the third matching distance is larger, the corresponding matching score is higher when the third matching distance is smaller, namely the first score rule describes the mapping relation between the matching result of the t-th edge and the matching score of the t-th edge, and the specific first score rule can be set according to actual requirements and is combined with the matching result of the t-th edge and the first score rule to determine the matching score of the t-th edge.

Step 20412, determining the tth lane line matching score corresponding to each tth particle pose based on the tth lane line matching result corresponding to each tth particle pose and the second scoring rule.

The tth lane line matching result includes a fourth matching distance between the first lane line information and the first lane line element, and is similar to the road edge principle, and a second scoring rule can be set for the lane line according to actual requirements for representing a mapping relationship between the tth lane line matching result and the tth lane line matching score, so that the tth lane line matching score corresponding to each tth particle pose is determined based on the tth lane line matching result and the second scoring rule corresponding to each tth particle pose, and details are not repeated.

20413, determining the tth matching score corresponding to each first particle based on the tth road edge matching score, the tth lane line matching score, the preset road edge weight and the preset lane line weight corresponding to each tth particle pose; the preset road edge weight is greater than the preset lane line weight.

The specific values of the preset road edge weight and the preset lane line weight can be set according to actual requirements, in order to further guarantee the matching accuracy of the lane lines, the preset road edge weight can be set to be larger than the preset lane line weight, and when the road edge is successfully matched, the road edge matching score can be increased by a larger value relative to the lane line matching score, so that the lane line matching dislocation can be effectively avoided.

Exemplary, first

The tth matching score for each first particle is expressed as:

wherein the content of the first and second substances,

is shown as

The preset road edge weight corresponding to each first particle,

is shown as

The weight of the lane line corresponding to the first particle,

denotes the first

The t-th road edge matching score corresponding to the first particle,

is shown as

Corresponding to the first particleMatching score of the t lane line:

wherein the content of the first and second substances,

indicating a first score determined based on the road-edge matching distance and a certain preset rule, which may be set according to actual requirements,

indicating a lane line matching success threshold,

indicating the lane line matching distance, i.e. the fourth matching distance,

the sampling points of the lane lines are shown,

the road edge score control parameter is expressed as follows:

optionally, the preset rule may be: will be provided with

Set to a preset, large fixed value, so that when the path matching is successful,

=1, corresponding road edge matching scoreThe road edge matching score is a fixed value, the road edge matching score is irrelevant to the road edge matching distance under the condition, the road edge matching distance is only used for judging whether the road edge matching is successful or not, when the road edge matching fails, the initial moment is returned, the transverse sampling is carried out again, and then the initialization process is started again.

In an exemplary manner, the first and second electrodes are,

，

the maximum matching distance is indicated and is,

the constant is preset, and can be specifically set according to actual requirements.

Optionally, the preset rule may also be: similar to the lane lines,

the calculation is carried out through a road edge matching success threshold and a road edge matching distance, and is represented as follows:

wherein the content of the first and second substances,

indicating a threshold for the success of the road edge matching,

indicating the road edge matching distance, i.e. the third matching distance,

and indicating a road edge sampling point, and judging whether the road edge matching is successful or not according to the road edge matching distance and the road edge matching success threshold.

Or in order to make the road-edge score large,

the method can also comprise the following steps:

wherein the content of the first and second substances,

the preset parameter value can be specifically set according to actual requirements.

Accordingly, first

The tth weight of the first particle is expressed as:

wherein the content of the first and second substances,

i.e. to represent

T-1 th weight of the first particle, i.e. the

The updated weight of the first particle at time t-1.

Determining the tth positioning pose of the movable device according to the tth weight corresponding to each first particle can be expressed as

：

Wherein the content of the first and second substances,

is the number of the first particles and,

is a positive integer;

is shown as

The tth particle pose of each first particle;

is shown as

The tth weight of the first particle.

Or the tth weight of each first particle may be normalized first, and the tth particle pose of each first particle may be weighted based on the normalized weight, which is expressed as follows:

wherein the content of the first and second substances,

is shown as

And the t-th weight of each first particle corresponds to the normalized weight.

Alternatively, in the embodiment of the present disclosure, the matching distance (including the road edge matching distance (i.e., the third matching distance) and the lane line matching distance (i.e., the fourth matching distance)) refers to a distance in the lateral direction of the movable device.

Optionally, since the distance between the observation of the road edge itself and the matching of the map road edge elements may be relatively large, the disclosed road edge matching is used to ensure the lane line-level matching accuracy, and therefore, in order to ensure the lane matching accuracy, the judgment criterion for the success of the road edge matching and the judgment criterion for the success of the lane line matching may be set to be different.

This is disclosed through setting up the curbstone weight to be greater than lane line weight, when the road edge matches successfully, makes the curbstone match score have the great value, can effectively guarantee the degree of accuracy of lane line position, avoids the condition of lane line dislocation matching to take place.

Fig. 7 is a flowchart illustrating a method for determining an initial pose according to another exemplary embodiment of the present disclosure.

In an optional example, after step 202, the method of the present disclosure further comprises:

step 4011, obtaining a tth matching score corresponding to each first particle.

The manner of obtaining the tth matching score is referred to the above, and is not described herein again.

Step 4012, delete the first particle whose t-th matching score is lower than the first score threshold, and obtain the remaining first particles.

The first score threshold may be set according to actual requirements, and this disclosure is not limited thereto, and a lower score indicates that the particle is less likely to be the real position of the mobile device, so that a particle with a too low score is less effective in estimating the positioning pose of the mobile device, and deleting the particle helps the particle to converge.

And 4013, performing lateral sampling in a second preset range based on the second GNSS pose at the t-th time to obtain a second number of second particles.

The second GNSS pose is similar to the first GNSS pose, and is not described herein again. The second preset range may be the same as the first preset range, or may be set to be different according to actual requirements, for example, the second preset range is determined according to the number of particles to be replenished, and the number of particles to be replenished may be sampled according to a second sampling interval, where the second sampling interval may also be the same as or different from the first sampling interval, and is not limited specifically. The specific operation of performing the horizontal sampling again based on the second GNSS pose is similar to that of the first GNSS, and is not described herein again.

Step 4014, the remaining first particles and a second number of second particles are taken as N first particles, the second number being the number of the deleted first particles.

Specifically, the remaining first particles after screening and the second number of second particles after completion are used as N first particles for the subsequent processing flow. Correspondingly, in each of the first particles after the completion, the tth particle pose of the remaining first particles is still the corresponding tth particle pose obtained by the prediction, the corresponding weight is the t-1 weight of the previous moment, the tth particle pose of the first particles after the completion is the particle pose obtained by sampling, and the corresponding weight is the weight obtained by sampling.

In an optional example, after the t-1 positioning pose of the mobile device is determined according to the t-1 weight corresponding to each first particle at the previous time (t-1 time), based on the t-1 matching score corresponding to each first particle, the first particles with the t-1 matching score lower than the first score threshold are deleted, and the remaining first particles are obtained; performing transverse sampling in a certain preset range based on the GNSS position and posture at the t-1 moment to obtain a second number of second particles; the remaining first particles and the second number of second particles are used as N first particles, which are obtained at the t-th time point in the step 201.

In an optional example, after the step 205, the first particles may be filtered and supplemented in the manner of the above-mentioned

steps

4012 and 4014 based on the tth matching score corresponding to each first particle, and the filtered and supplemented first particles are taken as N first particles at the next time.

The positions of the specific particle screening and filling steps in the process can be set according to actual requirements.

According to the method and the device, the particles with the scores lower than the score threshold are deleted, and resampling and filling are performed, so that the convergence of the particles can be effectively promoted, and the initialization efficiency is further improved.

Fig. 8 is a schematic processing flow diagram when t =1 according to an exemplary embodiment of the present disclosure.

In an optional example, when t =1, step 201 specifically includes:

in step 2011, when t =1, initial particle poses corresponding to the N first particles at the initial time are obtained.

Before predicting the pose of each first particle at the t-th time based on the t-1 th particle pose respectively corresponding to each first particle and obtaining the t-th particle pose respectively corresponding to each first particle in step 202, the method of the present disclosure further includes:

step 4021, acquiring initial matching results corresponding to the initial particle poses respectively based on the initial particle poses respectively corresponding to the first particles.

The initial matching result comprises an initial road edge matching result and an initial lane line matching result; an initial road edge matching result corresponding to one initial particle pose comprises a first matching distance between a road edge observed based on the initial particle pose and a road edge element in a map, and an initial lane line matching result corresponding to one initial particle pose comprises a second matching distance between a lane line observed based on the initial particle pose and a lane line element in the map.

The obtaining manner of the initial matching result is similar to that of the tth matching result, and is not described herein again.

Correspondingly, step 202 specifically includes:

step 2021, if the first matching distance corresponding to the at least one initial particle pose is smaller than the first distance threshold and the second matching distance is smaller than the second distance threshold, predicting the pose of each first particle at the 1 st time based on the initial particle pose corresponding to each first particle and the odometer information corresponding to the 1 st time, and obtaining the 1 st particle pose corresponding to each first particle.

And when the first matching distance is smaller than the first distance threshold, the road edge matching is successful, when the second matching distance is smaller than the second distance threshold, the lane line matching is successful, and when the road edge and the lane line are both successfully matched, the subsequent particle convergence process is started. Otherwise, returning to the initial moment to perform sampling judgment again.

Optionally, it may be determined whether the main lane line where the vehicle is located is successfully matched, that is, when the road edge is successfully matched and the main lane line is successfully matched, a subsequent convergence process is performed, where the convergence process is an iteration process in which the weight of the particle is continuously updated iteratively, so that both the road edge and the lane line of the particle are successfully matched, and the number of times of success exceeds a certain number of times.

Optionally, the preset road edge weight and the preset lane line weight may be set to different values in different processing flows, for example, when the matching score is calculated before entering the convergence flow and when the matching score is calculated after entering the convergence flow after the weight is updated, the preset road edge weight is set to different weight values respectively, and the preset lane line weight is also set to different weight values respectively.

Illustratively, in the convergence procedure, settings are made

，

In the early stage, the road edges are aligned firstly, so that the lane line matching is not wrong.

According to the method, whether the road edge and the lane line are successfully matched or not is judged by sampling particles at the 1 st moment, the follow-up convergence process is entered after the matching is successful, otherwise, the initial moment is returned for resampling, and the accuracy of estimating the positioning pose of the movable equipment by the particle swarm is effectively improved.

In an optional example, when it is determined in step 2021 that the first matching distance corresponding to at least one initial particle pose is smaller than the first distance threshold and the second matching distance is smaller than the second distance threshold, before the pose of each first particle at the 1 st time is predicted based on the initial particle pose corresponding to each first particle and the odometer information corresponding to the 1 st time, and the 1 st particle pose corresponding to each first particle is obtained, the particle screening and alignment processes in

step

4011 and 4014 may be performed. When entering the convergence process, the particle screening and supplementing are firstly carried out, and the particles with lower scores are filtered out, so that the subsequent particle convergence is promoted.

In an alternative example, in the particle convergence procedure, the particle screening and filling process based on the matching score in the

above steps

4011 and 4014 may not be performed, but the particles are screened based on the tth weight corresponding to each first particle, and then filled by lateral sampling. Specifically, in step 204, the t-1 th weight corresponding to each first particle may be updated based on the t-th matching result corresponding to each t-th particle pose, the t-th weight corresponding to each first particle may be obtained, the first particles with the t-th weight lower than the first weight threshold may be deleted, the remaining first particles may be obtained, the second GNSS pose at the t-th time may be transversely sampled within the second preset range, the second number of second particles may be obtained, and the remaining first particles and the second number of second particles may be regarded as N first particles, and the second number may be the number of deleted first particles.

For example, when entering the convergence process, the particle screening and completion is performed based on the matching score, and after entering the convergence process, the particle screening and completion is performed based on the weight of the particle in the iteration process, which is equivalent to resampling the particle by combining the previous matching score and the particle weight of the convergence process in the whole process, thereby further improving the initialization efficiency.

In an optional example, in the particle convergence process, before or after predicting the poses of the first particles at the t-th time based on the t-1-th particle poses respectively corresponding to the first particles in step 202 to obtain the t-th particle poses respectively corresponding to the first particles, the particles are screened based on the t-1-th weights respectively corresponding to the first particles, and then the horizontal sampling and the completion are performed in the second preset range based on the second GNSS poses at the t-th time, where the completion process is referred to the foregoing content, and is not described herein again.

Fig. 9 is a flowchart illustrating a method for determining an initial pose according to still another exemplary embodiment of the present disclosure.

In an optional example, after determining the tth positioning pose of the movable device according to the tth weight corresponding to each first particle in step 205, the method of the present disclosure further includes:

4031, a road edge matching result and a lane line matching result corresponding to the tth positioning pose are obtained.

The road edge matching result and the lane line matching result corresponding to the tth positioning pose are obtained by determining a self-coordinate system of the movable device based on the tth positioning pose, observing the road edge and the lane line, and respectively matching the observed road edge and the observed lane line with a road edge element and a lane line element in a map.

Correspondingly, if each first particle in step 206 converges, taking the tth positioning pose as the initial pose of the mobile device specifically includes:

step 2061, if the road edge matching result and the lane line matching result corresponding to the tth positioning pose satisfy a first preset condition and each first particle is converged, taking the tth positioning pose as the initial pose of the movable equipment.

The first preset condition can be set according to actual requirements, for example, the first preset condition means that the road edge matching result corresponding to the t-th positioning pose is that the road edge matching is successful, and the lane line matching is successful. For another example, the first preset condition means that the road edge matching result corresponding to the t-th positioning pose is that the road edge matching is successful, the lane line matching is successful, the number of times of successful matching exceeds a certain number of times, and the details are not limited. For the judgment whether matching of the tth positioning pose is successful or not, the judgment can be also performed based on the matching distance, for example, a third distance threshold and a fourth distance threshold are set, when the road edge matching distance is smaller than the third distance threshold, it indicates that the road edge matching is successful, when the lane line matching distance is smaller than the fourth distance threshold, it indicates that the lane line matching is successful, and the specific third distance threshold and the specific fourth distance threshold can be set according to actual requirements, which is not limited in this embodiment.

According to the method and the device, after the tth positioning pose is determined, the road edge and the lane line are observed and matched based on the tth positioning pose, and the tth positioning pose is used as the initial pose of the movable equipment when the matching result meets a certain condition, so that the accuracy of the initial pose is further improved.

In one optional example, the method of the present disclosure further comprises:

and step 207, if the road edge matching result and the lane line matching result corresponding to the tth positioning pose meet a first preset condition and the first particle is not converged, entering a processing flow at the t +1 th moment.

Specifically, when the road edge matching result and the lane line matching result satisfy the first preset condition, but the first particle is not converged, it is proved that the particle swarm cannot simulate the real pose of the mobile device, so that iteration is further performed continuously, at the t +1 th moment, step 201 and step 206 are repeatedly performed according to the execution flow, where specific operations of the steps are referred to in the foregoing, and are not described herein again.

And 208, if the road edge matching result corresponding to the tth positioning pose meets a first preset condition and the lane line matching result corresponding to the tth positioning pose does not meet the first preset condition, entering a processing flow at the t +1 th moment.

Illustratively, for example, the first preset condition is that the road edge matching result is that the road edge matching is successful, and the lane line matching result is that the lane line matching is successful, when the road edge matching result corresponding to the tth positioning pose is that the road edge matching is successful, the road edge matching result corresponding to the tth positioning pose meets a first preset condition, when the lane line matching result corresponding to the t-th positioning pose is unsuccessful, the lane line matching result corresponding to the t-th positioning pose does not meet a first preset condition, the observation matching of the particle swarm to the lane line does not meet the requirement yet, so the particle swarm cannot accurately simulate the positioning pose of the movable equipment and needs to continue iteration, therefore, at the time t +1, let t = t +1, and repeat the

step

201 and 206, and the specific operations of the steps are referred to the foregoing content and will not be described herein again.

And 209, returning to the initial moment if the road edge matching result corresponding to the tth positioning pose does not meet the first preset condition.

Specifically, when the road edge matching result corresponding to the tth positioning pose does not satisfy the first preset condition, it indicates that the actual position difference of the current particle swarm relative to the movable device is large, in order to improve the initialization efficiency, the sampling is performed again at the initial time, that is, the current tth time is taken as the initial time, the current GNSS pose is obtained as the first GNSS pose, the initial particle poses corresponding to the N first particles are obtained by performing the horizontal sampling in the first preset range based on the first GNSS pose, and the iteration is performed according to the step 201 and the step 206, which is not described in detail again.

In an alternative example, fig. 10 is an overall flowchart of a method for determining an initial pose provided by an exemplary embodiment of the present disclosure. In this example, the method comprises:

1. and at the initial moment, acquiring a first GNSS pose.

2. And carrying out transverse sampling in a first preset range based on the first GNSS position and posture to obtain N first particles and initial particle position and posture corresponding to each first particle.

3. At the 1 st moment, initial matching results corresponding to the initial particle poses are obtained, and the initial matching results comprise an initial road edge matching distance (first matching distance) and an initial lane line matching distance (second matching distance).

4. And judging whether a first matching distance corresponding to at least one initial particle pose is smaller than a first distance threshold value and a second matching distance is smaller than a second distance threshold value. If yes, turning to step 5, otherwise returning to step 1.

5. And at the t moment, acquiring the t-1 particle pose corresponding to each first particle and the odometer information corresponding to the t moment, predicting the pose of each first particle at the t moment, and acquiring the t particle pose corresponding to each first particle.

Where T =1,2, …, T represents the maximum number of iterations. And the t-1 particle pose when t =1 is the initial particle pose.

Optionally, when t =1, after step 5 and before step 6, the method may further include: determining initial matching scores corresponding to the first particles based on initial matching results corresponding to the first particles, deleting the first particles with the initial matching scores lower than a first score threshold value to obtain the remaining first particles, performing horizontal sampling within a second preset range based on a second GNSS (global navigation satellite system) pose at the 1 st moment to obtain a second number of second particles, and taking the remaining first particles and the second number of second particles as N first particles, wherein the second number is the number of the deleted first particles.

6. And respectively taking the tth particle poses as target particle poses, determining a self-coordinate system of the movable equipment based on the target particle poses, observing the road edge and the lane line, respectively matching the observed first road edge information and the observed first lane line information with the first road edge element and the first lane line element in the map, and obtaining tth matching results respectively corresponding to the tth particle poses.

7. And determining the t-th matching score corresponding to each first particle based on the t-th matching result corresponding to each t-th particle pose.

8. And updating the t-1 th weight corresponding to each first particle based on the t-matching score corresponding to each first particle to obtain the t-weight corresponding to each first particle.

9. And deleting the first particles with the weight t lower than the first weight threshold value to obtain the remaining first particles.

10. And performing transverse sampling in a second preset range based on the second GNSS position and posture at the t-th moment to obtain a second number of second particles, and acquiring the t-th weight corresponding to each second particle.

11. And taking the rest first particles and a second number of second particles as N first particles, wherein the second number is the number of the deleted first particles.

12. And determining the tth positioning pose of the movable equipment according to the tth weight corresponding to each first particle.

13. And acquiring a road edge matching result and a lane line matching result corresponding to the t-th positioning pose.

14. And judging whether the road edge matching result corresponding to the t-th positioning pose meets a first preset condition. If not, taking the current time as the initial time, returning to the step 1, and restarting initialization; if yes, go to step 15.

15. And judging whether the lane line matching result corresponding to the t-th positioning pose meets a first preset condition. If not, making t = t +1, and returning to the step 5; if yes, go to step 16.

16. And judging whether each first particle converges. If yes, go to step 17; if not, let t = t +1, return to step 5.

17. And taking the tth positioning pose as the initial pose of the movable equipment.

The specific operations of the steps 1-17 are referred to the above, and will not be described herein again.

Illustratively, fig. 11 is a schematic diagram of a particle convergence process provided by an exemplary embodiment of the present disclosure. Fig. 12 is a schematic diagram of a principle of matching a road edge and a lane line according to an exemplary embodiment of the disclosure. Wherein the transverse short rectangle represents matching, and the self vehicle represents the movable equipment body.

According to the method, a small amount of particles are obtained through transverse sampling based on the GNSS pose, the positioning pose of the movable equipment is determined by combining the observation of the road edge and the lane line and the matching of the map road edge elements and the lane line elements, the road edge matching and the lane line matching based on the positioning pose are successfully matched, and the positioning pose is taken as the initial pose of the movable equipment when the particles are converged, so that the accurate positioning of the initial pose of the movable equipment is effectively realized under the condition of less calculated amount.

Any of the methods for determining the initial pose provided by the embodiments of the present disclosure may be performed by any suitable device with data processing capability, including but not limited to: terminal equipment, a server and the like. Alternatively, any of the determination methods for the initial pose provided by the embodiments of the present disclosure may be executed by a processor, for example, the processor may execute any of the determination methods for the initial pose mentioned in the embodiments of the present disclosure by calling corresponding instructions stored in a memory. And will not be described in detail below.

Exemplary devices

Fig. 13 is a schematic structural diagram of an initial pose determination apparatus according to an exemplary embodiment of the present disclosure. The apparatus of this embodiment may be used to implement the corresponding method embodiment of the present disclosure, and the apparatus shown in fig. 13 includes: a first obtaining module 501, a first processing module 502, a second processing module 503, a third processing module 504, a fourth processing module 505 and a fifth processing module 506.

A first obtaining module 501, configured to obtain, at a t-th time, t-1-th particle poses corresponding to N first particles at the t-1-th time, respectively; t is a positive integer, and the initial particle poses corresponding to the N first particles at the initial moment are obtained by performing transverse sampling in a first preset range on the basis of the first GNSS pose; the first GNSS pose is the GNSS pose of the movable device at the initial time.

The first processing module 502 is configured to predict the pose of each first particle at the t-th time based on the t-1-th particle pose respectively corresponding to each first particle acquired by the first acquiring module 501, so as to obtain the t-th particle pose respectively corresponding to each first particle.

A second processing module 503, configured to obtain, based on the tth particle poses respectively corresponding to the first particles obtained by the first processing module 502, tth matching results respectively corresponding to the tth particle poses; the tth matching result includes at least one of a tth road edge matching result and a tth lane line matching result.

A third processing module 504, configured to update the t-1 th weights respectively corresponding to the first particles based on the t-th matching results respectively corresponding to the t-th particle poses obtained by the second processing module 503, so as to obtain the t-th weights respectively corresponding to the first particles.

And a fourth processing module 505, configured to determine a tth positioning pose of the movable device according to the tth weight obtained by the third processing module 504 and corresponding to each first particle.

A fifth processing module 506, configured to take the tth positioning pose obtained by the fourth processing module 505 as the initial pose of the movable apparatus if each first particle converges.

Fig. 14 is a schematic structural diagram of the second processing module 503 according to an exemplary embodiment of the present disclosure.

In one optional example, the second processing module 503 includes: a first obtaining unit 5031, a second obtaining unit 5032, a first mapping unit 5033, a first matching unit 5034 and a first processing unit 5035. A first obtaining unit 5031, configured to determine, based on the target particle pose and taking each tth particle pose obtained by the first processing module 502 as a target particle pose, a self-coordinate system of the mobile device, and obtain observed first route information and first lane line information in the self-coordinate system; a second obtaining unit 5032, configured to obtain an original road edge element and an original lane line element in a map coordinate system in a map; a first mapping unit 5033, configured to map the original road edge element and the original lane line element obtained by the second obtaining unit 5032 to a self-coordinate system, so as to obtain a corresponding first road edge element and a corresponding first lane line element in the self-coordinate system; a first matching unit 5034, configured to match the observed first edge information obtained by the first obtaining unit 5031 with the first edge element obtained by the first mapping unit 5033 to obtain a tth edge matching result, and match the observed first lane line information obtained by the first obtaining unit 5031 with the first lane line element obtained by the first mapping unit 5033 to obtain a tth lane line matching result; the first processing unit 5035 is configured to take the tth road edge matching result and the tth lane line matching result obtained by the first matching unit 5034 as a tth matching result corresponding to the pose of the target particle.

In an optional example, the second processing module 503 further includes: a third obtaining unit 5036, configured to obtain a first area map in the map based on the first GNSS pose and the preset range parameter; accordingly, the second obtaining unit 5032 is specifically configured to obtain the original road edge element and the original lane line element in the first area map.

In an alternative example, fig. 15 is a schematic structural diagram of a third processing module 504 provided in an exemplary embodiment of the present disclosure, and in this example, the third processing module 504 includes: a first determination unit 5041 and a first update unit 5042. A first determining unit 5041, configured to determine, according to the tth matching result corresponding to each tth particle pose, a tth matching score corresponding to each first particle; a first updating unit 5042, configured to update the t-1 th weight corresponding to each first particle based on the t-th matching score corresponding to each first particle obtained by the first determining unit 5041, so as to obtain the t-th weight corresponding to each first particle.

In an optional example, the first determining unit 5041 is specifically configured to: determining a t-th road edge matching score corresponding to each t-th particle pose based on the t-th road edge matching result corresponding to each t-th particle pose and a first score rule; determining a t-th lane line matching score corresponding to each t-th particle pose based on a t-th lane line matching result corresponding to each t-th particle pose and a second score rule; determining the t-th matching score corresponding to each first particle based on the t-th road edge matching score, the t-th lane line matching score, the preset road edge weight and the preset lane line weight corresponding to each t-th particle pose; the preset road edge weight is greater than the preset lane line weight.

Fig. 16 is a schematic structural diagram of an initial pose determination apparatus according to another exemplary embodiment of the present disclosure.

In one optional example, the apparatus of the present disclosure, further comprising: a second acquisition module 507, a first filtering module 508, a first sampling module 509, and a first determination module 510. A second obtaining module 507, configured to obtain a tth matching score corresponding to each first particle; a first screening module 508, configured to delete the first particle with the tth matching score lower than the first score threshold, and obtain the remaining first particles; a first sampling module 509, configured to perform lateral sampling within a second preset range based on the second GNSS pose at the t-th time to obtain a second number of second particles; the first determining module 510 is configured to use the remaining first particles and a second number of second particles as N first particles, where the second number is the number of the deleted first particles.

In an alternative example, when t =1, the apparatus of the present disclosure further includes: a third obtaining module 511. A third obtaining module 511, configured to obtain initial matching results corresponding to the initial particle poses based on the initial particle poses corresponding to the first particles, respectively; the initial matching result comprises an initial road edge matching result and an initial lane line matching result; an initial road edge matching result corresponding to an initial particle pose comprises a first matching distance between a road edge observed based on the initial particle pose and a road edge element in a map, and an initial lane line matching result corresponding to an initial particle pose comprises a second matching distance between a lane line observed based on the initial particle pose and a lane line element in the map; correspondingly, the first processing module 502 is specifically configured to: if the first matching distance corresponding to at least one initial particle pose is smaller than a first distance threshold value, and the second matching distance is smaller than a second distance threshold value, predicting the pose of each first particle at the 1 st moment based on the initial particle pose corresponding to each first particle and the odometer information corresponding to the 1 st moment, and obtaining the 1 st particle pose corresponding to each first particle.

In one optional example, the apparatus of the present disclosure, further comprising: a fourth acquisition module 512. A fourth obtaining module 512, configured to obtain a road edge matching result and a lane line matching result corresponding to the tth positioning pose; correspondingly, the fifth processing module 506 is specifically configured to: and if the road edge matching result and the lane line matching result corresponding to the tth positioning pose meet a first preset condition and each first particle is converged, taking the tth positioning pose as the initial pose of the movable equipment.

In an optional example, the fifth processing module 506 is further configured to: if the road edge matching result and the lane line matching result corresponding to the tth positioning pose meet a first preset condition and the first particles do not converge, entering a processing flow at the t +1 th moment; if the road edge matching result corresponding to the t-th positioning pose meets a first preset condition and the lane line matching result corresponding to the t-th positioning pose does not meet the first preset condition, entering a processing flow at a t +1 th moment; and if the road edge matching result corresponding to the tth positioning pose does not meet the first preset condition, returning to the initial moment.

Exemplary electronic device

An embodiment of the present disclosure further provides an electronic device, including: a memory for storing a computer program;

a processor, configured to execute the computer program stored in the memory, and when the computer program is executed, implement the method for determining the initial pose according to any of the above embodiments of the present disclosure.

Fig. 17 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 a memory 12.

The 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 the electronic device 10 to perform desired functions.

Memory 12 may include one or more computer program products that 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), cache memory (cache), and/or the like. The non-volatile memory may include, for example, Read Only Memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium and executed by the processor 11 to implement the above-described determination method of the initial pose of the various embodiments of the present disclosure and/or other desired functions. Various contents such as an input signal, a signal component, a 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, which are interconnected by a bus system and/or other form of connection mechanism (not shown).

The input means 13 may be, for example, a microphone or a microphone array as described above for capturing an input signal of a sound source.

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

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

Of course, for simplicity, only some of the components of the electronic device 10 relevant to the present disclosure are shown in fig. 17, omitting components such as buses, input/output interfaces, and the like. In addition, the electronic device 10 may include any other suitable components depending on the particular application.

Exemplary computer program product and computer-readable storage Medium

In addition to the above-described methods and apparatuses, embodiments of the present disclosure may also be a computer program product including computer program instructions that, when executed by a processor, cause the processor to perform the steps in the method of determining an initial pose according to various embodiments of the present disclosure described in the above-described "exemplary methods" section of this specification.

The computer program product may write program code for carrying out operations for embodiments of the present disclosure in any combination of one or more programming languages, including an object oriented programming language such as Java, C + + or the like and conventional procedural programming languages, such as the "C" programming 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.

Furthermore, embodiments of the present disclosure may also be a computer-readable storage medium having stored thereon computer program instructions that, when executed by a processor, cause the processor to perform the steps in the method for determining an initial pose according to various embodiments of the present disclosure described in the above-mentioned "exemplary methods" section of this specification.

The computer-readable storage medium may take 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, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples (a non-exhaustive list) of the readable storage medium include: an electrical connection having one or more wires, a portable disk, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

The foregoing describes the general principles of the present disclosure in conjunction with specific embodiments, however, it is noted that the advantages, effects, etc. mentioned in the present disclosure are merely examples and are not limiting, and they should not be considered essential to the various embodiments of the present disclosure. Furthermore, the foregoing disclosure of specific details is for the purpose of illustration and description and is not intended to be limiting, since the disclosure is not intended to be limited to the specific details so described.

In the present specification, the embodiments are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same or similar parts in the embodiments are referred to each other. For the system embodiment, since it basically corresponds to the method embodiment, the description is relatively simple, and for the relevant points, reference may be made to the partial description of the method embodiment.

The block diagrams of devices, apparatuses, systems referred to in this disclosure are only given as illustrative examples and are not intended to require or imply that the connections, arrangements, configurations, etc. must be made in the manner shown in the block diagrams. These devices, apparatuses, devices, systems may be connected, arranged, configured in any manner, as will be appreciated by those skilled in the art. Words such as "including," "comprising," "having," and the like are open-ended words that mean "including, but not limited to," and are used interchangeably therewith. The words "or" and "as used herein mean, and are used interchangeably with, the word" and/or, "unless the context clearly dictates otherwise. The word "such as" is used herein to mean, and is used interchangeably with, the phrase "such as but not limited to".

The methods and apparatus of the present disclosure may be implemented in a number of ways. For example, the methods and apparatus of the present disclosure may be implemented by software, hardware, firmware, or any combination of software, hardware, and firmware. The above-described order for the steps of the method is for illustration only, and the steps of the method of the present disclosure are not limited to the order specifically described above unless specifically stated otherwise. Further, in some embodiments, the present disclosure may also be embodied as programs recorded in a recording medium, the programs including machine-readable instructions for implementing the determination method of the initial pose 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 is also noted that in the devices, apparatuses, and methods of the present disclosure, each component or step can be decomposed and/or recombined. These decompositions and/or recombinations are to be considered equivalents of the present disclosure.

The previous 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 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 embodiments of the disclosure to the form disclosed herein. While a number of example aspects and embodiments have been discussed above, those of skill in the art will recognize certain variations, modifications, alterations, additions and sub-combinations thereof.

Claims

1. A method for determining an initial pose includes:

at the t moment, acquiring t-1 particle poses respectively corresponding to N first particles at the t-1 moment; t is a positive integer, and the initial particle poses corresponding to the N first particles at the initial moment are obtained by performing transverse sampling in a first preset range on the basis of the first GNSS pose; the first GNSS pose is the GNSS pose of the movable equipment at the initial moment;

predicting the pose of each first particle at the t moment based on the t-1 particle pose corresponding to each first particle to obtain the t particle pose corresponding to each first particle;

acquiring a tth matching result corresponding to each tth particle pose based on the tth particle pose corresponding to each first particle; the tth matching result comprises at least one of a tth road edge matching result and a tth lane line matching result;

updating the t-1 weight corresponding to each first particle based on the t matching result corresponding to each t particle pose to obtain the t weight corresponding to each first particle;

determining a tth positioning pose of the movable equipment according to the tth weight corresponding to each first particle;

acquiring a road edge matching result and a lane line matching result corresponding to the tth positioning pose;

and if the road edge matching result and the lane line matching result corresponding to the tth positioning pose meet a first preset condition and each first particle is converged, taking the tth positioning pose as the initial pose of the movable equipment.

2. The method of claim 1, wherein the obtaining the tth matching result corresponding to each tth particle pose based on the tth particle pose corresponding to each first particle comprises:

respectively taking each tth particle pose as a target particle pose, and obtaining a tth matching result corresponding to each tth particle pose based on the following steps:

determining a self-coordinate system of the movable equipment based on the pose of the target particle, and acquiring observed first path information and first lane line information under the self-coordinate system;

acquiring original road edge elements and original lane line elements in a map coordinate system in a map;

mapping the original road edge elements and the original lane line elements to the self-coordinate system to obtain corresponding first road edge elements and first lane line elements in the self-coordinate system;

matching the observed first path information with the first path element to obtain a tth path matching result;

matching the observed first lane line information with the first lane line element to obtain a tth lane line matching result;

and taking the t-th road edge matching result and the t-th lane line matching result as the t-th matching result corresponding to the target particle pose.

3. The method of claim 2, wherein prior to obtaining the original road-edge elements and the original lane-line elements in the map coordinate system in the map, further comprising:

acquiring a first area map in the map based on the first GNSS position and orientation and a preset range parameter;

the acquiring of the original road edge element and the original lane line element in the map coordinate system in the map includes:

and acquiring the original road edge element and the original lane line element in the first regional map.

4. The method according to claim 2, wherein the updating the t-1 th weight corresponding to each of the first particles based on the t-th matching result corresponding to each of the t-th particle poses to obtain the t-th weight corresponding to each of the first particles comprises:

determining a tth matching score corresponding to each first particle according to the tth matching result corresponding to each tth particle pose;

and updating the t-1 th weight corresponding to each first particle based on the t-th matching score corresponding to each first particle, so as to obtain the t-th weight corresponding to each first particle.

5. The method according to claim 4, wherein the determining the tth matching score corresponding to each of the first particles according to the tth matching result corresponding to each of the tth particle poses comprises:

determining a tth road edge matching score corresponding to each tth particle pose based on the tth road edge matching result corresponding to each tth particle pose and a first score rule;

determining a tth lane line matching score corresponding to each tth particle pose based on the tth lane line matching result corresponding to each tth particle pose and a second score rule;

determining the t-th matching score corresponding to each first particle based on the t-th road edge matching score, the t-th lane line matching score, the preset road edge weight and the preset lane line weight corresponding to each t-th particle pose; the preset road edge weight is greater than the preset lane line weight.

6. The method according to claim 1, wherein after predicting the pose of each first particle at the tth time based on the t-1 particle pose corresponding to each first particle, and obtaining the tth particle pose corresponding to each first particle, the method further comprises:

acquiring the t-th matching score corresponding to each first particle;

deleting the first particles with the t-th matching score lower than a first score threshold value to obtain the rest first particles;

performing transverse sampling in a second preset range based on the second GNSS position and posture at the t-th moment to obtain a second number of second particles;

and taking the rest first particles and the second number of second particles as N first particles, wherein the second number is the number of the deleted first particles.

7. The method according to claim 1, wherein at t =1, before predicting the pose of each first particle at the tth time based on the t-1 particle pose corresponding to each first particle, and obtaining the tth particle pose corresponding to each first particle, the method further comprises:

acquiring initial matching results corresponding to the initial particle poses respectively based on the initial particle poses respectively corresponding to the first particles; the initial matching result comprises an initial road edge matching result and an initial lane line matching result; the initial road edge matching result corresponding to one initial particle pose comprises a first matching distance between a road edge observed based on the initial particle pose and a road edge element in a map, and the initial lane line matching result corresponding to one initial particle pose comprises a second matching distance between a lane line observed based on the initial particle pose and a lane line element in the map;

predicting the pose of each first particle at the tth moment based on the t-1 th particle pose respectively corresponding to each first particle to obtain the tth particle pose respectively corresponding to each first particle, wherein the method comprises the following steps:

if the first matching distance corresponding to at least one initial particle pose is smaller than a first distance threshold and the second matching distance is smaller than a second distance threshold, predicting the pose of each first particle at the 1 st moment based on the initial particle pose corresponding to each first particle and the odometer information corresponding to the 1 st moment, and obtaining the 1 st particle pose corresponding to each first particle.

8. The method according to claim 1, wherein if the road edge matching result and the lane line matching result corresponding to the tth positioning pose satisfy a first preset condition and the first particle does not converge, the processing flow at the t +1 th moment is entered;

if the road edge matching result corresponding to the t-th positioning pose meets a first preset condition and the lane line matching result corresponding to the t-th positioning pose does not meet the first preset condition, entering a processing flow at a t +1 th moment;

and if the road edge matching result corresponding to the tth positioning pose does not meet the first preset condition, returning to the initial moment.

9. An initial pose determination apparatus comprising:

the first acquisition module is used for acquiring t-1 particle poses respectively corresponding to the N first particles at the t-1 moment; t is a positive integer, and the initial particle poses corresponding to the N first particles at the initial moment are obtained by performing transverse sampling in a first preset range on the basis of the first GNSS pose; the first GNSS pose is the GNSS pose of the movable equipment at the initial moment;

the first processing module is used for predicting the pose of each first particle at the t-th moment based on the t-1-th particle pose corresponding to each first particle to obtain the t-th particle pose corresponding to each first particle;

the second processing module is used for acquiring a tth matching result corresponding to each tth particle pose based on the tth particle pose corresponding to each first particle; the tth matching result comprises at least one of a tth road edge matching result and a tth lane line matching result;

a third processing module, configured to update a t-1 th weight corresponding to each first particle based on a t-th matching result corresponding to each t-th particle pose, so as to obtain a t-th weight corresponding to each first particle;

the fourth processing module is used for determining the tth positioning pose of the movable equipment according to the tth weight corresponding to each first particle;

the fourth acquisition module is used for acquiring a road edge matching result and a lane line matching result corresponding to the t-th positioning pose;

and the fifth processing module is used for taking the tth positioning pose as the initial pose of the movable equipment if the road edge matching result and the lane line matching result corresponding to the tth positioning pose meet a first preset condition and each first particle is converged.

10. A computer-readable storage medium storing a computer program for executing the method of determining an initial pose according to any one of claims 1 to 8.

11. An electronic device, the electronic device comprising:

a processor;

a memory for storing the processor-executable instructions;

the processor is used for reading the executable instructions from the memory and executing the instructions to realize the method for determining the initial pose as claimed in any one of the claims 1 to 8.