An unmanned vehicle pose estimation method in a communication denial environment

Abstract

The present application relates to a kind of unmanned vehicle pose estimation method in communication denial environment, belong to unmanned vehicle pose estimation field, solve the problem of greater pose estimation error of prior art, including: respectively obtaining all vehicle detection frame in each frame image;Respectively cluster and project each frame point cloud data, all target vehicles in each frame are calculated using IOU algorithm in combination with the vehicle detection frame point cloud;Respectively to the point cloud of each target vehicle in each frame is based on the self-adapting purification of divergence, obtain the pure point cloud of each target vehicle in each frame, realize the relative pose estimation of each target vehicle using ICP algorithm;The self-adapting purification based on divergence includes: the point cloud of current frame target vehicle is filtered based on divergence noise;The intersection point set of the point set after the projection of the pure point cloud of previous frame target vehicle and the current frame target point cloud is calculated;Adaptive loop, the most points of intersection point set is regarded as the pure point cloud of current frame target vehicle. Realize accurate pose estimation.

Description

Technical Field

[0001] This invention relates to the field of autonomous vehicle pose estimation technology, and in particular to an autonomous vehicle pose estimation method in a communication denied environment. Background Technology

[0002] The rapid development and advancement of vehicle-to-everything (V2X) technology in recent years has laid a solid foundation for the development of vehicle platooning technology. Research on platooning-related technologies has quietly emerged in multiple fields, such as unmanned truck platooning in the civilian logistics industry and unmanned combat vehicle platooning in the military field. Especially in the military field, unmanned platoons, with their unique advantages, play a crucial role in key tasks such as battlefield support, material transportation, and intelligence gathering behind enemy lines, effectively reducing manpower input and the risk of casualties.

[0003] However, the complexity of the driving environment, such as strong electromagnetic interference and severe weather, can easily cause communication between vehicles in a platoon to cease. In such cases, vehicles in an unmanned platoon can only perceive the position and orientation of the vehicle in front through their own sensors. This is especially important when the vehicle in front malfunctions and stops, as it needs to promptly detect the position of the vehicle in front and take emergency evasive action or stop.

[0004] To achieve stable pose estimation for vehicles ahead in a convoy under communication denial conditions, the current mainstream approach is to fuse vision and LiDAR data, and then use ICP (Integrated Photon Propagation) to estimate the pose of vehicles in adjacent frames. However, in many harsh environments, the dust and smoke encountered during convoy movement result in significant noise in the point cloud, so directly using ICP can lead to large pose estimation errors. Summary of the Invention

[0005] Based on the above analysis, the embodiments of the present invention aim to provide a pose estimation method for unmanned vehicles in a communication denied environment, so as to solve the problem of large pose estimation errors in existing methods.

[0006] This invention provides a method for estimating the pose of an unmanned vehicle in a communication-denied environment, comprising:

[0007] Target detection and tracking are performed on each frame of images captured by the camera to obtain all vehicle detection boxes in each frame of images;

[0008] The point cloud data of each frame collected by the lidar are clustered and projected onto the pixel coordinate system to obtain the 2D projection of the clustered data. The point cloud of all target vehicles in each frame is calculated by combining the vehicle detection box with the IOU algorithm.

[0009] The point clouds of each target vehicle in each frame are subjected to divergence-based adaptive purification to obtain the clean point clouds of each target vehicle in each frame, and the relative pose estimation of each target vehicle is realized by using the ICP algorithm. The divergence-based adaptive purification of the point cloud of a target vehicle in the current frame includes: performing voxel segmentation on the obtained point cloud of the target vehicle in the current frame, obtaining the average divergence of each voxel based on the center point of the point cloud in each voxel, and filtering to obtain the target point cloud of the current frame; randomly sampling the target point cloud of the current frame, calculating the pose of the target point cloud of the current frame to obtain the set of overlapping points of the clean point cloud of the target vehicle in the previous frame and the target point cloud of the current frame; iteratively calculating, and taking the set of overlapping points with the most points as the clean point cloud of the target vehicle in the current frame.

[0010] Based on the further improvement of the above method, the center point of the point cloud within each voxel is calculated using the following formula:

[0011]

[0012] Where m represents the number of point clouds within a voxel, p i Let represent the coordinates of the i-th point cloud within the voxel in the lidar coordinate system, p represent the center point of the point cloud within the voxel, and (x0, y0, z0) represent the coordinates of p in the lidar coordinate system.

[0013] Based on a further improvement of the above method, the mean divergence of each voxel is calculated using the following formula:

[0014] F pi =[(x i -x0) 2 ,(y i -y0) 2 ,(z i -z0) 2 ]

[0015]

[0016] Among them, F pi Point p i The vector function to p, F represents the divergence value of point pi. pix F represents pi The component in the x-direction, F piy F represents pi The component in the y-direction, F piz F represents pi The component in the z-direction, F avg represents the mean divergence of the voxels, and ||·|| represents the modulus value.

[0017] Based on a further improvement to the above method, the filtering to obtain the target point cloud of the current frame includes:

[0018] Voxels with a divergence mean greater than the divergence threshold are filtered out to obtain the target point cloud of the current frame.

[0019] Based on a further improvement to the above method, the step of randomly sampling the target point cloud and calculating the pose of the target point cloud in the current frame includes:

[0020] The target point cloud is randomly sampled to obtain an initial target point cloud, and the sample size is fixed for each sampling.

[0021] Based on the initial target point cloud, the attitude matrix of the corresponding target point cloud relative to the lidar is calculated using the PCA algorithm. Where l represents the lidar and c represents the target point cloud;

[0022] Based on all point cloud vectors contained in the initial target point cloud, the position vector of the target point cloud is calculated using the following formula:

[0023] r k =(x k -x s ,y k -y s ,z k -z s )

[0024]

[0025] Where, r k Let x represent the point cloud vector of the k-th point in the initial target point cloud, (x s ,y s ,z s (x) represents the origin coordinates of the lidar coordinate system. k ,y k ,z k () represents the coordinates of the k-th point in the lidar coordinate system. The vector represents the position of the target point cloud, where l represents the lidar, c represents the target point cloud, and d represents the number of points in the initial target point cloud.

[0026] The pose of the target point cloud is represented as follows:

[0027] Based on a further improvement to the above method, obtaining the set of overlapping points between the clean point cloud of the target vehicle in the previous frame and the target point cloud in the current frame includes:

[0028] Based on the pose of the clean point cloud of the target vehicle in the previous frame and the pose of the target point cloud in the current frame, the transformation matrix from the clean point cloud of the target vehicle in the previous frame to the target point cloud in the current frame is calculated.

[0029] Based on the transformation matrix, the set of overlapping points of the clean point cloud of the target vehicle in the previous frame and the target point cloud in the current frame are obtained.

[0030] Based on a further improvement of the above method, the calculation of the transformation matrix includes:

[0031] The pose of the target vehicle based on the clean point cloud of the previous frame. pose of the target point cloud in the current frame The transformation matrix from the clean point cloud of the target vehicle in the previous frame to the target point cloud in the current frame can be calculated using the following formula:

[0032]

[0033] Where c1 represents the pose of the target point cloud at time c1, and c2 represents the pose of the target point cloud at time c2.

[0034] Based on a further improvement of the above method, obtaining the set of coincidence points includes:

[0035] The pure point cloud of the target vehicle in the previous frame is projected onto the target point cloud in the current frame using the transformation matrix to obtain the projection points;

[0036] Find the matching points closest to each projection point in the target point cloud of the current frame in turn to obtain the matching point set S;

[0037] Calculate the distance between each matching point and its corresponding projection point in sequence. When the distance is less than or equal to the matching threshold, add the matching point to the coincidence point set S. inside Otherwise, add the matching point to the set of free points S. outside .

[0038] Based on a further improvement to the above method, the iterative calculation, which considers the set of overlapping points with the most points as the clean point cloud of the target vehicle in the current frame, includes:

[0039] The number of adaptive iterations for the current frame can be calculated using the following formula:

[0040]

[0041] Where k represents the number of adaptive iterations for the current frame, y represents the expected probability, and n represents the sample size of the current frame. N represents the number of coincident points. s Represents the number of matching point concentration points;

[0042] If the calculated k becomes smaller, then update k and recalculate the number of iterations; in each iteration, randomly sample the target point cloud again and complete the subsequent operations; until the k iterations are completed;

[0043] The set of overlapping points with the most points is considered as the clean point cloud of the target vehicle in the current frame.

[0044] Based on a further improvement to the above method, the clustering of each frame of point cloud data acquired by the lidar includes performing voxel filtering with a preset voxel size and Euclidean point cloud clustering on each frame of point cloud data acquired by the lidar. Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0045] 1. For situations where the point cloud data collected by lidar may include a large amount of smoke and dust, adaptive purification based on divergence is performed on the point cloud of each target vehicle to obtain accurate and clean point cloud of the target vehicle.

[0046] 2. By combining YOLO-V10 with DeepSort, the detection and tracking of all vehicle targets in each frame of the image are achieved, enabling stable target tracking across multiple frames. Even if the target disappears, reappears, intersects, or is occluded in some frames, the target detection and tracking can be achieved efficiently and accurately.

[0047] 3. Based on the current driving speed of the autonomous vehicle, voxel filtering with a preset voxel size is performed on the point cloud to avoid the point cloud data being too dense and to ensure the calculation speed.

[0048] In this invention, the above-described technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of this invention will be set forth in the following description, and some advantages may become apparent from the description or be learned by practicing the invention. The objects and other advantages of this invention can be realized and obtained from what is particularly pointed out in the description and drawings. Attached Figure Description

[0049] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.

[0050] Figure 1 This is a flowchart illustrating a method for estimating the pose of an unmanned vehicle in a communication denied environment, as shown in an embodiment of the present invention.

[0051] Figure 2 This is an input block diagram of the IOU algorithm shown in an embodiment of the present invention;

[0052] Figure 3 This is a schematic diagram illustrating the computational principle of the IOU algorithm shown in an embodiment of the present invention. Attached image description:

[0054] 1- Vehicle 2D detection bounding box; 2- Maximum intersection; 3- 2D detection bounding box of clustered data Detailed Implementation

[0055] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.

[0056] A specific embodiment of the present invention discloses a method for estimating the pose of an unmanned vehicle in a communication denied environment, such as... Figure 1 As shown, it includes:

[0057] S1: Perform target detection and tracking on each frame of images captured by the camera to obtain all vehicle detection boxes in each frame of images;

[0058] S2: Cluster the point cloud data of each frame collected by the lidar and project it onto the pixel coordinate system to obtain the 2D projection of the clustered data. Combine the vehicle detection box with the IOU algorithm to calculate the point cloud of all target vehicles in each frame.

[0059] S3: Perform divergence-based adaptive purification on the point cloud of each target vehicle in each frame to obtain the clean point cloud of each target vehicle in each frame, and use the ICP algorithm to estimate the relative pose of each target vehicle.

[0060] The divergence-based adaptive purification of the point cloud of a target vehicle in the current frame includes: performing voxel segmentation on the point cloud of the target vehicle in the current frame, obtaining the average divergence of each voxel based on the center point of the point cloud within each voxel, and filtering to obtain the target point cloud of the current frame; randomly sampling the target point cloud of the current frame, calculating the pose of the target point cloud of the current frame to obtain the set of overlapping points of the clean point cloud of the target vehicle in the previous frame and the target point cloud of the current frame; cyclically calculating, and considering the set of overlapping points with the most points as the clean point cloud of the target vehicle in the current frame.

[0061] Specifically, during the platooning of autonomous vehicles, since the vehicles behind need to continuously lock onto the vehicles in front, it is necessary to track the vehicles in front, which may be one or more. When performing target detection and tracking on each frame of images captured by the camera, this embodiment of the invention employs the currently very mature deep learning framework for target detection, YOLO-V10, and combines the detection results with DeepSort to obtain all vehicle detection boxes in each frame of images.

[0062] It should be noted that YOLO-V10 is an advanced real-time object detection algorithm, while DeepSORT is a deep learning-based object tracking algorithm capable of stably tracking objects across multiple frames, even when the object disappears or reappears in some frames, or faces complex situations such as intersections and occlusions. Combining YOLO-V10 and DeepSORT achieves efficient and accurate object detection and tracking.

[0063] Furthermore, clustering is performed on each frame of point cloud data acquired by the lidar, including voxel filtering with a preset voxel size and Euclidean point cloud clustering for each frame of point cloud data acquired by the lidar.

[0064] Specifically, when autonomous vehicles are traveling in platoons, the distance between the current autonomous vehicle and the vehicle in front is determined by the overall speed of the platoon. Specifically, when the platoon speed is high, the distance between the vehicle in front and the current autonomous vehicle is greater; when the platoon speed is low, the distance between the vehicle in front and the current autonomous vehicle is smaller. In this case, the point cloud data collected by the LiDAR is relatively dense, affecting the calculation speed of this method. This embodiment of the invention uses dynamic voxel filtering to perform voxel filtering on each frame of point cloud data. The higher the platoon speed, the smaller the preset voxel size used for voxel filtering. The preset voxel size is obtained using the following formula:

[0065]

[0066] Where Lifesize represents the preset voxel size, v r v represents the current speed of the driverless car. max This indicates the current speed limit for the driverless car;

[0067] Furthermore, Euclidean clustering is performed on the voxel-filtered point cloud data, and KD-TREE proximity query is used to obtain point cloud clusters, thereby extracting meaningful cluster data from unordered point cloud data.

[0068] Furthermore, such as Figure 2 As shown, the obtained clustering information is projected onto the pixel coordinate system to obtain the 2D projection of each cluster data. Combined with the vehicle detection box, the point cloud of all target vehicles in each frame is calculated using the IOU algorithm. The method includes: projecting the points in the LiDAR coordinate system space to the camera coordinate system space; and then using the camera's intrinsic parameter matrix, projecting from the camera coordinate system to the pixel coordinate system. The projection of a point from the LiDAR coordinate system to the pixel coordinate system is achieved using the following formula:

[0069]

[0070] Where (u, v) represents the coordinates of the point in the pixel coordinate system, Z c K represents the z-coordinate of a point in the camera coordinate system. c R represents the intrinsic parameter matrix of the camera. cl The rotation matrix t represents the rotation from the camera coordinate system to the lidar coordinate system. cl P represents the translation matrix from the camera coordinate system to the lidar coordinate system. l This represents the coordinates of a point in the lidar coordinate system.

[0071] like Figure 2 As shown, the minimum envelope rectangle of the 2D projection of the clustered data is calculated to obtain the 2D detection box of the clustered data; combined with the vehicle detection box, the point cloud of all target vehicles in each frame is calculated using the IOU algorithm. Specifically, the calculation principle of the IOU algorithm is as follows: Figure 3 As shown, the maximum intersection of each vehicle detection box and the corresponding 2D detection box of the cluster data is calculated to obtain the point cloud of each target vehicle.

[0072] It should be noted that the acquired images and point cloud data are all in frames. The IOU algorithm is used to obtain the point cloud of all target vehicles in the current frame by using the vehicle detection box and the 2D projection of the cluster data in the current frame. This can improve the accuracy of target vehicle extraction and facilitate the calculation of subsequent methods.

[0073] Furthermore, the point clouds of each target vehicle in each frame are subjected to divergence-based adaptive purification to obtain the clean point clouds of each target vehicle in each frame, and the relative pose estimation of each target vehicle is achieved by using the ICP algorithm.

[0074] The step of performing divergence-based adaptive purification of the point cloud of a target vehicle in the current frame includes steps S31-S33:

[0075] S31: Perform voxel segmentation on the point cloud of the target vehicle in the current frame, obtain the average divergence of each voxel based on the center point of the point cloud within each voxel, and filter to obtain the target point cloud of the current frame; for example, the length, width and height of the voxel segmentation cube are all 0.3 meters.

[0076] It's important to note that in physics, divergence represents the degree of divergence of a vector field at a given point. In point cloud data, points belonging to dust or particulate matter exhibit a three-dimensional structure due to their penetrability and semi-transparency; point clouds representing object surfaces have planar properties; and point clouds only exist on the non-planar regions of an object on the surfaces facing the lidar scanning direction. Therefore, point clouds of dust or particulate matter can be distinguished from point clouds of object surfaces based on divergence.

[0077] Specifically, the center point of the point cloud within each voxel is calculated using the following formula:

[0078]

[0079] Where m represents the number of point clouds within a voxel, p i Let represent the coordinates of the i-th point cloud within the voxel in the lidar coordinate system, p represent the center point of the point cloud within the voxel, and (x0, y0, z0) represent the coordinates of p in the lidar coordinate system.

[0080] Furthermore, the mean divergence of each voxel is calculated using the following formula:

[0081] F pi =[(x i -x0) 2 ,(y i -y0) 2 ,(z i -z0) 2 ]

[0082]

[0083] Among them, F pi Point p i The vector function to p, Point p i The divergence value, F pix F represents pi The component in the x-direction, F piy F represents pi The component in the y-direction, F piz F represents pi The component in the z-direction, F avg represents the mean divergence of the voxels, and ||·|| represents the modulus value.

[0084] The filtering process to obtain the target point cloud of the current frame includes: filtering out voxels with a divergence mean greater than a divergence threshold to obtain the target point cloud of the current frame; for example, the divergence threshold is set to 0.8 in order to filter out dust and particulate matter point clouds.

[0085] S32: Randomly sample the target point cloud of the current frame and calculate the pose of the target point cloud of the current frame to obtain the set of overlapping points of the clean point cloud of the target vehicle in the previous frame and the target point cloud of the current frame.

[0086] Specifically, the step of randomly sampling the target point cloud and calculating the pose of the target point cloud in the current frame includes:

[0087] The target point cloud is randomly sampled to obtain an initial target point cloud, and the sample size is fixed for each sampling; for example, the sample size is 100.

[0088] Based on the initial target point cloud, the attitude matrix of the initial corresponding target point cloud relative to the lidar is calculated using the PCA algorithm. Where l represents the lidar and c represents the target point cloud, the steps include: calculating the covariance matrix of the initial target point cloud; performing SVD decomposition on the covariance matrix to obtain the eigenvalues ​​and eigenvectors of the covariance matrix; sorting the obtained eigenvalues ​​from largest to smallest, selecting the three largest eigenvalues, and obtaining the three corresponding eigenvectors; based on the three corresponding eigenvectors, obtaining the direction cosine matrix of the initial target point cloud relative to the lidar coordinate system, and converting it into Euler angle form to obtain the attitude matrix of the initial target point cloud relative to the current vehicle lidar, which is regarded as the attitude matrix of the corresponding target point cloud relative to the lidar.

[0089] Furthermore, the position vector of the target point cloud can be approximately determined by the geometric center vector of the initial target point cloud. Based on all point cloud vectors contained in the initial target point cloud, the position vector of the target point cloud is calculated using the following formula:

[0090] r k =(x k -x s ,y k -y s ,z k -z s )

[0091]

[0092] Where, r k Let x represent the point cloud vector of the k-th point in the initial target point cloud, (x s ,y s ,z s (x) represents the origin coordinates of the lidar coordinate system. k ,y k ,z k () represents the coordinates of the k-th point in the lidar coordinate system. The vector represents the position of the target point cloud, where l represents the lidar, c represents the target point cloud, and d represents the number of points in the initial target point cloud.

[0093] The pose of the target point cloud is represented as follows:

[0094] Further, obtaining the set of overlapping points between the clean point cloud of the target vehicle in the previous frame and the target point cloud in the current frame includes: calculating the transformation matrix from the clean point cloud of the target vehicle in the previous frame to the target point cloud in the current frame based on the pose of the clean point cloud of the target vehicle in the previous frame and the pose of the target point cloud in the current frame; and obtaining the set of overlapping points between the clean point cloud of the target vehicle in the previous frame and the target point cloud in the current frame based on the transformation matrix.

[0095] The calculation is performed iteratively frame by frame. Based on the clean point cloud of the target vehicle in the previous frame, the clean point cloud of the target vehicle in the current frame is obtained. The clean point cloud of the target vehicle in the first frame is the target point cloud obtained after voxel segmentation.

[0096] Specifically, calculating the transformation matrix includes:

[0097] The pose of the target vehicle based on the clean point cloud of the previous frame. pose of the target point cloud in the current frame The transformation matrix from the clean point cloud of the target vehicle in the previous frame to the target point cloud in the current frame can be calculated using the following formula:

[0098]

[0099] Where c1 represents the pose of the target point cloud at time c1, and c2 represents the pose of the target point cloud at time c2. Specifically, calculating the set of coincident points includes:

[0100] The clean point cloud of the target vehicle in the previous frame is projected onto the target point cloud in the current frame using the transformation matrix to obtain projection points. Then, the nearest matching points to each projection point are found sequentially in the target point cloud of the current frame to obtain a set of matching points S. The distance between each matching point and its corresponding projection point is calculated sequentially. When the distance is less than or equal to a matching threshold, the matching point is added to the set of coincidence points S. inside Otherwise, add the matching point to the set of free points S. outside And S = S inside US outside For example, the matching threshold is 0.1; it should be noted that one projection point only matches one matching point, when the obtained S inside When the number of midpoints is less than 200, delete the resulting S. inside And form S from all the points in the target point cloud of the current frame. inside .

[0101] S33: Iterative calculation, considering the set of overlapping points with the most points as the clean point cloud of the target vehicle in the current frame, including:

[0102] The number of adaptive iterations for the current frame can be calculated using the following formula:

[0103]

[0104] Where k represents the number of adaptive iterations for the current frame, y represents the expected probability, and n represents the sample size of the current frame. N represents the number of coincident points. s This represents the number of points in the matching point set; for example, y is 0.9.

[0105] If the calculated k becomes smaller, then update k and recalculate the number of iterations; in each iteration, randomly sample the target point cloud again and complete the subsequent operations; until the k iterations are completed;

[0106] The set of overlapping points with the most points is considered as the clean point cloud of the target vehicle in the current frame.

[0107] It should be noted that the value of k will decrease during the update, thereby reducing the number of loops. Since the target point cloud is randomly sampled each time the pose is calculated, the more points in the overlapping point set, the higher the overlap between the projection points of the pure point cloud of the target vehicle in the previous frame and the target point cloud in the current frame, and the more accurate the corresponding transformation matrix, thus obtaining the pure point cloud of the target vehicle.

[0108] Furthermore, based on the clean point cloud of the target vehicle, the relative pose estimation of the target vehicle is achieved using the ICP algorithm; by traversing all target vehicles, the relative pose estimation of all target vehicles is completed; the ICP algorithm can achieve accurate registration results, and under good initial conditions, it can achieve good algorithm convergence and has low dependence on the initial pose; after divergence-based adaptive point cloud purification, the noise of the clean point cloud is significantly reduced, meeting the input requirements of the ICP algorithm.

[0109] Compared with existing technologies, the pose estimation method provided in this embodiment can perform divergence-based adaptive purification of the point cloud data of each target vehicle when the point cloud data collected by LiDAR may include a large amount of smoke and dust, so as to obtain accurate and clean point clouds of the target vehicles. By combining YOLO-V10 and DeepSort, it can detect and track all vehicle targets in each frame of the image, and achieve stable target tracking across multiple frames. Even if the target disappears, reappears, intersects or is occluded in some frames, it can still achieve efficient and accurate target detection and tracking. Based on the current driving speed of the unmanned vehicle, voxel filtering with a preset voxel size is performed on the point cloud to avoid the point cloud data being too dense and to ensure the calculation speed.

[0110] Those skilled in the art will understand that all or part of the processes of the methods described in the above embodiments can be implemented by a computer program instructing related hardware, and the program can be stored in a computer-readable storage medium. The computer-readable storage medium may be a disk, optical disk, read-only memory, or random access memory, etc.

[0111] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for estimating the pose of an unmanned vehicle under communication denial conditions, characterized in that, include: Target detection and tracking are performed on each frame of images captured by the camera to obtain all vehicle detection boxes in each frame of images; The point cloud data of each frame collected by the lidar are clustered and projected onto the pixel coordinate system to obtain the 2D projection of the clustered data. The point cloud of all target vehicles in each frame is calculated by combining the vehicle detection box with the IOU algorithm. The point clouds of each target vehicle in each frame are subjected to divergence-based adaptive purification to obtain the clean point clouds of each target vehicle in each frame, and the relative pose estimation of each target vehicle is realized by using the ICP algorithm. The divergence-based adaptive purification of the point cloud of a target vehicle in the current frame includes: performing voxel segmentation on the obtained point cloud of the target vehicle in the current frame, obtaining the average divergence of each voxel based on the center point of the point cloud in each voxel, and filtering to obtain the target point cloud of the current frame; randomly sampling the target point cloud of the current frame, calculating the pose of the target point cloud of the current frame to obtain the set of overlapping points of the clean point cloud of the target vehicle in the previous frame and the target point cloud of the current frame; iteratively calculating, and taking the set of overlapping points with the most points as the clean point cloud of the target vehicle in the current frame.

2. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to claim 1, characterized in that, The center point of the point cloud within each voxel is calculated using the following formula: Where m represents the number of point clouds within a voxel, p i Let represent the coordinates of the i-th point cloud within the voxel in the lidar coordinate system, p represent the center point of the point cloud within the voxel, and (x0, y0, z0) represent the coordinates of p in the lidar coordinate system.

3. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to claim 2, characterized in that, The mean divergence of each voxel is calculated using the following formula: F pi =[(x i -x0) 2 ,(y i -y0) 2 ,(z i -z0) 2 ] Among them, F pi Point p i The vector function to p, Point p i The divergence value, F pix F represents pi The component in the x-direction, F piy F represents pi The component in the y-direction, F piz F represents pi The component in the z-direction, F avg represents the mean divergence of the voxels, and ||·|| represents the modulus value.

4. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to any one of claims 1-3, characterized in that, The filtering process to obtain the target point cloud of the current frame includes: Voxels with a divergence mean greater than the divergence threshold are filtered out to obtain the target point cloud of the current frame.

5. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to claim 1, characterized in that, The step of randomly sampling the target point cloud and calculating the pose of the target point cloud in the current frame includes: The target point cloud is randomly sampled to obtain an initial target point cloud, and the sample size is fixed for each sampling. Based on the initial target point cloud, the attitude matrix of the corresponding target point cloud relative to the lidar is calculated using the PCA algorithm. Where l represents the lidar and c represents the target point cloud; Based on all point cloud vectors contained in the initial target point cloud, the position vector of the target point cloud is calculated using the following formula: r k =(x k -x s ,y k -y s ,z k -z s ) Where, r k Let x represent the point cloud vector of the k-th point in the initial target point cloud, (x s ,y s ,z s (x) represents the origin coordinates of the lidar coordinate system. k ,y k ,z k () represents the coordinates of the k-th point in the lidar coordinate system. The vector represents the position of the target point cloud, where l represents the lidar, c represents the target point cloud, and d represents the number of points in the initial target point cloud. The pose of the target point cloud is represented as follows:

6. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to claim 5, characterized in that, The process of obtaining the set of overlapping points between the clean point cloud of the target vehicle in the previous frame and the target point cloud in the current frame includes: Based on the pose of the clean point cloud of the target vehicle in the previous frame and the pose of the target point cloud in the current frame, the transformation matrix from the clean point cloud of the target vehicle in the previous frame to the target point cloud in the current frame is calculated. Based on the transformation matrix, the set of overlapping points of the clean point cloud of the target vehicle in the previous frame and the target point cloud in the current frame are obtained.

7. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to claim 6, characterized in that, The calculation of the transformation matrix includes: The pose of the target vehicle based on the clean point cloud of the previous frame. pose of the target point cloud in the current frame The transformation matrix from the clean point cloud of the target vehicle in the previous frame to the target point cloud in the current frame can be calculated using the following formula: Where c1 represents the pose of the target point cloud at time c1, and c2 represents the pose of the target point cloud at time c2.

8. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to claim 7, characterized in that, The acquisition of the set of coincidence points includes: The pure point cloud of the target vehicle in the previous frame is projected onto the target point cloud in the current frame using the transformation matrix to obtain the projection points; Find the matching points closest to each projection point in the target point cloud of the current frame in turn to obtain the matching point set S; Calculate the distance between each matching point and its corresponding projection point in sequence. When the distance is less than or equal to the matching threshold, add the matching point to the coincidence point set S. inside Otherwise, add the matching point to the set of free points S. outside .

9. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to claim 8, characterized in that, The iterative calculation, which considers the set of overlapping points with the most points as the clean point cloud of the target vehicle in the current frame, includes: The number of adaptive iterations for the current frame can be calculated using the following formula: Where k represents the number of adaptive iterations for the current frame, y represents the expected probability, and n represents the sample size of the current frame. N represents the number of coincident points. s Represents the number of matching point concentration points; If the calculated k becomes smaller, then update k and recalculate the number of iterations; in each iteration, randomly sample the target point cloud again and complete the subsequent operations; until the k iterations are completed; The set of overlapping points with the most points is considered as the clean point cloud of the target vehicle in the current frame.

10. The method for estimating the pose of an unmanned vehicle under communication denial conditions according to claim 1, characterized in that, The process of clustering the point cloud data of each frame acquired by the lidar includes performing voxel filtering with a preset voxel size and Euclidean point cloud clustering on each frame of point cloud data acquired by the lidar.

Images