CN113671530B

CN113671530B - Pose determination method and device, storage medium and electronic equipment

Info

Publication number: CN113671530B
Application number: CN202110899800.1A
Authority: CN
Inventors: 孙晓峰; 孔旗; 张金凤
Original assignee: Beijing Jingdong Qianshi Technology Co Ltd
Current assignee: Beijing Jingdong Qianshi Technology Co Ltd
Priority date: 2021-08-06
Filing date: 2021-08-06
Publication date: 2024-01-12
Anticipated expiration: 2041-08-06
Also published as: CN113671530A

Abstract

The embodiment of the invention provides a pose determining method, a pose determining device, a storage medium and electronic equipment. The method comprises the following steps: calculating the initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired in real time; searching in each collected laser point cloud frame to obtain a key frame; and taking the initial absolute pose of the laser radar sensor corresponding to each key frame as an initial value of the absolute pose, continuously adjusting the absolute pose of the laser radar sensor corresponding to each key frame, so that the sum of the first relative pose residual errors corresponding to all first frame cloud pairs, the second relative pose residual errors corresponding to all second key frame pairs and the third relative pose residual errors corresponding to all third key frame pairs is minimum, and obtaining the optimal absolute pose of the laser radar sensor corresponding to each key frame. The embodiment of the invention improves the calculation accuracy of the pose of the laser radar sensor.

Description

Pose determination method and device, storage medium and electronic equipment

Technical Field

The present invention relates to the field of laser point cloud technologies, and in particular, to a pose determining method and apparatus, a readable storage medium, and an electronic device.

Background

The laser point cloud is point cloud data obtained by continuously emitting a laser beam to a scene target by a laser radar sensor (LiDAR, light Detection and Ranging), and detecting and analyzing the reflected laser signal. Due to the advantages of long detection distance, high detection precision, strong anti-interference capability, insensitivity to illumination environment change and the like, the laser radar sensor is deployed in a large number of automatic driving vehicles, and the three-dimensional point cloud mapping technology based on the laser radar has become one of basic core technologies in the related field of automatic driving.

In a static state, only three-dimensional point clouds in a local range around the laser radar sensor can be obtained through one laser scanning. In order to construct a large-range point cloud map, a laser radar sensor is assembled on a motion carrier such as a vehicle, an airplane and the like to perform continuous scanning, and the three-dimensional pose of the laser radar sensor under a global coordinate system at each moment is calculated. And finally, based on the three-dimensional pose of the laser radar sensor under the global coordinate system at each moment, performing global projection and accumulation on the local point cloud data acquired at each moment to obtain a point cloud map covering the whole scene.

At present, a mapping and mapping method based on high-precision combined inertial navigation is mainly adopted, and the method needs to carry high-precision combined inertial navigation (GNSS (global navigation satellite system) +IMU (inertial navigation unit)) hardware equipment on a motion carrier, directly calculates high-precision three-dimensional pose of a laser radar sensor under a world coordinate system based on a differential positioning and combined navigation technology, and further constructs a point cloud map.

Because the quality of mapping is mainly dependent on the pose accuracy of the laser radar sensor output by the combined inertial navigation system, the quality of GNSS signals is sensitive. Reliable point cloud mapping results cannot be obtained in GNSS signal shielding or weak GNSS signal areas with serious electromagnetic interference, multipath effects and the like such as luxuriant trees, forestation of buildings, spreading of bridge tunnels and the like.

Disclosure of Invention

The embodiment of the invention provides a pose determining method and device, a readable storage medium and electronic equipment, so as to improve the calculation accuracy of the pose of a laser radar sensor.

The technical scheme of the embodiment of the invention is realized as follows:

a pose determination method, the method comprising:

calculating the initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired in real time;

searching in each collected laser point cloud frame to obtain a key frame;

for each key frame, searching a strong signal frame in a preset first neighborhood of the key frame, forming a local subgraph from all the strong signal frames of the key frame, searching laser point clouds registered with the key frame in the local subgraph, calling the key frame and the laser point clouds registered with the key frame as a first frame cloud pair, calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint, and calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint; the strong signal frame is: when the laser point cloud frame is acquired, a GNSS signal intensity value is not smaller than a frame of a preset signal intensity threshold;

Each two adjacent key frames are called a second key frame pair, the relative pose between the two key frames in the second key frame pair is called a second relative pose, and the second relative pose constraint between the two key frames in the second key frame pair is calculated according to the initial absolute pose of the laser radar sensor corresponding to the two key frames in the second key frame pair respectively;

each pair of key frames meeting the loop constraint condition is called a third key frame pair, the relative pose between two key frames in the third key frame pair is called a third relative pose, and the third relative pose constraint between the two key frames is calculated according to the inter-frame matching degree of the two key frames in the third key frame pair;

the initial absolute pose of the laser radar sensor corresponding to each key frame is taken as an initial absolute pose value, the absolute pose of the laser radar sensor corresponding to each key frame is continuously adjusted, so that the sum of the first relative pose residual errors corresponding to all first frame cloud pairs, the second relative pose residual errors corresponding to all second key frame pairs and the third relative pose residual errors corresponding to all third key frame pairs is minimum, the optimal absolute pose of the laser radar sensor corresponding to each key frame is obtained,

The first relative pose residual error corresponding to the first frame cloud pair is used for measuring the difference value between the first relative pose of the first frame cloud pair and the first relative pose constraint, the second relative pose residual error corresponding to the second key frame pair is used for measuring the difference value between the second relative pose of the second key frame pair and the second relative pose constraint, and the third relative pose residual error corresponding to the third key frame pair is used for measuring the difference value between the third relative pose of the third key frame pair and the third relative pose constraint.

The initial absolute pose of the laser radar sensor when each laser point cloud frame is calculated and acquired in real time comprises the following steps:

calculating the initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired in real time by adopting a mapping and mapping method based on GNSS+inertial navigation unit IMU; the laser radar sensor is carried on the mobile equipment and continuously emits a laser beam to a scene target;

the searching in each collected laser point cloud frame to obtain a key frame includes:

determining whether the initial absolute pose of the laser radar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired according to the GNSS signal intensity value when each laser point cloud frame is acquired;

Each group of continuous weak signal pose is respectively used as a weak signal communication track segment;

and searching in the laser point cloud frames corresponding to each weak signal communication track segment to obtain the key frames.

Determining whether the initial absolute pose of the laser radar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired according to the GNSS signal intensity value when each laser point cloud frame is acquired, comprising:

for each laser point cloud frame, if the GNSS signal intensity value when the frame is acquired is smaller than a preset signal intensity threshold, the initial absolute pose of the laser radar sensor when the frame is acquired is a weak signal pose, otherwise, the initial absolute pose of the laser radar sensor is a strong signal pose.

When it is determined whether the initial absolute pose of the lidar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired,

the pose signal of the pose with weak signal is marked as 0, the pose signal of the pose with strong signal is marked as 1,

for the initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired, calculating the sum of pose signal marks of all initial absolute poses in a preset second field of the initial absolute pose, if the sum is smaller than a preset first threshold, finally determining that the initial absolute pose is a weak signal pose, otherwise, finally determining that the initial absolute pose is a strong signal pose.

Searching in the laser point cloud frames corresponding to each weak signal communication track segment to obtain the key frames, wherein the key frames comprise:

for each weak signal communication track segment, all laser points Yun Zhen corresponding to the track segment are acquired;

sequencing all the obtained laser points Yun Zhen from head to tail according to the corresponding positions of the obtained laser point cloud frames on the track segment from head to tail;

taking the frames arranged at the first and last positions as key frames;

for each frame except the first and last frames, if the track length between the frame and the nearest key frame arranged in front of the frame is greater than a preset second threshold value, the frame is taken as the key frame; or if the angle of deviation between the frame and the nearest key frame arranged in front of the frame is greater than a preset third threshold value, the frame is taken as the key frame.

After searching the acquired each laser point cloud frame to obtain a key frame, for each key frame, before searching a strong signal frame in a preset first neighborhood of the key frame, the method further includes:

for each key frame, searching for key frames meeting the following first condition and the following second condition in all key frames which are not located in the same weak signal communication track segment with the key frame, searching for key frames meeting the following first condition, the following second condition and the following third condition in all key frames which are located in the same weak signal communication track segment with the key frame, and taking the key frame and each searched key frame as a key frame pair meeting the loop constraint condition respectively;

First condition: the Euclidean distance between the positions of the laser radar sensor when the two key frames are acquired is smaller than a preset fourth threshold value;

second condition: the inter-frame matching degree between the two key frames is larger than a preset fifth threshold;

third condition: the track length of the laser radar sensor when the two key frames are acquired is greater than a preset sixth threshold.

After the key frame and each searched key frame are respectively used as a key frame pair meeting the loop constraint condition, the method further comprises the following steps:

for each key frame pair meeting the loop constraint condition, if two key frames in the key frame pair correspond to different weak signal communication track segments respectively, according to the principle that the corresponding points of the two key frames in the weak signal communication track segments coincide, the weak signal communication track segments corresponding to the two key frames are aggregated into a weak signal communication track segment subset.

For each key frame, searching a strong signal frame in a preset first neighborhood of the key frame comprises the following steps:

for each key frame, searching a strong signal frame positioned in a preset first adjacent area of the key frame in a weak signal communication track fragment subset corresponding to the key frame;

The step of designating each two adjacent key frames as a second key frame pair includes:

and taking two adjacent key frames which are positioned in the same weak signal communication track segment subset as a second key frame pair.

After obtaining the optimal absolute pose of the laser radar sensor corresponding to each key frame, the method further comprises:

for each second key frame pair, calculating the first inter-frame matching degree of the two key frames according to the initial absolute pose of the laser radar sensor corresponding to the two key frames in the second key frame pair; calculating a second inter-frame matching degree of the two key frames according to the optimal absolute pose of the laser radar sensor corresponding to the two key frames in the second key frame pair;

taking the larger one of the first inter-frame matching degree and the second inter-frame matching degree, and taking the relative pose between the two key frames corresponding to the larger one as a new second relative pose constraint;

the optimal absolute pose of the laser radar sensor corresponding to each key frame is taken as an absolute pose initial value, the absolute pose of the laser radar sensor corresponding to each key frame is continuously adjusted, so that the sum of all first relative pose residuals corresponding to all first frame cloud pairs, all second relative pose residuals corresponding to all second key frame pairs and all third relative pose residuals corresponding to all third key frame pairs is minimum, the final optimal absolute pose of the laser radar sensor corresponding to each key frame is obtained,

The first relative pose residual corresponding to the first frame cloud pair is used for measuring the difference value between the first relative pose of the first frame cloud pair and the first relative pose constraint, the second relative pose residual corresponding to the second key frame pair is used for measuring the difference value between the second relative pose of the second key frame pair and the new second relative pose constraint, and the third relative pose residual corresponding to the third key frame pair is used for measuring the difference value between the third relative pose of the third key frame pair and the third relative pose constraint.

A pose determination apparatus, the apparatus comprising:

the initial absolute pose calculation module is used for calculating the initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired in real time;

the key frame searching module is used for searching in each acquired laser point cloud frame to obtain a key frame;

the optimal absolute pose determining module is used for:

for each key frame, searching a strong signal frame in a preset first neighborhood of the key frame, forming a local subgraph from all the strong signal frames of the key frame, searching laser point clouds registered with the key frame in the local subgraph, calling the key frame and the laser point clouds registered with the key frame as a first frame cloud pair, calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint, and calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint; the strong signal frame is: when the laser point cloud frame is acquired, a GNSS signal strength value is not smaller than a frame of a preset signal strength threshold value; and, in addition, the method comprises the steps of,

Each two adjacent key frames are called a second key frame pair, the relative pose between the two key frames in the second key frame pair is called a second relative pose, and the second relative pose constraint between the two key frames in the second key frame pair is calculated according to the initial absolute pose of the laser radar sensor corresponding to the two key frames in the second key frame pair respectively; and, in addition, the method comprises the steps of,

each pair of key frames meeting the loop constraint condition is called a third key frame pair, the relative pose between two key frames in the third key frame pair is called a third relative pose, and the third relative pose constraint between the two key frames is calculated according to the inter-frame matching degree of the two key frames in the third key frame pair; and, in addition, the method comprises the steps of,

A non-transitory computer readable storage medium storing instructions that, when executed by a processor, cause the processor to perform the steps of the pose determination method according to any of the above claims.

An electronic device comprising a non-transitory computer readable storage medium as described above, and the processor having access to the non-transitory computer readable storage medium.

In the embodiment of the invention, the key frames and the laser point clouds registered with the key frames are called a first frame cloud pair, every two adjacent key frames are called a second key frame pair, every pair of key frames meeting the loop constraint condition is called a third key frame pair, the initial absolute pose of the laser radar sensor corresponding to each key frame is taken as the initial absolute pose value of the laser radar sensor, the absolute pose of the laser radar sensor corresponding to each key frame is continuously adjusted, so that the sum of the first relative pose residual errors corresponding to all the first frame cloud pairs, the second relative pose residual errors corresponding to all the second key frame pairs and the third relative pose residual errors corresponding to all the third key frame pairs is minimum, the optimal absolute pose of the laser radar sensor corresponding to each key frame is obtained, the pose calculation precision of the laser radar sensor is improved, and the precision of the final point cloud map can be improved.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the description of the embodiments will be briefly described below, it being obvious that the drawings in the following description are only some embodiments of the present invention, and that other drawings may be obtained according to these drawings without inventive effort to a person skilled in the art.

FIG. 1 is a flow chart of a pose determining method according to an embodiment of the present invention;

FIG. 2 is an exemplary diagram of 7 weak signal communication trace segments aggregated into a subset of 3 communication trace segments, provided by an embodiment of the invention;

FIG. 3 is a flowchart of a pose determining method according to another embodiment of the present invention;

fig. 4 is a schematic structural diagram of a pose determining device according to an embodiment of the present invention;

fig. 5 is an exemplary structural schematic diagram of an electronic device according to an embodiment of the present invention.

Detailed Description

The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.

The terms "first," "second," "third," "fourth" and the like in the description and in the claims and in the above drawings, if any, are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the invention described herein may be implemented, for example, in sequences other than those illustrated or otherwise described herein. Furthermore, the terms "comprise" and "have," as well as any variations thereof, are intended to cover a non-exclusive inclusion. For example, a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those elements but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.

The technical scheme of the invention is described in detail below by specific examples. The following embodiments may be combined with each other, and some embodiments may not be repeated for the same or similar concepts or processes.

Fig. 1 is a flowchart of a pose determining method according to an embodiment of the present invention, which specifically includes the following steps:

Step 101: and calculating the initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired in real time.

Specifically, an initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired is calculated in real time by adopting a mapping and mapping method based on GNSS+IMU; wherein, the laser radar sensor is carried on the mobile device and continuously emits a laser beam to the scene object.

That is, the absolute pose of the laser radar sensor is taken as the initial absolute pose when each laser point cloud frame is acquired by adopting a mapping method based on GNSS+IMU.

Step 102: and searching in each acquired laser point cloud frame to obtain a key frame.

Step 103: for each key frame, searching a strong signal frame in a preset first neighborhood of the key frame, forming a local subgraph from all the strong signal frames of the key frame, searching laser point clouds registered with the key frame in the local subgraph, calling the key frame and the laser point clouds registered with the key frame as a first frame cloud pair, calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint, and calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint; the strong signal frame is: when the laser point cloud frame is acquired, a GNSS signal strength value is not smaller than a frame of a preset signal strength threshold value; and taking the laser point cloud registered with the key frame as a virtual key frame.

Since the laser point cloud registration algorithm is a mature algorithm, the description of the laser point cloud registration algorithm is omitted in this embodiment.

Each first frame cloud pair corresponds to a first relative pose constraint, and each first frame cloud pair can calculate a first relative pose.

The difference between the absolute poses of the laser radar sensors corresponding to the two laser point cloud frames respectively forms the relative pose between the two laser point cloud frames. The absolute pose is generally composed of a plurality of position parameters and a plurality of pose parameters, so that the relative pose between the two laser point cloud frames can be obtained by correspondingly subtracting the values of the plurality of position parameters and the values of the plurality of pose parameters corresponding to the two laser point cloud frames.

The radius of the first field may be determined empirically, etc., for example: and searching a strong signal frame acquired when the laser radar sensor is positioned in a circle with the position of the laser radar sensor as the center and the preset radius r as the radius.

Step 104: and (3) each two adjacent key frames are called a second key frame pair, the relative pose between the two key frames in the second key frame pair is called a second relative pose, and the second relative pose constraint between the two key frames in the second key frame pair is calculated according to the initial absolute pose of the laser radar sensor corresponding to the two key frames in the second key frame pair.

Step 105: and each pair of key frames meeting the loop constraint condition is called a third key frame pair, the relative pose between two key frames in the third key frame pair is called a third relative pose, and the third relative pose constraint between the two key frames is calculated according to the inter-frame matching degree of the two key frames in the third key frame pair.

Step 106: the initial absolute pose of the laser radar sensor corresponding to each key frame is taken as an initial absolute pose value, the absolute pose of the laser radar sensor corresponding to each key frame is continuously adjusted, so that the sum of the first relative pose residual errors corresponding to all first frame cloud pairs, the second relative pose residual errors corresponding to all second key frame pairs and the third relative pose residual errors corresponding to all third key frame pairs is minimum, the optimal absolute pose of the laser radar sensor corresponding to each key frame is obtained,

In the above embodiment, the key frames and the laser point clouds registered with the key frames are referred to as a first frame cloud pair, every two adjacent key frames are referred to as a second key frame pair, every pair of key frames meeting the loop constraint condition is referred to as a third key frame pair, the initial absolute pose of the laser radar sensor corresponding to each key frame is used as the initial absolute pose value of the laser radar sensor corresponding to each key frame, the absolute poses of the laser radar sensor corresponding to each key frame are continuously adjusted, so that the sum of the first relative pose residual errors corresponding to all the first frame cloud pairs, the second relative pose residual errors corresponding to all the second key frame pairs and the third relative pose residual errors corresponding to all the third key frame pairs is minimum, and therefore, the optimal absolute pose of the laser radar sensor corresponding to each key frame is obtained, the pose calculation precision of the laser radar sensor is improved, and the precision of the final point cloud map can be improved.

In an optional embodiment, in step 102, in each collected laser point cloud frame, a search is performed to obtain a key frame, which specifically includes: determining whether the initial absolute pose of the laser radar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired according to the GNSS signal intensity value when each laser point cloud frame is acquired; each group of continuous weak signal pose is respectively used as a weak signal communication track segment; and searching in the laser point cloud frames corresponding to each weak signal communication track segment to obtain a key frame.

By the embodiment, the key frames are searched only in the weak signal communication track segments, so that pose optimization is performed on the weak signal frames, the computational complexity is reduced, and the pose calculation accuracy of the laser radar sensor can be improved when GNSS signals are weak.

In an alternative embodiment, determining whether the initial absolute pose of the laser radar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired according to the GNSS signal intensity value when each laser point cloud frame is acquired specifically includes: for each laser point cloud frame, if the GNSS signal intensity value when the frame is acquired is smaller than a preset signal intensity threshold, the initial absolute pose of the laser radar sensor when the frame is acquired is a weak signal pose, otherwise, the initial absolute pose of the laser radar sensor is a strong signal pose.

Correspondingly, the laser point cloud frame corresponding to the weak signal pose is called a weak signal frame, and the laser point cloud frame corresponding to the strong signal pose is called a strong signal frame.

For example: let s be _i To acquire the GNSS signal strength value at the ith frame, L _i In order to acquire a binarization tag of a strong signal pose and a weak signal pose corresponding to an initial absolute pose of a laser radar sensor in an ith frame, wherein 1 represents the strong signal pose, 0 represents the weak signal pose and theta _s If the signal strength threshold is preset, then:

the value of the preset signal strength threshold may be set empirically, etc.

In an alternative embodiment, in order to obtain a smooth and continuous weak signal communication track segment, based on strong and weak signal pose information corresponding to each frame in the neighborhood of each frame, the obtained strong and weak signal pose information corresponding to each frame is subjected to smooth low-pass filtering, which specifically includes the following steps:

when determining that the initial absolute pose of the laser radar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired, setting a pose signal mark of the weak signal pose as 0 and a pose signal mark of the strong signal pose as 1; for the initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired, calculating the sum of pose signal marks of all initial absolute poses in a preset second field of the initial absolute pose, if the sum is smaller than a preset first threshold, finally determining that the initial absolute pose is a weak signal pose, otherwise, finally determining that the initial absolute pose is a strong signal pose.

The values of the first threshold and the radius of the second neighborhood may be set empirically, etc.

For example: is provided withFor the binarization label of the strong and weak signal pose corresponding to the initial absolute pose of the laser radar sensor in the i-th frame acquisition after smoothing, n is a preset neighborhood radius, phi _i Is the neighborhood set corresponding to the ith frame and phi _i ∈[i-n,i+n]。

The value of n can be set empirically, etc.

In an alternative embodiment, searching is performed in the laser point cloud frames corresponding to each weak signal communication track segment to obtain a key frame, which specifically includes:

for each weak signal communication track segment, all laser points Yun Zhen corresponding to the track segment are acquired; sequencing all the obtained laser points Yun Zhen from head to tail according to the corresponding positions of the obtained laser point cloud frames on the track segment from head to tail; taking the frames arranged at the head and tail as key frames; for each frame except for the frames arranged in the first and last bits, if the track length between the frame and the nearest key frame arranged in front of the frame is greater than a preset second threshold value, the frame is taken as the key frame; or if the angle of deviation between the frame and the nearest key frame arranged in front of the frame is greater than a preset third threshold value, the frame is taken as the key frame.

The values of the second threshold and the third threshold may be set empirically or the like.

In an alternative embodiment, in order to eliminate accumulated errors caused by combined inertial navigation interframe bit pushing in the weak signal communication track segments, and ensure consistency of point cloud mapping results of overlapping areas among the weak signal communication track segments, a interframe matching algorithm is used to establish interframe loop-back constraint, which is specifically as follows:

After step 102 and before step 103, the method further comprises: for each key frame, searching for key frames meeting the following first condition and second condition in all key frames which are not located in the same weak signal communication track segment with the key frame, searching for key frames meeting the following first condition, second condition and third condition in all key frames which are located in the same weak signal communication track segment with the key frame, and taking the key frame and each searched key frame as a key frame pair meeting the loop constraint condition respectively:

The values of the fourth threshold, the fifth threshold, and the sixth threshold may be empirically set.

In an alternative embodiment, after loop detection is completed, a connection between key frames in the overlapping region between the weak signal connection track segments is established, so that a connection between the weak signal connection track segments can be established through the connection between the key frames, and the weak signal connection track segments with overlapping regions in space are aggregated into a connection track segment subset. The method comprises the following steps:

After the key frame and each searched key frame are respectively used as a key frame pair meeting the loop constraint condition, the method further comprises the following steps: for each key frame pair meeting the loop constraint condition, if two key frames in the key frame pair correspond to different weak signal communication track segments respectively, according to the principle that the corresponding points of the two key frames in the weak signal communication track segments coincide, the weak signal communication track segments corresponding to the two key frames are aggregated into a weak signal communication track segment subset.

Fig. 2 gives an example in which 7 weakly signal connected track segments are aggregated into a subset of 3 weakly signal connected track segments.

In an alternative embodiment, for each key frame, searching for a strong signal frame in a preset first neighborhood of the key frame includes: for each key frame, searching a strong signal frame positioned in a preset first adjacent area of the key frame in a weak signal communication track fragment subset corresponding to the key frame; and, every two adjacent key frames are called a second key frame pair, including: and taking two adjacent key frames which are positioned in the same weak signal communication track segment subset as a second key frame pair.

In an alternative embodiment, in order to reduce the influence of the adjacent inter-frame errors caused by the excessively weak signals on the final point cloud imaging precision, the embodiment of the present invention further optimizes the optimal absolute pose of each key frame obtained in step 106, which is specifically as follows:

After step 106, further comprising:

The above process can be repeated multiple times to obtain absolute poses with higher precision for each key frame.

Fig. 3 is a flowchart of a pose determining method according to another embodiment of the present invention, which specifically includes the following steps:

step 301: calculating the initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired in real time by adopting a mapping and mapping method based on GNSS+IMU; wherein, the laser radar sensor is carried on the mobile device and continuously emits a laser beam to the scene object.

Step 302: and determining whether the initial absolute pose of the laser radar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired according to the GNSS signal intensity value when each laser point cloud frame is acquired.

Step 303: and respectively taking each group of continuous weak signal pose as a weak signal communication track segment, and searching a key frame in the laser point cloud frames corresponding to each weak signal communication track segment.

Specifically, for each weak signal communication track segment, all laser points Yun Zhen corresponding to the track segment are acquired; sequencing all the obtained laser points Yun Zhen from head to tail according to the corresponding positions of the obtained laser point cloud frames on the track segment from head to tail; taking the frames arranged at the head and tail as key frames; for each frame except for the frames arranged in the first and last bits, if the track length between the frame and the nearest key frame arranged in front of the frame is greater than a preset second threshold value, the frame is taken as the key frame; or if the angle of deviation between the frame and the nearest key frame arranged in front of the frame is greater than a preset third threshold value, the frame is taken as the key frame.

Step 304: for each key frame, searching for key frames meeting the following first condition and the following second condition in all key frames which are not located in the same weak signal communication track segment with the key frame, searching for key frames meeting the following first condition, the following second condition and the following third condition in all key frames which are located in the same weak signal communication track segment with the key frame, and taking the key frame and each searched key frame as a key frame pair meeting the loop constraint condition respectively;

Step 305: for each key frame pair meeting the loop constraint condition, if two key frames in the key frame pair correspond to different weak signal communication track segments respectively, according to the principle that the corresponding points of the two key frames in the weak signal communication track segments coincide, the weak signal communication track segments corresponding to the two key frames are aggregated into a weak signal communication track segment subset.

Step 306: for each key frame, searching a strong signal frame positioned in a preset first adjacent area of the key frame in a weak signal communication track fragment subset corresponding to the key frame; forming a local subgraph from all strong signal frames of the key frame, searching laser point clouds registered with the key frame in the local subgraph, enabling the key frame and the laser point clouds registered with the key frame to be called as a first frame cloud pair, enabling relative pose between the key frame and the laser point clouds in the first frame cloud pair to be called as a first relative pose, and enabling the relative pose between the key frame and the laser point clouds in the first frame cloud pair to be used as a first relative pose constraint; and taking the laser point cloud registered with the key frame as a virtual key frame.

Step 307: and taking two adjacent keyframes which are positioned in the same weak signal communication track segment subset as a second keyframe pair, and taking the relative pose between the two keyframes in the second keyframe pair as a second relative pose, and calculating the second relative pose constraint between the two keyframes in the second keyframe pair according to the initial absolute pose of the laser radar sensor respectively corresponding to the two keyframes in the second keyframe pair.

And calculating the relative pose between the two key frames in the second key frame pair according to the initial absolute pose of the laser radar sensor respectively corresponding to the two key frames in the second key frame pair, wherein the relative pose is the second relative pose constraint.

Step 308: and each pair of key frames meeting the loop constraint condition is called a third key frame pair, the relative pose between two key frames in the third key frame pair is called a third relative pose, and the third relative pose constraint between the two key frames is calculated according to the inter-frame matching degree of the two key frames in the third key frame pair.

Point cloud registration (Point Cloud Registration) refers to inputting two point clouds P _s (Source) and P _t (target) outputting a transformation matrix T so that the transformed source point cloud T (P _s ) And target point cloud T (P) _t ) The degree of overlap of (2) is as high as possible. Because the relative pose between the two point clouds can be directly obtained by decomposition according to the transformation matrix T, in the embodiment of the present invention, the third relative pose constraint and the second relative pose constraint can be respectively obtained by the point cloud registration algorithm in the process of "inter-frame matching" related to the second condition in step 304 and "searching the laser point cloud registered with the key frame in the local subgraph" related to step 306.

It should be noted that, in the existing disclosed point cloud registration algorithm, a method based on ICP (Iterative Closest Point ) or GICP (Iterative Closest Point, generalized iterative closest point), a method based on NDT (Normal Distribution Transform, normal distribution transformation), and a feature matching method extracted based on manual design or machine learning are all applicable to the present invention.

Step 309: the initial absolute pose of the laser radar sensor corresponding to each key frame is taken as an initial absolute pose value, the absolute pose of the laser radar sensor corresponding to each key frame is continuously adjusted, so that the sum of the first relative pose residual errors corresponding to all first frame cloud pairs, the second relative pose residual errors corresponding to all second key frame pairs and the third relative pose residual errors corresponding to all third key frame pairs is minimum, the optimal absolute pose of the laser radar sensor corresponding to each key frame is obtained,

For example: the keyframes A, B are adjacent keyframes, namely a second keyframe pair, the initial absolute pose of the lidar sensor corresponding to the keyframe A, B is taken as an absolute pose initial value, the absolute pose of the lidar sensor corresponding to the keyframe A, B is continuously adjusted, and each time the absolute pose of the lidar sensor corresponding to the keyframe is adjusted, the relative pose between the keyframes A, B, namely the second relative pose between the keyframes A, B, can be calculated according to the absolute pose of the lidar sensor corresponding to the adjusted keyframe A, B, and then residual error calculation is performed on the second relative pose and the second relative pose constraint, so as to obtain a second relative pose residual error.

The sum of the first relative pose residuals corresponding to all first frame cloud pairs, the second relative pose residuals corresponding to all second key frame pairs, and the third relative pose residuals corresponding to all third key frame pairs may be expressed by the following formula:

F(X)＝E _M (X)+E _G (X)+E _L (X) (3)

wherein X represents a set of initial absolute poses of the lidar sensor corresponding to each keyframe, E _M (X) represents the sum of the first relative pose residuals corresponding to each first frame cloud pair, E _G (X) represents the sum of the second keyframes and the corresponding second relative pose residuals, E _L (X) represents the sum of the corresponding third pose residuals for each third keyframe pair.

E _M (X)、E _G (X)、E _L The expression of (X) is the same as E _M (X) is exemplified by:

wherein M is a first frame cloud pair set, i, j is a key in any first frame cloud pairNumbering of frames and laser point clouds (which can be seen as virtual key frames), X _i 、X _j The absolute pose vectors corresponding to the key frame i and the laser point cloud j in the first frame cloud pair are respectively; r is R _i,j A first relative pose constraint vector between the key frame i and the laser point cloud j; the manifold () is a manifold conversion function, is used for measuring the function of the difference between the first relative pose and the first relative pose constraint between the key frame i and the laser point cloud j, is used for converting the absolute pose vector and the first relative pose constraint vector corresponding to the key frame i and the laser point cloud j into residual vectors, and the specific expression of the manifold () can adopt the existing mature expression; omega shape _i,j Is an information matrix and can be set according to experience; t is the matrix transpose operator and-1 is the matrix inversion operator.

The equation (3) can be solved by the existing Levenberg-Marquardt method to obtain the optimal absolute pose of the laser radar sensor corresponding to each key frame or each laser point cloud (which can be regarded as a virtual key frame).

After obtaining the optimal absolute pose of the laser radar sensor corresponding to each key frame, replacing the initial absolute pose of the corresponding frame by the optimal absolute pose, so as to obtain an absolute pose set corresponding to all laser point cloud frames. Thereafter, each frame L in the original set ρ of laser spots Yun Zhen is sequentially set according to equation (4) _i Projecting to a world coordinate system, and accumulating laser point clouds of each frame to obtain a high-precision point cloud map Q, wherein in the formulaAnd the projection transformation matrix corresponding to the laser point cloud of the ith frame.

Fig. 4 is a schematic structural diagram of a pose determining device according to an embodiment of the present invention, where the pose determining device mainly includes:

the initial absolute pose calculating module 41 is configured to calculate an initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired in real time.

Specifically, an initial absolute pose of the laser radar sensor when each laser point cloud frame is acquired can be calculated in real time by adopting a mapping and mapping method based on GNSS+IMU; wherein, the laser radar sensor is carried on the mobile device and continuously emits a laser beam to the scene object.

The key frame searching module 42 is configured to search for a key frame in each collected laser point cloud frame.

An optimal absolute pose determination module 43 for:

for each key frame, searching a strong signal frame in a preset first neighborhood of the key frame, forming a local subgraph from all the strong signal frames of the key frame, searching laser point clouds registered with the key frame in the local subgraph, calling the key frame and the laser point clouds registered with the key frame as a first frame cloud pair, calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint, and calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint; the strong signal frame is: when the laser point cloud frame is acquired, a GNSS signal strength value is not smaller than a frame of a preset signal strength threshold value; the laser point cloud registered with the key frame is used as a virtual key frame; and, in addition, the method comprises the steps of,

In an alternative embodiment, the key frame searching module 42 searches for a key frame in each collected laser point cloud frame includes: determining whether the initial absolute pose of the laser radar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired according to the GNSS signal intensity value when each laser point cloud frame is acquired; each group of continuous weak signal pose is respectively used as a weak signal communication track segment; and searching in the laser point cloud frames corresponding to each weak signal communication track segment to obtain a key frame.

In an alternative embodiment, the key frame searching module 42 determines, according to the GNSS signal strength value when each laser point cloud frame is acquired, whether the initial absolute pose of the lidar sensor is a weak signal pose or a strong signal pose when each laser point cloud frame is acquired, including: for each laser point cloud frame, if the GNSS signal intensity value when the frame is acquired is smaller than a preset signal intensity threshold, the initial absolute pose of the laser radar sensor when the frame is acquired is a weak signal pose, otherwise, the initial absolute pose of the laser radar sensor is a strong signal pose.

In an alternative embodiment, the key frame search module 42 sets the pose signal flag of the weak signal pose to 0 and the pose signal flag of the strong signal pose to 1 when determining that the initial absolute pose of the laser radar sensor is the weak signal pose or the strong signal pose when collecting each laser point cloud frame, calculates the sum of the pose signal flags of all the initial absolute poses in the preset second area of the initial absolute pose when collecting each laser point cloud frame, and finally determines that the initial absolute pose is the weak signal pose if the sum is smaller than the preset first threshold, otherwise, finally determines that the initial absolute pose is the strong signal pose.

In an alternative embodiment, the key frame searching module 42 searches for a key frame in the laser point cloud frame corresponding to each weak signal connection track segment, where the searching includes: for each weak signal communication track segment, all laser points Yun Zhen corresponding to the track segment are acquired; sequencing all the obtained laser points Yun Zhen from head to tail according to the corresponding positions of the obtained laser point cloud frames on the track segment from head to tail; taking the frames arranged at the head and tail as key frames; for each frame except for the frames arranged in the first and last bits, if the track length between the frame and the nearest key frame arranged in front of the frame is greater than a preset second threshold value, the frame is taken as the key frame; or if the angle of deviation between the frame and the nearest key frame arranged in front of the frame is greater than a preset third threshold value, the frame is taken as the key frame.

In an alternative embodiment, the apparatus further comprises: the loop detection module is used for searching for a key frame meeting the following first condition and the following second condition from all key frames which are not located in the same weak signal communication track segment with the key frame, searching for a key frame meeting the following first condition, the following second condition and the following third condition from all key frames which are located in the same weak signal communication track segment with the key frame, and taking the key frame and each searched key frame as a key frame pair meeting the loop constraint condition respectively;

In an alternative embodiment, after the loop detection module uses the key frame and each searched key frame as a key frame pair that satisfies the loop constraint condition, the loop detection module further includes: for each key frame pair meeting the loop constraint condition, if two key frames in the key frame pair correspond to different weak signal communication track segments respectively, according to the principle that the corresponding points of the two key frames in the weak signal communication track segments coincide, the weak signal communication track segments corresponding to the two key frames are aggregated into a weak signal communication track segment subset.

In an alternative embodiment, the optimal absolute pose determining module 43 searches, for each key frame, a strong signal frame in a preset first neighborhood of the key frame, including: for each key frame, searching a strong signal frame positioned in a preset first adjacent area of the key frame in a weak signal communication track fragment subset corresponding to the key frame;

The optimal absolute pose determination module 43 refers to each two neighboring key frames as a second key frame pair, comprising: and taking two adjacent key frames which are positioned in the same weak signal communication track segment subset as a second key frame pair.

In an alternative embodiment, after the optimal absolute pose determining module 43 obtains the optimal absolute pose of the lidar sensor corresponding to each keyframe, the method further includes:

for each second key frame pair, calculating the first inter-frame matching degree of the two key frames according to the initial absolute pose of the laser radar sensor corresponding to the two key frames in the second key frame pair; calculating a second inter-frame matching degree of the two key frames according to the optimal absolute pose of the laser radar sensor corresponding to the two key frames in the second key frame pair; and, in addition, the method comprises the steps of,

taking the larger one of the first inter-frame matching degree and the second inter-frame matching degree, and taking the relative pose between the two key frames corresponding to the larger one as a new second relative pose constraint; and, in addition, the method comprises the steps of,

Embodiments also provide a computer readable storage medium storing instructions that, when executed by a processor, may perform the steps in the pose determination method as described above. In practice, the computer readable medium may be comprised by or separate from the apparatus/device/system of the above embodiments, and may not be incorporated into the apparatus/device/system. Wherein the instructions are stored in a computer readable storage medium, which stored instructions, when executed by a processor, may perform the steps as in the pose determination method above.

According to embodiments disclosed herein, the computer-readable storage medium may be a non-volatile computer-readable storage medium, which may include, for example, but is not limited to: portable computer diskette, hard disk, random Access Memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), portable compact disc read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination of the foregoing, but are not intended to limit the scope of the protection herein. In the embodiments disclosed herein, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.

As shown in fig. 5, the embodiment of the invention further provides an electronic device. As shown in fig. 5, a schematic structural diagram of an electronic device according to an embodiment of the present invention is shown, specifically:

the electronic device may include a processor 51 of one or more processing cores, a memory 52 of one or more computer-readable storage media, and a computer program stored on the memory and executable on the processor. The above-described pose determination method may be implemented when the program of the memory 52 is executed.

Specifically, in practical application, the electronic device may further include a power supply 53, an input/output unit 55, and other components. It will be appreciated by those skilled in the art that the structure of the electronic device shown in fig. 5 is not limiting of the electronic device and may include more or fewer components than shown, or may combine certain components, or a different arrangement of components. Wherein:

the processor 51 is a control center of the electronic device, connects various parts of the entire electronic device using various interfaces and lines, and performs various functions of a server and processes data by running or executing software programs and/or modules stored in the memory 52 and calling data stored in the memory 52, thereby performing overall monitoring of the electronic device.

The memory 52 may be used to store software programs and modules, i.e., the computer-readable storage media described above. The processor 51 executes various functional applications and data processing by running software programs and modules stored in the memory 52. The memory 52 may mainly include a storage program area and a storage data area, wherein the storage program area may store an operating system, application programs required for at least one function, and the like; the storage data area may store data created according to the use of the server, etc. In addition, memory 52 may include high-speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other volatile solid-state storage device. Accordingly, the memory 52 may also include a memory controller to provide access to the memory 52 by the processor 51.

The electronic device further comprises a power supply 53 for supplying power to the various components, which may be logically connected to the processor 51 via a power management system, so that functions of managing charging, discharging, power consumption management, etc. are achieved via the power management system. The power supply 53 may also include one or more of any of a direct current or alternating current power supply, a recharging system, a power failure detection circuit, a power converter or inverter, a power status indicator, and the like.

The electronic device may also include an input output unit 54, which input unit output 54 may be used to receive input numeric or character information and to generate keyboard, mouse, joystick, optical or trackball signal inputs related to user settings and function control. The input unit output 54 may also be used to display information entered by a user or provided to a user as well as various graphical user interfaces that may be composed of graphics, text, icons, video, and any combination thereof.

The flowcharts and block diagrams in the figures of the present application illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams or flowchart illustration, and combinations of blocks in the block diagrams or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.

Those skilled in the art will appreciate that the features recited in the various embodiments of the disclosure and/or the claims may be combined in various combinations and/or combinations, even if such combinations or combinations are not explicitly recited in the present application. In particular, the features recited in the various embodiments and/or claims of the present application may be combined in various combinations and/or combinations without departing from the spirit and teachings of the application, all of which are within the scope of the disclosure.

The principles and embodiments of the present invention have been described herein with reference to specific examples, which are intended to be included herein for purposes of illustration only and not to be limiting of the invention. It will be apparent to those skilled in the art that variations can be made in the present embodiments and in the scope of the application in accordance with the spirit and principles of the present invention, and any modifications, equivalent substitutions, improvements, etc. are intended to be included within the scope of the present application.

Claims

1. A pose determination method, characterized in that the method comprises:

Searching in each collected laser point cloud frame to obtain a key frame;

for each key frame, searching a strong signal frame in a preset first neighborhood of the key frame, forming a local subgraph from all the strong signal frames of the key frame, searching laser point clouds registered with the key frame in the local subgraph, calling the key frame and the laser point clouds registered with the key frame as a first frame cloud pair, calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint, and calling the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint; the strong signal frame is: when the laser point cloud frame is acquired, a GNSS signal intensity value is not smaller than a frame of a preset signal intensity threshold; the laser point cloud registered with the key frame is used as a virtual key frame;

2. The method of claim 1, wherein calculating an initial absolute pose of the lidar sensor for each laser point cloud frame in real time comprises:

3. The method of claim 2, wherein determining whether the initial absolute pose of the lidar sensor at the time of acquisition of each laser point cloud frame is a weak signal pose or a strong signal pose based on the GNSS signal strength value at the time of acquisition of each laser point cloud frame comprises:

4. The method of claim 3, wherein when determining whether the initial absolute pose of the lidar sensor is a weak signal pose or a strong signal pose at the time of acquisition of each laser point cloud frame,

5. The method of claim 2, wherein the searching in the laser point cloud frames corresponding to each weak signal communication track segment to obtain the key frame includes:

taking the frames arranged at the head and tail as key frames;

for each frame except for the frames arranged in the first and last bits, if the track length between the frame and the nearest key frame arranged in front of the frame is greater than a preset second threshold value, the frame is taken as the key frame; or if the angle of deviation between the frame and the nearest key frame arranged in front of the frame is greater than a preset third threshold value, the frame is taken as the key frame.

6. The method of claim 2, wherein after searching for a key frame in each collected laser point cloud frame, before searching for a strong signal frame in a preset first neighborhood of the key frame for each key frame, the method further comprises:

7. The method of claim 6, wherein after each of the key frames and the searched key frames is respectively used as a key frame pair satisfying a loop constraint, further comprising:

8. The method of claim 7, wherein for each key frame, searching for a strong signal frame within a preset first neighborhood of the key frame comprises:

9. The method according to claim 1, wherein after obtaining the optimal absolute pose of the lidar sensor corresponding to each keyframe, further comprises:

10. A pose determination apparatus, characterized in that the apparatus comprises:

the optimal absolute pose determining module is used for:

11. A non-transitory computer readable storage medium storing instructions which, when executed by a processor, cause the processor to perform the steps of the pose determination method according to any of claims 1 to 9.

12. An electronic device comprising the non-transitory computer-readable storage medium of claim 11, and the processor having access to the non-transitory computer-readable storage medium.