Double-vehicle cooperative control method, device and equipment and storage medium

Abstract

The invention discloses a double-vehicle cooperative control method, device and equipment and a storage medium, and relates to the technical field of intelligent driving, and the method comprises the steps: obtaining scanning data of a single-line laser radar for a reflection identifier on a rear vehicle; obtaining distance measurement data of a single-beam laser range finder on the reflection identifier; performing data fusion based on the scanning data and the ranging data, and determining the relative pose of the rear vehicle and the front vehicle; generating a control instruction of the front vehicle based on the relative poses of the rear vehicle and the front vehicle and a preset cooperative motion track; and controlling the front vehicle to execute the control instruction of the front vehicle. According to the method, a single-beam laser range finder is introduced to be fused with radar data, the range finding error is effectively corrected, the estimation precision of the relative pose is improved, and closed-loop feedback control of the front vehicle is realized based on the high-precision pose, so that the stability and trajectory tracking precision of cooperative movement of the double vehicles are enhanced, and meanwhile, the advantage of low cost of the system is kept.

Description

[0001] This application claims priority to the Chinese Patent Application No. 2025118330979, filed on December 8, 2025, and entitled "A double vehicle cooperative control method, device, equipment and storage medium", the entire contents of which are incorporated herein by reference. TECHNICAL FIELD

[0002] The present application relates to the technical field of intelligent driving, in particular to a double vehicle cooperative control method, device, equipment and storage medium. BACKGROUND

[0003] With the rapid development of intelligent driving and vehicle networking technology, multi-vehicle cooperative system shows great potential in unmanned driving formation, adaptive vehicle following, intelligent cooperative parking and other scenarios. In such systems, achieving high-precision and high-reliability relative pose perception between vehicles is the core prerequisite for ensuring cooperative control performance and driving safety. Currently, the mainstream vehicle cooperative positioning method mainly relies on the following technical routes: one is the differential positioning method based on navigation satellite system, which can achieve centimeter-level absolute positioning, but has poor stability in signal shielding environments such as tunnels and urban canyons, and the system cost is high; the second is the cooperative perception scheme based on vision or millimeter wave radar, but its performance is easily affected by lighting conditions, weather and target reflection characteristics, making it difficult to guarantee all-weather robustness. Therefore, the existing cooperative positioning scheme based on laser radar is provided, which arranges cooperative targets on the vehicle and uses laser radar for scanning and recognition to realize relative pose calculation.

[0004] However, the existing scheme based on single-line laser radar still has the following outstanding defects: first, the radar itself has limited ranging accuracy, and the error will be amplified in pose calculation, resulting in relative distance estimation fluctuation, which is difficult to meet the high-precision control requirement; second, the system has poor anti-interference ability, and environmental light variation or decrease in reflective rate of reflective tape can easily cause data jump and noise surge, damaging stability; third, it is difficult to achieve low cost and high precision at the same time, and the use of high-line number radar or structured light scheme will increase the cost, deviating from the original intention of low-end application; finally, the existing scheme lacks deep closed loop with the control link, and the ranging noise is directly transmitted to the pose output, causing the estimation result to be unstable, resulting in lag and oscillation in cooperative control, which cannot realize stable formation keeping and accurate trajectory tracking. Therefore, there is an urgent need in the art for an innovative solution that combines low cost, high precision, high robustness and tight integration with cooperative control. SUMMARY

[0005] The present application provides a double vehicle cooperative control method, device, equipment and storage medium, which can realize high-precision and high-stability double vehicle relative positioning and cooperative motion control under low-cost sensor configuration.

[0006] To achieve the above object, the application adopts the following technical solutions: In a first aspect, the application provides a dual-vehicle cooperative control method, comprising: obtaining scanning data of a single-line laser radar on a reflection mark on a rear vehicle; wherein the single-line laser radar is installed at the center of the rear of a front vehicle; obtaining ranging data of a single-beam laser range finder on the reflection mark; wherein the single-beam laser range finder is installed on both sides of the single-line laser radar at the rear of the front vehicle, and the ranging direction of the single-beam laser range finder is coplanar with the scanning plane of the single-line laser radar; performing data fusion based on the scanning data and the ranging data to determine the relative pose of the rear vehicle and the front vehicle; generating a control instruction of the front vehicle based on the relative pose of the rear vehicle and the front vehicle and a preset cooperative motion trajectory; controlling the front vehicle to execute the control instruction of the front vehicle.

[0007] In some possible implementation manners, the determination of the relative pose of the rear vehicle and the front vehicle based on the data fusion of the scanning data and the ranging data comprises: determining the coordinates of each reflection mark based on the scanning data and the ranging data according to a variance weighting model; obtaining an optimized coordinate estimation value of each reflection mark based on the coordinates of each reflection mark and an extended Kalman filtering algorithm; calculating the relative rotation matrix and the translation vector between the front vehicle and the rear vehicle based on at least three optimized coordinate estimation values of the reflection marks through a rigid body transformation algorithm; determining the relative pose of the rear vehicle and the front vehicle according to the relative rotation matrix and the translation vector between the front vehicle and the rear vehicle.

[0008] In some possible implementation manners, the determination of the coordinates of each reflection mark based on the scanning data and the ranging data according to the variance weighting model comprises: processing the scanning data to identify each reflection mark and extract the angle information and the first distance information of each reflection mark in the single-line laser radar coordinate system; associating and matching the ranging data obtained by each single-beam laser range finder with the first distance information of the corresponding reflection mark; fusing the ranging data and the first distance information that are successfully matched based on the variance weighting model to obtain the fused distance information of each reflection mark; determining the coordinates of each reflection mark based on the angle information and the fused distance information of each reflection mark.

[0009] In some possible implementation manners, the obtaining of the optimized coordinate estimation value of each reflection identifier based on the coordinate of each reflection identifier and an extended Kalman filtering algorithm comprises: The coordinate of each reflection identifier is taken as a state variable, the angle information and the fusion distance information are taken as observation variables, and an extended Kalman filtering algorithm is used for time series fusion to obtain the optimized coordinate estimation value of each reflection identifier.

[0010] In some possible implementation manners, the calculation of the relative rotation matrix and the translation vector between the front vehicle and the rear vehicle based on the at least three optimized coordinate estimation values of the reflection identifiers comprises: A first judgment result is obtained by judging whether the coordinate estimation values of the plurality of reflection identifiers satisfy a collinear condition or not; If the first judgment result indicates that the coordinate estimation values of the plurality of reflection identifiers do not satisfy the collinear condition, a singular value decomposition algorithm is used to calculate the relative rotation matrix and the translation vector; If the first judgment result indicates that the coordinate estimation values of the plurality of reflection identifiers satisfy the collinear condition, a principal component analysis algorithm is used to extract a principal direction of the reflection identifier point set, and a relative rotation matrix is calculated based on the principal direction of the reflection identifier point set using a Rodrigues formula, and then a translation vector is calculated according to the relative rotation matrix.

[0011] In some possible implementation manners, the generation of the control instruction of the front vehicle and the control instruction of the rear vehicle based on the relative pose of the rear vehicle and the front vehicle and a preset cooperative motion trajectory comprises: A geometric center of the front vehicle and the rear vehicle is set as a cooperative center point; An expected motion speed and an expected relative pose of the cooperative center point are obtained; According to a rigid body kinematics principle, the expected motion speed of the cooperative center point is decomposed to obtain a first expected speed of the front vehicle; The first expected speed is corrected based on a pose error between the relative pose of the rear vehicle and the front vehicle and the expected relative pose to generate the control instruction of the front vehicle.

[0012] In some possible implementation manners, the correction of the first expected speed based on the pose error between the relative pose of the rear vehicle and the front vehicle and the expected relative pose to generate the control instruction of the front vehicle comprises: The pose error between the relative pose of the rear vehicle and the front vehicle and the expected relative pose is calculated; A speed compensation amount is calculated according to the pose error; The speed compensation amount and the first expected speed are added to obtain the control instruction of the front vehicle.

[0013] In a second aspect, the present application provides a dual-vehicle cooperative control device, the device comprising: an acquisition module configured to acquire scanning data of a single-line laser radar on a reflected mark on a rear vehicle, wherein the single-line laser radar is installed at the center of the rear of a front vehicle; and acquire ranging data of a single-beam laser range finder on the reflected mark, wherein the single-beam laser range finder is installed on both sides of the single-line laser radar at the rear of the front vehicle, and the ranging direction of the single-beam laser range finder is coplanar with the scanning plane of the single-line laser radar; a fusion module configured to perform data fusion based on the scanning data and the ranging data to determine the relative pose of the rear vehicle and the front vehicle; a control module configured to generate a control instruction of the front vehicle based on the relative pose of the rear vehicle and the front vehicle and a preset cooperative motion trajectory, and control the front vehicle to execute the control instruction of the front vehicle.

[0014] In a third aspect, the present application provides a computing device comprising a memory and a processor; wherein one or more computer programs are stored in the memory, the one or more computer programs comprising instructions; when the instructions are executed by the processor, the computing device performs the method of any one of the first aspect.

[0015] In a fourth aspect, the present application provides a computer-readable storage medium for storing a computer program, the computer program being used to perform the method of any one of the first aspect.

[0016] In a fifth aspect, the present application provides a computer program product comprising one or more computer instructions, when the computer instructions are executed by a computer, the computer performs the method of any one of the first aspect.

[0017] From the above technical solutions, the present application has at least the following beneficial effects: In the present application, the scanning data of a single-line laser radar on a reflection mark on a rear vehicle is obtained; the ranging data of a single-beam laser range finder on the reflection mark is obtained; data fusion is performed based on the scanning data and the ranging data to determine the relative pose of the rear vehicle and the front vehicle; a control instruction of the front vehicle is generated based on the relative pose of the rear vehicle and the front vehicle and a preset cooperative motion trajectory; and the front vehicle is controlled to execute the control instruction of the front vehicle. In the prior art, the relative pose is usually calculated only by relying on a single-line laser radar, and the inherent ranging error of the single-line laser radar will directly lead to inaccurate pose estimation, and in a dynamic scene, there is a lack of effective closed-loop control mechanism, so that the stability and precision of the cooperative motion of the two vehicles are limited. It can be seen that, by introducing the single-beam laser range finder and fusing the radar data, the ranging error is effectively corrected, the estimation precision of the relative pose is improved, and the closed-loop feedback control of the front vehicle is realized based on the high-precision pose, so that the stability and trajectory tracking precision of the cooperative motion of the two vehicles are enhanced, while the low-cost advantage of the system is maintained.

[0018] It should be understood that the description of technical features, technical solutions, beneficial effects or similar language in the present application does not imply that all features and advantages can be achieved in any single embodiment. On the contrary, it can be understood that the description of a feature or a beneficial effect means that the specific technical feature, technical solution or beneficial effect is included in at least one embodiment. Therefore, the description of technical features, technical solutions or beneficial effects in the specification does not necessarily refer to the same embodiment. Furthermore, the technical features, technical solutions and beneficial effects described in the embodiments can be combined in any appropriate manner. Those skilled in the art will understand that the embodiments can be implemented without one or more specific technical features, technical solutions or beneficial effects of a specific embodiment. In other embodiments, additional technical features and beneficial effects can be identified in specific embodiments that do not embody all embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 A flowchart of a two-vehicle cooperative control method provided by an embodiment of the present application; Figure 2 A schematic diagram of a two-laser ranging fusion vehicle cooperative positioning system provided by an embodiment of the present application; Figure 3 A two-laser ranging fusion vehicle cooperative positioning single-line radar scanning reflection sticker schematic diagram provided by an embodiment of the present application; Figure 4 A schematic diagram of an experimental vehicle used in a two-vehicle cooperative control method based on single-line radar and two-laser ranging fusion positioning provided by an embodiment of the present application; Figure 5 A two-vehicle cooperative control method based on single-line radar and two-laser ranging fusion positioning provided by an embodiment of the present application is based on a cooperative center point speed two-level decomposition schematic diagram; Figure 6 a two-vehicle cooperative chart in a two-vehicle cooperative control based on single-line radar and double-laser ranging fusion positioning provided by an embodiment of the present application; Figure 7 a two-vehicle cooperative chart in a two-vehicle cooperative control based on single-line radar and double-laser ranging fusion positioning provided by an embodiment of the present application; Figure 8 a schematic diagram of a two-vehicle cooperative control device provided by an embodiment of the present application; Figure 9 a schematic diagram of a computing device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0020] The terms "first", "second", and "third" and the like in the specification and the drawings of the present application are used to distinguish different objects, rather than to define a particular order.

[0021] In the embodiments of the present application, the words "exemplary" or "for example" are used to mean serving as an example, instance, or illustration, any implementation or design solution described as "exemplary" or "for example" in the embodiments of the present application should not be construed as being more preferred or advantageous than other implementations or design solutions. Rather, the use of the words "exemplary" or "for example" is intended to present related concepts in a concrete manner.

[0022] In order to make the following embodiments clear and simple, a brief introduction of related technologies is given first: At present, the two-vehicle cooperative positioning scheme based on single-line laser radar has limited ranging accuracy and is easily affected by environmental interference, resulting in fluctuations in relative pose estimation, which is difficult to meet the needs of high-precision cooperative control.

[0023] While the use of high-precision multi-line radar or structured light sensors can improve accuracy, it will increase the cost of the system and is not suitable for low-cost application scenarios.

[0024] Therefore, the embodiments of the present application provide a two-vehicle cooperative control method, in which scanning data of a single-line laser radar on a reflective mark on a rear vehicle is obtained; ranging data of a single-beam laser range finder on the reflective mark is obtained; data fusion is performed based on the scanning data and the ranging data to determine the relative pose of the rear vehicle and a front vehicle; a control instruction of the front vehicle is generated based on the relative pose of the rear vehicle and the front vehicle and a preset cooperative motion trajectory; and the front vehicle is controlled to execute the control instruction of the front vehicle. As can be seen, the present application introduces a single-beam laser range finder and radar data fusion to effectively correct ranging errors, improve the estimation accuracy of the relative pose, and achieve closed-loop feedback control of the front vehicle based on high-precision pose, thereby enhancing the stability and trajectory tracking accuracy of two-vehicle cooperative motion while maintaining the low-cost advantage of the system.

[0025] In order to make the technical solutions of the present application clearer and easier to understand, the following describes a double-vehicle cooperative control method provided by an embodiment of the present application with reference to the accompanying drawings. As shown in Figure 1 The figure is a flowchart of a double-vehicle cooperative control method provided by an embodiment of the present application.

[0026] The method can be applied to a processing device, which can be a terminal or a server. The terminal includes but is not limited to a smartphone, a tablet computer, a notebook computer, a personal digital assistant, or a smart wearable device, etc. The server can be a cloud server, for example, a central server in a central cloud computing cluster or an edge server in an edge cloud computing cluster. Of course, the server can also be a server in a local data center. The local data center refers to a data center directly controlled by a user.

[0027] The method includes: S101, acquiring scanning data of a single-line laser radar on a reflection mark on a rear vehicle.

[0028] The purpose of this step is to use a single-line laser radar to achieve preliminary identification and positioning of a cooperative target (reflection mark), to provide reliable angle information and initial distance information for subsequent high-precision fusion positioning. Its technical advantage lies in that the single-line laser radar can scan at a high frequency and high angle resolution, ensuring the rapid response of the system to relative pose changes, and at the same time, through processing algorithms, feature points can be stably extracted from complex environments.

[0029] The single-line laser radar is a two-dimensional laser scanning device that acquires distance and angle information of a target object in a two-dimensional plane by emitting a single beam of laser and receiving reflected signals, with high angle resolution and real-time performance. The single-line laser radar is usually installed at the central position of the rear of the front vehicle, and its scanning plane is parallel to the ground, so as to effectively cover the direction of the rear vehicle.

[0030] The reflection mark is a mark point made of high-reflectivity material pasted on the body of the rear vehicle, usually circular or rectangular, which can produce strong echo signals when scanned by the laser radar, facilitating identification and extraction.

[0031] The scanning data refers to the original point cloud information acquired by the laser radar through continuous scanning, including the distance value, angle value, and reflection intensity of each scanning point. The original point cloud data contains a large number of invalid points from the vehicle body, the ground, and other environments, therefore, through intensity threshold screening, clustering analysis, and centroid calculation, the angle and preliminary distance information of each reflection mark in the radar coordinate system can be accurately identified, providing a basis for subsequent data fusion and pose solving.

[0032] In some embodiments, a single-line laser radar is installed in the center of the front of the vehicle, such as RPLIDAR A3, whose scanning plane is parallel to the ground, and the installation height is about 0.5-0.6 meters, which is used to detect the reflective markers on the rear vehicle; three high-reflectivity rectangular reflective stickers, i.e., reflective markers, are installed on the front bumper of the rear vehicle, numbered 、 、 , which form a non-isosceles triangle arrangement, which helps to eliminate the ambiguity of pose solution in subsequent steps.

[0033] Specifically, the single-line laser radar of the front vehicle scans at a frequency of 10 Hz to obtain point cloud data, identifies the reflection points of the reflective markers through intensity threshold, and extracts the center angle and distance of each reflective marker.

[0034] First, high-reflectivity point cloud data is obtained by intensity threshold screening. Let the laser radar scanning data be , where is the distance between the i-th point and the radar, i.e., the ranging value, is the scanning angle of the i-th point relative to the zero-degree direction of the radar, is the reflection intensity of the i-th point, and n is the total number of data points obtained in a single scan.

[0035] Using the high-reflectivity characteristic of the reflective markers, a first rough screening is performed to quickly eliminate most irrelevant environmental points (such as vehicle paint, road surface, etc.), reducing the data volume for subsequent processing. The high-intensity point set is defined as:

[0036] wherein is the set of high-reflectivity point clouds obtained after screening, is a certain point in , is an adaptive intensity threshold, is the ranging value, is the reflection intensity of the i-th point, is the minimum value of the effective ranging range, is the maximum value of the effective ranging range.

[0037] wherein the adaptive intensity threshold is calculated by the formula:

[0038] wherein is the mean value of the reflection intensity value, is the standard deviation of the reflection intensity value, is an adaptive factor. ​

[0039] Using a continuous clustering algorithm based on Euclidean distance, the distance between adjacent points is calculated as follows:

[0040] The Cartesian coordinate transformation relationship is as follows:

[0041]

[0042] Clustering criteria: ,in This parameter represents the clustering tolerance. Multiple scan points that may spatially belong to the same reflective marker, selected in the previous step, are grouped into a single cluster. Since a reflective marker is typically detected by multiple laser beams during radar scanning, clustering avoids misclassifying a marker as multiple targets. A continuous clustering algorithm based on Euclidean distance is employed. The advantage of this method is its ability to accurately aggregate all discrete, strongly reflective points on a reflective marker, while simultaneously distinguishing spatially separated marker points, laying the foundation for calculating the single center coordinates of each marker in the next step.

[0043] For each cluster, generate a two-dimensional coordinate system that optimally quantifies the location of the reflective marker. Simply calculating the geometric center of all points is affected by uneven point cloud density and boundary noise.

[0044] This application employs the intensity-weighted centroid method. The advantage of this method is that it assumes points with higher reflectivity are closer to the geometric center of the reflective sign, thus assigning them higher weights. The center point calculated in this way is more representative, more accurate, and more resistant to noise than the center point obtained from a simple arithmetic mean, and more accurately reflects the physical center position of the reflective sign. The formula for calculating the center position of the reflective column using the intensity-weighted centroid method is as follows:

[0045] in, For point and points Straight-line distance in Cartesian coordinate system for The Cartesian x-coordinate of the plane. for The Cartesian y-coordinate of the plane. for The Cartesian x-coordinate of the plane. for The Cartesian y-coordinate of the plane. This is the starting angle for this scan by the single-line lidar. For the angular resolution of a single-line lidar, The two-dimensional coordinates of the calculated reflection mark center, the weight The normalized intensity value.

[0046] The system uses a four-quadrant arctangent function to calculate the angle of the reflection mark center relative to the radar zero reference direction, i.e. the center angle The distance is obtained by calculating the straight-line distance between the reflection mark center coordinates and the radar origin, i.e. the distance .

[0047] S102, obtaining the ranging data of the single-beam laser range finder to the reflection mark.

[0048] The single-beam laser range finder is a high-precision ranging sensor based on the time-of-flight principle, which directly measures the distance of the target by emitting a single-beam laser and measuring its round-trip time, with millimeter-level precision and high anti-interference capability. In this scheme, two single-beam laser range finders are installed on the left and right sides of the front vehicle tail radar, respectively, with their ranging directions coplanar with the radar scanning plane and perpendicular to the front plane of the rear vehicle, to ensure that they can independently range the reflection mark on the rear vehicle. The ranging data refers to the distance value output by the laser range finder, which is usually updated at a high frequency (such as 20Hz) and aligned with the radar scanning data through a time synchronization mechanism to avoid measurement deviation caused by vehicle motion. This data serves as an effective supplement to radar ranging and is used to correct radar ranging errors in the subsequent fusion process, improving the distance estimation accuracy and stability of the overall system.

[0049] In some embodiments, two single-beam laser range finders (Laser Range Finder, LRF) are symmetrically installed on the left and right sides of the front vehicle, such as TFmini Plus, with their ranging directions coplanar with the radar scanning plane and perpendicular to the front plane of the rear vehicle, to achieve high-precision direct distance measurement.

[0050] Specifically, the left and right LRFs output ranging data , at a frequency of 20Hz, and after timestamp synchronization alignment, the ranging sequence is obtained.

[0051] To prevent ranging drift caused by vehicle dynamics, the system uses a combination of hardware time synchronization (time error <5 ms) and software interpolation compensation to ensure that the radar data and the range finder data are accurately aligned in time stamp, avoiding data misplacement caused by vehicle motion.

[0052] In the subsequent data fusion stage, the system will associate the distance value measured by the laser range finder with the reflection mark detected by the radar that best matches in angle based on the pre-calibrated relative pose relationship between the laser range finder and the radar, and determine that they are observing the same target.

[0053] S103, data fusion is performed based on the scanning data and the ranging data to determine the relative pose of the rear vehicle and the front vehicle.

[0054] In the data fusion link of the double-vehicle cooperative positioning, angle matching and target association are the core prerequisites for effective fusion of the single-line laser radar and the double single-beam laser range finder (LRF), and the core goal is to determine that the observation data of the two types of sensors correspond to the same reflective marker on the rear vehicle, thereby providing a reliable data association basis for subsequent distance fusion.

[0055] The relative installation positions of the single-beam laser range finder (LRF-L, LRF-R) on the left and right sides of the front vehicle and the single-line laser radar have been determined through the external parameter calibration in the system initialization stage, and the calibration parameters include the translation amounts (Δx_L, Δy_L) and (Δx_R, Δy_R) of the LRF relative to the radar coordinate system, and the rotation angles Δθ_L and Δθ_R. Based on these calibration parameters, the beam direction angles φ_L (left LRF) and φ_R (right LRF) of each LRF can be calculated.

[0056] The calculation of the beam direction angle takes the scanning reference direction of the single-line laser radar (usually the forward direction of the center axis of the rear of the front vehicle) as the reference, combines the installation offset angle and the lateral offset of the LRF, and is derived through geometric coordinate transformation, ensuring that φ_L and φ_R can accurately reflect the actual pointing direction of the LRF ranging beam and maintaining coplanarity with the scanning plane of the single-line laser radar, thereby providing a unified coordinate reference for angle matching.

[0057] In the set of reflective marker angles {θ_i} detected by the radar, the reflective marker index with the smallest angle difference from φ_L and φ_R and less than the threshold value α_th (5°~10°) is searched. If the matching is successful, it is considered that the LRF observation and the radar observation correspond to the same target reflective marker.

[0058] For matching failure in extreme scenarios (such as single-sided LRF being blocked or reflective marker reflectivity dropping sharply), the system adopts a fault-tolerant mechanism, only retaining the valid matching LRF data for fusion, and the invalid LRF data does not affect the overall process, ensuring that the system can still operate stably when some sensor data is abnormal, and maintaining the continuity and reliability of the double-vehicle cooperative positioning.

[0059] Through the above complete angle matching and target association process, the scanning data of the single-line laser radar and the double LRF ranging data are matched, providing a reliable data association basis for subsequent variance-weighted distance fusion and Extended Kalman Filter Fusion Strategy (EKF) temporal fusion, ensuring the effectiveness of multi-source data fusion, and thereby improving the accuracy and stability of the relative pose estimation of the double vehicles.

[0060] Then, in order to overcome the defects of traditional direct replacement method, the application proposes a dynamic fusion method based on ranging variance weighting.

[0061] Let the single-line radar ranging value be: , and the variance be: ; the LRF ranging value be: , and the variance be: .

[0062] The fusion formula is:

[0063] Fusion distance and angle Combine the calculated retroreflective sticker coordinates:

[0064] wherein, is the distance between the front vehicle and the retroreflective sticker of the rear vehicle, is the fusion weight, is the x-coordinate of the retroreflective sticker, is the y-coordinate of the retroreflective sticker, is the central angle of the retroreflective sticker.

[0065] In order to further improve the precision and stability, the application proposes an extended Kalman filter (EKF) time sequence fusion algorithm.

[0066] The state vector is:

[0067] The prediction equation is:

[0068] The observation model includes: Radar observation:

[0069] LRF observation:

[0070] Linearized Jacobian matrix:

[0071] Kalman update:

[0072] wherein, is the state variable, the state of the retroreflective sticker at time k. T is the transpose, is the posterior state estimate, which represents the observation value z k at time k and the estimated value After that, the optimal estimation of the target state is obtained. is the prior state estimation, is the posterior state estimation at time k-1, is the posterior estimation covariance matrix at time k, is the prior estimation covariance matrix, is the posterior estimation covariance matrix at time k-1, is the process noise covariance matrix, is the radar observation vector, is the target distance measured by radar, is the target angle measured by radar, is the laser ranging (LRF) observation value, is the target distance measured by laser radar, is the Jacobian matrix of the radar observation model, is the distance of the target in the radar coordinate system, is the Jacobian matrix of the laser ranging observation model, is the installation offset of the laser ranging sensor relative to the radar sensor in the x direction, is the installation offset of the laser ranging sensor relative to the radar sensor in the y direction, is the x coordinate of the retroreflective patch in the radar coordinate system, is the y coordinate of the retroreflective patch in the radar coordinate system, is the distance of the target to the laser radar sensor, is the Kalman gain, is the observation noise covariance matrix, is the observation vector, is the observation model function, is the identity matrix.

[0073] This filtering scheme can output the optimal estimated position of each retroreflective patch and its covariance in real time, which is used for attitude solving and control links, improving the positioning stability in noisy and dynamic environments.

[0074] Then, the relative pose between the front and rear vehicles (i.e., the rotation angle and translation distance between the two vehicles) is calculated.

[0075] When the three retroreflective patches are collinear or nearly collinear (i.e., the area of the triangle formed by the three points ), the traditional SVD method will fail due to insufficient rank of the covariance matrix. At this time, a special processing method is needed.

[0076] First, determine whether the three points are collinear using the area formula of a triangle:

[0077] where is the two-dimensional coordinate of the first reflection landmark in radar coordinate system, is the two-dimensional coordinate of the second reflection landmark in radar coordinate system, is the two-dimensional coordinate of the third reflection landmark in radar coordinate system, is the area of the triangle formed by the three points.

[0078] When (where is a very small threshold, such as ), the three points are considered to be collinear.

[0079] For the centroidized point set , construct the covariance matrix:

[0080] where is the matrix (in two dimensions).

[0081] Perform eigenvalue decomposition on :

[0082] where: diag , ; is the eigenvector matrix The principal direction vector is the eigenvector corresponding to the largest eigenvalue:

[0083] This vector represents the principal axis direction of the source point set.

[0084] Similarly, for the target point set , the same processing is performed to obtain:

[0085] is the rotation of the source principal direction to the target principal direction . First, calculate the rotation axis and rotation angle:

[0086]

[0087] Special handling is required when the vectors are parallel or anti-parallel: If , the vectors are parallel or anti-parallel; If , the vectors are in the same direction, ; If , the vector is reversed, requiring a 180° rotation; For the general case, the Rodrigues rotation formula is used:

[0088] For the linear configuration, there is a 180° ambiguity in the rotation. An additional constraint is needed to resolve the ambiguity: use the order information of the points, if the points have a clear order (e.g., from left to right), the correct rotation can be determined by comparing the relative positions of the points:

[0089] If the signs are different, the principal direction needs to be reversed:

[0090] After determining the first rotation matrix, the translation vector is calculated the same as the general case:

[0091] where, is the principal direction of the source point set, is the principal direction of the target point set, is the rotation axis unit vector, is the rotation angle, is the cross product matrix of k, is the second reference point in the target point set, is the first reference point in the target point set, is the translation vector, is the centroid of the target point set, is the centroid of the source point set, is the first rotation matrix.

[0092] In the dual vehicle cooperative control system, after obtaining the pose of the radar in the world coordinate system (the first rotation matrix R and the translation vector t) through the reflective tape, the next task is to convert the pose to the geometric center of the vehicle to realize the vehicle-centered motion control and trajectory tracking.

[0093] In the vehicle system, the radar is usually installed at the center of the rear, and there is a fixed rigid transformation relationship between its coordinate system and the vehicle center coordinate system. This transformation relationship can be obtained through the calibration process in the system initialization phase, which specifically includes: Fixed transformation relationship of radar to vehicle center (calibration obtained): Rotation: , translation: (position of vehicle center in radar coordinate system).

[0094] Rotation of vehicle center in world coordinate system: The rotation matrix of the radar in the world coordinate system is known as The rotation matrix of the vehicle center in the world coordinate system can be calculated by the composite rotation relationship:

[0095] The translation of the vehicle center in the world coordinate system is:

[0096] First, the vehicle center position vector in the radar coordinate system is transformed to the world coordinate system by the rotation matrix of the radar: Then, the position of the radar origin in the world coordinate system is superimposed to obtain the accurate position of the vehicle center in the world coordinate system.

[0097] The core advantage of converting the radar pose to the vehicle geometric center is to realize the unification from the sensor coordinate to the vehicle control coordinate. This step decouples perception and control, so that all control instructions (such as speed and steering) are calculated based on a stable and vehicle rigid body kinematics-compliant reference, thereby fundamentally eliminating the control deviation caused by the sensor installation position, improving the accuracy and stability of the relative pose estimation, formation keeping and trajectory tracking of the double vehicles, and laying a solid foundation for system adaptation to different vehicle models and implementation of high-order control algorithms.

[0098] S104, based on the relative pose of the rear vehicle and the front vehicle and the preset cooperative motion trajectory, generating a control instruction of the front vehicle.

[0099] In the double-vehicle cooperative control system, generating accurate control instructions is a key link to realize stable cooperative motion. Based on the high-precision relative pose information obtained in the previous link, this step generates the control instruction of the front vehicle by combining the preset cooperative motion trajectory, rigid body motion decomposition and closed-loop feedback control strategy.

[0100] Specifically, to realize the coordinated motion of the two vehicles, the system defines the center point of the line connecting the geometric centers of the two vehicles as the cooperative center point. The motion of this point represents the overall motion state of the entire double-vehicle system.

[0101] The task planning layer gives the expected motion state of the cooperative center point:

[0102] wherein, and represent the linear velocity of the cooperative center point in the x and y directions, respectively, represents the angular velocity around the vertical axis.

[0103] ​​According to the expected relative position of the two vehicles in the coordination center coordinate system r1, r1r2, r2r1, r2, the expected speed of each vehicle after decomposition is calculated by using the rigid body motion principle:

[0104] wherein, is the position vector of the vehicle relative to the coordination center, and J is the second rotation matrix.

[0105] The matrix realizes a 90-degree rotation of the vector, ensuring that the angular velocity component correctly contributes to the tangential velocity of each vehicle.

[0106] Based on the relative pose (x, y, θ) of the two vehicles output by the fusion positioning module, the deviation vector e from the expected value is calculated:

[0107] wherein, and represents the position error, represents the heading angle error.

[0108] On the basis of the decomposed speed, the error feedback correction term is superimposed to form the front vehicle control command :

[0109] The rear vehicle only executes the decomposed speed :

[0110] wherein, is the expected decomposed speed of the front vehicle, which is the basic speed calculated from the coordination center point motion state and the relative position of the front vehicle, is a proportional control coefficient, e is the deviation vector from the expected value, is an integral control coefficient, which eliminates the small static error existing in the system for a long time. is a differential control coefficient, which predicts the error change trend and improves the stability of the system. is the expected decomposed speed of the rear vehicle, which does not need additional correction and directly executes the speed.

[0111] The control command is transmitted to the chassis control unit of the two vehicles through the communication module, and is executed after speed limiting, filtering and smoothing to ensure motion safety and coordination.

[0112] S105, control the front vehicle to execute the control command of the front vehicle.

[0113] The speed limiting, acceleration smoothing and anomaly detection are performed before the instruction sending; the reliable transmission of the instruction to the chassis control unit is ensured through the real-time communication link and the redundant design; the execution state is continuously monitored and automatically switched to the safety mode when the deviation is detected; the real-time maintenance of the double vehicle cooperation is simultaneously performed, the relative distance safety is ensured and the emergency collision avoidance capability is provided. The complete control instruction execution process realizes the accurate conversion from the expected trajectory to the actual motion, ensures that the double vehicle cooperation system still has the high-precision trajectory tracking capability and motion stability under the low-cost sensor configuration, and effectively solves the control oscillation and formation instability problems caused by the ranging error in the prior art.

[0114] In order to make the scheme of the present application clearer, an embodiment of the present application provides a schematic diagram of a double laser ranging fusion vehicle cooperation positioning system, as shown in the figure, the system comprises a single-line laser radar 1, a left LRF 2, a right LRF 3, a reflective sticker group 4, a data fusion module 5, a relative pose calculation module 6, a filter output module 7 and a communication module 8. Figure 2

[0115] Specifically, the single-line laser radar 1 is installed at the center position of the rear of the front vehicle, the scanning plane is parallel to the ground, and is responsible for obtaining the reflective sticker angle information and the preliminary distance information; The left LRF 2 and the right LRF 3 are symmetrically installed on the two sides of the radar, the ranging direction is coplanar with the radar scanning plane, and the beam included angle is ±15°; the reflective sticker group 4 is fixedly installed on the front bumper of the rear vehicle and is composed of three high-reflectivity circular reflective stickers, arranged in an isosceles triangle shape, used for being scanned and detected by the radar; the data fusion module 5 is connected with the single-line radar and the left and right LRF data interfaces, used for realizing the ranging fusion according to the angle matching and the distance consistency determination; the relative pose calculation module 6 performs SVD rigid registration based on the fused reflective sticker coordinates, and outputs the relative position and attitude angle of the two vehicles; the filter output module 7 performs sliding window smoothing processing on the continuous frame results, and improves the stability; the communication and control module 8 is responsible for sending the relative pose results to the cooperation control system and sending the front vehicle and rear vehicle speeds after the speed decomposition at the cooperation center point; the front vehicle speed execution module 9 receives the speed and executes; the rear vehicle speed execution module 10 receives the speed and executes.

[0116] The above-mentioned modules are connected through the CAN bus or the Ethernet, forming a complete data acquisition and calculation link. The system not only can realize the high-precision relative pose estimation, but also can perform the real-time double vehicle cooperation control on this basis, realizing the positioning and control integration function.

[0117] ​Single-line laser radar provides high angular resolution and target recognition capability, while double LRF provides high-precision absolute distance measurement in the vertical direction. The system realizes multi-source observation correlation through angle matching, and fuses the two types of ranging information using EKF time series fusion. The sliding window filtering process further smooths the error and suppresses the dynamic noise in the time domain, achieving millimeter-level relative positioning accuracy.

[0118] The three-reflective-tape geometric relationship is used for SVD or PCA-Rodrigues to solve the rotation and translation between the two vehicles, so as to obtain the complete relative pose. The pose result is directly input into the cooperative control module, the front vehicle superimposes error compensation control on the basis of decomposed speed, and the rear vehicle maintains the formation constraint according to the decomposed speed, so as to realize the dynamic cooperation and stable motion of the two vehicles. The system forms a closed-loop system from data acquisition, fusion positioning, pose solving to cooperative control, has high robustness and scalability, and is suitable for intelligent formation, automatic parking and low-speed cooperative operation scenes.

[0119] As shown in Figure 3 The schematic diagram clearly shows the working scene of the single-line laser radar scanning the group of reflective tapes on the rear vehicle. The radar scanning plane, the intersection of the laser beam and the reflective tape, and the center points of each reflective tape obtained by clustering and centroid calculation are marked in the figure. This figure directly reflects the core link of using single-line radar for target recognition and initial positioning in the present application, and provides basic angle and preliminary distance information for subsequent data fusion.

[0120] Figure 4 The front and rear experimental vehicles used in the real vehicle verification stage of the present application are shown. In the figure, the single-line laser radar installed in the center of the tail of the front vehicle and the single-beam laser range finder (LRF) symmetrically arranged on both sides of the single-line laser radar, as well as the group of high-reflectivity reflective tapes pasted on the front bumper of the rear vehicle in a specific geometric shape (such as a triangle) can be clearly seen. The figure proves that the present technical solution has actual engineering deployment feasibility.

[0121] Figure 5 The speed two-stage decomposition based on the cooperative center point proposed in the present application is shown. The figure explains in a geometric way how to decompose the overall expected motion (linear velocity and angular velocity) of the cooperative center point into independent expected speed instructions of each vehicle according to the expected positions of the two vehicles relative to the center through rigid body kinematics. This is a key step to realize accurate cooperative motion control.

[0122] Figure 6 The actual cooperative scene of the front and rear vehicles in the motion process under cooperative control is presented. The figure dynamically shows how the two vehicles maintain the preset formation and relative pose during straight driving or turning according to the fusion positioning result and cooperative control instruction, which embodies the coordination and stability of the present method in actual operation.

[0123] Figure 7 The performance evaluation results of the application in the real vehicle tracking test are shown, including the tracking effects under two typical trajectories of straight lines and circular arcs, and the cooperative errors (such as relative distance error and relative angle error) in the straight line tracking process are quantitatively given. The test results have proved the significant effect of the method described in the application in improving the accuracy and stability of double-vehicle cooperative control.

[0124] Based on the above, the scanning data of the single-line laser radar on the reflective mark on the rear vehicle is obtained; the ranging data of the single-beam laser range finder on the reflective mark is obtained; the relative pose of the rear vehicle and the front vehicle is determined based on data fusion of the scanning data and the ranging data; the control instruction of the front vehicle is generated based on the relative pose of the rear vehicle and the front vehicle and the preset cooperative motion trajectory; and the front vehicle executes the control instruction of the front vehicle. It can be seen that the application effectively corrects the ranging error by introducing data fusion of the single-beam laser range finder and the radar, improves the estimation accuracy of the relative pose, and realizes closed-loop feedback control of the front vehicle based on high-precision pose, thereby enhancing the stability and trajectory tracking accuracy of double-vehicle cooperative motion, while maintaining the low-cost advantage of the system.

[0125] The above Figures 1 to 7 The double-vehicle cooperative control method provided by the embodiments of the application is described in detail, and the device and equipment provided by the embodiments of the application will be introduced below with reference to the drawings.

[0126] The embodiments of the application also provide a double-vehicle cooperative control device, as shown in Figure 8 The figure is a schematic diagram of a double-vehicle cooperative control device provided by an embodiment of the application, which comprises: The acquisition module 801 is configured to obtain the scanning data of the single-line laser radar on the reflective mark on the rear vehicle; wherein the single-line laser radar is installed at the center of the rear of the front vehicle; and obtain the ranging data of the single-beam laser range finder on the reflective mark; wherein the single-beam laser range finder is installed on both sides of the single-line laser radar at the rear of the front vehicle, and the ranging direction of the single-beam laser range finder is coplanar with the scanning plane of the single-line laser radar; The fusion module 802 is configured to perform data fusion based on the scanning data and the ranging data to determine the relative pose of the rear vehicle and the front vehicle. The control module 803 is configured to generate the control instruction of the front vehicle based on the relative pose of the rear vehicle and the front vehicle and the preset cooperative motion trajectory, and control the front vehicle to execute the control instruction of the front vehicle.

[0127] In some possible implementation manners, the fusion module 802 is specifically configured to determine the coordinates of each reflective mark based on the scanning data and the ranging data based on a variance weighting model. obtain an optimized coordinate estimation value of each reflection mark based on the coordinate of each reflection mark and an extended Kalman filtering algorithm; calculate a relative rotation matrix and a translation vector between the front vehicle and the rear vehicle by a rigid body transformation algorithm based on the optimized coordinate estimation values of at least three reflection marks; determine a relative pose of the rear vehicle and the front vehicle according to the relative rotation matrix and the translation vector between the front vehicle and the rear vehicle.

[0128] In some possible implementation manners, the fusion module 802 is specifically configured to process the scanning data, identify each reflection mark, and extract angle information and first distance information of each reflection mark in the single-line laser radar coordinate system; associate and match ranging data obtained by each single-beam laser range finder with the first distance information of the corresponding reflection mark; based on a variance weighting model, fuse the ranging data matched successfully and the first distance information to obtain fusion distance information of each reflection mark; determine the coordinate of each reflection mark based on the angle information and the fusion distance information of each reflection mark.

[0129] In some possible implementation manners, the fusion module 802 is specifically configured to take the coordinate of each reflection mark as a state quantity, take the angle information and the fusion distance information as an observation quantity, and adopt an extended Kalman filtering algorithm for time series fusion to obtain an optimized coordinate estimation value of each reflection mark.

[0130] In some possible implementation manners, the fusion module 802 is specifically configured to determine whether the coordinate estimation values of the plurality of reflection marks satisfy a collinear condition to obtain a first determination result; if the first determination result indicates that the coordinate estimation values of the plurality of reflection marks do not satisfy the collinear condition, a singular value decomposition algorithm is adopted to calculate the relative rotation matrix and the translation vector; if the first determination result indicates that the coordinate estimation values of the plurality of reflection marks satisfy the collinear condition, a principal component analysis algorithm is adopted to extract a principal direction of the reflection mark point set, and a relative rotation matrix is calculated based on the principal direction of the reflection mark point set using a Rodrigues formula, and then a translation vector is calculated according to the relative rotation matrix.

[0131] In some possible implementation manners, the control module 803 is specifically configured to set a geometric center of the front vehicle and the rear vehicle as a cooperative center point; obtain an expected motion speed and an expected relative pose of the cooperative center point; according to a rigid body kinematics principle, decompose the expected motion speed of the cooperative center point to obtain a first expected speed of the front vehicle. correct the first expected speed based on a pose error between the relative pose of the rear vehicle and the front vehicle and an expected relative pose, to generate a control instruction of the front vehicle.

[0132] In some possible implementation manners, the control module 803 is specifically configured to calculate a pose error between the relative pose of the rear vehicle and the front vehicle and an expected relative pose; calculate a speed compensation amount according to the pose error; add the speed compensation amount to the first expected speed to obtain the control instruction of the front vehicle.

[0133] The double-vehicle cooperative control device according to the embodiments of the present application can correspond to the method described in the embodiments of the present application, and the above-mentioned other operations and / or functions of each module / unit of the double-vehicle cooperative control device are respectively implemented to realize the corresponding flow of each method in the embodiments of the present application. For brevity, the above-mentioned operations and / or functions will not be described here again. Figure 1 The above-mentioned operations and / or functions of each module / unit of the double-vehicle cooperative control device are respectively implemented to realize the corresponding flow of each method in the embodiments of the present application. For brevity, the above-mentioned operations and / or functions will not be described here again.

[0134] The embodiments of the present application further provide a computing device. As shown in the Figure 9 The figure is a schematic diagram of a computing device provided by the embodiments of the present application. The computing device 400 includes a bus 401, a processor 402, a communication interface 403 and a memory 404. The processor 402, the memory 404 and the communication interface 403 communicate through the bus 401.

[0135] The bus 401 can be a peripheral component interconnect (PCI) bus or an extended industry standard architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience, only one thick line is used to represent the bus in the figure, but it does not mean that there is only one bus or only one type of bus. Figure 9

[0136] The processor 402 can be any one or more of a central processing unit (CPU), a graphics processing unit (GPU), a microprocessor (MP), or a digital signal processor (DSP), etc.

[0137] ​The communication interface 403 is configured to communicate with the outside. For example, in the case that the computing device is the first switch, the communication interface 403 can be configured to communicate between the first switch and the first user terminal, or between the first switch and the second switch.

[0138] The memory 404 can include a volatile memory, such as a random access memory (RAM) for example. The memory 404 can also include a non-volatile memory, such as a read-only memory (ROM), a flash memory, a hard disk drive (HDD), or a solid state drive (SSD) for example.

[0139] The memory 404 stores executable code, and the processor 402 executes the executable code to perform the aforementioned double-vehicle cooperative control method.

[0140] Specifically, in the case of the embodiment shown in the figure, and Figure 8 In the case of the embodiment shown in the figure, and Figure 8 In the case that the modules or units of the double-vehicle cooperative control device described in the embodiment are implemented by software, the software or program code required to perform the functions of the modules / units in the figure can be stored partially or entirely in the memory 404. The processor 402 executes the program code corresponding to each unit stored in the memory 404 to perform the aforementioned double-vehicle cooperative control method. Figure 8

[0141] The embodiment of the present application also provides a computer readable storage medium. The computer readable storage medium can be any available medium or data storage device that can be accessed by the computing device, such as a data center or the like. The available medium can be a magnetic medium (such as a floppy disk, a hard disk, a magnetic tape), an optical medium (such as a DVD), or a semiconductor medium (such as a solid state disk), etc. The computer readable storage medium includes instructions that instruct the computing device to perform the double-vehicle cooperative control method described above.

[0142] The embodiment of the present application also provides a computer program product, which includes one or more computer instructions. When the computer instructions are loaded and executed on the computing device, the flow or function described in the embodiment of the present application is generated entirely or partially.

[0143] ​The computer instructions can be stored in or transferred from one computer-readable medium to another computer-readable medium, such as from one website, computer, or data center to another website, computer, or data center, through wired (e.g., coaxial cable, optical fiber) or wireless (e.g., infrared, wireless, microwave, etc.) means.

[0144] The computer program product is executed by a computer, and the computer executes any one of the aforementioned dual-vehicle cooperative control methods. The computer program product can be a software installation package, and in a case where any one of the aforementioned dual-vehicle cooperative control methods is needed, the computer program product can be downloaded and executed on the computer.

[0145] The descriptions of the corresponding processes or structures of the above respective figures are each focused on, and parts not described in detail in a certain process or structure can be referred to the related descriptions of other processes or structures.

[0146] The above merely describes specific embodiments of the present application, but the protection scope of the present application is not limited thereto, and any change or replacement within the technical scope disclosed in the present application should be covered within the protection scope of the present application.

Claims

1. A dual-vehicle cooperative control method, characterized in that, The method includes: Acquire scanning data of a reflected mark on a following vehicle using a single-line lidar; wherein the single-line lidar is installed at the center of the rear of the preceding vehicle; Acquire ranging data of the reflected mark by a single-beam laser rangefinder; wherein, the single-beam laser rangefinder is installed on both sides of the single-line lidar at the rear of the front vehicle, and the ranging direction of the single-beam laser rangefinder is coplanar with the scanning plane of the single-line lidar; The relative pose of the rear vehicle and the front vehicle is determined by data fusion based on the scanning data and the ranging data. Based on the relative poses of the rear vehicle and the front vehicle and the preset cooperative motion trajectory, control commands for the front vehicle are generated. Control the preceding vehicle to execute the control commands of the preceding vehicle.

2. The method according to claim 1, characterized in that, The step of fusing the scanned data and the ranging data to determine the relative pose of the rear vehicle and the front vehicle includes: Based on the variance-weighted model, the coordinates of each reflection marker are determined using the scan data and the ranging data; The optimized coordinate estimate of each reflection marker is obtained based on the coordinates of each reflection marker and the extended Kalman filter algorithm; Based on the coordinate estimates of at least three optimized reflection markers, the relative rotation matrix and translation vector between the front vehicle and the rear vehicle are calculated using a rigid body transformation algorithm. The relative pose of the rear vehicle and the front vehicle is determined based on the relative rotation matrix and translation vector between the front vehicle and the rear vehicle.

3. The method according to claim 2, characterized in that, The method for determining the coordinates of each reflection marker based on the variance-weighted model and the ranging data includes: The scan data is processed to identify each reflection marker and extract the angle information and first distance information of each reflection marker in the single-line lidar coordinate system; The ranging data obtained by each single-beam laser rangefinder is associated and matched with the first distance information of the corresponding reflection marker; Based on the variance weighted model, the successfully matched ranging data is fused with the first distance information to obtain the fused distance information of each reflection identifier; Based on the angle information of each reflection marker and the fusion distance information, the coordinates of each reflection marker are determined.

4. The method according to claim 3, characterized in that, The optimized coordinate estimate of each reflection marker is obtained by the extended Kalman filter algorithm based on the coordinates of each reflection marker, including: Using the coordinates of each reflection marker as a state variable and the angle information and the fused distance information as observations, an extended Kalman filter algorithm is used for temporal fusion to obtain the optimized coordinate estimate of each reflection marker.

5. The method according to claim 4, characterized in that, The calculation of the relative rotation matrix and translation vector between the front vehicle and the rear vehicle using a rigid body transformation algorithm, based on coordinate estimates from at least three optimized reflection markers, includes: Determine whether the coordinate estimates of multiple reflective markers satisfy the collinearity condition to obtain the first judgment result; If the first judgment result indicates that the coordinate estimates of multiple reflection markers do not meet the collinearity condition, then the singular value decomposition algorithm is used to calculate the relative rotation matrix and translation vector. If the first judgment result indicates that the coordinate estimates of multiple reflection markers meet the collinearity condition, then the principal component analysis algorithm is used to extract the principal direction of the reflection marker point set, and the Rodriguez formula is used to calculate the relative rotation matrix based on the principal direction of the reflection marker point set, and then the translation vector is calculated based on the relative rotation matrix.

6. The method according to claim 5, characterized in that, The step of generating control commands for the front vehicle based on the relative poses of the rear vehicle and the front vehicle and a preset cooperative motion trajectory includes: The geometric centers of the front vehicle and the rear vehicle are set as the cooperative center point; Obtain the desired motion velocity and desired relative pose of the cooperative center point; Based on the principles of rigid body kinematics, the desired velocity of the coordinating center point is decomposed to obtain the first desired velocity of the vehicle in front. Based on the pose error between the relative pose of the rear vehicle and the front vehicle and the desired relative pose, the first desired speed is corrected, and control commands for the front vehicle are generated.

7. The method according to claim 6, characterized in that, The step of correcting the first desired speed based on the pose error between the relative pose of the rear vehicle and the front vehicle and the desired relative pose, and generating control commands for the front vehicle, includes: Calculate the pose error between the relative pose of the rear vehicle and the front vehicle and the desired relative pose; Calculate the speed compensation amount based on the pose error; The speed compensation amount is added to the first desired speed to obtain the control command for the vehicle in front.

8. A dual-vehicle cooperative control device, characterized in that, The device includes: The acquisition module is used to acquire scanning data of a reflective mark on a following vehicle by a single-line lidar; wherein the single-line lidar is installed at the center of the rear of the preceding vehicle; and to acquire ranging data of the reflective mark by a single-beam laser rangefinder; wherein the single-beam laser rangefinder is installed on both sides of the single-line lidar at the rear of the preceding vehicle, and the ranging direction of the single-beam laser rangefinder is coplanar with the scanning plane of the single-line lidar. The fusion module is used to perform data fusion based on the scan data and the ranging data to determine the relative pose of the rear vehicle and the front vehicle; The control module is used to generate control commands for the front vehicle based on the relative poses of the rear vehicle and the front vehicle and a preset cooperative motion trajectory; and to control the front vehicle to execute the control commands.

9. A computing device, characterized in that, Including memory and processor; The memory stores one or more computer programs, the one or more computer programs including instructions; when the instructions are executed by the processor, the computing device performs the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium is used to store a computer program for performing the method as described in any one of claims 1 to 7.

Images