Pose determination method and device, storage medium and electronic equipment

文档序号：1860182 发布日期：2021-11-19 浏览：30次中文

阅读说明：本技术 位姿确定方法、装置及存储介质和电子设备 (Pose determination method and device, storage medium and electronic equipment ) 是由孙晓峰孔旗张金凤于 2021-08-06 设计创作，主要内容包括：本发明实施例提出位姿确定方法、装置及存储介质和电子设备。方法包括：实时计算采集每一激光点云帧时激光雷达传感器的初始绝对位姿；在采集到的每一激光点云帧中,进行搜索得到关键帧；以每个关键帧对应的激光雷达传感器的初始绝对位姿为其绝对位姿初始值,不断调整每个关键帧对应的激光雷达传感器的绝对位姿,使得所有第一帧云对对应的第一相对位姿残差、所有第二关键帧对对应的第二相对位姿残差以及所有第三关键帧对对应的第三相对位姿残差之和最小,从而得到每个关键帧对应的激光雷达传感器的最优绝对位姿。本发明实施例提高了激光雷达传感器的位姿的计算精度。(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 each collected laser point cloud frame to obtain a key frame; and continuously adjusting the absolute pose of the laser radar sensor corresponding to each key frame by taking the initial absolute pose of the laser radar sensor corresponding to each key frame as the initial absolute pose value of the laser radar sensor, so that the sum of first relative pose residuals corresponding to all the first frame clouds, second relative pose residuals corresponding to all the second key frames and third relative pose residuals corresponding to all the third key frames is minimum, and the optimal absolute pose of the laser radar sensor corresponding to each key frame is obtained. The embodiment of the invention improves the calculation precision of the pose of the laser radar sensor.)

1. A pose determination method, characterized by comprising:

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

searching 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 all the strong signal frames of the key frame into a local subgraph, 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 a relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose, 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 collected, the GNSS signal intensity value of the global navigation satellite system is not less than the frame of the preset signal intensity threshold value; wherein, the laser point cloud registered with the key frame is used as a virtual key frame;

every 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 second relative pose constraints between the two key frames in the second key frame pair are calculated according to the initial absolute poses of the laser radar sensors corresponding to the two key frames in the second key frame pair;

each pair of key frames meeting the loop constraint condition is called a third key frame pair, the relative pose between the 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 interframe matching degree of the two key frames in the third key frame pair;

continuously adjusting the absolute pose of the laser radar sensor corresponding to each key frame by taking the initial absolute pose of the laser radar sensor corresponding to each key frame as the initial absolute pose value of the laser radar sensor, so that the sum of first relative pose residuals corresponding to all the first frame clouds, second relative pose residuals corresponding to all the second key frames and third relative pose residuals corresponding to all the third key frames is minimum, thereby obtaining the optimal absolute pose of the laser radar sensor corresponding to each key frame, wherein,

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

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

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

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

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;

taking each group of continuous weak signal poses as a weak signal communication track segment;

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

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:

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

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

setting the position signal of the weak signal position as 0, setting the position signal of the strong signal position as 1,

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

5. The method of claim 2, wherein the searching for the key frame in the laser point cloud frame corresponding to each weak signal connection track segment comprises:

for each weak signal communication track segment, acquiring all laser point cloud frames corresponding to the track segment;

sequencing all the obtained laser point cloud frames 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 the tail as key frames;

regarding each frame except for the frame arranged at the head and the tail, 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, taking the frame as the key frame; or if the yaw angle between the frame and the nearest key frame which is arranged in front of the frame is larger 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 of the collected laser point cloud frames, and before searching for a strong signal frame in a preset first neighborhood of the key frame for each key frame, further comprising:

for each key frame, searching 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 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 respectively taking the key frame and each searched key frame as a key frame pair meeting a loop-back constraint condition;

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

the second condition is that: the interframe matching degree between the two key frames is greater than a preset fifth threshold value;

a third condition: and the track length of the laser radar sensor in the time interval for collecting the two key frames is greater than a preset sixth threshold value.

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

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

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

for each key frame, searching a strong signal frame located in a preset first neighborhood of the key frame in a weak signal connected track segment subset corresponding to the key frame;

said designating each two adjacent key frames as a second key frame pair comprises:

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

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

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

taking the larger 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 as a new second relative pose constraint;

continuously adjusting the absolute pose of the laser radar sensor corresponding to each key frame by taking the optimal absolute pose of the laser radar sensor corresponding to each key frame as the initial value of the absolute pose, so that the sum of first relative pose residuals corresponding to all the first frame clouds, second relative pose residuals corresponding to all the second key frames and third relative pose residuals corresponding to all the third key frames is minimum, thereby obtaining the final optimal absolute pose of the laser radar sensor corresponding to each key frame, wherein,

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

10. A pose determination apparatus characterized by 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 each collected laser point cloud frame to obtain a key frame;

an optimal absolute pose determination module to:

for each key frame, searching a strong signal frame in a preset first neighborhood of the key frame, forming all the strong signal frames of the key frame into a local subgraph, 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 a relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose, 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 collected, the GNSS signal intensity value is not less than a frame of a preset signal intensity threshold value; wherein, the laser point cloud registered with the key frame is used as a virtual key frame; and the number of the first and second electrodes,

11. 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 one 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.

Technical Field

The invention relates to the technical field of laser point cloud, in particular to a pose determination method and device, a readable storage medium and electronic equipment.

Background

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

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

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

The quality of the mapping is mainly determined by the pose accuracy of the laser radar sensor output by the combined inertial navigation system, so that the mapping is sensitive to the quality of GNSS signals. Reliable point cloud mapping results cannot be obtained in weak GNSS signal areas with serious GNSS signal shielding or electromagnetic interference, multipath effect and the like, such as trees are flourishing, buildings are erected, bridges and tunnels are spread all over.

Disclosure of Invention

The embodiment of the invention provides a pose determining method and device, a readable storage medium and electronic equipment, and aims 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 each collected laser point cloud frame to obtain a key frame;

The initial absolute pose of the laser radar sensor during real-time calculation and collection of each laser point cloud frame comprises the following steps:

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

taking each group of continuous weak signal poses as a weak signal communication track segment;

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

The method for determining whether the initial absolute pose of the laser radar sensor is the weak signal pose or the 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 comprises the following steps:

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

setting the position signal of the weak signal position as 0, setting the position signal of the strong signal position as 1,

The searching is carried out in the laser point cloud frames corresponding to each weak signal communication track segment respectively to obtain the key frames, and the searching comprises the following steps:

for each weak signal communication track segment, acquiring all laser point cloud frames corresponding to the track segment;

sequencing all the obtained laser point cloud frames 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 the last as key frames;

for each frame except the first frame and the last frame, 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, taking the frame as the key frame; or if the yaw angle between the frame and the nearest key frame which is arranged in front of the frame is larger than a preset third threshold value, the frame is taken as the key frame.

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

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

the second condition is that: the interframe matching degree between the two key frames is greater than a preset fifth threshold value;

a third condition: and the track length of the laser radar sensor in the time interval for collecting the two key frames is greater than a preset sixth threshold value.

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 includes:

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 located in a preset first neighborhood of the key frame in a weak signal connected track segment subset corresponding to the key frame;

said designating each two adjacent key frames as a second key frame pair comprises:

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

After the optimal absolute pose of the laser radar sensor corresponding to each keyframe is obtained, the method further includes:

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 each collected laser point cloud frame to obtain a key frame;

an optimal absolute pose determination module to:

for each key frame, searching a strong signal frame in a preset first neighborhood of the key frame, forming all the strong signal frames of the key frame into a local subgraph, 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 a relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose, 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 collected, the GNSS signal intensity value is not less than a frame of a preset signal intensity threshold value; and the number of the first and second electrodes,

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 of any one of the above.

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, each pair of key frames meeting the loop constraint condition is called a third key frame pair, continuously adjusting the absolute pose of the laser radar sensor corresponding to each key frame by taking the initial absolute pose of the laser radar sensor corresponding to each key frame as the initial absolute pose value, the sum of the first relative pose residuals corresponding to all the first frame cloud pairs, the second relative pose residuals corresponding to all the second key frame pairs and the third relative pose residuals corresponding to all the third key frames is minimized, 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 final point cloud mapping precision can be improved.

Drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without inventive labor.

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

FIG. 2 is an exemplary diagram of 7 weak signal connected trace segments grouped into 3 connected trace segment subsets according to an embodiment of the present invention;

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

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

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

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

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

The technical solution of the present invention will be described in detail with specific examples. Several of the following embodiments may be combined with each other and some details of the same or similar concepts or processes may not be repeated in some embodiments.

Fig. 1 is a flowchart of a pose determination method according to an embodiment of the present invention, which includes the following specific 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, 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; the laser radar sensor is mounted on the mobile equipment and continuously emits laser beams to the scene targets.

Namely, the 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 and is used as an initial absolute pose.

Step 102: and searching each collected 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 all the strong signal frames of the key frame into a local subgraph, 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 a relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose, 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 collected, the GNSS signal intensity value is not less than a frame of a preset signal intensity 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 detailed description thereof 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.

And the difference value between the absolute poses of the laser radar sensors respectively corresponding to the two laser point cloud frames forms the relative pose between the two laser point cloud frames. Since the absolute pose is usually composed of a plurality of position parameters and a plurality of attitude parameters, the relative pose between two laser point cloud frames can be obtained by subtracting the values of the plurality of position parameters and the values of the plurality of attitude parameters corresponding to the two laser point cloud frames.

The radius of the first domain may be determined empirically or the like, 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 of the circle and the preset radius r as the radius when the key frame is acquired.

Step 104: and calculating second relative pose constraints between the two key frames in the second key frame pair according to the initial absolute poses of the laser radar sensors respectively 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 the 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 interframe matching degree of the two key frames in the third key frame pair.

Step 106: continuously adjusting the absolute pose of the laser radar sensor corresponding to each key frame by taking the initial absolute pose of the laser radar sensor corresponding to each key frame as the initial absolute pose value of the laser radar sensor, so that the sum of first relative pose residuals corresponding to all the first frame clouds, second relative pose residuals corresponding to all the second key frames and third relative pose residuals corresponding to all the third key frames is minimum, thereby obtaining the optimal absolute pose of the laser radar sensor corresponding to each key frame, wherein,

In the above embodiment, the key frames and the laser point clouds registered therewith are referred to as a first key frame cloud pair, every two adjacent key frames are referred to as a second key frame pair, each pair of key frames satisfying the loop constraint condition is referred to as a third key frame pair, continuously adjusting the absolute pose of the laser radar sensor corresponding to each key frame by taking the initial absolute pose of the laser radar sensor corresponding to each key frame as the initial absolute pose value, the sum of the first relative pose residuals corresponding to all the first frame cloud pairs, the second relative pose residuals corresponding to all the second key frame pairs and the third relative pose residuals corresponding to all the third key frames is minimized, 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 final point cloud mapping precision can be improved.

In an optional embodiment, in step 102, searching is performed in each collected laser point cloud frame 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; taking each group of continuous weak signal poses as a weak signal communication track segment; and searching in the laser point cloud frame corresponding to each weak signal communication track segment to obtain a key frame.

By the embodiment, the key frame is searched only in the weak signal connected track segment, so that the pose of the weak signal frame is optimized, the calculation complexity is reduced, and the pose calculation accuracy of the laser radar sensor can be improved when the GNSS signal is weak.

In an optional embodiment, determining, according to the GNSS signal intensity value at the time of acquiring each laser point cloud frame, whether the initial absolute pose of the laser radar sensor at the time of acquiring each laser point cloud frame is a weak signal pose or a strong signal pose specifically includes: for each laser point cloud frame, if the GNSS signal intensity value during the collection of the frame is smaller than a preset signal intensity threshold value, the initial absolute pose of the laser radar sensor during the collection of the frame is a weak signal pose, and otherwise, the initial absolute pose 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_iFor collecting GNSS signal intensity value, L, at the i-th frame_iAcquiring a binarization label of strong and weak signal poses corresponding to the initial absolute pose of the laser radar sensor at the ith frame, wherein 1 represents the strong signal pose, 0 represents the weak signal pose, and theta represents the strong signal pose_sA preset signal strength threshold value, then:

the value of the preset signal strength threshold can be set according to experience and the like.

In an optional embodiment, in order to obtain a smooth and continuous weak signal connected trajectory segment, based on the strong and weak signal pose information corresponding to each frame in the neighborhood of each frame, smooth low-pass filtering is performed on the obtained strong and weak signal pose information corresponding to each frame, which is specifically as follows:

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

The values of the first threshold and the radius of the second neighborhood can be set according to experience and the like.

For example: is provided withA binarization label of the strong and weak signal poses corresponding to the initial absolute pose of the laser radar sensor during the collection of the ith frame after smoothing processing, wherein n is a preset neighborhood radius phi_iIs the neighborhood set corresponding to the ith frame, and phi_i∈[i-n,i+n]。

The value of n can be set according to experience and the like.

In an optional embodiment, the searching is performed in the laser point cloud frame corresponding to each weak signal connected track segment to obtain a key frame, and specifically includes:

for each weak signal communication track segment, acquiring all laser point cloud frames corresponding to the track segment; sequencing all the obtained laser point cloud frames 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 the tail as key frames; regarding each frame except for the frame arranged at the head and the tail, 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, taking the frame as the key frame; or if the yaw angle between the frame and the nearest key frame which is arranged in front of the frame is larger than a preset third threshold value, the frame is taken as the key frame.

The values of the second threshold and the third threshold can be set according to experience and the like.

In an optional embodiment, in order to eliminate an accumulated error caused by inter-frame interpolation of the combined inertial navigation in the weak signal communication track segments and ensure consistency of point cloud imaging results of a coincidence region between the weak signal communication track segments, loop constraints between key frames are established through an inter-frame matching algorithm, which is specifically as follows:

after step 102 and before step 103, further comprising: for each key frame, searching 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 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 respectively taking the key frame and each searched key frame as a key frame pair meeting a loop-back constraint condition:

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

the second condition is that: the interframe matching degree between the two key frames is greater than a preset fifth threshold value;

a third condition: and the track length of the laser radar sensor in the time interval for collecting the two key frames is greater than a preset sixth threshold value.

Values of the fourth threshold, the fifth threshold, and the sixth threshold may be set according to experience and the like.

In an optional embodiment, after loop detection is completed, the association between the keyframes in the overlapping regions between the weak signal connected track segments is established, and further, the association between the weak signal connected track segments can be established through the association between the keyframes, so that the weak signal connected track segments with the overlapping regions in space are aggregated into a connected track segment subset. The method comprises the following specific 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 respectively correspond to different weak signal connected track segments, according to the principle that corresponding points of the two key frames in the weak signal connected track segments are overlapped, the weak signal connected track segments corresponding to the two key frames are aggregated into a weak signal connected track segment subset.

FIG. 2 gives an example of 7 weak signal connected trace segments aggregated into 3 weak signal connected trace segment subsets.

In an alternative embodiment, for each key frame, searching for strong signal frames within a predetermined first neighborhood of the key frame comprises: for each key frame, searching a strong signal frame located in a preset first neighborhood of the key frame in a weak signal connected track segment 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 connected track segment subset as a second key frame pair.

In an optional embodiment, in order to reduce the influence of the adjacent inter-frame error caused by the too weak signal on the final point cloud mapping accuracy, the embodiment of the present invention performs further optimization on the basis of the optimal absolute pose of each key frame obtained in step 106, specifically as follows:

after step 106, further comprising:

The above process can be repeated for multiple times so as to obtain the absolute pose of each key frame with higher precision.

Fig. 3 is a flowchart of a pose determination method according to another embodiment of the present invention, which includes the following specific 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; the laser radar sensor is mounted on the mobile equipment and continuously emits laser beams to the scene targets.

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 poses as a weak signal communication track segment, and respectively searching key frames in the laser point cloud frame corresponding to each weak signal communication track segment.

Specifically, for each weak signal connected track segment, all laser point cloud frames corresponding to the track segment are obtained; sequencing all the acquired laser point cloud frames from head to tail according to the corresponding positions of the acquired laser point cloud frames on the track segment from head to tail; taking the frames arranged at the head and the tail as key frames; regarding each frame except for the frame arranged at the head and the tail, 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, taking the frame as the key frame; or if the yaw angle between the frame and the nearest key frame which is arranged in front of the frame is larger than a preset third threshold value, the frame is taken as the key frame.

Step 304: for each key frame, searching 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 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 respectively taking the key frame and each searched key frame as a key frame pair meeting a loop-back constraint condition;

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

the second condition is that: the interframe matching degree between the two key frames is greater than a preset fifth threshold value;

a third condition: and the track length of the laser radar sensor in the time interval for collecting the two key frames is greater than a preset sixth threshold value.

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

Step 306: for each key frame, searching a strong signal frame located in a preset first neighborhood of the key frame in a weak signal connected track segment subset corresponding to the key frame; forming a local sub-graph by using all strong signal frames of the key frame, searching laser point clouds registered with the key frame in the local sub-graph, calling the key frame and the laser point clouds registered with the key frame as a first frame cloud pair, calling a relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose, and using the relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose constraint when the key frame and the laser point clouds are registered; and taking the laser point cloud registered with the key frame as a virtual key frame.

Step 307: and calculating second relative pose constraints between the two key frames in the second key frame pair according to the initial absolute poses of the laser radar sensors respectively corresponding to the two key frames in the second key frame pair.

And calculating the relative pose between the two key frames in the second key frame pair according to the initial absolute poses of the laser radar sensors 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 the 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 interframe 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 a target point cloud T (P)_t) The degree of overlap of (a) is as high as possible. Because the relative pose between two point clouds can be obtained by direct decomposition according to the transformation matrix T, in the embodiment of the present invention, the "inter-frame matching" involved in the second condition of step 304 and the "searching the laser point cloud registered with the key frame in the local sub-graph" involved in step 306 can respectively obtain the third relative pose constraint and the second relative pose constraint through a point cloud registration algorithm.

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), a method based on NDT (Normal Distribution Transform), and a feature matching method based on manual design or machine learning extraction are all applicable to the present invention.

Step 309: continuously adjusting the absolute pose of the laser radar sensor corresponding to each key frame by taking the initial absolute pose of the laser radar sensor corresponding to each key frame as the initial absolute pose value of the laser radar sensor, so that the sum of first relative pose residuals corresponding to all the first frame clouds, second relative pose residuals corresponding to all the second key frames and third relative pose residuals corresponding to all the third key frames is minimum, thereby obtaining the optimal absolute pose of the laser radar sensor corresponding to each key frame, wherein,

For example: the key frame A, B is an adjacent key frame, that is, a second key frame pair, and the initial absolute pose of the lidar sensor corresponding to the key frame A, B is used as an initial absolute pose value, the absolute pose of the lidar sensor corresponding to the key frame A, B is continuously adjusted, and each adjustment is performed, so that the relative pose between the key frames A, B, that is, the second relative pose between the key frames A, B, can be calculated according to the absolute pose of the lidar sensor corresponding to the adjusted key frame A, B, and the second relative pose are constrained to perform residual calculation, so as to obtain a second relative pose residual.

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

F(X)＝E_M(X)+E_G(X)+E_L(X) (3)

wherein X represents the set of initial absolute poses of the laser radar sensor corresponding to each key frame, E_M(X) represents the sum of first relative pose residuals corresponding to each first frame cloud pair, E_G(X) represents the sum of the second relative pose residuals corresponding to each second keyframe pair,E_Land (X) represents the sum of the third relative pose residuals corresponding to each third keyframe pair.

E_M(X)、E_G(X)、E_LThe expression of (X) is the same, with E_M(X) is exemplified by:

where M is a set of first frame cloud pairs, i, j is the number of key frames and laser point clouds (which can be regarded as virtual key frames) in any first frame cloud pair, and X_i、X_jRespectively corresponding absolute pose vectors of a key frame i and a laser point cloud j in the first frame cloud pair; r_i,jA first relative pose constraint vector between the key frame i and the laser point cloud j is obtained; the method comprises the following steps that a manifest () is a manifold conversion function, a function for measuring a difference between a first relative pose between a key frame i and a laser point cloud j and a first relative pose constraint is used, an absolute pose vector and a first relative pose constraint vector corresponding to the key frame i and the laser point cloud j are converted into a residual vector, and a specific expression of the manifest () can adopt an existing mature expression; omega_i,jThe information matrix can be set according to experience; t is the matrix transpose operator, and-1 is the matrix inversion operator.

The formula (3) can be solved by adopting the existing Levenberg-Marquardt method, and 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) is obtained.

And after the optimal absolute pose of the laser radar sensor corresponding to each key frame is obtained, replacing the initial absolute pose of the corresponding frame with the optimal absolute pose, so that an absolute pose set corresponding to all the laser point cloud frames is obtained. Then, each frame L in the original laser point cloud frame set rho is sequentially processed according to the formula (4)_iProjecting the point cloud to a world coordinate system, and accumulating each frame of laser point cloud to obtain a high-precision point cloud map Q, wherein in the formulaProjection variation corresponding to ith frame of laser point cloudAnd (5) matrix changing.

Fig. 4 is a schematic structural diagram of a pose determination apparatus provided in an embodiment of the present invention, where the apparatus mainly includes:

and the initial absolute pose calculation 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, a GNSS + IMU-based mapping and mapping method can be adopted to calculate the initial absolute pose of the laser radar sensor in real time when each laser point cloud frame is acquired; the laser radar sensor is mounted on the mobile equipment and continuously emits laser beams to the scene targets.

And the key frame searching module 42 is configured to search each acquired laser point cloud frame to obtain a key frame.

An optimal absolute pose determination module 43 to:

for each key frame, searching a strong signal frame in a preset first neighborhood of the key frame, forming all the strong signal frames of the key frame into a local subgraph, 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 a relative pose between the key frame and the laser point clouds in the first frame cloud pair as a first relative pose, 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 collected, the GNSS signal intensity value is not less than a frame of a preset signal intensity threshold value; wherein, the laser point cloud registered with the key frame is used as a virtual key frame; and the number of the first and second electrodes,

In an optional embodiment, the searching for the key frame by the key frame searching module 42 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; taking each group of continuous weak signal poses as a weak signal communication track segment; and searching in the laser point cloud frame corresponding to each weak signal communication track segment to obtain a key frame.

In an alternative embodiment, the determining, by the key frame searching module 42, whether the initial absolute pose of the lidar sensor at the time of acquiring each laser point cloud frame is a weak signal pose or a strong signal pose according to the GNSS signal strength value at the time of acquiring each laser point cloud frame includes: for each laser point cloud frame, if the GNSS signal intensity value during the collection of the frame is smaller than a preset signal intensity threshold value, the initial absolute pose of the laser radar sensor during the collection of the frame is a weak signal pose, and otherwise, the initial absolute pose is a strong signal pose.

In an optional embodiment, when determining whether the initial absolute pose of the laser radar sensor at the time of acquiring each laser point cloud frame is a weak signal pose or a strong signal pose, 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, calculates the sum of the pose signal flags of all the initial absolute poses located in the preset second field of the initial absolute pose for the initial absolute pose of the laser radar sensor at the time of acquiring each laser point cloud frame, and finally determines that the initial absolute pose is the weak signal pose if the sum is smaller than a preset first threshold, or finally determines that the initial absolute pose is the strong signal pose.

In an optional embodiment, the key frame searching module 42 searches the laser point cloud frame corresponding to each weak signal connected track segment to obtain a key frame, including: for each weak signal communication track segment, acquiring all laser point cloud frames corresponding to the track segment; sequencing all the obtained laser point cloud frames 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 the tail as key frames; regarding each frame except for the frame arranged at the head and the tail, 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, taking the frame as the key frame; or if the yaw angle between the frame and the nearest key frame which is arranged in front of the frame is larger than a preset third threshold value, the frame is taken as the key frame.

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

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

the second condition is that: the interframe matching degree between the two key frames is greater than a preset fifth threshold value;

a third condition: and the track length of the laser radar sensor in the time interval for collecting the two key frames is greater than a preset sixth threshold value.

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

In an alternative embodiment, the optimal absolute pose determination module 43 searches for a strong signal frame in a preset first neighborhood of each key frame, including: for each key frame, searching a strong signal frame located in a preset first neighborhood of the key frame in a weak signal connected track segment subset corresponding to the key frame;

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

In an optional 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:

Embodiments of the present application also provide a computer-readable storage medium storing instructions, which when executed by a processor may perform the steps in the pose determination method as described above. In practical applications, the computer readable medium may be included in each device/apparatus/system of the above embodiments, or may exist separately and not be assembled into the device/apparatus/system. Wherein instructions are stored in a computer readable storage medium, which stored instructions, when executed by a processor, may perform steps as in the upper posture determination method.

According to embodiments disclosed herein, the computer-readable storage medium may be a non-volatile computer-readable storage medium, which may include, for example and without limitation: a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing, without limiting the scope of the present disclosure. 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, an embodiment of the present 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, 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 can be implemented when the program of the memory 52 is executed.

Specifically, in practical applications, the electronic device may further include a power supply 53, an input/output unit 55, and the like. Those skilled in the art will appreciate that the configuration of the electronic device shown in fig. 5 is not intended to be limiting of the electronic device and may include more or fewer components than shown, or some components in combination, 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 the 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 executing 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, an application program required for at least one function, and the like; the storage data area may store data created according to the use of the server, and the like. Further, the 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 the processor 51 access to the memory 52.

The electronic device further comprises a power supply 53 for supplying power to the various components, which can be logically connected to the processor 51 through a power management system, so as to implement functions of managing charging, discharging, and power consumption through the power management system. The power supply 53 may also include any component including one or more dc or ac power sources, recharging systems, power failure detection circuitry, power converters or inverters, power status indicators, and the like.

The electronic device may also include an input-output unit 54, the input unit output 54 operable to receive input numeric or character information and 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 input by or provided to the user as well as various graphical user interfaces, which may be composed of graphics, text, icons, video, and any combination thereof.

The flowchart 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 disclosed herein. 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 various combinations and/or combinations of features recited in the various embodiments and/or claims of the present disclosure can be made, 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 and/or coupled in various ways, all of which fall within the scope of the present disclosure, without departing from the spirit and teachings of the present application.

The principles and embodiments of the present invention are explained herein using specific examples, which are provided only to help understanding the method and the core idea of the present invention, and are not intended to limit the present application. It will be appreciated by those skilled in the art that changes may be made in this embodiment and its broader aspects and without departing from the principles, spirit and scope of the invention, and that all such modifications, equivalents, improvements and equivalents as may be included within the scope of the invention are intended to be protected by the claims.

24页详细技术资料下载

Pose determination method and device, storage medium and electronic equipment

相关技术

网友询问留言