A pose estimation method for unmanned vehicle tracking moving targets based on multimodal

Abstract

The present invention discloses a multimodal pose estimation method for tracking a moving target by an unmanned vehicle. The method comprises the following steps: processing point cloud data to obtain a potential motion region and the initial poses of the moving target at the previous and current moments in a camera coordinate system; projecting the potential motion region into image data to obtain a projection region; processing the projection region to obtain a first ORB keypoint pair; processing the projection region using a two-stage front-end odometer to obtain a first camera pose at the current moment; calculating the scene flow of the first ORB keypoint pair to determine the moving target; and then determining a global initial relative velocity based on the camera pose at the previous moment, the first camera pose at the current moment, the moving target pose at the previous moment, and the initial pose; and determining a global first relative velocity based on the global initial relative velocity, the current projection point coordinates, the current first camera pose, and the current first ORB keypoint coordinates. The present invention accurately estimates the six-dimensional pose and velocity of the moving target.

Description

Technical Field

[0001] The present invention relates to the field of navigation, and more particularly to a multimodal pose estimation method for unmanned vehicles tracking moving targets. Background Art

[0002] Simultaneous Localization and Mapping (SLAM) is a key technology used in the autonomous driving field. SLAM addresses the challenges of traditional Global Navigation Satellite System (GNSS) positioning and navigation, such as low positioning accuracy, susceptibility to atmospheric influences, and inability to support digital maps. It enables high-precision vehicle positioning and digital mapping. The basic SLAM algorithm estimates the vehicle's motion based on sensor observations and constructs an incremental map along the way, enabling autonomous vehicles to perform autonomous positioning, mapping, path planning, and obstacle avoidance.

[0003] However, existing SLAM technologies eliminate moving objects, making them incapable of supporting advanced applications such as intention recognition of traffic participants and multi-target tracking and prediction in future autonomous driving scenarios. Furthermore, the pose estimation of moving objects in SLAM is based solely on feature point tracking, which only yields relative poses relative to the initial moment, but not the 6D pose in the real world.

[0004] Therefore, there is an urgent need for a method for estimating the pose of moving targets so that autonomous vehicles can accurately estimate the pose and speed of surrounding moving targets, achieve safer autonomous driving and increase inter-vehicle communication. Summary of the Invention

[0005] The present invention is proposed based on the above-mentioned requirements of the prior art. The technical problem to be solved by the present invention is to provide a multi-modal pose estimation method for unmanned vehicle tracking a moving target to accurately estimate the pose and speed of the moving target.

[0006] In order to solve the above problems, the present invention is implemented by adopting the following technical solutions:

[0007] A method for estimating the pose of a moving target by an unmanned vehicle based on multimodality, the method comprising: the unmanned vehicle obtains point cloud data from a laser radar and image data from a monocular camera; the point cloud data is processed using a target detection model to obtain a potential motion area, and the initial poses of the moving target at the previous moment and the current moment in the camera coordinate system; the potential motion area is projected into the image data to obtain a projection area composed of projection points; and the first ORB key point feature is extracted from the projection area; the first ORB key points of two adjacent frames of images are matched based on optical flow estimation to obtain a first ORB key point pair; the projection area is divided into a target area and a background area using a convex hull generation algorithm; the background area is processed by a visual odometer to obtain a first relative pose of the camera at the current moment relative to the previous moment; the first relative pose is used as the initial prior pose of the laser odometer, and the background area is processed by the laser odometer to obtain a first camera pose at the current moment; based on the first camera pose at the current moment and the first ORB key point at the previous moment, The coordinates in the global coordinate system and the coordinates of the first ORB key point in the camera coordinate system at the current moment are used to determine the scene flow of the matching first ORB key point pair; based on the scene flow, it is determined whether the target in the target area is a moving target; if it is a moving target, the global initial relative velocity of the moving target at the current moment relative to the previous moment in the global coordinate system is determined based on the camera pose at the previous moment, the first camera pose at the current moment, the pose of the moving target in the global coordinate system at the previous moment, and the initial poses of the moving target in the camera coordinate system at the previous moment and the current moment; based on the global initial relative velocity of the moving target, the coordinates of the projection point corresponding to the first ORB key point at the current moment, the first camera pose at the current moment and the coordinates of the first ORB key point in the global coordinate system at the current moment, a first reprojection error function is constructed to obtain the optimized global first relative velocity corresponding to the global initial relative velocity by iteratively solving the first reprojection error function through nonlinear optimization, where the global first relative velocity is the coordinate in six-dimensional space.

[0008] Optionally, the point cloud data is processed using a target detection model to obtain a potential motion area, including: processing the point cloud data using a target detection model to obtain multiple detection frames; judging whether the confidence of the detection frame is greater than a preset threshold, if it is greater than the preset threshold, the corresponding detection frame is a potential motion area.

[0009] Optionally, the background area is processed by visual odometry to obtain a first relative pose of the camera at the current moment relative to the previous moment, including: extracting second ORB key point features of the background area, and matching the second ORB key points extracted from two adjacent frames of images based on optical flow estimation; constructing a second reprojection error function based on the camera pose at the previous moment, the coordinates of the second ORB key point at the previous moment in the camera coordinate system, the coordinates of the second ORB key point at the previous moment in the global coordinate system, the coordinates of the projection point corresponding to the matched second ORB key point at the current moment, and the initial camera pose at the current moment; iteratively solving the second reprojection error function by the LM algorithm to obtain the second camera pose at the current moment; and determining the first relative poses of adjacent frames based on the camera pose at the previous moment and the second camera pose at the current moment.

[0010] Optionally, the first relative posture is used as the initial prior posture of the laser odometry, and the laser odometry is used to process the background area to obtain the first camera posture at the current moment, including: performing geometric feature extraction on the point cloud data corresponding to the background area to obtain an edge point feature set and a plane point feature set; based on the first relative posture, the edge point feature set and the plane point feature set at the current moment are mapped to the laser radar coordinate system at the previous moment; each point in the edge point feature set at the current moment is respectively separated from the edge line formed in the edge point feature set at the previous moment by the edge line with the smallest distance, and obtaining the first distance between the edge point at the current moment and the edge line at the previous moment. A first matching relationship is formed; each point in the plane point feature set at the current moment is respectively separated from the plane formed by the plane point feature set at the previous moment by the plane with the smallest distance, and a second matching relationship is obtained between the plane point at the current moment and the plane at the previous moment; based on the first matching relationship and the second matching relationship, a third reprojection error function is constructed to minimize the distance between the edge point and the edge line and the distance between the plane point and the plane, and the third reprojection error function is iteratively solved by the two-step LM algorithm to determine the second relative pose of the adjacent frames; based on the second relative pose and the camera pose at the previous moment, the first camera pose at the current moment is determined.

[0011] Optionally, the projection area is divided into a target area and a background area using a convex hull generation algorithm, including: starting from the first point at the bottom of the leftmost points of all projection points, taking the first point as the pole, calculating the polar angles of points other than the first point; sorting the projection points counterclockwise from large to small according to the polar angles; determining the dividing line with the first and second projection points after sorting; traversing the projection points starting from the third projection point after sorting, if the projection point is on the left side of the straight line, then the projection point is a point on the convex hull, that is, a pixel belonging to the target area; otherwise, the projection point is not a point on the convex hull, that is, a point belonging to the background area; all projection points not on the convex hull constitute the background area.

[0012] Optionally, feature extraction is performed on key points, where the key points include a first ORB key point and a second ORB key point, including: converting image data into grayscale image data; processing the grayscale image data using a constructed multi-resolution pyramid model to obtain a multi-scale image; processing the multi-scale image using a grayscale centroid algorithm to obtain multiple FAST corner points; performing non-maximum suppression processing on the FAST corner points, and calculating Harris response values on the processed FAST corner points, and selecting the top N corner points with the largest Harris response values as key points.

[0013] Optionally, geometric features are extracted from the point cloud data corresponding to the background area to obtain an edge point feature set and a plane point feature set, including: projecting the point cloud data corresponding to the background area to obtain a distance image, wherein each pixel in the distance image corresponds one-to-one to the point cloud data, and the value of the pixel represents the distance from the point cloud data to the center of the lidar; identifying and extracting the distance image to obtain ground points; filtering the distance image, including: dividing the distance image into multiple clusters; determining whether the number of pixels in each cluster is less than a third threshold, and if so, deleting the corresponding cluster; horizontally dividing the filtered distance image to obtain multiple sub-images of the same size, and calculating the curvature value of each row of pixels in each sub-image; sorting the curvature values, and taking the pixel corresponding to the maximum curvature value in each sub-image as the edge feature point, and all edge feature points constitute an edge point feature set, and taking the pixel corresponding to the minimum curvature value and some ground points in each sub-image as plane feature points, and all plane feature points constitute a plane feature point set.

[0014] Optionally, a factor graph is constructed to obtain a joint optimization function to optimize the first camera pose at the current moment and the optimized global first relative velocity.

[0015] Optionally, the second ORB key point also comes from feature extraction of a projection area of a static target.

[0016] Optionally, the first reprojection error function expression includes:

[0017]

[0018] in, express Reprojection error, Represents the corresponding projection point coordinates of the first ORB key point matching the i-th first ORB key point at time k-1, represents the inverse of the first camera pose at the current moment, represents the global initial relative velocity of the moving target, is the global coordinate system coordinate of the first ORB key point i at time k-1, represents the global first relative velocity, ρ h represents the Huber kernel function, express The transpose of .

[0019] Compared to existing technologies, this invention provides a multimodal pose estimation method for tracking moving targets by unmanned vehicles. First, a two-stage front-end odometry is constructed, improving its robustness through a two-stage solution of visual and geometric features. Then, a motion tracking module is constructed, leveraging 3D target detection information and optical flow estimation to accurately identify, track, and estimate the motion of dynamic rigid objects in the scene, precisely estimating the six-dimensional pose and velocity of the moving target. Furthermore, a back-end joint optimization algorithm is provided, integrating information about the moving target and static background into a unified optimization framework based on multi-factor graph optimization, improving computational efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the embodiments of this specification or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in the embodiments of this specification. For ordinary technicians in this field, other drawings can also be obtained based on these drawings.

[0021] Figure 1 This is a flowchart of a method for estimating the position and posture of a moving target tracked by an unmanned vehicle based on multimodality provided in this embodiment;

[0022] Figure 2 This is a system architecture diagram provided by this embodiment;

[0023] Figure 3 Schematic diagram of the relationship between key points, projection points and camera poses provided in this embodiment;

[0024] Figure 4 This is a schematic diagram of scanning information at time k-1 and time k before feature matching provided by this embodiment;

[0025] Figure 5 Schematic diagram of feature matching at time k-1 and time k provided in this embodiment;

[0026] Figure 6 This is a schematic diagram of the two-stage odometer designed in this embodiment;

[0027] Figure 7 It is the factor graph of the back-end joint optimization provided by this embodiment. DETAILED DESCRIPTION

[0028] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of the present invention.

[0029] To facilitate understanding of the embodiments of the present invention, the following will be further explained with reference to specific embodiments in conjunction with the accompanying drawings. The embodiments do not constitute a limitation on the scope of protection of the present invention.

[0030] Example 1

[0031] At present, it is very important for autonomous vehicles to perceive and understand the surrounding environment during driving. However, the existing technology for autonomous driving tracking moving target vehicles can only obtain the three-dimensional posture of the moving target vehicle, that is, position information, and cannot or finds it difficult to cope with complex scenarios.

[0032] Therefore, this embodiment provides a multi-modal pose estimation method for unmanned vehicles tracking moving targets, so as to obtain the six-dimensional pose of the moving target vehicle, i.e., position and attitude information, and then based on more accurate relative six-dimensional velocity information, better complete the autonomous driving task and improve the safety and reliability of the autonomous driving vehicle. The process of the pose estimation method is as follows: Figure 1 and Figure 2 Shown, including:

[0033] S1: The unmanned vehicle obtains point cloud data from the lidar and image data from the monocular camera.

[0034] This embodiment involves four coordinate systems: the camera coordinate system, the vehicle coordinate system, the radar coordinate system, and the global coordinate system. Assuming the camera coordinate system and the vehicle coordinate system coincide, the camera coordinate system at the initial moment serves as the global coordinate system. The camera pose at time k represents the relative camera pose relative to the initial moment, and the radar coordinate system and the camera coordinate system are convertible between them. During the calculation process in this embodiment, the relative pose of the radar coordinate system is converted to the camera coordinate system. The camera pose described below defaults to the camera pose in the global coordinate system.

[0035] The camera pose is represented by Lie algebra, which is expressed as ξ=[t x , t y , t z ,θ roll ,θ pitch ,θ yaw ] T , where ξ represents the camera pose, t x Indicates the x-axis coordinate value, ty Indicates the y-axis coordinate value, t z Indicates the z-axis coordinate value, θ roll Indicates the x-axis rotation value, θ pitch Indicates the y-axis rotation value, θ yaw Indicates the z-axis rotation value.

[0036] Assume that all point clouds are represented as P = {P1, ..., P a ,...,P m}, where m represents the number of point clouds, P a Represents the a-th point cloud.

[0037] S2: Use the target detection model to process the point cloud data to obtain the potential motion area and the initial pose of the moving target at the previous and current moments in the camera coordinate system; project the potential motion area into the image data to obtain a projection area composed of projection points; and extract the first ORB key point features of the projection area; match the first ORB key points of two adjacent frames of images based on optical flow estimation to obtain the first ORB key point pair.

[0038] The point cloud data is processed using an object detection model to obtain potential motion regions, including: processing the point cloud data using the 3D object detection model Dense Voxel Fusion to obtain multiple detection boxes; for details on the 3D object detection model Dense Voxel Fusion, see https: / / doi.org / 10.48550 / arXiv.2203.00871. A determination is made as to whether the confidence level of the detection box exceeds a preset threshold. If so, the corresponding detection box is identified as a potential motion region. In this embodiment, the preset threshold is set to 0.7. Based on the spatial distribution of the detection boxes of the potential target region, the point cloud within the potential target region is identified as a region containing a target.

[0039] The detection frame can separate the static background and the potential moving target, and segment the potential moving target; however, not all targets are moving. In order to reduce unnecessary computational redundancy, this embodiment only performs motion tracking and velocity estimation on real moving targets. Therefore, the scene flow method is used to screen out the real moving targets, wherein the scene flow vectors of the static background and the static moving target are close to zero, while the scene flow vector of the real moving target is not zero.

[0040] In this step, key points are extracted by extracting the Oriented FAST key points from the image ORB (Oriented FAST and Rotated BRIEF) features, without extracting descriptors, to improve computational efficiency. This embodiment includes two types of key points, namely, first ORB key points and second ORB key points. The first ORB key points are extracted from the projection area of the potential moving target, and the second ORB key points are extracted from the static background. The specific extraction steps include:

[0041] Convert image data to grayscale image data.

[0042] The constructed multi-resolution pyramid model is used to process the grayscale image data to obtain multi-scale images.

[0043] The grayscale centroid algorithm is used to process the multi-scale image and obtain multiple FAST corner points.

[0044] Non-maximum suppression processing is performed on the FAST corner points, and Harris response value calculation is performed on the processed FAST corner points, and the first N corner points with the largest Harris response values are selected as key points.

[0045] Since real moving targets usually appear in a small range in a scene, the accuracy of extracting sparse ORB features for motion tracking is poor. Therefore, in this embodiment, multiple first ORB key points with a relatively high density are extracted from the pixel area of the real moving target.

[0046] Furthermore, the optical flow prediction network PWC-Net is used to generate an optical flow estimation map. Based on the optical flow estimation map, the movement of key points between adjacent images is tracked to perform key point matching.

[0047] S3: Use the convex hull generation algorithm to divide the projection area into the target area and the background area.

[0048] Starting from the first point at the bottom of the leftmost points of all projection points, with the first point as the pole, calculate the polar angles of all points except the first point; sort the projection points counterclockwise from large to small according to the polar angle; determine the dividing line with the first and second projection points after sorting; traverse the projection points starting from the third projection point after sorting. If the projection point is on the left side of the straight line, the projection point is a point on the convex hull, that is, a pixel belonging to the target area; otherwise, the projection point is not a point on the convex hull, that is, a pixel belonging to the background area.

[0049] S4: Processing the background area through the visual odometry to obtain the first relative pose of the camera at the current moment relative to the previous moment.

[0050] Specifically, this step includes:

[0051] Step 1: Extract the second ORB key point features of the background area and match the second ORB key points extracted from two adjacent frames based on optical flow estimation.

[0052] In this step, assuming that there are n pairs of matched second ORB key points, the three-dimensional spatial coordinates of the second ORB key points in the camera coordinate system at the previous moment are obtained based on the depth information obtained by the depth completion network, and then the relative error of the current moment to the previous moment is estimated by minimizing the three-dimensional spatial point projection deviation.

[0053] Step 2: Construct the second reprojection error function based on the camera pose at the previous moment, the coordinates of the second ORB key point in the camera coordinate system at the previous moment, the coordinates of the second ORB key point in the global coordinate system at the previous moment, the coordinates of the projection point corresponding to the second ORB key point matched at the current moment, and the initial camera pose at the current moment.

[0054] Specifically, the coordinates of the second ORB key point in the global coordinate system at the previous moment are determined based on the camera pose at the previous moment and the coordinates of the second ORB key point in the camera coordinate system at the previous moment. The expressions include: in, Indicates the coordinates of the second ORB key point in the global coordinate system at the previous moment. Indicates the camera pose of the visual odometry at the last moment, Indicates the coordinates of the second ORB key point of the visual odometry at the previous moment in the camera coordinate system.

[0055] Based on the coordinates of the second ORB key point in the global coordinate system at the previous moment, the coordinates of the projection point corresponding to the second ORB key point matched at the current moment, and the camera pose at the current moment, a second reprojection error function is constructed.

[0056] Assume that the projection pixel coordinates of the spatial point at the current moment are Set the camera pose Then the relationship between pixel coordinates and spatial point coordinates is:

[0057]

[0058] The above formula implies the transformation from homogeneous coordinates to non-homogeneous coordinates, and its schematic diagram is as follows Figure 3 As shown. Since the initial relative pose of the camera is unknown and there are noise errors in the optical flow calculation and key point matching, the above formula cannot be established for all key points at the same time. Therefore, this embodiment solves the optimal pose of the camera by minimizing the second reprojection error. The expression of the second reprojection error function includes:

[0059]

[0060] in, Represents the output of the visual odometry Reprojection error, Indicates the projection point coordinates corresponding to the second ORB key point matched at the current moment, represents the current second camera pose output by the visual odometry, i represents the key point number, n represents the number of second ORB key point matches, and ρ h Represents the Huber kernel function, argmin() represents the minimization function, express The transpose of .

[0061] Step 3: Iterate and solve the second reprojection error function through the LM algorithm to obtain the second camera pose at the current moment; determine the first relative pose of the adjacent frames based on the camera pose at the previous moment and the second camera pose at the current moment. Its expression includes:

[0062]

[0063] in, represents the first relative pose of adjacent frames output by the visual odometry, Represents the camera pose at the last moment output by the visual odometry.

[0064] S5: Using the first relative pose as the initial prior pose of the laser odometry, and using the laser odometry to process the background area to obtain the first camera pose at the current moment.

[0065] S500: Extracting geometric features from the point cloud data corresponding to the background area to obtain an edge point feature set and a plane point feature set.

[0066] The point cloud data corresponding to the background area is projected to obtain a distance image. Each pixel in the distance image corresponds one-to-one to the point cloud data, and the value of the pixel represents the distance from the point cloud data to the center of the lidar; the ground point is identified and extracted from the distance image.

[0067] The range image is filtered, including: dividing the range image into a plurality of clusters; determining whether the number of pixels in each cluster is less than a third threshold, and if the number is less than the third threshold, deleting the corresponding cluster.

[0068] The range image is obtained by horizontal rotation scanning of a multi-line laser radar, and has vertical resolution and horizontal resolution. Each line of data in the range image is obtained by scanning one of the radar lines.

[0069] The distance image is horizontally divided into multiple sub-images of the same size. The curvature value of each row of pixels in each sub-image is calculated and sorted. The pixel point corresponding to the maximum curvature value in each sub-image is used as the edge feature point. All edge feature points constitute the edge point feature set. The pixel point corresponding to the minimum curvature value in each sub-image and some ground points are used as plane feature points. All plane feature points constitute the plane feature point set.

[0070] The curvature value calculation includes:

[0071]

[0072] Among them, c a Indicates the curvature value of the pixel corresponding to the point cloud numbered a, r a Represents the point cloud P numbered a a The Euclidean distance from the corresponding pixel to the sensor center, r b Represents the point cloud P numbered b b The Euclidean distance from the corresponding pixel to the center of the sensor, S represents the set of pixels in the same row in the distance image.

[0073] S510: Based on the first relative pose, the edge point feature set and the plane point feature set at the current moment are mapped to the lidar coordinate system at the previous moment.

[0074] In order to overcome the limitation of over-reliance on visual optical flow information and improve the robustness of the system, this embodiment uses geometric features to estimate the motion between two consecutive scans of the lidar sensor. Specifically: the edge point feature set scanned at the last moment is used to estimate the motion between two consecutive scans of the lidar sensor. and plane point feature sets Find the edge point feature set at the current moment and plane point feature sets Matching features, because point cloud geometric features do not have auxiliary information such as descriptors and optical flow estimation, so basically can only be estimated based on the uniform motion model and Euclidean distance matching relationship, that is, the iterative closest point algorithm. However, if the vehicle itself moves at a high speed and there is no good prior motion information, feature matching based on the principle of closest distance will produce huge deviations, which will seriously affect the accuracy of pose estimation. Therefore, this embodiment executes the visual odometry and laser odometry in parallel through multiple processes, and uses the results of the visual odometry as the initial prior pose of the laser odometry.

[0075] In this step, according to the first relative pose of adjacent frames Transform the edge point feature set and plane point feature set at the current moment into the laser radar coordinate system at the previous moment to obtain the mapped edge point feature set and plane point feature sets

[0076] S520: Obtain the first matching relationship between the edge point at the current moment and the edge line at the previous moment by comparing each point in the edge point feature set at the current moment with the edge line formed in the edge point feature set at the previous moment with the edge line with the smallest distance therebetween; obtain the second matching relationship between the plane point at the current moment and the plane at the previous moment by comparing each point in the plane point feature set at the current moment with the plane formed in the plane point feature set at the previous moment with the plane with the smallest distance therebetween.

[0077] In this step, for the mapped edge point feature set The edge points in the previous moment are the edge point feature set Determine the edge line with the closest Euclidean distance; for the mapped plane point feature set The plane points in the previous moment are the plane point feature set Determine the plane with the closest Euclidean distance. According to the inverse mapping, the edge point feature set at the current moment is obtained. and the edge point feature set at the previous moment The matching relationship of the plane point feature set at the current moment and the plane point feature set at the previous moment Matching relationship. Figure 4 and Figure 5 As shown in FIG, the local result of a feature matching relationship is described, where the targets numbered (1), (1') and (1") are the scan information at time k-1. After the camera moves, the targets numbered (2), (2') and (2") are the scan information at time k. The rectangle in (1) represents the corresponding plane of the plane point matching numbered (2), the rectangle in (1') represents the corresponding plane of the plane point matching numbered (2'), and the line in (1") represents the corresponding edge line of the edge point matching numbered (2")

[0078] The improved feature matching method of this embodiment can not only solve the problem of poor matching accuracy caused by excessive motion, but also narrow the potential search range to improve matching efficiency.

[0079] S530: Based on the first matching relationship and the second matching relationship, construct a third reprojection error function to minimize the distance between edge points and edge lines and the distance between plane points and planes, iteratively solve the third reprojection error function through a two-step LM algorithm to determine the second relative pose of adjacent frames; based on the second relative pose and the camera pose at the previous moment, determine the first camera pose at the current moment.

[0080] The expressions in this step include:

[0081]

[0082] in, Represents the relative position of the camera output by the laser odometry from time k-1 to time k based on the camera coordinate system, express Reprojection error, σ represents the distance function, Represents the second relative pose of adjacent frames.

[0083] The second relative pose of the adjacent frames is obtained by iteratively solving the above equation through two-step LM Including: determining the z-axis coordinate value, x-axis rotation value, and y-axis rotation value in the second relative posture through the matching relationship between the plane point feature set at the current moment and the plane point feature set at the previous moment, that is, [t z ,θ roll ,θ pitch ] T ; Through the matching relationship between the edge point feature set at the current moment and the edge point feature set at the previous moment, the x-axis coordinate value, y-axis coordinate value and z-axis rotation value in the second relative posture are determined, that is, [t x , t y ,θ yaw ] T .

[0084] The second relative pose corresponding to the starting time to the current time is accumulated to obtain the second relative pose of the current time for the first frame. Since the camera coordinate system at the initial time in this embodiment is used as the global coordinate system, the second relative pose of the current time for the initial time is also the camera pose in the global coordinate system at the current time. That is the output result of the laser odometry.

[0085] In addition, if Figure 6 As shown, this embodiment uses a multi-process mode to simultaneously run the pose calculation modules of the visual odometry and laser odometry stages, and uses process communication to synchronize the two modules. The laser odometry will set a maximum waiting time. If the first relative pose of the adjacent frame of the visual odometry is not received before the timeout, the laser odometry based on the motion smoothing model will be started. When the environmental visual and geometric features are sufficient, the sensor is working normally, and the system is running stably, the static geometric and texture information can be fully utilized to obtain the second relative pose of the adjacent frame, and then determine the first camera pose at the current moment. In the case of missing geometric features or laser radar failure, a degraded visual odometry is obtained, but it can still meet the basic positioning and mapping requirements. In the case of missing visual features and large optical flow estimation errors, a degraded laser odometry is obtained, which can also meet the basic accuracy requirements. This design can improve the ability to cope with complex and changing challenging scenes.

[0086] S6: Determine a scene flow of a matching first ORB keypoint pair based on the first camera pose at the current moment, the coordinates of the first ORB keypoint at the previous moment in the global coordinate system, and the coordinates of the first ORB keypoint at the current moment in the camera coordinate system.

[0087] The scene flow size of all sparse ORB key point pairs in the projection area is calculated. The scene flow calculation formula is as follows:

[0088]

[0089] in, Indicates that the calculation result of the front-end odometer is the output result of the laser odometer, that is, the first camera pose at the current moment, is the global coordinate system coordinate of the first ORB key point i at time k-1, is the observation value of the first ORB key point i in the camera coordinate system at time k.

[0090] S7: Determine whether the target in the target area is a moving target based on the scene flow.

[0091] This step includes: counting the number of first ORB key points whose scene flow is greater than a first threshold among all first ORB key points at a single moment, and determining whether the number of the first ORB key points is greater than a second threshold. If it is greater than the second threshold, the target is a moving target; the single moment includes the previous moment and the current moment.

[0092] If the number of feature points with scene flow values greater than the first threshold is greater than the second threshold, the target is considered to be moving. Otherwise, the target is considered to be stationary. In this embodiment, the second threshold is set to 30%. By combining scene flow and sparse ORB feature points, real moving targets can be identified efficiently and accurately, improving system performance and reducing unnecessary resource waste.

[0093] In addition, in this embodiment This information can be derived not only from static background areas but also from static targets detected by motion. When a large number of targets block the field of view, preventing the system from perceiving sufficient static background information, extracting available information from static targets can aid pose estimation and improve system robustness, such as parked vehicles on the roadside.

[0094] S8: If it is a moving target, determine the global initial relative velocity of the moving target at the current moment relative to the previous moment in the global coordinate system based on the camera pose at the previous moment, the first camera pose at the current moment, the pose of the moving target in the global coordinate system at the previous moment, and the initial poses of the moving target in the camera coordinate system at the previous moment and the current moment.

[0095] In this embodiment, the 3D detection frame is used to provide an initial value for the pose estimation of the moving target. Specifically, the initial 6D pose of the moving target in the camera coordinate system at the previous moment is obtained based on the DenseVoxel Fusion target detection result. And the initial 6D pose of the moving target in the camera coordinate system at the current moment The rough motion state of the moving target can be calculated, and its expressions include:

[0096]

[0097] in, Indicates the position of the moving target from time k-1 to time k based on the moving target in the global coordinate system at the previous moment The relative speed, The calculation result of the front-end odometer at the previous moment.

[0098] according to and the position of the moving target in the global coordinate system at the previous moment Determine the global initial relative velocity of the moving target at the current moment relative to the previous moment in the global coordinate system Its expressions include:

[0099]

[0100] S9: Based on the global initial relative velocity of the moving target, the coordinates of the projection point corresponding to the first ORB key point at the current moment, the first camera pose at the current moment, and the coordinates of the first ORB key point at the current moment in the global coordinate system, a first reprojection error function is constructed to obtain the optimized global first relative velocity corresponding to the global initial relative velocity by iteratively solving the first reprojection error function through nonlinear optimization.

[0101] For the matched first ORB keypoint pair, construct the first reprojection error function to solve the target motion. The expression includes:

[0102]

[0103] in, represents the global initial relative velocity of the moving target, express Reprojection error, Represents the corresponding projection point coordinates of the first ORB key point matching the i-th first ORB key point at time k-1, represents the global first relative velocity, express The transpose of .

[0104] Use nonlinear optimization iterative solution to obtain high-precision moving target velocity estimation in the global coordinate system This joint estimation method, which uses the Dense Voxel Fusion detection box to provide initial values and minimize the reprojection error, improves the computational efficiency of target velocity estimation.

[0105] Furthermore, a factor graph is constructed to obtain a joint optimization function to optimize the first camera pose at the current moment and the optimized global first relative velocity.

[0106] This embodiment can optimize the camera position and moving target speed in a global range in order to obtain more accurate prediction information. Specifically, Figure 7 As shown in the figure, a factor graph is modeled to refine the camera pose and moving target velocity, and then jointly optimize them based on dynamic and static information. The factor graph includes two-stage odometry observation factors, key point projection observation factors, feature point projection observation factors, detection box motion observation factors, feature point motion observation factors, and motion smoothing observation factors. Figure 7 middle Represent the camera poses at time k-2, time k-1 and time k respectively; and Represent the motion target poses at time k-2, time k-1 and time k respectively; 0 P 1 Represents the coordinates of the second ORB key point at time k-2 and time k-1, 0 P 2 Represents the coordinates of the second ORB key point at time k-1 and time k, that is, the coordinates of the static feature point; and Represent the coordinates of the first ORB key point at time k-2, k-1 and k respectively, that is, the dynamic feature point; Indicates the relative speed of the moving target from time k-2 to time k-1 in the global coordinate system, The relative velocity of the moving target from time k-1 to time k in the global coordinate system.

[0107] The two-stage odometry observation factor includes a visual odometry observation factor and a laser odometry observation factor.

[0108] The expression of the visual odometry observation factor includes:

[0109]

[0110] in, Represents the visual odometry observation factor.

[0111] The expression of the laser odometry observation factor includes:

[0112]

[0113] in, Represents the laser odometry observation factor.

[0114] The feature point projection observation factor includes a first ORB key point projection observation factor and a second ORB key point projection observation factor, and the expression of the second ORB key point projection observation factor includes:

[0115]

[0116] in, represents the second ORB keypoint projection observation factor, is the second ORB key point of the i-th static vision at time k O P i In the projection observation of the image, in this embodiment This information comes not only from the static background but also from the identified static moving targets. When a large number of targets block the field of view, preventing the system from perceiving sufficient static background information, extracting available information from the static moving targets, such as parked vehicles on the roadside, can assist in pose estimation and improve system robustness.

[0117] Similarly, the expression of the first ORB key point projection observation factor includes:

[0118]

[0119] in, represents the first ORB keypoint projection observation factor, is the first ORB key point of the moving target at time k Observation in the projection of the image.

[0120] The expression of the detection frame motion observation factor includes:

[0121]

[0122] in, represents the 3D detection box motion observation factor, is the pose of the moving target observed based on the 3D detection frame at time k.

[0123] The expression of the feature point motion observation factor includes:

[0124]

[0125] in, Represents the feature point motion observation factor.

[0126] The expression of the motion smoothing observation factor includes:

[0127]

[0128] in, represents the motion smoothing observation factor, It represents the inverse of the position of the moving target in the global coordinate system from time k-2 to time k-1.

[0129] Let θ = X∪H represent the set of variable nodes to be solved in the factor graph, then the optimal solution is:

[0130]

[0131] in, represents the visual odometry noise covariance matrix, represents the laser odometry noise covariance matrix, represents the second ORB keypoint projection noise covariance matrix, represents the first ORB keypoint projection noise covariance matrix, represents the MSF-Det (Multi-Strategy Fused Detection framework) perception noise covariance matrix, represents the noise covariance matrix of feature point motion observation, Denotes the motion smoothing covariance matrix. The nonlinear least squares problem is solved iteratively using the LM algorithm.

[0132] Compared to existing technologies, this embodiment provides a multimodal pose estimation method for tracking moving targets by unmanned vehicles. First, a two-stage front-end odometry is constructed, which improves the robustness of the odometry through a two-stage solution of visual and geometric features. Then, a motion tracking module is constructed, which uses 3D target detection information and optical flow estimation information to accurately identify, track, and estimate the motion of dynamic rigid objects in the scene, thereby accurately estimating the six-dimensional pose and velocity of the moving target. Furthermore, this embodiment provides a back-end joint optimization algorithm that integrates information about the moving target and static background into a unified optimization framework based on multi-factor graph optimization, improving computational efficiency.

[0133] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A multimodal pose estimation method for unmanned vehicle tracking a moving target, characterized in that: include: The unmanned vehicle obtains point cloud data from the lidar and image data from the monocular camera; The target detection model is used to process the point cloud data to obtain the potential motion area and the initial pose of the moving target in the camera coordinate system at the previous and current moments. Project the potential motion area into the image data to obtain a projection area composed of projection points; and perform the first ORB key point feature extraction on the projection area; Match the first ORB key points of two adjacent frames of images based on optical flow estimation to obtain the first ORB key point pair; The projection area is divided into target area and background area using convex hull generation algorithm; The visual odometry is used to process the background area to obtain the first relative pose of the camera at the current moment relative to the previous moment; the first relative pose is used as the initial prior pose of the laser odometry, and the laser odometry is used to process the background area to obtain the first camera pose at the current moment; Determine the scene flow of the matching first ORB keypoint pair based on the first camera pose at the current moment, the coordinates of the first ORB keypoint at the previous moment in the global coordinate system, and the coordinates of the first ORB keypoint at the current moment in the camera coordinate system; Determine whether the target in the target area is a moving target based on the scene flow; If it is a moving target, the global initial relative velocity of the moving target at the current moment relative to the previous moment in the global coordinate system is determined based on the camera pose at the previous moment, the first camera pose at the current moment, the pose of the moving target in the global coordinate system at the previous moment, and the initial poses of the moving target in the camera coordinate system at the previous moment and the current moment; Based on the global initial relative velocity of the moving target, the coordinates of the projection point corresponding to the first ORB key point at the current moment, the first camera pose at the current moment, and the coordinates of the first ORB key point at the current moment in the global coordinate system, a first reprojection error function is constructed, and the first reprojection error function is solved iteratively through nonlinear optimization to obtain an optimized global first relative velocity corresponding to the global initial relative velocity, where the global first relative velocity is a coordinate in six-dimensional space.

2. The method for estimating the pose of a moving target tracked by an unmanned vehicle based on multimodality according to claim 1, characterized in that: The target detection model is used to process the point cloud data to obtain potential motion areas, including: The point cloud data is processed using the target detection model to obtain multiple detection frames; the confidence level of the detection frame is determined to be greater than a preset threshold. If so, the corresponding detection frame is a potential motion area.

3. The method for estimating the pose of a moving target by an unmanned vehicle based on multimodality according to claim 1, characterized in that: The visual odometry is used to process the background area to obtain the first relative pose of the camera at the current moment relative to the previous moment, including: Extract the second ORB key point features of the background area, and match the second ORB key points extracted from two adjacent frames of images based on optical flow estimation; Based on the camera pose at the previous moment, the coordinates of the second ORB key point in the camera coordinate system at the previous moment, the coordinates of the second ORB key point in the global coordinate system at the previous moment, the coordinates of the projection point corresponding to the second ORB key point matched at the current moment, and the initial camera pose at the current moment, a second reprojection error function is constructed; the second reprojection error function is iteratively solved by the LM algorithm to obtain the second camera pose at the current moment; based on the camera pose at the previous moment and the second camera pose at the current moment, the first relative pose of the adjacent frames is determined.

4. The method for estimating the pose of a moving target tracked by an unmanned vehicle based on multimodality according to claim 1, characterized in that: The first relative pose is used as the initial prior pose of the laser odometry, and the laser odometry is used to process the background area to obtain the first camera pose at the current moment, including: Perform geometric feature extraction on the point cloud data corresponding to the background area to obtain edge point feature sets and plane point feature sets; Based on the first relative pose, the edge point feature set and the plane point feature set at the current moment are mapped to the lidar coordinate system at the previous moment; The first matching relationship between the edge point at the current moment and the edge line at the previous moment is obtained by the edge line with the smallest distance between each point in the edge point feature set at the current moment and the edge line formed by the edge point feature set at the previous moment; the second matching relationship between the plane point at the current moment and the plane at the previous moment is obtained by the plane with the smallest distance between each point in the plane point feature set at the current moment and the plane formed by the plane point feature set at the previous moment; Based on the first and second matching relationships, a third reprojection error function is constructed to minimize the distance between edge points and edge lines, as well as the distance between plane points and planes. The third reprojection error function is iteratively solved using a two-step LM algorithm to determine the second relative pose of adjacent frames. The first camera pose at the current moment is determined based on the second relative pose and the camera pose at the previous moment.

5. The method for estimating the pose of a moving target tracked by an unmanned vehicle based on multimodality according to claim 1, characterized in that: The projection area is divided into the target area and the background area using the convex hull generation algorithm, including: From the first point at the bottom of the leftmost point of all projection points, take the first point as the pole and calculate the polar angle of all points except the first point; Sort the projection points counterclockwise from largest to smallest polar angle; Determine the dividing line by the sorted first projection point and the second projection point; Starting from the third projection point after sorting, traverse the projection points. If the projection point is on the left side of the straight line, the projection point is a point on the convex hull, that is, a pixel belonging to the target area; otherwise, the projection point is not a point on the convex hull, that is, a point belonging to the background area; all projection points not on the convex hull constitute the background area.

6. The method for estimating the pose of a moving target tracked by an unmanned vehicle based on multimodality according to claim 3, characterized in that: Feature extraction is performed on key points, where the key points include a first ORB key point and a second ORB key point, including: Convert image data into grayscale image data; The constructed multi-resolution pyramid model is used to process the grayscale image data to obtain multi-scale images; The grayscale centroid algorithm is used to process the multi-scale image to obtain multiple FAST corner points; Non-maximum suppression processing is performed on the FAST corner points, and Harris response value calculation is performed on the processed FAST corner points, and the first N corner points with the largest Harris response values are selected as key points.

7. The method for estimating the pose of a moving target tracked by an unmanned vehicle based on multimodality according to claim 4, characterized in that: Perform geometric feature extraction on the point cloud data corresponding to the background area to obtain edge point feature sets and plane point feature sets, including: Project the point cloud data corresponding to the background area to obtain a range image, in which each pixel corresponds one-to-one to the point cloud data, and the value of the pixel represents the distance from the point cloud data to the center of the lidar; Recognize and extract the ground points from the distance image; Filtering the range image includes: dividing the range image into a plurality of clusters; determining whether the number of pixels in each cluster is less than a third threshold, and if so, deleting the corresponding cluster; The filtered range image is horizontally divided into multiple sub-images of the same size, and the curvature value of each row of pixels in each sub-image is calculated; The curvature values are sorted, and the pixel points corresponding to the maximum curvature value in each sub-image are taken as edge feature points. All edge feature points constitute an edge point feature set. The pixel points corresponding to the minimum curvature value in each sub-image and some ground points are taken as plane feature points. All plane feature points constitute a plane feature point set.

8. The method for estimating the pose of a moving target tracked by an unmanned vehicle based on multimodality according to claim 1, characterized in that: A factor graph is constructed to obtain a joint optimization function to optimize the first camera pose at the current moment and the optimized global first relative velocity.

9. The method for estimating the pose of a moving target tracked by an unmanned vehicle based on multimodality according to claim 3, characterized in that: The second ORB key point also comes from feature extraction of the projection area of the static target.

10. The method for estimating the pose of a moving target tracked by an unmanned vehicle based on multimodality according to claim 1, characterized in that: The first reprojection error function expression includes: in, express Reprojection error, Represents the corresponding projection point coordinates of the first ORB key point matching the i-th first ORB key point at time k-1, represents the inverse of the first camera pose at the current moment, represents the global initial relative velocity of the moving target, is the global coordinate system coordinate of the first ORB key point i at time k-1, represents the global first relative velocity, ρ h represents the Huber kernel function, express The transpose of .

Images