Multi-vehicle linkage pose recognition method and system, storage medium and program product

Abstract

The invention discloses a multi-vehicle linkage pose recognition method and system, a storage medium and a program product, and relates to the field of general image data processing, and the method comprises the steps: collecting a current image frame containing an ARUCO identifier on a target slave vehicle, and obtaining two-dimensional image data; identifying the pixel coordinate information and the identification color value of the ARUCO identification; determining physical size data corresponding to the ARUCO identifier with the identifier color value; according to the physical size data, the pixel coordinate information and the pre-calibration internal reference of the 2D camera, calculating to obtain a distance estimation value; obtaining a distance measurement value, and taking the distance measurement value as a relative distance between the target master vehicle and the target slave vehicle when a deviation value between the distance measurement value and the distance estimation value does not exceed a preset deviation threshold value; according to the pixel coordinate information and the pre-calibrated internal reference of the 2D camera, the relative deflection angle of the target master vehicle and the target slave vehicle is determined through a pose resolving algorithm; and generating a pose recognition result. According to the invention, the pose recognition deviation can be reduced.

Description

Technical Field

[0001] This application relates to the general field of image data processing, and in particular to a method, system, storage medium, and program product for multi-vehicle coordinated pose recognition. Background Technology

[0002] In applications such as automated logistics, intelligent manufacturing, and robotic collaboration, precise collaborative operation among multiple agents (such as automated guided vehicles, AGVs) is crucial. The foundation for achieving this collaboration lies in the real-time and reliable acquisition of relative pose information between the agents (e.g., the master vehicle and the slave vehicle), namely, relative distance and relative orientation. Therefore, developing a relative pose recognition method that combines high accuracy, high reliability, and low cost is key to driving technological advancements and commercial applications in related fields.

[0003] In related technologies, a scheme combining a 2D camera and a single-point laser rangefinder is proposed. In this scheme, a 2D camera and a laser rangefinder are deployed simultaneously on the host vehicle, while a visual identifier (such as an ARUCO identifier) ​​is configured on the slave vehicle. During system operation, the 2D camera on the host vehicle first identifies the visual identifier in the image and calculates the relative attitude angle between the slave vehicle and the host vehicle based on its position in the imaging plane. Simultaneously, the system controls the laser rangefinder to measure in the direction of the visual identifier, using the laser rangefinder's measurement as the precise distance. Finally, the system combines the relative attitude angle calculated by the camera with the distance value measured by the laser sensor to obtain the relative pose information.

[0004] However, when there are temporary obstructions between the master vehicle and the slave vehicle (e.g., passing pedestrians, other equipment, or dust), or when there is an installation deviation in the laser rangefinder, incorrect measurement values ​​will be obtained instead of the actual distance to the target slave vehicle, resulting in deviations in pose recognition and affecting subsequent linkage control. Summary of the Invention

[0005] This application provides a multi-vehicle linkage pose recognition method, system, storage medium, and program product to reduce pose recognition deviation.

[0006] In a first aspect, this application provides a multi-vehicle linkage pose recognition method, applied to a multi-vehicle linkage control system. The method includes: acquiring a current image frame containing an ARUCO identifier on a target slave vehicle using a 2D camera on the target master vehicle to obtain two-dimensional image data; identifying the pixel coordinate information and identifier color value of the ARUCO identifier in the two-dimensional image data; determining the physical size data corresponding to the ARUCO identifier with the identifier color value based on a color-size mapping table; calculating a distance estimate using a perspective projection model based on the physical size data, pixel coordinate information, and pre-calibration intrinsic parameters of the 2D camera; acquiring a distance measurement value from a laser rangefinder sensor, and using the distance measurement value as the relative distance between the target master vehicle and the target slave vehicle when the deviation between the distance measurement value and the distance estimate value does not exceed a preset deviation threshold; determining the relative deflection angle between the target master vehicle and the target slave vehicle using a pose calculation algorithm based on the pixel coordinate information of the ARUCO identifier and the pre-calibration intrinsic parameters of the 2D camera; and generating a pose recognition result based on the relative distance and the relative deflection angle.

[0007] In the above embodiments, the multi-vehicle linkage control system calculates the distance estimate based on the physical size and pixel size of the ARUCO identifier, and uses this as a basis to verify the validity of the distance measurement value of the laser ranging sensor, ensuring the validity of the laser measurement value. This effectively filters out abnormal laser ranging data caused by occlusion or sensor installation deviation, avoids using incorrect distance values ​​for pose calculation, and reduces the deviation of the final pose recognition result.

[0008] In conjunction with some embodiments of the first aspect, in some embodiments, the ARUCO identifier is segmented into multiple sub-identifiers, which are respectively configured at different vehicle body positions of the target vehicle's rotatable mechanism or articulated structure; the step of identifying the pixel coordinate information and identifier color value of the ARUCO identifier in the two-dimensional image data specifically includes: identifying the pixel coordinate information of the multiple sub-identifiers of the ARUCO identifier in the two-dimensional image data to obtain multiple sets of molecular coordinate data; and identifying the identifier color value of the multiple sub-identifiers in the two-dimensional image data.

[0009] In the above embodiments, the multi-vehicle linkage control system divides and arranges ARUCO markers on different parts of the vehicle articulation structure, so that the system can not only identify the overall position and pose of the vehicle, but also analyze the relative position and attitude of each sub-marker.

[0010] In conjunction with some embodiments of the first aspect, in some embodiments, after the step of identifying the pixel coordinate information of multiple sub-identifiers of ARUCO in two-dimensional image data to obtain multiple sets of molecular coordinate data, the method further includes: identifying a main identifier with preset features from multiple sub-identifiers, and using sub-identifiers other than the main identifier as other sub-identifiers; using the molecular coordinate data of the main identifier as the main body, transforming the molecular coordinate data of other sub-identifiers to the corresponding angles and positions for splicing to obtain a main identifier image, and determining the main body deflection angle corresponding to the main identifier image; using the molecular coordinate data of other sub-identifiers as the main body for transformation, determining the corresponding sub-body identifier image and sub-body deflection angle; and determining the working posture of the target vehicle based on the main body deflection angle and the sub-body deflection angle.

[0011] In the above embodiments, the multi-vehicle linkage control system can determine the posture of the vehicle body not only through the main identifier but also accurately calculate the working posture parameters such as the rotation angle of the articulation mechanism by distinguishing the primary and secondary sub-identifiers and virtually splicing them together.

[0012] In conjunction with some embodiments of the first aspect, in some embodiments, before the step of acquiring a current image frame containing an ARUCO identifier on the target vehicle using a 2D camera of the target vehicle to obtain two-dimensional image data, the method further includes: controlling a laser rangefinder to measure the distance to the target vehicle to obtain an initial distance value; calculating the expected pixel size of the ARUCO identifier on the imaging plane of the initial distance value based on the initial distance value and the preset identifier size of the ARUCO identifier; and adjusting the pre-calibration intrinsic parameters of the 2D camera based on the expected pixel size.

[0013] In the above embodiments, before the pose recognition task begins, the multi-vehicle linkage control system uses the precise initial distance value of laser ranging to reverse calculate the pixel size that the ARUCO logo should have at that distance, and uses this as a benchmark to dynamically adjust the camera intrinsic parameters to avoid incomplete or blurry sampling due to distance during visual sampling.

[0014] In conjunction with some embodiments of the first aspect, in some embodiments, after controlling the laser rangefinder to measure the distance of the target vehicle to obtain an initial distance value, the method further includes: calculating the distance difference between the initial distance value and the distance measurement value; recording the acquisition timestamps of the initial distance value and the distance measurement value respectively, and calculating the time difference; and determining the travel speed of the target vehicle based on the relative deflection angle, the distance difference, and the time difference.

[0015] In the above embodiments, the multi-vehicle linkage control system records the timestamps of two distance measurements and calculates their difference. Combined with the distance change, it realizes the calculation of the relative speed of the target from the vehicle along the line of sight of the master vehicle, thereby improving the dynamic response capability and predictability of the linkage control.

[0016] In conjunction with some embodiments of the first aspect, in some embodiments, after the step of identifying the pixel coordinate information and the identifier color value of the ARUCO identifier in the two-dimensional image data, the method further includes: when the identifier color value does not belong to the color feature library of a preset interactive device, determining the device identifier code corresponding to the identifier color value; and generating a device avoidance prompt based on the pose recognition result and the device identifier code.

[0017] In the above embodiments, the multi-vehicle linkage control system uses the color of the ARUCO logo to distinguish targets in the scene into known cooperating vehicles and unknown non-interactive devices, which enhances the system's environmental perception and safety redundancy capabilities. This enables the multi-vehicle linkage system to not only cooperate efficiently, but also to actively avoid potential collision risks in mixed traffic environments, thereby improving the overall safety of the operation.

[0018] In conjunction with some embodiments of the first aspect, in some embodiments, after generating the pose recognition result based on the relative distance and relative deflection angle, the method further includes: acquiring the real-time motion parameters of the target master vehicle and the target slave vehicle respectively; determining the relative motion trajectory within a future preset time interval based on the pose recognition result and the real-time motion parameters; and determining the motion adjustment parameters of the target slave vehicle according to the real-time motion parameters and the relative motion trajectory.

[0019] In the above embodiments, the multi-vehicle linkage control system combines the real-time pose recognition results with the vehicle's own motion parameters (such as speed, acceleration, and angular velocity), and predicts the relative motion trajectory in the short term through kinematic or dynamic models. It calculates in advance the motion adjustments that the vehicles need to make to maintain formation or complete tasks, avoiding control lag problems caused by system delays and vehicle inertia, making the coordinated movement of the convoy smoother and more precise.

[0020] In a second aspect, embodiments of this application provide a multi-vehicle linkage control system, which includes: one or more processors and a memory; the memory is coupled to the one or more processors, and the memory is used to store computer program code, which includes computer instructions, and the one or more processors call the computer instructions to cause the multi-vehicle linkage control system to perform the method described in the first aspect and any possible implementation thereof.

[0021] Thirdly, embodiments of this application provide a computer program product containing instructions that, when the computer program product is run on a multi-vehicle linkage control system, cause the multi-vehicle linkage control system to execute the method described in the first aspect and any possible implementation thereof.

[0022] Fourthly, embodiments of this application provide a computer-readable storage medium including instructions that, when executed on a multi-vehicle linkage control system, cause the multi-vehicle linkage control system to perform the method described in the first aspect and any possible implementation thereof.

[0023] Understandably, the multi-vehicle linkage control system provided in the second aspect, the computer program product provided in the third aspect, and the computer storage medium provided in the fourth aspect are all used to execute the methods provided in the embodiments of this application. Therefore, the beneficial effects they can achieve can be referred to the beneficial effects in the corresponding methods, and will not be repeated here.

[0024] One or more technical solutions provided in the embodiments of this application have at least the following technical effects or advantages: 1. By employing a technical solution that calculates distance estimates using physical dimensions and pixel information based on ARUCO identifiers from a 2D camera, and then compares and verifies these estimates with distance measurements from a laser rangefinder, the multi-vehicle linkage control system can construct a cross-validation mechanism for sensor data fusion. Although the distance estimates provided by the camera have lower accuracy, they are less susceptible to small-scale physical obstructions and can serve as a benchmark for the reliability of laser rangefinder values. The system can promptly identify and discard abnormal data by comparing the deviation between the two with a preset threshold, effectively solving the problem in existing technologies where reliance on laser rangefinders is easily affected by obstructions and installation deviations, leading to serious deviations in pose recognition results. This achieves high accuracy and high availability of the entire pose recognition result in complex dynamic environments.

[0025] 2. Because the system employs a technical solution that first uses a laser rangefinder to obtain the initial distance before pose recognition, and then combines it with the preset physical size of the marker to calculate the expected pixel size at the current distance, and then dynamically adjusts the pre-calibration intrinsic parameters of the 2D camera based on this expected pixel size, the multi-vehicle linkage control system can adjust the focal length and wide-angle in a timely manner. This avoids obtaining blurry ARUCO markers due to excessive vehicle distance, or incomplete field of view due to excessive vehicle distance, thus improving the accuracy of ARUCO markers and ensuring the accuracy of pose recognition results.

[0026] 3. By adopting a technical solution that obtains the real-time motion parameters (such as speed and angular velocity) of the master and slave vehicles after generating the pose recognition results, and predicts the relative motion trajectory within a preset time interval in the future through a kinematic model based on the current relative pose and these motion parameters, and finally determines the motion adjustment parameters of the slave vehicle based on this predicted trajectory, the multi-vehicle linkage control system can calculate in advance the acceleration, deceleration or steering commands that the slave vehicle needs to execute in order to achieve the desired coordinated state, thus improving the real-time performance of coordinated control. Attached Figure Description

[0027] Figure 1 This is a flowchart illustrating a multi-vehicle coordinated pose recognition method in an embodiment of this application. Figure 2 This is another flowchart illustrating the multi-vehicle coordinated pose recognition method in this application embodiment; Figure 3 This is a schematic diagram of the physical device structure of a multi-vehicle linkage control system in the embodiments of this application. Detailed Implementation

[0028] The terminology used in the following embodiments of this application is for the purpose of describing particular embodiments only and is not intended to be limiting of this application. As used in the specification of this application, the singular expressions “a,” “an,” “the,” “the,” and “this” are intended to include the plural expressions as well, unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used in this application refers to any or all possible combinations including one or more of the listed items.

[0029] Hereinafter, the terms "first" and "second" are used for descriptive purposes only and should not be construed as implying or suggesting relative importance or implicitly indicating the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature, and in the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0030] In the specific implementation scenarios of this application, such as automated warehousing or port logistics, a multi-vehicle linkage control system is the core for commanding and coordinating the collaborative work of multiple autonomous mobile robots (such as AGVs). The target master vehicle typically refers to the lead vehicle that issues commands or serves as a reference, while the target slave vehicle is the vehicle that follows or cooperates with the master vehicle. A 2D camera is a standard camera mounted on the master vehicle to capture visual information. An ARUCO identifier is a black and white square mark, often used as a positioning reference in computer vision due to its ease of detection and unique encoding; here it is affixed to the surface of the slave vehicle. Two-dimensional image data refers to the digital image captured by the camera. Pixel coordinate information indicates the position of the ARUCO identifier in the image, usually the pixel coordinates of its four corner points. The identifier color value is the color information identified through image processing after coloring the ARUCO identifier or its border, used to distinguish different types of slave vehicles or identifiers. Physical size data is the actual side length of the ARUCO identifier in the real world. Pre-calibration intrinsic parameters are a set of internal parameters measured before the camera leaves the factory or is used, including focal length, principal point coordinates, etc., which describe how the camera projects three-dimensional spatial points onto a two-dimensional image plane. The perspective projection model is a mathematical model describing this projection process. The distance estimate is an approximate distance calculated using this model and image information, while the distance measurement is a more accurate distance measured directly by a laser rangefinder. Pose calculation algorithms (such as the PnP algorithm) are used to calculate the three-dimensional relative rotation and translation relationship between the camera (master vehicle) and the marker (slave vehicle) based on the marker's pixel coordinates and physical dimensions; that is, the relative deflection angle and relative distance. The entire scheme achieves accurate and robust recognition of the slave vehicle's relative pose by fusing information from these two different sensors, providing a reliable basis for subsequent path planning and motion control.

[0031] The following describes the process of the method provided in this implementation. Please refer to [link / reference]. Figure 1 This is a flowchart illustrating a multi-vehicle coordinated pose recognition method in an embodiment of this application.

[0032] S101. The 2D camera of the target vehicle acquires the current image frame containing the ARUCO identifier of the target vehicle, and obtains two-dimensional image data.

[0033] In this context, the target master vehicle refers to the vehicle that undertakes the main perception and decision-making functions in a multi-vehicle platoon. The 2D camera is a standard image sensor mounted on the master vehicle. The target slave vehicle is a cooperative vehicle that needs to be identified and tracked by the master vehicle. The ARUCO identifier is a visual marker fixed to the surface of the slave vehicle. The current image frame refers to a still image captured at a specific point in time. The two-dimensional image data is the digital representation of this image, typically a pixel matrix.

[0034] Specifically, when the multi-vehicle linkage control system begins or performs a pose recognition task, it activates a 2D camera mounted on the front or top of the target vehicle, aligning it with the approximate direction of the target vehicle. The camera captures a frame containing the target vehicle and its ARUCO logo at a preset frequency (e.g., 10 frames per second) or as triggered by a command. This image is then converted into digitized two-dimensional image data, such as a pixel array containing RGB or grayscale values, and transmitted to the processor of the multi-vehicle linkage control system for further analysis.

[0035] In some embodiments, this step of triggering and data acquisition can be implemented in several ways: Optionally, the multi-vehicle linkage control system can be configured to continuously capture video streams and extract a frame as the current image frame for processing at fixed time intervals (e.g., every 100 milliseconds), suitable for scenarios requiring high-frequency pose updates; Optionally, the multi-vehicle linkage control system can also adopt an event-triggered mechanism, for example, triggering image acquisition only when the inertial measurement unit (IMU) of the master vehicle detects significant vehicle movement or enters the camera's field of view from the vehicle's predicted position, which can save computational resources. It is understood that other methods can also be used to achieve image acquisition, such as the upper-level task scheduling system directly issuing acquisition commands, which are not limited here.

[0036] S102. Identify the pixel coordinate information and color value of the ARUCO identifier in the two-dimensional image data.

[0037] Pixel coordinate information refers to the precise location of the ARUCO identifier in the two-dimensional image data, typically represented by the (x, y) coordinates of its four corner points in the image coordinate system (in pixels). Identifier color value refers to the quantized value obtained by analyzing the color of the ARUCO identifier itself or a specific area around it through image processing algorithms, such as RGB or HSV values.

[0038] Specifically, after receiving the two-dimensional image data generated in step S101, the multi-vehicle linkage control system invokes an image processing module. This module first preprocesses the image, performing grayscale conversion, binarization, and edge detection to highlight the outline of the ARUCO logo. Next, it searches the image for candidate regions that match the square characteristics of the ARUCO logo and decodes these regions to confirm their validity. After confirmation, the algorithm precisely extracts the sub-pixel coordinates of its four corner points. Simultaneously, the algorithm analyzes the pixel colors of the internal coded region or predefined border region of the logo, calculating the average color value as the logo's color value.

[0039] In some embodiments, the recognition process can be implemented in several ways: Optionally, the multi-vehicle linkage control system can use the ARUCO detection function integrated in an open-source vision library (such as OpenCV). This function can directly output the ID of the identified marker and the pixel coordinates of its four corner points. Then, an auxiliary function can be written to extract color information based on the region defined by the corner point coordinates. Optionally, the multi-vehicle linkage control system can also train a deep learning model, such as YOLO or Faster R-CNN, which can directly locate the ARUCO marker in the image (outputting the bounding box coordinates and then extracting the corner points) and classify its color attributes. It is understood that other methods can also be used to achieve marker recognition and information extraction, such as traditional template matching combined with color histogram analysis; this is not limited here.

[0040] A key issue that may arise during implementation is the presence of reflections, shadows, or partial occlusion in the image, leading to ARUCO marker recognition failures or inaccurate corner location. To address this, the multi-vehicle linkage control system can incorporate robustness enhancement measures into the recognition algorithm. For example, adaptive threshold binarization can be added during the preprocessing stage to handle uneven lighting. For partial occlusion, line detection based on Hough transform can be used to infer the occluded edges and corners. Furthermore, the system can maintain a short-term temporal model of the marker's position and pose (such as a Kalman filter). When recognition fails in a particular frame, the model's predictions can be used as a temporary replacement, ensuring the continuity of pose output.

[0041] S103. Based on the color-size mapping table, determine the physical size data corresponding to the ARUCO identifier with the identifier color value.

[0042] The color-size mapping table is a pre-configured data structure that stores the correspondence between different identifier color values ​​and the actual physical dimensions of the ARUCO identifier. The physical dimensions typically refer to the side length of the identifier square, in meters or millimeters.

[0043] Specifically, after the multi-vehicle linkage control system successfully identifies the ARUCO logo's color value in step S102, it queries its internally stored color-size mapping table. This table acts like a dictionary, using the logo color value as the key and the corresponding physical size data as the value. For example, the table might record that red (RGB: 255, 0, 0) corresponds to a physical size of 0.2 meters, and blue (RGB: 0, 0, 255) corresponds to a physical size of 0.3 meters. Based on the queried color value, the multi-vehicle linkage control system retrieves the corresponding physical size data from the table for subsequent calculations.

[0044] A key issue that may arise during implementation is the discrepancy between the colors captured by the camera and preset color values ​​due to variations in ambient lighting, leading to query failures. To address this, multi-vehicle linkage control systems should not use precise value comparisons for color matching. Instead, they should employ color clustering or color distance methods. In the color size mapping table, each color should not be a single point, but rather a range or a center point plus a tolerance radius. When a detected color value falls within a preset color space range (for example, in the CIELAB color space, the color difference ΔE from "standard red" is less than a certain threshold), it can be identified as that color, thus successfully finding the corresponding physical size.

[0045] S104. Based on the physical size data, pixel coordinate information, and the pre-calibration intrinsic parameters of the 2D camera, the distance estimate is obtained through perspective projection model calculation.

[0046] The pre-calibration intrinsic parameters are matrices describing the camera's imaging characteristics, primarily including focal length and principal point coordinates. The perspective projection model is a mathematical formula based on the pinhole camera principle, relating the object's actual size, its size in the image, and its distance from the camera. The distance estimate is the approximate distance between the driver and slave vehicles calculated based on this model.

[0047] Specifically, the multi-vehicle linkage control system utilizes classic camera imaging geometry for calculation. According to the perspective projection model, the distance (Z) from the object to the camera is approximately equal to (focal length * actual object height) / the object's height in the image. In this step, the multi-vehicle linkage control system uses the physical dimension data obtained in step S103 as the "actual object height," calculates the pixel height of the ARUCO marker in the image using the pixel coordinate information obtained in step S102, and then combines this with the focal length information in the pre-calibration intrinsic parameters to calculate the distance from the camera's optical center to the center of the ARUCO marker using the above formula. This result is the distance estimate.

[0048] In some embodiments, this calculation process can be implemented in several ways: Optionally, the multi-vehicle linkage control system can simply use the average pixel length of the four sides of the marker as its size on the image for calculation, which is simple and fast; alternatively, the multi-vehicle linkage control system can also utilize the coordinates of the four corner points of the ARUCO marker and call a pose calculation algorithm (such as OpenCV's solvePnP function), which can directly return a solution containing a three-dimensional translation vector. The magnitude of this vector is a more accurate distance estimate because it incorporates the marker's tilt pose. It is understood that other methods can also be used to estimate the distance, such as using a trained neural network to directly regress the distance value from the marker image; this is not limited here.

[0049] A key issue that may arise during implementation is that when the ARUCO marker is tilted at a large angle relative to the camera, its projection onto the image becomes severely distorted (no longer a square), leading to significant errors in the distance estimate calculated based on pixel side lengths. To address this, a solution based on solvePnP can be employed. This algorithm utilizes the known geometric relationships of the four corner points in 3D space (a planar square) and their projections onto the 2D image to accurately inversely solve for the 6D pose (3 rotations, 3 translations) between the camera and this square. The Z component of the translation vector, or its magnitude, is the distance estimate incorporating the tilt effect, achieving a much higher accuracy than the simple similar triangle method.

[0050] S105. Obtain the distance measurement value from the laser rangefinder sensor, and when the deviation between the distance measurement value and the distance estimate value does not exceed the preset deviation threshold, use the distance measurement value as the relative distance between the target master vehicle and the target slave vehicle.

[0051] A laser rangefinder is a device that emits a laser beam and receives the reflected signal to accurately measure distance. The distance measurement value is a high-precision distance reading output by the sensor. A preset deviation threshold is a tolerance value used to determine whether the camera's estimated distance and the laser-measured distance are consistent. The relative distance is the final confirmed and adopted master-slave distance.

[0052] Specifically, during the image acquisition of the ARUCO marker, the multi-vehicle linkage control system controls the target vehicle to send a measurement command in parallel to the laser rangefinder sensor mounted on the vehicle. This sensor emits a laser beam in the direction of the ARUCO marker and receives the signal returned from the target object, calculating a precise distance measurement. Then, the system calculates the absolute difference between this distance measurement and the distance estimate obtained in step S104. This difference is compared to a preset deviation threshold (e.g., 0.1 meters). If the difference is less than or equal to the threshold, it indicates that the laser sensor has likely hit the correct target vehicle, and the system adopts this high-precision distance measurement as the final relative distance. Conversely, if the difference is too large, the laser measurement is considered unreliable (it may have hit an obstacle or the ground) and is discarded.

[0053] In some embodiments, the verification and adoption logic of this step can be implemented in several ways: Optionally, the deviation threshold can be a fixed value, suitable for scenarios where the working distance does not change much; alternatively, the deviation threshold can also be dynamic, for example, set to 10% of the distance estimate, thus tolerating greater deviation at long distances and requiring higher consistency at short distances, making it more adaptive. It is understood that other fusion strategies can also be adopted, such as using a Kalman filter to fuse the laser measurement value and the camera estimate when the deviation is within the threshold, resulting in a smoother and more accurate distance output; this is not limited here.

[0054] During implementation, when laser measurements are deemed invalid, the system needs to determine the relative distance. To address this, the multi-vehicle linkage control system should design a degradation and fusion strategy, rather than simply discarding the data. The core of this strategy is adaptive weighted fusion based on the confidence levels of the two sensor data. First, the system needs to evaluate the reliability of both types of data: for the distance estimate calculated by the camera, its confidence level can be quantified by the reprojection error after pose calculation; the smaller the error, the higher the confidence level. For the distance measurement from the laser ranging sensor, its confidence level can be determined by calculating the variance within a short time window (e.g., the last 5 data points); the smaller the variance, the more stable the data, and the higher the confidence level. When the deviation between the two exceeds a preset threshold, it means that one of the sensors may have a serious error (e.g., the laser is blocked). In this case, the system will compare the confidence levels of the two and fully adopt the data source with the higher confidence level as the relative distance for the current frame. For example, if the variance of the laser data suddenly increases sharply, the system will switch to using the distance estimate calculated by the camera, ensuring the continuity of pose output. When the deviation between the two is within a threshold, it indicates that both are effective. At this point, the system performs weighted fusion to obtain a more accurate and smoother result. The weights w_v (visual) and w_l (laser) are proportional to their respective confidence levels (i.e., inversely proportional to the error or variance) and are normalized (w_v + w_l = 1). The final fused relative distance d_fused is calculated using the formula d_fused = w_v * d_vision + w_l * d_laser. This method achieves high accuracy when sensor data quality is good and can seamlessly switch to a reliable data source when a single sensor is interfered with, enhancing the robustness of the entire pose recognition system.

[0055] S106. Based on the pixel coordinate information identified by ARUCO and the pre-calibration intrinsic parameters of the 2D camera, determine the relative deflection angle between the target master vehicle and the target slave vehicle through the pose calculation algorithm.

[0056] Pose calculation algorithms are used to inversely solve the 3D spatial relationship between a camera and an object from 2D image features. The most typical example is the PnP (Perspective-n-Point) series of algorithms. Relative yaw angles describe the rotation relationship between the vehicle coordinate system and the master vehicle coordinate system, and are usually represented by Euler angles (pitch, yaw, roll) or quaternions.

[0057] Specifically, the multi-vehicle linkage control system takes the pixel coordinates of the four corner points of the ARUCO marker obtained in step S102, the physical size data of the marker obtained in step S103 (used to define the 3D vertex coordinates of the marker in its own coordinate system), and the pre-calibration intrinsic parameters of the 2D camera (camera matrix and distortion coefficients) as inputs to the pose calculation algorithm (such as the solvePnP function in OpenCV). This algorithm iteratively optimizes to find an optimal 3D rotation and translation, ensuring that the 3D vertices of the marker, after this transformation and projection onto the camera's imaging plane, best match the observed 2D pixel coordinates. The rotation component output by the algorithm (usually a rotation vector, which can be converted into a rotation matrix or Euler angles) represents the relative deflection angle between the target master vehicle and the target slave vehicle.

[0058] A core issue that may arise during the implementation of the solution is the ambiguity and instability of pose calculation. This stems not only from geometric blurring at specific viewpoints but also from minor deviations in ARUCO corner detection caused by image noise. To address this, in addition to utilizing temporal information to ensure solution continuity, this application introduces the Random Sample Consensus (RANSAC) algorithm into the implementation of pose calculation algorithms (such as solvePnP) to actively remove abnormal corner data, thereby fundamentally improving the stability and accuracy of the solution. The mathematical logic of RANSAC is as follows: among the four detected corner points, a minimum subset is randomly selected (usually three points for PnP problems) to calculate an initial pose solution (rotation and translation). Then, this solution is used to project the 3D model coordinates of all four corner points back onto the image plane, and the distance between the projected point and the actual detected pixel coordinates (i.e., reprojection error) is calculated. If the error is less than a preset pixel threshold (e.g., 2 pixels), the corner point is considered an "inlier," meaning it is consistent with the current pose solution. This process is repeated iteratively, selecting a different random subset each time. Ultimately, the algorithm chooses the pose solution that receives the most "interior" support as the optimal solution. For example, even if one of the four detected corner points deviates from its true position by 10 pixels due to reflection, RANSAC, through multiple samplings, will likely find a subset consisting of the other three accurate corner points. The pose calculated from this subset will receive support from these three points, while the pose solution affected by the incorrect corner point will only receive support from a few points. Therefore, by introducing RANSAC, the system can effectively identify mismatched corner points as "outliers" and exclude them from the final pose calculation, ensuring that the output attitude parameters such as the relative deflection angle remain highly stable even under noise interference, meeting the accuracy requirements of multi-vehicle coordinated control.

[0059] S107. Generate pose recognition results based on relative distance and relative deflection angle.

[0060] The relative distance is the master-slave vehicle distance determined in step S105. The relative deflection angle is the master-slave vehicle attitude rotation relationship calculated in step S106. The pose recognition result is a comprehensive data structure that fully describes the three-dimensional spatial position and attitude of the slave vehicle relative to the master vehicle.

[0061] Specifically, the multi-vehicle linkage control system combines the high-precision or downgraded relative distance determined in step S105 with the relative deflection angle calculated in step S106. This combination process typically integrates the relative distance information into the translation vector output by the pose calculation algorithm, or constructs a complete 4x4 homogeneous transformation matrix. This matrix contains rotation (determined by the relative deflection angle) and translation (determined by the relative distance and direction) information, enabling the transformation of any point in the vehicle coordinate system to the master vehicle coordinate system. This final transformation matrix, or equivalent (translation + rotation) data pair, is the final pose recognition result generated in this recognition cycle, which can be used by the upper-level motion control or path planning modules.

[0062] In some embodiments, the result of this step can be generated in several ways: Optionally, the pose recognition result can be a vector containing six degrees of freedom [x, y, z, roll, pitch, yaw], where [x, y, z] are determined by the relative distance and the deflection angle; alternatively, the result can also be a quaternion plus a three-dimensional translation vector, which is more computationally stable and avoids the gimbal lock problem. It is understood that the representation of the result can be customized according to the needs of downstream modules, for example, directly outputting a 4x4 homogeneous transformation matrix; this is not limited here.

[0063] A key issue that may arise during implementation is the high-frequency jitter in the output pose results. Even when the vehicle is stationary, measurement noise can cause slight fluctuations in the results, affecting the smoothness of control. To address this, the multi-vehicle linkage control system should add a filtering step before outputting the final pose recognition result. A simple time-window moving average filter can be used to smooth the output pose parameters, or a more advanced Kalman filter can be employed. The Kalman filter can predict the pose at the next moment based on the vehicle's motion model and fuse the current measurement results with the prediction results, thereby effectively filtering out measurement noise and outputting a smoother and more stable pose estimate, improving the control quality of the entire linkage system.

[0064] The following provides a more detailed description of the process of the method provided in this implementation. Please refer to [link / reference]. Figure 2 This is another flowchart illustrating the multi-vehicle linkage pose recognition method in this application embodiment.

[0065] S201. Control the laser rangefinder to measure the distance to the target vehicle and obtain the initial distance value.

[0066] The laser rangefinder is a high-precision ranging device installed on the main vehicle. The target vehicle is the object to be ranged. The initial distance value is the result of one or more reference distance measurements performed before the formal commencement of the continuous pose recognition task.

[0067] Specifically, during the initialization phase or a pre-calibration procedure of the multi-vehicle linkage control system, the system instructs the laser rangefinder to align with a stationary or slowly moving target vehicle. At this time, it is typically ensured that there are no obstructions between the sensor and the vehicle to guarantee the accuracy of the measurement results. The sensor emits a laser beam, measures the round-trip time, and calculates a highly accurate distance. To improve reliability, the system may perform multiple measurements and take the average; this stable and accurate measurement result is recorded as the initial distance value.

[0068] In some embodiments, this step can be implemented in several ways to ensure the accuracy of the initial distance value: Optionally, before measurement, the system can first roughly locate the center of the ARUCO marker using a camera, and then guide a steerable laser rangefinder sensor to precisely align with that center before taking the measurement, ensuring that the target center point is being measured; alternatively, if the laser rangefinder sensor is fixedly mounted, the system can require the operator to place the master and slave vehicles in specific calibration positions during initial setup, ensuring that the laser beam can stably illuminate a specific reflector or ARUCO marker area on the slave vehicle. It is understood that other methods can also be used to obtain the initial distance, such as obtaining an initial vehicle distance in an open area using higher-precision differential GPS (RTK-GPS), which is not limited here.

[0069] A key challenge during implementation is ensuring that, in the initial calibration scenario, the laser sensor is indeed measuring the distance from the vehicle in question, rather than from other objects in the background. To address this, a multi-vehicle linkage control system can employ a handshake mechanism. For example, the master vehicle can issue a command to have a specific indicator light on the slave vehicle (e.g., the LED next to the ARUCO logo) flash at a specific frequency. While the master vehicle's camera observes the area, the laser rangefinder performs scanning distance measurement. Only when the camera detects a light signal matching the flashing frequency, and the laser distance measurement value in the direction of the signal source is within a reasonable range, does the system confirm that the distance value is a valid initial distance value.

[0070] S202. Based on the initial distance value and the preset ARUCO identifier size, calculate the expected pixel size of the ARUCO identifier on the imaging plane of the initial distance value.

[0071] The initial distance value is the precise distance measured in step S201. The preset ARUCO identifier size is the known real-world side length of the identifier. The imaging plane is the two-dimensional plane of the sensor inside the camera. The expected pixel size refers to the pixel size that the ARUCO identifier should appear in on the imaging plane under ideal conditions without any camera parameter deviations.

[0072] Specifically, the multi-vehicle linkage control system again applies the inverse process of the perspective projection model. The ideal pinhole camera model formula is: object height in the image = (focal length * actual object height) / distance from the object to the camera. The multi-vehicle linkage control system substitutes the known preset marker size (actual object height), the initial distance value obtained in step S201, and the focal length in the camera's pre-calibration intrinsic parameters into this formula. The calculated result is the number of pixels (e.g., the side length in pixels) that the ARUCO marker should theoretically occupy in the camera image at the current initial distance; this value is the expected pixel size.

[0073] A key issue that may arise during implementation is that when calculating the expected pixel size, the ARUCO logo on the vehicle may not be perfectly aligned with the main vehicle's camera, exhibiting a slight tilt angle. This causes distortion in its projection onto the image, making the concept of pixel height ambiguous. To address this, the multi-vehicle linkage control system should, as far as possible, ensure that the vehicle is directly aligned with the main vehicle during the initial calibration phase. If this cannot be guaranteed, the system should also perform a complete pose calculation during the initial calibration phase to obtain the initial relative tilt angle. Then, when calculating the expected pixel size, instead of calculating a single side length value, a complete quadrilateral with perspective distortion is simulated and generated; the coordinates of its four vertices are the expected pixel coordinates.

[0074] S203. Adjust the pre-calibration intrinsic parameters of the 2D camera based on the expected pixel size.

[0075] The expected pixel size is the theoretical value calculated in step S202. The pre-calibration intrinsic parameters of the 2D camera are the internal parameters currently used by the camera. This step aims to physically adjust the camera's optical parameters to ensure that the ARUCO logo is presented at an optimal size in the image, facilitating subsequent accurate identification.

[0076] Specifically, the multi-vehicle linkage control first sets an ideal range for the pixel size of the ARUCO sign. For example, it is considered optimal for the sign's side length to be between 80 and 200 pixels in the image. Too small a size may result in blurry recognition due to insufficient resolution, while too large a size may prevent complete imaging if a vehicle is too close. Then, the system compares the "expected pixel size" calculated in step S202 with this ideal range. 1. If the expected pixel size is less than the lower limit of the ideal range (e.g., less than 80 pixels), it indicates that the target is too far from the vehicle, and the marker is too small in the image. In this case, the system will send a command to the camera driver to increase its physical focal length (i.e., "adjust focal length"). Increasing the focal length is equivalent to the "zooming in" operation of optical zoom, which makes the distant ARUCO marker larger and clearer on the imaging plane, thus avoiding a blurry marker due to the distance from the vehicle.

[0077] 2. If the expected pixel size exceeds the upper limit of the ideal range (e.g., greater than 200 pixels), it indicates that the target is very close to the vehicle, and the logo is too large in the image, posing a risk of being outside the field of view. In this case, the system will send a command to reduce its physical focal length. Reducing the focal length will expand its field of view (i.e., "adjust wide-angle"), which is equivalent to the "shrinking" operation of optical zoom. This ensures that even at close range, the entire ARUCO logo can be captured completely in the image, avoiding recognition failure due to incomplete field of view.

[0078] 3. If the expected pixel size is within the ideal range, no adjustment is needed.

[0079] After the physical focal length is adjusted, the camera's intrinsic parameters (especially the focal length parameters fx and fy) will change. Therefore, the intrinsic parameter matrix used at the software level also needs to be updated. The system will look up the updated intrinsic parameter matrix from a pre-calibrated focal length-intrinsic parameter mapping table or directly obtain it from the camera driver based on the new focal length setting, for use in subsequent pose calculation steps (S209). Through this closed-loop dynamic adjustment, the system ensures that the camera can always acquire high-quality, appropriately sized ARUCO marker images regardless of changes in the distance between the master and slave vehicles, greatly improving the accuracy and robustness of marker recognition, thereby ensuring the accuracy of the final pose recognition result.

[0080] In addition, the system sets an effective search range for marker size based on the expected pixel size. For example, if the expected pixel size is 100 pixels, the system can set a dynamic threshold, such as adjusting the minimum and maximum marker perimeters in the detector parameters to only search for candidate objects with perimeters within a certain range of the expected value (e.g., 50% to 200%). This can effectively filter out noise or irrelevant patterns in the image and avoid misidentification. The system can use the expected pixel size to generate a dynamic Region of Interest (ROI). Based on the perspective projection model, the expected pixel size w_img satisfies the following relationship with the initial distance value d measured by laser, the physical size of the marker s, and the camera focal length f_x: w_img ≈ (s * f_x) / d. The system can then calculate the center position (usually based on the previous frame or the predicted vehicle position) and size of the ROI (e.g., set to 1.5 times the expected pixel size) and instruct the image processing module to perform ARUCO marker search and recognition only within this ROI. For example, if the expected size is calculated to be 80x80 pixels, the system can search within a 120x120 pixel ROI. In this way, not only is the computational resource consumption of full-image search significantly reduced and the recognition frame rate improved, but the interference from other similar features in the background environment is also significantly reduced by narrowing the search range, thereby indirectly improving the camera's accuracy and robustness in recognizing ARUCO markers at different distances.

[0081] S204. Acquire the current image frame containing the ARUCO identifier on the target vehicle using the 2D camera of the target master vehicle, obtaining two-dimensional image data. S205. Identify the pixel coordinates and color values ​​of the ARUCO identifier in the two-dimensional image data. S206. Determine the physical size data corresponding to the ARUCO identifier with the specified color value based on a color-size mapping table. S207. Calculate the estimated distance using a perspective projection model based on the physical size data, pixel coordinates, and the pre-calibration intrinsic parameters of the 2D camera. S208. Obtain the distance measurement value from the laser rangefinder sensor, and use the distance measurement value as the relative distance between the target master vehicle and the target vehicle if the deviation between the distance measurement value and the estimated distance value does not exceed a preset deviation threshold. S209. Determine the relative deflection angle between the target master vehicle and the target vehicle using a pose calculation algorithm based on the pixel coordinates of the ARUCO identifier and the pre-calibration intrinsic parameters of the 2D camera. S210. Generate a pose recognition result based on the relative distance and relative deflection angle.

[0082] Refer to steps S101-S107, which will not be repeated here.

[0083] S211. Obtain the real-time motion parameters of the target master vehicle and the target slave vehicle respectively.

[0084] Real-time motion parameters refer to physical quantities that describe the vehicle's current instantaneous motion state, mainly including linear velocity, angular velocity, linear acceleration, and angular acceleration. These parameters are usually measured by the vehicle's own sensors, such as wheel speed encoders and inertial measurement units (IMUs).

[0085] Specifically, simultaneously with or after generating the pose recognition results, the multi-vehicle linkage control system requests the current real-time motion parameters of the master vehicle from its own underlying controller via the in-vehicle network (such as the CAN bus). At the same time, if communication bandwidth allows, the master vehicle will also send requests to the target slave vehicle to obtain its real-time motion parameters. These parameters are collected and used, along with the pose recognition results, for higher-order analysis and control. For example, the master vehicle's IMU can provide its own acceleration and angular velocity, while the wheel speed encoders can provide its linear velocity.

[0086] In some embodiments, parameter acquisition in this step can be achieved in several ways: Optionally, both the master vehicle and the slave vehicle periodically broadcast their status information, including motion parameters, to a central node (or the master vehicle), and the multi-vehicle linkage control system directly parses the required data from the latest broadcast packet; Optionally, a request-response mode is adopted, where the multi-vehicle linkage control system actively queries other vehicles for motion parameters only when needed, in order to reduce unnecessary network communication. It is understood that if the motion parameters of the slave vehicle cannot be directly obtained, the system can also estimate the relative motion parameters of the slave vehicle by performing differential and filtering on historical pose recognition results; this is not limited here.

[0087] A key issue that may arise during implementation is the asynchronous timestamps of motion parameters uploaded by different vehicles, leading to difficulties in data fusion. For example, the master vehicle might use its current speed, while the received speed from a slave vehicle is from 50 milliseconds ago. To address this, the multi-vehicle linkage control system must implement a time synchronization mechanism. All vehicles can align their local clocks with a unified time base via Network Time Protocol (NTP) or GPS signals. Each data point uploaded with motion parameters must be accompanied by a precise timestamp. This allows the control system to interpolate or extrapolate data from different sources to the same point in time during data fusion, ensuring data consistency.

[0088] S212. Based on the pose recognition results and real-time motion parameters, determine the relative motion trajectory within a preset time interval in the future.

[0089] The pose recognition result is the current relative position and orientation generated in step S210. The real-time motion parameters are the motion states of each vehicle obtained in step S211. The preset time interval is a short future time period, such as the next 0.5 seconds. The relative motion trajectory is the predicted path of the vehicle's pose relative to the master vehicle changing over time within this time period.

[0090] Specifically, the multi-vehicle linkage control system uses a kinematic model for trajectory prediction. It takes the current relative pose as the initial state and, combined with the real-time linear and angular velocities of the master and slave vehicles, calculates the relative pose at the next moment through integration. For example, future relative position = current relative position + (slave vehicle velocity - master vehicle velocity) * time step. By continuously iterating this calculation process within a preset time interval, the system can obtain a series of relative poses at future time points; connecting these points constitutes the predicted relative motion trajectory.

[0091] In some embodiments, trajectory prediction in this step can be achieved in several ways: Optionally, a simple linear or uniform motion model can be used, assuming the vehicle maintains its current speed for a short period of time. This method is computationally simple and suitable for low-dynamic scenarios. Optionally, a more complex dynamic model can be used, which integrates factors such as the vehicle's acceleration, mass, and moment of inertia, and can more accurately predict the vehicle's trajectory during acceleration, deceleration, or turning, making it suitable for high-dynamic scenarios. It is understood that data-driven methods can also be used, such as training a recurrent neural network (RNN) or a long short-term memory network (LSTM), to predict future trajectories based on historical pose and motion parameter sequences; this is not limited here.

[0092] It's important to note that a kinematic model is a mathematical model that describes the geometric motion of a vehicle without considering mechanical effects, and it's crucial for trajectory prediction. For a typical differential drive or steering wheel AGV, its kinematic model can be described by a state vector X = [x, y, θ], where (x, y) is the vehicle's position in the world coordinate system, and θ is its heading angle. The model's inputs are the vehicle's control variables, namely linear velocity v and angular velocity w. The core of the model is a set of differential equations describing the rate of change of the state over time. This model shows that the vehicle's position change is the result of decomposing its linear velocity along the current heading angle, and the rate of change of the heading angle is its angular velocity. This model has a simple structure and high computational efficiency, sufficient to accurately describe the vehicle's trajectory in short-term predictions. Unlike deep learning models that require large amounts of data for "training," kinematic models are based on physical principles, and their "training" process is essentially "parameter identification" or "calibration." The key parameters in this model are not learned but are inherent physical properties of the vehicle. For example, for a differential drive AGV, its wheelbase L and wheel radius r need to be accurately measured. These parameters are typically obtained through physical measurements and fine-tuned through actual testing. For instance, a known left and right wheel speed command can be given to the vehicle, and then high-precision positioning equipment (such as a laser tracker) can be used to measure its actual linear and angular velocities over a period of time. By comparing the theoretically calculated values ​​with the actual measured values, the wheelbase L and effective wheel radius r are corrected in reverse to compensate for the effects of non-ideal factors such as tire slippage and wear, ensuring the accuracy of the model parameters.

[0093] In step S212, this model is used for trajectory prediction. The input is: 1) The current relative pose of the master and slave vehicles obtained in step S210 (as the initial state for prediction); 2) The real-time motion parameters (v_m, w_m and v_f, w_f) of the master vehicle and slave vehicle obtained in step S211.

[0094] Numerical integration is performed, meaning the system iterates within a preset future time interval (e.g., 0.5 seconds) at small time steps Δt (e.g., 0.01 seconds). Within each step, the model updates the predicted poses of the master and slave vehicles using their respective velocities v and angular velocities w, through the aforementioned differential equations (e.g., x_new = x_old + v * cos(θ) * Δt). Then, the difference between the two predicted poses is calculated to obtain the relative pose at the next moment. This process is repeated until the entire prediction time interval is covered. The output is a sequence of relative poses of the master and slave vehicles at future time points; these points, when connected, constitute the relative motion trajectory within the preset future time interval.

[0095] S213. Based on real-time motion parameters and relative motion trajectory, determine the target's motion adjustment parameters from the vehicle.

[0096] Among them, the real-time motion parameters and relative motion trajectory are the products of steps S211 and S212. The motion adjustment parameters are specific instructions sent to the vehicle's underlying controller, such as target speed, target steering angle, or target acceleration, to guide it to change its current motion state.

[0097] Specifically, the process of determining the target vehicle's motion adjustment parameters is based on an advanced control strategy that combines hierarchical PID control with dynamic feedforward compensation. This strategy decouples the complex pose tracking task into two levels: angle control and distance control, and incorporates the vehicle's motion state to reduce lag. First, there is hierarchical PID control: the system sets control priorities, adjusting the angle first, then the distance.

[0098] When the angular deviation e_θ exceeds a preset threshold (e.g., 2°), the system determines that the directional deviation is the primary issue and will prioritize activating the angle PID controller. This controller calculates the required angular velocity w for the vehicle based on the angular deviation e_θ(t) and its integral and derivative using the formula w(t) = K_pθ*e_θ(t) + K_iθ*∫e_θ(τ)dτ + K_dθ*de_θ(t) / dt. At this point, the linear velocity is limited to a lower value to ensure steering safety.

[0099] Once the angular deviation converges to within the threshold, the system simultaneously activates the distance PID controller. This controller, based on the distance deviation e_d(t), also integrates to calculate the required linear velocity v of the vehicle. This layered decoupling avoids instability in the motion path caused by blindly adjusting the distance when the angular deviation is large.

[0100] Secondly, dynamic feedforward compensation is used: To address the inherent lag problem of pure feedback control (PID) during the movement of the master vehicle, feedforward compensation is introduced into the system. It directly acquires the real-time motion parameters of the master vehicle (linear velocity v_m and angular velocity w_m) and adds them as compensation quantities to the control commands of the slave vehicle. Ultimately, the commands executed by the slave vehicle are v_final = v_pid + v_m and w_final = w_pid + w_m.

[0101] For example, when the main vehicle moves forward at a speed of 0.5 m / s, even if the speed calculated by the PID controller is 0, the slave vehicle will still receive a base speed command of 0.5 m / s, thus achieving synchronized movement with the main vehicle, instead of starting to catch up only after the distance has increased. Through this entire set of control logic, the motion adjustment parameters finally generated by the system are the final linear velocity v_final and angular velocity w_final, which have been precisely calculated and compensated.

[0102] In some embodiments, parameter determination in this step can be achieved in several ways: Optionally, a classic PID controller can be used to control lateral, longitudinal, and heading deviations separately, which is simple in structure and easy to implement and debug; alternatively, a more advanced model predictive control (MPC) can be employed. MPC can solve an optimization problem in each control cycle to find a series of optimal control inputs (motion adjustment parameters) for the future, so that the predicted trajectory is closest to the desired trajectory while satisfying vehicle dynamics constraints. MPC can better handle constraint and multivariable coupling problems, resulting in better control performance. It is understood that other control algorithms, such as sliding mode control or adaptive control, can also be used, and are not limited here.

[0103] In some embodiments, the multi-vehicle linkage control system performs more refined attitude recognition for slave vehicles with articulated structures or rotatable mechanisms. That is, the ARUCO identifier is cut into multiple sub-identifiers and respectively configured at different vehicle body positions of the target slave vehicle's rotatable mechanism or articulated structure. The multi-vehicle linkage control system identifies the pixel coordinate information of the multiple sub-identifiers of the ARUCO identifier in the two-dimensional image data to obtain multiple sets of molecular coordinate data. It also identifies the identifier color values ​​of the multiple sub-identifiers in the two-dimensional image data.

[0104] In this context, a sub-identifier refers to multiple smaller blocks logically or physically divided into a complete ARUCO identifier, each of which is itself a recognizable ARUCO identifier. These sub-identifiers are installed on different moving parts of the target vehicle, such as the cab and cargo box of an articulated truck. Multiple sets of sub-identifier coordinate data refer to multiple sets of pixel coordinates obtained after corner detection for each sub-identifier.

[0105] Specifically, when the multi-vehicle linkage control system is handling an articulated vehicle, it activates a special recognition mode. In this mode, the image processing module no longer searches for a single large ARUCO identifier, but instead searches for and recognizes multiple sub-identifiers with predefined IDs in parallel. For example, the sub-identifier with ID 10 is installed at the front of the vehicle, and the sub-identifier with ID 11 is installed in the passenger compartment. The system scans the image, detects all these sub-identifiers individually, and records the independent pixel coordinates (four corner points) and identifier color value for each successfully recognized sub-identifier. Ultimately, this step outputs a data structure containing multiple members, each member corresponding to a sub-identifier and its related information.

[0106] In some embodiments, this step can be implemented in several ways to improve the efficiency and accuracy of multi-identifier recognition: Optionally, multi-threading or parallel computing can be used to simultaneously detect sub-identifiers with different IDs in a single image scan to speed up processing; alternatively, spatial constraints between sub-identifiers can be utilized to assist in recognition. For example, if the system knows that the sub-identifiers of the front and the carriage usually appear in adjacent areas in an image, and if the front identification is detected in a region, the system can focus on searching for carriage identifications in its neighborhood, thereby improving the detection success rate and eliminating false targets in the distance. It is understood that other multi-target detection techniques can also be used, and are not limited here.

[0107] A key issue that may arise during implementation is that when a vehicle makes a sharp turn, a sub-marker might be obscured by another part of the vehicle. For example, the passenger compartment might block a sub-marker on the front of the vehicle. To address this, the multi-vehicle linkage control system should be capable of handling missing information. The system can maintain an independent Kalman filter for the pose estimation of each sub-marker. When a sub-marker is temporarily undetectable due to occlusion, the system will not interrupt the entire recognition process but will use the predicted value of the Kalman filter corresponding to that sub-marker as its temporary pose. Simultaneously, the system will utilize information from other unoccluded sub-markers to continue calculating the overall vehicle pose and joint angles.

[0108] In some embodiments, the multi-vehicle linkage control system uses the identified sub-identifier information to calculate the internal working posture of the articulated vehicle. Specifically, the multi-vehicle linkage control system identifies a main identifier with preset features from the multiple sub-identifiers, and uses the sub-identifiers other than the main identifier as other sub-identifiers. Using the molecular coordinate data of the main identifier as the main body, the molecular coordinate data of other sub-identifiers are transformed to the corresponding angles and positions and stitched together to obtain the main identifier image, and the main body deflection angle corresponding to the main identifier image is determined. Using the molecular coordinate data of other sub-identifiers as the main body, the transformation is performed to determine the corresponding sub-body identifier image and sub-body deflection angle. Based on the main body deflection angle and the sub-body deflection angle, the working posture of the target vehicle is determined.

[0109] The main logo is typically a sub-logo mounted on the main body of the vehicle. The main body deflection angle is the attitude of the main body of the vehicle. The sub-body deflection angle is the attitude of the movable parts of the vehicle (such as the passenger compartment). Operating attitude refers to parameters describing the angles of joints within the vehicle, such as hinge angles.

[0110] Specifically, the multi-vehicle linkage control system first designates one sub-identifier (such as the front vehicle identifier) ​​as the main identifier, and others (such as the vehicle body identifier) ​​as sub-body identifiers. Then, the multi-vehicle linkage control system uses the molecular coordinate data of the main identifier as the main body, transforms the molecular coordinate data of the other sub-identifiers to the corresponding angles and positions, and stitches them together to obtain a main identifier image with the main identifier as the image orientation angle; then, using the molecular coordinate data of the other identifiers as the main body, it transforms the molecular coordinate data of the main identifier and other other sub-identifiers to the corresponding angles and positions and stitches them together to obtain multiple sub-body identifier images with their own identifiers as the image orientation angles. Then, it calls the pose calculation algorithm (such as step S106) on the main identifier image and the sub-body identifier images respectively to obtain the attitude of the main vehicle body relative to the camera (main body deflection angle) and the attitude of the sub-vehicle body relative to the camera (sub-body deflection angle). Since these two attitudes are described relative to the same camera coordinate system, the relative attitude of the sub-body relative to the main body can be calculated by a simple coordinate system transformation (for example, multiplying the inverse of the attitude matrix of the main body by the attitude matrix of the sub-body). The yaw angle component in this relative attitude directly corresponds to the articulation angle of the articulated vehicle, that is, the working attitude of the target vehicle.

[0111] In some embodiments, the multi-vehicle linkage control system uses pose information to estimate the speed of the trailing vehicle. Specifically, the multi-vehicle linkage control system calculates the distance difference between the initial distance value and the distance measurement value; records the acquisition timestamps of the initial distance value and the distance measurement value respectively, calculates the time difference; and determines the speed of the target trailing vehicle based on the relative deflection angle, the distance difference, and the time difference.

[0112] The initial distance value and the measured distance value are two valid distance readings from different time points. The acquisition timestamp is the precise time at which these two readings occurred. The travel speed is a vector describing the speed and direction of the target's movement from the vehicle.

[0113] Specifically, in multi-vehicle linkage, the linkage control system maintains a historical distance database, storing the most recent valid distance measurements and their timestamps. When speed calculation is needed, the system retrieves the latest distance measurement and timestamp, and finds an earlier distance value (e.g., the initial distance value from 1 second ago) and its timestamp from the database. The distance difference is obtained by subtracting the two distance values, and the time difference is obtained by subtracting the two timestamps. Dividing the distance difference by the time difference yields the average relative speed (radial speed) of the vehicle in the line-of-sight direction of the master vehicle. Combined with the relative deflection angle calculated in step S209, the system can project this radial speed onto the world coordinate system or the master vehicle coordinate system, thereby obtaining more complete target vehicle velocity vector information.

[0114] In some embodiments, velocity determination in this step can be achieved in several ways: Optionally, a simple two-point difference method can be used, as described above, which is simple and fast to calculate, but sensitive to measurement noise; alternatively, linear regression can be performed on multiple distance measurements over a period of time to fit a straight line showing the distance changing over time, the slope of which is the relative velocity. This method utilizes more data points, resulting in smoother results and stronger noise resistance. It is understood that velocity can also be treated as a state variable and incorporated into an extended Kalman filter (EKF) for joint estimation with pose to obtain the optimal velocity estimate; however, this is not limited here.

[0115] A key issue that may arise during implementation is that distance changes alone only yield radial velocity, not tangential velocity (i.e., movement perpendicular to the line of sight). To address this, the multi-vehicle linkage control system must incorporate changes in relative deflection angle to calculate tangential velocity. The system can perform difference calculations on the target's translation vectors (tx, ty, tz) in the camera coordinate system calculated from two consecutive frames. The rate of change of tz corresponds to radial velocity, while the rates of change of tx and ty correspond to the target's movement on the imaging plane, from which tangential velocity can be calculated. Finally, combining the radial and tangential velocities into a complete three-dimensional velocity vector is necessary to comprehensively describe the target's speed relative to the vehicle.

[0116] In some embodiments, the multi-vehicle linkage control system uses color information for target classification and obstacle avoidance. Specifically, when the color value of the identifier does not belong to the color feature library of the preset interactive device, the multi-vehicle linkage control system determines the device identifier code corresponding to the identifier color value; and generates a device avoidance prompt based on the pose recognition result and the device identifier code.

[0117] The preset color feature library for interactive devices is a collection containing the ARUCO identification colors of all friendly and cooperative vehicles. The device identification code is a unique identifier for non-friendly devices, which can be the color value itself. Device avoidance warnings are warnings or instructions issued to the main vehicle control system or driver to avoid collisions with the device.

[0118] Specifically, after the multi-vehicle linkage control system identifies the color value of the ARUCO identifier in step S102, it first queries an internal friendly color library. If the color value exists in the library, it proceeds according to the normal multi-vehicle collaborative process. If it does not exist, the system marks this target as an "unknown external device." It uses the color value itself or a hash value generated based on the color as a temporary device identifier for this unknown device. Then, the system continues to identify the pose of the unknown device and, combined with its pose (position and orientation) and size information, assesses whether it poses a collision risk to the main vehicle. If a risk exists, the system generates a device avoidance prompt, such as highlighting the device on the interface and issuing a voice alarm like "Caution: Avoid the unknown forklift ahead."

[0119] In some embodiments, the avoidance logic for this step can be implemented in several ways: optionally, the avoidance prompt can be a simple warning, decided by a human driver; optionally, for fully autonomous vehicles, the prompt can be a specific motion planning instruction, such as triggering emergency braking or replanning a local path that can bypass the device. It is understood that the system can also continuously track unknown devices, even if they do not have an ARUCO identifier, by tracking them through features such as their visual outline and color, thereby achieving avoidance of unmarked obstacles; this is not limited here.

[0120] A key challenge during implementation was handling third-party equipment within a factory, often marked with various colors, that were not part of the system's capabilities. Simply treating all unknown colors as obstacles could lead to an overly conservative system, resulting in frequent avoidance maneuvers and reduced efficiency. To address this, a multi-vehicle linkage control system can introduce a graylist or learning mechanism. When the system first encounters an unknown color, it issues a warning. However, if the operator manually confirms the equipment is harmless, or if the system observes the equipment appearing multiple times on the work path without causing conflict, the system can add that color to a graylist. For equipment on the graylist, the system still tracks its position, but issues lower-priority warnings instead of triggering emergency avoidance, thus achieving a balance between safety and efficiency.

[0121] In this embodiment, by employing a fusion strategy of cross-validation between camera-estimated distance and laser-measured distance, and a flexible design based on mapping physical dimensions using marker colors, this method can effectively identify and filter out erroneous ranging data caused by physical occlusion or sensor deviation, while also being compatible with target vehicles of different specifications. This effectively solves the problems of poor reliability and easy deviation in pose recognition of single-sensor solutions in complex environments in existing technologies, thereby achieving robust and accurate identification of the relative poses of multiple vehicles in high-dynamic and high-interference scenarios. Furthermore, by introducing technologies such as dynamic camera intrinsic parameter calibration, articulated structure attitude calculation, motion speed estimation, and forward-looking trajectory prediction, the accuracy, applicability, and intelligence level of the system are further improved, providing a solid technical foundation for achieving safer, smoother, and more efficient multi-vehicle collaborative operations.

[0122] The multi-vehicle linkage control system in the embodiments of this invention is described below from the perspective of hardware processing. Please refer to [link / reference needed]. Figure 3 This is a schematic diagram of the physical device structure of a multi-vehicle linkage control system in the embodiments of this application.

[0123] It should be noted that, Figure 3 The structure of the multi-vehicle linkage control system shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of the present invention.

[0124] like Figure 3 As shown, the multi-vehicle linkage control system includes a CPU 301, which can perform various appropriate actions and processes according to a program stored in ROM 302 or a program loaded from storage section 308 into RAM 303, such as executing the methods described in the above embodiments. RAM 303 also stores various programs and data required for system operation. The CPU 301, ROM 302, and RAM 303 are interconnected via bus 304. I / O interface 305 is also connected to bus 304.

[0125] The following components are connected to I / O interface 305: input section 306 including audio input devices, push-button switches, etc.; output section 307 including liquid crystal display (LCD) and audio output devices, indicator lights, etc.; storage section 308 including hard disks, etc.; and communication section 309 including network interface cards such as LAN (Local Area Network) cards, modems, etc. Communication section 309 performs communication processing via a network such as the Internet. Drive 310 is also connected to I / O interface 305 as needed. Removable media 311, such as disks, optical disks, magneto-optical disks, semiconductor memories, etc., are installed on drive 310 as needed so that computer programs read from them can be installed into storage section 308 as needed.

[0126] In particular, according to embodiments of the present invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of the present invention include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing computer programs for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 309, and / or installed from removable medium 311. When the computer program is executed by CPU 301, it performs the various functions defined in the present invention.

[0127] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. Each block in a flowchart or block diagram may represent a module, program segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those shown in the drawings.

[0128] Specifically, the multi-vehicle linkage control system of this embodiment includes a processor and a memory. The memory stores a computer program. When the computer program is executed by the processor, it implements the multi-vehicle linkage pose recognition method provided in the above embodiment.

[0129] In another aspect, the present invention also provides a computer-readable storage medium, which may be included in the multi-vehicle linkage control system described in the above embodiments; or it may exist independently and not be assembled into the multi-vehicle linkage control system. The storage medium carries one or more computer programs, which, when executed by a processor of the multi-vehicle linkage control system, enable the multi-vehicle linkage control system to implement the multi-vehicle linkage pose recognition method provided in the above embodiments.

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

[0131] As used in the above embodiments, depending on the context, the term "when..." can be interpreted as meaning "if...", "after...", "in response to determining...", or "in response to detecting...". Similarly, depending on the context, the phrase "when determining..." or "if (the stated condition or event) is interpreted as meaning "if determining...", "in response to determining...", "when (the stated condition or event) is detected", or "in response to detecting (the stated condition or event)".

Claims

1. A method for multi-vehicle coordinated pose recognition, characterized in that, The method, applied to a multi-vehicle linkage control system, includes: Two-dimensional image data is obtained by acquiring the current image frame containing the ARUCO identifier of the target from the vehicle using the 2D camera of the target vehicle; Identify the pixel coordinate information and color value of the ARUCO identifier in the two-dimensional image data; Based on the color-size mapping table, determine the physical size data corresponding to the ARUCO identifier with the specified identifier color value; Based on the physical size data, the pixel coordinate information, and the pre-calibration intrinsic parameters of the 2D camera, the distance estimate is calculated through a perspective projection model. The distance measurement value of the laser rangefinder sensor is obtained, and when the deviation between the distance measurement value and the distance estimate value does not exceed a preset deviation threshold, the distance measurement value is used as the relative distance between the target master vehicle and the target slave vehicle. Based on the pixel coordinate information of the ARUCO identifier and the pre-calibration intrinsic parameters of the 2D camera, the relative deflection angle between the target master vehicle and the target slave vehicle is determined by a pose calculation algorithm; Based on the relative distance and the relative deflection angle, a pose recognition result is generated.

2. The method according to claim 1, characterized in that, The ARUCO identifier is divided into multiple sub-identifiers, which are respectively disposed at different vehicle body positions of the target vehicle's rotatable mechanism or articulated structure. The step of identifying the pixel coordinate information and color value of the ARUCO identifier in the two-dimensional image data specifically includes: The pixel coordinate information of multiple sub-identifiers of the ARUCO identifier in the two-dimensional image data is identified to obtain multiple sets of molecular coordinate data; Identify the color values ​​of the multiple sub-identifiers in the two-dimensional image data.

3. The method according to claim 2, characterized in that, After the step of identifying the pixel coordinate information of multiple sub-identifiers of the ARUCO identifier in the two-dimensional image data to obtain multiple sets of molecular coordinate data, the method further includes: Identify a main identifier with preset features from the plurality of sub-identifiers, and use sub-identifiers other than the main identifier as other sub-identifiers; Using the molecular coordinate data of the main identifier as the main body, the molecular coordinate data of the other sub-identifiers are transformed to the corresponding angles and positions and then stitched together to obtain the main identifier image, and the main body deflection angle corresponding to the main identifier image is determined; The molecular coordinate data of the other sub-identifiers are used as the main body for transformation to determine the corresponding sub-body identifier image and sub-body deflection angle; The working posture of the target vehicle is determined based on the main body deflection angle and the sub-body deflection angle.

4. The method according to claim 1, characterized in that, Before the step of acquiring a current image frame containing the ARUCO identifier of the target vehicle from the 2D camera of the target vehicle to obtain two-dimensional image data, the method further includes: The laser rangefinder is controlled to measure the distance to the target from the vehicle to obtain an initial distance value; Based on the initial distance value and the preset ARUCO identifier size, calculate the expected pixel size of the ARUCO identifier on the imaging plane of the initial distance value; The pre-calibration intrinsic parameters of the 2D camera are adjusted based on the expected pixel size.

5. The method according to claim 4, characterized in that, After the step of controlling the laser rangefinder to measure the distance to the target from the vehicle and obtaining an initial distance value, the method further includes: Calculate the distance difference between the initial distance value and the measured distance value; Record the timestamps of the initial distance value and the distance measurement value respectively, and calculate the time difference; The target's speed from the vehicle is determined based on the relative deflection angle, the distance difference, and the time difference.

6. The method according to claim 1, characterized in that, After the step of identifying the pixel coordinate information and identifier color value of the ARUCO identifier in the two-dimensional image data, the method further includes: When the identified color value does not belong to the color feature library of the preset interactive device, the device identification code corresponding to the identified color value is determined; Based on the pose recognition result and the device identification code, a device avoidance prompt is generated.

7. The method according to claim 1, characterized in that, After the step of generating pose recognition results based on the relative distance and the relative deflection angle, the method further includes: The real-time motion parameters of the target master vehicle and the target slave vehicle are obtained respectively; Based on the pose recognition results and the real-time motion parameters, the relative motion trajectory within a future preset time interval is determined; Based on the real-time motion parameters and the relative motion trajectory, the motion adjustment parameters of the target from the vehicle are determined.

8. A multi-vehicle linkage control system, characterized in that, The multi-vehicle linkage control system includes: one or more processors and a memory; the memory is coupled to the one or more processors, the memory is used to store computer program code, the computer program code includes computer instructions, and the one or more processors call the computer instructions to cause the multi-vehicle linkage control system to perform the method as described in any one of claims 1-7.

9. A computer-readable storage medium comprising instructions, characterized in that, When the instruction is executed on the multi-vehicle linkage control system, the multi-vehicle linkage control system performs the method as described in any one of claims 1-7.

10. A computer program product, characterized in that, When the computer program product is run on the multi-vehicle linkage control system, the multi-vehicle linkage control system performs the method as described in any one of claims 1-7.

Images