Method and device for acquiring initial pose of unmanned vehicle, electronic equipment and storage medium

Abstract

The application provides an unmanned vehicle initial pose acquisition method and device, electronic equipment and storage medium. The method is applied to an unmanned vehicle, unmanned driving equipment or automatic driving equipment, comprising: acquiring a single point positioning result at an initial position, sampling a plurality of initial poses according to a fixed step length with the single point positioning result as the center; acquiring a point cloud scanned at the initial position, initializing matching of the point cloud scanned at the initial position and a point cloud map according to each initial pose to obtain a matching result corresponding to each initial pose; taking the initial pose after the highest matching score as a new initial pose, and performing iterative matching using the new initial pose; and taking the initial pose after the iterative matching when the change value between the initial pose before and after the iterative matching is less than a threshold value as the initial pose of the unmanned vehicle. The application can quickly acquire a correct and high-precision initial pose, and improve the success rate of positioning initialization of the unmanned vehicle.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, and in particular to a method, device, electronic device and storage medium for obtaining the initial pose of an autonomous vehicle. Background Technology

[0002] Autonomous vehicles are integrated systems that combine environmental perception, planning and decision-making, and multi-level assisted driving functions. They are also known as self-driving vehicles or driverless cars. In the fields of autonomous driving and mobile robotics, accurate positioning of vehicles or robots is one of the fundamental conditions for the system to function properly, and LiDAR positioning based on point cloud maps is one of the main positioning methods.

[0003] In existing technologies, when using LiDAR for autonomous vehicle localization, a relatively good initial pose of the vehicle needs to be provided from the outside. This means acquiring a high-precision initial pose of the autonomous vehicle is necessary for initial matching with the map. Currently, there are two main methods for acquiring the initial pose of the autonomous vehicle: the first method uses a high-precision pose provided by GNSS-RTK as the initial value, and the second method uses scan-context to provide the initial pose. However, the first method relies on a GNSS base station and communication between the base station and the vehicle, and also requires high-quality satellite signals; otherwise, the GNSS receiver cannot enter RTK mode, causing the LiDAR localization module to fail to initialize successfully. The second method significantly consumes storage resources when the map is large, and when there are many similar environmental features on the map, this method may find incorrect matching frames, resulting in an incorrect initial pose and ultimately causing vehicle localization initialization failure.

[0004] It is evident that current methods for obtaining the initial pose of autonomous vehicles rely on network communication and satellite signals, consume significant storage resources, and are prone to obtaining incorrect initial poses, leading to vehicle positioning initialization failures. Summary of the Invention

[0005] In view of this, embodiments of this application provide a method, apparatus, electronic device and storage medium for obtaining the initial pose of an unmanned vehicle, in order to solve the problems of existing technologies that rely on network communication and satellite signals, occupy a large amount of storage resources, and are prone to obtaining incorrect initial poses, leading to vehicle positioning initialization failure.

[0006] A first aspect of this application provides a method for obtaining the initial pose of an unmanned vehicle, comprising: obtaining a single-point localization result corresponding to the initial position of the unmanned vehicle; sampling in a pre-established point cloud map with the single-point localization result as the center at a fixed step size to obtain multiple initial poses; obtaining a point cloud scanned by the unmanned vehicle using a lidar at the initial position; performing initial matching between the point cloud scanned at the initial position and the point cloud map according to each initial pose to obtain the initial matched pose and matching score corresponding to each initial pose; taking the initial matched pose corresponding to the highest matching score as the new initial pose; performing iterative matching between the point cloud scanned at the initial position and the point cloud map according to the new initial pose until the change value between the new initial pose and the matched pose is less than a threshold; and taking the iteratively matched pose corresponding to the change value being less than the threshold as the initial pose of the unmanned vehicle.

[0007] A second aspect of this application provides an apparatus for acquiring the initial pose of an unmanned vehicle, comprising: a pose sampling module configured to acquire a single-point localization result corresponding to the initial position of the unmanned vehicle, and to sample in a pre-established point cloud map with the single-point localization result as the center at a fixed step size to obtain multiple initial poses; an initialization matching module configured to acquire a point cloud scanned by the unmanned vehicle using a lidar at the initial position, and to perform initialization matching between the point cloud scanned at the initial position and the point cloud map according to each initial pose to obtain the initialized matched pose and matching score corresponding to each initial pose; and an iterative matching module configured to take the initialized matched pose corresponding to the highest matching score as the new initial pose, and to perform iterative matching between the point cloud scanned at the initial position and the point cloud map according to the new initial pose until the change value between the new initial pose and the matched pose is less than a threshold, and to take the iteratively matched pose corresponding to the change value being less than the threshold as the initial pose of the unmanned vehicle.

[0008] A third aspect of this application provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the above-described method.

[0009] A fourth aspect of this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described method.

[0010] The above-described technical solutions adopted in the embodiments of this application can achieve the following beneficial effects: By acquiring the single-point localization result of the unmanned vehicle at its initial position, sampling is performed in a pre-established point cloud map with a fixed step size, centered on the single-point localization result, to obtain multiple initial poses. The point cloud scanned by the unmanned vehicle at its initial position using LiDAR is then acquired. Based on each initial pose, the scanned point cloud at the initial position is initialized and matched with the point cloud map to obtain the initialized and matched pose and matching score for each initial pose. The initialized and matched pose corresponding to the highest matching score is taken as the new initial pose. Based on the new initial pose, the scanned point cloud at the initial position is iteratively matched with the point cloud map until the change value between the new initial pose and the matched pose is less than a threshold. The iteratively matched pose corresponding to the change value less than the threshold is taken as the initial pose of the unmanned vehicle. This application can accurately and quickly acquire the initial pose of the unmanned vehicle, reduce dependence on network communication and satellite signals, obtain high-precision initial poses of the unmanned vehicle, and avoid the problem of vehicle localization initialization failure when using initial poses. Attached Figure Description

[0011] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0012] Figure 1 This is a flowchart illustrating the method for obtaining the initial pose of an unmanned vehicle provided in an embodiment of this application; Figure 2 This is a schematic diagram illustrating the implementation effect of the unmanned vehicle initial pose acquisition algorithm provided in the embodiments of this application; Figure 3 This is a schematic diagram of the structure of the device for acquiring the initial pose of an unmanned vehicle provided in an embodiment of this application; Figure 4 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

[0013] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0014] As disclosed in the background technology, autonomous vehicles, also known as self-driving vehicles, unmanned vehicles, or wheeled mobile robots, are integrated and intelligent new-era technological products that combine environmental perception, path planning, state recognition, and vehicle control. In the fields of autonomous driving and mobile robots, accurate vehicle or robot localization is one of the fundamental conditions for the system to function properly, and LiDAR localization based on point cloud maps is one of the main localization methods. The LiDAR localization method obtains the current vehicle's pose by matching the laser point cloud obtained from real-time vehicle scanning with a pre-established point cloud map. When initializing the vehicle's pose using the LiDAR localization method (i.e., the first localization calculation), a good initial value of the vehicle's pose (the initial pose of the vehicle) needs to be provided externally to facilitate the first matching with the map.

[0015] Currently, the initial pose of autonomous vehicles is mainly obtained in two ways: the first method uses the high-precision pose provided by GNSS-RTK as the initial value, and the second method uses scan-context to provide the initial pose. The following detailed explanation of these two methods for obtaining the initial pose of autonomous vehicles, using specific embodiments, includes the following: The first method relies on GNSS base stations and communication between the base stations and the vehicle. It also requires high-quality satellite signals; otherwise, the GNSS receiver cannot enter RTK mode, which will cause the lidar positioning module to fail to initialize in many cases.

[0016] In the second approach, certain features of the laser point cloud obtained from the current scan are encoded, and then scan frames with similar encoded values ​​are searched in the scan frames used during mapping. The pose of the searched mapping frame is used as the initial value for initialization. This method requires that the encoded data of the scan frames used during mapping be stored on the vehicle side, which will significantly consume storage resources when the map is large. In addition, when the map is very large or there are many similar environmental features in the map, this method will find incorrect matching frames, obtain incorrect initial poses, and thus cause the vehicle localization initialization to fail.

[0017] This application embodiment is an improvement on the first method described above. When using lidar to initialize the positioning of unmanned vehicles, the vehicle does not need to rely on the GNSS-RTK state (1~2 cm positioning error). Even if the GNSS positioning is in a single-point state (5~10 meter error), it can still successfully initialize NDT positioning, thereby reducing the dependence of NDT positioning initialization on the satellite signal reception environment and network communication.

[0018] It should be noted that the embodiments of this application use NDT positioning technology to initialize the positioning of the unmanned vehicle. However, it should be understood that the core inventive point of this application does not lie in the NDT positioning technology itself. The embodiments of this application use the NDT positioning method as an example to describe the method of obtaining the initial pose of the unmanned vehicle. However, this application is not limited to using the NDT positioning method. Any positioning technology that can match the laser point cloud obtained by real-time scanning with the pre-established point cloud map to obtain the current vehicle pose is applicable to this application.

[0019] In view of this, this application provides a method for obtaining the initial pose of an unmanned vehicle. Using the single-point localization result of the unmanned vehicle at its initial position as the center, sampling is performed on a point cloud map with a fixed step size to obtain several initial poses. For each initial pose, the point cloud scanned by the unmanned vehicle at its initial position using LiDAR is initialized and matched with the point cloud map to obtain an initial matching result. The result with the highest matching score is selected from the initial matching results, and the initial matched pose corresponding to the result with the highest matching score is used as the new initial pose. Further iterative matching is performed until the change in the unmanned vehicle's pose before and after iterative matching is less than a threshold. The iteratively matched pose corresponding to the point where the change is less than the threshold is used as the initial pose for initializing the unmanned vehicle's localization. This technical solution reduces the dependence on network communication and satellite signals, thereby obtaining a higher-precision initial pose for the unmanned vehicle and avoiding the problem of failure in vehicle localization initialization using the initial pose.

[0020] The technical solution of this application will be described in detail below with reference to specific embodiments. Figure 1 This is a flowchart illustrating the method for obtaining the initial pose of an unmanned vehicle provided in an embodiment of this application. Figure 1 The initial pose of an autonomous vehicle can be obtained by electronic devices or servers within the autonomous driving system. For example... Figure 1 As shown, the method for obtaining the initial pose of the autonomous vehicle may specifically include: S101, Obtain the single-point localization result of the unmanned vehicle at the initial position, and sample the pre-established point cloud map with the single-point localization result as the center according to a fixed step size to obtain multiple initial poses; S102, acquire the point cloud scanned by the autonomous vehicle using LiDAR at the initial position, and perform initial matching between the point cloud scanned at the initial position and the point cloud map according to each initial pose to obtain the initial matched pose and matching score corresponding to each initial pose. S103, take the initial pose after matching with the highest matching score as the new initial pose, and perform iterative matching between the point cloud scanned at the initial position and the point cloud map according to the new initial pose until the change value between the new initial pose and the matched pose is less than the threshold. Then, take the iteratively matched pose corresponding to the change value being less than the threshold as the initial pose of the autonomous vehicle.

[0021] Specifically, the initial position of the autonomous vehicle can be considered as its starting position when powered on. This application's embodiment proposes a technical solution for obtaining the initial pose of the autonomous vehicle using the NDT positioning method for pose initialization. Normal Distribution Transformation (NDT) is a technique that utilizes existing high-precision point cloud maps and real-time lidar measurement data to achieve high-precision positioning.

[0022] Furthermore, point cloud can also be called point cloud data. Point cloud data can be considered as a set of vectors in a three-dimensional coordinate system. In autonomous driving technology, LiDAR installed on unmanned vehicles is used to scan the road environment in real time and collect the corresponding point cloud data. Point cloud data is recorded in the form of points, each point containing three-dimensional coordinates, and may also contain color information or reflection intensity information. The point cloud in this application embodiment refers to the point cloud data obtained by scanning with LiDAR installed on the unmanned vehicle after it is powered on.

[0023] In some embodiments, the single-point localization result corresponding to the initial position of the unmanned vehicle is obtained, and sampling is performed in a pre-established point cloud map with a fixed step size centered on the single-point localization result to obtain multiple initial poses. This includes: after the unmanned vehicle is powered on, the coordinates of the initial position of the unmanned vehicle in the world coordinate system are obtained using satellite navigation technology, and sampling is performed uniformly with the coordinates of the initial position of the unmanned vehicle in the world coordinate system centered on the preset fixed step size to obtain multiple initial poses, and multiple initial poses are used to form an initial pose set.

[0024] Specifically, the single-point positioning result of this application embodiment is the coordinate of the initial position of the unmanned vehicle in the world coordinate system obtained by satellite navigation technology. In practical applications, satellite navigation technology can adopt GNSS single-point positioning technology. GNSS single-point positioning refers to using the precise satellite orbit and satellite clock bias calculated from the GNSS observation data of several ground tracking stations around the world to perform positioning calculation on the phase and pseudorange observation values ​​collected by a single GNSS receiver. The precise ephemeris of such predicted GNSS satellites or the precise ephemeris after the fact is used as the known coordinate starting data. In other words, GNSS single-point positioning is a method of determining the absolute position of the point to be determined in the Earth-fixed coordinate system using satellite ephemeris and a single receiver. Its advantages are that a single receiver can be used for positioning, observation organization and implementation are convenient, and data processing is simple.

[0025] Furthermore, when using satellite navigation technology to obtain the position of an unmanned vehicle in the world coordinate system, since GNSS single-point positioning can only obtain an initial position with low accuracy, GNSS-RTK can be used to obtain a higher-precision initial position to improve the accuracy of the obtained initial position (i.e., single-point positioning result). In practical applications, RTK is a technique for real-time dynamic relative positioning using GPS carrier phase observations, while GNSS-RTK is a measurement method capable of obtaining centimeter-level positioning accuracy in real time in the field, employing a carrier phase dynamic real-time differential method.

[0026] Furthermore, although GNSS-RTK can obtain the coordinates of the initial position in the world coordinate system with higher accuracy than GNSS, the accuracy of the single-point positioning result of the unmanned vehicle obtained by GNSS-RTK is still low and the error is large, which cannot meet the accuracy requirements of the initial pose during initialization. Therefore, in order to obtain a high-precision initial pose to complete the first matching of initialization, this embodiment of the application needs to perform further sampling and initialization matching based on the single-point positioning result to obtain a higher-precision initial pose.

[0027] In some embodiments, uniform sampling is performed according to a preset fixed step size to obtain multiple initial poses, including: searching for sampling points in the point cloud map along the three directions corresponding to the center in the world coordinate system according to a fixed step size to obtain multiple sampling points, each sampling point corresponding to an initial pose; wherein, the point cloud map includes an NDT map, the fixed step size is smaller than the resolution of the grid in the NDT map, and the initial pose after uniform sampling can cover the error range of the single-point localization result.

[0028] Specifically, when sampling based on the positioning result in GNSS single-point state, the position is offset and searched in the point cloud map according to a fixed step size. When offsetting in the point cloud map, uniform sampling is performed in six dimensions (x, y, z, yaw, ptich, roll) to generate a set of candidate initial poses (i.e., initial pose set). Here, (x, y, z) represents the coordinates of the initial position of the unmanned vehicle in the point cloud map, yaw represents the equilibrium state, ptich represents the pitch state, and roll represents the roll state.

[0029] In practical applications, to save computing power, when performing uniform sampling in point cloud maps, sampling can be performed only on a few dimensions with poor initial accuracy among the six dimensions (x, y, z, yaw, pitch, roll). For example, sampling can be performed along the three axes corresponding to (x, y, z) to obtain several sampling points, each corresponding to an initial pose. For example, taking an autonomous vehicle as an example, during initialization, it can be assumed that the vehicle is level, that is, the angles corresponding to roll and pitch are both 0. With a dual-antenna GNSS receiver installed, the yaw angle that meets the accuracy requirements can be obtained. Therefore, during initialization, this embodiment can perform uniform sampling only on the three dimensions (x, y, z) to generate a set of candidate poses (i.e., initial poses).

[0030] Furthermore, the NDT map in this embodiment can be considered a pre-built point cloud map. Unlike a general point cloud map, the original point cloud map is divided into several grids in the NDT map. In practical applications, the fixed step size is smaller than the resolution of the grids in the NDT map, and the sampling points obtained by sampling with a fixed step size should cover the error range of the single-point localization result. For example, if the error range of the single-point localization result is 5 meters, then when using fixed step size sampling to obtain the initial pose, the initial pose should cover the 5-meter error range.

[0031] In some embodiments, based on each initial pose, the point cloud scanned at the initial position is initialized and matched with the point cloud map to obtain the initialized and matched pose and matching score corresponding to each initial pose. This includes: based on each initial pose of the autonomous vehicle, the point cloud scanned by the autonomous vehicle at the initial position is transformed into the coordinate system corresponding to the point cloud map through each initial pose, and the point cloud matching algorithm is used to initialize and match the transformed point cloud scanned at the initial position with the point cloud in the point cloud map to obtain the matching result corresponding to each initial pose of the autonomous vehicle; wherein, the matching result includes the initialized and matched pose corresponding to the initial pose and the matching score corresponding to the matching result.

[0032] Specifically, after uniformly sampling a set of initial poses centered on the single-point localization result of the autonomous vehicle, NDT initialization matching needs to be performed for each initial pose. In performing NDT initialization matching, the point cloud scanned by the autonomous vehicle at the initial position is first transformed into the coordinate system corresponding to the point cloud map according to each initial pose. That is to say, the initialization matching of the point cloud is performed in the point cloud map. Therefore, it is necessary to put the point cloud scanned by the autonomous vehicle at the initial position into the coordinate system corresponding to the point cloud map based on the position and attitude of the autonomous vehicle at each initial pose. In other words, the point cloud scanned by the autonomous vehicle at the initial position is transformed into the world coordinate system corresponding to the point cloud map.

[0033] Furthermore, after transforming the point cloud scanned by the autonomous vehicle at its initial position to the world coordinate system corresponding to the point cloud map using each initial pose, a point cloud matching algorithm is used to perform initial matching between the transformed point cloud scanned at the initial position and the point cloud in the point cloud map. This initial matching process can be considered a convergence process of the point cloud scanned at the initial position, bringing the objects in the point cloud scanned at the initial position closer to the positions of the objects in the actual point cloud map. However, due to the insufficient precision of the initial pose, there is still an error between the positions of the objects in the point cloud scanned at the initial position and the positions of the objects in the actual point cloud map. Therefore, after initial matching using the initial pose, the result with the highest matching score is selected as the initial value for further iterative matching.

[0034] It should be noted that since the position and orientation corresponding to each initial pose obtained after uniform sampling may be different, the position of the point cloud scanned at the initial position in the point cloud map will also be different when transforming the point cloud scanned at the initial position to the world coordinate system corresponding to the point cloud map using the initial pose. In practical applications, to prevent the initialization matching from failing to converge and entering an infinite loop, the embodiments of this application set a maximum number of iterations for the initialization matching process. If efficiency is to be improved, the initialization matching operations under different initial poses can also be executed in parallel by multiple threads.

[0035] In some embodiments, based on the new initial pose, the point cloud scanned at the initial position is iteratively matched with the point cloud map, including: transforming the point cloud scanned by the autonomous vehicle at the initial position to the coordinate system corresponding to the point cloud map through the new initial pose, and using a point cloud matching algorithm to iteratively match the transformed point cloud scanned at the initial position with the point cloud in the point cloud map to obtain the matching result corresponding to the autonomous vehicle at the new initial pose.

[0036] Specifically, based on different initial poses, the point cloud scanned at the transformed initial position is initialized and matched with the point cloud in the point cloud map. After obtaining the matching result corresponding to each initial pose, the initialized and matched pose corresponding to the result with the highest matching score is selected from all the matching results. This initialized and matched pose is then used as the new initial pose (i.e., as the new initial value) for further iterative matching. The iterative matching process is similar to the initial matching process. First, based on the new initial pose, the point cloud scanned at the initial position is transformed into the coordinate system corresponding to the point cloud map. Then, the point cloud matching algorithm is used to iteratively match the point cloud scanned at the transformed initial position with the point cloud in the point cloud map in the coordinate system of the point cloud map.

[0037] Furthermore, to ensure the accuracy of the final iterative matching result, this embodiment sets the maximum allowed number of iterations to a large value until the change value of the iterative matching result is less than a preset threshold. Then, the pose of the iterative matching result corresponding to the iterative matching result when the change value is less than the threshold is used as the final iterative matching result output, and the output pose is used for NDT initialization.

[0038] In some embodiments, the initial pose of the autonomous vehicle is taken as the iteratively matched pose when the change value between the new initial pose and the matched pose is less than a threshold. This includes: comparing the new initial pose before iterative matching with the pose obtained after iterative matching to obtain the change value of each iterative matching; continuing iterative matching when the change value is greater than a threshold; stopping iterative matching when the change value is less than a threshold; and taking the iteratively matched pose when the change value is less than a threshold as the final determined initial pose of the autonomous vehicle.

[0039] Specifically, in order to obtain a correct and more accurate initial pose of the autonomous vehicle, the poses before and after the iteration matching are compared during multiple rounds of iterative matching. The change in the pose in the point cloud map before and after each iteration matching is determined (i.e., the change value). This process continues until the change in the pose before and after the iteration matching is less than a threshold. The pose after the iteration matching that is less than the threshold is then used as the initial pose finally determined in real time by this application.

[0040] In some embodiments, after taking the pose after iterative matching corresponding to the change value being less than a threshold as the final determined initial pose of the unmanned vehicle, the method further includes: based on the initial pose of the unmanned vehicle, using the NDT positioning method to perform localization calculation on the pose of the unmanned vehicle during driving, so as to obtain the real-time pose of the unmanned vehicle during driving, and to track the motion posture of the unmanned vehicle.

[0041] Specifically, after obtaining the initial pose of the autonomous vehicle at its initial position, a localization initialization operation is performed based on the obtained initial pose. For example, the NDT localization method is used to perform the first localization calculation so as to achieve the first matching of the point cloud data measured by the lidar with the NDT map. The result of the first localization calculation can also be used for subsequent localization calculations during the autonomous vehicle's driving process, so as to obtain the real-time pose during the driving process and track the motion posture of the autonomous vehicle.

[0042] It should be noted that the positioning calculation using the NDT positioning method based on the final initial pose obtained in the embodiments of this application is merely one application scenario of this application in practice. The purpose of this application is to obtain a correct and more accurate initial pose. As for what operations need to be performed after obtaining the initial pose, those skilled in the art can choose according to actual needs. Therefore, the positioning calculation using the NDT positioning method based on the obtained initial pose does not constitute a limitation on the application scenario of this application.

[0043] The following describes the implementation process and principle of the method for obtaining the initial pose of the unmanned vehicle according to the embodiments of this application from the perspective of algorithm implementation, specifically including the following: Step 1: Set the sampling step size: sample_step; this value should not be greater than the side length of the cube grid in the NDT map; Step 2: Set the sampling range: x_sample_size, y_sample_size, z_sample_size. This value can be set according to the error range of each dimension to ensure that the search range can cover the error range. Generally, the altitude error of GNSS is relatively large, so the value of z_sample_size is set to be large. Step 3: Obtain the single-point positioning results (X0, Y0, Z0, Yaw0) of the dual-antenna GNSS receiver. Since Yaw0 meets the initialization accuracy requirements, uniform sampling is only performed in the three dimensions (x, y, z). Step 4: Calculate alternative initialization values: For i = -x_sample_size:1:x_sample_size { For j = -y_sample_size:1:y_sample_size { For k = -z_sample_size:1:z_sample_size { candidates[i,j,k].x = X0 + i*sample_step; candidates[i,j,k].y = Y0 + j*sample_step; candidates[i,j,k].y = Z0 + k*sample_step; candidates[i,j,k].yaw = Yaw0; / / Yaw0 meets the initialization precision requirements, so no sampling is performed on this dimension. } } } Step 5: Use each value (initial pose) from the candidate initialization values ​​(i.e., all calculated initial poses) to perform initialization matching, and obtain a batch of matched results candidate_results; Step 6: Retrieve the matching result with the highest matching score from candidate_results (i.e., select the pose after initial matching with the highest matching score), and denote it as P1(X1, Y1, Z1, Yaw1). Step 7: During the matching process, since a maximum number of matches is set, P1 may not be the final converged result. Further iterative matching is needed to obtain a fully converged result. Therefore, iterative matching is performed again with P1 as the initial value (that is, the pose after initial matching is used as the new initial pose) until the change value of the iterative matching is less than the threshold. At this time, it is considered to be fully converged. The result obtained at this time is recorded as P2(X2, Y2, Z2, Yaw2), and the corresponding matching score is score2. Step 8: If the value of score2 is less than the threshold or the number of iterations in step 7 is greater than the threshold, return to step 3; otherwise, proceed to the next step. Step 9: Output P2 for NDT initialization.

[0044] The following example demonstrates the principles and effects of the algorithm in practical applications using a more specific scenario. Figure 2 This is a schematic diagram illustrating the implementation effect of the unmanned vehicle initial pose acquisition algorithm provided in an embodiment of this application. For example... Figure 2 As shown, the algorithm for obtaining the initial pose of the autonomous vehicle mainly includes the following: First of all Figure 2The meaning of each element is explained below: inspva_pose 16 represents the GNSS single-point positioning result; candidates represents a batch of candidate initialization values ​​obtained by sampling at equal intervals based on inspva_pose 16; best candidate result represents the matching result P1 with the highest matching score selected from the results after matching calculation using each candidate initialization value; best candidate represents the candidate initialization value corresponding to P1; final initpose represents P2 obtained after further iterative matching using P1; inspva_pose 50 represents the GNSS RTK positioning result, which can be regarded as the true value of positioning.

[0045] Depend on Figure 2 It can be seen that the batch of candidate initialization values ​​(initial poses after uniform sampling) generated by sampling fully covers the error range of GNSS single-point positioning. After performing initialization matching calculations on each candidate initialization value, the best candidate result with the highest matching score is selected from the initialization matching calculation results. This best candidate result is already quite close to the true value isnpva_pose_50. The final init pose obtained after further iterative matching using the best candidate result with the highest matching score is even closer to the true value isnpva_pose_50. Finally, the final init pose is used as the final determined initialization value, and the NDT initialization operation is performed using the final determined initialization value.

[0046] According to the technical solution provided in the embodiments of this application, the method for obtaining the initial pose of an unmanned vehicle (UAV) can obtain a correct and more accurate initial pose of the UAV through a series of sampling and matching operations, even when only GNSS single-point positioning results and point clouds scanned by LiDAR at the initial position of the UAV are acquired. The method for obtaining the initial pose of the UAV in this application greatly reduces the dependence on network communication and satellite signals, and can obtain a higher accuracy initial pose of the UAV, thus avoiding the problem of vehicle positioning initialization failure when using the initial pose. Based on the initial pose obtained in the embodiments of this application, NDT positioning initialization can be successfully completed, thereby making the initialization of the NDT positioning function more environmentally adaptable.

[0047] The following are embodiments of the apparatus described in this application, which can be used to execute the embodiments of the method described in this application. For details not disclosed in the apparatus embodiments of this application, please refer to the embodiments of the method described in this application.

[0048] Figure 3This is a schematic diagram of the structure of the device for acquiring the initial pose of an unmanned vehicle provided in an embodiment of this application. Figure 3 As shown, the device for acquiring the initial pose of the unmanned vehicle includes: The pose sampling module 301 is configured to obtain the single-point localization result of the unmanned vehicle at the initial position, and to sample in the pre-established point cloud map with the single-point localization result as the center according to a fixed step size to obtain multiple initial poses. The initialization matching module 302 is configured to acquire the point cloud scanned by the unmanned vehicle using LiDAR at the initial position, and perform initial matching between the point cloud scanned at the initial position and the point cloud map according to each initial pose to obtain the initial matched pose and matching score corresponding to each initial pose. The iterative matching module 303 is configured to take the initial matched pose corresponding to the highest matching score as the new initial pose, and perform iterative matching between the point cloud scanned at the initial position and the point cloud map based on the new initial pose until the change value between the new initial pose and the matched pose is less than a threshold. Then, the iterative matched pose corresponding to the change value being less than the threshold is taken as the initial pose of the autonomous vehicle.

[0049] In some embodiments, Figure 3 After the unmanned vehicle is powered on, the pose sampling module 301 uses satellite navigation technology to obtain the coordinates of the unmanned vehicle's initial position in the world coordinate system. Centered on the coordinates of the unmanned vehicle's initial position in the world coordinate system, it performs uniform sampling according to a preset fixed step size to obtain multiple initial poses. The multiple initial poses are then used to form an initial pose set.

[0050] In some embodiments, Figure 3 The pose sampling module 301 searches for sampling points in the point cloud map along the three directions corresponding to the center in the world coordinate system with a fixed step size, and obtains multiple sampling points. Each sampling point corresponds to an initial pose. The point cloud map includes an NDT map. The fixed step size is smaller than the resolution of the grid in the NDT map. The initial pose after uniform sampling can cover the error range of the single-point localization result.

[0051] In some embodiments, Figure 3 The initialization matching module 302 transforms the point cloud scanned by the unmanned vehicle at the initial position to the coordinate system corresponding to the point cloud map for each initial pose. Then, it uses a point cloud matching algorithm to perform initial matching between the transformed point cloud scanned at the initial position and the point cloud in the point cloud map to obtain the matching result corresponding to the unmanned vehicle at each initial pose. The matching result includes the initial pose after matching corresponding to the initial pose and the matching score corresponding to the matching result.

[0052] In some embodiments, Figure 3 The iterative matching module 303 transforms the point cloud scanned by the unmanned vehicle at the initial position into the coordinate system corresponding to the point cloud map through the new initial pose, and uses the point cloud matching algorithm to iteratively match the point cloud scanned at the transformed initial position with the point cloud in the point cloud map to obtain the matching result corresponding to the unmanned vehicle at the new initial pose.

[0053] In some embodiments, Figure 3 The iterative matching module 303 compares the new initial pose before iterative matching with the pose obtained after iterative matching to obtain the change value of each iterative matching. When the change value is greater than the threshold, iterative matching continues until the change value is less than the threshold. Then iterative matching stops and the pose after iterative matching corresponding to the change value being less than the threshold is taken as the final determined initial pose of the autonomous vehicle.

[0054] In some embodiments, Figure 3 After taking the pose after iterative matching when the change value is less than the threshold as the final determined initial pose of the unmanned vehicle, the localization calculation module 304 uses the NDT localization method to calculate the pose of the unmanned vehicle during the driving process based on the initial pose of the unmanned vehicle, so as to obtain the real-time pose of the unmanned vehicle during the driving process and track the motion attitude of the unmanned vehicle.

[0055] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0056] Figure 4 This is a schematic diagram of the structure of the electronic device 4 provided in an embodiment of this application. Figure 4 As shown, the electronic device 4 of this embodiment includes a processor 401, a memory 402, and a computer program 403 stored in the memory 402 and executable on the processor 401. When the processor 401 executes the computer program 403, it implements the steps in the various method embodiments described above. Alternatively, when the processor 401 executes the computer program 403, it implements the functions of each module / unit in the various device embodiments described above.

[0057] For example, computer program 403 may be divided into one or more modules / units, which are stored in memory 402 and executed by processor 401 to complete this application. The one or more modules / units may be a series of computer program instruction segments capable of performing a specific function, which describe the execution process of computer program 403 in electronic device 4.

[0058] Electronic device 4 can be a desktop computer, laptop, handheld computer, cloud server, or other electronic device. Electronic device 4 may include, but is not limited to, processor 401 and memory 402. Those skilled in the art will understand that... Figure 4 This is merely an example of electronic device 4 and does not constitute a limitation on electronic device 4. It may include more or fewer components than shown, or combine certain components, or different components. For example, electronic device may also include input / output devices, network access devices, buses, etc.

[0059] Processor 401 can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor.

[0060] The memory 402 can be an internal storage unit of the electronic device 4, such as a hard disk or RAM. The memory 402 can also be an external storage device of the electronic device 4, such as a plug-in hard disk, Smart Media Card (SMC), Secure Digital (SD) card, or Flash Card. Furthermore, the memory 402 can include both internal and external storage units of the electronic device 4. The memory 402 is used to store computer programs and other programs and data required by the electronic device. The memory 402 can also be used to temporarily store data that has been output or will be output.

[0061] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0062] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0063] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments claimed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0064] In the embodiments provided in this application, it should be understood that the disclosed apparatus / computer devices and methods can be implemented in other ways. For example, the apparatus / computer device embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. Multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces, and the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0065] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0066] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0067] If an integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program may include computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. A computer-readable medium may include: any entity or device capable of carrying computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in a computer-readable medium can be appropriately added to or subtracted according to the requirements of legislation and patent practice in a jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0068] The above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A method for obtaining the initial pose of an unmanned vehicle, characterized in that, include: After the unmanned vehicle is powered on, satellite navigation technology is used to obtain the coordinates of the unmanned vehicle's initial position in the world coordinate system. Taking the coordinates of the unmanned vehicle's initial position in the world coordinate system as the center, uniform sampling is performed according to a preset fixed step size to obtain multiple initial poses. Multiple initial poses are used to form an initial pose set. The point cloud scanned by the unmanned vehicle at the initial position using LiDAR is obtained. Based on each initial pose, the point cloud scanned at the initial position is initialized and matched with the point cloud map to obtain the initialized and matched pose and matching score corresponding to each initial pose. The initial pose corresponding to the highest matching score is taken as the new initial pose. Based on the new initial pose, the point cloud scanned at the initial position is iteratively matched with the point cloud map until the change value between the new initial pose and the matched pose is less than a threshold. The iteratively matched pose corresponding to the change value being less than the threshold is taken as the initial pose of the autonomous vehicle.

2. The method according to claim 1, characterized in that, The process of uniformly sampling according to a preset fixed step size to obtain multiple initial poses includes: Along the three directions corresponding to the center in the world coordinate system, a sampling point is searched in the point cloud map according to the fixed step size to obtain multiple sampling points, each of which corresponds to an initial pose. The point cloud map includes an NDT map, the fixed step size is smaller than the resolution of the grid in the NDT map, and the initial pose after uniform sampling can cover the error range of the single-point positioning result.

3. The method according to claim 1, characterized in that, The step involves initializing and matching the point cloud scanned at the initial position with the point cloud map based on each initial pose, to obtain the initialized and matched pose and matching score corresponding to each initial pose, including: Based on each initial pose of the unmanned vehicle, the point cloud scanned by the unmanned vehicle at the initial position is transformed into the coordinate system corresponding to the point cloud map through each initial pose. Then, the point cloud scanning at the transformed initial position is initialized and matched with the point cloud in the point cloud map using a point cloud matching algorithm to obtain the matching result corresponding to the unmanned vehicle at each initial pose. The matching result includes the initial pose after matching corresponding to the initial pose and the matching score corresponding to the matching result.

4. The method according to claim 1, characterized in that, The step of iteratively matching the point cloud scanned at the initial position with the point cloud map based on the new initial pose includes: The point cloud scanned by the unmanned vehicle at its initial position is transformed into the coordinate system corresponding to the point cloud map through the new initial pose. Then, the point cloud scanned at the transformed initial position is iteratively matched with the point cloud in the point cloud map using a point cloud matching algorithm to obtain the matching result corresponding to the unmanned vehicle at the new initial pose.

5. The method according to claim 4, characterized in that, The step of using the iteratively matched pose corresponding to the point where the change value between the new initial pose and the matched pose is less than a threshold as the initial pose of the autonomous vehicle includes: The new initial pose before iterative matching is compared with the pose obtained after iterative matching to obtain the change value of each iterative matching. When the change value is greater than a threshold, iterative matching continues until the change value is less than the threshold, at which point iterative matching stops. The iteratively matched pose corresponding to the change value being less than the threshold is taken as the final determined initial pose of the autonomous vehicle.

6. The method according to claim 5, characterized in that, After taking the pose obtained through iterative matching when the change value is less than a threshold as the final determined initial pose of the autonomous vehicle, the method further includes: Based on the initial pose of the unmanned vehicle, the NDT positioning method is used to calculate the pose of the unmanned vehicle during the driving process, so as to obtain the real-time pose of the unmanned vehicle during the driving process and track the motion posture of the unmanned vehicle.

7. A device for acquiring the initial pose of an unmanned vehicle, characterized in that, include: The pose sampling module is configured to obtain the coordinates of the initial position of the unmanned vehicle in the world coordinate system using satellite navigation technology after the unmanned vehicle is powered on. Taking the coordinates of the initial position of the unmanned vehicle in the world coordinate system as the center, it performs uniform sampling according to a preset fixed step size to obtain multiple initial poses, and uses the multiple initial poses to form an initial pose set. The initialization matching module is configured to acquire the point cloud scanned by the unmanned vehicle using LiDAR at the initial position, and perform initial matching between the point cloud scanned at the initial position and the point cloud map according to each initial pose to obtain the initial matched pose and matching score corresponding to each initial pose. The iterative matching module is configured to take the pose after initial matching corresponding to the highest matching score as the new initial pose, and perform iterative matching between the point cloud scanned at the initial position and the point cloud map according to the new initial pose, until the change value between the new initial pose and the matched pose is less than a threshold, and take the iteratively matched pose corresponding to the change value being less than the threshold as the initial pose of the unmanned vehicle.

8. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the method of any one of claims 1 to 6.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1 to 6.

Images