Method, apparatus and equipment for relative pose measurement of dual-motion platforms in cooperative mode

Abstract

This application relates to a method, apparatus, and device for relative pose measurement of a dual-motion platform in a cooperative mode. The method includes: determining the three-dimensional coordinate data of each infrared marker light on a first platform; acquiring the two-dimensional coordinate data of each infrared marker light in an infrared image acquired by a second platform; calculating candidate poses based on the three-dimensional and two-dimensional coordinate data, and then verifying the correspondence of the candidate poses to obtain pose estimation results; predicting the pose of the current frame based on the pose estimation results of two consecutive infrared images to obtain the current frame pose prediction result, and then verifying the correspondence of the current frame pose prediction result to obtain the current frame estimated pose; iteratively optimizing the current frame estimated pose based on minimizing the reprojection error to obtain a high-precision relative pose between the first and second platforms; and adjusting the relative position of the platforms according to the relative pose to achieve good interaction between the dual-motion platforms.

Description

Technical Field

[0001] This application relates to the field of infrared optical measurement technology, and in particular to a method, apparatus and equipment for measuring the relative pose of a dual-motion platform in a cooperative mode. Background Technology

[0002] Accurate interaction between dual-motion platforms is crucial for the autonomy and intelligence of mechanical systems. Dual-motion platforms exhibit different motion patterns and operate in complex and ever-changing environments. A key technical challenge in achieving accurate interaction between these platforms lies in how to rapidly and precisely measure their relative positions and postures.

[0003] Currently, pose measurement methods for cooperative dual-motion platforms mainly include two types: those based on planar cooperative markers and those based on visible light LEDs and reflective markers. The planar cooperative marker method, using commonly used markers such as AprilTags, ARTags, and ArUco, involves deploying the markers and an image acquisition system on two interactive platforms. It detects the markers using rectangle detection and template matching, and calculates the marker ID and the relative pose parameters between the marker's platform and the image acquisition system. This method requires a large platform area, limiting its application. The pose measurement method based on visible light LEDs and reflective markers suffers significant performance degradation in cluttered lighting environments and is unsuitable for tasks requiring all-day, all-weather operation. Summary of the Invention

[0004] Therefore, it is necessary to provide a method, device, and equipment for measuring the relative pose of two moving platforms in a cooperative mode that is adaptable to all times and all weather conditions and can quickly and accurately calculate the relative pose between the two moving platforms.

[0005] A method for relative pose measurement of a dual-motion platform in a cooperative mode, the method comprising:

[0006] Construct a deployment model for the first platform, the deployment model including several infrared marker lights, and determine the three-dimensional coordinate data of the infrared marker lights on the first platform;

[0007] An image acquisition system is set up on the second platform to acquire infrared images of each infrared marker light. Then, each infrared marker light in the infrared image is located to obtain the two-dimensional coordinate data of each infrared marker light in the infrared image.

[0008] Candidate poses are calculated based on the three-dimensional coordinate data and the two-dimensional coordinate data, and then the correspondence of the candidate poses is verified to obtain the pose estimation result.

[0009] The pose of the current frame is predicted based on the pose estimation results of two consecutive infrared images to obtain the pose prediction result of the current frame. Then, the correspondence of the pose prediction result of the current frame is verified to obtain the estimated pose of the current frame.

[0010] The estimated pose of the current frame is iteratively optimized by minimizing the reprojection error to obtain the relative pose between the first platform and the second platform.

[0011] Adjust the relative positions of the first platform and the second platform according to the relative pose.

[0012] In one embodiment, the infrared marker lights in the deployment model are asymmetrical, non-coplanar, and dispersed.

[0013] In one embodiment, the image acquisition system uses a filter corresponding to the infrared marker light band to acquire infrared images.

[0014] In one embodiment, the feature is that an image acquisition system is set up on a second platform to acquire infrared images of each of the infrared marker lights, and then the infrared marker lights in the infrared images are located to obtain two-dimensional coordinate data of each infrared marker light in the infrared images, including:

[0015] An image acquisition system is set up on the second platform to acquire infrared images of each of the infrared sign lights;

[0016] The infrared image is segmented by thresholding to obtain a binary grayscale image; the binary grayscale image is processed to remove noise and irregular convex hulls, and the center of the infrared marker light spot in the binary grayscale image is located.

[0017] Considering distortion imaging error, the center of the infrared marker light spot is corrected to obtain the two-dimensional coordinates of the infrared marker light in the infrared image.

[0018] In one embodiment, the two-dimensional coordinates of the infrared marker light in the infrared image are represented as follows:

[0019] ;

[0020] In the formula, Indicates the center of the infrared indicator light; , , This represents the phase difference coefficient related to radial distortion; This represents the distance of the point to be processed from the origin of the coordinate system; Normalized image plane coordinates representing the ideal imaging point; , This represents the phase difference coefficient related to tangential distortion.

[0021] In one embodiment, a candidate pose is calculated based on the three-dimensional coordinate data and the two-dimensional coordinate data, and then the candidate pose is verified to obtain a pose estimation result, including:

[0022] The two-dimensional coordinate data and the three-dimensional coordinate data are grouped separately, and a correspondence is established between the two-dimensional coordinate data and the three-dimensional coordinate data. Then, the PNP algorithm is used to calculate and obtain several first candidate poses.

[0023] A threshold is set, and the correspondence of the first candidate pose is judged by the reprojection method to obtain the pose estimation result.

[0024] In one embodiment, the pose of the current frame is predicted based on the pose estimation results of two consecutive infrared images to obtain the current frame pose prediction result. Then, the correspondence of the current frame pose prediction result is verified to obtain the estimated pose of the current frame, including:

[0025] Based on the assumption of uniform motion, the pose estimation results of the current frame are obtained by linear prediction using the pose estimation results of two consecutive frames.

[0026] Based on the current frame pose prediction result, each infrared marker light is matched with the center of the infrared marker light spot, and then the PNP algorithm is used to calculate and obtain several second candidate poses.

[0027] A threshold is set, and the correspondence of the second candidate pose is judged by the reprojection method to obtain the estimated pose of the current frame.

[0028] In one embodiment, the formula for minimizing the reprojection error is expressed as:

[0029] ;

[0030] In the formula, This indicates that the infrared cooperative sign light will be based on the pose matrix. Projected onto the camera image; Indicates the 3D position of the infrared cooperation sign light; This indicates the 2D position detection results of the sign light spot; This indicates the correspondence between the 3D position of the infrared cooperative sign light and the 2D detection result of the sign light imaging spot.

[0031] A relative pose measurement device for a dual-motion platform in a cooperative mode, the device comprising:

[0032] The deployment model construction module is used to construct the deployment model of the first platform. The deployment model includes several infrared marker lights, and the three-dimensional coordinate data of the infrared marker lights on the first platform are determined.

[0033] An infrared image processing module is used to set up an image acquisition system on the second platform, acquire infrared images of each infrared marker light through the image acquisition system, locate each infrared marker light in the infrared image, and acquire two-dimensional coordinate data of each infrared marker light in the infrared image.

[0034] The pose hypothesis verification module is used to calculate candidate poses based on the three-dimensional coordinate data and the two-dimensional coordinate data, and then verify the correspondence of the candidate poses to obtain the pose estimation result.

[0035] The initial pose prediction module is used to predict the pose of the current frame based on the pose estimation results of two consecutive infrared images, to obtain the pose prediction result of the current frame, and then to verify the correspondence of the pose prediction result of the current frame to obtain the estimated pose of the current frame.

[0036] The pose parameter optimization module iteratively optimizes the estimated pose of the current frame based on minimizing the reprojection error to obtain the relative pose between the first platform and the second platform.

[0037] The platform pose adjustment module is used to adjust the relative position of the first platform and the second platform according to the relative pose.

[0038] A computer device includes a memory and a processor, the memory storing a computer program, the processor executing the computer program to implement the steps of any of the methods described above.

[0039] The method, apparatus, and equipment for relative pose measurement of dual-motion platforms under the aforementioned cooperative mode involve: constructing a deployment model of the first platform, which includes several infrared marker lights, and determining the three-dimensional coordinate data of the infrared marker lights on the first platform; setting up an image acquisition system on the second platform to acquire infrared images of each infrared marker light, then locating each infrared marker light in the infrared images to obtain its two-dimensional coordinate data; calculating candidate poses based on the three-dimensional and two-dimensional coordinate data, then verifying the correspondence of the candidate poses to obtain pose estimation results; predicting the pose of the current frame based on the pose estimation results of two consecutive frames of infrared images, obtaining the current frame pose prediction result, then verifying the correspondence of the current frame pose prediction result to obtain the current frame estimated pose; iteratively optimizing the current frame estimated pose based on minimizing the reprojection error to obtain the relative pose between the first and second platforms; and adjusting the relative positions of the first and second platforms based on the relative poses.

[0040] This invention achieves rapid pose calculation through correspondence verification, obtaining an accurate estimated pose for the current frame. Then, using this accurate estimated pose as an initial estimate, iterative optimization is performed by minimizing the reprojection error to obtain a high-precision relative pose between the two motion platforms. This drives the two-motion platform control system to adjust the platform pose, achieving good interaction between the two motion platforms. This invention has the advantages of all-weather operation and strong robustness, enabling rapid and high-precision calculation of the relative pose between the two motion platforms, thus effectively achieving good interaction between them. Attached Figure Description

[0041] Figure 1 This is a flowchart illustrating a method for measuring the relative pose of a dual-motion platform in a cooperative mode, as shown in one embodiment.

[0042] Figure 2 This is a schematic diagram of the composition of a dual-motion platform device in one embodiment;

[0043] Figure 3 This is a structural block diagram of a dual-motion platform relative pose measurement device in a cooperative mode according to one embodiment;

[0044] Figure 4 This is an internal structural diagram of a computer device in one embodiment. Detailed Implementation

[0045] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0046] It should be noted that in this invention, the use of terms such as "first," "second," etc., is for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0047] The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0048] In one embodiment, such as Figure 1 As shown, a method for relative pose measurement of a dual-motion platform in a cooperative mode is provided, including the following steps:

[0049] Step 202: Construct a deployment model of the first platform. The deployment model includes several infrared marker lights. Determine the three-dimensional coordinate data of the infrared marker lights on the first platform.

[0050] It is understandable that the constructed infrared marker light deployment model is obtained by using a total station to mark points after the actual deployment plan is implemented on the dual-dynamic platform. Generally, LED infrared cooperative marker lights with specific wavelengths are selected so that the infrared images acquired by the second platform through filters of the corresponding wavelengths have the characteristic of exceptionally bright marker lights and suppressed backgrounds. During deployment, the LED infrared cooperative marker lights are arbitrarily arranged on the cooperative platform according to their geometry, with the following deployment requirements:

[0051] 1. Infrared cooperative marker lights must meet the requirements of vertical, front-back, and left-right asymmetry to prevent multiple possible orientations;

[0052] 2. Infrared cooperative marker lights should be arranged in a non-coplanar manner to reduce the ambiguity of pose estimation;

[0053] 3. The distribution of infrared cooperative marker lights should be as dispersed as possible to improve the accuracy of pose measurement.

[0054] After the LED infrared cooperative marker lights are deployed, a total station is used to mark the marker lights on the cooperative platform, obtaining the three-dimensional positions of the marker lights within the platform's coordinate system. The three-dimensional coordinates of the marker lights on the cooperative platform are represented as follows: The number of infrared cooperation indicator lights is The three-dimensional coordinate dataset of the infrared cooperative sign light is: , It is necessary to make prior arrangements as a prerequisite.

[0055] It is worth noting that the power supply system for the infrared cooperative sign light uses a separate power supply device.

[0056] Step 204: Set up an image acquisition system on the second platform, acquire infrared images of each infrared marker light through the image acquisition system, then locate each infrared marker light in the infrared image, and obtain the two-dimensional coordinate data of each infrared marker light in the infrared image.

[0057] It is understandable that before acquiring infrared images, a filter matching the working band of the infrared cooperative sign light is selected for the camera on the image acquisition platform in the dual-motion platform, the camera is calibrated in advance, and the camera's intrinsic and extrinsic parameter matrices and distortion parameters are obtained.

[0058] Based on the interactive movement mode of the dual-motion platform, the installation positions of the infrared cooperative marker lights and the installation angle of the image acquisition system are adjusted. The image acquisition system and its camera are fixed to ensure that, during the relative movement of the dual-motion platform, the camera can effectively and continuously capture a sufficient number of clear infrared images of the marker lights, thereby improving detection and positioning accuracy. It is worth noting that the fixed pose relationship between the camera coordinate system and the image acquisition platform within the dual-motion platform also needs to be calibrated. The image acquisition program sends acquisition commands to the image acquisition system on the second platform, and the image acquisition system begins acquiring infrared images upon receiving the commands.

[0059] The image acquisition system uses an infrared camera paired with a filter corresponding to the wavelength of the LED infrared cooperative sign lights to capture infrared images that are exceptionally bright compared to the surrounding environment. Then, the moment method is used to locate the light spots of the infrared cooperative sign lights with sub-pixel precision, obtaining the two-dimensional coordinate data of each infrared sign light in the infrared image.

[0060] It is worth noting that there are adjustable parameters in the process of detecting and locating the two-dimensional position of the infrared marker light, including the threshold for threshold segmentation and the size range of the target light spot. The threshold parameter setting depends on the camera's aperture and shutter speed settings, and the size range of the target light spot is affected by many factors such as the working environment, the working band of the marker light, and image quality.

[0061] Specifically, the infrared marker lights in the acquired infrared images are accurately detected and located, and their two-dimensional coordinate results are denoted as follows: The number of infrared marker lights detected, measured in pixels, is: The two-dimensional coordinate dataset of the infrared cooperative sign lights in the image is then represented as follows: .

[0062] Preferably, infrared LEDs with wavelengths of 850nm or 940nm and corresponding filters are used.

[0063] Step 206: Calculate the candidate pose based on the three-dimensional coordinate data and the two-dimensional coordinate data, and then verify the correspondence of the candidate pose to obtain the pose estimation result.

[0064] It is understandable that by randomly grouping 3D coordinate data and 2D coordinate data to form a 2D-3D point pair correspondence, then calculating candidate poses using the PNP algorithm, and then verifying the correspondence of 2D-3D point pairs by calculating the reprojection error through reprojection, the correct correspondence combination of all 2D-3D point pairs can be obtained and the pose parameters can be acquired.

[0065] Correspondence refers to the 2D detection points of infrared marker lights in infrared images. 3D configuration points of infrared marker lights on the first platform The correspondence between them is denoted as The correct set of 2D-3D point pair correspondences is obtained, i.e., the pose estimation result, to calculate the pose of the subsequent infrared cooperative marker light coordinate system relative to the camera coordinate system, denoted as... .

[0066] The PNP algorithm used here is preferably the P3P algorithm.

[0067] It's worth noting that a statistical method was used to determine the correct 3D-2D point pair combinations. Each detected infrared image spot was paired with an infrared cooperative marker light to form a 2D-3D point pair. During traversal and reprojection verification, the number of successful correspondences was counted, generating a histogram. When a 2D-3D point pair was considered a successful correspondence, its success count increased. After traversal, the 2D-3D point pair with the most successful correspondences was considered the correct correspondence. The set of all correctly matched 2D-3D point pairs was then returned for subsequent pose calculation.

[0068] Step 208: Predict the pose of the current frame based on the pose estimation results of two consecutive infrared images to obtain the pose prediction result of the current frame. Then, verify the correspondence of the pose prediction result of the current frame to obtain the estimated pose of the current frame.

[0069] It is understandable that when making predictions, a linear model is used based on the assumption of uniform motion. Then, the accuracy of the current frame pose prediction result is checked with the goal of the reprojection error being less than a threshold. If the pose prediction conditions are not met or the pose prediction verification fails, the process returns to step 206 to perform a brute-force search again.

[0070] Specifically, using two consecutive and accurate pose estimation results, and based on the assumption of uniform motion, a linear prediction method is used to obtain the pose of the current frame. The correctness of the prediction is verified by setting the reprojection error to be less than a threshold. In other words, candidate poses are calculated by obtaining the correct 2D-3D point pair combination of the image spot and the infrared marker light. The corresponding combination of the two-dimensional position of the spot and the three-dimensional position of the marker light (as a priori conditions) is used for correspondence search. The correct 2D-3D correspondence combination is determined by setting the reprojection error to be less than a threshold, thus obtaining the correct relative estimated pose.

[0071] It is worth noting that adjustments can be made to the motion assumptions and prediction models when facing different dual-motion platforms, different working environments, and different tasks. For example, a non-uniform motion model assumption can be proposed, and a nonlinear model can be used for prediction, or multiple frames of known positions can be used as prior conditions for prediction.

[0072] Step 210: Iteratively optimize the estimated pose of the current frame based on minimizing the reprojection error to obtain the relative pose between the first platform and the second platform.

[0073] It can be understood that the estimated pose of the current frame that satisfies the correct correspondence is used as the initial estimate, and the pose is iteratively optimized by minimizing the reprojection error. This optimization process uses the Gauss-Newton method to obtain the accurate relative pose parameters.

[0074] An optimization problem is established between the error between the infrared marker light reprojecting into the camera image based on the pose estimation result and the image acquisition. Solving the optimization problem yields the relative pose between the first platform and the second platform with the minimized error.

[0075] Step 212: Adjust the relative positions of the first platform and the second platform according to their relative poses.

[0076] The dual-motion platform relative pose measurement method under the above cooperative mode involves constructing a deployment model for the first platform, which includes several infrared marker lights, and determining the three-dimensional coordinate data of the infrared marker lights on the first platform. An image acquisition system is set up on the second platform to acquire infrared images of each infrared marker light. The infrared marker lights in the infrared images are then located, and their two-dimensional coordinate data in the infrared images is obtained. Candidate poses are calculated based on the three-dimensional and two-dimensional coordinate data, and their correspondence is verified to obtain pose estimation results. The pose estimation results of two consecutive infrared images are used to predict the pose of the current frame, resulting in a predicted pose. This predicted pose is then verified to obtain the estimated pose of the current frame. The estimated pose of the current frame is iteratively optimized based on minimizing the reprojection error to obtain the relative pose between the first and second platforms. The relative positions of the first and second platforms are adjusted according to the relative pose.

[0077] This invention achieves rapid pose calculation through correspondence verification, obtaining an accurate estimated pose for the current frame. Then, using this accurate estimated pose as an initial estimate, iterative optimization is performed by minimizing the reprojection error to obtain a high-precision relative pose between the two motion platforms. This drives the two-motion platform control system to adjust the platform pose, achieving good interaction between the two motion platforms. This invention has the advantages of all-weather operation and strong robustness, enabling rapid and high-precision calculation of the relative pose between the two motion platforms, thus effectively achieving good interaction between them.

[0078] In one embodiment, an image acquisition system is set up on the second platform to acquire infrared images of each infrared marker light. Then, the infrared marker lights in the infrared images are located to obtain two-dimensional coordinate data of each infrared marker light in the infrared images, including:

[0079] An image acquisition system is set up on the second platform to acquire infrared images of each infrared marker light. Thresholding is performed on the infrared images to obtain binary grayscale images. The binary grayscale images are then processed to remove noise and irregular convex hulls, and the center of the infrared marker light's spot is located within the grayscale image. Considering distortion imaging errors, the center of the infrared marker light's spot is corrected to obtain the two-dimensional coordinates of the infrared marker light in the infrared image.

[0080] First, threshold segmentation is performed. Specifically, since LED infrared cooperative marker lights are used, their infrared wavelengths match the infrared filter. In the acquired images, the infrared marker lights are very prominent while background details are suppressed. Therefore, threshold segmentation is used to coarsely process the image, resulting in a binary grayscale image, as shown in the following expression:

[0081] ;

[0082] In the formula, Indicates infrared image in Pixel value at that location, Indicates infrared image in The grayscale value at that location.

[0083] Then, erosion and dilation are performed. Specifically, Gaussian smoothing is used to combine adjacent pixels, removing noise that affects positioning accuracy and irregular convex hulls on the edges of target spots that affect positioning precision in the binary grayscale image after thresholding.

[0084] Finally, the center of the light spot is located subpixel using the moment method. Specifically, the intensity of the image is used to measure the pixel; in this embodiment, the grayscale value of the image pixel is... Within the extracted infrared cooperation marker spot, the sub-pixel center of the spot region is calculated using the first-order moment of the image. The origin moment of the image is expressed as:

[0085] ;

[0086] The coordinates of the centroid of the light spot and the detection position of the infrared marker light in the image are represented as follows:

[0087] ;

[0088] In the formula, Let be the zeroth moment of the light spot region, i.e., the area of ​​the light spot region; , These are the light spot areas related to shaft and The moments of an axis are expressed as follows:

[0089] ;

[0090] Considering the imaging error caused by the pinhole model of the camera under ideal conditions and the lens distortion of the camera under actual conditions, the extracted 2D center point of the infrared marker light is... Radial and tangential distortion corrections were performed using the Brown aberration model:

[0091] ;

[0092] In the formula, These are the actual imaging points; For ideal imaging points; , These are the normalized image plane coordinates of the ideal imaging point; , , It is the phase difference coefficient related to radial distortion; , It is the phase difference coefficient related to tangential distortion; among which, , The expression is:

[0093]

[0094] Corrected infrared marker light center In other words, the two-dimensional coordinates (detection point position) of the infrared marker light in the infrared image in the ideal imaging model are as follows:

[0095] ;

[0096] In the formula, The distance of the point to be processed from the origin of the coordinate system can be expressed by the formula. calculate,; Represents the normalized image plane coordinates of the ideal imaging point.

[0097] In one embodiment, candidate poses are calculated based on three-dimensional coordinate data and two-dimensional coordinate data, and then the candidate poses are verified to obtain pose estimation results, including:

[0098] The 2D and 3D coordinate data are grouped separately, and a correspondence is established between them. The PNP algorithm is then used to calculate several first candidate poses. A threshold is set, and the correspondence of the first candidate poses is judged using a reprojection method to obtain the pose estimation result.

[0099] Understandably, the first step is to group the two-dimensional coordinate data and the three-dimensional coordinate data separately. Specifically, the two-dimensional coordinate dataset is formed from the light spots detected in the image. Generate several 2D coordinate combinations of 3 points each. Simultaneously, from the three-dimensional coordinate dataset of infrared cooperative sign lights Similarly, several 3D coordinate combinations of three points are generated. .

[0100] Then, the candidate poses are calculated. Specifically, since the correspondence between the light spots and the actual infrared marker lights is currently unknown, the two-dimensional coordinates of each set of light spots are combined. Combined with the three-dimensional coordinates of each set of infrared marker lights For each corresponding 2D-3D point pair, the P3P algorithm is used to solve for them. The four candidate poses are arranged and denoted as the first candidate pose.

[0101] Finally, the correspondence is verified through reprojection. Specifically, for each first candidate pose, the 3D coordinate dataset is... The remaining cooperative infrared marker lights not used to calculate the first candidate pose The image is reprojected based on the first candidate pose; if the infrared marker light Reprojection results Spot detection results in the input image nearest neighbor distance Less than the distance threshold Then it is considered that the infrared marker light 3D point The spot detects 2D points This forms a correct correspondence. Preferably, the threshold value used is... A resolution of 5 pixels achieves a good result.

[0102] It's worth noting that, to improve robustness against outliers, each detected light spot is paired with an infrared marker light to form a 2D-3D point pair. During traversal and reprojection verification, the number of successful pairings is counted, generating a histogram. When a 2D-3D point pair is considered a successful pairing, its success count increases. After traversal, the 2D-3D point pair with the most successful pairings is considered the correct pairing, and the set of correct pairings for all 2D-3D point pairs is returned for subsequent pose calculation.

[0103] In one embodiment, the pose of the current frame is predicted based on the pose estimation results of two consecutive infrared images to obtain the pose prediction result of the current frame. Then, the correspondence of the pose prediction result of the current frame is verified to obtain the estimated pose of the current frame, including:

[0104] Based on the assumption of uniform motion, linear prediction is performed using the pose estimation results of two consecutive frames to obtain the pose prediction result for the current frame. According to the pose prediction result of the current frame, each infrared marker light is mapped to its spot center, and then the PNP algorithm is used to calculate several second candidate poses. A threshold is set, and the correspondence of the second candidate poses is judged using a reprojection method to obtain the estimated pose for the current frame.

[0105] Specifically, within the time frame of one image, the motion between the target platform equipped with infrared marker lights and the image acquisition platform is considered as uniform motion, and the pose matrix is... 6D pose vectors derived from Lie algebra Parametric representation is performed. Since a uniform velocity model is used, the previous frame is employed. pose and current pose pose for the next frame Linear prediction is performed as follows:

[0106] ;

[0107] in, It is the first The time of the step, It is the number of poses that were successfully estimated immediately following the current frame in the previous image sequence. That is, pose prediction for the current frame can only be performed after the poses of the previous two frames have been successfully estimated.

[0108] Based on the predicted pose of the current frame, each infrared cooperative marker light is reprojected into the image. Then, the pixel distance between the reprojection of each marker light and the spot detection point is calculated. If the pixel distance is less than a threshold, the corresponding marker light is associated with that spot detection point. Preferably, the threshold is 5 pixels.

[0109] Then, the correspondences selected from the current frame pose prediction results are verified. Three correspondences are selected from the predictions to generate several combinations. The P3P algorithm is used to calculate four second candidate poses for each combination. Then, for each second candidate pose, the two-dimensional reprojection of the remaining infrared cooperative marker lights is calculated. If at least 75% of the remaining infrared cooperative marker lights are below the reprojection threshold, the result is considered valid. If more than 70% of the corresponding combinations meet the correspondence requirement, the predicted 2D-3D correspondence is considered correct, thus obtaining the correct estimated pose for the current frame; if the predicted pose cannot be correctly corresponded, return to step 206 to reinitialize the tracking.

[0110] In one embodiment, the estimated pose of the current frame is iteratively optimized based on minimizing the reprojection error to obtain the relative pose between the first platform and the second platform, including:

[0111] To more accurately estimate the pose of the target object relative to the monocular camera In the process of solving pose parameters optimization, it is used All correspondences are identified, and the reprojection error is iteratively optimized. A correctly corresponding solution is obtained using the P3P algorithm as an initial estimate, and then the reprojection error is minimized. The optimized solution is shown in the formula: ;

[0112] In the formula, This indicates that the infrared cooperation marker light will be adjusted according to the pose parameters. Projected onto the camera image; Indicates the 3D position of the infrared cooperation sign light; This indicates the 2D position detection results of the sign light spot; This represents the correspondence between the 3D position of the infrared cooperative sign light and the 2D detection result of the sign light's imaging spot. For ease of calculation during optimization, the pose matrix is... The exponential mapping is to a Lie algebra 6D pose vector. The method is parameterized and then optimized using the Gauss-Newton method.

[0113] In one embodiment, such as Figure 2 The diagram shows the device structure of the first and second platforms. The second platform includes an image acquisition system and an embedded computer. The image acquisition system and the embedded computer communicate with each other, and the embedded computer also communicates with a host computer. The connection can be wired or wireless. The embedded computer controls the image acquisition system to acquire images and processes the acquired infrared images to obtain relative pose data between the first and second platforms with minimized reprojection error. This relative pose data is then transmitted to the control system of the mobile platform to control the relative position of the first and / or second platforms, enabling interaction between the two platforms according to the measured relative pose.

[0114] It is worth noting that during the installation of the first and second platforms, the operating band, power, and scattering angle of the infrared marker lights should be adjusted according to the working scenario of the dual-motion platform to obtain bright and clear infrared images with suppressed backgrounds, ensuring the accuracy of pose measurement between the platforms. At the same time, the layout scheme of the marker lights and the installation angle of the image acquisition equipment also need to be adjusted according to the relative motion during the interaction process of the dual-motion platform to avoid missed detections, false detections, and errors caused by installation. Compared with traditional methods, this scheme largely overcomes the influence of cluttered lighting and complex backgrounds.

[0115] The host computer connects to the embedded computer wirelessly or via wired connection. It can read and write the embedded computer's built-in program, which controls the image acquisition system, processes image and pose data, transmits the pose results to the platform control system, and controls the platform's movement to achieve interaction.

[0116] It should be understood that, although Figure 1 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 1 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0117] In one embodiment, such as Figure 3 As shown, a dual-motion platform relative pose measurement device in cooperative mode is provided, including: a deployment model construction module 402, an infrared image processing module 404, a pose hypothesis verification module 406, an initial pose prediction module 408, a pose parameter optimization module 410, and a platform pose adjustment module 412, wherein:

[0118] The deployment model construction module 402 is used to construct the deployment model of the first platform. The deployment model includes several infrared marker lights and determines the three-dimensional coordinate data of the infrared marker lights on the first platform.

[0119] The infrared image processing module 404 is used to set up an image acquisition system on the second platform, acquire infrared images of each infrared marker light through the image acquisition system, locate each infrared marker light in the infrared image, and obtain the two-dimensional coordinate data of each infrared marker light in the infrared image.

[0120] The pose hypothesis verification module 406 is used to calculate candidate poses based on three-dimensional coordinate data and two-dimensional coordinate data, and then verify the correspondence of the candidate poses to obtain the pose estimation result.

[0121] The initial pose prediction module 408 is used to predict the pose of the current frame based on the pose estimation results of two consecutive infrared images, to obtain the pose prediction result of the current frame, and then to verify the correspondence of the pose prediction result of the current frame to obtain the estimated pose of the current frame.

[0122] The pose parameter optimization module 410 iteratively optimizes the estimated pose of the current frame based on minimizing the reprojection error to obtain the relative pose between the first platform and the second platform.

[0123] The platform pose adjustment module 412 is used to adjust the relative position of the first platform and the second platform according to the relative pose.

[0124] Specific limitations regarding the relative pose measurement device for a dual-motion platform in cooperative mode can be found in the limitations of the relative pose measurement method for a dual-motion platform in cooperative mode described above, and will not be repeated here. Each module in the aforementioned relative pose measurement device for a dual-motion platform in cooperative mode can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the corresponding operations of each module.

[0125] In one embodiment, a computer device is provided, which is an embedded device, and its internal structure diagram can be as follows: Figure 4 As shown, the device includes a processor, memory, and a WiFi module connected via a system bus. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The WiFi module is used for communication with external terminals via a network connection. When executed by the processor, the computer program implements a cooperative method for relative pose measurement of a dual-motion platform.

[0126] Those skilled in the art will understand that Figure 4 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0127] In one embodiment, a computer device is provided, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to perform the following steps:

[0128] Step 202: Construct a deployment model of the first platform. The deployment model includes several infrared marker lights. Determine the three-dimensional coordinate data of the infrared marker lights on the first platform.

[0129] Step 204: Set up an image acquisition system on the second platform, acquire infrared images of each infrared marker light through the image acquisition system, then locate each infrared marker light in the infrared image, and obtain the two-dimensional coordinate data of each infrared marker light in the infrared image.

[0130] Step 206: Calculate the candidate pose based on the three-dimensional coordinate data and the two-dimensional coordinate data, and then verify the correspondence of the candidate pose to obtain the pose estimation result.

[0131] Step 208: Predict the pose of the current frame based on the pose estimation results of two consecutive infrared images to obtain the pose prediction result of the current frame. Then, verify the correspondence of the pose prediction result of the current frame to obtain the estimated pose of the current frame.

[0132] Step 210: Iteratively optimize the estimated pose of the current frame based on minimizing the reprojection error to obtain the relative pose between the first platform and the second platform.

[0133] Step 212: Adjust the relative positions of the first platform and the second platform according to their relative poses.

[0134] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0135] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0136] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A method for measuring the relative pose of a dual-motion platform in a cooperative mode, characterized in that, The method includes: Construct a deployment model for the first platform, the deployment model including several infrared marker lights, and determine the three-dimensional coordinate data of the infrared marker lights on the first platform; An image acquisition system is set up on the second platform to acquire infrared images of each infrared marker light. Then, each infrared marker light in the infrared image is located to obtain the two-dimensional coordinate data of each infrared marker light in the infrared image. Candidate poses are calculated based on the three-dimensional coordinate data and the two-dimensional coordinate data, and then the correspondence of the candidate poses is verified to obtain the pose estimation result. The pose of the current frame is predicted based on the pose estimation results of two consecutive infrared images to obtain the pose prediction result of the current frame. Then, the correspondence of the pose prediction result of the current frame is verified to obtain the estimated pose of the current frame. The estimated pose of the current frame is iteratively optimized by minimizing the reprojection error to obtain the relative pose between the first platform and the second platform. Adjust the relative positions of the first platform and the second platform according to the relative pose; An image acquisition system is set up on the second platform to acquire infrared images of each of the infrared marker lights. Then, each infrared marker light in the infrared image is located, and its two-dimensional coordinate data in the infrared image is obtained, including: An image acquisition system is set up on the second platform to acquire infrared images of each of the infrared sign lights; The infrared image is segmented by thresholding to obtain a binary grayscale image; the binary grayscale image is processed to remove noise and irregular convex hulls, and the center of the infrared marker light spot in the binary grayscale image is located. Considering the distortion imaging error, the center of the infrared marker light spot is corrected to obtain the two-dimensional coordinates of the infrared marker light in the infrared image; The two-dimensional coordinates of the infrared marker light in the infrared image are represented as follows: ； In the formula, Indicates the center of the infrared indicator light; , , This represents the phase difference coefficient related to radial distortion; This represents the distance of the point to be processed from the origin of the coordinate system; Normalized image plane coordinates representing the ideal imaging point; , This represents the phase difference coefficient related to tangential distortion; Based on the pose estimation results of two consecutive infrared images, the pose of the current frame is predicted to obtain the pose prediction result of the current frame. Then, the correspondence of the pose prediction result of the current frame is verified to obtain the estimated pose of the current frame, including: Based on the assumption of uniform motion, the pose estimation results of the current frame are obtained by linear prediction using the pose estimation results of two consecutive frames. Based on the current frame pose prediction result, each infrared marker light is matched with the center of the infrared marker light spot, and then the PNP algorithm is used to calculate and obtain several second candidate poses. A threshold is set, and the correspondence of the second candidate pose is judged by the reprojection method to obtain the estimated pose of the current frame.

2. The method for measuring the relative pose of a dual-motion platform in a cooperative mode according to claim 1, characterized in that, The infrared marker lights in the layout model are asymmetrical, non-coplanar, and dispersed.

3. The method for measuring the relative pose of a dual-motion platform in a cooperative mode according to claim 2, characterized in that, The image acquisition system uses a filter corresponding to the infrared marker light band to acquire infrared images.

4. The method for measuring the relative pose of a dual-motion platform in a cooperative mode according to any one of claims 1 to 3, characterized in that, Candidate poses are calculated based on the three-dimensional coordinate data and the two-dimensional coordinate data. Then, the correspondence of the candidate poses is verified to obtain the pose estimation result, including: The two-dimensional coordinate data and the three-dimensional coordinate data are grouped separately, and a correspondence is established between the two-dimensional coordinate data and the three-dimensional coordinate data. Then, the PNP algorithm is used to calculate and obtain several first candidate poses. A threshold is set, and the correspondence of the first candidate pose is judged by the reprojection method to obtain the pose estimation result.

5. The method for measuring the relative pose of a dual-motion platform in a cooperative mode according to any one of claims 1 to 3, characterized in that, The formula for minimizing the reprojection error is expressed as: ； In the formula, This indicates that the infrared cooperative sign light will be based on the pose matrix. Projected onto the camera image; Indicates the 3D position of the infrared cooperation sign light; This indicates the 2D position detection results of the sign light spot; This indicates the correspondence between the 3D position of the infrared cooperative sign light and the 2D detection result of the sign light imaging spot.

6. A relative pose measurement device for a dual-motion platform in a cooperative mode, characterized in that, The apparatus for measuring the relative pose of a dual-motion platform in a cooperative mode as described in any one of claims 1 to 5 includes: The deployment model construction module is used to construct the deployment model of the first platform. The deployment model includes several infrared marker lights, and the three-dimensional coordinate data of the infrared marker lights on the first platform are determined. An infrared image processing module is used to set up an image acquisition system on the second platform, acquire infrared images of each infrared marker light through the image acquisition system, locate each infrared marker light in the infrared image, and acquire two-dimensional coordinate data of each infrared marker light in the infrared image. The pose hypothesis verification module is used to calculate candidate poses based on the three-dimensional coordinate data and the two-dimensional coordinate data, and then verify the correspondence of the candidate poses to obtain the pose estimation result. The initial pose prediction module is used to predict the pose of the current frame based on the pose estimation results of two consecutive infrared images, to obtain the pose prediction result of the current frame, and then to verify the correspondence of the pose prediction result of the current frame to obtain the estimated pose of the current frame. The pose parameter optimization module iteratively optimizes the estimated pose of the current frame based on minimizing the reprojection error to obtain the relative pose between the first platform and the second platform. The platform pose adjustment module is used to adjust the relative position of the first platform and the second platform according to the relative pose.

7. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 5.

Images