WO2023131048A1 - 位姿信息的确定方法、装置、电子设备和存储介质 - Google Patents
位姿信息的确定方法、装置、电子设备和存储介质 Download PDFInfo
- Publication number
- WO2023131048A1 WO2023131048A1 PCT/CN2022/143426 CN2022143426W WO2023131048A1 WO 2023131048 A1 WO2023131048 A1 WO 2023131048A1 CN 2022143426 W CN2022143426 W CN 2022143426W WO 2023131048 A1 WO2023131048 A1 WO 2023131048A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- pose
- particles
- particle
- predicted
- mobile device
- Prior art date
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
Definitions
- the present disclosure relates to mobile device technology, in particular to a method, device, electronic device and storage medium for determining pose information.
- GNSS Global Navigation Satellite System
- the particle filter algorithm is usually used to achieve, but the existing pose determination method based on the particle filter algorithm has the coupling of horizontal and vertical observations, which easily leads to conflicts in the positioning of lane lines and arrow landmarks, resulting in low positioning accuracy .
- Embodiments of the present disclosure provide a method, device, electronic device, and storage medium for determining pose information.
- a method for determining pose information including: acquiring the first particle poses of N first particles corresponding to the mobile device, where N is a positive integer greater than 1; the first The particle pose is the longitudinally corrected posterior pose corresponding to the first particle obtained at the previous moment; based on the first particle pose of each first particle, the pose of each first particle at the current moment is laterally positioned Forecasting, obtaining the first predicted pose corresponding to each first particle; determining the first estimated pose of the movable device based on the first predicted pose corresponding to each first particle; pose as the pose information of the mobile device at the current moment.
- an apparatus for determining pose information including: a first acquisition module, configured to acquire the first particle poses of N first particles corresponding to the mobile device, where N is A positive integer greater than 1; the first particle pose is the longitudinally corrected posterior pose corresponding to the first particle obtained at the previous moment; the first processing module is used to respectively base on the first particle pose of each first particle , perform lateral positioning prediction on the pose of each first particle at the current moment, and obtain the first predicted pose corresponding to each first particle; the second processing module is used to obtain the first predicted pose corresponding to each first particle based on The pose is to determine the first estimated pose of the mobile device; the third processing module is configured to use the first estimated pose of the movable device as the pose information of the mobile device at the current moment.
- a computer-readable storage medium stores a computer program, and the computer program is used to execute the method for determining pose information in any of the above-mentioned embodiments of the present disclosure.
- an electronic device includes: a processor;
- a memory for instructions configured to read executable instructions from the memory, and execute the instructions to implement the method for determining pose information in any of the above-mentioned embodiments of the present disclosure.
- the horizontal positioning prediction is performed based on the particles after the vertical correction at the previous moment, without introducing vertical noise, and avoiding the distribution of particles in the vertical direction, so that each particle is only different in the horizontal direction, and the vertical direction is different.
- the correction is carried out in the follow-up, realizing the decoupling of horizontal correction and vertical correction, which effectively solves the problem of low positioning accuracy due to the coupling of horizontal and vertical observations in the prior art.
- FIG. 1 is an exemplary application scenario of the method for determining pose information provided by the present disclosure
- Fig. 2 is a schematic flowchart of a method for determining pose information provided by an exemplary embodiment of the present disclosure
- Fig. 3 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure
- FIG. 4 is a schematic flowchart of step 301 provided by an exemplary embodiment of the present disclosure.
- FIG. 5 is a schematic flowchart of step 203 provided by an exemplary embodiment of the present disclosure.
- Fig. 6 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure
- Fig. 7 is a schematic diagram of a first grid coordinate area provided by an exemplary embodiment of the present disclosure.
- Fig. 8 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure.
- Fig. 9 is a schematic diagram of the principle of gridding-based particle swarm resampling provided by an exemplary embodiment of the present disclosure.
- FIG. 10 is a schematic diagram of the principle of grid-based particle swarm resampling provided by another exemplary embodiment of the present disclosure.
- FIG. 11 is a schematic flowchart of step 3011 provided by an exemplary embodiment of the present disclosure.
- Fig. 12 is a schematic flowchart of step 3015 provided by an exemplary embodiment of the present disclosure.
- FIG. 13 is a schematic flowchart of step 2031 provided by an exemplary embodiment of the present disclosure.
- Fig. 14 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure.
- Fig. 15 is a schematic diagram of the principle of horizontal and vertical correction of particle swarms provided by an exemplary embodiment of the present disclosure
- Fig. 16 is a schematic structural diagram of an apparatus for determining pose information provided by an exemplary embodiment of the present disclosure
- Fig. 17 is a schematic structural diagram of an apparatus for determining pose information provided by another exemplary embodiment of the present disclosure.
- Fig. 18 is a schematic structural diagram of a first vertical processing module 505 provided by an exemplary embodiment of the present disclosure
- Fig. 19 is a schematic structural diagram of a second processing module 503 provided by an exemplary embodiment of the present disclosure.
- Fig. 20 is a schematic structural diagram of a device for determining pose information provided by another exemplary embodiment of the present disclosure.
- Fig. 21 is a schematic structural diagram of an application embodiment of the electronic device of the present disclosure.
- plural may refer to two or more than two, and “at least one” may refer to one, two or more than two.
- the term "and/or" in the present disclosure is only an association relationship describing associated objects, indicating that there may be three relationships, for example, A and/or B may indicate: A exists alone, and A and B exist at the same time , there are three cases of B alone.
- the character "/" in the present disclosure generally indicates that the contextual objects are an "or" relationship.
- Embodiments of the present disclosure may be applied to electronic devices such as terminal devices, computer systems, servers, etc., which may operate with numerous other general purpose or special purpose computing system environments or configurations.
- Examples of well known terminal devices, computing systems, environments and/or configurations suitable for use with electronic devices such as terminal devices, computer systems, servers include, but are not limited to: personal computer systems, server computer systems, thin clients, thick client Computers, handheld or laptop devices, microprocessor-based systems, set-top boxes, programmable consumer electronics, networked personal computers, minicomputer systems, mainframe computer systems, and distributed cloud computing technology environments including any of the foregoing, etc.
- Electronic devices such as terminal devices, computer systems, servers, etc. may be described in the general context of computer system-executable instructions, such as program modules, being executed by the computer system.
- program modules may include routines, programs, objects, components, logic, data structures, etc., that perform particular tasks or implement particular abstract data types.
- the computer system/server can be practiced in distributed cloud computing environments where tasks are performed by remote processing devices that are linked through a communications network.
- program modules may be located in both local and remote computing system storage media including storage devices.
- the inventors found that when a mobile device (such as a vehicle) is positioned with high precision based on visual observation and a high-precision map, the particle filter algorithm is usually used to determine the pose of the mobile device.
- the motion model when predicting, the motion model based on it will introduce translation component noise and rotation noise to represent the cumulative error of motion.
- the translation component noise includes lateral noise and longitudinal noise, so that The coupling of horizontal and vertical observations can easily lead to conflicts in the positioning of lane lines and arrow road signs, resulting in low positioning accuracy.
- Fig. 1 is an exemplary application scenario of the method for determining pose information provided by the present disclosure.
- the horizontal pose correction and the vertical pose correction can be decoupled, and at the current moment, based on the particles of the vertical correction at the previous moment
- Horizontal positioning prediction based on the particle pose after horizontal correction, determine the current pose of the vehicle, and at the same time correct the particle pose after vertical correction at the current moment and use it as the particle at the next moment, so as to avoid horizontal and vertical Observe the coupling situation to improve the positioning accuracy of the pose.
- x and y represent the x-axis and y-axis of the vehicle's self-coordinate system respectively
- the y direction is the horizontal direction
- the x direction is the longitudinal direction.
- the observation results refer to the matching results of the lane lines and arrow landmarks recognized by the environment image collected by the camera and the lane lines and arrows in the map.
- the lane line mainly affects the horizontal correction
- the arrow mainly affects the vertical correction.
- the observation of the lane line and the observation of the arrow are used for the horizontal correction and the vertical correction respectively, thereby decoupling the horizontal observation and the vertical observation, avoiding For better observation of arrows, the observation of lane markings becomes worse, or for better observation of lane markings, the observation of arrows becomes worse.
- the pose includes three degrees of freedom including a horizontal coordinate component y, a vertical coordinate component x, and a heading angle ⁇ .
- Fig. 2 is a schematic flowchart of a method for determining pose information provided by an exemplary embodiment of the present disclosure. This embodiment can be applied to electronic equipment, specifically on a vehicle-mounted computing platform, as shown in Figure 2, including the following steps:
- Step 201 obtain the first particle poses of the N first particles corresponding to the mobile device, where N is a positive integer greater than 1; the first particle poses are obtained at the previous moment after longitudinal correction of the corresponding first particles. Check pose.
- the mobile device may be any mobile device that needs to be positioned, such as a vehicle or a robot.
- the first particle poses of the N first particles may be the poses of the N first particles after vertical correction at the previous moment.
- Step 202 Based on the first particle poses of the first particles, perform lateral positioning prediction on the poses of the first particles at the current moment, and obtain the first predicted poses corresponding to the first particles.
- the horizontal positioning prediction refers to the correction without considering the vertical direction, that is, the vertical noise of the translation component is not introduced into the motion model during the prediction process.
- the motion model may adopt an odometer-based motion model (Odometry Sample Motion Model) or other practicable motion models, which are not limited in the present disclosure.
- the first particle pose of the first particle is the original pose, based on the odometer information, the translation distance and rotation angle during the period from the previous moment to the current moment can be determined , inputting the original pose, the translation distance and the rotation angle into the motion model, the first predicted pose corresponding to the first particle can be output.
- Step 203 Determine a first estimated pose of the mobile device based on the first predicted poses corresponding to the respective first particles.
- the first estimated pose of the mobile device can be determined based on the first predicted poses corresponding to each first particle.
- the weights corresponding to the first particles are weighted and averaged for each of the first predicted poses as the first estimated pose of the mobile device, wherein the weights corresponding to the first particles used in the weighted average can be based on certain rules The updated weights.
- step 204 the first estimated pose of the mobile device is used as pose information of the mobile device at the current moment.
- the correction at the current moment comprehensively considers the effects of vertical correction and horizontal correction, and the first estimated pose has a higher value in both horizontal and vertical directions.
- Using the first estimated pose as the pose information of the mobile device in the map at the current moment can achieve high-precision positioning of the mobile device.
- the vertical correction at each moment is performed after the horizontal correction.
- the horizontal correction and the vertical correction are a cyclic and iterative process with the passage of time, and the above steps also exist at the previous moment.
- each particle is vertically corrected based on the first estimated pose, and the pose after vertical correction is obtained, as the above-mentioned N The first particle pose of the first particle.
- the method for determining the pose information provided in this embodiment at the current moment, based on the particle swarm corrected vertically at the previous moment, first performs lateral positioning prediction to obtain the predicted pose of the particles, and then estimates the position of the mobile device based on the predicted pose of the particles. For the vertical orientation, the current moment introduces the influence of the vertical correction at the previous moment.
- the horizontal positioning prediction process is based on the particles after the vertical correction at the previous moment, the horizontal positioning prediction is performed without introducing vertical noise, so as to avoid particles in the
- the distribution in the vertical direction makes each particle only different in the horizontal direction, and the vertical correction is carried out later, realizing the decoupling of the horizontal correction and the vertical correction, effectively solving the problem of low positioning accuracy due to the coupling of horizontal and vertical observations in the prior art question.
- FIG. 3 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure.
- the disclosed method further includes:
- Step 301 based on a first estimated pose of a mobile device, determine a first longitudinal pose correction amount.
- Step 302 Perform longitudinal correction on the first predicted pose of each first particle based on the first longitudinal pose correction amount, and obtain the posterior pose and pose corresponding to each first particle at the current moment.
- the first vertical pose correction amount used for vertical correction is determined based on the first estimated pose of the mobile device, so that the first predicted pose corresponding to each first particle Perform vertical correction, as the posterior pose of the current moment corresponding to each first particle, and use it as the basis of the processing flow at the next moment, or perform resampling as the basis of the processing flow at the next moment, so that the vertical direction at the current moment Corrective effects are introduced at the next moment.
- the present disclosure determines the current pose of the mobile device based on the pose of the particle swarm after the horizontal correction at the current moment, and at the same time performs vertical correction on the particle swarm after the horizontal correction, so that the influence of the vertical correction will take effect at the next moment.
- the vertical correction at the current moment is avoided to reduce the horizontal positioning accuracy, so as to ensure the horizontal positioning accuracy.
- FIG. 4 is a schematic flowchart of step 301 provided by an exemplary embodiment of the present disclosure.
- step 301 may specifically include the following steps:
- Step 3011 centering on the first estimated pose, sampling in the vertical direction to obtain the second particle poses of M second particles, where M is a positive integer greater than 1.
- a certain particle may be sampled forward and backward at fixed intervals in the vertical direction with the first estimated pose as the center, so as to obtain the second particle pose including M second particles in the center.
- the specific interval may be set according to actual requirements, which is not limited in this embodiment of the present disclosure.
- Step 3012 Obtain prior weights corresponding to each second particle based on the Gaussian distribution.
- a Gaussian distribution is established centering on the first estimated pose to determine the prior weights corresponding to each second particle, as follows:
- Step 3013 obtaining the arrow landmark observation results and pose measurement results corresponding to each second particle.
- the arrow landmark observation result corresponding to a second particle includes the observation probability of the arrow landmark (referred to as the arrow observation probability) under the second particle pose corresponding to the second particle; the pose measurement result corresponding to a second particle is included in The observation probability of the pose of the mobile device under the pose of the second particle corresponding to the second particle (referred to as the pose observation probability).
- the pose of the second particle corresponding to the second particle is used as the origin of the self-coordinate system of the mobile device, and the self-coordinate system of the mobile device obtained by image recognition is Transform the arrow landmarks of the map to the map coordinate system, or transform the arrow elements in the map to the self-coordinate system of the mobile device, match the observed arrow landmarks with the arrow elements of the map in the same coordinate system, and obtain
- the specific score calculation function can be set according to actual needs, which is not limited in this disclosure.
- the pose measurement result corresponding to the second particle is determined based on the matching result between the GNSS pose of the mobile device and the second particle pose corresponding to the second particle, specifically based on the GNSS pose of the mobile device
- the distance from the pose of the second particle is determined. The larger the distance, the smaller the probability of pose observation included in the pose measurement result.
- Step 3014 Based on the arrow landmark observation results and pose measurement results corresponding to the second particles, the prior weights corresponding to the second particles are updated to obtain the first weights corresponding to the second particles.
- the product of the prior weight of the second particle, the arrow observation probability, and the pose observation probability may be used as the first weight corresponding to the second particle.
- the first weight corresponding to the i-th second particle That is, it represents the prior weight of the i-th second particle sampled
- ⁇ means proportional
- exp() means exponential function
- Indicates the GNSS pose observed at time t express will Converted from UTM (Universal Transverse Mercator Grid System, Universal Transverse Mercator Grid System) to the map coordinate system
- (x, y) represents the translation component of the pose X.
- X refers to the i-th second particle
- the corresponding second particle pose means to calculate the matching score corresponding to the i-th second particle
- f ARROW () means to extract the arrow signpost from the semantically segmented image
- ⁇ (,X) means to assume that the pose of the origin of the self-coordinate system of the movable device is X In the case of , project the arrow onto the map coordinate system.
- X is the pose of the second particle corresponding to the i-th second particle d(I t ) represents the semantic segmentation of the image collected at time t, Represents an arrow element in the map.
- Step 3015 Determine a first longitudinal pose correction amount based on the first weights corresponding to each second particle.
- the first longitudinal pose correction amount may be obtained by means of density estimation.
- This disclosure obtains the vertical pose correction amount by longitudinally sampling M second particles, combining arrow observation and pose measurement, acting on each first particle, and then using Positioning at the next moment, so that the influence of the vertical correction at the current moment will be applied to the next moment, effectively realizing the decoupling of the horizontal and vertical corrections, and improving the accuracy of horizontal and vertical positioning.
- FIG. 5 is a schematic flowchart of step 203 provided by an exemplary embodiment of the present disclosure.
- step 203 may specifically include the following steps:
- Step 2031 based on the first predicted pose corresponding to each first particle, update the second weight corresponding to each first particle respectively, and obtain the third weight corresponding to each first particle;
- the second weight is the prior weight of the first particle.
- the prior weight of the first particle may be the weight after vertical correction at the previous moment, which may be specifically set according to actual requirements.
- the update of the second weight of a first particle it means that the first predicted pose of the first particle is used as the origin of the self-coordinate system of the mobile device to observe the lane line, and based on the observation result of the lane line, the The second weight of the first particle is updated to obtain the third weight corresponding to the first particle.
- Step 2032 Determine a first estimated pose of the mobile device based on the first predicted pose and the third weight respectively corresponding to each first particle.
- the first predicted poses are weighted and averaged, and the obtained results are used as the first estimated poses of the mobile device.
- the determination of the first estimated pose of the mobile device may also be determined based on some of the first particles whose third weight is greater than a certain threshold among the N first particles, for example, there are a total of 20 first particles , where the weights of two first particles are very small and are smaller than the minimum threshold, these two particles are not considered when performing weighted averaging, and the specific minimum threshold can be set according to actual needs.
- the present disclosure updates the weights of each first particle based on the predicted pose of lateral positioning prediction, and then performs weighted average of the first predicted poses of each first particle based on the updated weights to determine the pose of the mobile device. Since the particle swarm pose based on the horizontal positioning prediction is the vertically corrected pose at the previous moment, the horizontal prediction does not introduce vertical noise, so that the update of the particle swarm weights comprehensively considers the influence of vertical and horizontal noise, and there is no coupling between them. , so that the pose information of the mobile device obtained has high accuracy in both horizontal and vertical directions.
- FIG. 6 is a schematic flowchart of a method for determining pose information provided in another exemplary embodiment of the present disclosure.
- the method of the present disclosure further includes:
- Step 4011 based on the pose information of the mobile device at the previous moment and the odometer information at the current moment, determine the second predicted pose of the movable device at the current moment.
- the pose information of the mobile device at the previous moment refers to the estimated pose after lateral correction obtained by the processing flow at the previous moment, which is consistent with the first estimated pose at the current moment
- the odometer information includes the The translational distance and rotation angle during the time period from time to time, based on the pose information of the mobile device at the previous time and the odometer information at the current time, using the pre-acquired motion model, the current time Predict the pose of the mobile device to obtain the second predicted pose of the mobile device at the current moment.
- the motion model may adopt an odometer-based motion model (Odometry Sample Motion Model). Consistent with particle prediction, in the motion model adopted, the vertical noise parameter is 0, that is, no vertical noise is introduced, and only horizontal positioning prediction is performed.
- Step 4012 establish the first grid coordinate area based on the second predicted pose; the first coordinate axis of the first grid coordinate area is the lateral direction of the pose, and the second coordinate axis of the first grid coordinate area is the Heading direction.
- the size of the first grid coordinate area can be based on the abscissa and The heading angle is determined based on the principle that, for example, the two-dimensional coordinate points composed of the abscissa and heading angle of most of the first particles are in the first grid coordinate area, which can be set according to actual needs.
- the cell step sizes in the y and ⁇ directions are set as d y and d ⁇ respectively to establish the first grid coordinate area, and each cell uses [m, n] to represent its position, including the following situations:
- the coordinate range of each cell in the first grid coordinate area can be based on the specific conditions of the cell position [m, n], N y and N ⁇ , and the y and ⁇ directions of each cell
- the step size d y and d ⁇ are determined, and details will not be repeated here.
- Step 4013 based on the first predicted pose corresponding to each first particle, map each first particle to the first grid coordinate area, and obtain the cell to which each first particle belongs.
- the coordinates of each first particle in the cell are the transformation of the first predicted pose of the first particle relative to the prior pose of the first particle (that is, the pose after longitudinal correction at the previous moment) ,details as follows:
- the coordinates in the first grid coordinate area obtained by transformation also include three degrees of freedom, namely, the longitudinal coordinate c[0], the transverse coordinate c[1] and the heading angle coordinate c[2].
- mapping to the first grid coordinate area it is enough to map based on the horizontal coordinates and the heading angle coordinates.
- the inverse transformation of the above transformation is required to transform back to the self-coordinate system , so the coordinates of the three degrees of freedom need to be combined.
- FIG. 7 is a schematic diagram of a first grid coordinate area provided by an exemplary embodiment of the present disclosure.
- y represents the lateral direction of the mobile device in the self-coordinate system
- ⁇ represents the direction of the heading angle.
- Step 4014 based on the cell to which each first particle belongs, determine the third particle corresponding to each cell and the third particle pose corresponding to each third particle.
- determining the third particle corresponding to each cell means, based on the cell to which each first particle belongs, each cell is represented by a third particle, regardless of whether there is a first particle in the cell, it is converted into a third particle.
- the specific conversion method can be set according to actual needs. For example, for a cell with multiple first particles, multiple first particles can be merged, and the merged result can be used as the third particle corresponding to the cell. For cells without first particles cell, a particle can be supplemented as the third particle corresponding to the cell according to the distance between the cell and the center.
- the third particle pose of the only third particle in the merged cell is:
- the weight of the jth third particle obtained after merging is:
- K represents the number of particles contained in the j-th cell
- Indicates the weight of the kth particle distributed in the jth cell (here, the normalized weight value)
- Indicates the coordinates of the kth particle distributed in the jth cell in the grid coordinate system Indicates the second predicted pose of the mobile device at the current moment.
- the kth particle is the first particle with the first predicted pose, then Equal to the posterior weight of the kth first particle at the previous moment is the coordinates of the first grid coordinate area after the above transformation, if the kth particle is a supplementary particle, then is equal to the weight set for supplementary particles,
- the horizontal coordinates and heading angle coordinates in are the center coordinates of the cell to which the particle belongs,
- the vertical coordinate in is the initialization coordinate, and the specific initialization coordinate can be set according to actual needs, for example, it is set to 0, and there is no specific limitation.
- determining the first estimated pose of the mobile device based on the first predicted pose corresponding to each first particle in step 203 includes:
- Step 4015 Determine the first estimated pose of the movable device based on the poses of the third particles corresponding to the third particles.
- the third particle pose determines the first estimated pose of the movable device.
- this disclosure achieves resampling by gridding the first particles, so that the particle swarm is stable. Distributed in the first grid coordinate area, it can effectively solve the problem of unstable particle distribution.
- each first particle is mapped to the first grid coordinate area, and the cell to which each first particle belongs is obtained .
- the method can also include:
- step 4013 may specifically include mapping each new first particle to the first grid coordinate area based on the new first predicted pose corresponding to each new first particle, and obtaining the cell to which each new first particle belongs , the subsequent related steps are all processed based on the new first particle, and finally the third particle corresponding to each cell and the third particle pose corresponding to each third particle are obtained.
- the specific process is the same as the grid of the first particle above Similar to the above, the only difference is that the number of particles increases, so I won’t go into details here.
- the multi-sampling method for any first particle may be: different h new first particles corresponding to the first particle are obtained by introducing different random noises in each sampling.
- the positioning accuracy is improved on the basis of ensuring the stability of particle distribution.
- FIG. 8 is a schematic flowchart of a method for determining pose information provided by yet another exemplary embodiment of the present disclosure.
- the first prediction based on each first particle respectively corresponds to pose, mapping each first particle to the first grid coordinate area, and after obtaining the cell to which each first particle belongs, the disclosed method further includes:
- Step 4021 Delete the first particles mapped outside the coordinate area of the first grid to obtain the remaining first particles.
- Step 4022 add a fourth particle to the center of each cell in the first grid coordinate area, and each fourth particle corresponds to a fourth weight.
- step 4014 based on the cell to which each first particle belongs, determine the third particle corresponding to each cell and the third particle pose corresponding to each third particle, including:
- Step 40141 based on the cell to which each remaining first particle belongs, the second weight corresponding to each remaining first particle, and the fourth weight corresponding to each fourth particle, merge the particles in each cell to obtain each The third particles corresponding to the cells and the poses of the third particles respectively corresponding to the third particles.
- each fourth particle corresponds to a fourth weight
- the fourth weight can be set according to actual needs, for example, a small weight can be set for it, such as 1/(N y *N ⁇ )/100.
- the fourth particle pose corresponding to each fourth particle is the pose corresponding to the center of the corresponding cell.
- each cell After the fourth particle is added, each cell has a particle distribution, and then based on the remaining first particles and the added fourth particles, the cells are particleized, and each cell is converted into a corresponding third particle, Thus, N third particles are obtained, which are stably distributed in each cell.
- FIG. 9 is a schematic diagram of the principle of gridding-based particle swarm resampling provided by an exemplary embodiment of the present disclosure.
- each first particle is represented by a black dot
- the added fourth particle is represented by a white dot
- each third particle obtained by cell particleization is represented by a gray dot.
- the fourth particle only in the cell where no first particle falls, and not increase the other cells.
- a certain amount of The weight can be set according to actual needs.
- FIG. 10 is a schematic diagram of the principle of gridding-based particle swarm resampling provided by another exemplary embodiment of the present disclosure.
- 3 new first particles corresponding to each first particle are obtained through 5 samples, so that 3*N new first particles are mapped to the first grid coordinate area, and N is still represented by 20 as an example, the new first particles are represented by black dots, the added fourth particles are represented by white dots, and the third particles obtained by cell particleization are represented by gray dots.
- FIG. 11 is a schematic flowchart of step 3011 provided by an exemplary embodiment of the present disclosure.
- step 3011 may specifically include the following steps:
- Step 30111 take the first estimated pose as the center particle, sample H fifth particles forward and backward according to the first interval in the longitudinal direction, and obtain 2H fifth particles, and each fifth particle and the first estimated position
- the second interval of posture, H is a positive integer.
- Step 30112 Determine the pose of each fifth particle based on the first estimated pose and the second distance between each fifth particle and the first estimated pose.
- FIG. 12 is a schematic flowchart of step 3015 provided by an exemplary embodiment of the present disclosure.
- the first longitudinal position is determined based on the first weights corresponding to each second particle.
- Amount of posture correction including:
- Step 30151 based on the first weights corresponding to the second particles, determine the sum of the first weights of the second particles.
- Step 30152 Determine the vertical correction amount based on the sum of the first weights of each second particle, the first weights corresponding to each second particle, and the second distance between each second particle and the first estimated pose.
- the vertical correction amount d correct is expressed as:
- the first weight corresponding to the i-th second particle represents the sum of the first weights of each second particle
- d i represents the second distance between the i-th second particle and the first estimated pose.
- Step 30153 Determine the corresponding pose increment based on the longitudinal correction amount as the first longitudinal pose correction amount.
- the first longitudinal pose correction amount D correct obtained based on the above-mentioned longitudinal correction amount d correct is expressed as:
- the first predicted pose of each first particle is longitudinally corrected based on the first longitudinal pose correction amount, and the posterior position corresponding to each first particle at the current moment is obtained posture
- FIG. 13 is a schematic flowchart of step 2031 provided by an exemplary embodiment of the present disclosure.
- step 2031 based on the first predicted pose corresponding to each first particle, each first particle The second weights corresponding to each particle are updated to obtain the third weights corresponding to each first particle, including:
- Step 20311 obtain the lane line observation results corresponding to each first particle.
- the lane line observation result corresponding to a first particle includes the lane line observation probability (lane line observation probability for short) at the first predicted pose corresponding to the first particle.
- first particle uses the first predicted pose of the first particle as the origin of the self-coordinate system of the mobile device to observe the lane line, and compare the observed lane line with the lane in the map
- the line elements are transformed into the same coordinate system for matching. The higher the matching degree, the higher the score, the greater the probability of lane line observation, and the lower the matching degree, the lower the score, the lower the lane line observation probability.
- Step 20312 based on the lane line observation results corresponding to the first particles, update the second weights corresponding to the first particles respectively, to obtain the third weights corresponding to the first particles.
- ⁇ LANE represents the preset weight of lane lines
- max_dist represents the maximum distance threshold allowed for matching between observed lane lines and map lane line elements
- K positive integer indicates the number of lane lines.
- step 202 based on the first particle poses of each first particle, the lateral positioning prediction is performed on the pose of each first particle at the current moment, and the first prediction corresponding to each first particle is obtained pose, including:
- the first predicted pose corresponding to each first particle is obtained; the longitudinal noise parameter in the translation component in the motion model based on the odometer is 0.
- the odometry-based motion model is a probabilistic robot motion model implemented using relative motion information measured by the odometry of the mobile device, more specifically, within the time interval (t-1,t),
- the movable device advances from the pose x t-1 to the pose x t , and the odometer feeds back the relative progress during this period.
- the motion model based on the odometer expresses the motion of the movable device with three series of basic motions, namely Rotation, linear motion (i.e. translation) and another rotation, the following equation expresses the reading from the odometer (in, T stands for transpose) computes two rotation values and one translation value:
- ⁇ rot1 is a rotation value
- ⁇ trans is a translation value
- ⁇ rot2 is another rotation value
- atan2 is a function that returns an azimuth.
- parameters ⁇ 1 - ⁇ 4 are error parameters for a robot (referred to as a mobile device in this disclosure), which specify the cumulative error of motion.
- the longitudinal noise parameter ⁇ 3 is set to 0, and only lateral positioning is performed during prediction. Forecast without introducing vertical noise, decoupling horizontal and vertical observations.
- FIG. 14 is a schematic flowchart of a method for determining pose information provided by another exemplary embodiment of the present disclosure.
- the method includes:
- the pose of the first particle corresponding to the jth first particle is expressed as That is, the posterior pose after longitudinal correction at the previous moment.
- the first predicted pose corresponding to each first particle is obtained; the longitudinal noise in the translation component in the motion model based on the odometer The parameter is 0.
- the first predicted pose corresponding to the jth first particle is expressed as
- the first coordinate axis of the first grid coordinate area is the lateral direction of the pose
- the second coordinate axis of the first grid coordinate area is the heading of the pose angular direction.
- the coordinates of the jth first particle in the first grid coordinate area are expressed as
- a fourth particle is added to each cell center in the first grid coordinate area, and each fourth particle corresponds to a fourth weight.
- each remaining first particle belongs, the second weight corresponding to each remaining first particle, and the fourth weight corresponding to each fourth particle, merge the particles in each cell to obtain each unit
- the poses of the third particles corresponding to the jth third particles are expressed as That is, through gridded resampling, the particle swarm of the first particle is transformed into the particle swarm of the third particle:
- the original weights corresponding to the third particles are updated to obtain the updated weights corresponding to the third particles.
- the weight update principle of this step is similar to the specific operation of the aforementioned step 2031 (specifically, steps 20311-20312), that is, the third particle pose corresponding to the third particle is used as the origin of the self-coordinate system of the mobile device to observe the lane line, based on The lane line observation results are used to update the weights, and the details will not be repeated.
- the original weights corresponding to the third particles are the sum of the weights of all particles in the cell.
- the first estimated pose of the mobile device is used as pose information of the mobile device at the current moment.
- the first weight corresponding to the i-th second particle That is, it represents the prior weight of the i-th second particle sampled
- the vertical correction amount d correct is expressed as:
- the first weight corresponding to the i-th second particle represents the sum of the first weights of each second particle
- d i represents the second distance between the i-th second particle and the first estimated pose.
- the first longitudinal pose correction D correct obtained based on the above-mentioned longitudinal correction d correct is expressed as:
- the a posteriori pose at the current moment corresponding to each third particle is used as the a posteriori pose at the current moment corresponding to each first particle, and is applied to the next moment.
- FIG. 15 is a schematic diagram of the principle of horizontal and vertical correction of particle swarms provided by an exemplary embodiment of the present disclosure.
- the horizontal particle swarm is the particle swarm obtained by horizontal correction based on the particle swarm after the vertical correction at the previous moment.
- the horizontal correction can include the above steps 2-9.
- the obtained horizontal particle swarm is the third particle formation after the weight update Particle swarms, the vertical observation to determine the vertical correction amount includes the above steps 12-18, and the vertical correction amount acts on the horizontal particle swarm includes the above step 19, which will not be described in detail.
- Any method for determining pose information provided in the embodiments of the present disclosure may be executed by any appropriate device with data processing capabilities, including but not limited to: terminal devices, servers, and the like.
- the method for determining any pose information provided by the embodiments of the present disclosure may be executed by a processor, for example, the processor executes the determination of any pose information mentioned in the embodiments of the present disclosure by calling the corresponding instructions stored in the memory method. I won't go into details below.
- Fig. 16 is a schematic structural diagram of an apparatus for determining pose information provided by an exemplary embodiment of the present disclosure.
- the device of this embodiment can be used to implement the corresponding method embodiment of the present disclosure.
- the device shown in FIG. 16 is a schematic structural diagram of an apparatus for determining pose information provided by an exemplary embodiment of the present disclosure.
- the device of this embodiment can be used to implement the corresponding method embodiment of the present disclosure.
- the first acquisition module 501 is configured to acquire the first particle poses of the N first particles corresponding to the mobile device, where N is a positive integer greater than 1; the first particle poses are the corresponding first particles obtained at the previous moment The posterior pose after vertical correction; the first processing module 502 is used to perform horizontal processing on the pose of each first particle at the current moment based on the first particle pose of each first particle acquired by the first acquisition module 501, respectively.
- Positioning prediction obtaining the first predicted poses corresponding to the first particles; the second processing module 503, configured to determine the movable device based on the first predicted poses corresponding to the first particles obtained by the first processing module 502 The first estimated pose of the mobile device; the third processing module 504 is configured to use the first estimated pose of the mobile device obtained by the second processing module 503 as the pose information of the mobile device at the current moment.
- Fig. 17 is a schematic structural diagram of an apparatus for determining pose information provided by another exemplary embodiment of the present disclosure.
- the apparatus of the present disclosure further includes: a first vertical processing module 505 and a second vertical processing module 506 .
- the first longitudinal processing module 505 is configured to determine the first longitudinal pose correction amount based on the first estimated pose of the mobile device obtained by the second processing module 503;
- the second longitudinal processing module 506 is configured to determine the first longitudinal pose correction amount based on the first longitudinal processing
- the first longitudinal pose correction amount obtained by module 505 performs longitudinal correction on the first predicted pose of each first particle, and obtains the posterior pose and pose corresponding to each first particle at the current moment.
- Fig. 18 is a schematic structural diagram of a first vertical processing module 505 provided by an exemplary embodiment of the present disclosure.
- the first vertical processing module 505 includes: a first sampling unit 5051 , a first acquiring unit 5052 , a second acquiring unit 5053 , a first weight updating unit 5054 and a first determining unit 5055 .
- the first sampling unit 5051 is configured to take the first estimated pose obtained by the second processing module 503 as the center and perform sampling in the longitudinal direction to obtain the second particle poses of M second particles, where M is a positive integer greater than 1
- the first obtaining unit 5052 is used to obtain the prior weights corresponding to the second particles based on the Gaussian distribution;
- the second obtaining unit 5053 is used to obtain the arrow landmark observation results and the pose measurement results corresponding to the second particles respectively;
- the arrow landmark observation result corresponding to a second particle includes the observation probability of the arrow landmark under the second particle pose corresponding to the second particle;
- the pose measurement result corresponding to a second particle includes the second particle corresponding to the second particle
- Fig. 19 is a schematic structural diagram of a second processing module 503 provided by an exemplary embodiment of the present disclosure.
- the second processing module 503 includes: a second weight updating unit 5031 and a second determining unit 5032 .
- the second weight update unit 5031 is configured to update the second weights corresponding to the first particles based on the first predicted poses corresponding to the first particles obtained by the first processing module 502, and obtain the first particles respectively The corresponding third weight; the second weight corresponding to a first particle is the prior weight of the first particle; the second determination unit 5032 is configured to obtain the first particle corresponding to each first particle based on the first processing module 502
- the predicted pose and second weight updating unit 5031 obtains the third weight corresponding to each first particle to determine the first estimated pose of the mobile device.
- Fig. 20 is a schematic structural diagram of an apparatus for determining pose information provided by yet another exemplary embodiment of the present disclosure.
- the apparatus of the present disclosure further includes: a first determining module 507 , a first establishing module 508 , a first mapping module 509 and a second determining module 510 .
- the first determination module 507 is used to determine the second predicted pose of the mobile device at the current moment based on the pose information of the movable device at the previous moment and the odometer information at the current moment;
- the first establishment module 508 is used to determine the second predicted pose of the movable device based on The second predicted pose obtained by the first determination module 507 establishes a first grid coordinate area;
- the first coordinate axis of the first grid coordinate area is the lateral direction of the pose, and the second coordinate axis of the first grid coordinate area is The heading angle direction of the pose;
- the first mapping module 509 is configured to
- the apparatus of the present disclosure further includes: a first screening module 511 and a first supplementary module 512 .
- the first screening module 511 is used to delete the first particles mapped outside the first grid coordinate area to obtain the remaining first particles; the first supplementary module 512 is used for each cell in the first grid coordinate area A fourth particle is added to the center respectively, and each fourth particle corresponds to a fourth weight.
- the second determining module 510 is specifically configured to: based on the cell to which each remaining first particle belongs, the second weight corresponding to each remaining first particle, and the fourth weight corresponding to each fourth particle, divide each The particles in the cells are merged to obtain the third particles corresponding to each cell and the poses of the third particles respectively corresponding to the third particles.
- the device of the present disclosure may further include: a resampling module 513, configured to perform multiple samplings for each first particle, for example, h times of sampling, and finally h*N new th particles can be obtained New first predicted poses corresponding to a particle and each new first particle respectively.
- the first mapping module 509 is specifically configured to: map each new first particle to the first grid coordinate area based on the new first predicted pose corresponding to each new first particle, and obtain each new first particle The cell it belongs to.
- the second determination module 510 is specifically configured to determine the third particle corresponding to each cell and the third particle pose corresponding to each third particle based on the cell to which each new first particle belongs.
- the first sampling unit 5051 is specifically configured to: take the first estimated pose as the center particle, sample H fifth particles forward and backward according to the first interval in the longitudinal direction, and obtain 2H
- the first determining unit 5055 is specifically configured to: determine the sum of the first weights of each second particle based on the first weights corresponding to each second particle; The sum, the first weight corresponding to each second particle, and the second interval between each second particle and the first estimated pose determine the vertical correction amount; determine the corresponding pose increment based on the vertical correction amount as the first Amount of vertical pose correction.
- the second weight updating unit 5031 is specifically configured to: acquire the lane line observation results corresponding to each first particle; the lane line observation result corresponding to a first particle is included in the first particle corresponding to the first particle Predicting the observation probability of the lane line in the pose; based on the lane line observation results corresponding to the first particles, updating the second weights corresponding to the first particles respectively to obtain the third weights corresponding to the first particles.
- the first processing module 502 is specifically configured to: obtain the first predicted position corresponding to each first particle respectively based on the first particle pose of each first particle and the motion model based on the odometer attitude; the longitudinal noise parameter in the translation component in the odometry-based motion model is 0.
- An embodiment of the present disclosure also provides an electronic device, including: a memory configured to store a computer program;
- the processor is configured to execute the computer program stored in the memory, and when the computer program is executed, implement the method for determining pose information described in any one of the above-mentioned embodiments of the present disclosure.
- Fig. 21 is a schematic structural diagram of an application embodiment of the electronic device of the present disclosure.
- the electronic device 10 includes one or more processors 11 and memory 12 .
- Processor 11 may be a central processing unit (CPU) or other form of processing unit having data processing capabilities and/or instruction execution capabilities, and may control other components in electronic device 10 to perform desired functions.
- CPU central processing unit
- Processor 11 may control other components in electronic device 10 to perform desired functions.
- Memory 12 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and/or non-volatile memory.
- the volatile memory may include, for example, random access memory (RAM) and/or cache memory (cache).
- the non-volatile memory may include, for example, a read-only memory (ROM), a hard disk, a flash memory, and the like.
- One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 11 may execute the program instructions to implement the methods for determining pose information and the methods for determining pose information in various embodiments of the present disclosure described above. / or other desired functionality.
- Various contents such as input signal, signal component, noise component, etc. may also be stored in the computer-readable storage medium.
- 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 be the above-mentioned microphone or microphone array, which is used to capture the input signal of the sound source.
- the input device 13 may also include, for example, a keyboard, a mouse, and the like.
- the output device 14 can output various information to the outside, including determined distance information, direction information, and the like.
- the output device 14 may include, for example, a display, a speaker, a printer, a communication network and its connected remote output devices, and the like.
- the electronic device 10 may also include any other suitable components.
- embodiments of the present disclosure may also be computer program products, which include computer program instructions that, when executed by a processor, cause the processor to perform the above-mentioned "exemplary method" of this specification. Steps in methods according to various embodiments of the present disclosure described in section.
- the computer program product can be written in any combination of one or more programming languages to execute the program codes for performing the operations of the embodiments of the present disclosure, and the programming languages include object-oriented programming languages, such as Java, C++, etc. , also includes conventional procedural programming languages, such as the "C" language or similar programming languages.
- the program code may execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device, or entirely on the remote computing device or server to execute.
- embodiments of the present disclosure may also be a computer-readable storage medium, on which computer program instructions are stored, and the computer program instructions, when executed by a processor, cause the processor to execute the above-mentioned "Exemplary Method" section of this specification. Steps in methods according to various embodiments of the present disclosure described in .
- the computer readable storage medium may employ any combination of one or more readable media.
- the readable medium may be a readable signal medium or a readable storage medium.
- a readable storage medium may include, but is not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or device, or any combination thereof, for example. More specific examples (non-exhaustive list) of readable storage media include: electrical connection with one or more conductors, portable disk, hard disk, random access memory (RAM), read only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.
- the methods and apparatus of the present disclosure may be implemented in many ways.
- the methods and apparatuses of the present disclosure may be implemented by software, hardware, firmware or any combination of software, hardware, and firmware.
- the above sequence of steps for the method is for illustration only, and the steps of the method of the present disclosure are not limited to the sequence specifically described above unless specifically stated otherwise.
- the present disclosure can also be implemented as programs recorded in recording media, the programs including machine-readable instructions for realizing the method according to the present disclosure.
- the present disclosure also covers a recording medium storing a program for executing the method according to the present disclosure.
- each component or each step can be decomposed and/or reassembled. These decompositions and/or recombinations should be considered equivalents of the present disclosure.
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- General Health & Medical Sciences (AREA)
- General Engineering & Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Evolutionary Computation (AREA)
- Biophysics (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- Computational Linguistics (AREA)
- Biomedical Technology (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Health & Medical Sciences (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种位姿信息的确定方法,方法包括:获取可移动设备对应的N个第一粒子的第一粒子位姿,N为大于1的正整数,第一粒子位姿是前一时刻获得的对应第一粒子的纵向矫正后的后验位姿(201);分别基于各第一粒子的第一粒子位姿,对各第一粒子的当前时刻的位姿进行横向定位预测,获得各第一粒子分别对应的第一预测位姿(202);基于各第一粒子分别对应的第一预测位姿,确定可移动设备的第一估计位姿(203);将可移动设备的第一估计位姿作为可移动设备当前时刻的位姿信息(204)。该方法可以实现横向矫正和纵向矫正的解耦,有效提高定位精度。还提供了一种位姿信息的确定装置、电子设备和存储介质。
Description
本公开要求在2022年1月6日提交的、申请号为202210013207.7、发明名称为“位姿信息的确定方法和装置、电子设备和存储介质”的中国专利申请的优先权,其全部内容通过引用结合在本公开中。
本公开涉及可移动设备技术,尤其是一种位姿信息的确定方法、装置、电子设备和存储介质。
在基于视觉观测和高精度地图对可移动设备(比如车辆)进行高精度定位时,通常采用GNSS(Global Navigation Satellite System,全球导航卫星***)确定可移动设备在地图中的全局位姿(包括位置和姿态),进而基于预测-矫正-预测-矫正的重复过程进行精准的位置跟踪。在位置跟踪中,通常采用粒子滤波算法实现,但是,现有的基于粒子滤波算法的位姿确定方法存在横向和纵向观测耦合的情况,容易导致车道线和箭头路标定位冲突,导致定位精度较低。
发明内容
为了解决上述横向和纵向观测耦合的技术问题,提出了本公开。本公开的实施例提供了一种位姿信息的确定方法、装置、电子设备和存储介质。
根据本公开实施例的一个方面,提供了一种位姿信息的确定方法,包括:获取可移动设备对应的N个第一粒子的第一粒子位姿,N为大于1的正整数;第一粒子位姿是前一时刻获得的对应第一粒子的纵向矫正后的后验位姿;分别基于各第一粒子的第一粒子位姿,对各第一粒子的当前时刻的位姿进行横向定位预测,获得各第一粒子分别对应的第一预测位姿;基于各第一粒子分别对应的第一预测位姿,确定可移动设备的第一估计位姿;将可移动设备的第一估计位姿作为可移动设备当前时刻的位姿信息。
根据本公开实施例的另一个方面,提供了一种位姿信息的确定装置,包括:第一获取模块,用于获取可移动设备对应的N个第一粒子的第一粒子位姿,N为大于1的正整数;第一粒子位姿是前一时刻获得的对应第一粒子的纵向矫正后的后验位姿;第一处理模块,用于分别基于各第一粒子的第一粒子位姿,对各第一粒子的当前时刻的位姿进行横向定位预测,获得各第一粒子分别对应的第一预测位姿;第二处理模块,用于基于各第一粒子分别对应的第一预测位姿,确定可移动设备的第一估计位姿;第三处理模块,用于将可移动设备的第一估计位姿作为可移动设备当前时刻的位姿信息。
根据本公开实施例的再一方面,提供一种计算机可读存储介质,存储介质存储有计算机程序,计算机程序用于执行本公开上述任一实施例的位姿信息的确定方法。
根据本公开实施例的又一方面,提供一种电子设备,电子设备包括:处理器;用于存储处理器可执行
指令的存储器;处理器,用于从存储器中读取可执行指令,并执行指令以实现本公开上述任一实施例的位姿信息的确定方法。
基于本公开上述实施例提供的位姿信息的确定方法和装置、电子设备和存储介质,先进行横向定位预测获得粒子预测位姿,进而基于粒子预测位姿估计可移动设备的位姿,纵向上则基于估计的可移动设备的位姿确定纵向位姿矫正量,用于对横向定位预测中的粒子进行纵向矫正,纵向矫正的影响在下一时刻引入,则当前时刻引入的是前一时刻的纵向矫正影响,由于横向定位预测过程中基于前一时刻的纵向矫正后的粒子进行横向定位预测,而不引入纵向的噪声,避免粒子在纵向上的分布,使得各粒子只在横向上存在区别,纵向矫正在后续执行,实现了横向矫正和纵向矫正的解耦合,有效解决了现有技术横向和纵向观测耦合,导致定位精度较低的问题。
下面通过附图和实施例,对本公开的技术方案做进一步的详细描述。
通过结合附图对本公开实施例进行更详细的描述,本公开的上述以及其他目的、特征和优势将变得更加明显。附图用来提供对本公开实施例的进一步理解,并且构成说明书的一部分,与本公开实施例一起用于解释本公开,并不构成对本公开的限制。在附图中,相同的参考标号通常代表相同部件或步骤。
图1是本公开提供的位姿信息的确定方法的一个示例性的应用场景;
图2是本公开一示例性实施例提供的位姿信息的确定方法的流程示意图;
图3是本公开另一个示例性实施例提供的位姿信息的确定方法的流程示意图;
图4是本公开一个示例性实施例提供的步骤301的流程示意图;
图5是本公开一个示例性实施例提供的步骤203的流程示意图;
图6是本公开再一个示例性实施例提供的位姿信息的确定方法的流程示意图;
图7是本公开一示例性实施例提供的第一网格坐标区域的示意图;
图8是本公开又一个示例性实施例提供的位姿信息的确定方法的流程示意图;
图9是本公开一示例性实施例提供的基于网格化的粒子群重采样的原理示意图;
图10本公开另一示例性实施例提供的基于网格化的粒子群重采样的原理示意图;
图11是本公开一个示例性实施例提供的步骤3011的流程示意图;
图12是本公开一个示例性实施例提供的步骤3015的流程示意图;
图13是本公开一个示例性实施例提供的步骤2031的流程示意图;
图14是本公开又一示例性实施例提供的位姿信息的确定方法的流程示意图;
图15是本公开一示例性实施例提供的粒子群横向和纵向矫正原理示意图;
图16是本公开一示例性实施例提供的位姿信息的确定装置的结构示意图;
图17是本公开另一示例性实施例提供的位姿信息的确定装置的结构示意图;
图18是本公开一示例性实施例提供的第一纵向处理模块505的结构示意图;
图19是本公开一示例性实施例提供的第二处理模块503的结构示意图;
图20是本公开再一示例性实施例提供的位姿信息的确定装置的结构示意图;
图21是本公开电子设备一个应用实施例的结构示意图。
下面,将参考附图详细地描述根据本公开的示例实施例。显然,所描述的实施例仅仅是本公开的一部分实施例,而不是本公开的全部实施例,应理解,本公开不受这里描述的示例实施例的限制。
应注意到:除非另外具体说明,否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本公开的范围。
本领域技术人员可以理解,本公开实施例中的“第一”、“第二”等术语仅用于区别不同步骤、设备或模块等,既不代表任何特定技术含义,也不表示它们之间的必然逻辑顺序。
还应理解,在本公开实施例中,“多个”可以指两个或两个以上,“至少一个”可以指一个、两个或两个以上。
还应理解,对于本公开实施例中提及的任一部件、数据或结构,在没有明确限定或者在前后文给出相反启示的情况下,一般可以理解为一个或多个。
另外,本公开中术语“和/或”,仅仅是一种描述关联对象的关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。另外,本公开中字符“/”,一般表示前后关联对象是一种“或”的关系。
还应理解,本公开对各个实施例的描述着重强调各个实施例之间的不同之处,其相同或相似之处可以相互参考,为了简洁,不再一一赘述。
同时,应当明白,为了便于描述,附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。
以下对至少一个示例性实施例的描述实际上仅仅是说明性的,决不作为对本公开及其应用或使用的任何限制。
对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论,但在适当情况下,所述技术、方法和设备应当被视为说明书的一部分。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步讨论。
本公开实施例可以应用于终端设备、计算机***、服务器等电子设备,其可与众多其它通用或专用计算***环境或配置一起操作。适于与终端设备、计算机***、服务器等电子设备一起使用的众所周知的终端设备、计算***、环境和/或配置的例子包括但不限于:个人计算机***、服务器计算机***、瘦客户机、厚客户机、手持或膝上设备、基于微处理器的***、机顶盒、可编程消费电子产品、网络个人电脑、小型 计算机***﹑大型计算机***和包括上述任何***的分布式云计算技术环境,等等。
终端设备、计算机***、服务器等电子设备可以在由计算机***执行的计算机***可执行指令(诸如程序模块)的一般语境下描述。通常,程序模块可以包括例程、程序、目标程序、组件、逻辑、数据结构等等,它们执行特定的任务或者实现特定的抽象数据类型。计算机***/服务器可以在分布式云计算环境中实施,分布式云计算环境中,任务是由通过通信网络链接的远程处理设备执行的。在分布式云计算环境中,程序模块可以位于包括存储设备的本地或远程计算***存储介质上。
本公开概述
在实现本公开的过程中,发明人发现,在基于视觉观测和高精度地图对可移动设备(比如车辆)进行高精度定位时,通常采用粒子滤波算法实现可移动设备位姿的确定,但是,现有的基于粒子滤波算法的位姿确定方法,在预测时,基于的运动模型会引入平移分量的噪声和旋转噪声,以表征运动的累计误差,平移分量的噪声包括横向噪声和纵向噪声,从而导致横向和纵向观测耦合的情况,容易导致车道线和箭头路标定位冲突,导致定位精度较低。
示例性概述
图1是本公开提供的位姿信息的确定方法的一个示例性的应用场景。
针对可移动设备的高精度定位场景,以车辆为例,在需要对车辆进行高精度定位时,需要首先确定车辆在导航地图中的初始位姿,进而基于初始位姿在车辆后续运动中进行高精度定位,在运动过程中,利用本公开实施例提供的位姿信息的确定方法,可以将横向位姿矫正和纵向位姿矫正进行解耦合,在当前时刻,基于前一时刻纵向矫正的粒子进行横向定位预测,基于横向矫正后的粒子位姿,确定车辆的当前时刻的位姿,同时在当前时刻对横向矫正后的粒子位姿进行纵向矫正后作为下一时刻的粒子,从而避免横向和纵向观测耦合的情况,提高位姿的定位精度。图1中,x、y分别表示车辆自坐标系的x轴和y轴,y方向为横向,x方向为纵向。在对车辆进行定位时,需要结合车道线和箭头地标的观测结果,观测结果是指通过摄像头采集环境图像识别的车道线和箭头地标与地图中的车道线与箭头的匹配结果。其中,车道线主要影响横向的矫正,箭头主要影响纵向的矫正,本公开实施例将车道线的观测与箭头的观测分别用于横向矫正和纵向矫正,从而将横向观测和纵向观测解耦,避免为了箭头观测更好导致让车道线观测变差,或者为了车道线观测更好导致箭头观测变差的情况发生。本公开实施例中,位姿是包括横向坐标分量y、纵向坐标分量x和航向角θ三个自由度。
示例性方法
图2是本公开一示例性实施例提供的位姿信息的确定方法的流程示意图。本实施例可应用在电子设备上,具体比如车载计算平台上,如图2所示,包括如下步骤:
步骤201,获取可移动设备对应的N个第一粒子的第一粒子位姿,N为大于1的正整数;第一粒子位 姿是前一时刻获得的对应第一粒子的纵向矫正后的后验位姿。
其中,可移动设备可以是车辆、机器人等任意需要进行定位的可移动设备。N个第一粒子的第一粒子位姿可以是在前一时刻纵向矫正后的N个第一粒子的位姿。
步骤202,分别基于各第一粒子的第一粒子位姿,对各第一粒子的当前时刻的位姿进行横向定位预测,获得各第一粒子分别对应的第一预测位姿。
其中,横向定位预测是指不考虑纵向的矫正,也即在预测过程中,运动模型中不引入平移分量的纵向噪声。运动模型可以采用基于里程计的运动模型(Odometry Sample Motion Model)或其他可实施的运动模型,本公开不做限定。
示例性的,以一个第一粒子为例,该第一粒子的第一粒子位姿为原位姿,基于里程计信息可以确定在前一时刻到当前时刻这一时段内的平移距离和旋转角度,将原位姿、及该平移距离和旋转角度输入运动模型,即可输出该第一粒子对应的第一预测位姿。
步骤203,基于各第一粒子分别对应的第一预测位姿,确定可移动设备的第一估计位姿。
在获得各第一粒子分别对应的第一预测位姿后,则可以基于各第一粒子分别对应的第一预测位姿来确定可移动设备的第一估计位姿,具体来说,需要基于各第一粒子分别对应的权重,对各第一预测位姿进行加权平均,作为可移动设备的第一估计位姿,其中,加权平均采用的各第一粒子分别对应的权重,可以是基于一定规则进行更新后的权重。
步骤204,将可移动设备的第一估计位姿作为可移动设备当前时刻的位姿信息。
由于当前时刻是基于前一时刻纵向矫正后的粒子群来进行横向矫正的,因此当前时刻的矫正综合考虑了纵向矫正和横向矫正的影响,第一估计位姿在横向和纵向上均具有较高的精度,将第一估计位姿作为可移动设备当前时刻在地图中的位姿信息,可实现对可移动设备的高精度定位。
本公开实施例中,每个时刻的纵向矫正是在横向矫正之后进行的,可以理解地,横向矫正和纵向矫正是随着时间推移的循环迭代的过程,在前一时刻同样存在上述步骤相关的操作,在前一时刻的步骤203获得可移动设备的第一估计位姿后,基于第一估计位姿对各粒子进行纵向矫正,获得纵向矫正后的位姿,作为当前时刻获取的上述N个第一粒子的第一粒子位姿。
本实施例提供的位姿信息的确定方法,在当前时刻,基于前一时刻纵向矫正后的粒子群,先进行横向定位预测获得粒子预测位姿,进而基于粒子预测位姿估计可移动设备的位姿,对于纵向上,当前时刻引入的是前一时刻的纵向矫正影响,由于横向定位预测过程中基于前一时刻的纵向矫正后的粒子进行横向定位预测,而不引入纵向的噪声,避免粒子在纵向上的分布,使得各粒子只在横向上存在区别,纵向矫正在后续执行,实现了横向矫正和纵向矫正的解耦合,有效解决了现有技术横向和纵向观测耦合,导致定位精度较低的问题。
在一个可选示例中,图3是本公开另一个示例性实施例提供的位姿信息的确定方法的流程示意图,本示例中,在步骤203的基于各第一粒子分别对应的第一预测位姿,确定可移动设备的第一估计位姿之后, 本公开的方法还包括:
步骤301,基于可移动设备的第一估计位姿,确定第一纵向位姿矫正量。
步骤302,基于第一纵向位姿矫正量对各第一粒子的第一预测位姿进行纵向矫正,获得各第一粒子分别对应的当前时刻的后验位姿。
具体的,在当前时刻进行横向矫正后,基于可移动设备的第一估计位姿来确定用于纵向矫正的第一纵向位姿矫正量,以对各第一粒子分别对应的第一预测位姿进行纵向矫正,作为各第一粒子分别对应的当前时刻的后验位姿,以作为下一时刻处理流程的基础,或者进行重采样后作为下一时刻处理流程的基础,从而将当前时刻的纵向矫正的影响在下一时刻引入。
本公开通过基于当前时刻横向矫正后的粒子群的位姿确定可移动设备的当前时刻的位姿,同时对横向矫正后的粒子群进行纵向矫正,将纵向矫正的影响在下一时刻起作用,在实现横向和纵向解耦的情况下,避免当前时刻的纵向矫正降低横向的定位精度,以保证横向定位精度。
在一个可选示例中,图4是本公开一个示例性实施例提供的步骤301的流程示意图,在本示例中,步骤301具体可以包括以下步骤:
步骤3011,以第一估计位姿为中心,在纵向上进行采样,获得M个第二粒子的第二粒子位姿,M为大于1的正整数。
具体的,可以以第一估计位姿为中心,在纵向上按照固定间隔分别向前和向后采样一定的粒子,从而获得包括中心的M个第二粒子的第二粒子位姿。具体间隔可以根据实际需求设置,本公开实施例不做限定。
步骤3012,基于高斯分布获取各第二粒子分别对应的先验权重。
示例性的,以第一估计位姿为中心建立一个高斯分布,来确定每个第二粒子分别对应的先验权重,具体如下:
其中,
表示采样的第i个第二粒子的先验权重,i=-H,-(H-1),…,0,1,…,H,H为正整数,M=2H+1,x表示第i个第二粒子与中心的纵向距离,dx表示采样间隔,mean表示均值,σ表示方差。
可选地,以第一估计位姿为中心向前采样的粒子数量与向后采样的粒子数量也可以不同,比如,i=-S,-(S-1),…,0,1,…,H,S、H均为正整数,M=S+H+1,具体可以根据实际需求设置,本公开不做限定。
步骤3013,获取各第二粒子分别对应的箭头路标观测结果和位姿测量结果。
其中,一个第二粒子对应的箭头路标观测结果包括在该第二粒子对应的第二粒子位姿下箭头路标的观测概率(简称箭头观测概率);一个第二粒子对应的位姿测量结果包括在该第二粒子对应的第二粒子位姿下可移动设备位姿的观测概率(简称位姿观测概率)。以一个第二粒子对应的箭头路标观测结果为例,是将该第二粒子对应的第二粒子位姿作为可移动设备的自坐标系的原点,将图像识别获得的可移动设备自坐 标系下的箭头路标转换到地图坐标系,或者将地图中的箭头元素转换到可移动设备的自坐标系下,即将观测到的箭头路标与地图的箭头元素在同一坐标系下进行匹配,基于匹配结果获得该第二粒子对应的箭头路标观测结果,匹配度越好得分越高则箭头路标的观测概率越大,具体得分计算函数可以根据实际需求设置,本公开不做限定。对于该第二粒子对应的位姿测量结果,是基于可移动设备的GNSS位姿与该第二粒子对应的第二粒子位姿的匹配结果来确定的,具体是基于可移动设备的GNSS位姿与该第二粒子位姿的距离来确定,距离越大则位姿测量结果包括的位姿观测概率越小。
步骤3014,基于各第二粒子分别对应的箭头路标观测结果和位姿测量结果,对各第二粒子分别对应的先验权重进行更新,获得各第二粒子分别对应的第一权重。
可选地,对于一个第二粒子,可以将该第二粒子的先验权重、与箭头观测概率和位姿观测概率的乘积作为该第二粒子对应的第一权重。
其中,
即表示第i个第二粒子对应的第一权重,
即表示采样的第i个第二粒子的先验权重,
表示第i个第二粒子对应的位姿观测概率,
表示第i个第二粒子对应的箭头观测概率,i=-H,-(H-1),…,0,1,…,H,H为正整数,M=2H+1。
其中,∝表示成比例,exp()表示指数函数,
表示t时刻观测的GNSS位姿,
表示将
从UTM(Universal Transverse Mercator Grid System,通用横墨卡托格网***)转换到地图坐标系下,(x,y)表示位姿X的平移分量,本示例中X即指第i个第二粒子对应的第二粒子位姿
表示计算第i个第二粒子对应的匹配得分,f
ARROW()表示从语义分割后的图像中提取出箭头路标,π(,X)表示在假定可移动设备的自坐标系原点位姿为X的情况下,把箭头投影到地图坐标系下,本示例中,X即为第i个第二粒子对应的第二粒子位姿
d(I
t)表示对t时刻采集的图像进行语义分割,
表示地图中的箭头元素。
步骤3015,基于各第二粒子分别对应的第一权重,确定第一纵向位姿矫正量。
可选地,可以基于各第二粒子分别对应的第一权重,采用密度估计方式得到第一纵向位姿矫正量。
本公开通过纵向采样M个第二粒子,结合箭头观测和位姿测量获得纵向位姿矫正量,作用于各第一粒子,对各第一粒子分别对应的第一预测位姿进行纵向矫正后用于下一时刻的定位,从而将当前时刻纵向矫正的影响作用于下一时刻,有效实现了横向和纵向矫正的解耦合,提高横向和纵向定位精度。
在一个可选示例中,图5是本公开一个示例性实施例提供的步骤203的流程示意图,在本示例中,步骤203具体可以包括以下步骤:
步骤2031,基于各第一粒子分别对应的第一预测位姿,对各第一粒子分别对应的第二权重进行更新, 获得各第一粒子分别对应的第三权重;一个第一粒子对应的第二权重为该第一粒子的先验权重。
其中,第一粒子的先验权重可以是在前一时刻纵向矫正后的权重,具体可以根据实际需求设置。以一个第一粒子的第二权重的更新为例,是指将该第一粒子的第一预测位姿作为可移动设备的自坐标系的原点对车道线进行观测,基于车道线观测结果对该第一粒子的第二权重进行更新,获得该第一粒子对应的第三权重。
步骤2032,基于各第一粒子分别对应的第一预测位姿及第三权重,确定可移动设备的第一估计位姿。
具体的,基于各第一粒子分别对应的第三权重对各第一预测位姿进行加权平均,将获得的结果作为可移动设备的第一估计位姿。
可选地,也可以是先对各第一粒子分别对应的第三权重进行归一化,进而基于归一化权重对各第一预测位姿进行加权即可。
在一个可选示例中,可移动设备的第一估计位姿的确定还可以是基于N个第一粒子中第三权重大于一定阈值的部分第一粒子来确定,比如总共有20个第一粒子,其中有两个第一粒子的权重非常小,小于了最小阈值,则在进行加权平均时,则不考虑这两个粒子,具体最小阈值可以根据实际需求设置。
本公开基于横向定位预测的预测位姿实现对各第一粒子的权重的更新,进而基于更新后的权重对各第一粒子的第一预测位姿进行加权平均,确定可移动设备的位姿,由于横向定位预测基于的粒子群位姿是前一时刻纵向矫正后的位姿,横向预测未引入纵向噪声,从而使得粒子群权重的更新综合考虑了纵向和横向噪声影响,且相互不会产生耦合,从而获得的可移动设备的位姿信息在横向和纵向上都具有较高的精度。
在一个可选示例中,图6是本公开再一个示例性实施例提供的位姿信息的确定方法的流程示意图,在本示例中,在步骤202之后,本公开的方法还包括:
步骤4011,基于可移动设备前一时刻的位姿信息、及当前时刻的里程计信息,确定可移动设备当前时刻的第二预测位姿。
其中,可移动设备前一时刻的位姿信息是指前一时刻处理流程获得的横向矫正后的估计位姿,与当前时刻的第一估计位姿一致,里程计信息包括了可移动设备从前一时刻到当前时刻这一时段内的平移距离和旋转角度,基于可移动设备前一时刻的位姿信息、及当前时刻的里程计信息,采用预先获得的运动模型,即可对可移动设备当前时刻的位姿进行预测,获得可移动设备当前时刻的第二预测位姿。其中,运动模型可以采用基于里程计的运动模型(Odometry Sample Motion Model)。与粒子预测一致,采用的运动模型中,纵向噪声参数为0,即不引入纵向噪声,只进行横向定位预测。
步骤4012,基于第二预测位姿建立第一网格坐标区域;第一网格坐标区域的第一坐标轴为位姿的横向方向,第一网格坐标区域的第二坐标轴为位姿的航向角方向。
其中,第一网格坐标区域包括N个单元格,N=N
y*N
θ,N
y和N
θ分别表示第一网格坐标区域在横向方向和航向角方向的单元格数量。
其中,将第二预测位姿的横坐标和航向角作为第一网格坐标区域的中心,第一网格坐标区域的大小可以根据各第一粒子分别对应的第一预测位姿的横坐标和航向角来确定,比如以大部分的第一粒子的横坐标和航向角组成的二维坐标点在第一网格坐标区域中为原则,具体可以根据实际需求设置。
示例性的,设置y和θ方向的单元格步长分别为d
y和d
θ建立第一网格坐标区域,每个单元格用[m,n]表示其位置,包括以下情况:
若N
y和N
θ一个是奇数,另一个是偶数,则m,n根据上述两种情况来确定,具体不再赘述。
相应的,每个单元格在第一网格坐标区域中的坐标范围可以根据该单元格位置[m,n]、N
y和N
θ的具体情况、及每个单元格的y和θ方向的步长d
y和d
θ来确定,具体不再赘述。
比如,当N
y和N
θ均为偶数时,单元格[m,n]=[1,2],则该单元格[m,n]的坐标范围为:
y向:[d
y*(m-1),d
y*m]=[0,d
y]
θ向:[d
θ*(n-1),d
θ*n]=[d
θ,2d
θ]
步骤4013,基于各第一粒子分别对应的第一预测位姿,将各第一粒子映射到第一网格坐标区域中,获得各第一粒子所属的单元格。
具体的,每个第一粒子在单元格中的坐标为该第一粒子的第一预测位姿相对于该第一粒子的先验位姿(即前一时刻纵向矫正后的位姿)的变换,具体如下:
其中,
即表示第j个第一粒子在第一网格坐标区域中的坐标,
表示当前时刻(即t时刻)可移动设备的先验位姿,也即基于运动模型对可移动设备当前时刻的位姿进行预测获得的上述第二预测位姿,
表示第j个第一粒子当前时刻的第一预测位姿
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]
可知,变换获得的第一网格坐标区域中的坐标同样包括3个自由度,即纵向坐标c[0]、横向坐标c[1]和航向角坐标c[2],在将各第一粒子映射到第一网格坐标区域中时,基于横向坐标和航向角坐标进行映射即可,在合并单元格中的粒子时,由于后续在合并后需要按照上述变换的逆变换,变换回自坐标系下,因此需要合并三个自由度的坐标。
示例性的,图7是本公开一示例性实施例提供的第一网格坐标区域的示意图。其中,y表示可移动设备自坐标系下的横向方向,θ表示航向角方向。以N=24、N
y=6、N
θ=4为例,也即粒子群包括20个第一粒子(图中黑色圆点),映射到第一网格坐标区域中,映射结果是,可能部分单元格内落入了一个或多个第一粒子,而另一部分单元格则没有第一粒子落入,还有可能有的第一粒子落到了第一网格坐标区域外。
步骤4014,基于各第一粒子所属的单元格,确定各单元格分别对应的第三粒子、及各第三粒子分别对应的第三粒子位姿。
其中,确定各单元格分别对应的第三粒子是指,基于各第一粒子所属的单元格将每个单元分别用一个第三粒子表示,无论单元格中有没有第一粒子,都转换成一个第三粒子。具体转换方式可以根据实际需求设置,比如对于有多个第一粒子的单元格,可以将多个第一粒子进行合并,合并结果作为该单元格对应的第三粒子,对于没有第一粒子的单元格,可以根据单元格与中心的距离补充一个粒子作为该单元格对应的第三粒子。
示例性的,合并后单元格内唯一的第三粒子的第三粒子位姿为:
合并后获得的第j个第三粒子的权重为:
其中,
表示第j个单元格粒子化后对应的第三粒子的第三粒子位姿,K表示第j个单元格内包含的粒子数量,
表示第j个单元格内的粒子合并后在网格坐标系下的坐标,
表示分布在第j个单元格内的第k个粒子的权重(这里为归一化后的权重值),
表示分布在第j个单元格内的第k个粒子在网格坐标系下的坐标,
表示移动设备当前时刻的第二预测位姿。若第k个粒子是上述具有第一预测位姿的第一粒子,则
等于第k个第一粒子在前一时刻的后验权重
为上述变换后的第一网格坐标区域的坐标,若第k个粒子是补充的粒子,则
等于为补充粒子设置的权重,
中的横向坐标和航向角坐标是该粒子所属单元格的中心坐标,
中的纵向坐标是初始化的坐标,具体初始化坐标可以根据实际需求设置,比如设置为0,具体 不做限定。
相应的,步骤203的基于各第一粒子分别对应的第一预测位姿,确定可移动设备的第一估计位姿,包括:
步骤4015,基于各第三粒子分别对应的第三粒子位姿,确定可移动设备的第一估计位姿。
在获得各单元格分别对应的第三粒子的第三粒子位姿后,相当于是对第一粒子的粒子群进行了重采样,使得粒子群的粒子分布稳定,进而基于各第三粒子分别对应的第三粒子位姿,确定可移动设备的第一估计位姿。
由于在每次预测、观测的过程中,粒子的聚散情况会有很大的差别,为了提高粒子分布的稳定性,本公开通过将第一粒子网格化,实现重采样,使得粒子群稳定分布在第一网格坐标区域中,可以有效解决粒子分布不稳定的问题。
在一个可选示例中,在步骤4013的基于各第一粒子分别对应的第一预测位姿,将各第一粒子映射到第一网格坐标区域中,获得各第一粒子所属的单元格之前,该方法还可以包括:
针对每个第一粒子,进行多次采样,比如进行h次采样,则最终可以获得h*N个新第一粒子及各新第一粒子分别对应的新第一预测位姿。相应的,步骤4013具体可以包括基于各新第一粒子分别对应的新第一预测位姿,将各新第一粒子映射到第一网格坐标区域中,获得各新第一粒子所属的单元格,后续相关步骤则均基于新第一粒子来处理,最终获得各单元格分别对应的第三粒子、及各第三粒子分别对应的第三粒子位姿,具体过程与上述第一粒子的网格化类似,区别仅在于粒子数量增多,在此不再赘述。其中,针对任一个第一粒子的多次采样方式可以为:通过每次采样引入不同的随机噪声来获得该第一粒子对应的不同的h个新第一粒子。
通过对N个第一粒子分别进行多次采样,扩充粒子数量,基于扩充后的粒子群进行网格化流程,在保证粒子分布稳定性的基础上,提高定位精度。
在一个可选示例中,图8是本公开又一个示例性实施例提供的位姿信息的确定方法的流程示意图,在本示例中,在步骤4013的基于各第一粒子分别对应的第一预测位姿,将各第一粒子映射到第一网格坐标区域中,获得各第一粒子所属的单元格之后,本公开的方法还包括:
步骤4021,将映射到第一网格坐标区域外的第一粒子删除,获得剩余第一粒子。
步骤4022,在第一网格坐标区域中的各单元格中心分别增加一个第四粒子,每个第四粒子对应一个第四权重。
相应的,步骤4014的基于各第一粒子所属的单元格,确定各单元格分别对应的第三粒子、及各第三粒子分别对应的第三粒子位姿,包括:
步骤40141,基于各剩余第一粒子所属的单元格、各剩余第一粒子分别对应的第二权重,及各第四粒子分别对应的第四权重,将各单元格中的粒子进行合并,获得各单元格分别对应的第三粒子及各第三粒子分 别对应的第三粒子位姿。
具体的,由于有一些第一粒子可能会映射到第一网格坐标区域外,而有一些单元格可能会没有第一粒子落入,为了进一步提高粒子分布的稳定性,在每个时刻,将映射到第一网格坐标区域外的第一粒子删除,获得剩余第一粒子,针对没有第一粒子落入的单元格情况,在第一网格坐标区域中的各单元格中心分别增加一个第四粒子,每个第四粒子对应一个第四权重,第四权重可以根据实际需求设置,比如可以为其设置一个很小的权重,比如1/(N
y*N
θ)/100。各第四粒子分别对应的第四粒子位姿即为对应单元格中心对应的位姿。添加第四粒子后,每个单元格都有粒子分布,进而基于各剩余第一粒子和增加的各第四粒子,将单元格粒子化,将每个单元格转换成一个对应的第三粒子,从而获得N个第三粒子,稳定地分布在每个单元格内。
示例性的,图9是本公开一示例性实施例提供的基于网格化的粒子群重采样的原理示意图。其中,各第一粒子用黑色圆点表示,增加的第四粒子用白色圆点表示,单元格粒子化获得的各第三粒子用灰色圆点表示。
在一个可选示例中,也可以只在没有第一粒子落入的单元格中增加第四粒子,其他单元格不增加,相应的为了保证各单元格的公平性,其他单元格可以增加一定的权重,具体可以根据实际需求设置。
示例性的,图10是本公开另一示例性实施例提供的基于网格化的粒子群重采样的原理示意图。其中,针对每个第一粒子通过5次采样获得各第一粒子分别对应的3个新第一粒子,从而将3*N个新第一粒子映射到第一网格坐标区域中,N仍以20为例新第一粒子用黑色圆点表示,增加的第四粒子用白色圆点表示,单元格粒子化获得的各第三粒子用灰色圆点表示。
在一个可选示例中,图11是本公开一个示例性实施例提供的步骤3011的流程示意图,在本示例中,步骤3011具体可以包括以下步骤:
步骤30111,以第一估计位姿为中心粒子,在纵向上按照第一间隔向前和向后分别采样H个第五粒子,获得2H个第五粒子、及各第五粒子与第一估计位姿的第二间隔,H为正整数。
步骤30112,基于第一估计位姿、及各第五粒子与第一估计位姿的第二间隔,确定各第五粒子的位姿。
步骤30113,将中心粒子及各第五粒子作为第二粒子,获得M个第二粒子的第二粒子位姿,M=2H+1。
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]
在一个可选示例中,图12是本公开一个示例性实施例提供的步骤3015的流程示意图,在本示例中,步骤3015的基于各第二粒子分别对应的第一权重,确定第一纵向位姿矫正量,包括:
步骤30151,基于各第二粒子分别对应的第一权重,确定各第二粒子的第一权重之和。
步骤30152,基于各第二粒子的第一权重之和、各第二粒子分别对应的第一权重、及各第二粒子与第一估计位姿的第二间隔,确定纵向矫正量。
示例性的,纵向矫正量d
correct表示为:
步骤30153,基于纵向矫正量确定对应的位姿增量,作为第一纵向位姿矫正量。
示例性的,基于上述的纵向矫正量d
correct获得的第一纵向位姿矫正量D
correct表示为:
D
correct=(d
correct,0,0)
在一个可选示例中,图13是本公开一个示例性实施例提供的步骤2031的流程示意图,在本示例中,步骤2031的基于各第一粒子分别对应的第一预测位姿,对各第一粒子分别对应的第二权重进行更新,获得各第一粒子分别对应的第三权重,包括:
步骤20311,获取各第一粒子分别对应的车道线观测结果。
其中,一个第一粒子对应的车道线观测结果包括在该第一粒子对应的第一预测位姿下车道线的观测概率(简称车道线观测概率)。
具体来说,以一个第一粒子为例,将该第一粒子的第一预测位姿作为可移动设备的自坐标系的原点对车道线进行观测,将观测获得的车道线与地图中的车道线元素变换到同一坐标系下进行匹配,匹配度越高得分越高则车道线观测概率越大,匹配度越低得分越低则车道线观测概率越低。
步骤20312,基于各第一粒子分别对应的车道线观测结果,对各第一粒子分别对应的第二权重进行更新,获得各第一粒子分别对应的第三权重。
其中,
即表示第j个第一粒子在t时刻的第三权重,
表示第i个第一粒子在j时刻的第二权重,也即将第t-1时刻更新后的权重作为t时刻的先验权重;
表示第j个第一粒子对应的车道线观测概率,j=1,2,…,N。
其中,ω
LANE表示预设的车道线权重,max_dist表示观测车道线与地图车道线元素之间匹配允许的最大距离阈值,
表示地图车道线元素与t时刻的观测车道线之间的欧式距离,K(正整数)表示车道线数量,可见,匹配的欧式距离越大,表示匹配度越低,则车道线观测概率越小,相应的该第一粒子的权重更新量越小;匹配的欧式距离越小,表示匹配度越高,则车道线观测概率越大,相应的该第一粒子的权重更新量越大。
在一个可选示例中,步骤202的分别基于各第一粒子的第一粒子位姿,对各第一粒子的当前时刻的位姿进行横向定位预测,获得各第一粒子分别对应的第一预测位姿,包括:
分别基于各第一粒子的第一粒子位姿、及基于里程计的运动模型,获得各第一粒子分别对应的第一预测位姿;基于里程计的运动模型中平移分量中的纵向噪声参数为0。
基于里程计的运动模型是概率机器人的运动模型,是使用相对运动信息实现的,该相对运动信息由可移动设备的里程计测量,更具体的,在时间间隔(t-1,t)内,可移动设备从位姿x
t-1前进到位姿x
t,里程计反馈了这一时段内的相对前进,基于里程计的运动模型将可移动设备的运动用三个串联的基本运动表示,即旋转、直线运动(即平移)和另一个旋转,以下方程表示了从里程计读数
(其中,
T表示转置)计算两个旋转值和一个平移值:
其中,δ
rot1为一个旋转值,δ
trans为平移值,δ
rot2为另一个旋转值,atan2是一个函数,返回的是方位角。
本公开实施例在使用上述基于里程计的运动模型对各第一粒子当前时刻的位姿进行横向定位预测时,将其中的纵向噪声参数α
3设置为0,在预测的时候,只进行横向定位预测,而不引入纵向上的噪声,实现横向和纵向观测解耦。
在一个可选示例中,图14是本公开又一示例性实施例提供的位姿信息的确定方法的流程示意图,本示例中,该方法包括:
1、获取可移动设备对应的N个第一粒子的第一粒子位姿。
2、分别基于各第一粒子的第一粒子位姿、及基于里程计的运动模型,获得各第一粒子分别对应的第一预测位姿;基于里程计的运动模型中平移分量中的纵向噪声参数为0。
3、基于可移动设备前一时刻的位姿信息、及当前时刻的里程计信息,确定可移动设备当前时刻的第二预测位姿。
4、基于第二预测位姿建立第一网格坐标区域;第一网格坐标区域的第一坐标轴为位姿的横向方向,第一网格坐标区域的第二坐标轴为位姿的航向角方向。
5、基于各第一粒子分别对应的第一预测位姿,将各第一粒子映射到第一网格坐标区域中,获得各第一粒子所属的单元格。
6、将映射到第一网格坐标区域外的第一粒子删除,获得剩余第一粒子。
7、在第一网格坐标区域中的各单元格中心分别增加一个第四粒子,每个第四粒子对应一个第四权重。
8、基于各剩余第一粒子所属的单元格、各剩余第一粒子分别对应的第二权重,及各第四粒子分别对应 的第四权重,将各单元格中的粒子进行合并,获得各单元格分别对应的第三粒子及各第三粒子分别对应的第三粒子位姿。
9、基于各第三粒子分别对应的第三粒子位姿,对各第三粒子分别对应的原权重进行更新,获得各第三粒子分别对应的更新后的权重。
该步骤权重更新原理与前述步骤2031(具体为步骤20311-20312)的具体操作类似,即将第三粒子对应的第三粒子位姿作为可移动设备的自坐标系的原点对车道线进行观测,基于车道线观测结果来进行权重更新,具体不再赘述。第三粒子分别对应的原权重即为单元格中所有粒子的权重之和。
10、基于各第三粒子分别对应的第三粒子位姿及更新后的权重,确定可移动设备的第一估计位姿:
该步骤与前述步骤2032的原理一致,在此不再赘述。
11、将可移动设备的第一估计位姿作为可移动设备当前时刻的位姿信息。
12、以第一估计位姿为中心粒子,在纵向上按照第一间隔向前和向后分别采样H个第五粒子,获得2H个第五粒子、及各第五粒子与第一估计位姿的第二间隔,H为正整数。
13、基于第一估计位姿、及各第五粒子与第一估计位姿的第二间隔,确定各第五粒子的位姿。
14、将中心粒子及各第五粒子作为第二粒子,获得M个第二粒子的第二粒子位姿,M=2H+1。
15、基于高斯分布获取各第二粒子分别对应的先验权重。
16、获取各第二粒子分别对应的箭头路标观测结果和位姿测量结果。
17、基于各第二粒子分别对应的箭头路标观测结果和位姿测量结果,对各第二粒子分别对应的先验权 重进行更新,获得各第二粒子分别对应的第一权重。
其中,
即表示第i个第二粒子对应的第一权重,
即表示采样的第i个第二粒子的先验权重,
表示第i个第二粒子对应的位姿观测概率,
表示第i个第二粒子对应的箭头观测概率,i=-H,-(H-1),…,0,1,…,H,H为正整数,M=2H+1。
18、基于各第二粒子分别对应的第一权重,确定第一纵向位姿矫正量。
示例性的,纵向矫正量d
correct表示为:
基于上述的纵向矫正量d
correct获得的第一纵向位姿矫正量D
correct表示为:
D
correct=(d
correct,0,0)
19、基于第一纵向位姿矫正量对各第三粒子的第三粒子位姿进行纵向矫正,获得各第三粒子分别对应的当前时刻的后验位姿。
将各第三粒子分别对应的当前时刻的后验位姿作为各第一粒子分别对应的当前时刻的后验位姿,作用于下一时刻。
20、进入下一时刻,将下一时刻作为当前时刻,返回上述步骤1。
上述步骤1-20的具体操作已在前述内容中进行了详细说明,在此不再赘述。
示例性的,图15是本公开一示例性实施例提供的粒子群横向和纵向矫正原理示意图。其中,横向粒子群即基于前一时刻纵向矫正后的粒子群进行横向矫正获得的粒子群,这里横向矫正可以包括上述步骤2-9,获得的横向粒子群即为权重更新后的第三粒子形成的粒子群,纵向观测确定纵向矫正量即包括上述步骤12-18,纵向矫正量作用于横向粒子群则包括上述步骤19,具体不再赘述。
本公开实施例提供的任一种位姿信息的确定方法可以由任意适当的具有数据处理能力的设备执行,包括但不限于:终端设备和服务器等。或者,本公开实施例提供的任一种位姿信息的确定方法可以由处理器执行,如处理器通过调用存储器存储的相应指令来执行本公开实施例提及的任一种位姿信息的确定方法。下文不再赘述。
示例性装置
图16是本公开一示例性实施例提供的位姿信息的确定装置的结构示意图。该实施例的装置可用于实现 本公开相应的方法实施例,如图16所示的装置包括:第一获取模块501、第一处理模块502、第二处理模块503和第三处理模块504。
第一获取模块501,用于获取可移动设备对应的N个第一粒子的第一粒子位姿,N为大于1的正整数;第一粒子位姿是前一时刻获得的对应第一粒子的纵向矫正后的后验位姿;第一处理模块502,用于分别基于第一获取模块501获取的各第一粒子的第一粒子位姿,对各第一粒子的当前时刻的位姿进行横向定位预测,获得各第一粒子分别对应的第一预测位姿;第二处理模块503,用于基于第一处理模块502获得的各第一粒子分别对应的第一预测位姿,确定可移动设备的第一估计位姿;第三处理模块504,用于将第二处理模块503获得的可移动设备的第一估计位姿作为可移动设备当前时刻的位姿信息。
图17是本公开另一示例性实施例提供的位姿信息的确定装置的结构示意图。
在一个可选示例中,本公开的装置还包括:第一纵向处理模块505和第二纵向处理模块506。第一纵向处理模块505,用于基于第二处理模块503获得的可移动设备的第一估计位姿,确定第一纵向位姿矫正量;第二纵向处理模块506,用于基于第一纵向处理模块505获得的第一纵向位姿矫正量对各第一粒子的第一预测位姿进行纵向矫正,获得各第一粒子分别对应的当前时刻的后验位姿。
图18是本公开一示例性实施例提供的第一纵向处理模块505的结构示意图。
在一个可选示例中,第一纵向处理模块505包括:第一采样单元5051、第一获取单元5052、第二获取单元5053、第一权重更新单元5054和第一确定单元5055。第一采样单元5051,用于以第二处理模块503获得的第一估计位姿为中心,在纵向上进行采样,获得M个第二粒子的第二粒子位姿,M为大于1的正整数;第一获取单元5052,用于基于高斯分布获取各第二粒子分别对应的先验权重;第二获取单元5053,用于获取各第二粒子分别对应的箭头路标观测结果和位姿测量结果;一个第二粒子对应的箭头路标观测结果包括在该第二粒子对应的第二粒子位姿下箭头路标的观测概率;一个第二粒子对应的位姿测量结果包括在该第二粒子对应的第二粒子位姿下可移动设备位姿的观测概率;第一权重更新单元5054,基于各第二粒子分别对应的箭头路标观测结果和位姿测量结果,对各第二粒子分别对应的先验权重进行更新,获得各第二粒子分别对应的第一权重;第一确定单元5055,基于各第二粒子分别对应的第一权重,确定第一纵向位姿矫正量。
图19是本公开一示例性实施例提供的第二处理模块503的结构示意图。
在一个可选示例中,第二处理模块503包括:第二权重更新单元5031和第二确定单元5032。第二权重更新单元5031,用于基于第一处理模块502获得的各第一粒子分别对应的第一预测位姿,对各第一粒子分别对应的第二权重进行更新,获得各第一粒子分别对应的第三权重;一个第一粒子对应的第二权重为该第一粒子的先验权重;第二确定单元5032,用于基于第一处理模块502获得的各第一粒子分别对应的第一预测位姿及第二权重更新单元5031获得的各第一粒子分别对应的第三权重,确定可移动设备的第一估计位姿。
图20是本公开再一示例性实施例提供的位姿信息的确定装置的结构示意图。
在一个可选示例中,本公开的装置还包括:第一确定模块507、第一建立模块508、第一映射模块509和第二确定模块510。第一确定模块507,用于基于可移动设备前一时刻的位姿信息、及当前时刻的里程计信息,确定可移动设备当前时刻的第二预测位姿;第一建立模块508,用于基于第一确定模块507获得的第二预测位姿建立第一网格坐标区域;第一网格坐标区域的第一坐标轴为位姿的横向方向,第一网格坐标区域的第二坐标轴为位姿的航向角方向;第一网格坐标区域包括N个单元格,N=N
y*N
θ,N
y和N
θ分别表示第一网格坐标区域在横向方向和航向角方向的单元格数量;第一映射模块509,用于基于第一处理模块502获得的各第一粒子分别对应的第一预测位姿,将各第一粒子映射到第一建立模块508建立的第一网格坐标区域中,获得各第一粒子所属的单元格;第二确定模块510,用于基于各第一粒子所属的单元格,确定各单元格分别对应的第三粒子、及各第三粒子分别对应的第三粒子位姿;相应的,第二处理模块503,具体用于基于第二确定模块510获得的各第三粒子分别对应的第三粒子位姿,确定可移动设备的第一估计位姿。
在一个可选示例中,本公开的装置还包括:第一筛选模块511和第一增补模块512。第一筛选模块511,用于将映射到第一网格坐标区域外的第一粒子删除,获得剩余第一粒子;第一增补模块512,用于在第一网格坐标区域中的各单元格中心分别增加一个第四粒子,每个第四粒子对应一个第四权重。
相应的,第二确定模块510,具体用于:基于各剩余第一粒子所属的单元格、各剩余第一粒子分别对应的第二权重,及各第四粒子分别对应的第四权重,将各单元格中的粒子进行合并,获得各单元格分别对应的第三粒子及各第三粒子分别对应的第三粒子位姿。
在一个可选示例中,本公开的装置还可以包括:重采样模块513,用于针对每个第一粒子,进行多次采样,比如进行h次采样,则最终可以获得h*N个新第一粒子及各新第一粒子分别对应的新第一预测位姿。相应的,第一映射模块509具体用于:基于各新第一粒子分别对应的新第一预测位姿,将各新第一粒子映射到第一网格坐标区域中,获得各新第一粒子所属的单元格。第二确定模块510,具体用于基于各新第一粒子所属的单元格,确定各单元格分别对应的第三粒子、及各第三粒子分别对应的第三粒子位姿。
在一个可选示例中,第一采样单元5051,具体用于:以第一估计位姿为中心粒子,在纵向上按照第一间隔向前和向后分别采样H个第五粒子,获得2H个第五粒子、及各第五粒子与第一估计位姿的第二间隔,H为正整数;基于第一估计位姿、及各第五粒子与第一估计位姿的第二间隔,确定各第五粒子的位姿;将中心粒子及各第五粒子作为第二粒子,获得M个第二粒子的第二粒子位姿,M=2H+1。
在一个可选示例中,第一确定单元5055,具体用于:基于各第二粒子分别对应的第一权重,确定各第二粒子的第一权重之和;基于各第二粒子的第一权重之和、各第二粒子分别对应的第一权重、及各第二粒子与第一估计位姿的第二间隔,确定纵向矫正量;基于纵向矫正量确定对应的位姿增量,作为第一纵向位姿矫正量。
在一个可选示例中,第二权重更新单元5031,具体用于:获取各第一粒子分别对应的车道线观测结果;一个第一粒子对应的车道线观测结果包括在该第一粒子对应的第一预测位姿下车道线的观测概率;基于各 第一粒子分别对应的车道线观测结果,对各第一粒子分别对应的第二权重进行更新,获得各第一粒子分别对应的第三权重。
在一个可选示例中,第一处理模块502,具体用于:分别基于各第一粒子的第一粒子位姿、及基于里程计的运动模型,获得各第一粒子分别对应的第一预测位姿;基于里程计的运动模型中平移分量中的纵向噪声参数为0。
示例性电子设备
本公开实施例还提供了一种电子设备,包括:存储器,用于存储计算机程序;
处理器,用于执行所述存储器中存储的计算机程序,且所述计算机程序被执行时,实现本公开上述任一实施例所述的位姿信息的确定方法。
图21是本公开电子设备一个应用实施例的结构示意图。本实施例中,该电子设备10包括一个或多个处理器11和存储器12。
处理器11可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元,并且可以控制电子设备10中的其他组件以执行期望的功能。
存储器12可以包括一个或多个计算机程序产品,所述计算机程序产品可以包括各种形式的计算机可读存储介质,例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令,处理器11可以运行所述程序指令,以实现上文所述的本公开的各个实施例的位姿信息的确定方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如输入信号、信号分量、噪声分量等各种内容。
在一个示例中,电子设备10还可以包括:输入装置13和输出装置14,这些组件通过总线***和/或其他形式的连接机构(未示出)互连。
例如,该输入装置13可以是上述的麦克风或麦克风阵列,用于捕捉声源的输入信号。
此外,该输入装置13还可以包括例如键盘、鼠标等等。
该输出装置14可以向外部输出各种信息,包括确定出的距离信息、方向信息等。该输出装置14可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
当然,为了简化,图21中仅示出了该电子设备10中与本公开有关的组件中的一些,省略了诸如总线、输入/输出接口等等的组件。除此之外,根据具体应用情况,电子设备10还可以包括任何其他适当的组件。
示例性计算机程序产品和计算机可读存储介质
除了上述方法和设备以外,本公开的实施例还可以是计算机程序产品,其包括计算机程序指令,所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本公开各种实施例的方法中的步骤。
所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本公开实施例操作的程序代码,所述程序设计语言包括面向对象的程序设计语言,诸如Java、C++等,还包括常规的过程式程序设计语言,诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
此外,本公开的实施例还可以是计算机可读存储介质,其上存储有计算机程序指令,所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本公开各种实施例的方法中的步骤。
所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的***、装置或器件,或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
以上结合具体实施例描述了本公开的基本原理,但是,需要指出的是,在本公开中提及的优点、优势、效果等仅是示例而非限制,不能认为这些优点、优势、效果等是本公开的各个实施例必须具备的。另外,上述公开的具体细节仅是为了示例的作用和便于理解的作用,而非限制,上述细节并不限制本公开为必须采用上述具体的细节来实现。
本说明书中各个实施例均采用递进的方式描述,每个实施例重点说明的都是与其它实施例的不同之处,各个实施例之间相同或相似的部分相互参见即可。对于***实施例而言,由于其与方法实施例基本对应,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
本公开中涉及的器件、装置、设备、***的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的,可以按任意方式连接、布置、配置这些器件、装置、设备、***。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇,指“包括但不限于”,且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”,且可与其互换使用,除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”,且可与其互换使用。
可能以许多方式来实现本公开的方法和装置。例如,可通过软件、硬件、固件或者软件、硬件、固件的任何组合来实现本公开的方法和装置。用于所述方法的步骤的上述顺序仅是为了进行说明,本公开的方法的步骤不限于以上具体描述的顺序,除非以其它方式特别说明。此外,在一些实施例中,还可将本公开实施为记录在记录介质中的程序,这些程序包括用于实现根据本公开的方法的机器可读指令。因而,本公开还覆盖存储用于执行根据本公开的方法的程序的记录介质。
还需要指出的是,在本公开的装置、设备和方法中,各部件或各步骤是可以分解和/或重新组合的。这 些分解和/或重新组合应视为本公开的等效方案。
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本公开。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的,并且在此定义的一般原理可以应用于其他方面而不脱离本公开的范围。因此,本公开不意图被限制到在此示出的方面,而是按照与在此公开的原理和新颖的特征一致的最宽范围。
为了例示和描述的目的已经给出了以上描述。此外,此描述不意图将本公开的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例,但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。
Claims (13)
- 一种位姿信息的确定方法,包括:获取可移动设备对应的N个第一粒子的第一粒子位姿,N为大于1的正整数;所述第一粒子位姿是前一时刻获得的对应第一粒子的纵向矫正后的后验位姿;分别基于各所述第一粒子的第一粒子位姿,对各所述第一粒子的当前时刻的位姿进行横向定位预测,获得各所述第一粒子分别对应的第一预测位姿;基于各所述第一粒子分别对应的第一预测位姿,确定所述可移动设备的第一估计位姿;将所述可移动设备的第一估计位姿作为所述可移动设备当前时刻的位姿信息。
- 根据权利要求1所述的方法,其中,在基于各所述第一粒子分别对应的第一预测位姿,确定所述可移动设备的第一估计位姿之后,还包括:基于所述可移动设备的第一估计位姿,确定第一纵向位姿矫正量;基于所述第一纵向位姿矫正量对各所述第一粒子的第一预测位姿进行纵向矫正,获得各所述第一粒子分别对应的当前时刻的后验位姿。
- 根据权利要求2所述的方法,其中,所述基于所述可移动设备的第一估计位姿,确定第一纵向位姿矫正量,包括:以所述第一估计位姿为中心,在纵向上进行采样,获得M个第二粒子的第二粒子位姿,M为大于1的正整数;基于高斯分布获取各所述第二粒子分别对应的先验权重;获取各所述第二粒子分别对应的箭头路标观测结果和位姿测量结果;一个所述第二粒子对应的箭头路标观测结果包括在该第二粒子对应的第二粒子位姿下箭头路标的观测概率;一个所述第二粒子对应的位姿测量结果包括在该第二粒子对应的第二粒子位姿下可移动设备位姿的观测概率;基于各所述第二粒子分别对应的箭头路标观测结果和位姿测量结果,对各所述第二粒子分别对应的先验权重进行更新,获得各所述第二粒子分别对应的第一权重;基于各所述第二粒子分别对应的第一权重,确定所述第一纵向位姿矫正量。
- 根据权利要求1所述的方法,其中,所述基于各所述第一粒子分别对应的第一预测位姿,确定所述可移动设备的第一估计位姿,包括:基于各所述第一粒子分别对应的第一预测位姿,对各所述第一粒子分别对应的第二权重进行更新,获得各所述第一粒子分别对应的第三权重;一个第一粒子对应的第二权重为该第一粒子的先验权重;基于各所述第一粒子分别对应的第一预测位姿及第三权重,确定所述可移动设备的第一估计位姿。
- 根据权利要求4所述的方法,其中,在分别基于各所述第一粒子的第一粒子位姿,对各所述第一粒子的当前时刻的位姿进行横向定位预测,获得各所述第一粒子分别对应的第一预测位姿之后,还包括:基于所述可移动设备前一时刻的位姿信息、及当前时刻的里程计信息,确定所述可移动设备当前时刻 的第二预测位姿;基于所述第二预测位姿建立第一网格坐标区域;所述第一网格坐标区域的第一坐标轴为位姿的横向方向,所述第一网格坐标区域的第二坐标轴为位姿的航向角方向;所述第一网格坐标区域包括N个单元格,N=N y*N θ,N y和N θ分别表示所述第一网格坐标区域在横向方向和航向角方向的单元格数量;基于各所述第一粒子分别对应的第一预测位姿,将各所述第一粒子映射到所述第一网格坐标区域中,获得各所述第一粒子所属的单元格;基于各所述第一粒子所属的单元格,确定各所述单元格分别对应的第三粒子、及各所述第三粒子分别对应的第三粒子位姿;所述基于各所述第一粒子分别对应的第一预测位姿,确定所述可移动设备的第一估计位姿,包括:基于各所述第三粒子分别对应的第三粒子位姿,确定所述可移动设备的第一估计位姿。
- 根据权利要求5所述的方法,其中,在所述基于各所述第一粒子分别对应的第一预测位姿,将各所述第一粒子映射到所述第一网格坐标区域中,获得各所述第一粒子所属的单元格之后,还包括:将映射到所述第一网格坐标区域外的第一粒子删除,获得剩余第一粒子;在所述第一网格坐标区域中的各单元格中心分别增加一个第四粒子,每个所述第四粒子对应一个第四权重;所述基于各所述第一粒子所属的单元格,确定各所述单元格分别对应的第三粒子、及各所述第三粒子分别对应的第三粒子位姿,包括:基于各所述剩余第一粒子所属的单元格、各所述剩余第一粒子分别对应的第二权重,及各所述第四粒子分别对应的第四权重,将各所述单元格中的粒子进行合并,获得各所述单元格分别对应的第三粒子及各所述第三粒子分别对应的第三粒子位姿。
- 根据权利要求3所述的方法,其中,以所述第一估计位姿为中心,在纵向上进行采样,获得M个第二粒子的第二粒子位姿,包括:以所述第一估计位姿为中心粒子,在纵向上按照第一间隔向前和向后分别采样H个第五粒子,获得2H个第五粒子、及各所述第五粒子与所述第一估计位姿的第二间隔,H为正整数;基于所述第一估计位姿、及各所述第五粒子与所述第一估计位姿的第二间隔,确定各所述第五粒子的位姿;将所述中心粒子及各所述第五粒子作为第二粒子,获得所述M个第二粒子的第二粒子位姿,M=2H+1。
- 根据权利要求7所述的方法,其中,所述基于各所述第二粒子分别对应的第一权重,确定所述第一纵向位姿矫正量,包括:基于各所述第二粒子分别对应的第一权重,确定各所述第二粒子的第一权重之和;基于各所述第二粒子的第一权重之和、各所述第二粒子分别对应的第一权重、及各所述第二粒子与所 述第一估计位姿的第二间隔,确定纵向矫正量;基于所述纵向矫正量确定对应的位姿增量,作为所述第一纵向位姿矫正量。
- 根据权利要求4所述的方法,其中,所述基于各所述第一粒子分别对应的第一预测位姿,对各所述第一粒子分别对应的第二权重进行更新,获得各所述第一粒子分别对应的第三权重,包括:获取各所述第一粒子分别对应的车道线观测结果;一个所述第一粒子对应的车道线观测结果包括在该第一粒子对应的第一预测位姿下车道线的观测概率;基于各所述第一粒子分别对应的车道线观测结果,对各所述第一粒子分别对应的第二权重进行更新,获得各所述第一粒子分别对应的第三权重。
- 根据权利要求1-9任一所述的方法,其中,所述分别基于各所述第一粒子的第一粒子位姿,对各所述第一粒子的当前时刻的位姿进行横向定位预测,获得各所述第一粒子分别对应的第一预测位姿,包括:分别基于各所述第一粒子的第一粒子位姿、及基于里程计的运动模型,获得各所述第一粒子分别对应的第一预测位姿;所述基于里程计的运动模型中平移分量中的纵向噪声参数为0。
- 一种位姿信息的确定装置,包括:第一获取模块,用于获取可移动设备对应的N个第一粒子的第一粒子位姿,N为大于1的正整数;所述第一粒子位姿是前一时刻获得的对应第一粒子的纵向矫正后的后验位姿;第一处理模块,用于分别基于各所述第一粒子的第一粒子位姿,对各所述第一粒子的当前时刻的位姿进行横向定位预测,获得各所述第一粒子分别对应的第一预测位姿;第二处理模块,用于基于各所述第一粒子分别对应的第一预测位姿,确定所述可移动设备的第一估计位姿;第三处理模块,用于将所述可移动设备的第一估计位姿作为所述可移动设备当前时刻的位姿信息。
- 一种计算机可读存储介质,所述存储介质存储有计算机程序,所述计算机程序用于执行上述权利要求1-10任一所述的位姿信息的确定方法。
- 一种电子设备,所述电子设备包括:处理器;用于存储所述处理器可执行指令的存储器;所述处理器,用于从所述存储器中读取所述可执行指令,并执行所述指令以实现上述权利要求1-10任一所述的位姿信息的确定方法。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210013207.7 | 2022-01-06 | ||
CN202210013207.7A CN114355415A (zh) | 2022-01-06 | 2022-01-06 | 位姿信息的确定方法和装置、电子设备和存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2023131048A1 true WO2023131048A1 (zh) | 2023-07-13 |
Family
ID=81106525
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/CN2022/143426 WO2023131048A1 (zh) | 2022-01-06 | 2022-12-29 | 位姿信息的确定方法、装置、电子设备和存储介质 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN114355415A (zh) |
WO (1) | WO2023131048A1 (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117406259A (zh) * | 2023-12-14 | 2024-01-16 | 江西北斗云智慧科技有限公司 | 一种基于北斗的智慧工地车辆定位方法及*** |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114355415A (zh) * | 2022-01-06 | 2022-04-15 | 上海安亭地平线智能交通技术有限公司 | 位姿信息的确定方法和装置、电子设备和存储介质 |
CN115388906A (zh) * | 2022-08-24 | 2022-11-25 | 上海安亭地平线智能交通技术有限公司 | 位姿的确定方法、装置、电子设备和存储介质 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20080065267A1 (en) * | 2006-09-13 | 2008-03-13 | Samsung Electronics Co., Ltd. | Method, medium, and system estimating pose of mobile robots |
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 |
CN107246873A (zh) * | 2017-07-03 | 2017-10-13 | 哈尔滨工程大学 | 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 |
CN108053443A (zh) * | 2017-11-20 | 2018-05-18 | 中国科学院空间应用工程与技术中心 | 一种基于粒子群优化的物体点云位姿估算方法和*** |
CN108692701A (zh) * | 2018-05-28 | 2018-10-23 | 佛山市南海区广工大数控装备协同创新研究院 | 基于粒子滤波器的移动机器人多传感器融合定位方法 |
CN110554353A (zh) * | 2019-08-29 | 2019-12-10 | 华中科技大学 | 一种基于rfid***中移动机器人绝对定位方法 |
WO2020048623A1 (en) * | 2018-09-07 | 2020-03-12 | Huawei Technologies Co., Ltd. | Estimation of a pose of a robot |
CN111113422A (zh) * | 2019-12-30 | 2020-05-08 | 深圳市优必选科技股份有限公司 | 机器人定位方法、装置、计算机可读存储介质及机器人 |
WO2020237996A1 (zh) * | 2019-05-30 | 2020-12-03 | 魔门塔(苏州)科技有限公司 | 一种车辆位姿的修正方法和装置 |
CN114355415A (zh) * | 2022-01-06 | 2022-04-15 | 上海安亭地平线智能交通技术有限公司 | 位姿信息的确定方法和装置、电子设备和存储介质 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8452535B2 (en) * | 2010-12-13 | 2013-05-28 | GM Global Technology Operations LLC | Systems and methods for precise sub-lane vehicle positioning |
CN110543727B (zh) * | 2019-09-05 | 2023-01-03 | 北京工业大学 | 一种基于改进粒子群算法的全向移动智能轮椅机器人参数辨识方法 |
CN111174782B (zh) * | 2019-12-31 | 2021-09-17 | 智车优行科技(上海)有限公司 | 位姿估计方法、装置、电子设备及计算机可读存储介质 |
CN113238666B (zh) * | 2021-05-24 | 2024-01-23 | 江苏科技大学 | 基于麻雀搜索算法优化的gru的船舶运动姿态的预测方法 |
-
2022
- 2022-01-06 CN CN202210013207.7A patent/CN114355415A/zh active Pending
- 2022-12-29 WO PCT/CN2022/143426 patent/WO2023131048A1/zh unknown
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20080065267A1 (en) * | 2006-09-13 | 2008-03-13 | Samsung Electronics Co., Ltd. | Method, medium, and system estimating pose of mobile robots |
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 |
CN107246873A (zh) * | 2017-07-03 | 2017-10-13 | 哈尔滨工程大学 | 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 |
CN108053443A (zh) * | 2017-11-20 | 2018-05-18 | 中国科学院空间应用工程与技术中心 | 一种基于粒子群优化的物体点云位姿估算方法和*** |
CN108692701A (zh) * | 2018-05-28 | 2018-10-23 | 佛山市南海区广工大数控装备协同创新研究院 | 基于粒子滤波器的移动机器人多传感器融合定位方法 |
WO2020048623A1 (en) * | 2018-09-07 | 2020-03-12 | Huawei Technologies Co., Ltd. | Estimation of a pose of a robot |
WO2020237996A1 (zh) * | 2019-05-30 | 2020-12-03 | 魔门塔(苏州)科技有限公司 | 一种车辆位姿的修正方法和装置 |
CN110554353A (zh) * | 2019-08-29 | 2019-12-10 | 华中科技大学 | 一种基于rfid***中移动机器人绝对定位方法 |
CN111113422A (zh) * | 2019-12-30 | 2020-05-08 | 深圳市优必选科技股份有限公司 | 机器人定位方法、装置、计算机可读存储介质及机器人 |
CN114355415A (zh) * | 2022-01-06 | 2022-04-15 | 上海安亭地平线智能交通技术有限公司 | 位姿信息的确定方法和装置、电子设备和存储介质 |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117406259A (zh) * | 2023-12-14 | 2024-01-16 | 江西北斗云智慧科技有限公司 | 一种基于北斗的智慧工地车辆定位方法及*** |
CN117406259B (zh) * | 2023-12-14 | 2024-03-22 | 江西北斗云智慧科技有限公司 | 一种基于北斗的智慧工地车辆定位方法及*** |
Also Published As
Publication number | Publication date |
---|---|
CN114355415A (zh) | 2022-04-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2023131048A1 (zh) | 位姿信息的确定方法、装置、电子设备和存储介质 | |
CN113256712B (zh) | 定位方法、装置、电子设备和存储介质 | |
WO2022007504A1 (zh) | 位置确定方法、装置、***和计算机可读存储介质 | |
CN115265523B (zh) | 机器人同时定位与建图方法、装置及可读介质 | |
CN109270545B (zh) | 一种定位真值校验方法、装置、设备及存储介质 | |
WO2020108311A1 (zh) | 目标对象3d检测方法、装置、介质及设备 | |
JP2019215853A (ja) | 測位のための方法、測位のための装置、デバイス及びコンピュータ読み取り可能な記憶媒体 | |
WO2023142864A1 (zh) | 初始位姿的确定方法、装置、电子设备和存储介质 | |
US11544498B2 (en) | Training neural networks using consistency measures | |
CN110929802A (zh) | 基于信息熵的细分类识别模型训练、图像识别方法及装置 | |
CN113793370B (zh) | 三维点云配准方法、装置、电子设备及可读介质 | |
CN114677418A (zh) | 一种基于点云特征点提取的配准方法 | |
CN114993328B (zh) | 车辆定位评估方法、装置、设备和计算机可读介质 | |
CN113936046A (zh) | 一种物体定位方法、装置、电子设备及计算机可读介质 | |
WO2023138331A1 (zh) | 一种构建语义地图的方法及装置 | |
CN109816726B (zh) | 一种基于深度滤波器的视觉里程计地图更新方法和*** | |
CN113761090B (zh) | 一种基于点云地图的定位方法和装置 | |
CN116295466A (zh) | 地图生成方法、装置、电子设备、存储介质、及车辆 | |
CN110399892B (zh) | 环境特征提取方法和装置 | |
CN116012624B (zh) | 定位方法、装置、电子设备、介质以及自动驾驶设备 | |
CN116522781B (zh) | 样本数据生成方法、模型的训练方法和装置 | |
WO2024139465A1 (zh) | 定位信息处理方法、装置、设备和介质 | |
US20230104674A1 (en) | Machine learning techniques for ground classification | |
CN114091592A (zh) | 一种车辆感知信息融合方法、装置、设备及存储介质 | |
CN114612529A (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: 22918489 Country of ref document: EP Kind code of ref document: A1 |