Pose optimization method and system

Abstract

The embodiment of the invention discloses a pose optimization method and system. The system may determine the validity of the loopback based on the registration results of the subset of environmental information. Furthermore, the system can perform pose optimization according to the effectiveness of the loopback. In this way, effective pose optimization can be achieved.

Description

Technical Field

[0001] The present invention relates to the field of information technology, and in particular to a posture optimization method and system. Background Art

[0002] In some scenarios, pose optimization is required to improve the accuracy of pose estimation. The usual practice is to perform pose optimization directly based on the constructed loop constraints. When there are many errors or inaccurate constraints in the constructed loop constraints, these constraints reduce the accuracy of pose optimization.

[0003] In view of this, it is currently hoped to provide an effective pose optimization method. Summary of the invention

[0004] The purpose of this application is to provide an effective posture optimization method.

[0005] One of the embodiments of the present specification provides a posture optimization method, which is executed by at least one processor, and the method includes: obtaining estimated values ​​of multiple postures and multiple environmental information subsets corresponding to the multiple postures, wherein the environmental information subset corresponding to any posture is the environmental information subset collected by the collection device under the posture; based on the multiple environmental information subsets, detecting one or more loops from the multiple postures, wherein each loop includes two postures; for each loop of the one or more loops, aligning two environmental information subsets corresponding to the two postures of the loop to obtain an alignment result of the loop; based on the alignment results of the one or more loops, determining the validity of the one or more loops; and optimizing the estimated values ​​of at least some of the multiple postures according to the validity of the one or more loops.

[0006] In some embodiments, determining the validity of the one or more loops based on the registration results of the one or more loops includes: performing one or more verifications based on the registration results of the one or more loops to obtain verification results of the one or more verifications; and determining one or more valid loops from the one or more loops based on the verification results of the one or more verifications. Optimizing the estimated values ​​of at least some of the multiple poses based on the validity of the one or more loops includes: optimizing the estimated values ​​of at least some of the multiple poses based on the one or more valid loops.

[0007] In some embodiments, the registration is achieved by an ICP algorithm or an ICP-based registration algorithm.

[0008] In some embodiments, the environmental information subset is a point cloud, and for each of the one or more loops, two environmental information subsets corresponding to two poses of the loop are registered to obtain the registration result of the loop, including: performing point cloud registration on the two environmental information subsets to obtain the registration result of the loop. Based on the registration results of the one or more loops, determining the validity of the one or more loops, including: for each of the one or more loops, determining the validity of the loop based on the proportion of successfully matched points indicated by the registration result of the loop.

[0009] In some embodiments, the environmental information subset is a point cloud, and for each of the one or more loops, the two environmental information subsets corresponding to the two postures of the loop are registered to obtain the registration result of the loop, including: performing point cloud registration on the two environmental information subsets to obtain the registration result of the loop. The two environmental information subsets include a first original point cloud and a second original point cloud, and based on the registration results of the one or more loops, the validity of the one or more loops is determined, including: for each of the one or more loops: rotating the first semantic point cloud according to the rotation matrix in the registration result of the loop to obtain the rotated first semantic point cloud, the first semantic point cloud before rotation is obtained based on the first original point cloud; comparing the second semantic point cloud with the rotated first semantic point cloud to obtain a first comparison result, the second semantic point cloud is obtained based on the second original point cloud; determining the validity of the loop based on the first comparison result.

[0010] In some embodiments, the environmental information subset is a point cloud, and for each of the one or more loops, the two environmental information subsets corresponding to the two postures of the loop are aligned to obtain the alignment result of the loop, including: performing point cloud alignment on the two environmental information subsets to obtain the alignment result of the loop. The two environmental information subsets include a first original point cloud and a second original point cloud, and based on the alignment results of the one or more loops, the validity of the one or more loops is determined, including, for each of the one or more loops: rotating the first semantic description vector according to the rotation matrix in the alignment result of the loop to obtain a rotated semantic description vector, the first semantic description vector is obtained based on the first original point cloud; comparing the second semantic description vector with the rotated first semantic description vector to obtain a second comparison result, the second semantic point cloud is obtained based on the second original point cloud; determining the validity of the loop based on the second comparison result.

[0011] In some embodiments, the environmental information subset is a point cloud, and for each of the one or more loops, the two environmental information subsets corresponding to the two postures of the loop are aligned to obtain the alignment result of the loop, including: performing point cloud alignment on the two environmental information subsets to obtain the alignment result of the loop. The multiple postures are distributed on different driving trajectories, and the one or more loops are multiple loops; based on the alignment results of the one or more loops, determining the validity of the one or more loops, including: determining one or more pairs of target loops based on the multiple loops, each pair of target loops includes a first loop and a second loop, the first loop includes a first posture on the first driving trajectory and a second posture on the second driving trajectory, and the second loop includes a third posture on the first driving trajectory and a fourth posture on the second driving trajectory. For each pair of target loops in the one or more pairs of target loops: according to the first transformation matrix, the second transformation matrix, the third transformation matrix and the fourth transformation matrix, any one of the postures in the pair of target loops is transformed four times, the first transformation matrix is ​​the estimation result of the transformation between the first posture and the third posture by the acquisition device, the second transformation matrix is ​​the estimation result of the transformation between the second posture and the fourth posture by the acquisition device, the third transformation matrix is ​​the transformation matrix between the first posture and the second posture in the registration result of the first loop, and the fourth transformation matrix is ​​the transformation matrix between the third posture and the fourth posture in the registration result of the second loop; compare the any one posture before and after the four transformations to obtain a third comparison result; determine the validity of the pair of target loops based on the third comparison result.

[0012] One of the embodiments of this specification provides a computing device, including a processor and a storage device, wherein the storage device is used to store instructions. When the processor executes the instructions, the posture optimization method described in any embodiment of this specification is implemented.

[0013] One of the embodiments of this specification provides a computer-readable storage medium, which stores posture optimization instructions. When a computer runs the posture optimization instructions, the posture optimization method described in any embodiment of this specification is implemented.

[0014] One of the embodiments of this specification provides a posture optimization system, including an acquisition module, a determination module, a registration module, a verification module and an optimization module. The acquisition module is used to obtain estimated values ​​of multiple postures and multiple environmental information subsets corresponding to the multiple postures, wherein the environmental information subset corresponding to any posture is the environmental information subset collected by the collection device under the posture. The determination module is used to detect one or more loops from the multiple postures based on the multiple environmental information subsets, wherein each loop includes two postures. The registration module is used to align the two environmental information subsets corresponding to the two postures of each loop in the one or more loops to obtain the registration result of the loop. The verification module is used to determine the validity of the one or more loops based on the registration results of the one or more loops. The optimization module is used to optimize the estimated values ​​of at least some of the multiple postures according to the validity of the one or more loops.

[0015] This specification provides a posture optimization method and system. The system can determine the validity of the loop based on the registration result of the environmental information subset. Then, the system can perform posture optimization according to the validity of the loop. In this way, effective posture optimization can be achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] This specification will be further described in the form of exemplary embodiments, which will be described in detail by the accompanying drawings. These embodiments are not restrictive, and in these embodiments, the same number represents the same structure, wherein:

[0017] Figure 1 is an exemplary module diagram of a posture optimization system according to some embodiments of this specification;

[0018] Figure 2 is an exemplary flow chart of a posture optimization method according to some embodiments of this specification;

[0019] Figure 3A is a schematic diagram of a loop constraint based on a single trajectory according to some embodiments of this specification;

[0020] Figure 3B is a schematic diagram of loop closure constraints based on multiple trajectories according to some embodiments of this specification;

[0021] Figure 4 is a schematic diagram of a pair of target loops according to some embodiments of the present specification. DETAILED DESCRIPTION

[0022] In order to more clearly illustrate the technical solutions of the embodiments of this specification, the following is a brief introduction to the drawings required for the description of the embodiments. Obviously, the drawings described below are only some examples or embodiments of this specification. For ordinary technicians in this field, this specification can also be applied to other similar scenarios based on these drawings without creative work. Unless it is obvious from the language environment or otherwise explained, the same reference numerals in the figures represent the same structure or operation.

[0023] It should be understood that the "system", "device", "unit" and / or "module" used herein are a method for distinguishing different components, elements, parts, portions or assemblies at different levels. However, if other words can achieve the same purpose, the words can be replaced by other expressions.

[0024] As shown in this specification, unless the context clearly indicates an exception, the words "a", "an", "a kind" and / or "the" do not refer to the singular, but also include the plural. Generally speaking, the terms "include" and "comprise" only indicate the inclusion of the steps and elements that have been clearly identified, and these steps and elements do not constitute an exclusive list. The method or device may also include other steps or elements.

[0025] Flowcharts are used in this specification to illustrate the operations performed by the system according to the embodiments of this specification. It should be understood that the preceding or following operations are not necessarily performed precisely in order. Instead, the steps may be processed in reverse order or simultaneously. At the same time, other operations may also be added to these processes, or one or more operations may be removed from these processes.

[0026] This specification provides a posture optimization method and system. The system can determine the validity of the loop based on the registration result of the environmental information subset. Then, the system can perform posture optimization according to the validity of the loop. In this way, effective posture optimization can be achieved.

[0027] The posture optimization method and system provided in the embodiments of this specification can be applied to various scenarios, including but not limited to indoor navigation, automatic driving, and assisted driving. Taking indoor navigation as an example, in the data collection stage, one or more collection devices (such as a robot equipped with a laser radar) can perceive the surrounding environment (such as a warehouse environment) during driving, that is, collect information about the surrounding environment (such as a laser point cloud). Each collection device can collect local environmental information in multiple postures along its driving trajectory in sequence, wherein the multiple postures include the postures of the collection device at different times, and each posture includes the position and posture of the collection device at the corresponding time. In the data processing stage, the processing device can receive the environmental information collected by the one or more collection devices, and fuse the environmental information to reconstruct the complete environmental information. When the environmental information is presented in the form of a graph, the fusion of the environmental information is the fusion of the subgraph. The complete environmental information can be used for indoor navigation of mobile devices (for example, storage robots, cleaning robots). In the data collection stage, the one or more collection devices will estimate the values ​​of the multiple postures, and the accuracy of the estimated values ​​of the multiple postures will affect the fusion effect of the environmental information. For example, when the error between the estimated value of the pose and the true value is too large, it will cause ghosting and stratification in the fusion result. Therefore, in order to ensure the fusion effect of environmental information, the estimated value of the pose needs to be optimized.

[0028] Figure 1 1 is an exemplary module diagram of a posture optimization system according to some embodiments of this specification. The system 100 may be implemented on a processing device (such as a server). Figure 1 As shown, the posture optimization system 100 (referred to as system 100 ) may include an acquisition module 110 , a detection module 120 , a registration module 130 , a verification module 140 and an optimization module 150 .

[0029] The acquisition module 110 is used to acquire the estimated values ​​of multiple postures and multiple environmental information subsets corresponding to the multiple postures. The environmental information subset corresponding to any posture is the environmental information subset collected by the collection device at the posture.

[0030] The detection module 120 is used to detect one or more loops from the multiple postures based on the multiple environmental information subsets, wherein each loop includes two postures.

[0031] The registration module 130 is used to: for each of the one or more loops, align two subsets of environmental information corresponding to two postures of the loop to obtain a registration result of the loop.

[0032] The verification module 140 is used to determine the validity of the one or more loop closures based on the registration results of the one or more loop closures.

[0033] The optimization module 150 is used to optimize the estimated values ​​of at least part of the multiple postures according to the effectiveness of the one or more loops.

[0034] For more details about the system 100 and its modules, please refer to Figure 2 and its related description.

[0035] It should be understood that Figure 1 The system and its modules shown can be implemented in various ways. For example, in some embodiments, the system and its modules can be implemented by hardware, software or a combination of software and hardware. Among them, the hardware part can be implemented using dedicated logic; the software part can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or a dedicated design hardware. Those skilled in the art will understand that the above methods and systems can be implemented using computer executable instructions and / or included in a processor control code, such as a carrier medium such as a disk, CD or DVD-ROM, a programmable memory such as a read-only memory (firmware), or a data carrier such as an optical or electronic signal carrier. Such code is provided on the system and its modules of this specification. Not only can the hardware circuits such as ultra-large-scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, or programmable hardware devices such as field programmable gate arrays, programmable logic devices, etc. be implemented, it can also be implemented with software such as executed by various types of processors, and it can also be implemented by a combination of the above hardware circuits and software (for example, firmware).

[0036] It should be noted that the above description of the system and its modules is only for the convenience of description, and this specification cannot be limited to the scope of the embodiments. It can be understood that for those skilled in the art, after understanding the principle of the system, it is possible to arbitrarily combine the various modules or form a subsystem connected with other modules without deviating from this principle. For example, the verification module 140 and the optimization module 150 can be two modules or combined into one module. For another example, the system 100 can also include a fusion module (not shown), which is used to fuse the multiple environmental information subsets according to the estimated values ​​of the optimized multiple postures to obtain an environmental information set. Such variations are within the scope of protection of this specification.

[0037] Figure 2 is an exemplary flow chart of a posture optimization method according to some embodiments of this specification. Process 200 is executed by at least one processor (e.g., at least one processor of a mobile device or a server). In some embodiments, process 200 is executed by Figure 1 The posture optimization system 100 shown is executed. Figure 2 As shown, process 200 may include the following steps.

[0038] Step 210 , obtaining estimated values ​​of multiple postures and multiple environmental information subsets corresponding to the multiple postures, wherein the environmental information subset corresponding to any posture is the environmental information subset collected by the collection device at the posture. In some embodiments, step 210 is performed by the acquisition module 110 .

[0039] The pose includes the position and posture (posture is sometimes also called orientation). In the spatial coordinate system, the position can be represented by the coordinates of the point (for example, a 1×3 matrix), and the posture (such as the posture of a rigid body) can be represented by the direction matrix (such as a 3×3 matrix). In some representation methods, the representation of the pose can be a combination of the coordinates of the point and the direction matrix (that is, it includes two matrices). In other representation methods, the coordinates of the point and the direction matrix can be integrated into a single matrix (for example, a 4×4 matrix) to obtain another representation of the pose.

[0040] The acquisition device can estimate its own posture during driving (for example, through an odometer). Posture estimation is a recursive process. Specifically, the acquisition device can determine the estimated value of the current posture based on the estimated value of the previous posture and the estimated value of its own motion (including translation and rotation) within the acquisition interval. The acquisition interval here is the process from the previous posture to the current posture. Motion estimation within any acquisition interval can be achieved based on the alignment of two subsets of environmental information corresponding to adjacent postures. During the recursive process, the estimated error of the previous posture will be passed on to the optimization of the current posture. As the posture estimation continues, the error continues to accumulate. Therefore, it is necessary to reduce the cumulative error of posture estimation through posture optimization.

[0041] The environmental information subset includes environmental information collected by the collection device in a certain position. The environmental information subset can be expressed in the form of a graph. For example, the environmental information subset can be a point cloud, which is a collection of points, and objects in the environment (such as obstacles) can be represented as a collection of points. For another example, the environmental information subset can be an image, which is a collection of pixels (such as a pixel array). Of course, the environmental information subset can also be expressed in other forms, such as a three-dimensional model (such as a grid model).

[0042] The acquisition device may include various sensors, for example, radar (such as laser radar), camera (such as monocular camera, binocular camera, depth camera), etc. Taking laser radar as an example, the acquisition device may collect a subset of environmental information in the form of laser point cloud through the laser radar.

[0043] In some embodiments, the multiple postures are distributed on the same driving trajectory. In some embodiments, the multiple postures are distributed on different driving trajectories. In some embodiments, the multiple postures only include the postures of a single acquisition device at different times. In some embodiments, the multiple postures include the postures of different acquisition devices at different times. Just as an example. In order to improve data collection efficiency, multiple acquisition devices (such as robots) can be used to collect environmental information of the same area along their respective driving trajectories to obtain multiple subsets of environmental information corresponding to multiple postures. The multiple postures are the sum of the postures on the driving trajectories of the multiple acquisition devices. It should be understood that when the position in a posture is on a trajectory, the posture is considered to be on the trajectory.

[0044] Step 220 , based on the multiple subsets of environmental information, detect one or more loops from the multiple poses, wherein each loop includes two poses. In some embodiments, step 220 is performed by the detection module 120 .

[0045] Pose optimization can be achieved by constructing loop constraints. Loop constraints refer to constraints on two poses with the same position (allowing a certain deviation), and these two poses constitute a loop. An exemplary loop constraint may include that the difference between the two poses of the loop is less than a set threshold. The two poses that constitute the loop are close in space but have a long time interval (separated by at least one pose). As mentioned above, pose estimation is to constrain two adjacent poses. Therefore, the introduction of loop constraints can reduce the error transmission distance, thereby improving the accuracy of pose estimation.

[0046] Below through Figure 3A and Figure 3B Explain the role of loop constraints. Figure 3A As shown, the track 310 includes two straight segments close to each other and a turning segment between the two straight segments. i (i is an integer and 0≤i≤17) represents the position and posture at a certain moment on the trajectory 310. The larger the subscript i is, the higher the position and posture A i The later the time. 15 As an example, the loop constraint A 2 -A 15 (The dotted line indicates) skipping 12 pairs of adjacent poses reduces the cumulative error of 12 poses. Figure 3B As shown, track 320 and track 330 are two tracks close to each other. i (i is an integer and 0≤i≤8) represents the position and posture at a certain moment on the trajectory 320, B j (j is an integer and 0≤j≤8) indicates that the larger the subscript i of the posture at a certain moment on the trajectory 330, the larger the posture (A i or B j) is later. 6 As an example, the loop constraint A 6 -B 6 (Indicated by the dotted line) The cumulative error of multiple postures is reduced.

[0047] When the collection device passes the same position, the collected environment information subsets are similar. Based on this, the detection module 120 can detect one or more loops from the multiple postures based on the comparison results of the multiple environment information subsets.

[0048] In some embodiments, for each environmental information subset in the multiple environmental information subsets, the detection module 120 can generate a semantic description vector of the environmental information subset. Furthermore, the detection module 120 can compare the semantic description vectors of the multiple environmental information subsets in pairs to obtain comparison results of the multiple environmental information subsets in pairs. In some embodiments, the detection module 120 can generate a semantic description vector of an environmental information subset through a feature extraction model (which can be a model used to extract semantic description vectors from environmental information subsets in existing technology or future development technology, for example, a bag of words model, a VoxelNet model, a SpinNet model, a D3Feat model, a SuperLine3D model, a Pointnet++ model, etc.). The input of the feature extraction model is the environmental information subset, and the output of the feature extraction model is the semantic description vector of the environmental information subset.

[0049] In some embodiments, the detection module 120 may preprocess a subset of environmental information (also referred to as an original subset, for example, an original point cloud, an original subgraph), and then generate a semantic description vector of the subset of environmental information based on the preprocessed subset of environmental information. The preprocessing may include classifying and filtering the environmental information in the original subset, so as to retain only environmental information of the type of interest (for example, landmark lines, poles, light boards, and signboards). The subset of environmental information that only retains the type of environmental information of interest is also referred to as a semantic subset (for example, a semantic point cloud, a semantic subgraph). When the subset of environmental information is presented in the form of a graph (for example, a point cloud or an image), the classification may be instance segmentation, which refers to labeling different types of objects in the graph. In some embodiments, the screening in the preprocessing may be omitted, and the subset of environmental information that has only undergone information classification (such as instance segmentation) may also be referred to as a semantic subset.

[0050] In some embodiments, when the vector elements exceeding the first preset ratio in the semantic description vectors of any two subsets of environmental information are equal, the detection module 120 can determine a loop including the corresponding two postures. The value range of the first preset ratio can be determined by technicians based on experience or based on historical experimental data, and this application does not limit this. As an example only, the value range of the first preset ratio can be 50% to 90%.

[0051] In some embodiments, when the similarity between the semantic description vectors of any two subsets of environmental information exceeds a set threshold, the detection module 120 may determine the corresponding two postures as a loop.

[0052] Step 230 , for each of the one or more loops, align two subsets of environment information corresponding to two poses of the loop to obtain a registration result of the loop. In some embodiments, step 230 is performed by the registration module 130 .

[0053] Registration refers to matching elements in two sets by transformation (e.g., translation and / or rotation). The registration result represents the transformation relationship between the elements that are successfully matched between the two sets. Taking the environment information subset as a point set (such as a point cloud) as an example, the registration refers to matching points in two environment information subsets by translation and / or rotation operations, and the registration result represents the transformation relationship between the successfully matched points in the two environment information subsets.

[0054] In some embodiments, the registration result may include a translation vector and a rotation matrix. In some embodiments, the registration result is in matrix form, also referred to as a transformation matrix. That is, the transformation matrix may include the translation vector and the rotation matrix.

[0055] In some embodiments, the registration is implemented by an ICP (Iterative Closest Point) algorithm or an ICP-based registration algorithm. For example, when the environmental information subset is a point set (such as a point cloud), the registration module 130 can generate a loop-closed registration result by an ICP algorithm or an ICP-based registration algorithm.

[0056] Step 240 , based on the registration results of the one or more loop closures, determine the validity of the one or more loop closures. In some embodiments, step 240 is performed by the verification module 140 .

[0057] Since the environment information subsets and the poses correspond to each other, the transformation between the environment information subsets represented by the registration result is also the transformation between the poses. Using the transformation between the poses represented by the registration result, the verification module 140 can determine the validity of the loop (each loop includes two poses).

[0058] In some embodiments, the validity of the loop refers to whether the loop is valid, for example, "0" can be used to indicate invalidity, and "1" can be used to indicate validity. In some embodiments, the validity of the loop can be quantified by weight, and the weight of the loop can be a discrete value (for example, the weight is 0 or 1) or a continuous value (for example, the weight range is [0,1]).

[0059] In some embodiments, the verification module 140 may perform one or more verifications based on the registration results of the one or more loops to obtain verification results of the one or more verifications. Furthermore, the verification module 140 may determine the validity of the one or more loops based on the verification results of the one or more verifications. For example, the verification module 140 may determine one or more valid loops from the one or more loops based on the verification results of the one or more verifications.

[0060] In some embodiments, for each of the one or more loops, when the loop passes all verification items, the verification module 140 determines the loop as a valid loop. When the loop does not pass any verification item, the verification module 140 determines the loop as an invalid loop.

[0061] In some embodiments, for each of the one or more loops, the verification module 140 may determine the score of the loop under each verification item based on the verification result of each verification item. Furthermore, for each of the one or more loops, the verification module 140 may summarize the scores of the loop under all verification items to obtain the weight of the loop.

[0062] Some exemplary verification items are described below.

[0063] In some embodiments, the environmental information subset is a point cloud. For each of the one or more loops, the registration module 130 can perform point cloud registration on the two environmental information subsets corresponding to the two poses of the loop to obtain the registration result of the loop. Then, for each of the one or more loops, the verification module 140 can determine the validity of the loop based on the proportion of successfully matched points indicated by the registration result of the loop. Assuming that a loop is valid (or has high validity), the proportion of successfully matched points indicated by the registration result of the loop should be high. For example, when the proportion of successfully matched points indicated by the registration result of any loop exceeds the second preset proportion, it can be considered that the loop has passed this verification. The value range of the second preset proportion can be determined by a technician based on experience or based on historical experimental data, and this application does not limit this. Exemplarily, the value range of the second preset proportion can be 20% to 70%. It should be noted that when the scales of the two point clouds involved in the registration (ie, the number of points included) are not equal, the ratio of successfully matched points may refer to the ratio between the successfully matched points and the smaller point cloud scale.

[0064] In some embodiments, the environmental information subset is a point cloud. For each of the one or more loops, the registration module 130 can perform point cloud registration on two environmental information subsets corresponding to the two poses of the loop to obtain the registration result of the loop. Then, for each of the one or more loops, the verification module 140 can rotate the first semantic point cloud according to the rotation matrix in the registration result of the loop to obtain the rotated first semantic point cloud. The first semantic point cloud is obtained based on any subset of the two environmental information subsets (recorded as the first original point cloud). Furthermore, the verification module 140 can compare the second semantic point cloud with the rotated first semantic point cloud, and determine the validity of the loop based on the first comparison result obtained. The second semantic point cloud is obtained based on another subset of the two environmental information subsets (recorded as the second original point cloud). Assuming that a loop is valid (or has high validity), the difference between the second semantic point cloud and the rotated first semantic point cloud should be small. The subset of environmental information that only retains the type of environmental information of interest is also called a semantic subset (e.g., a semantic point cloud, a semantic subgraph). As an example only, the verification module 140 can determine whether the same type of objects in the second semantic point cloud and the rotated first semantic point cloud meet the preset consistency conditions, for example, whether the landmark lines are aligned, whether the rod-shaped objects have ghosting, whether the light board is deviated, whether the signboard is deviated, etc. For any loop, when the same type of objects in the second semantic point cloud and the rotated first semantic point cloud meet the preset consistency conditions, the loop can be considered to have passed this verification.

[0065] In some embodiments, when the environmental information subset is a point cloud, similarly, for each of the one or more loops, the verification module 140 can rotate the first semantic description vector according to the rotation matrix in the registration result of the loop to obtain the rotated first semantic description vector. The first semantic point cloud before rotation is obtained based on the first original point cloud. Furthermore, the verification module 140 can compare the second semantic description vector with the rotated first semantic description vector to obtain a second comparison result, and determine the validity of the loop based on the obtained second comparison result. The second semantic point cloud is obtained based on the second original point cloud. Assuming that a loop is valid (or has high validity), the difference between the second semantic description vector and the rotated first semantic description vector should be small. For example, for any loop, when the vector elements exceeding the third preset ratio in the second semantic description vector and the rotated first semantic description vector are equal, it can be considered that the loop passes this verification. The value range of the third preset ratio can be determined by technicians based on experience or based on historical experimental data, and this application is not limited to this. Exemplarily, the value range of the third preset ratio can be 50% to 90%.

[0066] In some embodiments, the first semantic point cloud refers to a semantic point cloud obtained by retaining only the point cloud portion corresponding to the object of the type of interest (e.g., landmark lines, poles, light boards, and signboards) in the first original point cloud, and the second semantic point cloud refers to a semantic point cloud obtained by retaining only the point cloud portion corresponding to the object of the type of interest (e.g., landmark lines, poles, light boards, and signboards) in the second original point cloud.

[0067] More details about the rotation matrix, the original point cloud, the semantic point cloud, and the semantic description vector can be found elsewhere in this specification (eg, in the detailed description of step 220 ).

[0068] In some embodiments, the environmental information subset is a point cloud, the multiple postures are distributed on different driving trajectories, and the one or more loops are multiple loops. Based on this, for each loop in the one or more loops, the registration module 130 can perform point cloud registration on the two environmental information subsets corresponding to the two postures of the loop to obtain the registration result of the loop. Furthermore, the verification module 140 can determine one or more pairs of target loops based on the multiple loops. For the convenience of description, any loop in each pair of target loops is referred to as the first loop, and the other loop is referred to as the second loop, that is, each pair of target loops includes the first loop and the second loop. Among them, the first loop includes the first posture on the first driving trajectory and the second posture on the second driving trajectory, and the second loop includes the third posture on the first driving trajectory and the fourth posture on the second driving trajectory. In some embodiments, the first driving trajectory and the second driving trajectory can be two trajectories whose feature points meet the preset distance requirements. For example, the first driving trajectory and the second driving trajectory can be two straight trajectories whose distance between the starting points of the trajectories is less than the first distance threshold and the distance between the end points of the trajectories is less than the second distance threshold.

[0069] For each pair of target loops in the one or more pairs of target loops, the verification module 140 can: perform four transformations on any one of the poses in the pair of target loops according to the first transformation matrix, the second transformation matrix, the third transformation matrix and the fourth transformation matrix. Wherein: the first transformation matrix is ​​the estimation result of the transformation between the first pose and the third pose by the acquisition device, the second transformation matrix is ​​the estimation result of the transformation between the second pose and the fourth pose by the acquisition device, the third transformation matrix is ​​the transformation matrix between the first pose and the second pose in the registration result of the first loop, and the fourth transformation matrix is ​​the transformation matrix between the third pose and the fourth pose in the registration result of the second loop. Furthermore, for each pair of target loops in the one or more pairs of target loops, the verification module 140 can compare any one of the poses before and after the four transformations, and determine the validity of the pair of target loops based on the third comparison result obtained. The pose transformation sequence of the four transformations forms a closed loop. Assuming that the target loop is valid (or highly valid), the difference between any one of the poses before and after the four transformations should be small (for example, the norm of the pose difference is less than the set threshold). For any pair of target loops, when the third comparison result indicates that the norm of the pose difference is less than the set threshold, it can be considered that the first loop and the second loop in the pair of target loops have passed this verification.

[0070] More details about the transformation matrix can be found elsewhere in this specification (eg, in the detailed description of step 220).

[0071] Just as an example, Figure 4 As shown, the first loop A 1 -B7 (dashed line) and the second loop A 2 -B 6 (dashed lines indicate) form a pair of target loops, where the first loop A 1 -B 7 Including the first position A on the first driving trajectory 410 1 and a second posture B on the second driving trajectory 420 7 , the second loop A 2 -B 6 Including the third posture A on the first driving trajectory 410 2 and the fourth posture B on the second driving trajectory 420 6 The larger the subscript of the posture, the later the time of the posture (A or B). The verification module 140 can 7 Four transformations are performed, the first transformation can be expressed as the first loop A 1 -B 7 The first pose A in the registration result 1 To the second pose B 7 The inverse matrix of the transformation matrix (i.e., the third transformation matrix) (the inverse matrix represents the second posture B 7 To the first position A 1 The second transformation can be expressed as the first pose A estimated by the acquisition device (e.g., the first robot experiencing the first driving trajectory 410) 1 To the third position A 2 The transformation matrix (i.e., the first transformation matrix) of the third transformation can be expressed as the second loop A 2 -B 6 The third pose A in the registration result 2 To the fourth position B 6 The fourth transformation matrix (ie, the fourth transformation matrix) can be expressed as the third posture A estimated by the acquisition device (eg, the second robot experiencing the second driving trajectory 420) 2 To the fourth position B 6 ’s transformation matrix (i.e., the second transformation matrix).

[0072] Step 250 , optimizing the estimated values ​​of at least some of the plurality of postures according to the validity of the one or more loop closures. In some embodiments, step 250 is performed by the optimization module 150 .

[0073] In some embodiments, the optimization module 150 can optimize the estimated values ​​of at least some of the multiple poses according to the one or more valid loops. Specifically, the optimization module 150 can construct loop constraints for the one or more valid loops. Furthermore, the optimization module 150 can optimize the estimated values ​​of at least some of the multiple poses based on the loop constraints of the one or more valid loops. Since invalid loops are eliminated, effective pose optimization can be achieved.

[0074] In some embodiments, the optimization of the pose estimation value (i.e., pose optimization) may adopt a graph optimization technique based on the least squares method, for example, open source libraries such as g2o, Ceres, GTSAM, SE-Sync, etc. may be called to implement pose optimization. In other embodiments, pose optimization may adopt other optimization techniques, for example, graph optimization techniques based on factor graphs.

[0075] In some embodiments, the optimization module 150 can determine the confidence of the loop constraints of the one or more loops according to the weights of the one or more loops. Furthermore, the optimization module 150 can optimize the estimated values ​​of at least some of the multiple poses based on the loop constraints of the one or more loops and the confidence of the loop constraints of the one or more loops. It can be understood that the higher the confidence of the loop constraint, the greater its influence in the pose optimization. In addition, for the same loop, the confidence of its loop constraint can be proportional to its weight. As an example only, the confidence interval can be equal to the weight interval (for example, both are [0,1]), the larger the weight of the loop, the higher its effectiveness, and accordingly, the higher the confidence of its loop constraint.

[0076] In some embodiments, the estimated values ​​of the multiple optimized postures can be used to fuse (such as stitching) multiple environmental information subsets (for example, multiple local point clouds or multiple local sub-maps) that correspond one to one with the multiple postures. Specifically, the processing device can fuse the multiple environmental information subsets according to the different postures reflected by the estimated values ​​of the multiple optimized postures to obtain an environmental information set (for example, a complete point cloud or sub-map). As an example only, the processing device can input the estimated values ​​of the multiple postures and the multiple environmental information subsets into a trained machine learning model to obtain an environmental information set.

[0077] It should be noted that the above description of the relevant process is only for example and explanation, and does not limit the scope of application of this specification. For those skilled in the art, various modifications and changes can be made to the process under the guidance of this specification. However, these modifications and changes are still within the scope of this specification.

[0078] The beneficial effects that may be brought about by the embodiments of this specification include but are not limited to: (1) The system determines the validity of the loop based on the registration results of the environmental information subset, and performs posture optimization based on the validity of the loop, so that effective posture optimization can be achieved; (2) Effective posture optimization can ensure the fusion effect of environmental information. It should be noted that different embodiments may produce different beneficial effects. In different embodiments, the beneficial effects that may be produced may be any one or a combination of the above, or any other possible beneficial effects.

[0079] The basic concepts have been described above. Obviously, for those skilled in the art, the above detailed disclosure is only for example and does not constitute a limitation on the embodiments of this specification. Although not explicitly stated here, those skilled in the art may make various modifications, improvements and corrections to the embodiments of this specification. Such modifications, improvements and corrections are suggested in the embodiments of this specification, so such modifications, improvements and corrections still belong to the spirit and scope of the exemplary embodiments of this specification.

[0080] At the same time, this specification uses specific words to describe the embodiments of this specification. For example, "one embodiment", "an embodiment" and / or "some embodiments" refer to a certain feature, structure or characteristic related to at least one embodiment of this specification. Therefore, it should be emphasized and noted that "one embodiment" or "an embodiment" or "an alternative embodiment" mentioned twice or more in different locations in this specification does not necessarily refer to the same embodiment. In addition, certain features, structures or characteristics in one or more embodiments of this specification can be appropriately combined.

[0081] In addition, it will be appreciated by those skilled in the art that the various aspects of the embodiments of this specification may be illustrated and described by a number of patentable categories or situations, including any new and useful process, machine, product or combination of substances, or any new and useful improvements thereto. Accordingly, the various aspects of the embodiments of this specification may be performed entirely by hardware, entirely by software (including firmware, resident software, microcode, etc.), or by a combination of hardware and software. The above hardware or software may all be referred to as "data blocks", "modules", "engines", "units", "components" or "systems". In addition, the various aspects of the embodiments of this specification may be represented as a computer product located in one or more computer-readable media, the product including computer-readable program code.

[0082] A computer storage medium may include a propagated data signal containing computer program code, for example, in baseband or as part of a carrier wave. The propagated signal may be in a variety of forms, including electromagnetic, optical, etc., or a suitable combination. A computer storage medium may be any computer-readable medium other than a computer-readable storage medium, which can be connected to an instruction execution system, device or apparatus to communicate, propagate or transmit the program for use. The program code on the computer storage medium may be transmitted via any suitable medium, including radio, cable, fiber optic cable, RF, or similar media, or any combination of the above media.

[0083] The computer program coding required for the operation of each part of the embodiment of this specification can be written in any one or more programming languages, including object-oriented programming languages ​​such as Java, Scala, Smalltalk, Eiffel, JADE, Emerald, C++, C#, VB.NET, Python, etc., conventional procedural programming languages ​​such as C language, VisualBasic, Fortran2003, Perl, COBOL2002, PHP, ABAP, dynamic programming languages ​​such as Python, Ruby and Groovy, or other programming languages, etc. The program coding can be run completely on the user's computer, or run on the user's computer as an independent software package, or partly run on the user's computer and partly run on the remote computer, or run completely on the remote computer or processing equipment. In the latter case, the remote computer can be connected to the user's computer by any network form, such as a local area network (LAN) or a wide area network (WAN), or connected to an external computer (e.g., by the Internet), or in a cloud computing environment, or used as a service such as software as a service (SaaS).

[0084] In addition, unless explicitly stated in the claims, the order of the processing elements and sequences described in the embodiments of this specification, the use of alphanumeric characters, or the use of other names are not intended to limit the order of the processes and methods of the embodiments of this specification. Although the above disclosure discusses some invention embodiments that are currently considered useful through various examples, it should be understood that such details are only for illustrative purposes, and the attached claims are not limited to the disclosed embodiments. On the contrary, the claims are intended to cover all modifications and equivalent combinations that are consistent with the essence and scope of the embodiments of this specification. For example, although the system components described above can be implemented by hardware devices, they can also be implemented only by software solutions, such as installing the described system on an existing processing device or mobile device.

[0085] Similarly, it should be noted that in order to simplify the description of the embodiments disclosed in this specification, thereby helping to understand one or more embodiments of the invention, in the above description of the embodiments of this specification, multiple features are sometimes combined into one embodiment, figure or description thereof. However, this disclosure method does not mean that the features required by the objects of the embodiments of this specification are more than the features mentioned in the claims. In fact, the features of the embodiments are less than all the features of the single embodiment disclosed above.

[0086] Each patent, patent application, patent application publication, and other materials, such as articles, books, specifications, publications, documents, etc., cited in this specification is hereby incorporated by reference in its entirety. Except for application history documents that are inconsistent with or conflicting with the contents of this specification, documents that limit the broadest scope of the claims of this application (currently or later attached to this application) are also excluded. It should be noted that if the descriptions, definitions, and / or use of terms in the materials attached to this specification are inconsistent or conflicting with the contents described in this specification, the descriptions, definitions, and / or use of terms in this specification shall prevail.

[0087] Finally, it should be understood that the embodiments described in this specification are only used to illustrate the principles of the embodiments of this specification. Other variations may also fall within the scope of the embodiments of this specification. Therefore, as an example and not a limitation, alternative configurations of the embodiments of this specification may be considered consistent with the teachings of this specification. Accordingly, the embodiments of this specification are not limited to the embodiments explicitly introduced and described in this specification.

Claims

1. A posture optimization method, characterized in that: The method is executed by at least one processor, and the method includes: Acquire estimated values ​​of multiple postures and multiple environmental information subsets corresponding to the multiple postures, wherein the environmental information subset corresponding to any posture is the environmental information subset collected by the collection device under the posture; Based on the multiple subsets of environmental information, detect one or more loops from the multiple poses, wherein each loop includes two poses; For each of the one or more loops, aligning two subsets of environment information corresponding to two postures of the loop to obtain an alignment result of the loop; Determining validity of the one or more loop closures based on the registration results of the one or more loop closures; According to the validity of the one or more loop closures, the estimated values ​​of at least some of the multiple poses are optimized.

2. The method according to claim 1, characterized in that Determining the validity of the one or more loops based on the registration results of the one or more loops, comprising: performing one or more verifications based on the registration results of the one or more loops to obtain verification results of the one or more verifications; determining one or more valid loops from the one or more loops based on the verification results of the one or more verifications; According to the validity of the one or more loop closures, the estimated values ​​of at least some of the multiple postures are optimized, including: according to the one or more valid loop closures, the estimated values ​​of at least some of the multiple postures are optimized.

3. The method according to claim 1, characterized in that The registration is achieved by an ICP algorithm or an ICP-based registration algorithm.

4. The method according to any one of claims 1 to 3, characterized in that The environmental information subset is a point cloud, and for each of the one or more loops, two environmental information subsets corresponding to two postures of the loop are registered to obtain a registration result of the loop, including: performing point cloud registration on the two environmental information subsets to obtain a registration result of the loop; Based on the registration results of the one or more loop closures, determining the validity of the one or more loop closures includes: for each of the one or more loop closures, determining the validity of the loop closure based on a proportion of successfully matched points indicated by the registration result of the loop closure.

5. The method according to any one of claims 1 to 3, characterized in that: The environmental information subset is a point cloud, and for each of the one or more loops, two environmental information subsets corresponding to two postures of the loop are registered to obtain a registration result of the loop, including: performing point cloud registration on the two environmental information subsets to obtain a registration result of the loop; The two environmental information subsets include a first original point cloud and a second original point cloud, and based on the registration results of the one or more loop closures, determining the validity of the one or more loop closures includes: For each of the one or more loops: rotate the first semantic point cloud according to the rotation matrix in the registration result of the loop to obtain the rotated first semantic point cloud, and the first semantic point cloud before rotation is obtained based on the first original point cloud; compare the second semantic point cloud with the rotated first semantic point cloud to obtain a first comparison result, and the second semantic point cloud is obtained based on the second original point cloud; determine the validity of the loop based on the first comparison result.

6. The method according to any one of claims 1 to 3, characterized in that: The environmental information subset is a point cloud, and for each of the one or more loops, two environmental information subsets corresponding to two postures of the loop are registered to obtain a registration result of the loop, including: performing point cloud registration on the two environmental information subsets to obtain a registration result of the loop; The two environmental information subsets include a first original point cloud and a second original point cloud, and based on the registration results of the one or more loop closures, determining the validity of the one or more loop closures includes: For each of the one or more loops: rotate the first semantic description vector according to the rotation matrix in the registration result of the loop to obtain a rotated semantic description vector, wherein the first semantic description vector is obtained based on the first original point cloud; compare the second semantic description vector with the rotated first semantic description vector to obtain a second comparison result, wherein the second semantic point cloud is obtained based on the second original point cloud; determine the validity of the loop based on the second comparison result.

7. The method according to any one of claims 1 to 3, characterized in that: The environmental information subset is a point cloud, and for each of the one or more loops, two environmental information subsets corresponding to two postures of the loop are registered to obtain a registration result of the loop, including: performing point cloud registration on the two environmental information subsets to obtain a registration result of the loop; The multiple postures are distributed on different driving trajectories, and the one or more loops are multiple loops; and based on the registration results of the one or more loops, determining the validity of the one or more loops includes: Determine one or more pairs of target loops based on the multiple loops, each pair of target loops includes a first loop and a second loop, the first loop includes a first posture on the first driving trajectory and a second posture on the second driving trajectory, and the second loop includes a third posture on the first driving trajectory and a fourth posture on the second driving trajectory; For each pair of target loops in the one or more pairs of target loops: according to the first transformation matrix, the second transformation matrix, the third transformation matrix and the fourth transformation matrix, any one of the postures in the pair of target loops is transformed four times, the first transformation matrix is ​​the estimation result of the transformation between the first posture and the third posture by the acquisition device, the second transformation matrix is ​​the estimation result of the transformation between the second posture and the fourth posture by the acquisition device, the third transformation matrix is ​​the transformation matrix between the first posture and the second posture in the registration result of the first loop, and the fourth transformation matrix is ​​the transformation matrix between the third posture and the fourth posture in the registration result of the second loop; compare the any one posture before and after the four transformations to obtain a third comparison result; determine the validity of the pair of target loops based on the third comparison result.

8. A computing device, characterized in that The method comprises a processor and a storage device, wherein the storage device is used to store instructions. When the processor executes the instructions, the posture optimization method as described in any one of claims 1 to 7 is implemented.

9. A computer-readable storage medium, characterized in that: The storage medium stores posture optimization instructions. When a computer runs the posture optimization instructions, the posture optimization method according to any one of claims 1 to 7 is implemented.

10. A posture optimization system, characterized in that: It includes an acquisition module, a determination module, a registration module, a verification module and an optimization module; The acquisition module is used to acquire estimated values ​​of multiple postures and multiple environmental information subsets corresponding to the multiple postures, wherein the environmental information subset corresponding to any posture is the environmental information subset collected by the collection device under the posture; The determination module is used to detect one or more loops from the multiple poses based on the multiple environmental information subsets, wherein each loop includes two poses; The registration module is used to register two environment information subsets corresponding to two postures of each loop in the one or more loops to obtain a registration result of the loop; The verification module is used to determine the validity of the one or more loops based on the registration results of the one or more loops; The optimization module is used to optimize the estimated values ​​of at least part of the multiple postures according to the effectiveness of the one or more loops.

Images