Target pose estimation method based on vision and radar

Abstract

The invention discloses a target pose estimation method based on vision and radar. The method comprises the following steps: calibrating a sensor; target detection is carried out based on a YOLOv5s model, and the category of a target object and the coordinates of an anchor frame are obtained; performing space-time synchronization on the vision and radar data, and determining an initial position coordinate of the target object in a navigation coordinate system; based on principal component analysis, principal direction features of the target point cloud are extracted, a directed KD tree is constructed, and nearest neighbor search is realized; a point-to-surface constrained ICP algorithm is adopted to realize high-precision estimation of a target pose; and in combination with the initial position coordinates, projecting the initial position coordinates into a unified navigation coordinate system to realize consistent pose expression. According to the method, visual data and radar data are fused for target pose estimation, and higher robustness and precision are shown in complex and changeable outdoor cruise and reconnaissance environments with unstable illumination conditions.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent perception and multi-sensor fusion, and in particular to a target pose estimation method based on vision and radar. Background Art

[0002] In fields such as autonomous driving, robotic navigation, and intelligent surveillance, object pose estimation is a crucial foundation for environmental perception and autonomous decision-making. The accuracy of pose estimation directly impacts the system's navigation, obstacle avoidance, control, and mission planning capabilities, making it a core technology in intelligent perception systems.

[0003] Traditional pose estimation methods typically rely on a single sensor type, such as camera-based vision methods or data inference based on lidar or millimeter-wave radar. Vision sensors provide high-resolution image information, suitable for capturing rich environmental features, but their performance can degrade in low light, strong light interference, severe occlusion, or inclement weather. In contrast, radar sensors offer superior immunity to environmental interference and stable ranging characteristics, maintaining reliable perception in complex environments such as rain and fog. However, due to their low spatial resolution, they struggle to accurately extract fine-grained environmental features.

[0004] To address the limitations of single sensors, multi-sensor fusion has become a key research direction in recent years. In particular, the fusion of vision and radar can fully leverage the strengths of both sensor types, leveraging both the detail-capturing capabilities of vision sensors and the stable ranging characteristics of radar sensors, thereby achieving more accurate and robust target pose estimation.

[0005] Multi-sensor fusion pose estimation methods primarily involve three strategies: data-level fusion, feature-level fusion, and decision-level fusion. Data-level fusion directly integrates raw sensor data, preserving information to the greatest extent possible but requiring high synchronization and registration accuracy. Feature-level fusion extracts features from data from different sensors and then performs joint modeling, improving estimation stability and accuracy. Decision-level fusion integrates the results of independent inference at a high level, offering greater flexibility and fault tolerance.

[0006] Despite significant progress in theory and experimentation, vision and radar fusion methods still face a series of challenges in practical applications. For example, differences in sampling frequency, field of view, and accuracy between vision and radar sensors lead to the accumulation of errors in temporal synchronization and spatial registration. The changing characteristics of targets in dynamic environments increase the difficulty of cross-modal feature matching. Furthermore, to ensure real-time system performance, an effective trade-off between accuracy and computational overhead is required. Summary of the Invention

[0007] In view of this, the purpose of the present invention is to provide a target pose estimation method based on vision and radar to solve the technical problem of insufficient stability and accuracy in estimating the target pose in complex scenes using single sensor data.

[0008] The target pose estimation method based on vision and radar of the present invention comprises the following steps:

[0009] 1) Calibrate the camera and radar used to collect navigation environment information. The calibration includes camera parameter calibration and radar and camera joint calibration. The calibration obtains the camera's intrinsic parameters and the transformation matrix between the radar and camera.

[0010] 2) Train the YOLOv5s model and feed the images captured by the camera into the trained YOLOv5s model to obtain the category and anchor frame coordinates of the target object;

[0011] 3) Using the camera intrinsic parameters obtained in step 1) and the radar-to-camera transformation matrix, a unified coordinate system is constructed to achieve spatial synchronization of visual and radar information. A timestamp-based synchronization strategy is used to achieve temporal synchronization of visual and radar information, ultimately achieving spatiotemporal synchronization of visual and radar data.

[0012] 4) Extracting the point that best matches the target's position from the radar point cloud set as the centroid point, and converting the three-dimensional point cloud coordinates of the centroid point into the navigation coordinate system to obtain the target's initial position;

[0013] 5) Constructing a KD tree: Extract the target point cloud from the radar point cloud, then extract the principal direction of the target point cloud. Next, calculate the stopping condition for tree construction, and determine whether to stop the tree construction process based on the size of the point cloud bounding box. Then, construct a plane perpendicular to the principal direction as a splitting plane, divide the target point cloud into sub-point cloud sets along the splitting plane, and then use the sub-point cloud sets as input to establish a hierarchical structure to complete the construction of a directed KD tree.

[0014] 6) When a point cloud to be matched appears in the target point cloud set extracted from the subsequent radar frame, the KD tree is searched to find the point cloud in the leaf node that matches the point to be matched. The ICP algorithm with point-to-surface constraints is used to calculate the optimal pose transformation that needs to be satisfied when aligning the matching target point clouds in the two previous and subsequent radar frames, thereby estimating the target pose. Combined with the initial position of the target obtained in step 4), the target pose is projected from the object coordinate system to the navigation coordinate system to obtain the pose estimation result of the target in the navigation coordinate system.

[0015] Furthermore, the construction of a unified coordinate system in step 3) to achieve spatial synchronization of visual and radar information includes:

[0016] 1) According to the homogeneous coordinates of a point in the radar coordinate system [XL ,Y L ,Z L ,1] T And the transformation matrix T between the radar and the camera constructs the transformation relationship from the radar coordinate system to the camera coordinate system:

[0017]

[0018] Among them, R in the transformation matrix T is the rotation matrix, and t is the translation;

[0019] 2) The camera coordinate system is divided into the image pixel coordinate system and the image physical coordinate system. According to the camera's intrinsic parameter f x ,f y ,c x ,c y Establish the transformation relationship between the image physical coordinate system and the image pixel coordinate system:

[0020]

[0021] According to the derivation results of equations (1) and (2), all point clouds in the radar coordinate system are converted to the camera image pixel coordinate system. The transformation relationship is as follows:

[0022]

[0023] Where: (X p ,Y p ,Z p ) represents the point cloud in the radar coordinate system, and (u, v) represents the coordinates of the point cloud projected to the image pixel coordinate system;

[0024] The time synchronization of visual and radar information achieved by adopting a timestamp-based synchronization strategy as described in step 3) includes: designing independent buffer containers to store the image data collected by the camera and the point cloud data collected by the radar, respectively, and using the timestamp of the image data as the synchronization reference; when the point cloud data arrives, retrieving the point cloud record closest to the current image timestamp from the corresponding buffer container to achieve alignment of the two data in the time domain.

[0025] Furthermore, the step 4) of extracting the point that best matches the position of the target object from the radar point cloud set as the centroid point of the target object includes:

[0026] 1) According to the coordinates of the two diagonal points (x1, y1) and (x2, y2) of the target object’s anchor frame, the centroid coordinates (x c ,y c ):

[0027]

[0028] 2) Calculate the Euclidean distance between the projection point of the radar point cloud data and the centroid in the image pixel coordinate system according to the following formula:

[0029]

[0030] Where (u i ,v i ) is the coordinate of the radar point cloud data projection point in the image pixel coordinate system;

[0031] 3) Select the projection points from the radar point cloud data so that d i The smallest projection point (u min ,v min ),

[0032] (u min ,v min )=argmind i (6)

[0033] If the projection point (u min ,v min ) satisfies the following constraints:

[0034] x1≤u min ≤x2,y1≤v min ≤y2 (7)

[0035] Then the projection point (u min ,v min ) in the anchor frame, and then the projection point (u min ,v min ) in the radar point cloud set (x i ,y i ,z i ) as the centroid point (x target ,y target ,z target ):

[0036] (x target ,y target ,z target )=(x i ,y i ,z i ) (8)

[0037] In step 4), the three-dimensional point cloud coordinates of the centroid are converted to the navigation coordinate system through the following transformation relationship:

[0038]

[0039] Among them, T LN is the transformation matrix from radar to navigation coordinate system, (x N ,y N ,zN ) is the position coordinate of the target object in the navigation coordinate system.

[0040] Furthermore, the point cloud set of the extracted target object in step 5) includes:

[0041] After synchronizing the time of visual and radar information, all point clouds in the radar coordinate system at the current time point are projected into the camera's image pixel coordinate system according to the following formula to complete spatiotemporal synchronization:

[0042] L={P i =(x i ,y i ,z i )|i=1,2,...,N} (10)

[0043] Let the two-dimensional projection coordinate set of the point cloud in the image pixel coordinate system be X, and we get:

[0044] X={(u i ,v i )|i=1,2,...,N} (11)

[0045] All projected point clouds can be filtered using the anchor frame coordinates B to obtain the point cloud set C. The anchor frame coordinates B are:

[0046] B=[u min ,v min ,u max ,v max ] (12)

[0047] Where (u min ,v min ) represents the pixel coordinates of the upper left corner of the anchor box, (u max ,v max ) represents the pixel coordinates of the lower right corner of the anchor box; the point cloud set C is:

[0048] C={P i ∈L|u min ≤u i ≤u max ,v min ≤v i ≤v max} (13)

[0049] Then, the background point cloud in the point cloud set C is removed to obtain the target object point cloud set D = {p1, p2, ..., p n};

[0050] The main direction of the target point cloud set extracted in step 5) includes:

[0051] Use the following formula to calculate the centroid of the target point cloud set D With the covariance matrix Σ:

[0052]

[0053] Perform eigenvalue decomposition on the covariance matrix Σ to obtain the eigenvalue λ i The corresponding eigenvector v i , expressed using formula (15); the eigenvector corresponding to the largest eigenvalue is the main direction of the target point cloud set

[0054] E = [v1, v2, v3] (15);

[0055] The tree building stop condition calculation described in step 5) is then performed to determine whether to stop the tree building process based on the size of the point cloud bounding box, including:

[0056] The point cloud set D={q1,q2,...,q n The data points in} are connected by the feature matrix E and the centroid Mapped to a new coordinate system with the main direction as one of the coordinate axes, the transformed point cloud set is recorded as D'={q1,q2,...,q n}, the mapping process is described as follows:

[0057]

[0058] For each dimension j∈{X,Y,Z}, extract the minimum and maximum boundaries of the point cloud bounding box, and use formula (17) to express it:

[0059]

[0060] Where: b min represents the minimum boundary, b max Indicates the maximum boundary;

[0061] Calculate the bounding box size b box for:

[0062]

[0063] If b z If it is less than the set threshold, it means that the point clouds in the bounding box are in the same plane, and the tree building process is stopped.

[0064] The step 5) of dividing the target point cloud set by the splitting plane to obtain the sub-point cloud set includes:

[0065] The splitting plane will be used to divide the target point cloud set D into the left sub-point cloud set D l and the right sub-point cloud set D r , the sub-point cloud set is represented as follows:

[0066]

[0067] In the formula Indicates the current point p i The centroid point of the point cloud collection The offset between

[0068] Then, the left and right sub-point cloud sets are used as input to construct the left and right sub-trees respectively, and a hierarchical structure is established to complete the construction of the directed KD tree.

[0069] Furthermore, the step of searching the KD tree in step 6) to find a point cloud in a leaf node that matches the point to be matched includes:

[0070] Start the recursive query from the root node r of the tree and complete the tree traversal. The judgment criterion is shown in formula (20). Iterate the query until the leaf node is reached. The point cloud in the leaf node is considered to be the point to be matched with the point p. cur Matched point cloud:

[0071]

[0072] Furthermore, in step 6), the ICP algorithm with point-to-plane constraints is used to calculate the optimal pose transformation that needs to be satisfied when aligning the matching target point clouds in the two radar frames, including:

[0073] The optimization criterion is to minimize the normal distance from the source point cloud to the local plane formed by the target point cloud. The error function is shown in (21):

[0074]

[0075] The source point cloud is the target point cloud from the previous radar frame, and the target point cloud is the point cloud that matches the source point cloud in the next radar frame. i Represents the three-dimensional coordinates of the source point cloud, q i Represents the three-dimensional coordinates of the target point cloud, Represents point q i The unit normal vector of the plane, R opt and t opt That is the optimal posture transformation that needs to be obtained through the ICP algorithm;

[0076] After setting the maximum number of iterations and the convergence termination condition, the point-to-plane ICP algorithm iteratively estimates the optimal rigid body transformation by minimizing the projection error in the normal direction of the tangent plane of each point in the source point cloud to its corresponding target point cloud in each iteration; the final estimated target object pose is given in the form of a rigid body transformation matrix:

[0077]

[0078] Then, based on the initial position (x N ,y N ,z N ), combined with the relative transformation T obtained by the current point cloud registration obj , complete the pose transformation of the target object from its own object coordinate system to the navigation coordinate system, as shown in formula (23):

[0079]

[0080] The last T nav That is the pose estimation result of the target object in the navigation coordinate system.

[0081] Beneficial effects of the present invention:

[0082] 1. The target pose estimation method based on vision and radar in this invention fuses visual data and radar data to estimate the target pose, showing stronger robustness and accuracy in outdoor cruising and reconnaissance environments with complex and changeable lighting conditions.

[0083] 2. Experiments have shown that the target pose estimation method based on vision and radar in the present invention is superior to the traditional ICP algorithm in terms of pose estimation accuracy and computational efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 It is a flowchart of the target pose estimation method based on vision and radar.

[0085] Figure 2 This is a flow chart of the target pose estimation method that integrates vision and radar information.

[0086] Figure 3 It is an unmanned car;

[0087] Figure 4 It is the target pose estimation experimental scene;

[0088] Figure 5 It is a schematic diagram of target detection results and point cloud projection.

[0089] Figure 6 It is the target point cloud extraction and denoising result. DETAILED DESCRIPTION

[0090] The present invention will be further described below with reference to the accompanying drawings and examples.

[0091] The target pose estimation method based on vision and radar in this embodiment includes the following steps:

[0092] 1) Calibrate the camera and radar used to collect navigation environment information. The calibration includes camera parameter calibration and radar and camera joint calibration. The calibration obtains the camera's intrinsic parameters and the transformation matrix between the radar and camera.

[0093] In this embodiment, camera parameter calibration includes: first, configuring the image topic and calibration board parameters, and starting the camera calibration node; then, using the camera to capture the calibration object image; finally, using the camera_calibration function package in ROS to calculate and obtain the camera calibration parameters; of course, other methods can also be used to implement camera parameter calibration in different embodiments.

[0094] In this embodiment, the joint calibration of the radar and camera involves first acquiring a synchronized camera image and radar point cloud data frame. Subsequently, using the lidar2camera tool, based on the obtained camera calibration parameters, the radar point cloud data is projected into a pixel coordinate system, achieving a visual mapping of the point cloud data onto the image. The rotation and translation adjustment functions provided by the lidar2camera interface are then used to optimize and adjust the point cloud to ensure proper alignment with the image target, ultimately obtaining the transformation matrix between the radar and camera. Of course, in different embodiments, other methods can also be used to achieve joint calibration of the radar and camera.

[0095] 2) Train the YOLOv5s model and feed the images captured by the camera into the trained YOLOv5s model to obtain the category and anchor frame coordinates of the target object.

[0096] The total training of the YOLOv5s model in this embodiment includes: using the Labelme tool to label the objects in the images of the training dataset. The training dataset can be self-built or an existing image dataset can be used. The labeled categories include car and person. The YOLOv5s model is trained using a frozen layer transfer learning strategy. Based on the pre-trained weight file of YOLOv5s, the parameters of the first nine layers of the Backbone network are frozen for network training to reduce computational overhead and accelerate convergence.

[0097] 3) Using the camera intrinsic parameters obtained in step 1) and the transformation matrix between radar and camera, a unified coordinate system is constructed to achieve spatial synchronization of visual and radar information. A timestamp-based synchronization strategy is used to achieve temporal synchronization of visual and radar information, ultimately achieving spatiotemporal synchronization of visual and radar data.

[0098] In this step, a unified coordinate system is constructed to achieve spatial synchronization of visual and radar information, including:

[0099] 1) According to the homogeneous coordinates of a point in the radar coordinate system [X L ,Y L ,Z L ,1]T And the transformation matrix T between the radar and the camera constructs the transformation relationship from the radar coordinate system to the camera coordinate system:

[0100]

[0101] Among them, R in the transformation matrix T is the rotation matrix and t is the translation.

[0102] 2) The camera coordinate system is divided into the image pixel coordinate system and the image physical coordinate system. According to the camera's intrinsic parameter f x ,f y ,c x ,c y Establish the transformation relationship between the image physical coordinate system and the image pixel coordinate system:

[0103]

[0104] According to the derivation results of equations (1) and (2), all point clouds in the radar coordinate system are converted to the camera image pixel coordinate system. The transformation relationship is as follows:

[0105]

[0106] Where: (X p ,Y p ,Z p ) represents the point cloud in the radar coordinate system, and (u, v) represents the coordinates of the point cloud projected to the image pixel coordinate system.

[0107] The time synchronization of visual and radar information achieved by adopting a timestamp-based synchronization strategy described in this step includes: designing independent buffer containers to store the image data collected by the camera and the point cloud data collected by the radar, respectively, and using the timestamp of the image data as the synchronization reference; when the point cloud data arrives, retrieving the point cloud record closest to the current image timestamp from the corresponding buffer container to achieve alignment of the two data in the time domain.

[0108] 4) Extract the point that best matches the target's position from the radar point cloud set as the centroid point, and convert the three-dimensional point cloud coordinates of the centroid point into the navigation coordinate system to obtain the initial position of the target.

[0109] In this step, the point that best matches the target's position is extracted from the radar point cloud set as the target's centroid point, including:

[0110] 1) According to the coordinates of the two diagonal points (x1, y1) and (x2, y2) of the target object’s anchor frame, the centroid coordinates (x c ,y c ):

[0111]

[0112] 2) Calculate the Euclidean distance between the projection point of the radar point cloud data and the centroid in the image pixel coordinate system according to the following formula:

[0113]

[0114] Where (u i ,v i ) is the coordinate of the radar point cloud data projection point in the image pixel coordinate system;

[0115] 3) Select the projection points from the radar point cloud data so that d i The smallest projection point (u min ,v min ),

[0116] (u min ,v min )=argmind i (6)

[0117] If the projection point (u min ,v min ) satisfies the following constraints:

[0118] x1≤u min ≤x2,y1≤v min ≤y2 (7)

[0119] Then the projection point (u min ,v min ) in the anchor frame, and then the projection point (u min ,v min ) in the radar point cloud set (x i ,y i ,z i ) as the centroid point (x target ,y target ,z target ):

[0120] (x target ,y target ,z target )=(x i ,y i ,z i ) (8)

[0121] In this step, the three-dimensional point cloud coordinates of the centroid are converted to the navigation coordinate system through the following transformation relationship:

[0122]

[0123] Among them, T LN is the transformation matrix from radar to navigation coordinate system, (xN ,y N ,z N ) is the position coordinate of the target object in the navigation coordinate system.

[0124] 5) Constructing a KD tree: Extract the target point cloud set from the radar point cloud set, and then extract the main direction of the target point cloud set; then calculate the stopping condition for tree construction, and determine whether to stop the tree construction process based on the size of the point cloud bounding box; then construct a plane perpendicular to the main direction as a splitting plane, divide the target point cloud set into sub-point cloud sets through the splitting plane, and then use the sub-point cloud sets as input to establish a hierarchical structure to complete the construction of a directed KD tree.

[0125] The point cloud set of the target object extracted in this step includes:

[0126] After synchronizing the time of visual and radar information, all point clouds in the radar coordinate system at the current time point are projected into the camera's image pixel coordinate system according to the following formula to complete spatiotemporal synchronization:

[0127] L={P i =(x i ,y i ,z i )|i=1,2,...,N} (10)

[0128] Let the two-dimensional projection coordinate set of the point cloud in the image pixel coordinate system be X, and we get:

[0129] X={(u i ,v i )|i=1,2,...,N} (11)

[0130] All projected point clouds can be filtered using the anchor frame coordinates B to obtain the point cloud set C. The anchor frame coordinates B are:

[0131] B=[u min ,v min ,u max ,v max ] (12)

[0132] Where (u min ,v min ) represents the pixel coordinates of the upper left corner of the anchor box, (u max ,v max ) represents the pixel coordinates of the lower right corner of the anchor box; the point cloud set C is:

[0133] C={P i ∈L|u min ≤u i ≤u max ,v min ≤v i ≤vmax} (13)

[0134] Then, the background point cloud in the point cloud set C is removed to obtain the target object point cloud set D = {p1, p2, ..., p n};

[0135] The main directions of the target point cloud set extracted in this step include:

[0136] Use the following formula to calculate the centroid of the target point cloud set D With the covariance matrix Σ:

[0137]

[0138] Perform eigenvalue decomposition on the covariance matrix Σ to obtain the eigenvalue λ i The corresponding eigenvector v i , expressed using formula (15); the eigenvector corresponding to the largest eigenvalue is the main direction of the target point cloud set

[0139] E = [v1, v2, v3] (15);

[0140] In this step, the tree building stop condition is calculated and the size of the point cloud bounding box is used to determine whether to stop the tree building process, including:

[0141] The point cloud set D={q1,q2,...,q n The data points in} are connected by the feature matrix E and the centroid Mapped to a new coordinate system with the main direction as one of the coordinate axes, the transformed point cloud set is recorded as D'={q1,q2,...,q n}, the mapping process is described as follows:

[0142]

[0143] For each dimension j∈{X,Y,Z}, extract the minimum and maximum boundaries of the point cloud bounding box, and use formula (17) to express it:

[0144]

[0145] Where: b min represents the minimum boundary, b max Indicates the maximum boundary;

[0146] Calculate the bounding box size b box for:

[0147]

[0148] If b zIf it is less than the set threshold, it means that the point clouds in the bounding box are in the same plane, and the tree building process is stopped.

[0149] In this step, dividing the target point cloud set by the splitting plane to obtain the sub-point cloud set includes:

[0150] The splitting plane will be used to divide the target point cloud set D into the left sub-point cloud set D l and the right sub-point cloud set D r , the sub-point cloud set is represented as follows:

[0151]

[0152] In the formula Indicates the current point p i The centroid point of the point cloud collection The offset between

[0153] Then, the left and right sub-point cloud sets are used as input to construct the left and right sub-trees respectively, and a hierarchical structure is established to complete the construction of the directed KD tree.

[0154] 6) When a point cloud to be matched appears in the target point cloud set extracted from the subsequent radar frame, the KD tree is searched to find the point cloud in the leaf node that matches the point to be matched. The ICP algorithm with point-to-surface constraints is used to calculate the optimal pose transformation that needs to be satisfied when aligning the matching target point clouds in the two previous and subsequent radar frames, thereby estimating the target pose. Combined with the initial position of the target obtained in step 4), the target pose is projected from the object coordinate system to the navigation coordinate system to obtain the pose estimation result of the target in the navigation coordinate system.

[0155] In this step, searching the KD tree to find a point cloud in a leaf node that matches the point to be matched includes:

[0156] Start the recursive query from the root node r of the tree and complete the tree traversal. The judgment criterion is shown in formula (20). Iterate the query until the leaf node is reached. The point cloud in the leaf node is considered to be the point to be matched with the point p. cur Matched point cloud:

[0157]

[0158] In this step, the ICP algorithm with point-to-plane constraints is used to calculate the optimal pose transformation that needs to be satisfied when aligning the matching target point clouds in the previous and next radar frames, including:

[0159] The optimization criterion is to minimize the normal distance from the source point cloud to the local plane formed by the target point cloud. The error function is shown in (21):

[0160]

[0161] The source point cloud is the target point cloud from the previous radar frame, and the target point cloud is the point cloud that matches the source point cloud in the next radar frame. i Represents the three-dimensional coordinates of the source point cloud, q i Represents the three-dimensional coordinates of the target point cloud, Represents point q i The unit normal vector of the plane, R opt and t opt That is the optimal posture transformation that needs to be obtained through the ICP algorithm;

[0162] After setting the maximum number of iterations and the convergence termination condition, the point-to-plane ICP algorithm iteratively estimates the optimal rigid body transformation by minimizing the projection error in the normal direction of the tangent plane of each point in the source point cloud to its corresponding target point cloud in each iteration; the final estimated target object pose is given in the form of a rigid body transformation matrix:

[0163]

[0164] Then, based on the initial position (x N ,y N ,z N ), combined with the relative transformation T obtained by the current point cloud registration obj , complete the pose transformation of the target object from its own object coordinate system to the navigation coordinate system, as shown in formula (23):

[0165]

[0166] The last T nav That is the pose estimation result of the target object in the navigation coordinate system.

[0167] The following is a comparative experiment of the target pose estimation method based on vision and radar in this embodiment and the traditional ICP method:

[0168] Experiments on target pose estimation in different scenarios. An integrated intelligent car platform is selected as a substitute for the target object (such as Figure 3(As shown). The smart car is equipped with sensors such as wheel encoders, IMU, and GNSS, which can accurately measure its own position transformation, thereby providing high-precision reference truth data for the experiment. Relying on the unmanned vehicle experimental platform, the motion data of the target vehicle was collected in two representative environmental scenes. Scene A is an open area, the distance between the target vehicle and the unmanned vehicle is large, and the number of target point clouds collected is small; in scene B, there are obstacles around and behind the target vehicle, and the point cloud data is more complex. To obtain high-quality reference data, the rosbag function in the ROS system is used to collect the GNSS, IMU, and wheel encoder data of the target vehicle. At the same time, the unmanned vehicle platform also synchronously records the raw sensor data of the image and radar point cloud. Figure 4 Two outdoor scenes of target pose estimation experiments are shown.

[0169] Target detection results and point cloud projection diagram are as follows Figure 5 As shown in , the target point cloud within the anchor frame interval is filtered out according to the anchor frame coordinates in the target detection result and the initial radar point cloud data. At this time, the extracted target point cloud data contains the real target point cloud and the background point cloud, as shown in Figure 6 Then, a clustering algorithm is used to remove the background point cloud from the point cloud set. The denoising result is shown in (a). Figure 6 As shown in (b).

[0170] The proposed method and the traditional ICP algorithm were used to perform point cloud registration on the denoised target point cloud cluster, achieving high-precision pose estimation of the target object. Table 1 shows the estimation results for different scenarios. The translational part uses GNSS data as the reference ground truth, while the rotational part uses odometry angle information obtained by fusion of the IMU and wheel encoders via an EKF as a reference. By comparing the algorithm's estimation results with the reference ground truth, the pose estimation error is calculated, as shown in Table 2.

[0171] Table 1

[0172]

[0173] Table 2

[0174]

[0175] The experimental results in Tables 1 and 2 show that the proposed method outperforms the traditional ICP algorithm in both pose estimation accuracy and computational efficiency. These experimental results demonstrate that the target pose estimation algorithm that integrates visual and lidar information not only achieves higher levels of position and pose estimation accuracy but also exhibits enhanced real-time performance, providing a superior solution for practical applications.

[0176] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A target pose estimation method based on vision and radar, characterized by: The following steps are involved: 1) Calibrate the camera and radar used to collect navigation environment information. The calibration includes camera parameter calibration and radar and camera joint calibration. The calibration obtains the camera's intrinsic parameters and the transformation matrix between the radar and camera. 2) Train the YOLOv5s model and feed the images captured by the camera into the trained YOLOv5s model to obtain the category and anchor frame coordinates of the target object; 3) Using the camera intrinsic parameters obtained in step 1) and the radar-to-camera transformation matrix, a unified coordinate system is constructed to achieve spatial synchronization of visual and radar information. A timestamp-based synchronization strategy is used to achieve temporal synchronization of visual and radar information, ultimately achieving spatiotemporal synchronization of visual and radar data. 4) Extracting the point that best matches the target's position from the radar point cloud set as the centroid point, and converting the three-dimensional point cloud coordinates of the centroid point into the navigation coordinate system to obtain the target's initial position; 5) Constructing a KD tree: Extract the target point cloud from the radar point cloud, then extract the principal direction of the target point cloud. Next, calculate the stopping condition for tree construction, and determine whether to stop the tree construction process based on the size of the point cloud bounding box. Then, construct a plane perpendicular to the principal direction as a splitting plane, divide the target point cloud into sub-point cloud sets along the splitting plane, and then use the sub-point cloud sets as input to establish a hierarchical structure to complete the construction of a directed KD tree. 6) When a point cloud to be matched appears in the target point cloud set extracted from the subsequent radar frame, the KD tree is searched to find the point cloud in the leaf node that matches the point to be matched. The ICP algorithm with point-to-surface constraints is used to calculate the optimal pose transformation that needs to be satisfied when aligning the matching target point clouds in the two previous and subsequent radar frames, thereby estimating the target pose. Combined with the initial position of the target obtained in step 4), the target pose is projected from the object coordinate system to the navigation coordinate system to obtain the pose estimation result of the target in the navigation coordinate system.

2. The method for target pose estimation based on vision and radar according to claim 1, wherein: The construction of a unified coordinate system to achieve spatial synchronization of visual and radar information in step 3) includes: 1) According to the homogeneous coordinates of a point in the radar coordinate system [X L ,Y L ,Z L ,1] T And the transformation matrix T between the radar and the camera constructs the transformation relationship from the radar coordinate system to the camera coordinate system: Among them, R in the transformation matrix T is the rotation matrix, and t is the translation; 2) The camera coordinate system is divided into the image pixel coordinate system and the image physical coordinate system. According to the camera's intrinsic parameter f x ,f y ,c x ,c y Establish the transformation relationship between the image physical coordinate system and the image pixel coordinate system: According to the derivation results of equations (1) and (2), all point clouds in the radar coordinate system are converted to the camera image pixel coordinate system. The transformation relationship is as follows: Where: (X p ,Y p ,Z p ) represents the point cloud in the radar coordinate system, and (u, v) represents the coordinates of the point cloud projected to the image pixel coordinate system; The time synchronization of visual and radar information achieved by adopting a timestamp-based synchronization strategy as described in step 3) includes: designing independent buffer containers to store the image data collected by the camera and the point cloud data collected by the radar, respectively, and using the timestamp of the image data as the synchronization reference; when the point cloud data arrives, retrieving the point cloud record closest to the current image timestamp from the corresponding buffer container to achieve alignment of the two data in the time domain.

3. The method for target pose estimation based on vision and radar according to claim 2, wherein: Extracting the point that best matches the target object's position from the radar point cloud set as the target object's centroid point in step 4) includes: 1) According to the coordinates of the two diagonal points (x1, y1) and (x2, y2) of the target object’s anchor frame, the centroid coordinates (x c ,y c ): 2) Calculate the Euclidean distance between the projection point of the radar point cloud data and the centroid in the image pixel coordinate system according to the following formula: Where (u i ,v i ) is the coordinate of the radar point cloud data projection point in the image pixel coordinate system; 3) Select the projection points from the radar point cloud data so that d i The smallest projection point (u min ,v min ), (u min ,v min )=argmind i (6) If the projection point (u min ,v min ) satisfies the following constraints: x1≤u min ≤x2, y1≤v min ≤y2 (7) Then the projection point (u min ,v min ) in the anchor frame, and then the projection point (u min ,v min ) in the radar point cloud set (x i ,y i ,z i ) as the centroid point (x target ,y target ,z target ): (x target ,y target ,z target )=(x i ,y i ,z i ) (8) In step 4), the three-dimensional point cloud coordinates of the centroid are converted to the navigation coordinate system through the following transformation relationship: Among them, T LN is the transformation matrix from radar to navigation coordinate system, (x N ,y N ,z N ) is the position coordinate of the target object in the navigation coordinate system.

4. The method for target pose estimation based on vision and radar according to claim 3, wherein: The point cloud collection of the extracted target object in step 5) includes: After synchronizing the time of visual and radar information, all point clouds in the radar coordinate system at the current time point are projected into the camera's image pixel coordinate system according to the following formula to complete spatiotemporal synchronization: L={P i (x i ,y i ,z i )∣i=1,2,…,N} (10) Let the two-dimensional projection coordinate set of the point cloud in the image pixel coordinate system be X, and we get: All projected point clouds can be filtered using the anchor frame coordinates B to obtain the point cloud set C. The anchor frame coordinates B are: B=[u min ,v min ,u max ,v max ] (12) Where (u min ,v min ) represents the pixel coordinates of the upper left corner of the anchor box, (u max ,v max ) represents the pixel coordinates of the lower right corner of the anchor box; the point cloud set C is: C={P i ∈L∣u min ≤u i ≤u max ,v min ≤v i ≤v max } (13) Then, the background point cloud in the point cloud set C is removed to obtain the target object point cloud set D = {p1, p2, ..., p n }; The main direction of the target point cloud set extracted in step 5) includes: Use the following formula to calculate the centroid of the target point cloud set D With the covariance matrix Σ: Perform eigenvalue decomposition on the covariance matrix Σ to obtain the eigenvalue λ i The corresponding eigenvector v i , expressed using formula (15); the eigenvector corresponding to the largest eigenvalue is the main direction of the target point cloud set E = [v1, v2, v3] (15); The tree building stop condition calculation described in step 5) is then performed to determine whether to stop the tree building process based on the size of the point cloud bounding box, including: The point cloud set D={q1,q2,…,q n The data points in} are connected by the feature matrix E and the centroid Mapped to a new coordinate system with the main direction as one of the coordinate axes, the transformed point cloud set is recorded as D'={q1,q2,…,q n }, the mapping process is described as follows: For each dimension j∈{X,Y,Z}, extract the minimum and maximum boundaries of the point cloud bounding box, and use formula (17) to express it: Where: b min represents the minimum boundary, b max Indicates the maximum boundary; Calculate the bounding box size b box for: If b z If it is less than the set threshold, it means that the point clouds in the bounding box are in the same plane, and the tree building process is stopped; The step 5) of dividing the target point cloud set by the splitting plane to obtain the sub-point cloud set includes: The splitting plane will be used to divide the target point cloud set D into the left sub-point cloud set D l and the right sub-point cloud set D r , the sub-point cloud set is represented as follows: In the formula Indicates the current point p i The centroid point of the point cloud collection The offset between Then, the left and right sub-point cloud sets are used as input to construct the left and right sub-trees respectively, and a hierarchical structure is established to complete the construction of the directed KD tree.

5. The method for target pose estimation based on vision and radar according to claim 4, characterized in that: The step 6) of searching the KD tree to find a point cloud in a leaf node that matches the point to be matched includes: Start the recursive query from the root node r of the tree and complete the tree traversal. The judgment criterion is shown in formula (20). Iterate the query until the leaf node is reached. The point cloud in the leaf node is considered to be the point to be matched with the point p. cur Matched point cloud:

6. The method for target pose estimation based on vision and radar according to claim 5, characterized in that: In step 6), the ICP algorithm with point-to-plane constraints is used to calculate the optimal pose transformation that needs to be satisfied when aligning the matching target point clouds in the two radar frames, including: The optimization criterion is to minimize the normal distance from the source point cloud to the local plane formed by the target point cloud. The error function is shown in (21): The source point cloud is the target point cloud from the previous radar frame, and the target point cloud is the point cloud that matches the source point cloud in the next radar frame. i Represents the three-dimensional coordinates of the source point cloud, q i Represents the three-dimensional coordinates of the target point cloud, Represents point q i The unit normal vector of the plane, R opt and t opt That is the optimal posture transformation that needs to be obtained through the ICP algorithm; After setting the maximum number of iterations and the convergence termination condition, the point-to-plane ICP algorithm iteratively estimates the optimal rigid body transformation by minimizing the projection error in the normal direction of the tangent plane of each point in the source point cloud to its corresponding target point cloud in each iteration; the final estimated target object pose is given in the form of a rigid body transformation matrix: Then, based on the initial position (x N ,y N ,z N ), combined with the relative transformation T obtained by the current point cloud registration obj , complete the pose transformation of the target object from its own object coordinate system to the navigation coordinate system, as shown in formula (23): The last T nav That is the pose estimation result of the target object in the navigation coordinate system.

Images