WO2024041447A1 - 位姿的确定方法、装置、电子设备和存储介质 - Google Patents

位姿的确定方法、装置、电子设备和存储介质 Download PDF

Info

Publication number
WO2024041447A1
WO2024041447A1 PCT/CN2023/113598 CN2023113598W WO2024041447A1 WO 2024041447 A1 WO2024041447 A1 WO 2024041447A1 CN 2023113598 W CN2023113598 W CN 2023113598W WO 2024041447 A1 WO2024041447 A1 WO 2024041447A1
Authority
WO
WIPO (PCT)
Prior art keywords
particle
lateral
state
particles
lane line
Prior art date
Application number
PCT/CN2023/113598
Other languages
English (en)
French (fr)
Inventor
林敏捷
Original Assignee
上海安亭地平线智能交通技术有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 上海安亭地平线智能交通技术有限公司 filed Critical 上海安亭地平线智能交通技术有限公司
Publication of WO2024041447A1 publication Critical patent/WO2024041447A1/zh

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching

Definitions

  • the present disclosure relates to positioning technology, and in particular, to a method, device, electronic device, and storage medium for determining posture.
  • a vehicle-mounted positioning system based on visual perception and high-precision maps realizes real-time high-precision positioning of the vehicle through system state estimation technology.
  • the system state estimation technology based on particle filtering has become a common state estimation method in vehicle positioning systems due to its low computational cost and high stability.
  • the differences in the system state setting and state update observation methods directly affect the estimation results of the filter, that is, the performance of vehicle positioning.
  • Embodiments of the present disclosure provide a posture determination method, device, electronic device, and storage medium.
  • a method for determining a pose including: determining first particle poses of m first particles corresponding to a movable device, where m is an integer greater than 1, and the first The particle pose is the pose corresponding to the first particle obtained at the previous moment; based on the pose of each first particle, the first lateral state and the first heading angle state respectively corresponding to each of the first particles at the current moment are determined; Based on the perceived lane line information, the first matching score and the second matching score respectively corresponding to each of the first particles are determined; based on each of the first matching scores, the first lateral weight of the corresponding first particle is determined.
  • Update is performed to obtain the second lateral weight corresponding to each of the first particles; based on each of the second matching scores, the first heading angle weight of the corresponding first particle is updated to obtain each of the The second heading angle weight respectively corresponding to the first particles; based on the second lateral weight, the second heading angle weight, the first lateral state, and the first heading angle corresponding to each first particle. status to determine the current positioning pose of the movable device.
  • a posture determination device including: a first determination module for determining the first particle postures of m first particles corresponding to the movable device, where m is greater than is an integer of 1, and the first particle pose is the pose corresponding to the first particle obtained at the previous moment; the first processing module is used to determine the current position of each first particle based on the pose of each first particle.
  • the current positioning pose of the device is used to determine the first matching score and the second matching score respectively corresponding to each of the first particles based on the perceived lane line information; the third A processing module, configured to update the first lateral weight of the corresponding first particle based on
  • a computer-readable storage medium stores a computer program, and the computer program is used to execute the posture determination method described in any of the above embodiments of the present disclosure. .
  • an electronic device includes: a processor; a memory for storing instructions executable by the processor; and the processor is configured to retrieve instructions from the memory.
  • the executable instructions are read and executed to implement the posture determination method described in any of the above embodiments of the present disclosure.
  • a computer program product is provided.
  • an instruction processor in the computer program product is executed, the method for determining the posture described in any of the above embodiments of the present disclosure is executed.
  • the lateral state and the heading angle state of the particle adopt different weights respectively. , thereby achieving both lateral positioning accuracy and heading angle positioning accuracy, and effectively improving positioning performance.
  • Figure 1 is an exemplary application scenario of the pose determination method provided by the present disclosure
  • Figure 2 is a schematic flowchart of a method for determining pose provided by an exemplary embodiment of the present disclosure
  • Figure 3 is a schematic flowchart of a method for determining pose provided by an exemplary embodiment of the present disclosure
  • Figure 4 is a schematic flowchart of step 203 provided by an exemplary embodiment of the present disclosure.
  • Figure 5 is a schematic flowchart of step 2022a provided by an exemplary embodiment of the present disclosure.
  • Figure 6 is a schematic flowchart of step 202 provided by an exemplary embodiment of the present disclosure.
  • Figure 7 is a schematic diagram of a first grid coordinate area provided by an exemplary embodiment of the present disclosure.
  • Figure 8 is a schematic diagram of a second particle mapping process provided by an exemplary embodiment of the present disclosure.
  • Figure 9 is a schematic flowchart of step 202 provided by another exemplary embodiment of the present disclosure.
  • Figure 10 is a schematic flowchart of a method for determining pose provided by yet another exemplary embodiment of the present disclosure.
  • Figure 11 is a schematic structural diagram of a posture determination device provided by an exemplary embodiment of the present disclosure.
  • Figure 12 is a schematic structural diagram of the second processing module 503 provided by an exemplary embodiment of the present disclosure.
  • Figure 13 is a schematic structural diagram of the fifth processing module 506 provided by an exemplary embodiment of the present disclosure.
  • Figure 14 is a schematic structural diagram of the first processing module 502 provided by an exemplary embodiment of the present disclosure.
  • Figure 15 is a schematic structural diagram of the first processing module 502 provided by another exemplary embodiment of the present disclosure.
  • Figure 16 is a schematic structural diagram of an application embodiment of the electronic device of the present disclosure.
  • a vehicle-mounted positioning system based on visual perception and high-precision maps can achieve real-time high-precision positioning of the vehicle through system state estimation technology.
  • the system state estimation technology based on particle filtering has become a common state estimation method in vehicle positioning systems due to its low computational cost and high stability.
  • the difference in the system's state setting and state update observation methods directly affects the estimation results of the filter, that is, it affects the performance of vehicle positioning. For example, if the position and attitude of the vehicle are set as the state of the system for real-time prediction and update during state estimation, one of the lateral positioning and the heading angle positioning may have higher accuracy while the other has lower accuracy, resulting in The performance of vehicle positioning is poor.
  • Figure 1 is an exemplary application scenario of the pose determination method provided by the present disclosure.
  • the initial pose of the vehicle in the navigation map can be determined first, and then high-precision positioning can be performed during the subsequent movement of the vehicle based on the initial pose.
  • Precision positioning During the movement, using the pose determination method provided by the embodiment of the present disclosure, different weights can be determined for the lateral state and the heading angle state in the positioning process based on particle filtering, so as to take into account both lateral positioning accuracy and The heading angle positioning accuracy helps to improve the overall positioning accuracy of the pose.
  • x and y represent the x-axis and y-axis of the vehicle's self-coordinate system respectively, the y-direction is the transverse direction, and the x-direction is the longitudinal direction.
  • the observation results of the lane lines can be combined.
  • the observation results of the lane lines refer to the matching results of the lane lines recognized through the environmental image collected by the camera (i.e., the perceived lane lines) and the lane lines in the map.
  • This disclosure is based on sensing lane lines and determines the first matching score and the second matching score in the particle filter that are used to update the lateral weight and heading angle weight of each particle respectively, so that different weights can be used to determine the lateral state and heading respectively.
  • the pose may include at least one of three degrees of freedom: transverse coordinate component Y (lon), longitudinal coordinate component X (lat), and heading angle ⁇ (yaw).
  • FIG. 2 is a schematic flowchart of a method for determining pose provided by an exemplary embodiment of the present disclosure. This embodiment can be applied to electronic devices, specifically such as vehicle-mounted computing platforms, as shown in Figure 2, including the following steps:
  • Step 201 Determine the first particle poses of m first particles corresponding to the movable device.
  • m is an integer greater than 1.
  • the first particle pose is the pose corresponding to the first particle obtained at the previous moment.
  • the movable device can be a vehicle, a robot, and other equipment, and is not specifically limited.
  • the first particle pose of the first particle is the pose of the particle in the particle filter determined during the positioning process at the previous moment.
  • the specific first particle number m can be set according to actual needs, and is not limited in the embodiment of the present disclosure.
  • the previous moment can be any moment before the current moment, such as the moment before the current moment, or a moment separated by a certain period of time from the current moment. The details can be determined according to actual needs.
  • step 201 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the first determination module run by the processor.
  • Step 202 Based on the pose of each first particle, determine the first lateral state and the first heading angle state respectively corresponding to the current moment of each first particle.
  • the first lateral state and the first heading angle state of the first particle at the current moment are the lateral state and heading angle state of the predicted particle pose at the current moment of the first particle relative to the predicted pose at the current moment of the movable device.
  • the predicted particle pose of the first particle at the current moment can be obtained using any implementable prediction method.
  • the predicted pose of the movable device at the current moment can also be obtained using any implementable prediction method.
  • a corresponding motion model is used for prediction.
  • the motion model can be set according to actual needs, such as an odometry-based motion model (Odometry Sample Motion Model) or other implementable motion models, which are not limited in the embodiments of the present disclosure.
  • step 202 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by the first processing module run by the processor.
  • Step 203 Based on the sensed lane line information, determine the first matching score and the second matching score respectively corresponding to each first particle.
  • the perceived lane line information is the lane line information determined based on the collected environment image in the sensing stage.
  • the perceived lane line information can include at least one perceived lane line parameter corresponding to the lane line, such as c 0 , c 1 , c 2 , c 3 , c 0 , c 1 , c 2 , c 3 represent the coefficients of the cubic equation parameterized by the curve equation of the fitted lane line.
  • the specific sensed lane line information can be set according to actual needs.
  • the first matching score and the second matching score are matching scores determined respectively for the lateral state and the heading angle state.
  • the first matching score corresponds to the lateral state
  • the second matching score corresponds to the heading angle state.
  • corresponding rules can be set. For example, for the determination of the first matching score, a first preset rule can be set, and for the determination of the second matching score, a second preset rule can be set. Default rules. The first preset rule and the second preset rule can be set according to actual needs.
  • the principle of setting the first preset rule and the second preset rule is relative to the movable device, so that the perception at a close distance of the movable device
  • the matching score of the lane line sampling point contributes greatly to the first matching score, and the score of the sampling point at the mid-to-long distance contributes less to the first matching score, so that the lateral positioning accuracy is higher; in the movable device at the mid-to-long distance
  • the matching score of the sensing lane line sampling point contributes more to the second matching score, and the matching score of the sampling point at a closer distance contributes less to the second matching score, so that the heading angle positioning accuracy is higher.
  • the weight is determined based on the total matching score of the lane line. Due to the lane acquisition by visual perception, There is a certain difference in the geometric shape of the line and the lane line in the map, so that the matching score of the perceived lane line matching the map lane line has the following characteristics when updating the particle state: When the matching score of the sensing lane line sampling point at a close distance is The lateral positioning accuracy is higher when the contribution to the total matching score is greater. The heading angle positioning accuracy is higher when the matching score of the sensing lane line sampling point at a far distance contributes to the total matching score.
  • the embodiments of the present disclosure update the lateral status and the heading angle status respectively through different matching scores, thereby taking into account the lateral positioning accuracy and the heading angle positioning accuracy, so that both the lateral positioning and the heading angle positioning can have higher accuracy, thereby contributing to Improve overall positioning performance.
  • step 203 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by a second processing module run by the processor.
  • Step 204 Based on each first matching score, update the first lateral weight of the corresponding first particle respectively, and obtain the second lateral weight corresponding to each first particle.
  • the first lateral weight of the first particle is the weight corresponding to the first lateral state, and the first lateral weight may be determined and stored during the positioning process at the previous moment. Updating the first horizontal weight based on the first matching score means adjusting the weight of the first horizontal state based on the matching situation at the current moment.
  • the specific update rules can be set according to actual needs. For example, when the matching effect is good, the first lateral weight of the first particle can be increased to obtain the corresponding updated second lateral weight. When the matching effect is poor, the first lateral weight of the first particle can be reduced, and the specific principle will not be described again.
  • step 204 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by a third processing module run by the processor.
  • Step 205 Based on each second matching score, update the first heading angle weight of the corresponding first particle respectively, and obtain the second heading angle weight corresponding to each first particle.
  • the first heading angle weight of the first particle is the weight corresponding to the first heading angle state, and the first heading angle weight is also determined and stored during the positioning process at the previous moment.
  • the updating principle of the first heading angle weight is similar to the above-mentioned first lateral weight, and will not be described again here.
  • Step 204 and step 205 are in no particular order.
  • step 205 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by a fourth processing module run by the processor.
  • Step 206 Determine the current positioning posture of the movable device based on the second lateral weight, the second heading angle weight, the first lateral state, and the first heading angle state corresponding to each first particle.
  • each first particle simulates the pose of a movable device
  • the second lateral weight, the second heading angle weight corresponding to each first particle, and the first lateral state and sum of the current moment of each first particle are determined.
  • the current positioning posture of the movable device can be determined based on the second lateral weight, the second heading angle weight, the first lateral state, and the first heading angle state corresponding to each first particle.
  • the pose can include three components: transverse, longitudinal and heading angles
  • the first longitudinal state and corresponding weight at the current moment corresponding to each first particle can be implemented based on any implementable particle filter, such as based on histogram filtering implementation, the embodiments of this disclosure are not limited.
  • step 206 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by the fifth processing module run by the processor.
  • the pose determination method provided by this embodiment determines different matching scores for the lateral state and heading angle state of each particle during the positioning process, and uses them to update the lateral weight and heading angle weight respectively, so that the particles
  • the lateral state and heading angle state have different weights respectively, which separates the lateral state and heading angle state, thereby achieving both lateral positioning accuracy and heading angle positioning accuracy, which helps to improve positioning performance.
  • FIG. 3 is a schematic flowchart of a pose determination method provided by an exemplary embodiment of the present disclosure.
  • step 203 determines the first matching score and the second matching score respectively corresponding to each first particle based on the perceived lane line information, which may include the following steps:
  • Step 2031 Based on the perceived lane line information, determine the lane line sampling point in the vehicle coordinate system.
  • the perceived lane line information is the lane line information determined based on the collected environment image in the sensing stage.
  • the perceived lane line information can include the perceived lane line parameters, such as c 0 , c 1 , c 2 , c 3 , c 0 , c 1 , c 2 , c 3 represent the coefficients of the cubic equation parameterized by the curve equation of the fitted lane line.
  • the specific sensed lane line information can be set according to actual needs.
  • the vehicle coordinate system is the vehicle's self-coordinate system with the center of the vehicle's rear axle as the origin, and the details will not be described again.
  • the lane line sampling points in the vehicle coordinate system are based on the perceived lane line information and are obtained by sampling the perceived lane lines in the vehicle coordinate system.
  • the specific sampling method can be set according to actual needs. For example, in the image coordinate system (the upper left corner of the image is the origin, the u axis is horizontally to the right, and the v axis is vertically downward), the center of the perceived image corresponding to the perceived lane line information is Starting point, sample along the v direction according to the preset sampling interval to obtain N original sampling points.
  • the N original sampling points are projected to the vehicle coordinates respectively through the camera's internal parameters, external parameters and the mapping relationship between the image coordinate system and the vehicle coordinate system.
  • the x-axis of the system is used to obtain N x-axis coordinates, where the i-th x-axis coordinate is expressed as Then, the lane line sampling points in the vehicle coordinate system corresponding to the N original sampling points can be determined based on the perceived lane line information.
  • t represents the current moment
  • c 0 , c 1 , c 2 , and c 3 represent the perceived lane line parameters, that is, the coefficients of the cubic equation parameterized by the curve equation of the lane line mentioned above.
  • step 2031 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the first determination unit run by the processor.
  • Step 2032 Use each first particle as a movable device, convert the first map lane line into the vehicle coordinate system, and obtain the second map lane line in the vehicle coordinate system corresponding to each first particle.
  • the first map lane line is a lane line in a high-precision map
  • the first map lane line may include relevant information of at least one lane line, and may be set according to actual needs. For example, based on the vehicle's positioning position at the previous moment, a local map within a certain range around the vehicle's positioning position can be obtained, and the lane lines in the local map can be obtained as the first map lane lines.
  • Treating the first particle as a movable device means that the first particle is considered a movable device, and the predicted particle pose of the first particle at the current moment (which can be called the first predicted particle pose) is used as the movable device.
  • the particle pose establishes the vehicle coordinate system of the movable device. Specifically, the position component (X, Y) in the first predicted particle pose is used as the origin of the vehicle coordinate system of the movable device, and the vehicle coordinate system is determined based on the heading angle (attitude component) in the first predicted particle pose. x-axis and y-axis directions. According to the mapping relationship between the vehicle coordinate system and the map coordinate system obtained in advance, the first map lane line is converted to the vehicle coordinate system, and the second map lane line in the vehicle coordinate system corresponding to each first particle is obtained.
  • step 2032 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by the first conversion unit run by the processor.
  • Step 2033 Determine the first matching score corresponding to each first particle based on the lane line sampling point, the second map lane line corresponding to each first particle, and the first preset rule.
  • the first preset rule is a matching score determination rule for lane line sampling points corresponding to the lateral state, which can be set according to actual needs.
  • the setting principle of the first preset rule is to make the matching scores of lane line sampling points at close distances relatively small. High, medium and long distance lane line sampling point matching scores are relatively low to improve lateral positioning accuracy.
  • the minimum lateral distance between the lane line sampling point and the second map lane line can be determined based on each lane line sampling point and the second map lane line, and then a maximum lateral distance threshold is preset based on the minimum lateral distance. and the above-mentioned first preset rule, determine the first sampling point score of the lane line sampling point, and determine the first matching score corresponding to the first particle based on the first sampling point score corresponding to each lane line sampling point.
  • the minimum lateral distance between the lane line sampling point and the second map lane line may refer to the lateral distance between the lane line sampling point and the nearest lane line among the second map lane lines.
  • the first matching score corresponding to the first particle may be the sum of the first sampling point scores of each lane line sampling point, and may be set according to actual requirements.
  • step 2033 may be executed by the processor calling a corresponding instruction stored in the memory, or may be executed by a second determination unit run by the processor.
  • Step 2034 Determine the second matching score corresponding to each first particle based on the lane line sampling point, the second map lane line corresponding to each first particle, and the second preset rule.
  • the second preset rule is the matching score determination rule of the lane line sampling point corresponding to the heading angle state, which can be set according to actual needs.
  • the setting principle of the second preset rule is to make the matching score of the lane line sampling point at a close distance relatively
  • the matching score of lane line sampling points at medium and long distances is relatively high to improve the heading angle positioning accuracy.
  • the minimum lateral distance between the lane line sampling point and the second map lane line can be determined based on each lane line sampling point and the second map lane line, and then a maximum lateral distance threshold is preset based on the minimum lateral distance. and the above-mentioned second preset rule, determine the second sampling point score of the lane line sampling point, and determine the second matching score corresponding to the first particle based on the second sampling point score corresponding to each lane line sampling point.
  • the minimum lateral distance between the lane line sampling point and the second map lane line may refer to the lateral distance between the lane line sampling point and the nearest lane line among the second map lane lines.
  • the second matching score corresponding to the first particle may be the sum of the second sampling point scores of each lane line sampling point, and may be set according to actual requirements.
  • step 2034 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by a third determination unit run by the processor.
  • Figure 4 is a schematic flowchart of step 203 provided by an exemplary embodiment of the present disclosure.
  • step 2033 determines the first matching score corresponding to each first particle based on the lane line sampling point, the second map lane line corresponding to each first particle, and the first preset rule, including:
  • Step 20331 For each first particle, based on each lane line sampling point and the second map lane line, determine the minimum lateral distance between each lane line sampling point and the second map lane line.
  • the minimum lateral distance between the lane line sampling point and the lane line on the second map refers to the lateral distance between the lane line sampling point and the nearest lane line among the lane lines on the second map.
  • step 20331 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the second determination unit run by the processor.
  • Step 20332 Based on each minimum lateral distance, the preset maximum lateral distance threshold and the first preset rule, determine the first sampling point score corresponding to each lane line sampling point.
  • the preset maximum lateral distance threshold can be set according to actual needs, and is not limited in the embodiment of the present disclosure.
  • the first sampling point score refers to the matching score at the lane line sampling point for the lateral state.
  • step 20332 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the second determination unit run by the processor.
  • Step 20333 Based on the first sampling point scores corresponding to each lane line sampling point, determine the first sampling point total score of each lane line sampling point as the first matching score corresponding to the first particle.
  • the first preset rule can be set as:
  • x represents the x-axis coordinate in the vehicle coordinate system
  • F(x) can be called the first matching score weight. It can be seen that when the lane line sampling point is closer to the origin of the vehicle coordinate system, the first matching score weight is larger. When the lane line sampling point is far away from the origin of the vehicle coordinate system, the first matching score has a smaller weight.
  • the i-th lane line sampling point The first match score weight of is:
  • the first sampling point score of the i-th lane line sampling point can be determined.
  • the minimum lateral distance between the lane line sampling point and the second map lane line can be determined based on each lane line sampling point and the second map lane line, and then a maximum lateral distance threshold is preset based on the minimum lateral distance. and the above-mentioned first matching score weight to determine the first sample of the lane line sampling point
  • the point score determines the first matching score corresponding to the first particle based on the first sampling point score corresponding to each lane line sampling point.
  • the first matching score corresponding to the first particle may be the sum of the first sampling point scores of each lane line sampling point, and may be set according to actual requirements.
  • the i-th lane line sampling point The score of the first sampling point can be expressed as
  • g represents the preset maximum lateral distance threshold
  • t represents the current moment
  • m represents the number of first particles
  • m is a positive integer.
  • the sum of the first sampling point scores of the N lane line sampling points can be used to obtain the first matching score corresponding to the first particle.
  • step 20333 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the second determination unit run by the processor.
  • step 2034 determines the second matching score corresponding to each first particle based on the lane line sampling point, the second map lane line corresponding to each first particle, and the second preset rule, including:
  • Step 20341 For each first particle, based on each lane line sampling point and the second map lane line, determine the minimum lateral distance between each lane line sampling point and the second map lane line.
  • This step is consistent with the above-mentioned step 20331 and will not be repeated here. In practical applications, this step and the above-mentioned step 20331 may be the same step.
  • step 20341 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by a third determination unit run by the processor.
  • Step 20342 Based on each minimum lateral distance, the preset maximum lateral distance threshold and the second preset rule, determine the second sampling point score corresponding to each lane line sampling point.
  • the second sampling point score refers to the matching score at the lane line sampling point for the heading angle state.
  • step 20342 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by a third determination unit run by the processor.
  • Step 20343 Based on the second sampling point scores corresponding to each lane line sampling point, determine the second sampling point total score of each lane line sampling point as the second matching score corresponding to the first particle.
  • x represents the x-axis coordinate in the vehicle coordinate system
  • G(x) can be called the second matching score weight. It can be seen that when the lane line sampling point is closer to the origin of the vehicle coordinate system, the second matching score weight is larger. When the lane line sampling point is far away from the origin of the vehicle coordinate system, the second matching score has a smaller weight.
  • the i-th lane line sampling point The second match score weight of is:
  • the second sampling point score of the i-th lane line sampling point can be determined.
  • the minimum lateral distance between the lane line sampling point and the second map lane line can be determined based on each lane line sampling point and the second map lane line, and then a maximum lateral distance threshold is preset based on the minimum lateral distance. and the above-mentioned second matching score weight, determine the second sampling point score of the lane line sampling point, and determine the second matching score corresponding to the first particle based on the second sampling point score corresponding to each lane line sampling point.
  • the minimum lateral distance between the lane line sampling point and the lane line on the second map refers to the lateral distance between the lane line sampling point and the nearest lane line among the lane lines on the second map.
  • the second matching score corresponding to the first particle may be the sum of the second sampling point scores of each lane line sampling point, and may be set according to actual requirements.
  • the i-th lane line sampling point The score of the second sampling point can be expressed as
  • g represents the preset maximum lateral distance threshold
  • t represents the current moment
  • m represents the number of first particles
  • m is a positive integer.
  • the sum of the second sampling point scores of the N lane line sampling points can be used to obtain the second matching score corresponding to the first particle.
  • the first transverse weight of the first particle can be and the first heading angle weight Update to obtain the second lateral weight corresponding to the first particle and the second heading angle weight as follows:
  • M G represents the observation score of the GNSS sensor, which is expressed as follows:
  • GNSS yaw represents the heading angle output by the GNSS sensor, Indicates that the l-th first particle corresponds to the first heading angle state.
  • 0.001 and 10 are preset values and can be adjusted according to actual needs.
  • step 20343 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by a third determination unit run by the processor.
  • This disclosed embodiment uses different matching scores to update the particle lateral weight and heading angle weight respectively, so that the lateral weight of the movable device makes a greater contribution to the matching score of the lane line sampling point at a short distance, and the heading angle weight is at a medium and long distance.
  • the matching score of lane line sampling points at a distance contributes greatly, so that both lateral positioning and heading angle positioning can have higher accuracy.
  • step 206 determines the current positioning posture of the movable device based on the second lateral weight, the second heading angle weight, the first lateral state, and the first heading angle state corresponding to each first particle, include:
  • Step 2061 Cluster each first particle to obtain a first number of clusters.
  • clustering can use any implementable clustering algorithm, such as using K-means clustering algorithm for clustering, and there is no specific limit.
  • the first quantity can be set according to actual needs, or determined according to a specific clustering algorithm, and is not specifically limited.
  • Each cluster includes at least one first particle.
  • step 2061 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the clustering unit run by the processor.
  • Step 2062 For each cluster, determine the second lateral state corresponding to the cluster based on the second lateral weight, the second heading angle weight, the first lateral state, and the first heading angle state corresponding to the first particle in the cluster. , the second heading angle state and the third lateral weight.
  • the second lateral state corresponding to the cluster can be obtained by weighting the first lateral state of each first particle in the cluster according to the second lateral weight.
  • the second heading angle state corresponding to the cluster can be obtained by weighting the second heading angle state of each first particle in the cluster according to the second heading angle weight.
  • the third transverse weight corresponding to the cluster may be the sum of the second transverse weights of each first particle in the cluster.
  • the first transverse state of the first particle is The first heading angle state is
  • the second horizontal weight is
  • the second horizontal state corresponding to the cluster Second heading angle state and the third horizontal weight are as follows:
  • the first lateral state, first heading angle state, second lateral weight and second heading angle weight of each first particle in the cluster are respectively consistent with the corresponding first particle obtained above.
  • the symbolic representation of The weights of the first lateral state, the first heading angle, and the second heading angle are the same and will not be repeated here.
  • step 2062 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by the first processing unit run by the processor.
  • Step 2063 Use the second lateral state and the second heading angle state of the cluster with the largest third lateral weight as the target lateral state and the target heading angle state respectively.
  • the second particle of the cluster can be The lateral state and the second heading angle state are used as the target lateral state and the target heading angle state to determine the pose of the movable device, which helps to improve the positioning accuracy of the pose of the movable device.
  • step 2063 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by a second processing unit run by the processor.
  • Step 2064 Obtain the target longitudinal state determined based on the first histogram filter.
  • the first histogram filter can be implemented in any implementable manner, which is not limited by the embodiment of the present disclosure.
  • Embodiments of the present disclosure may only use the longitudinal state determined by the first histogram filter, and may ignore its lateral state and heading angle state.
  • step 2064 may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by the first acquisition unit run by the processor.
  • Step 2065 Determine the current positioning posture of the movable device based on the target longitudinal state, the target lateral state, the target heading angle state, and the first predicted posture of the movable device at the current moment.
  • the first predicted pose of the movable device at the current moment may be determined based on the positioning pose of the movable device at the previous moment, the odometer information of the movable device, and the motion model. The specific prediction principles will not be described again.
  • the target lateral state at the current moment is expressed as The target heading angle status is
  • the target longitudinal state is expressed as
  • the first predicted pose of the mobile device at the current moment is expressed as Then the current positioning pose of the movable device is
  • T represents the transpose operation
  • SE2 pose which is specifically defined as follows:
  • step 2065 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by a third processing unit run by the processor.
  • step 202 determines the first lateral state and the first heading angle state respectively corresponding to the current moment of each first particle based on the pose of each first particle, including:
  • Step 2021a Based on the first particle pose, determine the first predicted particle pose corresponding to each first particle.
  • the first predicted particle pose is obtained by predicting the current pose of the first particle based on the movement of the movable device (such as odometer information) in the time period from the previous moment to the current moment.
  • the specific prediction method can be Any implementable method, such as prediction based on motion models.
  • the first predicted particle pose of the first particle at the current moment can be predicted based on the odometer relative increment ⁇ Odom calculated from the vehicle chassis CAN information:
  • t represents the current time
  • t-1 represents the previous time
  • m represents the number of first particles
  • m is a positive integer.
  • ⁇ Odom represents the relative increment of the odometry
  • W l represents the Gaussian white noise corresponding to the l-th first particle.
  • the Gaussian white noise can be different for different first particles, and can be set according to actual needs.
  • step 2021a may be executed by the processor calling a corresponding instruction stored in the memory, or may be executed by a fourth determination unit run by the processor.
  • Step 2022a Based on the first predicted particle pose, determine the first lateral state and the first heading angle state corresponding to each first particle.
  • the first lateral state and the first heading angle state of the first particle at the current moment are the lateral state and heading angle state of the predicted particle pose at the current moment of the first particle relative to the predicted pose at the current moment of the movable device. Therefore, the first lateral state and the first heading angle state respectively corresponding to each first particle can be determined based on each first predicted particle pose and the first positioning pose of the movable device.
  • step 2022a may be performed by the processor calling a corresponding instruction stored in the memory, or may be performed by a fifth determination unit run by the processor.
  • FIG. 5 is a schematic flowchart of step 2022a provided by an exemplary embodiment of the present disclosure.
  • step 2022a determines the first lateral state and the first heading angle state corresponding to each first particle based on each first predicted particle pose, including:
  • Step 2022a1 Determine the first predicted pose of the movable device at the current time based on the first positioning pose of the movable device determined at the previous time.
  • the first positioning pose is the positioning pose of the movable device determined in the positioning process at the previous time (t-1), which can be expressed as The first predicted pose of the mobile device at the current moment It can be determined based on the first positioning pose and the odometer relative increment ⁇ Odom, which is expressed as follows:
  • step 2022a1 may be executed by the processor calling a corresponding instruction stored in the memory, or may be executed by a fifth determination unit run by the processor.
  • Step 2022a2 Based on the first predicted particle pose and the first predicted pose, determine the first lateral state and the first heading angle state corresponding to each first particle.
  • the first lateral state of the l-th first particle is expressed as The first heading angle state is expressed as but:
  • t represents the current time
  • c[1] (a[1]-b[1])*cos(b[2])-(a[1]-b[1])*sin(b[2])
  • c[2] a[2]-b[2]
  • step 2022a2 may be executed by the processor calling a corresponding instruction stored in the memory, or may be executed by a fifth determination unit run by the processor.
  • FIG. 6 is a schematic flowchart of step 202 provided by an exemplary embodiment of the present disclosure.
  • step 202 determines the first lateral state and the first heading angle state respectively corresponding to the current moment of each first particle based on the pose of each first particle, including:
  • Step 2021b For each first particle pose, generate second predicted particle poses corresponding to n second particles based on the first particle pose.
  • n second particles can be generated based on odometry information and n different Gaussian white noises.
  • j represents the j-th second particle
  • ⁇ Odom represents the relative increment of the odometer of the mobile device
  • W lj represents the Gaussian white noise corresponding to the j-th second particle.
  • step 2021b may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by a fourth processing unit run by the processor.
  • Step 2022b Based on the second predicted particle pose corresponding to each second particle, determine the third lateral state and the third heading angle state corresponding to each second particle.
  • the determination principle of the third transverse state and the third heading angle state is similar to the first transverse state and the first heading angle state of the first particle, and will not be described again here.
  • the weight (including the transverse weight and the longitudinal weight) of each second particle corresponding to the l-th first particle is set to be the same as the first transverse weight and the first heading angle weight of the l-th first particle.
  • the first lateral weight and the first heading angle weight of the first particle are determined and stored at the previous moment.
  • the first lateral weight of the l-th first particle is expressed as Before the weight is updated at the current moment, the first heading angle weight is the same as the first lateral weight, which is also That is, after each positioning process is completed, the determined weight of the lateral state of the first particle will be used as the first lateral weight and the first heading angle weight of the first particle at the next moment. At the current moment, the first determined at the previous moment will be used.
  • the weight of the particle's lateral state As the first lateral weight and the first heading angle weight.
  • the corresponding fourth transverse weight of the j-th second particle and the fourth heading angle weight Respectively expressed as:
  • step 2022b may be performed by the processor calling corresponding instructions stored in the memory, or may be performed by a fifth processing unit run by the processor.
  • Step 2023b based on the third lateral state and the third heading angle state respectively corresponding to the m*n second particles, map each second particle to the first grid coordinate area, and obtain the cell to which each second particle belongs.
  • the first grid coordinate area is an area under a pre-established grid coordinate system.
  • the number of cells included in the first grid coordinate area is the same as the number of first particles m.
  • m yaw and m lat can be set according to actual needs.
  • the establishment of the first grid coordinate area can be based on the preset lateral state threshold, the preset heading angle state threshold, the preset lateral state step, and the preset heading angle state step.
  • Each preset threshold can be integrated with each second particle.
  • the corresponding third lateral state and third heading angle state are respectively determined so that all or most of the second particles can be projected into the first grid coordinate area, which can be set according to actual needs.
  • FIG. 7 is a schematic diagram of the first grid coordinate area provided by an exemplary embodiment of the present disclosure.
  • the abscissa of the first grid coordinate area corresponds to the yaw angle state (yaw), and the ordinate corresponds to the lateral state (lat).
  • d yaw and d lat represent the heading angle state step and lateral state step respectively.
  • the position of each cell in the first grid coordinate area relative to the origin of the coordinates is different. Specifically, it can be determined according to the actual situation. Requirement settings are not limited by the embodiments of the present disclosure.
  • the coordinate origin of the first grid coordinate area is the center point of the entire area.
  • Each cell uses [b, d] to represent its position relative to the origin, and has the following formula: situation:
  • the coordinate range of each cell in the first grid coordinate area can be based on the cell position [b, d], the specific conditions of m yaw and m lat , and the yaw and lat directions of each cell.
  • the step lengths d yaw and d lat are determined, and the details will not be described again.
  • each cell may be mapped to one or more second particles.
  • the abscissa may correspond to the lateral state (lat), and the ordinate may correspond to the yaw angle state (yaw). There is no specific limit.
  • step 2023b may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by a sixth processing unit run by the processor.
  • Step 2024b Based on the cells to which each second particle belongs, determine the third particle corresponding to each cell, the third predicted particle pose corresponding to each third particle, and the fourth horizontal direction corresponding to each third particle. state and the fourth heading angle state.
  • the third particle corresponding to the cell is a particle corresponding to the cell obtained by granulating the cell based on each second particle mapped to the cell.
  • the third predicted particle pose corresponding to each third particle may be determined by a weighted average of the second predicted particle poses of all second particles in the cell corresponding to the third particle.
  • the fourth lateral state and the fourth heading angle state corresponding to each third particle's posture are weighted respectively by the third lateral state and the third heading angle state of all second particles in the cell corresponding to the third particle. average gain.
  • the j-th second particle generated by the l-th first particle is mapped to the k-th cell and becomes the i-th second particle in the k-th cell, then I won’t go into details one by one.
  • the third transverse state corresponding to the second particle is Third heading angle status
  • the third particle corresponding to the k-th cell is called the k-th third particle
  • the third predicted particle pose corresponding to the third particle is expressed as
  • the fourth transverse state is expressed as and the fourth heading angle state
  • the particle weight is the weight determined at the previous moment of the first particle that generated the second particle. For example, the second particle is generated by the l-th first particle, then The details can be determined according to the actual mapping situation, and will not be repeated here.
  • step 2024b may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the seventh processing unit run by the processor.
  • Step 2025b The fourth lateral state and the fourth heading angle state respectively corresponding to each third particle are used as the first lateral state and the first heading angle state respectively corresponding to each first particle at the current moment.
  • FIG. 8 is a schematic diagram of a second particle mapping process provided by an exemplary embodiment of the present disclosure.
  • the black circle represents the second particle
  • the gray circle represents the third particle.
  • step 2025b may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the eighth processing unit run by the processor.
  • the embodiment of the present disclosure expands the number of particles by sampling m first particles multiple times, and performs a gridding process based on the expanded particles, thereby improving positioning accuracy on the basis of ensuring the stability of particle distribution.
  • Figure 9 is a schematic flowchart of step 202 provided by another exemplary embodiment of the present disclosure.
  • step 2021b generates, for each first particle pose, second predicted particle poses corresponding to n second particles based on the first particle pose, including:
  • Step 2021b1 Based on the first particle pose, odometry information, and n different Gaussian white noises, second predicted particle poses corresponding to n second particles are generated.
  • j represents the j-th second particle
  • ⁇ Odom represents the relative increment of the odometer of the mobile device
  • W lj represents the Gaussian white noise corresponding to the j-th second particle.
  • step 2021b1 may be executed by the processor calling a corresponding instruction stored in the memory, or may be executed by a fourth processing unit run by the processor.
  • step 2022b determines the third lateral state and the third heading angle state corresponding to each second particle based on the second predicted particle pose corresponding to each second particle, including:
  • Step 2022b1 Determine the first predicted pose of the movable device at the current time based on the first positioning pose of the movable device determined at the previous time.
  • This step is consistent with the aforementioned step 2022a1 and will not be described again.
  • step 2022b1 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the fifth processing unit run by the processor.
  • Step 2022b2 Based on the second predicted particle pose and the first predicted pose corresponding to each second particle, determine the third lateral state and the third heading angle state corresponding to each second particle.
  • step 2022b2 may be executed by the processor calling corresponding instructions stored in the memory, or may be executed by the fifth processing unit run by the processor.
  • FIG. 10 is a schematic flowchart of a pose determination method provided by yet another exemplary embodiment of the present disclosure.
  • the method includes:
  • the first particle pose of the l-th first particle is expressed as
  • For each first particle pose generate second predicted particle poses corresponding to n second particles based on the first particle pose, odometry information, and n different Gaussian white noises.
  • j represents the j-th second particle
  • ⁇ Odom represents the relative increment of the odometer of the mobile device
  • W lj represents the Gaussian white noise corresponding to the j-th second particle.
  • the first positioning pose is The first predicted pose of the mobile device at the current moment is Expressed as follows:
  • the fourth lateral weight of the j-th second particle and the fourth heading angle weight Respectively expressed as:
  • the j-th second particle generated by the l-th first particle is mapped to the k-th cell and becomes the i-th second particle in the k-th cell, then I won’t go into details one by one.
  • the third transverse state corresponding to the second particle is Third heading angle status
  • the third particle corresponding to the k-th cell is called the k-th third particle
  • the third predicted particle pose corresponding to the third particle is expressed as
  • the fourth transverse state is expressed as and the fourth heading angle state
  • the particle weight is the weight determined at the previous moment of the first particle that generated the second particle. For example, the second particle is generated by the l-th first particle, then The details can be determined according to the actual mapping situation, and will not be repeated here.
  • the fifth transverse weight of the k-th third particle and the fifth heading angle weight are respectively
  • the k-th first particle is the above-mentioned k-th third particle.
  • the first lateral weight of the kth first particle is the fifth lateral weight of the above-mentioned kth third particle
  • the first heading angle weight of the kth first particle is the above-mentioned kth third particle. The fifth heading angle weight.
  • t represents the current time
  • c 0 , c 1 , c 2 , and c 3 represent the perceived lane line parameters.
  • each first particle as a movable device, convert the first map lane line into the vehicle coordinate system, and obtain the second map lane line in the vehicle coordinate system corresponding to each first particle.
  • each first particle based on each lane line sampling point and the second map lane line, determine the minimum lateral distance between each lane line sampling point and the second map lane line.
  • the preset maximum lateral distance threshold and the first preset rule determine the first sampling point score corresponding to each lane line sampling point.
  • the i-th lane line sampling point The score of the first sampling point can be expressed as
  • g represents the preset maximum lateral distance threshold
  • t represents the current moment
  • m represents the number of first particles
  • m is a positive integer
  • the score weight of the first match
  • the preset maximum lateral distance threshold and the second preset rule determine the second sampling point score corresponding to each lane line sampling point.
  • the i-th lane line sampling point The score of the second sampling point can be expressed as
  • g represents the preset maximum lateral distance threshold
  • t represents the current moment
  • m represents the number of first particles
  • m is a positive integer
  • the second match score weight of is .
  • GNSS yaw represents the heading angle output by the GNSS sensor, Indicates that the k-th first particle corresponds to the first heading angle state.
  • 0.001 and 10 are preset values and can be adjusted according to actual needs.
  • the first transverse state of a particle is The first heading angle state is
  • the second horizontal weight is The second heading angle weight is Then the second horizontal state corresponding to the cluster Second heading angle state and the third horizontal weight
  • the first lateral state, first heading angle state, second lateral weight and second heading angle weight of each first particle in the cluster are respectively consistent with the corresponding first particle (third particle) obtained above.
  • the aforementioned k-th first particle corresponds to the f-th first particle in the s-th cluster
  • the f-th first particle in the s-th cluster is Second horizontal weight
  • the weights of the first lateral state, the first heading angle, and the second heading angle are the same and will not be repeated here.
  • the target horizontal state is expressed as
  • the target heading angle state is expressed as
  • the target longitudinal state is expressed as
  • T represents the transpose operation.
  • step 13.1 Use the second lateral weight corresponding to each first particle determined in step 13.1 as the first lateral weight and the first heading angle weight of the first particle at the next moment, and enter the iterative process at the next moment.
  • next time is regarded as the current time and the above steps 1-19 are repeated.
  • the pose determination method of the embodiment of the present disclosure separates the lateral state and the heading angle state in the process of calculating the state observation score and updating the state based on the particle filter, and calculates the lateral state and the heading angle state separately, thereby realizing the positioning process. It can simultaneously improve the lateral and heading angle accuracy. Whether the vehicle is driving on a straight road or a curve on a high-precision map, the vehicle's lateral and heading angle positioning accuracy can reach a consistent high level and is not easily affected by the curvature of the map lane line. Even when there is a certain deviation between the geometry of the perceived lane lines and the shape of the map lane lines, high-precision positioning results can still be obtained, effectively improving the accuracy and stability of the overall vehicle in the real-time positioning process.
  • the number of particles is expanded by generating multiple second particles for each first particle, and then combined with the grid coordinate area, the expanded second particles are combined into a third particle with the same number of first particles.
  • Any posture determination method provided by the embodiments of the present disclosure can be executed by any appropriate device with data processing capabilities, including but not limited to: terminal devices and servers.
  • any of the posture determination methods provided by the embodiments of the present disclosure can be executed by the processor.
  • the processor executes any of the posture determination methods mentioned in the embodiments of the present disclosure by calling corresponding instructions stored in the memory. No further details will be given below.
  • the aforementioned program can be stored in a computer-readable storage medium.
  • the program When the program is executed, It includes the steps of the above method embodiment; and the aforementioned storage medium includes: ROM, RAM, magnetic disk or optical disk and other various media that can store program codes.
  • Figure 11 is a schematic structural diagram of a posture determination device provided by an exemplary embodiment of the present disclosure.
  • the device of this embodiment can be used to implement the corresponding method embodiment of the present disclosure.
  • the device shown in Figure 11 includes: a first determination module 501, a first processing module 502, a second processing module 503, a third processing module 504, The fourth processing module 505 and the fifth processing module 506.
  • the first determination module 501 is used to determine the first particle poses of m first particles corresponding to the movable device. m is an integer greater than 1.
  • the first particle pose is the position corresponding to the first particle obtained at the previous moment. posture;
  • the first processing module 502 is used to determine the first lateral state and the first heading angle state corresponding to each first particle at the current moment based on the posture of each first particle determined by the first determination module 501;
  • the fourth processing module 505 is used to based on each second matching score obtained by the second processing module 503 , respectively update the first heading angle weight of the corresponding first particle, and obtain the second heading angle weight corresponding to each
  • FIG. 12 is a schematic structural diagram of the second processing module 503 provided by an exemplary embodiment of the present disclosure.
  • the second processing module 503 includes: a first determination unit 5031, a first conversion unit 5032, a second determination unit 5033, and a third determination unit 5034.
  • the first determination unit 5031 is used to determine the lane line sampling points in the vehicle coordinate system based on the perceived lane line information; the first conversion unit 5032 is used to use each first particle as a movable device to convert the first map lane line Convert to the vehicle coordinate system to obtain the second map lane line in the vehicle coordinate system corresponding to each first particle; the second determination unit 5033 is used to obtain the second map corresponding to each first particle based on the lane line sampling point.
  • the lane line and the first preset rule determine the first matching score corresponding to each first particle; the third determination unit 5034 is used to determine the second map lane line and the third map lane line corresponding to each first particle based on the lane line sampling point, respectively. Two preset rules determine the second matching score corresponding to each first particle.
  • the second determination unit 5033 is specifically configured to: for each first particle, based on each lane line sampling point and the second map lane line, determine the relationship between each lane line sampling point and the second map lane line. Minimum lateral distance; based on each minimum lateral distance, the preset maximum lateral distance threshold and the first preset rule, determine the first sampling point score corresponding to each lane line sampling point; based on the first sampling point corresponding to each lane line sampling point Point score determines the total score of the first sampling point of each lane line sampling point as the first matching score corresponding to the first particle.
  • the third determination unit 5034 is specifically configured to: for each first particle, based on each lane line sampling point and the second map lane line, determine the relationship between each lane line sampling point and the second map lane line. Minimum lateral distance; based on each minimum lateral distance, the preset maximum lateral distance threshold and the second preset rule, determine the second sampling point score corresponding to each lane line sampling point; based on the second sampling point corresponding to each lane line sampling point Point score determines the total score of the second sampling point of each lane line sampling point as the second matching score corresponding to the first particle.
  • FIG. 13 is a schematic structural diagram of the fifth processing module 506 provided by an exemplary embodiment of the present disclosure.
  • the fifth processing module 506 includes: a clustering unit 5061, a first processing unit 5062, a second processing unit 5063, a first obtaining unit 5064, and a third processing unit 5065.
  • the clustering unit 5061 is used for clustering each first particle to obtain a first number of clusters; the first processing unit 5062 is used for each cluster, based on the second particle corresponding to the first particle in the cluster.
  • the lateral weight, the second heading angle weight, the first lateral state, and the first heading angle state determine the second lateral state, the second heading angle state, and the third lateral weight corresponding to the cluster;
  • the second processing unit 5063 is used to The second lateral state and the second heading angle state of the cluster with the third largest lateral weight are respectively used as the target lateral state and the target heading angle state;
  • the first acquisition unit 5064 is used to acquire the target determined based on the first histogram filter Longitudinal state;
  • the third processing unit 5065 is configured to determine the current positioning posture of the movable device based on the target longitudinal state, the target lateral state, the target heading angle state, and the first predicted posture of the movable device at the current moment.
  • FIG. 14 is a schematic structural diagram of the first processing module 502 provided by an exemplary embodiment of the present disclosure.
  • the first processing module 502 includes: a fourth determination unit 5021a and a fifth determination unit 5022a.
  • the fourth determination unit 5021a is used to determine the first predicted particle pose corresponding to each first particle based on each first particle pose; the fifth determination unit 5022a is used to determine each first predicted particle pose based on each first predicted particle pose.
  • the particles respectively correspond to the first lateral state and the first heading angle state.
  • the fifth determination unit 5022a is specifically configured to: determine the first predicted pose of the movable device at the current moment based on the first positioning pose of the movable device determined at the previous moment; based on each first prediction The particle pose and the first predicted pose determine the first lateral state and the first heading angle state corresponding to each first particle.
  • FIG. 15 is a schematic structural diagram of the first processing module 502 provided by another exemplary embodiment of the present disclosure.
  • the first processing module 502 includes: a fourth processing unit 5021b, a fifth processing unit 5022b, a sixth processing unit 5023b, a seventh processing unit 5024b and an eighth processing unit 5025b.
  • the fourth processing unit 5021b is used for generating, for each first particle pose, second predicted particle poses corresponding to n second particles based on the first particle pose; and the fifth processing unit 5022b is used for generating n second predicted particle poses based on the first particle pose.
  • the second predicted particle poses corresponding to the two particles respectively determine the third lateral state and the third heading angle state corresponding to each second particle;
  • the sixth processing unit 5023b is used to determine the corresponding third lateral state and the third heading angle state respectively based on the m*n second particles.
  • the directional state and the fourth heading angle state are respectively the first lateral state and the first heading angle state corresponding to the current moment of each first particle.
  • the fourth processing unit 5021b is specifically configured to: generate second predicted particle positions corresponding to n second particles based on the first particle pose, odometry information, and n different Gaussian white noises. posture.
  • the fifth processing unit 5022b is specifically configured to: determine the first predicted pose of the movable device at the current moment based on the first positioning pose of the movable device determined at the previous moment; based on each second particle The corresponding second predicted particle pose and the first predicted pose respectively determine the third lateral state and the third heading angle state corresponding to each second particle.
  • Figure 16 is a schematic structural diagram of an application embodiment of the electronic device of the present disclosure.
  • the electronic device 10 includes one or more processors 11 and memories 12 .
  • the processor 11 may be a central processing unit (CPU) or other form of processing unit with data processing capabilities and/or instruction execution capabilities, and may control other components in the electronic device 10 to perform desired functions.
  • CPU central processing unit
  • the processor 11 may control other components in the electronic device 10 to perform desired functions.
  • Memory 12 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and/or non-volatile memory.
  • the volatile memory may include, for example, random access memory (RAM) and/or cache memory (cache).
  • the non-volatile memory may include, for example, 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 the processor 11 may execute the program instructions to implement the methods of various embodiments of the present disclosure described above and/or other desired Function.
  • Various contents such as input signals, signal components, noise components, etc. may also be stored in the computer-readable storage medium.
  • the electronic device 10 may further include an input device 13 and an output device 14, and these components are interconnected through a bus system and/or other forms of connection mechanisms (not shown).
  • the input device 13 may also include, for example, a keyboard, a mouse, and the like.
  • the output device 14 can output various information to the outside.
  • the output device 14 may include, for example, a display, a speaker, a printer, a communication network and remote output devices connected thereto, and the like.
  • the electronic device 10 may also include any other appropriate components depending on the specific application.
  • embodiments of the present disclosure may also be a computer program product, which includes computer program instructions that, when executed by a processor, cause the processor to perform the “exemplary method” described above in this specification
  • the steps in methods according to various embodiments of the present disclosure are described in Sec.
  • the computer program product may have program code for performing operations of embodiments of the present disclosure written in any combination of one or more programming languages, including object-oriented programming languages such as Java, C++, etc., and Includes conventional procedural programming languages, such as the "C" language or similar programming languages.
  • the program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device, or entirely on the remote computing device or server execute on.
  • embodiments of the present disclosure may also be a computer-readable storage medium having computer program instructions stored thereon.
  • the computer program instructions when executed by a processor, cause the processor to execute the above-mentioned “example method” part of this specification. The steps in methods according to various embodiments of the present disclosure are described in .
  • Computer-readable storage media can take the form of any combination of one or more computer-readable media.
  • the readable medium may be a readable signal medium or a readable storage medium.
  • the readable storage medium may include, for example, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices or devices, or any combination thereof. More specific examples (non-exhaustive list) of readable storage media include: electrical connection with one or more conductors, portable disk, hard disk, random access memory (RAM), read only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination of the above.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Analysis (AREA)

Abstract

一种位姿的确定方法、装置、电子设备和存储介质，其中，方法包括：基于可移动设备对应的m个第一粒子的第一粒子位姿，确定各第一粒子当前时刻分别对应的第一横向状态和第一航向角状态（202）；基于感知车道线信息，确定各第一粒子分别对应的第一匹配得分和第二匹配得分（203）；基于各第一匹配得分，分别对其对应的第一粒子的第一横向权重进行更新，获得第二横向权重（204）；基于各第二匹配得分，分别对其对应的第一粒子的第一航向角权重进行更新，获得第二航向角权重（205）；基于各第二横向权重、各第二航向角权重、各第一横向状态、各第一航向角状态，确定可移动设备的当前定位位姿（206）。可以实现横向定位精度和航向角定位精度的兼顾，有助于提高定位性能。

Description

位姿的确定方法、装置、电子设备和存储介质
本公开要求在2022年08月24日提交国家知识产权局、申请号为CN202211028797.7、发明名称为“位姿的确定方法、装置、电子设备和存储介质”的中国专利申请的优先权，其全部内容通过引用结合在本公开中。
技术领域
本公开涉及定位技术，尤其是一种位姿的确定方法、装置、电子设备和存储介质。
背景技术
基于视觉感知和高精地图的车载定位系统，通过系统状态估计技术实现车端的实时高精定位。其中，基于粒子滤波的系统状态估计技术因其计算代价小、稳定性高等特点，成为车载定位系统中常见的状态估计方法。在滤波器中，系统的状态设定和状态的更新观测方式的不同直接影响滤波器的估计结果，即车辆定位的性能。
发明内容
本公开的实施例提供了一种位姿的确定方法、装置、电子设备和存储介质。
根据本公开实施例的一个方面，提供了一种位姿的确定方法，包括：确定可移动设备对应的m个第一粒子的第一粒子位姿，m为大于1的整数，所述第一粒子位姿是在前时刻获得的对应第一粒子的位姿；基于各所述第一粒子位姿，确定各所述第一粒子当前时刻分别对应的第一横向状态和第一航向角状态；基于感知车道线信息，确定各所述第一粒子分别对应的第一匹配得分和第二匹配得分；基于各所述第一匹配得分，分别对其对应的所述第一粒子的第一横向权重进行更新，获得各所述第一粒子分别对应的第二横向权重；基于各所述第二匹配得分，分别对其对应的所述第一粒子的第一航向角权重进行更新，获得各所述第一粒子分别对应的第二航向角权重；基于各所述第一粒子分别对应的所述第二横向权重、所述第二航向角权重、所述第一横向状态、所述第一航向角状态，确定所述可移动设备的当前定位位姿。
根据本公开实施例的另一个方面，提供了一种位姿的确定装置，包括：第一确定模块，用于确定可移动设备对应的m个第一粒子的第一粒子位姿，m为大于1的整数，所述第一粒子位姿是在前时刻获得的对应第一粒子的位姿；第一处理模块，用于基于各所述第一粒子位姿，确定各所述第一粒子当前时刻分别对应的第一横向状态和第一航向角状态；第二处理模块，用于基于感知车道线信息，确定各所述第一粒子分别对应的第一匹配得分和第二匹配得分；第三处理模块，用于基于各所述第一匹配得分，分别对其对应的所述第一粒子的第一横向权重进行更新，获得各所述第一粒子分别对应的第二横向权重；第四处理模块，用于基于各所述第二匹配得分，分别对其对应的所述第一粒子的第一航向角权重进行更新，获得各所述第一粒子分别对应的第二航向角权重；第五处理模块，用于基于各所述第一粒子分别对应的所述第二横向权重、所述第二航向角权重、所述第一横向状态、所述第一航向角状态，确定所述可移动设备的当前定位位姿。
根据本公开实施例的再一方面，提供一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序用于执行本公开上述任一实施例所述的位姿的确定方法。
根据本公开实施例的又一方面，提供一种电子设备，所述电子设备包括：处理器；用于存储所述处理器可执行指令的存储器；所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述指令以实现本公开上述任一实施例所述的位姿的确定方法。
根据本公开实施例的又一方面，提供一种计算机程序产品，当所述计算机程序产品中的指令处理器执行时，执行本公开上述任一实施例所述的位姿的确定方法。
基于本公开上述实施例提供的位姿的确定方法、装置、电子设备和存储介质，通过将横向状态和航向角状态分离，在定位过程中，粒子的横向状态和航向角状态分别采用不同的权重，从而实现横向定位精度和航向角定位精度的兼顾，有效提高定位性能。
下面通过附图和实施例，对本公开的技术方案做进一步的详细描述。
附图说明
图1是本公开提供的位姿的确定方法的一个示例性的应用场景；
图2是本公开一示例性实施例提供的位姿的确定方法的流程示意图；
图3是本公开一个示例性实施例提供的位姿的确定方法的流程示意图；
图4是本公开一示例性实施例提供的步骤203的流程示意图；
图5是本公开一个示例性实施例提供的步骤2022a的流程示意图；
图6是本公开一个示例性实施例提供的步骤202的流程示意图；
图7是本公开一示例性实施例提供的第一网格坐标区域的示意图；
图8是本公开一示例性实施例提供的第二粒子映射流程示意图；
图9是本公开另一示例性实施例提供的步骤202的流程示意图；
图10是本公开再一示例性实施例提供的位姿的确定方法的流程示意图；
图11是本公开一示例性实施例提供的位姿的确定装置的结构示意图；
图12是本公开一示例性实施例提供的第二处理模块503的结构示意图；
图13是本公开一示例性实施例提供的第五处理模块506的结构示意图；
图14是本公开一示例性实施例提供的第一处理模块502的结构示意图；
图15是本公开另一示例性实施例提供的第一处理模块502的结构示意图；
图16是本公开电子设备一个应用实施例的结构示意图。
具体实施方式
为了解释本公开，下面将参考附图详细地描述本公开的示例实施例，显然，所描述的实施例仅是本公开的一部分实施例，而不是全部实施例，应理解，本公开不受示例性实施例的限制。
应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本公开的范围。
本公开概述
在实现本公开的过程中，发明人发现，基于视觉感知和高精地图的车载定位系统，通过系统状态估计技术实现车端的实时高精定位。其中，基于粒子滤波的系统状态估计技术因其计算代价小、稳定性高等特点，成为车载定位系统中常见的状态估计方法。在滤波器中，系统的状态设定和状态的更新观测方式的不同直接影响滤波器的估计结果，即影响车辆定位的性能。例如，若在进行状态估计时，设置车辆的位置和姿态作为系统的状态进行实时的预测和更新，可能使得横向定位和航向角定位中的一者精度较高而另一者精度较低，导致车辆定位的性能较差。
示例性概述
图1是本公开提供的位姿的确定方法的一个示例性的应用场景。
针对可移动设备的高精度定位场景，以车辆为例，在需要对车辆进行高精度定位时，可以首先确定车辆在导航地图中的初始位姿，进而基于初始位姿在车辆后续运动中进行高精度定位，在运动过程中，利用本公开实施例提供的位姿的确定方法，可以在基于粒子滤波的定位过程中，为横向状态和航向角状态分别确定不同的权重，以兼顾横向定位精度和航向角定位精度，从而有助于提高位姿的整体定位精度。图1中，x、y分别表示车辆自坐标系的x轴和y轴，y方向为横向，x方向为纵向。在对车辆进行定位时，可以结合车道线的观测结果，车道线的观测结果是指通过摄像头采集环境图像识别的车道线(即感知车道线)与地图中的车道线的匹配结果。本公开基于感知车道线，确定粒子滤波的滤波器中分别用于更新各粒子的横向权重和航向角权重的第一匹配得分和第二匹配得分，从而可以采用不同的权重分别对横向状态和航向角状态进行更新，进而基于更新后的状态确定可移动设备的位姿，实现横向定位和航向角定位均可以具有较高精度，有助于提高整体定位性能。本公开实施例中，位姿可以包括横向坐标分量Y(lon)、纵向坐标分量X(lat)和航向角θ(yaw)三个自由度中的至少一者。
示例性方法
图2是本公开一示例性实施例提供的位姿的确定方法的流程示意图。本实施例可应用在电子设备上，具体比如车载计算平台上，如图2所示，包括如下步骤：
步骤201，确定可移动设备对应的m个第一粒子的第一粒子位姿，m为大于1的整数，第一粒子位姿是在前时刻获得的对应第一粒子的位姿。
其中，可移动设备可以是车辆、机器人等设备，具体不做限定。第一粒子的第一粒子位姿是于在前时刻的定位过程中所确定的粒子滤波器中的粒子的位姿，具体第一粒子数量m可以根据实际需求设置，本公开实施例不做限定。在前时刻可以是当前时刻之前的任一时刻，比如当前时刻的前一时刻，或者与当前时刻间隔一定时间的某一时刻，具体可以根据实际需求确定。
在一个可选示例中，该步骤201可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第一确定模块执行。
步骤202，基于各第一粒子位姿，确定各第一粒子当前时刻分别对应的第一横向状态和第一航向角状态。
其中，第一粒子当前时刻的第一横向状态和第一航向角状态是第一粒子当前时刻的预测粒子位姿相对于可移动设备当前时刻的预测位姿的横向状态和航向角状态。第一粒子当前时刻的预测粒子位姿可以采用任意可实施的预测方式获得。同理可移动设备当前时刻的预测位姿也可以采用任意可实施的预测方式获得。比如基于第一粒子的第一粒子位姿及可移动设备的里程计信息，采用相应的运动模型进行预测。运动模型可以根据实际需求设置，比如基于里程计的运动模型(Odometry Sample Motion Model)或其他可实施的运动模型，本公开实施例不做限定。
在一个可选示例中，该步骤202可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第一处理模块执行。
步骤203，基于感知车道线信息，确定各第一粒子分别对应的第一匹配得分和第二匹配得分。
其中，感知车道线信息是感知阶段基于采集的环境图像，确定的车道线信息，感知车道线信息可以包括至少一个车道线对应的感知车道线参数，比如c0、c1、c2、c3，c0、c1、c2、c3表示拟合的车道线的曲线方程参数化的三次方程的系数。具体感知车道线信息可以根据实际需求设置。第一匹配得分和第二匹配得分是针对横向状态和航向角状态分别确定的匹配得分，第一匹配得分对应横向状态，第二匹配得分对应航向角状态。对于第一粒子对应的第一匹配得分和第二匹配得分，可以设置对应的规则，比如对于第一匹配得分的确定，可以设置第一预设规则，对于第二匹配得分的确定可以设置第二预设规则。第一预设规则和第二预设规则可以根据实际需求设置，第一预设规则和第二预设规则设置的原则是相对于可移动设备来说，使得在可移动设备近距离处的感知车道线采样点的匹配得分在第一匹配得分中贡献较大，中远距离处的采样点得分在第一匹配得分中贡献较小，以使横向定位精度较高；在可移动设备中远距离处的感知车道线采样点的匹配得分在第二匹配得分中贡献多，近距离处采样点的匹配得分在第二匹配得分中贡献较小，以使航向角定位精度较高。
若横向状态和航向角状态采用相同的权重更新，该权重基于车道线的总匹配得分来确定，由于视觉感知获取的车道 线和地图中的车道线的几何形状存在一定的差异，使得感知车道线匹配地图车道线的匹配得分对粒子状态进行更新时具有如下特性：当近距离处的感知车道线采样点的匹配得分在总匹配得分中贡献较多时横向定位精度较高，当中远距离处的感知车道线采样点的匹配得分在总匹配得分中贡献较多时航向角定位精度较高，这样的博弈导致车辆的横向(lat)定位精度和航向角(yaw)定位精度难以兼顾。本公开实施例通过不同匹配得分分别对横向状态和航向角状态进行更新，从而能够兼顾横向定位精度和航向角定位精度，使得横向定位和航向角定位均可以具有较高的精度，从而有助于提高整体定位性能。
在一个可选示例中，该步骤203可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第二处理模块执行。
步骤204，基于各第一匹配得分，分别对其对应的第一粒子的第一横向权重进行更新，获得各第一粒子分别对应的第二横向权重。
其中，第一粒子的第一横向权重是第一横向状态对应的权重，第一横向权重可以是在在前时刻的定位过程中确定并存储的。基于第一匹配得分对第一横向权重进行更新是指基于当前时刻的匹配情况调整第一横向状态的权重，具体更新规则可以根据实际需求设置。比如当匹配效果较好时，可以增大该第一粒子的第一横向权重，获得对应的更新后的第二横向权重。当匹配效果较差时，可以减小该第一粒子的第一横向权重，具体原理不再赘述。
在一个可选示例中，该步骤204可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第三处理模块执行。
步骤205，基于各第二匹配得分，分别对其对应的第一粒子的第一航向角权重进行更新，获得各第一粒子分别对应的第二航向角权重。
其中，第一粒子的第一航向角权重是第一航向角状态对应的权重，第一航向角权重也是于在前时刻的定位过程中确定并存储的。第一航向角权重的更新原理与上述第一横向权重类似，在此不再赘述。
步骤204与步骤205不分先后顺序。
在一个可选示例中，该步骤205可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第四处理模块执行。
步骤206，基于各第一粒子分别对应的第二横向权重、第二航向角权重、第一横向状态、第一航向角状态，确定可移动设备的当前定位位姿。
其中，由于各第一粒子是模拟可移动设备的位姿，在确定了各第一粒子分别对应的第二横向权重、第二航向角权重、以及各第一粒子当前时刻的第一横向状态和第一航向角状态后，可以基于各第一粒子分别对应的第二横向权重、第二航向角权重、第一横向状态、第一航向角状态，确定可移动设备的当前定位位姿。由于位姿可以包括横向、纵向和航向角三个分量，对于各第一粒子分别对应的当前时刻的第一纵向状态及对应的权重，可以基于任何可实施的粒子滤波实现，比如基于直方图滤波器实现，本公开实施例不做限定。
在一个可选示例中，该步骤206可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第五处理模块执行。
本实施例提供的位姿的确定方法，通过在定位过程中，针对每个粒子的横向状态和航向角状态，分别确定不同的匹配得分，分别用于横向权重和航向角权重的更新，使得粒子的横向状态和航向角状态分别具有不同的权重，将横向状态和航向角状态分离，从而实现横向定位精度和航向角定位精度的兼顾，有助于提高定位性能。
图3是本公开一个示例性实施例提供的位姿的确定方法的流程示意图。
在一个可选示例中，步骤203的基于感知车道线信息，确定各第一粒子分别对应的第一匹配得分和第二匹配得分，具体可以包括以下步骤：
步骤2031，基于感知车道线信息，确定车辆坐标系下的车道线采样点。
其中，感知车道线信息是感知阶段基于采集的环境图像所确定的车道线信息，感知车道线信息可以包括感知车道线参数，比如c0、c1、c2、c3，c0、c1、c2、c3表示拟合的车道线的曲线方程参数化的三次方程的系数。具体感知车道线信息可以根据实际需求设置。车辆坐标系是以车辆后轴中心为原点的车辆自坐标系，具体不再赘述。车辆坐标系下的车道线采样点是基于感知车道线信息，在车辆坐标系下对感知车道线进行采样获得。具体采样方式可以根据实际需求设置，比如在图像坐标系(以图像左上角为原点，横向向右为u轴，纵向向下为v轴)下，以感知车道线信息对应的感知图像的中心为起点，沿v方向按照预设采样间隔进行采样，获得N个原采样点，将N个原采样点通过相机的内参、外参及图像坐标系与车辆坐标系的映射关系，分别投射到车辆坐标系的x轴，获得N个x轴坐标，其中，第i个x轴坐标表示为进而可以基于感知车道线信息确定N个原采样点分别对应的车辆坐标系下的车道线采样点，第i(i＝1,2,…,N)个车道线采样点表示为：
其中，t表示当前时刻，c0、c1、c2、c3表示感知车道线参数，也即上述的车道线的曲线方程参数化的三次方程的系数。
在一个可选示例中，该步骤2031可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第一确定单元执行。
步骤2032，分别将各第一粒子作为可移动设备，将第一地图车道线转换到车辆坐标系下，获得各第一粒子分别对应的车辆坐标系下的第二地图车道线。
其中，第一地图车道线是高精地图中的车道线，第一地图车道线可以包括至少一条车道线的相关信息，具体可以根据实际需求设置。比如可以根据车辆在前时刻的定位位姿，获取该车辆定位位姿周围一定范围的局部地图，并获取该局部地图中的车道线，作为第一地图车道线。将第一粒子作为可移动设备是指认为该第一粒子就是可移动设备，从而将该第一粒子的当前时刻的预测粒子位姿(可称为第一预测粒子位姿)作为可移动设备的位姿，基于该第一粒子的第一预测 粒子位姿建立可移动设备的车辆坐标系。具体来说，将第一预测粒子位姿中的位置分量(X,Y)作为可移动设备的车辆坐标系原点，根据第一预测粒子位姿中的航向角(姿态分量)确定车辆坐标系的x轴和y轴方向。根据预先获得的车辆坐标系与地图坐标系的映射关系，将第一地图车道线转换到车辆坐标系下，获得各第一粒子分别对应的车辆坐标系下的第二地图车道线。
在一个可选示例中，该步骤2032可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第一转换单元执行。
步骤2033，基于车道线采样点、各第一粒子分别对应的第二地图车道线及第一预设规则，确定各第一粒子分别对应的第一匹配得分。
其中，第一预设规则是横向状态对应的车道线采样点的匹配得分确定规则，可以根据实际需求设置，第一预设规则的设置原则是使得近距离处的车道线采样点匹配得分相对较高，中远距离的车道线采样点匹配得分相对较低，以提高横向定位精度。
对于每个第一粒子，可以基于每个车道线采样点和第二地图车道线确定该车道线采样点与第二地图车道线的最小横向距离，进而基于最小横向距离、预设最大横向距离阈值及上述的第一预设规则，确定该车道线采样点的第一采样点得分，基于各车道线采样点分别对应的第一采样点得分，确定该第一粒子对应的第一匹配得分。其中，车道线采样点与第二地图车道线的最小横向距离可以是指该车道线采样点与第二地图车道线中最近的一条车道线的横向距离。该第一粒子对应的第一匹配得分可以是各车道线采样点的第一采样点得分的总和，具体可以根据实际需求设置。
在一个可选示例中，该步骤2033可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第二确定单元执行。
步骤2034，基于车道线采样点、各第一粒子分别对应的第二地图车道线及第二预设规则，确定各第一粒子分别对应的第二匹配得分。
其中，第二预设规则是航向角状态对应的车道线采样点的匹配得分确定规则，可以根据实际需求设置，第二预设规则的设置原则是使得近距离处的车道线采样点匹配得分相对较低，中远距离的车道线采样点匹配得分相对较高，以提高航向角定位精度。
对于每个第一粒子，可以基于每个车道线采样点和第二地图车道线确定该车道线采样点与第二地图车道线的最小横向距离，进而基于最小横向距离、预设最大横向距离阈值及上述的第二预设规则，确定该车道线采样点的第二采样点得分，基于各车道线采样点分别对应的第二采样点得分，确定该第一粒子对应的第二匹配得分。其中，车道线采样点与第二地图车道线的最小横向距离可以是指该车道线采样点与第二地图车道线中最近的一条车道线的横向距离。该第一粒子对应的第二匹配得分可以是各车道线采样点的第二采样点得分的总和，具体可以根据实际需求设置。
在一个可选示例中，该步骤2034可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第三确定单元执行。
图4是本公开一示例性实施例提供的步骤203的流程示意图。
在一个可选示例中，步骤2033的基于车道线采样点、各第一粒子分别对应的第二地图车道线及第一预设规则，确定各第一粒子分别对应的第一匹配得分，包括：
步骤20331，对于每个第一粒子，基于各车道线采样点和第二地图车道线，确定各车道线采样点分别与第二地图车道线的最小横向距离。
其中，车道线采样点与第二地图车道线的最小横向距离是指该车道线采样点与第二地图车道线中最近的一条车道线的横向距离。
在一个可选示例中，该步骤20331可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第二确定单元执行。
步骤20332，基于各最小横向距离、预设最大横向距离阈值及第一预设规则，确定各车道线采样点分别对应的第一采样点得分。
其中，预设最大横向距离阈值可以根据实际需求设置，本公开实施例不做限定。第一采样点得分是指针对横向状态的在该车道线采样点处的匹配得分。
在一个可选示例中，该步骤20332可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第二确定单元执行。
步骤20333，基于各车道线采样点分别对应的第一采样点得分，确定各车道线采样点的第一采样点总得分，作为第一粒子对应的第一匹配得分。
示例性的，第一预设规则可以设置为：
其中，x表示车辆坐标系下的x轴坐标，F(x)可以称为第一匹配得分权重，可见当车道线采样点距离车辆坐标系原点较近时，第一匹配得分权重较大，当车道线采样点距离车辆坐标系原点较远时，第一匹配得分权重较小。
第i个车道线采样点的第一匹配得分权重为：
基于该第一匹配得分权重，可以确定第i个车道线采样点的第一采样点得分。
对于每个第一粒子，可以基于每个车道线采样点和第二地图车道线确定该车道线采样点与第二地图车道线的最小横向距离，进而基于最小横向距离、预设最大横向距离阈值及上述的第一匹配得分权重，确定该车道线采样点的第一采样 点得分，基于各车道线采样点分别对应的第一采样点得分，确定该第一粒子对应的第一匹配得分。该第一粒子对应的第一匹配得分可以是各车道线采样点的第一采样点得分的总和，具体可以根据实际需求设置。
示例性的，第i个车道线采样点的第一采样点得分可以表示为
其中，g表示预设最大横向距离阈值，t表示当前时刻，表示第i个车道线采样点与第二地图车道线的最小横向距离，l(l＝1,2,…,m)表示第l个第一粒子，m表示第一粒子的数量，m为正整数。
对于第l个第一粒子，N个车道线采样点的第一采样点得分总和可以得到该第一粒子对应的第一匹配得分
在一个可选示例中，该步骤20333可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第二确定单元执行。
在一个可选示例中，步骤2034的基于车道线采样点、各第一粒子分别对应的第二地图车道线及第二预设规则，确定各第一粒子分别对应的第二匹配得分，包括：
步骤20341，对于每个第一粒子，基于各车道线采样点和第二地图车道线，确定各车道线采样点分别与第二地图车道线的最小横向距离。
该步骤与上述步骤20331一致，在此不再赘述。在实际应用中，该步骤与上述步骤20331可以为同一步骤。
在一个可选示例中，该步骤20341可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第三确定单元执行。
步骤20342，基于各最小横向距离、预设最大横向距离阈值及第二预设规则，确定各车道线采样点分别对应的第二采样点得分。
其中，第二采样点得分是指针对航向角状态的在该车道线采样点处的匹配得分。
在一个可选示例中，该步骤20342可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第三确定单元执行。
步骤20343，基于各车道线采样点分别对应的第二采样点得分，确定各车道线采样点的第二采样点总得分，作为第一粒子对应的第二匹配得分。
示例性的，第二预设规则可以设置为：
G(x)＝x0.3
其中，x表示车辆坐标系下的x轴坐标，G(x)可以称为第二匹配得分权重，可见当车道线采样点距离车辆坐标系原点较近时，第二匹配得分权重较大，当车道线采样点距离车辆坐标系原点较远时，第二匹配得分权重较小。
第i个车道线采样点的第二匹配得分权重为：
基于该第二匹配得分权重，可以确定第i个车道线采样点的第二采样点得分。
对于每个第一粒子，可以基于每个车道线采样点和第二地图车道线确定该车道线采样点与第二地图车道线的最小横向距离，进而基于最小横向距离、预设最大横向距离阈值及上述的第二匹配得分权重，确定该车道线采样点的第二采样点得分，基于各车道线采样点分别对应的第二采样点得分，确定该第一粒子对应的第二匹配得分。其中车道线采样点与第二地图车道线的最小横向距离是指该车道线采样点与第二地图车道线中最近的一条车道线的横向距离。该第一粒子对应的第二匹配得分可以是各车道线采样点的第二采样点得分的总和，具体可以根据实际需求设置。
示例性的，第i个车道线采样点的第二采样点得分可以表示为
其中，g表示预设最大横向距离阈值，t表示当前时刻，表示第i个车道线采样点与第二地图车道线的最小横向距离，l(l＝1,2,…,m)表示第l个第一粒子，m表示第一粒子的数量，m为正整数。
对于第l个第一粒子，N个车道线采样点的第二采样点得分总和可以得到该第一粒子对应的第二匹配得分
第l个第一粒子对应第一横向权重表示为第一航向角权重表示为具体如下：

其中，表示在前时刻的定位流程确定的该第一粒子的权重。
在获得第l个第一粒子对应的第一匹配得分和第二匹配得分后，可以对该第一粒子的第一横向权重和第一航向角权重进行更新，获得该第一粒子对应的第二横向权重和第二航向角权重如下：

其中，MG表示GNSS传感器的观测得分，表示如下：
其中，GNSSyaw表示GNSS传感器输出的航向角，表示第l个第一粒子对应第一航向角状态。0.001与10是预设值，可以根据实际需求调整。
在一个可选示例中，该步骤20343可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第三确定单元执行。
本公开实施例通过对粒子横向权重和航向角权重分别采用不同的匹配得分进行更新，使得在可移动设备的横向权重在近距离处车道线采样点的匹配得分贡献较大，航向角权重在中远距离处车道线采样点的匹配得分贡献较大，从而使得横向定位和航向角定位均可以具有较高的精度。
在一个可选示例中，步骤206的基于各第一粒子分别对应的第二横向权重、第二航向角权重、第一横向状态、第一航向角状态，确定可移动设备的当前定位位姿，包括：
步骤2061，对各第一粒子进行聚类，获得第一数量的聚簇。
其中，聚类可以采用任意可实施的聚类算法，比如采用K-means聚类算法进行聚类，具体不做限定。第一数量可以根据实际需求设置，或者根据具体的聚类算法确定，具体不做限定。每个聚簇中包括至少一个第一粒子。
在一个可选示例中，该步骤2061可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的聚类单元执行。
步骤2062，针对每个聚簇，基于聚簇中的第一粒子对应的第二横向权重、第二航向角权重、第一横向状态、第一航向角状态，确定聚簇对应的第二横向状态、第二航向角状态和第三横向权重。
其中，聚簇对应的第二横向状态可以通过该聚簇中的各第一粒子的第一横向状态按照第二横向权重加权平均获得。同理，聚簇对应的第二航向角状态可以通过该聚簇中的各第一粒子的第二航向角状态按照第二航向角权重加权平均获得。聚簇对应的第三横向权重可以是该聚簇中的各第一粒子的第二横向权重的和。
示例性的，在第s(s＝1,2,…,S；S表示聚簇数量)个聚簇中包括F个第一粒子，其中，第f(f＝1,2,…,F)个第一粒子的第一横向状态为第一航向角状态为第二横向权重为第二航向角权重为则该聚簇对应的第二横向状态第二航向角状态和第三横向权重分别如下：


其中，聚簇中每个第一粒子的第一横向状态、第一航向角状态、第二横向权重第二航向角权重分别与前述获得的对应第一粒子一致，这里只是针对聚簇进行了不同的符号表示，比如，前述的第l个第一粒子对应该第s个聚簇中的第f个第一粒子，则第s个聚簇中的第f个第一粒子的第二横向权重第一横向状态、第一航向角、第二航向角权重同理，在此不再一一赘述。
在一个可选示例中，该步骤2062可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第一处理单元执行。
步骤2063，将第三横向权重最大的聚簇的第二横向状态和第二航向角状态分别作为目标横向状态和目标航向角状态。
其中，对于各聚簇来说，若其中某一个聚簇的第三横向权重最大表示该聚簇的第一粒子可能更接近可移动设备的真实位姿，因此，可以将该聚簇的第二横向状态和第二航向角状态作为目标横向状态和目标航向角状态，用于可移动设备位姿的确定，有助于提高可移动设备位姿的定位准确性。
示例性的，若上述第s(比如s＝3)个聚簇的第三横向权重最大，则将其第二横向状态作为目标横向状态 将其第二航向角状态作为目标航向角状态
在一个可选示例中，该步骤2063可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第二处理单元执行。
步骤2064，获取基于第一直方图滤波器确定的目标纵向状态。
其中，第一直方图滤波器可以采用任意可实施的方式，本公开实施例不做限定。本公开实施例可以只采用第一直方图滤波器确定的纵向状态，对其横向状态和航向角状态可以不予考虑。
在一个可选示例中，该步骤2064可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第一获取单元执行。
步骤2065，基于目标纵向状态、目标横向状态、目标航向角状态及可移动设备当前时刻的第一预测位姿，确定可移动设备的当前定位位姿。
其中，可移动设备的当前时刻的第一预测位姿可以基于可移动设备在前时刻的定位位姿、可移动设备的里程计信息及运动模型确定。具体预测原理不再赘述。
示例性的，当前时刻的目标横向状态表示为目标航向角状态为目标纵向状态表示为可移动设备当前时刻的第一预测位姿表示为则可移动设备的当前定位位姿为
其中，T表示转置运算，表示SE2位姿的加法，具体定义如下：
定义
c[0]＝a[0]+b[0]*cos(a[2])-b[1]*sin(a[2])
c[1]＝a[1]+b[0]*sin(a[2])+b[1]*cos(a[2])
c[2]＝a[2]+b[2]
在一个可选示例中，该步骤2065可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第三处理单元执行。
在一个可选示例中，步骤202的基于各第一粒子位姿，确定各第一粒子当前时刻分别对应的第一横向状态和第一航向角状态，包括：
步骤2021a，基于各第一粒子位姿，确定各第一粒子分别对应的第一预测粒子位姿。
其中，第一预测粒子位姿是基于在前时刻到当前时刻的时间段内可移动设备的移动情况(比如里程计信息)对第一粒子当前时刻的位姿进行预测获得，具体预测方式可以采用任意可实施的方式，比如基于运动模型进行预测。
示例性的，对于车辆，可以基于车辆底盘CAN信息计算得到的里程计相对增量ΔOdom预测第一粒子当前时刻的第一预测粒子位姿：
其中，t表示当前时刻，t-1表示在前时刻，l(l＝1,2,…,m)表示第l个第一粒子，m表示第一粒子的数量，m为正整数，表示第l个第一粒子的第一粒子位姿，表示第l个第一粒子的第一预测粒子位姿，ΔOdom表示里程计相对增量，表示SE2位姿的加法，定义参见前述内容，Wl表示第l个第一粒子对应的高斯白噪声，不同第一粒子高斯白噪声可以不同，具体可以根据实际需求设置。
在一个可选示例中，该步骤2021a可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第四确定单元执行。
步骤2022a，基于各第一预测粒子位姿，确定各第一粒子分别对应的第一横向状态和第一航向角状态。
其中，第一粒子当前时刻的第一横向状态和第一航向角状态是第一粒子当前时刻的预测粒子位姿相对于可移动设备当前时刻的预测位姿的横向状态和航向角状态。因此，可以基于各第一预测粒子位姿及可移动设备的第一定位位姿来确定各第一粒子分别对应的第一横向状态和第一航向角状态。
在一个可选示例中，该步骤2022a可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第五确定单元执行。
在一个可选示例中，图5是本公开一个示例性实施例提供的步骤2022a的流程示意图。在本示例中，步骤2022a的基于各第一预测粒子位姿，确定各第一粒子分别对应的第一横向状态和第一航向角状态，包括：
步骤2022a1，基于在前时刻确定的可移动设备的第一定位位姿，确定可移动设备当前时刻的第一预测位姿。
其中，第一定位位姿是在前时刻(t-1)的定位流程中确定的可移动设备的定位位姿，可以表示为可移动设备当前时刻的第一预测位姿可以基于第一定位位姿和里程计相对增量ΔOdom确定，表示如下：
其中，表示SE2位姿的加法，定义如上，不再赘述。
在一个可选示例中，该步骤2022a1可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第五确定单元执行。
步骤2022a2，基于各第一预测粒子位姿和第一预测位姿，确定各第一粒子分别对应的第一横向状态和第一航向角状态。
示例性的，第l个第一粒子的第一横向状态表示为第一航向角状态表示为则：

其中，t表示当前时刻，表示第l个第一粒子的第一预测粒子位姿，表示可移动设备的第一预测位姿，是运算符，定义如下：

c[0]＝(a[0]-b[0])*cos(b[2])+(a[1]-b[1])*sin(b[2])
c[1]＝(a[1]-b[1])*cos(b[2])-(a[1]-b[1])*sin(b[2])
c[2]＝a[2]-b[2]
表示取c[1]，表示取c[2]，将作为a，将作为b，按照上述运算，获得c[1]作为第一横向状态c[2]作为第一航向角状态
在一个可选示例中，该步骤2022a2可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第五确定单元执行。
在一个可选示例中，图6是本公开一个示例性实施例提供的步骤202的流程示意图。在本示例中，步骤202的基于各第一粒子位姿，确定各第一粒子当前时刻分别对应的第一横向状态和第一航向角状态，包括：
步骤2021b，针对每个第一粒子位姿，基于第一粒子位姿生成n个第二粒子分别对应的第二预测粒子位姿。
其中，n个第二粒子可以基于里程计信息和n个不同的高斯白噪声生成。
示例性的，对于第l个第一粒子，基于该第一粒子的第一粒子位姿生成n个第二粒子分别对应的第二预测粒子位姿表示如下：
其中，j表示第j个第二粒子，ΔOdom表示可移动设备里程计相对增量，Wlj表示第j个第二粒子对应的高斯白噪声。
在一个可选示例中，该步骤2021b可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第四处理单元执行。
步骤2022b，基于各第二粒子分别对应的第二预测粒子位姿，确定各第二粒子分别对应的第三横向状态和第三航向角状态。
其中，对于每个第二粒子，第三横向状态和第三航向角状态的确定原理与前述第一粒子的第一横向状态和第一航向角状态类似，在此不再赘述。
示例性的，以第l个第一粒子为例，对于第j个第二粒子，其第三横向状态和第三航向角状态表示如下：

其中，定义如上，在此不再赘述。
第l个第一粒子对应的每个第二粒子的权重(包括横向权重和纵向权重)设置为与该第l个第一粒子的第一横向权重和第一航向角权重相同。第一粒子的第一横向权重和第一航向角权重是在在前时刻确定并存储的，第l个第一粒子的第一横向权重表示为在当前时刻未更新权重前，第一航向角权重与第一横向权重相同也为即在每个定位流程完成后将确定的第一粒子的横向状态的权重作为第一粒子下一时刻的第一横向权重和第一航向角权重，在当前时刻，将在前时刻确定的第一粒子的横向状态的权重作为第一横向权重和第一航向角权重。相应的第j个第二粒子的第四横向权重和第四航向角权重分别表示为：

在一个可选示例中，该步骤2022b可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第五处理单元执行。
步骤2023b，基于m*n个第二粒子分别对应的第三横向状态和第三航向角状态，将各第二粒子映射到第一网格坐标区域中，获得各第二粒子分别所属的单元格。
其中，第一网格坐标区域包括m个单元格，m＝myaw*mlat，myaw和mlat分别表示第一网格坐标区域在航向角方向和横向方向的单元格数量。第一网格坐标区域为预先建立的网格坐标系下的区域，第一网格坐标区域包括的单元格数量与第一粒子数量m相同，myaw和mlat可以根据实际需求设置。第一网格坐标区域的建立可以根据预设横向状态阈值、预设航向角状态阈值、预设横向状态步长、预设航向角状态步长来建立，各预设阈值可以综合各第二粒子分别对应的第三横向状态和第三航向角状态来确定，以使全部或大部分第二粒子能够投射到第一网格坐标区域中，具体可以根据实际需求设置。
示例性的，图7是本公开一示例性实施例提供的第一网格坐标区域的示意图，第一网格坐标区域的横坐标对应航向角状态(yaw)、纵坐标对应横向状态(lat)，dyaw和dlat分别表示航向角状态步长和横向状态步长。根据第一网格坐标区域包括的单元格数量、横坐标和纵坐标分别对应的单元格数量的不同，第一网格坐标区域中每个单元格相对于坐标原点的位置不同，具体可以根据实际需求设置，本公开实施例不做限定，比如，第一网格坐标区域的坐标原点为整个区域的中心点，每个单元格用[b，d]表示其相对于原点的位置，具有以下几种情况：
当myaw和mlat均为偶数时，则
当myaw和mlat均为奇数，则
若myaw和mlat一个是奇数，另一个是偶数，则m，n根据上述两种情况来确定，具体不再赘述。
相应的，每个单元格在第一网格坐标区域中的坐标范围可以根据该单元格位置[b，d]、myaw和mlat的具体情况、及每个单元格的yaw和lat方向的步长dyaw和dlat来确定，具体不再赘述。
比如，当myaw和mlat均为偶数时，单元格[b，d]＝[1,2]，则该单元格[b，d]的坐标范围为：
yaw轴：[dyaw*(b-1),dyaw*b]＝[0,dyaw]。
lat轴：[dlat*(d-1),dlat*d]＝[dlat,2dlat]。
第一网格坐标区域中，每个单元格可能映射有一个或多个第二粒子。
在实际应用中也可以是横坐标对应横向状态(lat)，纵坐标对应航向角状态(yaw)，具体不做限定。
在一个可选示例中，该步骤2023b可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第六处理单元执行。
步骤2024b，基于各第二粒子分别所属的单元格，确定各单元格分别对应的第三粒子、各第三粒子分别对应的第三预测粒子位姿、及各第三粒子分别对应的第四横向状态和第四航向角状态。
其中，单元格对应的第三粒子是基于映射到该单元格内的各第二粒子将该单元格进行粒子化获得的该单元格对应的一个粒子。每个第三粒子对应的第三预测粒子位姿可以通过该第三粒子对应的单元格内的所有第二粒子的第二预测粒子位姿加权平均确定。同理，每个第三粒子位姿对应的第四横向状态和第四航向角状态分别通过该第三粒子对应的单元格内的所有第二粒子的第三横向状态和第三航向角状态加权平均获得。
示例性的，对于第k(k＝1,2,…,m)个单元格内映射有M个第二粒子，其中，第i(i＝1,2,…,M)个第二粒子对应的第二预测粒子位姿表示为从前述的(l＝1,2,…,m，j＝1,2,…,n)获得。比如前述的第l个第一粒子生成的第j个第二粒子映射到第k个单元格内成为第k个单元格内的第i个第二粒子，则具体不再一一赘述。同理该第二粒子对应的第三横向状态为第三航向角状态
第k个单元格对应的第三粒子称为第k个第三粒子，该第三粒子对应的第三预测粒子位姿表示为第四横向状态表示为和第四航向角状态具体表示如下：


其中，表示第i个第二粒子对应的粒子权重，该粒子权重为上述生成该第二粒子的第一粒子的在前时刻确定的权重，比如该第二粒子是上述的第l个第一粒子生成的，则具体可以根据实际映射情况确定，在此不再一一赘述。
在一个可选示例中，该步骤2024b可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第七处理单元执行。
步骤2025b，将各第三粒子分别对应的第四横向状态和第四航向角状态，作为各第一粒子当前时刻分别对应的第一横向状态和第一航向角状态。
示例性的，图8是本公开一示例性实施例提供的第二粒子映射流程示意图。其中，黑色圆圈表示第二粒子，灰色圆圈表示第三粒子。
在一个可选示例中，该步骤2025b可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第八处理单元执行。
本公开实施例通过对m个第一粒子分别进行多次采样，扩充粒子数量，基于扩充后的粒子进行网格化流程，在保证粒子分布稳定性的基础上，提高定位精度。
图9是本公开另一示例性实施例提供的步骤202的流程示意图。
在一个可选示例中，步骤2021b的针对每个第一粒子位姿，基于第一粒子位姿生成n个第二粒子分别对应的第二预测粒子位姿，包括：
步骤2021b1，基于第一粒子位姿、里程计信息、及n个不同的高斯白噪声，生成n个第二粒子分别对应的第二预测粒子位姿。
示例性的对于第l个第一粒子基于该第一粒子的第一粒子位姿生成n个第二粒子分别对应的第二预测粒子位姿表示如下：
其中，j表示第j个第二粒子，ΔOdom表示可移动设备里程计相对增量，Wlj表示第j个第二粒子对应的高斯白噪声。
在一个可选示例中，该步骤2021b1可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第四处理单元执行。
在一个可选示例中，步骤2022b的基于各第二粒子分别对应的第二预测粒子位姿，确定各第二粒子分别对应的第三横向状态和第三航向角状态，包括：
步骤2022b1，基于在前时刻确定的可移动设备的第一定位位姿，确定可移动设备当前时刻的第一预测位姿。
该步骤与前述步骤2022a1一致，在此不再赘述。
在一个可选示例中，该步骤2022b1可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第五处理单元执行。
步骤2022b2，基于各第二粒子分别对应的第二预测粒子位姿、及第一预测位姿，确定各第二粒子分别对应的第三横向状态和第三航向角状态。
示例性的，以第l个第一粒子为例，对于第j个第二粒子，其第三横向状态和第三航向角状态表示如下：

其中，表示第l个第一粒子生成的第j个第二粒子的第二预测粒子位姿，定义参见前述内容，在此不再赘述。
在一个可选示例中，该步骤2022b2可以由处理器调用存储器存储的相应指令执行，也可以由被处理器运行的第五处理单元执行。
在一个可选示例中，图10是本公开再一示例性实施例提供的位姿的确定方法的流程示意图。在本示例中，该方法包括：
1、在当前时刻t，获取可移动设备对应的在前时刻获得的m个第一粒子的第一粒子位姿，m为大于1的整数。
其中，第l个第一粒子的第一粒子位姿表示为
2、针对每个第一粒子位姿，基于第一粒子位姿、里程计信息、及n个不同的高斯白噪声，生成n个第二粒子分别对应的第二预测粒子位姿。
其中，对于第l个第一粒子，基于该第一粒子的第一粒子位姿生成n个第二粒子分别对应的第二预测粒子位姿表示如下：
其中，j表示第j个第二粒子，ΔOdom表示可移动设备里程计相对增量，Wlj表示第j个第二粒子对应的高斯白噪声。
3、基于在前时刻确定的可移动设备的第一定位位姿，确定可移动设备当前时刻的第一预测位姿。
其中，第一定位位姿为可移动设备当前时刻的第一预测位姿为表示如下：
4、基于各第二粒子分别对应的第二预测粒子位姿、及第一预测位姿，确定各第二粒子分别对应的第三横向状态和第三航向角状态。
其中，以第l个第一粒子为例，对于第j个第二粒子，其第三横向状态和第三航向角状态表示如下：

其中，表示第l个第一粒子生成的第j个第二粒子的第二预测粒子位姿，定义参见前述内容，在此不再赘述。
相应的，第j个第二粒子的第四横向权重和第四航向角权重分别表示为：

5、基于m*n个第二粒子分别对应的第三横向状态和第三航向角状态，将各第二粒子映射到第一网格坐标区域中，获得各第二粒子分别所属的单元格。
其中，第一网格坐标区域包括m个单元格，m＝myaw*mlat，myaw和mlat分别表示第一网格坐标区域在航向角方向和横向方向的单元格数量。
6、基于各第二粒子分别所属的单元格，确定各单元格分别对应的第三粒子、各第三粒子分别对应的第三预测粒子位姿、及各第三粒子分别对应的第四横向状态和第四航向角状态。
其中，对于第k(k＝1,2,…,m)个单元格内映射有M个第二粒子，其中，第i(i＝1,2,…,M)个第二粒子对应的第二预测粒子位姿表示为从前述的(l＝1,2,…,m，j＝1,2,…,n)获得。比如前述的第l个第一粒子生成的第j个第二粒子映射到第k个单元格内成为第k个单元格内的第i个第二粒子，则具体不再一一赘述。同理该第二粒子对应的第三横向状态为第三航向角状态
第k个单元格对应的第三粒子称为第k个第三粒子，该第三粒子对应的第三预测粒子位姿表示为第四横向状态表示为和第四航向角状态具体表示如下：


其中，表示第i个第二粒子对应的粒子权重，该粒子权重为上述生成该第二粒子的第一粒子的在前时刻确定的权重，比如该第二粒子是上述的第l个第一粒子生成的，则具体可以根据实际映射情况确定，在此不再一一赘述。
相应的，第k个第三粒子第五横向权重和第五航向角权重分别为
7、将各第三粒子分别对应的第四横向状态和第四航向角状态，作为各第一粒子当前时刻分别对应的第一横向状态和第一航向角状态。
其中，第k个第一粒子即为上述的第k个第三粒子。相应的，第k个第一粒子的第一横向权重即为上述第k个第三粒子的第五横向权重，第k个第一粒子的第一航向角权重即为上述第k个第三粒子的第五航向角权重。
8、基于感知车道线信息，确定车辆坐标系下的车道线采样点。
其中，总共确定N个车辆坐标系下的车道线采样点，第i(i＝1,2,…,N)个车道线采样点表示为：
其中，t表示当前时刻，c0、c1、c2、c3表示感知车道线参数。
9、分别将各第一粒子作为可移动设备，将第一地图车道线转换到车辆坐标系下，获得各第一粒子分别对应的车辆坐标系下的第二地图车道线。
10、对于每个第一粒子，基于各车道线采样点和第二地图车道线，确定各车道线采样点分别与第二地图车道线的最小横向距离。
11.1、基于各最小横向距离、预设最大横向距离阈值及第一预设规则，确定各车道线采样点分别对应的第一采样点得分。
其中，第i个车道线采样点的第一采样点得分可以表示为
其中，g表示预设最大横向距离阈值，t表示当前时刻，表示第i个车道线采样点与第二地图车道线的最小横向距离，k(k＝1,2,…,m)表示第k个第一粒子(第三粒子)，m表示第一粒子的数量，m为正整数，表示第i个车道线采样点的第一匹配得分权重。
11.2、基于各最小横向距离、预设最大横向距离阈值及第二预设规则，确定各车道线采样点分别对应的第二采样点得分
其中，第i个车道线采样点的第二采样点得分可以表示为
其中，g表示预设最大横向距离阈值，t表示当前时刻，表示第i个车道线采样点与第二地图车道线的最小横向距离，k(k＝1,2,…,m)表示第k个第一粒子(第三粒子)，m表示第一粒子的数量，m为正整数，表示第i个车道线采样点的第二匹配得分权重为。
12.1、基于各车道线采样点分别对应的第一采样点得分，确定各车道线采样点的第一采样点总得分，作为第一粒子对应的第一匹配得分。
其中，第k个第一粒子对应的第一匹配得分
12.2、基于各车道线采样点分别对应的第二采样点得分，确定各车道线采样点的第二采样点总得分，作为第一粒子对应的第二匹配得分。
其中，第k个第一粒子对应的第二匹配得分
13.1、基于各第一匹配得分，分别对其对应的第一粒子的第一横向权重进行更新，获得各第一粒子分别对应的第二横向权重。
其中，第k个第一粒子对应的第二横向权重表示如下：


其中，GNSSyaw表示GNSS传感器输出的航向角，表示第k个第一粒子对应第一航向角状态。0.001与10是预设值，可以根据实际需求调整。
13.2、基于各第二匹配得分，分别对其对应的第一粒子的第一航向角权重进行更新，获得各第一粒子分别对应的第二航向角权重。
其中，第k个第一粒子对应的第二航向角权重表示如下：

14、对各第一粒子进行聚类，获得第一数量的聚簇。
15、针对每个聚簇，基于聚簇中的第一粒子对应的第二横向权重、第二航向角权重、第一横向状态、第一航向角状态，确定聚簇对应的第二横向状态、第二航向角状态和第三横向权重。
其中，在第s(s＝1,2,…,S；S表示聚簇数量)个聚簇中包括F个第一粒子，其中，第f(f＝1,2,…,F)个第一粒子的第一横向状态为第一航向角状态为第二横向权重为第二航向角权重为则该聚簇对应的第二横向状态第二航向角状态和第三横向权重分别如下：


其中，聚簇中每个第一粒子的第一横向状态、第一航向角状态、第二横向权重第二航向角权重分别与前述获得的对应第一粒子(第三粒子)一致，这里只是针对聚簇进行了不同的符号表示，比如，前述的第k个第一粒子对应该第s个聚簇中的第f个第一粒子，则第s个聚簇中的第f个第一粒子的第二横向权重第一横向状态、第一航向角、第二航向角权重同理，在此不再一一赘述。
16、将第三横向权重最大的聚簇的第二横向状态和第二航向角状态分别作为目标横向状态和目标航向角状态。
其中，目标横向状态表示为目标航向角状态表示为
17、获取基于第一直方图滤波器确定的目标纵向状态。
其中，目标纵向状态表示为
18、基于目标纵向状态、目标横向状态、目标航向角状态及可移动设备当前时刻的第一预测位姿，确定可移动设备的当前定位位姿。
其中，可移动设备的当前定位位姿表示如下：
其中，T表示转置运算。其他符号参见前述内容。
19、将步骤13.1中确定的各第一粒子分别对应的第二横向权重作为下一时刻的第一粒子的第一横向权重和第一航向角权重，进入下一时刻的迭代流程。
具体来说，将下一时刻作为当前时刻，重复上述步骤1-19。
本公开实施例的位姿确定方法，在基于粒子滤波器实现状态观测得分计算和状态更新的过程中，分离横向状态和航向角状态，将横向状态和航向角状态分别进行计算，实现了定位过程中能够兼顾提高横向和航向角精度，无论车辆在高精地图的直道行驶还是弯道行驶，车辆的横向和航向角的定位精度均能够达到一致的高水平，不易受到地图车道线曲率的影响。即便在感知车道线的几何形状与地图车道线形状存在一定偏差的情况下，仍能够获得高精度的定位结果，有效提升整体车辆在实时定位过程中的精度和稳定性。并且，在粒子更新过程中，通过对每个第一粒子生成多个第二粒子，进行粒子数量扩充，进而结合网格坐标区域将扩充后的第二粒子再合并成第一粒子数量相同的第三粒子，将第三粒子作为第一粒子进行感知车道线与地图车道线的匹配，实现了粒子的重采样，保证粒子分布的稳定性，有助于进一步提高定位精度。
本公开实施例提供的任一种位姿的确定方法可以由任意适当的具有数据处理能力的设备执行，包括但不限于：终端设备和服务器等。或者，本公开实施例提供的任一种位姿的确定方法可以由处理器执行，如处理器通过调用存储器存储的相应指令来执行本公开实施例提及的任一种位姿的确定方法。下文不再赘述。
本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
示例性装置
图11是本公开一示例性实施例提供的位姿的确定装置的结构示意图。该实施例的装置可用于实现本公开相应的方法实施例，如图11所示的装置包括：第一确定模块501、第一处理模块502、第二处理模块503、第三处理模块504、第四处理模块505和第五处理模块506。
第一确定模块501，用于确定可移动设备对应的m个第一粒子的第一粒子位姿，m为大于1的整数，第一粒子位姿是在前时刻获得的对应第一粒子的位姿；第一处理模块502，用于基于第一确定模块501确定的各第一粒子位姿，确定各第一粒子当前时刻分别对应的第一横向状态和第一航向角状态；第二处理模块503，用于基于感知车道线信息，确定各第一粒子分别对应的第一匹配得分和第二匹配得分；第三处理模块504，用于基于第二处理模块503获得的各第一匹配得分，分别对其对应的第一粒子的第一横向权重进行更新，获得各第一粒子分别对应的第二横向权重；第四处理模块505，用于基于第二处理模块503获得的各第二匹配得分，分别对其对应的第一粒子的第一航向角权重进行更新，获得各第一粒子分别对应的第二航向角权重；第五处理模块506，用于基于各第一粒子分别对应的第二横向权重、第二航向角权重、第一横向状态、第一航向角状态，确定可移动设备的当前定位位姿。
在一个可选示例中，图12是本公开一示例性实施例提供的第二处理模块503的结构示意图。本示例中，第二处理模块503包括：第一确定单元5031、第一转换单元5032、第二确定单元5033和第三确定单元5034。
第一确定单元5031，用于基于感知车道线信息，确定车辆坐标系下的车道线采样点；第一转换单元5032，用于分别将各第一粒子作为可移动设备，将第一地图车道线转换到车辆坐标系下，获得各第一粒子分别对应的车辆坐标系下的第二地图车道线；第二确定单元5033，用于基于车道线采样点、各第一粒子分别对应的第二地图车道线及第一预设规则，确定各第一粒子分别对应的第一匹配得分；第三确定单元5034，用于基于车道线采样点、各第一粒子分别对应的第二地图车道线及第二预设规则，确定各第一粒子分别对应的第二匹配得分。
在一个可选示例中，第二确定单元5033具体用于：对于每个第一粒子，基于各车道线采样点和第二地图车道线，确定各车道线采样点分别与第二地图车道线的最小横向距离；基于各最小横向距离、预设最大横向距离阈值及第一预设规则，确定各车道线采样点分别对应的第一采样点得分；基于各车道线采样点分别对应的第一采样点得分，确定各车道线采样点的第一采样点总得分，作为第一粒子对应的第一匹配得分。
在一个可选示例中，第三确定单元5034具体用于：对于每个第一粒子，基于各车道线采样点和第二地图车道线，确定各车道线采样点分别与第二地图车道线的最小横向距离；基于各最小横向距离、预设最大横向距离阈值及第二预设规则，确定各车道线采样点分别对应的第二采样点得分；基于各车道线采样点分别对应的第二采样点得分，确定各车道线采样点的第二采样点总得分，作为第一粒子对应的第二匹配得分。
在一个可选示例中，图13是本公开一示例性实施例提供的第五处理模块506的结构示意图。本示例中，第五处理模块506包括：聚类单元5061、第一处理单元5062、第二处理单元5063、第一获取单元5064和第三处理单元5065。
聚类单元5061，用于对各第一粒子进行聚类，获得第一数量的聚簇；第一处理单元5062，用于针对每个聚簇，基于聚簇中的第一粒子对应的第二横向权重、第二航向角权重、第一横向状态、第一航向角状态，确定聚簇对应的第二横向状态、第二航向角状态和第三横向权重；第二处理单元5063，用于将第三横向权重最大的聚簇的第二横向状态和第二航向角状态分别作为目标横向状态和目标航向角状态；第一获取单元5064，用于获取基于第一直方图滤波器确定的目标纵向状态；第三处理单元5065，用于基于目标纵向状态、目标横向状态、目标航向角状态及可移动设备当前时刻的第一预测位姿，确定可移动设备的当前定位位姿。
在一个可选示例中，图14是本公开一示例性实施例提供的第一处理模块502的结构示意图。本示例中，第一处理模块502包括：第四确定单元5021a和第五确定单元5022a。
第四确定单元5021a，用于基于各第一粒子位姿，确定各第一粒子分别对应的第一预测粒子位姿；第五确定单元5022a，基于各第一预测粒子位姿，确定各第一粒子分别对应的第一横向状态和第一航向角状态。
在一个可选示例中，第五确定单元5022a具体用于：基于在前时刻确定的可移动设备的第一定位位姿，确定可移动设备当前时刻的第一预测位姿；基于各第一预测粒子位姿和第一预测位姿，确定各第一粒子分别对应的第一横向状态和第一航向角状态。
在一个可选示例中，图15是本公开另一示例性实施例提供的第一处理模块502的结构示意图。本示例中，第一处理模块502包括：第四处理单元5021b、第五处理单元5022b、第六处理单元5023b、第七处理单元5024b和第八处理单元5025b。
第四处理单元5021b，用于针对每个第一粒子位姿，基于第一粒子位姿生成n个第二粒子分别对应的第二预测粒子位姿；第五处理单元5022b，用于基于各第二粒子分别对应的第二预测粒子位姿，确定各第二粒子分别对应的第三横向状态和第三航向角状态；第六处理单元5023b，用于基于m*n个第二粒子分别对应的第三横向状态和第三航向角状态，将各第二粒子映射到第一网格坐标区域中，获得各第二粒子分别所属的单元格，第一网格坐标区域包括m个单元格，m＝myaw*mlat，myaw和mlat分别表示第一网格坐标区域在航向角方向和横向方向的单元格数量；第七处理单元5024b，用于基于各第二粒子分别所属的单元格，确定各单元格分别对应的第三粒子、各第三粒子分别对应的第三预测粒子位姿、及各第三粒子分别对应的第四横向状态和第四航向角状态；第八处理单元5025b，用于将各第三粒子分别对应的第四横 向状态和第四航向角状态，作为各第一粒子当前时刻分别对应的第一横向状态和第一航向角状态。
在一个可选示例中，第四处理单元5021b具体用于：基于第一粒子位姿、里程计信息、及n个不同的高斯白噪声，生成n个第二粒子分别对应的第二预测粒子位姿。
在一个可选示例中，第五处理单元5022b具体用于：基于在前时刻确定的可移动设备的第一定位位姿，确定可移动设备当前时刻的第一预测位姿；基于各第二粒子分别对应的第二预测粒子位姿、及第一预测位姿，确定各第二粒子分别对应的第三横向状态和第三航向角状态。
本装置示例性实施例对应的有益技术效果可以参见上述示例性方法部分的相应有益技术效果，在此不再赘述。
示例性电子设备
图16是本公开电子设备一个应用实施例的结构示意图。本实施例中，该电子设备10包括一个或多个处理器11和存储器12。
处理器11可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备10中的其他组件以执行期望的功能。
存储器12可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器11可以运行所述程序指令，以实现上文所述的本公开的各个实施例的方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如输入信号、信号分量、噪声分量等各种内容。
在一个示例中，电子设备10还可以包括：输入装置13和输出装置14，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。
此外，该输入装置13还可以包括例如键盘、鼠标等等。
该输出装置14可以向外部输出各种信息。该输出装置14可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
当然，为了简化，图16中仅示出了该电子设备10中与本公开有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备10还可以包括任何其他适当的组件。
示例性计算机程序产品和计算机可读存储介质
除了上述方法和设备以外，本公开的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本公开各种实施例的方法中的步骤。
计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本公开实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
此外，本公开的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本公开各种实施例的方法中的步骤。
计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
以上结合具体实施例描述了本公开的基本原理，但是，在本公开中提及的优点、优势、效果等仅是示例而非限制，不能认为其是本公开的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本公开为必须采用上述具体的细节来实现。
本领域的技术人员可以对本公开进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本公开权利要求及其等同技术的范围之内，则本公开也意图包含这些改动和变型在内。

Claims (12)

  1. 一种位姿的确定方法，包括：
    确定可移动设备对应的m个第一粒子的第一粒子位姿，m为大于1的整数，所述第一粒子位姿是在前时刻获得的对应第一粒子的位姿；
    基于各所述第一粒子位姿，确定各所述第一粒子当前时刻分别对应的第一横向状态和第一航向角状态；
    基于感知车道线信息，确定各所述第一粒子分别对应的第一匹配得分和第二匹配得分；
    基于各所述第一匹配得分，分别对其对应的所述第一粒子的第一横向权重进行更新，获得各所述第一粒子分别对应的第二横向权重；
    基于各所述第二匹配得分，分别对其对应的所述第一粒子的第一航向角权重进行更新，获得各所述第一粒子分别对应的第二航向角权重；
    基于各所述第一粒子分别对应的所述第二横向权重、所述第二航向角权重、所述第一横向状态、所述第一航向角状态，确定所述可移动设备的当前定位位姿。
  2. 根据权利要求1所述的方法，其中，所述基于感知车道线信息，确定各所述第一粒子分别对应的第一匹配得分和第二匹配得分，包括：
    基于所述感知车道线信息，确定车辆坐标系下的车道线采样点；
    分别将各所述第一粒子作为所述可移动设备，将第一地图车道线转换到所述车辆坐标系下，获得各所述第一粒子分别对应的所述车辆坐标系下的第二地图车道线；
    基于所述车道线采样点、各所述第一粒子分别对应的所述第二地图车道线及第一预设规则，确定各所述第一粒子分别对应的所述第一匹配得分；
    基于所述车道线采样点、各所述第一粒子分别对应的所述第二地图车道线及第二预设规则，确定各所述第一粒子分别对应的所述第二匹配得分。
  3. 根据权利要求2所述的方法，其中，所述基于所述车道线采样点、各所述第一粒子分别对应的所述第二地图车道线及第一预设规则，确定各所述第一粒子分别对应的所述第一匹配得分，包括：
    对于每个所述第一粒子，基于各所述车道线采样点和所述第二地图车道线，确定各所述车道线采样点分别与所述第二地图车道线的最小横向距离；
    基于各所述最小横向距离、预设最大横向距离阈值及所述第一预设规则，确定各所述车道线采样点分别对应的第一采样点得分；
    基于各所述车道线采样点分别对应的所述第一采样点得分，确定各所述车道线采样点的第一采样点总得分，作为所述第一粒子对应的所述第一匹配得分；
    所述基于所述车道线采样点、各所述第一粒子分别对应的所述第二地图车道线及第二预设规则，确定各所述第一粒子分别对应的所述第二匹配得分，包括：
    对于每个所述第一粒子，基于各所述车道线采样点和所述第二地图车道线，确定各所述车道线采样点分别与所述第二地图车道线的最小横向距离；
    基于各所述最小横向距离、预设最大横向距离阈值及所述第二预设规则，确定各所述车道线采样点分别对应的第二采样点得分；
    基于各所述车道线采样点分别对应的所述第二采样点得分，确定各所述车道线采样点的第二采样点总得分，作为所述第一粒子对应的所述第二匹配得分。
  4. 根据权利要求1-3任一所述的方法，其中，所述基于各所述第一粒子分别对应的所述第二横向权重、所述第二航向角权重、所述第一横向状态、所述第一航向角状态，确定所述可移动设备的当前定位位姿，包括：
    对各所述第一粒子进行聚类，获得第一数量的聚簇；
    针对每个所述聚簇，基于所述聚簇中的所述第一粒子对应的所述第二横向权重、所述第二航向角权重、所述第一横向状态、所述第一航向角状态，确定所述聚簇对应的第二横向状态、第二航向角状态和第三横向权重；
    将所述第三横向权重最大的所述聚簇的所述第二横向状态和所述第二航向角状态分别作为目标横向状态和目标航向角状态；
    获取基于第一直方图滤波器确定的目标纵向状态；
    基于所述目标纵向状态、所述目标横向状态、所述目标航向角状态及所述可移动设备当前时刻的第一预测位姿，确定所述可移动设备的所述当前定位位姿。
  5. 根据权利要求1-3任一所述的方法，其中，所述基于各所述第一粒子位姿，确定各所述第一粒子当前时刻分别对应的第一横向状态和第一航向角状态，包括：
    基于各所述第一粒子位姿，确定各所述第一粒子分别对应的第一预测粒子位姿；
    基于各所述第一预测粒子位姿，确定各所述第一粒子分别对应的所述第一横向状态和所述第一航向角状态。
  6. 根据权利要求5所述的方法，其中，所述基于各所述第一预测粒子位姿，确定各所述第一粒子分别对应的所述第一横向状态和所述第一航向角状态，包括：
    基于在前时刻确定的所述可移动设备的第一定位位姿，确定所述可移动设备当前时刻的第一预测位姿；
    基于各所述第一预测粒子位姿和所述第一预测位姿，确定各所述第一粒子分别对应的所述第一横向状态和所述第一航向角状态。
  7. 根据权利要求1-3任一所述的方法，其中，所述基于各所述第一粒子位姿，确定各所述第一粒子当前时刻分别对应的第一横向状态和第一航向角状态，包括：
    针对每个所述第一粒子位姿，基于所述第一粒子位姿生成n个第二粒子分别对应的第二预测粒子位姿；
    基于各所述第二粒子分别对应的所述第二预测粒子位姿，确定各所述第二粒子分别对应的第三横向状态和第三航向角状态；
    基于m*n个所述第二粒子分别对应的所述第三横向状态和所述第三航向角状态，将各所述第二粒子映射到第一网格坐标区域中，获得各所述第二粒子分别所属的单元格，所述第一网格坐标区域包括m个单元格，m＝myaw*mlat，myaw和mlat分别表示所述第一网格坐标区域在航向角方向和横向方向的单元格数量；
    基于各所述第二粒子分别所属的单元格，确定各所述单元格分别对应的第三粒子、各所述第三粒子分别对应的第三预测粒子位姿、及各所述第三粒子分别对应的第四横向状态和第四航向角状态；
    将各所述第三粒子分别对应的所述第四横向状态和所述第四航向角状态，作为各所述第一粒子当前时刻分别对应的所述第一横向状态和所述第一航向角状态。
  8. 根据权利要求7所述的方法，其中，所述针对每个所述第一粒子位姿，基于所述第一粒子位姿生成n个第二粒子分别对应的第二预测粒子位姿，包括：
    基于所述第一粒子位姿、里程计信息、及n个不同的高斯白噪声，生成n个所述第二粒子分别对应的所述第二预测粒子位姿。
  9. 根据权利要求7所述的方法，其中，所述基于各所述第二粒子分别对应的所述第二预测粒子位姿，确定各所述第二粒子分别对应的第三横向状态和第三航向角状态，包括：
    基于在前时刻确定的所述可移动设备的第一定位位姿，确定所述可移动设备当前时刻的第一预测位姿；
    基于各所述第二粒子分别对应的所述第二预测粒子位姿、及所述第一预测位姿，确定各所述第二粒子分别对应的所述第三横向状态和所述第三航向角状态。
  10. 一种位姿的确定装置，包括：
    第一确定模块，用于确定可移动设备对应的m个第一粒子的第一粒子位姿，m为大于1的整数，所述第一粒子位姿是在前时刻获得的对应第一粒子的位姿；
    第一处理模块，用于基于各所述第一粒子位姿，确定各所述第一粒子当前时刻分别对应的第一横向状态和第一航向角状态；
    第二处理模块，用于基于感知车道线信息，确定各所述第一粒子分别对应的第一匹配得分和第二匹配得分；
    第三处理模块，用于基于各所述第一匹配得分，分别对其对应的所述第一粒子的第一横向权重进行更新，获得各所述第一粒子分别对应的第二横向权重；
    第四处理模块，用于基于各所述第二匹配得分，分别对其对应的所述第一粒子的第一航向角权重进行更新，获得各所述第一粒子分别对应的第二航向角权重；
    第五处理模块，用于基于各所述第一粒子分别对应的所述第二横向权重、所述第二航向角权重、所述第一横向状态、所述第一航向角状态，确定所述可移动设备的当前定位位姿。
  11. 一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序用于执行上述权利要求1-9任一所述的位姿的确定方法。
  12. 一种电子设备，所述电子设备包括：
    处理器；
    用于存储所述处理器可执行指令的存储器；
    所述处理器，用于从所述存储器中读取所述可执行指令，并执行所述指令以实现上述权利要求1-9任一所述的位姿的确定方法。
PCT/CN2023/113598 2022-08-24 2023-08-17 位姿的确定方法、装置、电子设备和存储介质 WO2024041447A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202211028797.7A CN115388906A (zh) 2022-08-24 2022-08-24 位姿的确定方法、装置、电子设备和存储介质
CN202211028797.7 2022-08-24

Publications (1)

Publication Number Publication Date
WO2024041447A1 true WO2024041447A1 (zh) 2024-02-29

Family

ID=84123206

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2023/113598 WO2024041447A1 (zh) 2022-08-24 2023-08-17 位姿的确定方法、装置、电子设备和存储介质

Country Status (2)

Country Link
CN (1) CN115388906A (zh)
WO (1) WO2024041447A1 (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115388906A (zh) * 2022-08-24 2022-11-25 上海安亭地平线智能交通技术有限公司 位姿的确定方法、装置、电子设备和存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080119961A1 (en) * 2006-11-16 2008-05-22 Samsung Electronics Co., Ltd. Methods, apparatus, and medium for estimating pose of mobile robot using particle filter
KR102103651B1 (ko) * 2018-11-28 2020-04-22 한국교통대학교산학협력단 지도의 차로 개수를 활용한 파티클 필터링 퇴화 경감 방법 및 시스템
CN114119673A (zh) * 2022-01-25 2022-03-01 北京地平线机器人技术研发有限公司 初始位姿的确定方法和装置、电子设备和存储介质
CN114248778A (zh) * 2020-09-22 2022-03-29 华为技术有限公司 移动设备的定位方法和定位装置
CN114355415A (zh) * 2022-01-06 2022-04-15 上海安亭地平线智能交通技术有限公司 位姿信息的确定方法和装置、电子设备和存储介质
CN115388906A (zh) * 2022-08-24 2022-11-25 上海安亭地平线智能交通技术有限公司 位姿的确定方法、装置、电子设备和存储介质

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9222788B2 (en) * 2012-06-27 2015-12-29 Microsoft Technology Licensing, Llc Proactive delivery of navigation options
CN110954113B (zh) * 2019-05-30 2021-10-15 北京初速度科技有限公司 一种车辆位姿的修正方法和装置
CN111998860B (zh) * 2020-08-21 2023-02-17 阿波罗智能技术(北京)有限公司 自动驾驶定位数据校验方法、装置、电子设备及存储介质
CN112977478B (zh) * 2021-04-13 2021-08-24 北京主线科技有限公司 一种车辆控制方法及系统
CN114034307B (zh) * 2021-11-19 2024-04-16 智道网联科技(北京)有限公司 基于车道线的车辆位姿校准方法、装置和电子设备
CN114162140B (zh) * 2021-12-08 2023-08-01 武汉中海庭数据技术有限公司 一种最优车道匹配方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080119961A1 (en) * 2006-11-16 2008-05-22 Samsung Electronics Co., Ltd. Methods, apparatus, and medium for estimating pose of mobile robot using particle filter
KR102103651B1 (ko) * 2018-11-28 2020-04-22 한국교통대학교산학협력단 지도의 차로 개수를 활용한 파티클 필터링 퇴화 경감 방법 및 시스템
CN114248778A (zh) * 2020-09-22 2022-03-29 华为技术有限公司 移动设备的定位方法和定位装置
CN114355415A (zh) * 2022-01-06 2022-04-15 上海安亭地平线智能交通技术有限公司 位姿信息的确定方法和装置、电子设备和存储介质
CN114119673A (zh) * 2022-01-25 2022-03-01 北京地平线机器人技术研发有限公司 初始位姿的确定方法和装置、电子设备和存储介质
CN115388906A (zh) * 2022-08-24 2022-11-25 上海安亭地平线智能交通技术有限公司 位姿的确定方法、装置、电子设备和存储介质

Also Published As

Publication number Publication date
CN115388906A (zh) 2022-11-25

Similar Documents

Publication Publication Date Title
KR102335389B1 (ko) 자율 주행 차량의 lidar 위치 추정을 위한 심층 학습 기반 특징 추출
KR102292277B1 (ko) 자율 주행 차량에서 3d cnn 네트워크를 사용하여 솔루션을 추론하는 lidar 위치 추정
US11364931B2 (en) Lidar localization using RNN and LSTM for temporal smoothness in autonomous driving vehicles
WO2020029758A1 (zh) 对象三维检测及智能驾驶控制的方法、装置、介质及设备
JP2021120864A (ja) 障害物を検出するための方法および装置、電子機器、記憶媒体並びにコンピュータプログラム
WO2022007776A1 (zh) 目标场景区域的车辆定位方法、装置、设备和存储介质
WO2024041447A1 (zh) 位姿的确定方法、装置、电子设备和存储介质
CN111427370A (zh) 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法
WO2022017131A1 (zh) 点云数据的处理方法、智能行驶控制方法及装置
JP2020530569A (ja) 車両センサの較正及び位置特定
WO2023185354A1 (zh) 实景导航方法、装置、设备及存储介质、程序产品
CN114119673B (zh) 初始位姿的确定方法和装置、电子设备和存储介质
CN110956662A (zh) 载体避障方法、装置及电子设备
WO2023131048A1 (zh) 位姿信息的确定方法、装置、电子设备和存储介质
JP2020122754A (ja) ３次元位置推定装置及びプログラム
CN111707262A (zh) 基于最近点向量投影的点云匹配方法、介质、终端和装置
CN111553954B (zh) 一种基于直接法单目slam的在线光度标定方法
CN110766793B (zh) 一种基于语义点云的地图构建方法及装置
CN114280583B (zh) 无gps信号下激光雷达定位精度验证方法及系统
CN115546319B (zh) 车道保持方法、装置、计算机设备及存储介质
CN113483769A (zh) 基于粒子滤波器的车辆自定位方法、系统、设备和介质
CN115388893B (zh) 基于遗传的滤波slam算法的移动机器人建图方法
CN117853548A (zh) 基于点云状态概率估计的改进ndt方法及装置
CN116597096A (zh) 场景重建方法、装置、存储介质及电子设备
CN115546603A (zh) 一种基于卷积神经网络的文物图像匹配方法、终端及介质

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23856537

Country of ref document: EP

Kind code of ref document: A1