阅读说明：本技术 一种位姿的确定方法、装置、设备及存储介质 (Pose determination method, device, equipment and storage medium ) 是由 曹军 于 2019-11-29 设计创作，主要内容包括：本发明实施例公开了一种位姿的确定方法、装置、设备及存储介质。该方法通过基于卫星定位,获取目标对象的第一位置；确定所述目标对象在预先构建的地图中与所述第一位置相映射的第二位置；获取所述目标对象在保持静止状态时,测量得到的、针对预置的标记点的观测数据；基于所述观测数据,在以所述第二位置为中心的预设范围内,确定目标对象在所述地图中的初始位姿,解决因使用固定位置进行目标对象的位置初始化所带来的灵活性低、放置目标对象容易产生误差的问题,实现降低对目标对象进行初始化时所处位置的要求,增加目标对象确定初始位姿的灵活性,并提高了目标对象在该初始位姿启动的安全性的技术效果。(The embodiment of the invention discloses a pose determining method, a pose determining device, pose determining equipment and a storage medium. The method comprises the steps of obtaining a first position of a target object through satellite-based positioning; determining a second position of the target object in a pre-constructed map, which is mapped with the first position; acquiring observation data which is obtained by measurement and aims at a preset mark point when the target object keeps a static state; based on the observation data, the initial pose of the target object in the map is determined within a preset range with the second position as the center, the problems of low flexibility and easy error in placing the target object caused by using a fixed position to initialize the position of the target object are solved, the requirement on the position of the target object during initialization is lowered, the flexibility of determining the initial pose of the target object is increased, and the safety of starting the target object at the initial pose is improved.)

1. A pose determination method, comprising:

acquiring a first position of a target object based on satellite positioning;

determining a second position of the target object in a pre-constructed map, which is mapped with the first position;

acquiring observation data which is obtained by measurement and aims at a preset mark point when the target object keeps a static state;

and determining the initial pose of the target object in the map within a preset range with the second position as the center based on the observation data.

2. The method of claim 1, wherein determining the second location of the target object mapped to the first location in the pre-constructed map comprises:

when a map is constructed, acquiring a third position and a fourth position with the same timestamp, wherein the third position is the position acquired based on satellite positioning, and the fourth position is the position acquired based on the map;

determining a mapping relationship between the third position and the fourth position;

and when the pose of the target object is determined, mapping the first position to the second position according to the mapping relation.

3. The method of claim 1, wherein determining an initial pose of a target object in the map within a preset range centered on the second position based on the observation data comprises:

generating a particle swarm composed of particles using a Gaussian distribution within a preset range centered on the second position, the particles being used for representing a predicted pose of the target object, the particle swarm being used for representing a probability distribution of the target object with respect to the predicted pose;

adding random noise to the predicted pose corresponding to each particle to update the predicted pose corresponding to the particle;

based on the observation data, an initial pose of the target object in the map is determined in a probability distribution of predicted poses represented by the population of particles.

4. The method of claim 3, wherein adding random noise to the predicted pose corresponding to each of the particles to update the predicted pose corresponding to the particle comprises:

calculating a first standard deviation of the particle swarm based on the predicted pose of the particle;

determining a probability distribution of random noise based on the first standard deviation;

generating random noise for each of the particles in the population of particles using a probability distribution of the random noise;

and superposing the random noise to the predicted pose of the particle so as to update the predicted pose of the particle.

5. The method of claim 4, wherein determining the probability distribution of random noise based on the first standard deviation comprises:

judging whether the first standard deviation is smaller than a preset threshold value or not;

if so, setting the second standard deviation of the random noise as a preset threshold;

if not, setting the second standard deviation of the random noise as the first standard deviation;

and determining the probability distribution of the random noise as the Gaussian distribution meeting the second standard deviation.

6. The method of any one of claims 3-5, wherein determining an initial pose of the target object in the map in a probability distribution of predicted poses represented by the population of particles based on the observation data comprises:

for each particle in the particle swarm, calculating estimation data of the target object aiming at a preset marking point when the target object is at the particle;

updating the weight value of each particle according to the difference between the observation data and the estimation data;

calculating a weighted average value of the predicted poses in the particle swarm according to the weight value;

and when the weighted average value meets a convergence condition, determining the weighted average value as the initial pose of the target object in the map.

7. The method according to claim 6, further comprising, after said calculating a weighted average of predicted poses in the particle swarm in accordance with the weight values:

when the weighted average value does not meet the convergence condition, resampling the particles in the particle swarm according to the weighted value;

and continuing to add random noise to the predicted pose corresponding to each particle by using the resampled particle swarm so as to update the predicted pose corresponding to the particle.

8. The method of claim 7, wherein the resampling the particles in the population of particles as a function of the weight values comprises:

and adjusting the number of particles corresponding to each predicted pose in the particle swarm according to the weight value.

9. An apparatus for determining a pose, comprising:

the first position acquisition module is used for acquiring a first position of a target object based on satellite positioning;

a second position determination module for determining a second position of the target object mapped with the first position in a pre-constructed map;

the observation data acquisition module is used for acquiring observation data which is obtained by measurement and aims at a preset mark point when the target object keeps a static state;

and the initial pose determining module is used for determining the initial pose of the target object in the map within a preset range taking the second position as the center based on the observation data.

10. An apparatus for determining a pose, characterized by comprising: a memory and one or more processors;

the memory for storing one or more programs;

when executed by the one or more processors, cause the one or more processors to implement the pose determination method according to any one of claims 1 to 8.

11. A storage medium containing computer-executable instructions for performing the pose determination method according to any one of claims 1 to 8 when executed by a computer processor.

Technical Field

The embodiment of the invention relates to a positioning technology, in particular to a pose determination method, a pose determination device, pose determination equipment and a storage medium.

Background

When the robot is initialized for the first time in a known map, or when the robot fails to position due to sensor data errors, less environmental information, accumulated errors and the like after the robot runs for a long time, in order to ensure the normal positioning of the robot, relocation is required, including re-determining the position, orientation and the like, namely the pose, of the robot.

The inventor finds that the prior art has the following technical defects in the implementation of the invention:

generally, in lidar based robot positioning applications, it may be set to start at a fixed position. In this method, it is necessary to set some fixed positions in a real use scene and record the coordinates of these positions on a map. These fixed positions may be, for example, robotic charging posts, etc. Further, the robot needs to be placed at the position for starting at each starting time, and the map coordinates of the position are used as initial values of the positioning algorithm. The method has the advantages of quick start and easy realization; the disadvantage is that the use is complicated, the user is required to place the robot at a fixed position for starting, and positioning errors are caused when starting without strictly placing the robot at the fixed position, so that danger is easy to occur when the robot performs a navigation task.

Disclosure of Invention

Embodiments of the present invention provide a pose determination method, apparatus, device, and storage medium, so as to reduce a requirement on a position of a target object when the target object is initialized, increase flexibility of determining an initial pose of the target object, and improve security of starting the target object at the initial pose.

In a first aspect, an embodiment of the present invention provides a pose determination method, where the method includes:

acquiring a first position of a target object based on satellite positioning;

determining a second position of the target object in a pre-constructed map, which is mapped with the first position;

acquiring observation data which is obtained by measurement and aims at a preset mark point when the target object keeps a static state;

and determining the initial pose of the target object in the map within a preset range with the second position as the center based on the observation data.

Further, the determining a second position of the target object mapped with the first position in the pre-constructed map includes:

when a map is constructed, acquiring a third position and a fourth position with the same timestamp, wherein the third position is the position acquired based on satellite positioning, and the fourth position is the position acquired based on the map;

determining a mapping relationship between the third position and the fourth position;

and when the pose of the target object is determined, mapping the first position to the second position according to the mapping relation.

Further, the determining an initial pose of the target object in the map within a preset range centered on the second position based on the observation data includes:

generating a particle swarm composed of particles using a Gaussian distribution within a preset range centered on the second position, the particles being used for representing a predicted pose of the target object, the particle swarm being used for representing a probability distribution of the target object with respect to the predicted pose;

adding random noise to the predicted pose corresponding to each particle to update the predicted pose corresponding to the particle;

based on the observation data, an initial pose of the target object in the map is determined in a probability distribution of predicted poses represented by the population of particles.

Further, the adding random noise to the predicted pose corresponding to each particle to update the predicted pose corresponding to the particle includes:

calculating a first standard deviation of the particle swarm based on the predicted pose of the particle;

determining a probability distribution of random noise based on the first standard deviation;

generating random noise for each of the particles in the population of particles using a probability distribution of the random noise;

and superposing the random noise to the predicted pose of the particle so as to update the predicted pose of the particle.

Further, the determining a probability distribution of random noise according to the first standard deviation includes:

judging whether the first standard deviation is smaller than a preset threshold value or not;

if so, setting the second standard deviation of the random noise as a preset threshold;

if not, setting the second standard deviation of the random noise as the first standard deviation;

and determining the probability distribution of the random noise as the Gaussian distribution meeting the second standard deviation.

Further, the determining an initial pose of the target object in the map in a probability distribution of predicted poses represented by the particle swarm based on the observation data includes:

for each particle in the particle swarm, calculating estimation data of the target object aiming at a preset marking point when the target object is at the particle;

updating the weight value of each particle according to the difference between the observation data and the estimation data;

calculating a weighted average value of the predicted poses in the particle swarm according to the weight value;

and when the weighted average value meets a convergence condition, determining the weighted average value as the initial pose of the target object in the map.

Further, after the calculating a weighted average value of the predicted poses in the particle swarm according to the weight values, the method further includes:

when the weighted average value does not meet the convergence condition, resampling the particles in the particle swarm according to the weighted value;

and continuing to add random noise to the predicted pose corresponding to each particle by using the resampled particle swarm so as to update the predicted pose corresponding to the particle.

Further, the resampling the particles in the particle swarm according to the weight values includes:

and adjusting the number of particles corresponding to each predicted pose in the particle swarm according to the weight value.

In a second aspect, an embodiment of the present invention further provides an apparatus for determining a pose, where the apparatus includes:

the first position acquisition module is used for acquiring a first position of a target object based on satellite positioning;

a second position determination module for determining a second position of the target object mapped with the first position in a pre-constructed map;

the observation data acquisition module is used for acquiring observation data which is obtained by measurement and aims at a preset mark point when the target object keeps a static state;

and the initial pose determining module is used for determining the initial pose of the target object in the map within a preset range taking the second position as the center based on the observation data.

In a third aspect, an embodiment of the present invention further provides a pose determination device, where the pose determination device includes: a memory and one or more processors;

the memory for storing one or more programs;

when executed by the one or more processors, cause the one or more processors to implement the pose determination method according to any one of the first aspects.

In a fourth aspect, embodiments of the present invention also provide a storage medium containing computer-executable instructions, which when executed by a computer processor, are used to perform the pose determination method according to any one of the first aspect.

The embodiment of the invention obtains the first position of the target object through satellite-based positioning; determining a second position of the target object in a pre-constructed map, which is mapped with the first position; acquiring observation data which is obtained by measurement and aims at a preset mark point when the target object keeps a static state; based on the observation data, the initial pose of the target object in the map is determined within a preset range with the second position as the center, the problems of low flexibility and easy error in placing the target object caused by using a fixed position to initialize the position of the target object are solved, the requirement on the position of the target object during initialization is lowered, the flexibility of determining the initial pose of the target object is increased, and the safety of starting the target object at the initial pose is improved.

Drawings

Fig. 1 is a flowchart of a pose determination method according to an embodiment of the present invention;

fig. 2 is a flowchart of a pose determination method according to a second embodiment of the present invention;

fig. 3 is a schematic structural diagram of a pose determination apparatus according to a third embodiment of the present invention;

fig. 4 is a schematic structural diagram of a pose determination apparatus according to a fourth embodiment of the present invention.

Detailed Description

The present invention will be described in further detail with reference to the accompanying drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting of the invention. It should be further noted that, for the convenience of description, only some of the structures related to the present invention are shown in the drawings, not all of the structures.