Point cloud trajectory optimization method and device, unmanned vehicle and storage medium

Abstract

The invention discloses a point cloud trajectory optimization method and device, an unmanned vehicle and a storage medium, and the method comprises the steps: obtaining a plurality of groups of point cloud collection trajectories, and determining an intra-group residual term of each group of point cloud collection trajectories, the intra-group residual terms comprise a first residual term of each intra-group point cloud frame determined on the basis of a prior constraint condition, a second residual term determined on the basis of a relative pose relationship between any two point cloud frames in the group, and a third residual term of each intra-group point cloud frame determined on the basis of a global navigation satellite system (GNSS) measurement value; determining inter-group residual terms between each group of point cloud acquisition tracks based on a relative pose relationship between any two point cloud frames between each group of point cloud acquisition tracks; constructing an objective function based on the intra-group residual terms and the inter-group residual terms; and based on the objective function, constructing a nonlinear factor graph and performing optimization solution to obtain an optimized point cloud trajectory. In this way, the consistency between the incremental quantity and the historical prior data and between multiple sets of incremental data can be guaranteed, and batch processing of the incremental data is achieved.

Description

Technical Field

[0001] The present application belongs to the field of autonomous driving technology, and specifically relates to a point cloud trajectory optimization method and device, an unmanned vehicle, and a storage medium. Background Art

[0002] In the field of autonomous driving and mobile robots, comprehensive and high-precision three-dimensional point cloud reconstruction is a prerequisite. Based on different technical methods, it is currently mainly divided into traditional GNSS / IMU combined navigation + laser point cloud stitching technology, and SLAM relative positioning and reconstruction technology. However, in the face of increasingly large-scale reconstruction needs, neither of these two technologies can meet the needs. Large-scale scene reconstruction often involves hundreds or thousands of kilometers of mileage, including complex environments such as open and occluded, and is completed by multiple acquisitions, multiple vehicles or even fleets. The same area is often collected multiple times, and the collection area often needs to be expanded. In order to ensure that the overlapping areas of the data obtained by multiple acquisitions are consistent in accuracy, without quality problems such as stratification and fuzziness, and to ensure high consistency between data, multi-trip optimization and incremental acquisition optimization are required.

[0003] Multi-pass optimization and incremental collection optimization are essentially a kind of joint optimization. For actual production tasks, in addition to considering the consistency between an incremental data and historical prior data, it is also necessary to consider the consistency between multiple sets of data collected in the same batch. How to design a solution for batch processing of incremental data is a technical problem that needs to be solved urgently.

[0004] The information disclosed in this background technology section is only intended to increase the understanding of the overall background of the application and should not be regarded as an admission or any form of suggestion that the information constitutes the prior art already known to a person skilled in the art. Summary of the invention

[0005] The purpose of this application is to provide a point cloud trajectory optimization method, which is used to solve the problem of how to batch process incremental data.

[0006] To achieve the above objectives, the present application provides a point cloud trajectory optimization method, the method comprising: Acquire multiple groups of point cloud acquisition trajectories, and respectively determine the intra-group residual items of each group of point cloud acquisition trajectories, wherein the intra-group residual items include at least one of a first residual item, a second residual item, and a third residual item, the first residual item is a residual item of each point cloud frame in the group determined based on a priori constraints, the priori constraints are determined based on each group of point cloud acquisition trajectories and a corresponding reference point cloud map, the second residual item is a residual item determined based on a relative posture relationship between any two point cloud frames in the group, and the third residual item is a residual item of each point cloud frame in the group determined based on a global satellite navigation system GNSS measurement value; Based on the relative pose relationship between any two frames of point cloud frames between each group of point cloud acquisition trajectories, the inter-group residual term between each group of point cloud acquisition trajectories is determined; Constructing an objective function based on the within-group residual term and the between-group residual term; Based on the objective function, a nonlinear factor graph is constructed and optimized to obtain an optimized point cloud trajectory.

[0007] In one embodiment, based on the relative pose relationship between any two frames of point cloud frames between each group of point cloud acquisition trajectories, the inter-group residual term between each group of point cloud acquisition trajectories is determined, specifically including: A first point cloud frame is determined from the first set of point cloud acquisition trajectories, and a second point cloud frame is determined from the second set of point cloud acquisition trajectories, wherein the INS data point P corresponding to the first point cloud frame m The INS data point P corresponding to the second point cloud frame in the INS data of the first set of point cloud acquisition trajectory n nearest point; Based on the registration of the first point cloud frame and the second point cloud frame, a cross constraint of the first set of point cloud acquisition trajectories and the second set of point cloud acquisition trajectories is calculated; Based on the cross constraints and the relative pose relationship between the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories, an inter-group residual term between the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories is determined.

[0008] In one embodiment, the method further comprises: Time-aligning the INS data of the first set of point cloud acquisition trajectories with the historical trajectories, and time-aligning the INS data of the second set of point cloud acquisition trajectories with the historical trajectories; Construct a KD tree based on the INS data of the first set of point cloud acquisition trajectories and determine the INS data point P m and INS data point P n Is the distance greater than the preset cross-correlation radius threshold? If so, The steps of aligning the first point cloud frame with the second point cloud frame and calculating the intersection constraints of the first set of point cloud acquisition trajectories and the second set of point cloud acquisition trajectories are skipped.

[0009] In one embodiment, the historical trajectory of the first set of point cloud acquisition trajectories is a fusion trajectory of the IMU data and the LiDAR scanning frame, and the historical trajectory of the second set of point cloud acquisition trajectories is a fusion trajectory of the IMU data and the LiDAR scanning frame.

[0010] In one embodiment, multiple groups of point cloud acquisition trajectories are obtained, and the residual items within each group of point cloud acquisition trajectories are determined respectively, specifically including: Determining the first residual term based on the prior constraints and the positions of each group of point cloud acquisition trajectories at each moment; and / or, Determining the second residual term based on the relative pose relationship and loop closure constraint between any two point cloud frames in the group; and / or, The third residual term is determined based on the GNSS measurement values ​​and the poses of the point cloud trajectories in each group at each time.

[0011] In one embodiment, based on the objective function, a nonlinear factor graph is constructed, which specifically includes: The nonlinear factor graph is constructed by taking the postures corresponding to the multiple groups of point cloud acquisition trajectories at each moment as variable nodes and taking the objective function corresponding to each moment as factor nodes.

[0012] In one embodiment, the objective function corresponding to each moment is used as a factor node, specifically including: According to the prior constraint condition, the variable nodes are divided into a first type of variable nodes and a second type of variable nodes, wherein the point cloud acquisition trajectory corresponding to the first type of variable nodes has a prior constraint condition with the reference point cloud map, and the point cloud acquisition trajectory corresponding to the second type of variable nodes does not have a prior constraint condition with the reference point cloud map; Setting the factor node corresponding to the first type of variable node to include only the first residual term, the second residual term and the between-group residual term; The factor node corresponding to the second type variable node is set to include only the second residual term, the third residual term and the between-group residual term.

[0013] The present application also provides a point cloud trajectory optimization device, comprising: An acquisition module is used to acquire multiple groups of point cloud acquisition trajectories, and determine the intra-group residual items of each group of point cloud acquisition trajectories based on the determination module, wherein the intra-group residual items include at least one of a first residual item, a second residual item, and a third residual item, the first residual item is a residual item of each point cloud frame in the group determined based on a priori constraints, the priori constraints are determined based on each group of point cloud acquisition trajectories and a corresponding reference point cloud map, the second residual item is a residual item determined based on a relative posture relationship between any two point cloud frames in the group, and the third residual item is a residual item of each point cloud frame in the group determined based on a global satellite navigation system GNSS measurement value; The determination module is further used to determine the inter-group residual term between each group of point cloud acquisition trajectories based on the relative posture relationship between any two frames of point cloud frames between each group of point cloud acquisition trajectories; A construction module, used for constructing an objective function based on the intra-group residual term and the inter-group residual term; The optimization module is used to construct a nonlinear factor graph and optimize the solution based on the objective function to obtain an optimized point cloud trajectory.

[0014] The present application also provides an unmanned vehicle, comprising: at least one processor; and A memory storing instructions, which, when executed by the at least one processor, enables the at least one processor to perform the point cloud trajectory optimization method as described above.

[0015] The present application also provides a machine-readable storage medium storing executable instructions, which, when executed, enable the machine to perform the point cloud trajectory optimization method as described above.

[0016] Compared with the prior art, according to the point cloud trajectory optimization method of the present application, for the multiple groups of point cloud acquisition trajectories obtained, the intra-group residual term of each group of point cloud acquisition trajectories and the inter-group residual term between each group of point cloud acquisition trajectories are determined respectively, and then the objective function is constructed, and the nonlinear factor graph optimization solution is constructed based on the objective function to obtain the optimized point cloud trajectory. In this process, the intra-group residual term can ensure the consistency of an incremental data with the historical prior data, and the inter-group residual term can ensure the consistency between multiple groups of incremental data. In this way, the point cloud acquisition trajectories between different times, different vehicles, and different data sources can be fused to achieve batch processing of incremental data. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is an application scenario diagram of a point cloud trajectory optimization method according to an embodiment of the present application; Figure 2 is a flow chart of a point cloud trajectory optimization method according to an embodiment of the present application; Figure 3 is a schematic diagram of a factor graph constructed in a point cloud trajectory optimization method according to an embodiment of the present application; Figure 4 is a module diagram of a point cloud trajectory optimization device according to an embodiment of the present application; Figure 5 It is a hardware structure diagram of an unmanned vehicle according to an embodiment of the present application. DETAILED DESCRIPTION

[0018] The present application will be described in detail below in conjunction with the various embodiments shown in the accompanying drawings. However, these embodiments do not limit the present application, and any structural, methodological, or functional changes made by a person of ordinary skill in the art based on these embodiments are all within the scope of protection of the present application.

[0019] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein, for example. In addition, the terms "including" and "corresponding to" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0020] Before introducing the embodiments of the present application, the basic technologies and some technical terms involved in the embodiments of the present application are schematically explained: Autonomous driving system: A system that implements different levels of autonomous driving functions of a vehicle, such as assisted driving system (L2), high-speed autonomous driving system that requires human supervision (L3), and highly / fully autonomous driving system (L4 / L5).

[0021] Intelligent Traffic System (ITS), also known as Intelligent Transportation System, effectively and comprehensively applies advanced science and technology (information technology, computer technology, data communication technology, sensor technology, electronic control technology, automatic control theory, operations research and artificial intelligence, etc.) to transportation, service control and vehicle manufacturing, strengthens the connection between vehicles, roads and users, and thus forms a comprehensive transportation system that ensures safety, improves efficiency, improves the environment and saves energy.

[0022] Intelligent Vehicle Infrastructure Cooperative Systems (IVICS), referred to as IVICS, is a development direction of intelligent transportation systems. IVICS uses advanced wireless communications and new generation Internet technologies to implement dynamic real-time information interaction between vehicles and between vehicles and roads in all directions, and conducts active vehicle safety control and road cooperative management based on the collection and integration of dynamic traffic information in all time and space, fully realizing the effective coordination of people, vehicles and roads, ensuring traffic safety, and improving traffic efficiency, thus forming a safe, efficient and environmentally friendly road traffic system.

[0023] Point cloud data refers to a set of vectors in a three-dimensional coordinate system. The set is recorded in the form of points, each of which contains three-dimensional coordinates and can carry other information about the properties of the point, such as color, reflectivity, intensity, etc. Point cloud data is usually obtained by laser scanners, cameras, 3D scanners and other devices, and can be used in applications such as 3D modeling, scene reconstruction, robot navigation, virtual reality and augmented reality.

[0024] The main characteristics of point cloud data are high-precision, high-resolution and high-dimensional geometric information, which can intuitively represent the shape, surface and texture of objects in space. The processing and analysis of point cloud data usually requires the use of computer vision and computer graphics techniques, such as point cloud filtering, registration, segmentation, reconstruction, recognition and classification.

[0025] Figure 1 FIG. 1 is an optional schematic diagram of the system architecture involved in the embodiment of the present application. Figure 1 As shown in the figure, sensors mounted on the data collection vehicle, such as LiDAR, visual sensors, inertial measurement units (IMUs), and RTK (Real-time kinematic) and other technologies, obtain multiple groups of point cloud collection trajectories. The server can obtain these point cloud collection trajectories and then determine the intra-group residual terms of each group of point cloud collection trajectories and the inter-group residual terms between each group of point cloud collection trajectories. The server also constructs an objective function based on the intra-group residual terms and the inter-group residual terms, and uses a nonlinear factor graph to optimize the solution to obtain the optimized point cloud trajectory.

[0026] Continue to participate Figure 1, a specific scenario in which the point cloud trajectory optimization method of the present application is applied may include a server and a terminal device. Among them, the terminal device may include a vehicle-mounted terminal and a user terminal. The vehicle-mounted terminal may include a driving computer or an on-board unit (On Board Unit, OBU), etc. The vehicle-mounted terminal may also be an application (application, APP) on the terminal, an APP on the smart rearview mirror, an APP or a small program on a mobile phone, etc., which are not limited here. The user terminal (userequipment, UE) may be a wireless terminal device or a wired terminal device. The wireless terminal device may refer to a device with a wireless transceiver function. The user terminal may be a mobile phone (mobile phone), a tablet computer (Pad), a computer with a wireless transceiver function, a virtual reality (Virtual Reality, VR) user device, an augmented reality (Augmented Reality, AR) user device, an intelligent voice interaction device, a smart home appliance, a vehicle-mounted terminal, an aircraft, etc., which are not limited here. The terminal communicates with the server through the network. The data storage system can store the data that the server needs to process. The data storage system can exist alone or be integrated on the server.

[0027] It can be understood that the above is just an example of a possible execution subject of the point cloud trajectory optimization method of the present application, and in more embodiments, the point cloud trajectory optimization can also be directly performed by a terminal device with sufficient computing power, and the present application does not limit this. Moreover, regardless of the type of server / terminal device, the point cloud trajectory optimization method provided in the embodiment of the present application can be adapted to the autonomous driving system of unmanned vehicles, including L2, L3, L4 and above autonomous driving systems.

[0028] Specific reference Figure 2 , introduces an embodiment of the point cloud trajectory optimization method of the present application. In this embodiment, the method includes: S11. Acquire multiple groups of point cloud acquisition trajectories, and respectively determine the intra-group residual items of each group of point cloud acquisition trajectories.

[0029] The point cloud acquisition trajectory can be a collection of multi-frame point clouds obtained by periodic scanning of the laser radar within a set period. The laser radar can be installed on an unmanned vehicle, for example, and the unmanned vehicle obtains the corresponding multi-frame point clouds during the driving process. At the same time, the unmanned vehicle can perform acquisition tasks based on a certain range of target areas. The unmanned vehicle can perform multiple trips in the area, or multiple unmanned vehicles can perform acquisition in the area, thereby obtaining multiple groups of point cloud acquisition trajectories. This application does not limit the specific acquisition form or scanning time of the point cloud acquisition trajectory.

[0030] The residual term within the group includes at least one of the first residual term, the second residual term and the third residual term. Among them, the first residual term is the residual term of each point cloud frame in the group determined based on the prior constraint condition, and the prior constraint condition is determined based on the point cloud acquisition trajectory of each group and the corresponding reference point cloud map. The second residual term is a residual term determined based on the relative posture relationship between any two point cloud frames in the group. The third residual term is a residual term of each point cloud frame in the group determined based on the GNSS measurement value of the global satellite navigation system. The following specifically introduces the determination method of the first residual term, the second residual term and the third residual term.

[0031] In this embodiment, the reference point cloud map is a map that has been constructed in advance and is used as a reference for point cloud matching, and the determination of the prior constraints can be implemented based on the branch and bound algorithm. Specifically, the point cloud acquisition trajectory can be sampled to obtain multiple target point cloud trajectory points, and the multiple target point cloud trajectory points can be roughly associated with the reference point cloud map to obtain N target point cloud trajectory points, where N is a positive integer; the N target point cloud trajectory points are associated with the reference point cloud map using a branch and bound algorithm with medium precision to determine the initial position and initial posture of each target point cloud trajectory point in the reference point cloud map; based on the initial position and initial posture, the iterative closest point (ICP) algorithm is used to associate the point cloud acquisition trajectory with the reference point cloud map with high precision. The association method is used to extract the constraint relationship Prior_Association between the current point cloud acquisition trajectory and the reference point cloud map (historical prior data) Prior_Map. The detailed implementation method can be found in the applicant's prior published application CN118229777A. The content of the application can be partially or completely combined with the embodiments of the present application and will not be repeated here.

[0032] After obtaining the prior constraints, the first residual term can be determined based on the prior constraints and the poses of each group of point cloud acquisition trajectories at each time. The first residual term is used to represent the residual between the pose at a certain time in the point cloud acquisition trajectory and the prior constraints between the point cloud frame and the point cloud map corresponding to that time. For example, the first residual term can be expressed as: (1) Among them, e P is the first residual term, P j -1 is the prior constraint condition P j The inverse transform, T j is the pose of the point cloud collection trajectory at time j.

[0033] In this embodiment, the second residual term can also be determined based on the relative pose relationship and loop constraint between any two point cloud frames in the group. For example, the second residual term can be expressed as: (2) (3) Among them, e loop is the second residual term, For loop constraints The inverse transformation of is the loop constraint between the nth frame point cloud and the mth frame point cloud in the point cloud acquisition trajectory, is the pose relationship between the nth frame point cloud and the mth frame point cloud in the point cloud acquisition trajectory, is the mth frame point cloud T in the point cloud acquisition trajectory m The inverse transform, T n The nth frame of point cloud in the point cloud acquisition trajectory.

[0034] In this embodiment, the third residual term can also be determined based on the GNSS measurement value and the position and posture of the point cloud trajectory in each group at each time. For example, the third residual term can be expressed as: (4) Among them, e G is the third residual term, G i -1 is the GNSS measurement value G at the i-th moment i The inverse transform, T i is the position and posture of the point cloud collection trajectory at the i-th moment.

[0035] It can be seen that the first residual term, the second residual term and the third residual term all reflect the constraints within a set of point cloud acquisition trajectories.

[0036] S12. Determine the inter-group residual term between each group of point cloud acquisition trajectories based on the relative pose relationship between any two frames of point cloud frames between each group of point cloud acquisition trajectories.

[0037] The inter-group residual term reflects the constraints between different groups of point cloud acquisition trajectories. In this embodiment, taking two groups of point cloud acquisition trajectories: the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories as examples, the first point cloud frame can be determined from the first group of point cloud acquisition trajectories, and the second point cloud frame can be determined from the second group of point cloud acquisition trajectories, and then the first point cloud frame and the second point cloud frame are aligned, and the cross constraints of the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories are calculated, and finally, based on the cross constraints and relative posture relationship between the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories, the inter-group residual term between the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories is determined.

[0038] For the point cloud collection trajectory set C = {C1, C2, ..., C N}, any set of point cloud collection trajectories Ci It may include IMU, INS, historical trajectory Odometry (for example, obtained by fusing IMU and LiDAR scan frames using SLAM front-end algorithms such as Fast-Lio), prior constraints Prior_Association, and point cloud frames. For any point cloud acquisition trajectory, its INS is in the global coordinate system. Therefore, this embodiment can calculate the cross constraints of different groups of point cloud acquisition trajectories based on INS, that is, determine the first point cloud frame and the second point cloud frame based on INS data points.

[0039] Specifically, the INS data point P corresponding to the first point cloud frame m The INS data point P corresponding to the second point cloud frame in the INS data of the first set of point cloud acquisition trajectory n In a specific implementation, the INS data of the first set of point cloud collection trajectories can be i Construct a KD-Tree (KD tree), denoted as Tree i , so for the INS data point P of the second set of point cloud acquisition trajectory n , you can use Tree i Search for the INS data point P closest to it in the first set of point cloud acquisition trajectories m .

[0040] The first set of point cloud collection trajectories is recorded as C i , the second set of point cloud collection trajectory is recorded as C j , the first point cloud frame is recorded as m, and the second point cloud frame is recorded as n. The pose relationship between the first point cloud frame m and the second point cloud frame n can be calculated by the odometer, which is expressed as: (5) in, The mth frame point cloud in the first set of point cloud acquisition trajectory The inverse transformation of The nth frame point cloud in the trajectory is collected for the second point cloud.

[0041] Furthermore, the residual term between groups can be expressed as: (6) Among them, e cloop is the between-group residual term, Cross Constraint The inverse transformation of The intersection constraint between the first set of point cloud collection trajectories and the second set of point cloud collection trajectories.

[0042] When searching for the nearest INS data point based on the KD tree constructed based on the INS data of the first set of point cloud acquisition trajectories, this embodiment also provides multiple ways to speed up the search rate. On the one hand, a cross-correlation radius threshold can be preset. In this way, the Tree can be first determined before searching. i and INS data point P n Whether the distance of the nearest INS data point is greater than the preset cross-correlation radius threshold, if so, the registration of the first point cloud frame and the second point cloud frame and the step of calculating the cross constraint of the first point cloud acquisition trajectory and the second point cloud acquisition trajectory can be skipped. On the other hand, when constructing the KD tree, the INS data of the first point cloud acquisition trajectory and the second point cloud acquisition trajectory can be filtered first, such as voxel filtering with a 1m grid length, so that the KD tree can be constructed based on the INS data of the first point cloud acquisition trajectory after sparseness, and the INS data of the second point cloud acquisition trajectory corresponding to the search match is also reduced accordingly.

[0043] In this embodiment, in order to determine the corresponding point cloud frame based on the INS data point, the INS data of the first set of point cloud acquisition trajectory and the historical trajectory can be time-aligned, and the INS data of the second set of point cloud acquisition trajectory and the historical trajectory can be time-aligned. Since the historical trajectory Odometry corresponds to the point cloud frame one-to-one, by time-aligning the INS data with the historical trajectory, the one-to-one correspondence between the INS data and the point cloud frame can be guaranteed.

[0044] Exemplarily, the historical trajectory of the first set of point cloud acquisition trajectories can be a fusion trajectory of the IMU data and the LiDAR scan frame, and the historical trajectory of the second set of point cloud acquisition trajectories can also be a fusion trajectory of the IMU data and the LiDAR scan frame, but in an alternative embodiment, the historical trajectory can also be determined based on other appropriate methods. In addition, the alignment accuracy can also be selected according to the needs of the actual application scenario, and necessary data interpolation operations can be performed when the timestamps are different, which is not limited in this application.

[0045] S13. Construct an objective function based on the within-group residual term and the between-group residual term.

[0046] The objective function can be determined based on the weighted sum of the within-group residual term and the between-group residual term. For example, the objective function can be expressed as: (7) in, , , , are the co-defense difference matrices corresponding to the residual terms.

[0047] U -1 is the covariance matrix corresponding to the first residual term, Q-1 is the covariance matrix corresponding to the second residual term, is the covariance matrix corresponding to the third residual term, K -1 is the covariance matrix corresponding to the residual terms between groups. for The transposed matrix of for The transposed matrix of for The transposed matrix of for The transposed matrix of .

[0048] It can be seen that in the formula (7), the weights corresponding to the residual items within each group and the residual items between groups can be regarded as 1. In an alternative embodiment, the weights corresponding to the residual items within each group and the residual items between groups may not be the same or may not be 1, and the present application does not limit this.

[0049] Moreover, for the residual terms within a group, the construction of the objective function may involve only one or two residual terms. For example, the objective function may be expressed as follows depending on the actual application environment: (8) (9) It can be seen that in the method shown in formula (8), the intra-group residual term uses the first residual term and the second residual term, and in the method shown in formula (9), the intra-group residual term only uses the first residual term.

[0050] S14. Based on the objective function, a nonlinear factor graph is constructed and optimized to obtain an optimized point cloud trajectory.

[0051] In this embodiment, the poses corresponding to the multiple sets of point cloud acquisition trajectories at each time can be used as variable nodes, and the objective functions corresponding to each time can be used as factor nodes to construct a nonlinear factor graph. Figure 3 , where T0, T1, T2, ..., T i , T i+1 , T i+2 Collect trajectories C1, C2, ..., C for each group of point clouds N The corresponding poses at different times constitute the variable nodes in the factor graph. G1 is the GNSS measurement value at time T1, G2 is the GNSS measurement value at time T2, and T2 i+2 is the time T2 and T i+2 The relative position relationship between the moments, P i+2 T i+2 The prior constraints between the point cloud frame corresponding to the time and the reference point cloud map, the point cloud acquisition trajectory C1 at T0, T3, T i+1The point cloud frame at time and the point cloud acquisition trajectory C2 at T0, T3, T i+1 There is a cross constraint between the point cloud frames at time T1 and the point cloud frame of point cloud acquisition trajectory C1 at time T1. N There is a cross constraint between the point cloud frames at time T3, and the point cloud acquisition trajectory C2 at T i Point cloud frame and point cloud acquisition trajectory C at time N In T i+1 There are cross constraints between the corresponding point cloud frames at each moment.

[0052] That is, taking the objective function shown in formula (7) as an example, it includes four optimization items, namely, odometer and loop closure error, corresponding loop factors (point cloud acquisition trajectory C1, C2, C N The point cloud frame at time T2 and T i+2 time between point cloud frames); GNSS and odometer error, corresponding to GNSS factor (point cloud acquisition trajectory C1, C2, C N point cloud frames at time T1 and T2); prior constraints and odometer errors, corresponding to prior constraint factors (point cloud acquisition trajectories C1, C2, C N Medium T i+2 point cloud frame at the moment); cross-constraints between data and odometry errors, corresponding to cross-constraint factors.

[0053] For any set of point cloud collection trajectories, INS constraints can provide global information and suppress the error accumulation of historical trajectories to prevent odometer drift. However, the error level of INS is unstable, and there is a widespread contradiction and conflict in accuracy between the sections where prior constraints exist and the prior constraints. In order to reconcile the error balance between the two and optimize the final fusion accuracy of the sections where INS constraints and prior constraints coexist, the embodiment of the present application proposes a mutually exclusive constraint strategy, that is, only map prior constraints are used in sections where prior constraints exist, and INS constraints are used as a supplement in sections where prior constraints do not exist, forming a pattern of GNSS and prior constraint partition control.

[0054] Corresponding to the specific nonlinear factor graph construction process, the variable nodes can be divided into the first type of variable nodes and the second type of variable nodes according to the prior constraints, wherein the point cloud acquisition trajectory corresponding to the first type of variable nodes has a priori constraints with the reference point cloud map, and the point cloud acquisition trajectory corresponding to the second type of variable nodes does not have a priori constraints with the reference point cloud map. Furthermore, the factor nodes corresponding to the first type of variable nodes can be set to include only the first residual term, the second residual term, and the inter-group residual term; the factor nodes corresponding to the second type of variable nodes can be set to include only the second residual term, the third residual term, and the inter-group residual term. In this way, the consistency between the current acquisition data and the historical data can be maintained, and INS provides error control for the new acquisition area, effectively balancing the accuracy control of the two, and does not cause accuracy conflicts and accuracy loss.

[0055] The solution of the nonlinear factor graph can be implemented, for example, by calling the open source nonlinear oil library GTSAM, and this application does not impose any limitation on this.

[0056] Ginseng Figure 4 , an embodiment of the point cloud trajectory optimization device of the present application is introduced. In this embodiment, the point cloud trajectory optimization device includes an acquisition module 21, a determination module 22, a construction module 23 and an optimization module 24.

[0057] The acquisition module 21 is used to acquire multiple groups of point cloud acquisition trajectories, and determine the intra-group residual items of each group of point cloud acquisition trajectories based on the determination module 22, wherein the intra-group residual items include at least one of a first residual item, a second residual item, and a third residual item. The first residual item is a residual item of each point cloud frame in the group determined based on a priori constraints, and the priori constraints are determined based on each group of point cloud acquisition trajectories and the corresponding reference point cloud map. The second residual item is determined based on the relative posture relationship between any two point cloud frames in the group. The third residual term is the residual term of each point cloud frame in the group determined based on the global satellite navigation system GNSS measurement value; the determination module 22 is also used to determine the inter-group residual term between each group of point cloud acquisition trajectories based on the relative posture relationship between any two frames of point cloud frames between each group of point cloud acquisition trajectories; the construction module 23 is used to construct an objective function based on the intra-group residual term and the inter-group residual term; the optimization module 24 is used to construct a nonlinear factor graph based on the objective function and optimize the solution to obtain an optimized point cloud trajectory.

[0058] In one embodiment, the determination module 22 is specifically used to determine a first point cloud frame from the first set of point cloud acquisition trajectories, and to determine a second point cloud frame from the second set of point cloud acquisition trajectories, wherein the INS data point P corresponding to the first point cloud frame is m The INS data point P corresponding to the second point cloud frame in the INS data of the first set of point cloud acquisition trajectory nThe nearest point; based on the first point cloud frame and the second point cloud frame, perform registration, calculate the cross constraints of the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories; based on the cross constraints and relative posture relationship between the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories, determine the inter-group residual term between the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories.

[0059] In one embodiment, the determination module 22 is specifically used to time-align the INS data of the first set of point cloud acquisition trajectories with the historical trajectories, and time-align the INS data of the second set of point cloud acquisition trajectories with the historical trajectories; construct a KD tree based on the INS data of the first set of point cloud acquisition trajectories, and determine the INS data point P m and INS data point P n whether the distance is greater than a preset cross-correlation radius threshold; if so, skip the steps of aligning the first point cloud frame with the second point cloud frame and calculating the cross constraints of the first set of point cloud acquisition trajectories and the second set of point cloud acquisition trajectories.

[0060] In one embodiment, the historical trajectory of the first set of point cloud acquisition trajectories is a fusion trajectory of the IMU data and the LiDAR scanning frame, and the historical trajectory of the second set of point cloud acquisition trajectories is a fusion trajectory of the IMU data and the LiDAR scanning frame.

[0061] In one embodiment, the acquisition module 21 is specifically used to determine the first residual term based on the prior constraint condition and the position and posture of each group of point cloud acquisition trajectories at each moment.

[0062] In one embodiment, the acquisition module 21 is specifically used to determine the second residual term based on the relative pose relationship and loop constraints between any two point cloud frames in the group.

[0063] In one embodiment, the acquisition module 21 is specifically used to determine the third residual term based on the GNSS measurement value and the position and posture of the point cloud trajectory in each group at each time.

[0064] In one embodiment, the construction module 23 is specifically used to construct the nonlinear factor graph using the postures corresponding to the multiple groups of point cloud acquisition trajectories at each moment as variable nodes and using the objective function corresponding to each moment as factor nodes.

[0065] In one embodiment, the optimization module 24 is specifically used to divide the variable nodes into first-category variable nodes and second-category variable nodes according to the prior constraints, wherein there are prior constraints between the point cloud acquisition trajectory corresponding to the first-category variable nodes and the reference point cloud map, and there are no prior constraints between the point cloud acquisition trajectory corresponding to the second-category variable nodes and the reference point cloud map; the factor nodes corresponding to the first-category variable nodes are set to include only the first residual term, the second residual term and the inter-group residual term; the factor nodes corresponding to the second-category variable nodes are set to include only the second residual term, the third residual term and the inter-group residual term.

[0066] As above Figures 1 to 3 , a point cloud trajectory optimization method according to an embodiment of this specification is described. The details mentioned in the above description of the method embodiment are also applicable to the point cloud trajectory optimization device of the embodiment of this specification. The above point cloud trajectory optimization device can be implemented by hardware, or by software, or a combination of hardware and software.

[0067] Figure 5 FIG. 1 shows a hardware structure diagram of an unmanned vehicle according to an embodiment of the present specification. Figure 5 As shown, the unmanned vehicle 30 may include at least one processor 31, a storage 32 (e.g., a non-volatile memory), a memory 33, and a communication interface 34, and the at least one processor 31, the storage 32, the memory 33, and the communication interface 34 are connected together via an internal bus 35. At least one processor 31 executes at least one computer-readable instruction stored or encoded in the memory 32.

[0068] It should be understood that the computer executable instructions stored in the memory 32, when executed, cause at least one processor 31 to perform the above combined operations in various embodiments of the present specification. Figures 1 to 3 Describes the various operations and functions.

[0069] In the embodiments of the present specification, the unmanned vehicle 30 can be configured with a functional terminal to carry the above-mentioned hardware structure, and the terminal may include but is not limited to: a personal computer, a server computer, a workstation, a desktop computer, a laptop computer, a notebook computer, a mobile electronic device, a smart phone, a tablet computer, a cellular phone, a personal digital assistant (PDA), a handheld device, a messaging device, a wearable electronic device, a consumer electronic device, and the like.

[0070] According to one embodiment, a program product such as a machine-readable medium is provided. The machine-readable medium may have instructions (i.e., the above-mentioned elements implemented in the form of software), which, when executed by a machine, causes the machine to perform the above-mentioned combination of various embodiments of this specification. Figure 1-Figure 3Specifically, a system or device equipped with a readable storage medium may be provided, on which a software program code implementing the functions of any of the above-mentioned embodiments is stored, and a computer or processor of the system or device reads and executes the instructions stored in the readable storage medium.

[0071] In this case, the program code itself read from the machine-readable medium can realize the function of any one of the above embodiments, and thus the machine-readable code and the machine-readable storage medium storing the machine-readable code constitute part of this specification.

[0072] Examples of readable storage media include floppy disks, hard disks, magneto-optical disks, optical disks (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD-RW), magnetic tapes, non-volatile memory cards, and ROMs. Optionally, the program code may be downloaded from a server computer or a cloud via a communication network.

[0073] Those skilled in the art should understand that the various embodiments disclosed above can be modified and altered in various ways without departing from the essence of the invention. Therefore, the protection scope of this specification should be defined by the appended claims.

[0074] It should be noted that not all steps and units in the above-mentioned processes and system structure diagrams are necessary, and some steps or units can be ignored according to actual needs. The execution order of each step is not fixed and can be determined as needed. The device structure described in the above-mentioned embodiments can be a physical structure or a logical structure, that is, some units may be implemented by the same physical client, or some units may be implemented by multiple physical clients, or some components in multiple independent devices may be implemented together.

[0075] In the above embodiments, the hardware unit or module can be implemented mechanically or electrically. For example, a hardware unit, module or processor can include permanent dedicated circuits or logic (such as special processors, FPGA or ASIC) to complete the corresponding operation. The hardware unit or processor can also include programmable logic or circuits (such as general-purpose processors or other programmable processors), which can be temporarily set by software to complete the corresponding operation. The specific implementation method (mechanical method, or dedicated permanent circuit, or temporarily set circuit) can be determined based on cost and time considerations.

[0076] The specific embodiments described above in conjunction with the accompanying drawings describe exemplary embodiments, but do not represent all embodiments that can be implemented or fall within the scope of protection of the claims. The term "exemplary" used throughout this specification means "used as an example, instance or illustration" and does not mean "preferred" or "having advantages" over other embodiments. For the purpose of providing an understanding of the described technology, the specific embodiments include specific details. However, these technologies can be implemented without these specific details. In some instances, in order to avoid making the concepts of the described embodiments difficult to understand, well-known structures and devices are shown in block diagram form.

[0077] The above description of the present disclosure is provided to enable any person of ordinary skill in the art to implement or use the present disclosure. Various modifications to the present disclosure will be apparent to those of ordinary skill in the art, and the general principles corresponding to the present disclosure may be applied to other variations without departing from the scope of protection of the present disclosure. Therefore, the present disclosure is not limited to the examples and designs described herein, but is consistent with the widest range of principles and novel features disclosed herein.

Claims

1. A point cloud trajectory optimization method, characterized in that: The method comprises: Acquire multiple groups of point cloud acquisition trajectories, and respectively determine the intra-group residual items of each group of point cloud acquisition trajectories, wherein the intra-group residual items include at least one of a first residual item, a second residual item, and a third residual item, the first residual item is a residual item of each point cloud frame in the group determined based on a priori constraints, the priori constraints are determined based on each group of point cloud acquisition trajectories and a corresponding reference point cloud map, the second residual item is a residual item determined based on a relative posture relationship between any two point cloud frames in the group, and the third residual item is a residual item of each point cloud frame in the group determined based on a global satellite navigation system GNSS measurement value; Based on the relative pose relationship between any two frames of point cloud frames between each group of point cloud acquisition trajectories, the inter-group residual term between each group of point cloud acquisition trajectories is determined; Constructing an objective function based on the within-group residual term and the between-group residual term; Based on the objective function, a nonlinear factor graph is constructed and optimized to obtain an optimized point cloud trajectory.

2. The point cloud trajectory optimization method according to claim 1, characterized in that: Based on the relative pose relationship between any two frames of point cloud frames between each group of point cloud acquisition trajectories, the inter-group residual terms between each group of point cloud acquisition trajectories are determined, specifically including: A first point cloud frame is determined from the first set of point cloud acquisition trajectories, and a second point cloud frame is determined from the second set of point cloud acquisition trajectories, wherein the INS data point P corresponding to the first point cloud frame m The INS data point P corresponding to the second point cloud frame in the INS data of the first set of point cloud acquisition trajectory n nearest point; Based on the registration of the first point cloud frame and the second point cloud frame, a cross constraint of the first set of point cloud acquisition trajectories and the second set of point cloud acquisition trajectories is calculated; Based on the cross constraints and the relative pose relationship between the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories, an inter-group residual term between the first group of point cloud acquisition trajectories and the second group of point cloud acquisition trajectories is determined.

3. The point cloud trajectory optimization method according to claim 2, characterized in that: The method further comprises: Time-aligning the INS data of the first set of point cloud acquisition trajectories with the historical trajectories, and time-aligning the INS data of the second set of point cloud acquisition trajectories with the historical trajectories; Construct a KD tree based on the INS data of the first set of point cloud acquisition trajectories and determine the INS data point P m and INS data point P n Is the distance greater than the preset cross-correlation radius threshold? If so, The steps of aligning the first point cloud frame with the second point cloud frame and calculating the cross constraints of the first set of point cloud acquisition trajectories and the second set of point cloud acquisition trajectories are skipped.

4. The point cloud trajectory optimization method according to claim 3, characterized in that: The historical trajectory of the first set of point cloud acquisition trajectories is a fusion trajectory of the IMU data and the LiDAR scanning frame, and the historical trajectory of the second set of point cloud acquisition trajectories is a fusion trajectory of the IMU data and the LiDAR scanning frame.

5. The point cloud trajectory optimization method according to claim 1, characterized in that: Obtain multiple groups of point cloud acquisition trajectories, and determine the intra-group residual items of each group of point cloud acquisition trajectories, including: Determining the first residual term based on the prior constraints and the positions of each group of point cloud acquisition trajectories at each moment; and / or, Determining the second residual term based on the relative pose relationship and loop closure constraint between any two point cloud frames in the group; and / or, The third residual term is determined based on the GNSS measurement values ​​and the poses of the point cloud trajectories in each group at each time.

6. The point cloud trajectory optimization method according to claim 1, characterized in that: Based on the objective function, a nonlinear factor graph is constructed, specifically including: The nonlinear factor graph is constructed by taking the postures corresponding to the multiple groups of point cloud acquisition trajectories at each moment as variable nodes and taking the objective function corresponding to each moment as factor nodes.

7. The point cloud trajectory optimization method according to claim 6, characterized in that: The objective function corresponding to each moment is taken as the factor node, which specifically includes: According to the prior constraint condition, the variable nodes are divided into a first type of variable nodes and a second type of variable nodes, wherein the point cloud acquisition trajectory corresponding to the first type of variable nodes has a prior constraint condition with the reference point cloud map, and the point cloud acquisition trajectory corresponding to the second type of variable nodes does not have a prior constraint condition with the reference point cloud map; Setting the factor node corresponding to the first type of variable node to include only the first residual term, the second residual term and the between-group residual term; The factor node corresponding to the second type variable node is set to include only the second residual term, the third residual term and the between-group residual term.

8. A point cloud trajectory optimization device, characterized in that: include: An acquisition module is used to acquire multiple groups of point cloud acquisition trajectories, and determine the intra-group residual items of each group of point cloud acquisition trajectories based on the determination module, wherein the intra-group residual items include at least one of a first residual item, a second residual item, and a third residual item, the first residual item is a residual item of each point cloud frame in the group determined based on a priori constraints, the priori constraints are determined based on each group of point cloud acquisition trajectories and a corresponding reference point cloud map, the second residual item is a residual item determined based on a relative posture relationship between any two point cloud frames in the group, and the third residual item is a residual item of each point cloud frame in the group determined based on a global satellite navigation system GNSS measurement value; The determination module is further used to determine the inter-group residual term between each group of point cloud acquisition trajectories based on the relative posture relationship between any two frames of point cloud frames between each group of point cloud acquisition trajectories; A construction module, used for constructing an objective function based on the intra-group residual term and the inter-group residual term; The optimization module is used to construct a nonlinear factor graph and optimize the solution based on the objective function to obtain an optimized point cloud trajectory.

9. An unmanned vehicle, characterized in that: include: at least one processor; as well as A memory storing instructions, which, when executed by the at least one processor, enables the at least one processor to execute the point cloud trajectory optimization method according to any one of claims 1 to 7.

10. A machine-readable storage medium storing executable instructions, characterized in that: When the instructions are executed, the machine performs the point cloud trajectory optimization method according to any one of claims 1 to 7.

Images