Pose graph generation method and apparatus

Abstract

A pose graph generation method is implemented via a pose graph generation device, which includes a processing module, a positioning module for receiving a series of positioning signals and signalally connected to the processing module, and a visual-inertial module signalally connected to the processing module and for obtaining a series of sensing data. When the processing module receives an initial positioning signal and initial sensing data, the processing module establishes an initial pose in the pose graph corresponding to the initial positioning signal. The processing module obtains a weight corresponding to the connection relationship between the next pose and the previous pose based on the strength of the positioning signals corresponding to the next pose and the previous pose relative to the next pose. The processing module optimizes the pose graph based on all the weights to obtain an optimized pose graph.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicles, and in particular to a method and apparatus for generating pose graphs. Prior Technology

[0002] In the field of autonomous vehicles, in order to achieve the goal of autonomous vehicles automatically traversing target areas, how to establish a repeatable path is the main problem in this field. Recently, a common approach is to obtain information about the surrounding environment and the location through sensors and positioning signals in the autonomous vehicle, and then establish a pose graph corresponding to the target area based on the information about the surrounding environment and the location.

[0003] Because of the various changes in the external environment during movement, the sensor data or positioning signals received by the autonomous vehicle may be inaccurate. For example, when the autonomous vehicle passes through an obstacle, it receives an unstable positioning signal. As a result, the autonomous vehicle constructs the pose in the pose diagram based on the unstable positioning signal data. This makes the optimized pose diagram less accurate than the actual environment, thus preventing the autonomous vehicle from using the inaccurate pose diagram for automatic traversal.

[0004] Therefore, how to mitigate the impact of unstable positioning signals on pose graph optimization is one of the challenges to be addressed. Summary of the Invention

[0005] Therefore, the objective of this invention is to provide a pose graph generation method with higher accuracy.

[0006] Therefore, the pose graph generation method of the present invention is implemented by a movable pose graph generation device, which includes a processing module, a positioning module connected to the processing module and used to receive a series of positioning signals related to the position of the pose graph generation device, and a visual-inertial module connected to the processing module and used to obtain a series of sensing data. Each sensing data includes an environmental image related to the surrounding environment of the pose graph generation device and inertial information corresponding to the movement state of the pose graph generation device. The pose graph generation method includes a step (A), a step (B), a step (C), a step (D), and a step (E).

[0007] Step (A) is that when the processing module first receives an initial positioning signal and initial sensing data, the processing module establishes an initial pose corresponding to the initial positioning signal in a pose diagram based on the initial positioning signal and the initial sensing data.

[0008] Step (B) is that when the processing module receives the next positioning signal, the processing module establishes a next pose corresponding to the next positioning signal in the pose diagram based on the next positioning signal.

[0009] Step (C) is that the processing module establishes a connection relationship in the pose graph corresponding to the next pose and the previous pose relative to the next pose based on the next sensing data and the previous sensing data relative to the next sensing data.

[0010] Step (D) is that the processing module obtains a weight corresponding to the connection relationship based on the strength of the positioning signal corresponding to the next pose and the previous pose relative to the next pose.

[0011] Step (E) is that the processing module optimizes the pose graph according to all the weights to obtain an optimized pose graph.

[0012] Another object of the present invention is to provide a pose graph generation apparatus for a pose graph generation method.

[0013] Therefore, the pose graph generation apparatus of the present invention for the pose graph generation method includes a processing module, a positioning module and a visual-inertial module.

[0014] The positioning module is used to receive a series of positioning signals related to the position of the pose map generation device, and the signals are connected to the processing module.

[0015] The visual inertial module signal is connected to the processing module and used to obtain a series of sensing data. Each sensing data includes an environmental image related to the surrounding environment of the pose graph generation device and inertial information corresponding to the movement state of the pose graph generation device.

[0016] Specifically, when the processing module first receives an initial positioning signal and initial sensing data, it establishes an initial pose corresponding to the initial positioning signal in a pose graph based on the initial positioning signal and the initial sensing data. When the processing module receives a next positioning signal and a next sensing data, it establishes a next pose corresponding to the next positioning signal and the next sensing data in the pose graph based on the next positioning signal and the next sensing data. The processing module establishes a connection relationship in the pose graph corresponding to the next pose and the previous pose relative to the next pose based on the next sensing data and the previous pose relative to the next pose. The processing module obtains a weight corresponding to the connection relationship based on the strength of the positioning signal corresponding to the next pose and the previous pose relative to the next pose. The processing module optimizes the pose graph based on all the weights to obtain an optimized pose graph.

[0017] The advantages of this invention are as follows: the processing module optimizes the pose graph according to all weights. Since each weight is obtained based on the positioning signal strength corresponding to the next pose and the previous pose relative to the next pose, these weights make the poses with stronger positioning signal strength in the optimized pose graph more reliable, and therefore less likely to be changed during the optimization process. At the same time, by assigning differences in the magnitude of these weights, the processing module can weaken the influence of poses with unstable positioning signals when optimizing the pose graph, thereby enabling the established pose graph to have higher accuracy. Simple Explanation of the Diagram

[0018] Other features and effects of the present invention will be clearly presented in the embodiments with reference to the drawings, wherein: Figure 1 is a block diagram illustrating a pose graph generation apparatus for performing an embodiment of the pose graph generation method of the present invention; Figure 2 is a flowchart illustrating an optimized procedure of an embodiment of the pose graph generation method of the present invention; Figure 3 is a flowchart illustrating a selection procedure of an embodiment of the pose graph generation method of the present invention; Figure 4 is a schematic diagram illustrating a pose graph before optimization; and Figure 5 is a schematic diagram illustrating an optimized pose graph. Implementation

[0019] Before the present invention is described in detail, it should be noted that similar elements are represented by the same numbers in the following description.

[0020] Referring to Figure 1, an embodiment of the pose graph generation method of the present invention is implemented via a pose graph generation device. The pose graph generation device includes a processing module, a positioning module connected to the processing module and used to receive a series of positioning signals related to the position of the pose graph generation device, and a visual-inertial module connected to the processing module and used to obtain a series of sensing data. Each sensing data includes an environmental image related to the surrounding environment of the pose graph generation device and inertial information corresponding to the movement state of the pose graph generation device.

[0021] In this embodiment, the processing module is, for example, a central processing unit or a microcontroller, but is not limited thereto.

[0022] It is worth mentioning that the pose graph generation device can be implemented as an autonomous vehicle or a mobile robot, but is not limited thereto.

[0023] The positioning module is used to receive a series of positioning signals related to the position of the pose map generation device and a series of intensity indicators indicating the strength of the received positioning signals. In this embodiment, each positioning signal is, for example, a Real-Time Kinematic (RTK) signal, but is not limited thereto.

[0024] The visual inertial module is used to acquire a series of sensing data and includes an inertial measurement unit (IMU) for measuring the movement state of the pose map generation device and an image sensor for capturing images of the surrounding environment of the pose map generation device to obtain a series of corresponding environmental images.

[0025] In this embodiment, the inertial measurement unit is, for example, a gyroscope, a three-axis inertial measurement unit (3-axis IMU), or a six-axis inertial measurement unit (6-axis IMU), but is not limited thereto. The image sensor is, for example, a monocular camera or a stereo camera, but is not limited thereto.

[0026] Referring to Figures 1 and 2, the following describes how the pose graph generation device performs pose graph optimization through an optimization procedure of an embodiment of the pose graph generation method of the present invention. The optimization procedure includes the following steps.

[0027] In step 11, when the processing module first receives an initial positioning signal and initial sensing data, the processing module establishes an initial pose corresponding to the initial positioning signal and the initial sensing data in a pose diagram based on the initial positioning signal and the initial sensing data.

[0028] In step 12, when the processing module receives the next positioning signal and the next sensing data, the processing module establishes a next pose corresponding to the next positioning signal and the next sensing data in the pose diagram based on the next positioning signal and the next sensing data.

[0029] In step 13, the processing module establishes a connection relationship in the pose graph corresponding to the next pose and the previous pose relative to the next pose based on the next sensing data and the previous sensing data relative to the next sensing data.

[0030] In step 14, the processing module obtains a weight corresponding to the connection relationship based on the strength of the positioning signals corresponding to the next pose and the previous pose. The strength of the positioning signals corresponding to the next pose and the previous pose (i.e., the strength indicated by the corresponding strength index) can be either high or low. The processing module determines the weight corresponding to the connection relationship based on a combination of the strengths of the positioning signals corresponding to the next pose and the previous pose. This strength combination can be one of a high-strength combination consisting of two high-strength signals, a medium-strength combination consisting of a high-strength signal and a low-strength signal, or a low-strength combination consisting of two low-strength signals. Furthermore, the weight is positively correlated with the strength of the strength combination.

[0031] In step 15, the processing module determines whether a closed loop exists in the pose graph. If the processing module determines that the pose graph has the closed loop, it executes step 16; if the processing module determines that the pose graph does not have the closed loop, it returns to step 12.

[0032] In this embodiment, the processing module uses an image determination loop method to determine whether a closed loop exists based on the environmental images. The image determination loop method includes the following steps: For an initial environmental image in the initial sensing data, the processing module utilizes a feature point extraction algorithm, such as a Scale-Invariant Feature Transform (SIFT) algorithm or a Speeded Up Robust feature extraction algorithm. The processing module obtains initial feature information from the initial environmental image using features (SURF) and other methods. Then, upon receiving the next environmental image from the next sensing data, the processing module uses the feature point extraction algorithm to obtain the next feature information from the next environmental image. Finally, the processing module determines whether the pose map contains a closed loop by judging whether the similarity between the initial feature information and the next feature information is greater than a similarity threshold. When the processing module determines that the similarity between the initial feature information and the next feature information is greater than the similarity threshold, the processing module determines that the closed loop exists; when the processing module determines that the similarity between the initial feature information and the next feature information is not greater than the similarity threshold, the processing module determines that the closed loop does not exist. However, in some embodiments, a point cloud-based loop determination method based on point cloud data can also be used, but this is not a limitation.

[0033] In step 16, the processing module optimizes the pose graph according to all the weights to obtain an optimized pose graph, and uses the optimized pose graph as a pose graph to be determined.

[0034] It is worth mentioning that the methods for optimizing the pose graph include a non-linear least squares method or a bundle adjustment method. This invention can utilize an open-source framework for graph optimization problems, such as Graph-based Graph Optimization (g2o), to implement the above methods, but is not limited thereto.

[0035] Referring to Figures 1 and 3, the following describes how the pose graph generation device obtains a preferred pose graph through a selection procedure of an embodiment of the pose graph generation method of the present invention. The selection procedure includes the following steps.

[0036] In step 21, when the processing module receives another next positioning signal and another next sensing data, the processing module establishes another next pose corresponding to the other next positioning signal in the pose diagram to be determined based on the other next positioning signal and the other next sensing data.

[0037] In step 22, the processing module establishes another connection relationship in the pose graph to be determined based on the other next sensing data and the other previous sensing data relative to the other next sensing data.

[0038] In step 23, the processing module obtains another weight corresponding to the other connection relationship based on the strength of the positioning signal corresponding to the next pose and the previous pose relative to the next pose. Similarly, the strength of the positioning signal corresponding to the next pose and the previous pose (i.e., the strength indicated by the corresponding strength index) can also be either high or low. The processing module determines the other weight corresponding to the connection relationship based on another combination of the strengths of the positioning signals corresponding to the next pose and the previous pose. This other combination of strengths can be one of the high-strength combination, the medium-strength combination, and the low-strength combination. At the same time, the magnitude of the other weight is positively correlated with the level of the other combination of strengths.

[0039] In step 24, the processing module optimizes the pose graph to be determined according to all the weights to obtain the optimized pose graph and uses the optimized pose graph as the pose graph to be determined.

[0040] In step 25, the processing module determines whether the difference between the pose to be determined and the previous optimized pose relative to the pose to be determined exceeds a tolerance threshold. When the processing module determines that the difference between the pose to be determined and the previous optimized pose relative to the pose to be determined does not exceed the tolerance threshold, step 26 is executed; when the processing module determines that the difference between the pose to be determined and the previous optimized pose relative to the pose to be determined exceeds the tolerance threshold, the process returns to step 21.

[0041] In step 26, the processing module uses the pose diagram to be determined as a preferred pose diagram.

[0042] Referring to Figures 1, 4 and 5, the following examples from Table 1 illustrate the state of the pose graph generation method of the present invention after optimizing the pose graph according to the weights.

[0043] Figure 4 shows a pose graph before optimization, and Figure 5 shows an optimized pose graph obtained by the pose graph generation method of the present invention. P1 to P8 represent eight poses, E12 represents the edge from pose P1 to pose P2 (i.e., the connection relationship between pose P1 and pose P2), and E23 represents the edge from pose P2 to pose P3, and so on. For ease of description, the direction of each pose is not shown in the figure, but only the connection relationship between poses is represented by line segments. The positioning signal strength corresponding to poses P1, P3, P4 and P7 is high intensity, and the positioning signal strength corresponding to the other poses is low intensity.

[0044] Table 1 illustrates the positions of poses P1 to P8 in the pose graph before optimization, a first optimization result, a second optimization result, and the weight of each edge. The processing module can obtain the first optimization result shown in Table 1 using an existing optimization method that does not consider the weights, based on the pose graph before optimization. The processing module can obtain the second optimization result shown in Table 1 using an embodiment of the pose graph generation method of the present invention, based on the pose graph before optimization. It is worth noting that the intensity combinations corresponding to poses P3 and P4, and poses P7 and P1, are all high-intensity combinations. The weights of edge E34 corresponding to poses P3 and P4, and edge E71 corresponding to poses P7 and P1, are all 25. It can be seen that, compared with the first optimization result, the positions of poses P1, P3, P4, and P7 obtained by the pose graph generation method of the present invention have not changed from their original positions. This indicates that when the pose graph generation method of the present invention uses the high-intensity combination of positioning signal intensity, the corresponding pose has a stable and unchanging position during the pose graph optimization process. Since the positioning signals corresponding to poses P1, P3, P4, and P7 are relatively strong, the positioning signals obtained under strong positioning signal strength will inevitably have high reliability. Therefore, in the process of positioning graph optimization, if we can avoid changing the poses with higher reliability and instead change the poses with lower reliability, we can not only improve the accuracy of the optimized positioning graph, but also accelerate the acquisition of the preferred positioning graph. Location First optimization result Second optimization result Weight P1 (0,0) (0.0) (0.0) E12 : 5 P2 (-0.2,1.5) (-0.3,1.5) (-0.3,1.4) E23 : 5 P3 (0.6,2.8) (0.5,2.9) (0.6,2.8) E34 : 25 P4 (1.8,2.2) (1.6,2.7) (1.8,2.2) E45 : 5 P5 (3.3,2.6) (3.0,2.6) (3.6,2.4) E56 : 1 P6 (3.2,0.9) (3.0,1.1) (3.0,0.7) E67 : 5 P7 (2.3,-0.5) (2.2,-0.3) (2.3,-0.5) E78 : 5 E71 : 25 P8 (1.2,-1.4) (0.7,-0.8) (0.8,-0.9) E81 : 5

[0045] In summary, the pose graph generation method of the present invention optimizes the pose graph based on all weights. Since each weight is obtained based on the localization signal strength corresponding to the next pose and the previous pose relative to the next pose, these weights will enable the poses with strong localization signal strength in the optimized pose graph to obtain a more robust position after optimization. When the optimized pose graph is a spatial model, the processing module can obtain a more accurate spatial model based on the localization signal strength, thus achieving the objective of the present invention.

[0046] However, the above description is merely an embodiment of the present invention and should not be construed as limiting the scope of the present invention. Any simple equivalent changes and modifications made in accordance with the scope of the patent application and the contents of the patent specification of the present invention shall still fall within the scope of the patent of the present invention.

[0047] Steps 11-16 Steps 21-26 9: Pose graph generation device 91: Processing Module 92: Positioning Module 93: Visual Inertial Module E12, E23, E34, E45, E56, E67, E71, E78, E81: Edge P1~P8: pose

Claims

1. A pose graph generation method, implemented by a movable pose graph generation device, the pose graph generation device comprising a processing module, a positioning module connected to the processing module and configured to receive a series of positioning signals related to the position of the pose graph generation device, and a visual-inertial module connected to the processing module and configured to obtain a series of sensing data, each sensing data comprising an environmental image related to the surrounding environment of the pose graph generation device and inertial information corresponding to the movement state of the pose graph generation device, the pose graph generation method comprising the following steps: (A) when the processing module first receives an initial positioning signal and an initial sensing data, the processing module establishes an initial pose corresponding to the initial positioning signal and the initial sensing data in a pose graph based on the initial positioning signal and the initial sensing data; (B) When the processing module receives the next positioning signal and the next sensing data, the processing module establishes a next pose corresponding to the next positioning signal and the next sensing data in the pose graph according to the next positioning signal and the next sensing data; (C) The processing module establishes a connection relationship corresponding to the next pose and the previous pose relative to the next pose in the pose graph according to the next sensing data and the previous sensing data relative to the next sensing data; (D) The processing module obtains a weight corresponding to the connection relationship according to the strength of the positioning signal corresponding to the next pose and the previous pose relative to the next pose; and (E) The processing module optimizes the pose graph according to all the weights to obtain an optimized pose graph.

2. The pose graph generation method as described in claim 1, further comprising the following steps before step (E): (F) the processing module determines whether the pose graph has a closed loop; (G) when the processing module determines that the pose graph does not have the closed loop, repeating steps (B) to (D) and (F) until the processing module determines that the pose graph has the closed loop; and (H) when the processing module determines that the pose graph has the closed loop, executing step (E).

3. The pose graph generation method as described in claim 2, further comprising the following steps after step (E): (I) The processing module uses the optimized pose graph containing the closed loop as a pose graph to be determined; (J) When the processing module receives another next positioning signal and another next sensing data, the processing module establishes another next pose corresponding to the other next positioning signal in the pose graph to be determined based on the other next positioning signal and the other next sensing data; (K) The processing module establishes another connection relationship corresponding to the other next pose and the other previous pose relative to the other next sensing data in the pose graph to be determined based on the other next sensing data and the other previous sensing data relative to the other next sensing data; (L) The processing module obtains another weight corresponding to the other connection relationship based on the strength of the positioning signal corresponding to the other next pose and the other previous pose relative to the other next pose; (M) The processing module optimizes the pose graph to be determined based on all the weights to obtain the optimized pose graph and uses the optimized pose graph as the pose graph to be determined. (N) The processing module determines whether the difference between the pose to be determined and the previous optimized pose relative to the pose to be determined exceeds a tolerance threshold; and (O) When the processing module determines that the difference between the pose to be determined and the previous optimized pose relative to the pose to be determined does not exceed the tolerance threshold, the processing module uses the pose to be determined as a preferred pose.

4. The pose graph generation method as described in claim 3, further comprising the following steps after step (N): (P) when the processing module determines that the difference between the pose graph to be determined and the previous optimized pose graph relative to the pose graph to be determined exceeds the tolerance threshold, repeating steps (J) to (N) until the processing module determines that the difference between the pose graph to be determined and the previous optimized pose graph relative to the pose graph to be determined does not exceed the tolerance threshold.

5. The pose graph generation method as described in claim 1 or 4, wherein, In step (D), the strength of the positioning signal corresponding to the next pose and the previous pose relative to the next pose can be either high or low.

6. The pose graph generation method as described in claim 5, wherein, In step (D), the processing module determines the weight corresponding to the connection relationship based on the strength combination of the positioning signals corresponding to the next pose and the previous pose. The strength combination may be one of a high strength combination consisting of two high signal strengths, a medium strength combination consisting of a high signal strength and a low signal strength, and a low strength combination consisting of two low strengths.

7. A pose map generation apparatus, comprising: a processing module; a positioning module for receiving a series of positioning signals related to the position of the pose map generation apparatus, the signals being connected to the processing module; and a visual-inertial module, the signals being connected to the processing module and used to obtain a series of sensing data, each sensing data comprising an environmental image related to the surrounding environment of the pose map generation apparatus and inertial information corresponding to the movement state of the pose map generation apparatus; wherein, When the processing module first receives an initial positioning signal and initial sensing data, it establishes an initial pose corresponding to the initial positioning signal in a pose graph based on the initial positioning signal and initial sensing data. When the processing module receives a next positioning signal and a next sensing data, it establishes a next pose corresponding to the next positioning signal and the next sensing data in the pose graph based on the next positioning signal and the next sensing data. The processing module establishes a connection relationship in the pose graph corresponding to the next pose and the previous pose relative to the next pose based on the next sensing data and the previous sensing data relative to the next sensing data. The processing module obtains a weight corresponding to the connection relationship based on the strength of the positioning signal corresponding to the next pose and the previous pose. The processing module optimizes the pose graph based on all the weights to obtain an optimized pose graph.

8. The pose graph generation apparatus as described in claim 7, before the processing module optimizes the pose graph according to all weights to obtain the optimized pose graph, the processing module determines whether a closed loop exists in the pose graph. When the processing module determines that the pose graph does not have the closed loop, the processing module continues to receive the next positioning signal and the next sensing data. The processing module establishes the next pose in the pose graph according to the next positioning signal and the next sensing data. The processing module establishes the connection relationship in the pose graph according to the next sensing data and the previous sensing data. The processing module obtains the weight corresponding to the connection relationship according to the strength of the positioning signal corresponding to the next pose and the previous pose, until the processing module determines that the pose graph has the closed loop. When the processing module determines that the pose graph has the closed loop, the processing module optimizes the pose graph according to all weights to obtain the optimized pose graph.

9. The pose graph generation apparatus as described in claim 8, wherein after the processing module optimizes the pose graph according to all weights to obtain the optimized pose graph, the processing module uses the optimized pose graph containing the closed loop as a pose graph to be determined; the processing module establishes another next pose corresponding to the other next positioning signal in the pose graph to be determined according to the other next positioning signal and the other next sensing data; the processing module establishes another connection relationship corresponding to the other next pose and the other previous pose in the pose graph to be determined according to the other next sensing data and the other previous sensing data relative to the other next sensing data; the processing module further establishes the connection relationship corresponding to the other next pose and the other previous pose relative to the other next pose in the pose graph to be determined according to the other next sensing data. The strength of the positioning signal corresponding to the next pose and the previous pose relative to the next pose is used to obtain another weight corresponding to the other connection relationship. The processing module optimizes the pose to be determined according to all the weights to obtain the optimized pose and uses the optimized pose as the pose to be determined. The processing module determines whether the difference between the pose to be determined and the previous optimized pose relative to the pose to be determined exceeds a tolerance threshold. When the processing module determines that the difference between the pose to be determined and the previous optimized pose relative to the pose to be determined does not exceed the tolerance threshold, the processing module uses the pose to be determined as a preferred pose.

10. The pose graph generation apparatus of claim 9, wherein when the processing module determines that the difference between the pose graph to be determined and the previous optimized pose graph relative to the pose graph to be determined exceeds the tolerance threshold and the processing module receives another next positioning signal and another next sensing data, the processing module establishes another next pose corresponding to the other next positioning signal in the pose graph based on the other next positioning signal and the other next sensing data, and the processing module establishes another next pose in the pose graph based on the other next sensing data and another previous sensing data relative to the other next sensing data. A connection relationship is established corresponding to the next pose and the previous pose relative to the next pose. The processing module obtains another weight corresponding to the connection relationship based on the strength of the positioning signal corresponding to the next pose and the previous pose. The processing module optimizes the pose graph according to all the weights to obtain the optimized pose graph and uses the optimized pose graph as the pose graph to be determined until the processing module determines that the difference between the pose graph to be determined and the previous optimized pose graph relative to the pose graph to be determined does not exceed the tolerance threshold.

11. The pose graph generation apparatus as described in claim 7 or 10, wherein, The strength of the positioning signal corresponding to the next pose and the previous pose relative to the next pose can be either high or low.

12. The pose graph generation apparatus as claimed in claim 11, wherein, The processing module determines the weight corresponding to the connection relationship based on the strength combination of the positioning signals corresponding to the next pose and the previous pose. The strength combination can be one of a high strength combination consisting of two high strengths, a mixed strength combination consisting of a high strength and a low strength, or a low strength combination consisting of two low strengths.

Images