Multi-vehicle cooperative docking method, system and device and storage medium

Abstract

The invention provides a multi-vehicle cooperative docking method, system and device and a storage medium, and relates to the technical field of vehicle engineering. The method comprises the following steps: firstly, acquiring position information, state information and expected docking distance of a plurality of vehicles which are divided into a pilot vehicle and a following vehicle, and constructing a pilot-following vehicle motion model according to the position information and the state information so as to control the docking distance between the following vehicle and the pilot vehicle to be equal to the expected distance as a target; and according to the pilot-following vehicle motion model, the position of the following vehicle is changed by using a feedback linearization speed controller, and when the docking distance is equal to the expected distance, the docking device of the following vehicle and the docking device of the pilot vehicle are controlled to carry out docking, so that multiple vehicles can keep dynamic driving at relatively stable positions, and the docking efficiency is improved. Therefore, multi-vehicle docking is achieved, and the process is flexible, high in real-time performance, high in efficiency and high in robustness.

Description

Technical Field

[0001] The present application relates to the technical field of vehicle engineering, and particularly relates to a multi-vehicle collaborative docking method, system, device and storage medium. Background Art

[0002] In recent years, with the continuous development of the global economy, especially the accelerating construction pace of basic industries such as electric power, chemical industry, and metallurgy, more and more large-piece transportation plays a greater role in society. With the increasing demand for modern large-piece cargo transportation, in such transportation projects, the transportation carriers often adopt traditional heavy-duty transportation vehicles, such as heavy vehicles and tractors, etc. However, the mechanical transmission configuration of traditional transportation vehicles has problems such as non-expandability, low transmission efficiency, complex structure, clumsy components, and poor maintainability, which have severely restricted the transportation efficiency and scene adaptability.

[0003] In the face of special load-bearing requirements, relevant solutions need to manually reorganize multiple transportation units to construct a multi-axis modular vehicle, and flexibly adjust the number of axles and axle loads according to the load-bearing capacity of bridges and special sections to complete the purpose of cargo loading and transportation. This static reorganization operation mode that relies on manual intervention not only increases the operation risk but also is not conducive to the development of collaborative tasks such as reconfiguration docking and formation. Summary of the Invention

[0004] The main purpose of the embodiments of the present disclosure is to propose a multi-vehicle collaborative docking method, system, device and storage medium, which can enable dynamic driving with a relatively stable position between multiple vehicles, thereby realizing intelligent and automatic reconfiguration docking of multiple vehicles.

[0005] To achieve the above object, on the one hand, an embodiment of the present application proposes a multi-vehicle collaborative docking method, including the following steps:

[0006] Obtain the position information, status information of a plurality of vehicles and the desired distance for vehicle docking, wherein the plurality of vehicles are divided into a leading vehicle and following vehicles;

[0007] Construct a leading-following vehicle motion model according to the position information and the status information;

[0008] With the goal of controlling the docking distance between the following vehicle and the leading vehicle to be equal to the desired distance, according to the leading-following vehicle motion model, use a feedback linearization speed controller to change the position of the following vehicle;

[0009] When the docking distance is equal to the desired distance, control the docking device of the following vehicle and the docking device of the leading vehicle to perform docking.

[0010] In some embodiments, the position information and status information of the vehicle are determined through the following steps:

[0011] Obtain the error data of the inertial measurement unit;

[0012] Through the inertial measurement unit, obtain the first speed information and the first position information of the vehicle according to the motion state of the vehicle;

[0013] Through the global navigation satellite system, obtain the second speed information and the second position information of the vehicle according to the motion state of the vehicle;

[0014] According to the error data of the inertial measurement unit, establish a state equation for the integrated navigation of the inertial measurement unit and the global navigation satellite system;

[0015] According to the first speed information, the first position information, the second speed information and the second position information, establish a measurement equation for the integrated navigation of the inertial measurement unit and the global navigation satellite system;

[0016] Based on the Kalman filter algorithm, obtain the vehicle position information and state information according to the state equation and the measurement equation.

[0017] In some embodiments, constructing a leader-follower vehicle motion model according to the position information and the state information includes the following steps:

[0018] Obtain a single-vehicle motion model;

[0019] According to the position information, the state information and the single-vehicle motion model, obtain a leader-follower vehicle motion model.

[0020] In some embodiments, the single-vehicle motion model is determined by the following steps:

[0021] Obtain the structural data of the vehicle, where the structural data includes the first structural position relationship between the front wheels and the center of mass of the vehicle, the second structural position relationship between the rear wheels and the center of mass of the vehicle, the front and rear wheel constraint relationship, and the relationship between the vehicle speed and the change in the position of the center of mass of the vehicle;

[0022] According to the first structural position relationship and the second structural position relationship, obtain a vehicle structure constraint equation;

[0023] According to the front and rear wheel constraint relationship and the vehicle structure constraint equation, obtain a first vehicle motion equation;

[0024] According to the relationship between the vehicle speed and the change in the position of the center of mass of the vehicle and the first vehicle motion equation, obtain a second vehicle motion equation;

[0025] According to the first vehicle motion equation and the second vehicle motion equation, determine a single-vehicle motion model.

[0026] In some embodiments, with the goal of controlling the docking distance between the following vehicle and the leading vehicle to be equal to the desired distance, according to the leading-following vehicle motion model, a feedback linearization speed controller is used to change the position of the following vehicle, including the following steps:

[0027] According to the desired distance and the leading-following vehicle motion model, obtain the error equation between the leading vehicle and the following vehicle;

[0028] According to the error equation and the speed information of the following vehicle, construct the feedback linearization speed controller, where the state information includes the speed information;

[0029] According to the feedback linearization speed controller, by controlling the speed of the following vehicle, change the position of the following vehicle.

[0030] In some embodiments, when the docking distance is equal to the desired distance, control the docking device of the following vehicle and the docking device of the leading vehicle to dock, including the following steps:

[0031] Through the millimeter-wave radar and vision sensor of the following vehicle, perform target recognition on the docking device of the leading vehicle to obtain the first image data collected by the millimeter-wave radar and the second image data collected by the vision sensor, where both the first image data and the second image data include time tags and position tags;

[0032] According to the time tags, synchronize the first image data and the second image data in time to obtain the first image data and the second image data after time alignment;

[0033] According to the position tags, align the first image data and the second image data after time alignment in space to obtain the first image data and the second image data after time and space alignment;

[0034] Fuse the first image data and the second image data after time and space alignment to obtain the image data coincidence ratio;

[0035] According to the image data coincidence ratio and the position tags, determine the position of the docking device of the leading vehicle;

[0036] Through the target detection algorithm, according to the position of the docking device of the leading vehicle, control the docking device of the following vehicle to dock with the docking device of the leading vehicle.

[0037] In some embodiments, the method of controlling the docking device of the following vehicle to dock with the docking device of the leading vehicle according to the position of the docking device of the leading vehicle through a target detection algorithm further includes the following steps:

[0038] Obtain the target of the leading vehicle;

[0039] Based on the target detection algorithm, obtain the relative pose of the docking devices between the leading vehicle and the following vehicle according to the target and the position of the docking device of the leading vehicle;

[0040] According to the relative pose, adjust the pose of the following vehicle and dock the docking device of the following vehicle with the docking device of the leading vehicle.

[0041] On the other hand, an embodiment of the present invention provides a multi-vehicle collaborative docking system, including:

[0042] A first module, configured to obtain the position information, status information of several vehicles, and the expected distance of vehicle docking, where the several vehicles are divided into a leading vehicle and following vehicles;

[0043] A second module, configured to construct a leading-following vehicle motion model according to the position information and the status information;

[0044] A third module, with the goal of controlling the docking distance between the following vehicle and the leading vehicle to be equal to the expected distance, according to the leading-following vehicle motion model, using a feedback linearization speed controller to change the position of the following vehicle;

[0045] A fourth module, configured to control the docking device of the following vehicle and the docking device of the leading vehicle to dock when the docking distance is equal to the expected distance.

[0046] On the other hand, an embodiment of the present invention provides an electronic device, including:

[0047] At least one processor;

[0048] At least one memory, configured to store at least one program;

[0049] When the at least one program is executed by the at least one processor, the at least one processor implements the multi-vehicle collaborative docking method as described in the previous embodiments.

[0050] On the other hand, an embodiment of the present invention further provides a computer-readable storage medium storing computer-executable instructions for causing a computer to execute the multi-vehicle collaborative docking method as described in the previous embodiments.

[0051] At least one of the above technical solutions of the present invention has the following advantages or beneficial effects: By constructing a leader-follower motion model, the position error equation and system tracking error model of the leader-follower vehicles are determined. With the longitudinal speed and lateral speed of the follower vehicle as the input variables of the system, the system error can be controlled by adjusting the input variables, thereby maintaining the relative position stability of the leader-follower vehicles in a dynamic driving scenario. Furthermore, the relative stable positions of multiple vehicles during dynamic driving are achieved. The hardware docking device completes the locking connection to realize the intelligent automatic reconstruction and docking of multiple vehicles. Description of the Drawings

[0052] Figure 1 is a flowchart of the multi-vehicle collaborative docking method provided by an embodiment of the present application;

[0053] Figure 2 is a schematic diagram of the IMU / GNSS data fusion process provided by an embodiment of the present application;

[0054] Figure 3 is a schematic diagram of the motion model structure of a single vehicle provided by an embodiment of the present application;

[0055] Figure 4 is a schematic diagram of the leader-follower vehicle motion model structure provided by an embodiment of the present application;

[0056] Figure 5 is a schematic diagram of the operation of a millimeter-wave radar provided by an embodiment of the present application;

[0057] Figure 6 is a schematic diagram of the ArUco code structure provided by an embodiment of the present application;

[0058] Figure 7 is a flowchart of the multi-vehicle docking process provided by an embodiment of the present application;

[0059] Figure 8 is a schematic diagram of the multi-vehicle docking structure provided by an embodiment of the present application;

[0060] Figure 9 is a schematic diagram of the vehicle docking system structure provided by an embodiment of the present application;

[0061] Figure 10 is a schematic diagram of the hardware structure of an electronic device provided by an embodiment of the present application. Detailed Embodiments

[0062] In order to make the objectives, technical solutions and advantages of the present application more clear and understandable, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0063] It should be noted that although functional module division is performed in the device schematic diagram and the logical sequence is shown in the flowchart, in some cases, the steps shown or described can be executed in a different order from the module division in the device or the sequence in the flowchart. Terms such as "first" and "second" in the specification, claims and the above-mentioned drawings are used to distinguish similar objects and do not necessarily need to describe a specific order or sequence.

[0064] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the technical field to which this application belongs. The terms used herein are only for the purpose of describing the embodiments of this application and are not intended to limit this application.

[0065] The embodiments of the present disclosure provide a multi-vehicle collaborative docking method, system, device and storage medium, which can enable multi-vehicles to maintain dynamic driving at a relatively stable position, thereby realizing intelligent reconfigurable docking of multi-vehicles.

[0066] Refer to Figure 1 as shown Figure 1 is an optional flowchart of a multi-vehicle collaborative docking method provided by some embodiments of the present application. A multi-vehicle collaborative docking method according to an embodiment of the present invention includes but is not limited to steps S100 to step S400.

[0067] Step S100, obtain the position information, status information of a plurality of vehicles and the expected distance of vehicle docking, wherein the plurality of vehicles are divided into a leading vehicle and following vehicles;

[0068] Step S200, construct a leading-following vehicle motion model according to the position information and status information;

[0069] Step S300, with the goal of controlling the docking distance between the following vehicle and the leading vehicle to be equal to the expected distance, according to the leading-following vehicle motion model, use a feedback linearization speed controller to change the position of the following vehicle;

[0070] Step S400, when the docking distance is equal to the expected distance, control the docking device of the following vehicle and the docking device of the leading vehicle to perform docking.

[0071] In step S100 of some embodiments, in the scenario of collaborative operation of carrier vehicles docking, it is necessary to obtain the position information, status information of multiple docking vehicles, and the expected distance of vehicle docking. This is crucial for achieving efficient and safe vehicle docking. Among these several vehicles, there is a distinction between a leading vehicle and a following vehicle. The position information of the vehicle includes latitude and longitude data, elevation data, etc., and the status information includes speed, steering angle, heading angle, etc. Both the leading vehicle and the following vehicle are equipped with a Global Navigation Satellite System (GNSS) receiver and an Inertial Measurement Unit (IMU). The GNSS receiver is used to continuously receive signals transmitted by multiple satellites. The satellite signal contains its own precise position information and timestamp. By measuring the time difference of the signal propagating from the satellite to the receiver and combining the speed of light, the distance between the leading vehicle and the satellite is calculated using the principle of triangulation. Considering error factors such as satellite clock error, it is corrected to obtain the high-precision latitude and longitude coordinates and elevation data of the vehicle.

[0072] On the one hand, the GNSS calculates the speed of the vehicle through satellite signals for speed information. On the other hand, the acceleration measured by the IMU can also assist in providing speed information through integral operation. The two complement and verify each other. The steering angle can be obtained through the angle sensor of the vehicle. The heading angle is obtained through complex attitude resolution algorithms using the angular velocity data measured by the IMU, reflecting the driving direction of the vehicle. At the same time, the GNSS combined with relevant algorithms can also assist in determining the heading angle to a certain extent and improve the measurement accuracy of the heading angle.

[0073] In addition, the expected distance can be confirmed according to the specific vehicle structure or the specific operation tasks to be performed after vehicle docking.

[0074] In some embodiments, step S100 may include but is not limited to steps S110 to S160:

[0075] Step S110, obtaining the error data of the inertial measurement unit;

[0076] Step S120, through the inertial measurement unit, obtaining the first speed information and the first position information of the vehicle according to the motion state of the vehicle.

[0077] Step S130, through the Global Navigation Satellite System, obtaining the second speed information and the second position information of the vehicle according to the motion state of the vehicle;

[0078] Step S140, according to the error data of the inertial measurement unit, establishing the state equation of the integrated navigation of the inertial measurement unit and the Global Navigation Satellite System;

[0079] Step S150: Establish a measurement equation for the integrated navigation of the inertial measurement unit and the global navigation satellite system based on the first velocity information, the first position information, the second velocity information, and the second position information.

[0080] Step S160: Based on the Kalman filter algorithm, obtain the vehicle position information and status information according to the state equation and the measurement equation.

[0081] In steps S110 to S160 of some embodiments, the inertial measurement unit error data includes attitude angles, velocity, displacement, gyroscope offset error, accelerometer offset error, etc. Refer to Figure 2 , when performing the velocity solution of the inertial navigation unit, the specific force equation is required; the specific force equation is a basic equation for inertial navigation solution near the Earth's surface. This equation provides the sum of the absolute acceleration of the carrier relative to the inertial space and the gravitational acceleration, and is the basis for the inertial navigation solution of the velocity differential equation. The expression of the specific force equation is shown in Equation (1).

[0082]

[0083] Where In the formula, is the specific force, usually referring to the acceleration of the carrier other than gravity, is the acceleration of the carrier relative to the Earth's motion, ω ie is the angular velocity of the carrier relative to the Earth's motion, v is the velocity of the carrier relative to the Earth's motion, ω en is the rotational angular velocity of the Earth-centered Earth-fixed coordinate system (e-system) relative to the Earth-centered inertial coordinate system (i-system), and g is the gravitational acceleration. Converting Equation (1) to the local navigation coordinate system (n-system), the expression of the velocity differential equation is shown in Equation (2).

[0084]

[0085] Where is the relative acceleration of the vehicle relative to the Earth coordinate system, is the projection of the accelerometer measurement information in the n-system, v n is the projection of the vehicle velocity in the navigation coordinate system, g n is the projection of the gravitational acceleration in the n-system. To convert the specific force information actually output by the accelerometer to the n-system, the attitude matrix of the carrier at the current moment can be used to obtain, that is

[0086] and are obtained by Formula (3) and Formula (4) respectively.

[0087]

[0088] Among them, is the representation of the angular velocity of the inertial coordinate system relative to the Earth in the navigation coordinate system, is the representation of the angular velocity of the Earth coordinate system relative to the navigation coordinate system in the navigation coordinate system.

[0089] Among them, ω e is the angular velocity of the Earth's rotation, v N and v E are the northward and eastward velocities of the carrier respectively, is the latitude of the Earth coordinate, h is the altitude of the carrier; R N and R M are the radius of curvature of the prime vertical and the radius of curvature of the meridian respectively, and the expressions are as shown in Equation (5).

[0090]

[0091] Among them, a is the semi-major axis radius of the Earth, e is the first eccentricity of the Earth ellipsoid model, is the latitude of the Earth coordinate.

[0092] Therefore, the velocity update equation of Formula (2) is as shown in Equation (6).

[0093]

[0094] Integrate both sides of Equation (6) within the time period [t k-1 , t k . After integration, the obtained expression is as shown in Equation (7). Simplifying Equation (7) gives Equation (8).

[0095]

[0096] Among them, and are the velocities at time k and time k-1 respectively, is the specific force integration term, is the gravity / Coriolis integration term. Thus, the first velocity information output by the IMU can be obtained.

[0097] The Coriolis integration term is related to the gravitational acceleration, Coriolis acceleration, and centripetal acceleration, and is a slow variable. It can be approximately processed by assuming that the gravitational acceleration and Coriolis acceleration are constant within an integration period, and taking the value at the middle time instead. It is obtained by trapezoidal integration, as shown in Equation (9).

[0098]

[0099] Among them, k-1 / 2 represents the middle time of the integration period [k-1 k].

[0100] The specific force integration term It is divided into a slow-varying term and a fast-changing term. According to the twin-sample algorithm, the expressions are as shown in Equations (10) to (12).

[0101]

[0102] In the equations represents the equivalent rotation vector corresponding to the rotation of the n-frame at time k-1 to the n-frame at time k; Δv k-1 and Δv k respectively represent the accelerometer incremental outputs at times k-1 and k; Δθ k-1 and Δθ k respectively represent the gyro angular incremental outputs at times k-1 and k; and respectively represent the rotation error compensation and the rowing error compensation.

[0103] According to Equation (8), the speed situation of the vehicle at each moment can be solved. After the speed is updated, the position can be updated by integrating the speed based on the previous moment. The position information of the vehicle needs to be combined with three data: longitude θ, latitude and elevation h. Thus, the differential function expression of the vehicle position in the driving state is as shown in Equation (13).

[0104]

[0105] Using Equation (11), the longitude, latitude and elevation of the vehicle's location can be calculated. Since the speed accuracy of the vehicle is second-order, trapezoidal integration is used here for discretization approximation. Assuming that the speed varies linearly with time within the integration period, the elevation h expression at time k is as shown in Equation (14).

[0106]

[0107] Assuming that R M is a constant within the integration period and the elevation takes the average elevation within the integration period then the latitude expression at time k is as shown in Equation (15).

[0108]

[0109] Assuming that R N is a constant within the integration period and the elevation and latitude respectively take the average elevation and the average latitude then the longitude θ expression at time k is as shown in Equation (16).

[0110]

[0111] Among them,

[0112] Thus, the longitude, latitude, and altitude data of the vehicle can be calculated through Equations (11) to (16), and the first position information of the vehicle can be determined accordingly.

[0113] By using the GNSS differential positioning model to linearly combine the original observation data from the receiver and the base station, the errors in the common part of the observation data are eliminated, solving the problem of difficult error determination, thereby improving the positioning accuracy.

[0114] Classify the errors according to the sources of various errors, and establish the observation equation expression of pseudorange measurement as shown in Equation (17).

[0115] ρ = r + c(δt u - δt (s) ) + I + T + ε ρ , (17)

[0116] where ρ is the measured pseudorange, r is the true geometric distance from the satellite to the receiver, c is the speed of light, δt u is the receiver clock error, δt (s) is the satellite clock error, I is the ionospheric delay, T is the tropospheric delay, and ε ρ is the pseudorange measurement error. Assuming there is no error, the relationship between the carrier phase difference and the geometric distance is as shown in Equation (18).

[0117] φ = λ -1 r + N, (18)

[0118] where φ is the carrier phase observation value, λ is the carrier wavelength, and N is the integer cycle ambiguity in units of cycles.

[0119] The relationship between the carrier wavelength and the carrier frequency of light speed is as shown in Equation (19).

[0120]

[0121] where f is the carrier frequency.

[0122] Considering errors such as clock error and atmospheric delay and adding them to Equation (17), the carrier phase observation equation expression can be obtained as shown in Equation (20).

[0123] φ = λ -1 (r + c(δt u - δt (s) ) - I + T) + N + ε φ , (20)

[0124] where ε φ is the carrier phase observation value noise term.

[0125] Equation (21) is obtained from Equation (19) and Equation (20).

[0126] φ = λ -1 (r - I + T) + f(δt u -δt (s) ) + N + ε φ , (21)

[0127] Assume that the receiver and the base station receive the satellite signal numbered simultaneously. Then, the expressions for the carrier phase observations of the receiver and the base station are shown in Equation (22) and Equation (23).

[0128]

[0129] Where and represent the carrier phase observations of the receiver a and the base station b for satellite i, and represent the geometric distances from satellite i to the receiver a and the base station b. The superscript represents the satellite number, and the subscript represents the receiver or the base station.

[0130] According to Equation (22) and Equation (23), the expression for the single-difference carrier phase observation value of the receiver a and the base station b for satellite i can be obtained, as shown in Equation (24).

[0131]

[0132] After single-differencing, the subscript represents the difference between the receiver and the base station. The single-difference expressions for each quantity are shown in Equation (25).

[0133]

[0134] Where represents the X single-difference observation of the receiver a and the base station b for satellite i, is the integer ambiguity after being eliminated by the single-difference model, is the carrier phase observation noise.

[0135] The expression for the pseudorange observation in the inter-station single-difference can be obtained from Equation (17), as shown in Equation (26).

[0136]

[0137] It can be known from Equation (24) that the satellite clock error is eliminated after single-differencing. In the case of short baselines, the ionospheric delay I and the tropospheric delay T of the receiver a and the base station b are basically the same. Therefore and can be approximated to zero. Therefore, Equation (24) is simplified to Equation (27), and Equation (26) can be simplified to Equation (28).

[0138]

[0139] As can be seen from Equation (27), the satellite clock error δt is eliminated through the single-difference model (s) . Since receivers a and base station b observe multiple satellites at the same time, it can be known from Equation (27) that the expression of the single-difference carrier phase observation value of receivers a and base station b for satellite j is as shown in Equation (29).

[0140]

[0141] Perform double-difference processing on the single-difference carrier phase observation value of satellite i and the single-difference carrier phase observation value of satellite j to eliminate the receiver clock error δt u . From Equations (27) and (29), it can be obtained that at the same time, the expression of the double-difference carrier phase measurement value is as shown in Equation (30).

[0142]

[0143] Wherein:

[0144]

[0145] The expression of the pseudorange observation value under the double-difference between stations can be obtained from Equation (28), as shown in Equation (34).

[0146]

[0147] Through Equations (30) and (34), the double-difference model can further eliminate the error terms related to the observation equipment, thereby converting the double-difference ambiguity into an integer, which can significantly improve the positioning accuracy and reduce the computational complexity at the same time.

[0148] In a general double-difference model, a group of satellites with the highest elevation angle and no data loss in this epoch are generally selected as reference satellites, and differences are made between other satellites and the reference satellites to form a double-difference model.

[0149] The integer ambiguity is fixed through the least squares ambiguity decorrelation adjustment algorithm based on the ambiguity domain, and finally the fixed ambiguity is substituted to obtain the positioning result of GNSS.

[0150] By eliminating the GNSS positioning error as described above, the second speed information and the second position information of the vehicle are obtained;

[0151] Since the cumulative error of the IMU will accumulate over time after long-term operation, and the GNSS positioning accuracy will decrease when the GNSS system is severely blocked by signals. The IMU and GNSS have strong complementarity, and the state equation and measurement equation of the integrated navigation are established to jointly constitute the GNSS / IMU fusion algorithm model.

[0152] The state equation of the IMU / GNSS integrated navigation system consists of two parts. One part is the sensor error of the IMU itself, and the other part is the error generated during the calculation process. The state equation includes five state variables: attitude angle, velocity, displacement, gyroscope offset error, and accelerometer offset error, with 15-dimensional information. These error parameters are used as variables in the integrated navigation state equation. Then, the integrated navigation state equation under the driving state is shown in Equation (35).

[0153]

[0154] Among them, F(t) represents the state transition matrix, X(t) represents the state vector, G(t) represents the noise driving matrix, and W(t) represents the noise vector. Based on the error model of the IMU, the expression of the state vector X(t) is established as shown in Equation (36).

[0155]

[0156] In the formula, φ = [φ E φ N φ U T is the initial value error of the misalignment angle in the northeast-up direction, is the velocity error in the northeast-up direction, is the longitude and latitude position error, is the constant drift error of the gyroscope, is the constant zero bias error of the accelerometer.

[0157] F(t), G(t), and W(t) in Equation (33) are shown in Equations (37), (38), and (39) respectively.

[0158] W(t) = [ω x ω y ω z a x a y a z T , (37)

[0159] Among them, the first three terms are the Gaussian white noise terms of the gyroscope, and the last three terms are the Gaussian white noise terms of the accelerometer.

[0160]

[0161] In Equation (38), F 11 , F 12 , F 13 , F 14 , F 21 , F 22 , F 23 , F​​24 、F 32 、F 33 are respectively as shown in Formula (40) to Formula (49)

[0162]

[0163] The measurement values of the integrated navigation system are composed of position and velocity information, which are obtained by subtracting the three-dimensional velocity and position information output by the IMU and GNSS, that is, the first velocity information, the first position information, the second velocity information, and the second position information. The measurement equation realizes the function of calibration and update in the state estimation process. In the embodiment of the present invention, based on the loose-coupling integrated navigation method, position and velocity are used as the analysis objects, and the integrated navigation measurement equation under the driving state is established as shown in Formula (50).

[0164] Z(t) = H(t)X(t) + V(t), (50)

[0165] Among them, Z(t) is the measurement vector of the system, H(t) is the measurement matrix of the system, and V(t) is the measurement noise vector of the system. The expressions are as shown in Formula (51) and Formula (52).

[0166]

[0167] Among them, V v and V p are respectively the position error and velocity error of the GNSS, which are usually obtained from the GNSS solution results.

[0168] The system equation of the integrated navigation is composed of the state equation of Formula (35) and the measurement equation of Formula (50), which together constitute the IMU / GNSS fusion algorithm model. According to the standard Kalman filter algorithm, the two can be fused to give the estimated value of the optimal state of the vehicle itself, and the position information and state information of the vehicle can be obtained.

[0169] In step S200 of some embodiments, based on the single-vehicle autonomous motion model (single-vehicle motion model), combined with the position information and state information, the system control problem of the two vehicles that need to be reconstructed can be transformed into the relative position control problem of the two vehicles, and then into the control problem of the desired distance and desired angle between the two vehicles. By controlling the distance and angle between the two vehicles, the stability of the relative position of the two-vehicle system can be maintained.

[0170] In some embodiments, step S200 may include but is not limited to steps S210 to S220:

[0171] Step S210, obtaining the single-vehicle motion model;

[0172] Step S220: Obtain the leader-follower vehicle motion model based on the position information, status information, and single-vehicle motion model.

[0173] In steps S210 to S220 of some embodiments, the docking vehicle structure is determined according to the single-vehicle motion model, so as to further obtain the constraint relationship between the longitudinal speed, lateral speed, steering angle, and heading angle of a single vehicle in the driving state. The single-vehicle motion model is combined with the acquired vehicle position information (latitude, longitude, altitude) and status information (speed, steering angle, heading angle, etc.). For the leader vehicle and the follower vehicle, the position and status changes at different times are analyzed respectively. The driving trajectory and status of the leader vehicle will guide the follower vehicle. By establishing the relative position and relative status relationship between the two, the leader-follower vehicle motion model is constructed. The follower vehicle can make corresponding adjustments according to the motion state of the leader vehicle to achieve safe, efficient following driving and precise docking.

[0174] In step S210 of some embodiments, step S210 may include but is not limited to steps S211 to S215:

[0175] Step S211: Obtain the structural data of the vehicle, where the structural data includes the first structural position relationship between the front wheels and the center of mass of the vehicle, the second structural position relationship between the rear wheels and the center of mass of the vehicle, the front and rear wheel constraint relationship, and the relationship between the vehicle speed and the change in the position of the center of mass of the vehicle;

[0176] Step S212: Obtain the vehicle structure constraint equation according to the first structural position relationship and the second structural position relationship;

[0177] Step S213: Obtain the first vehicle motion equation according to the front and rear wheel constraint relationship and the vehicle structure constraint equation;

[0178] Step S214: Obtain the second vehicle motion equation according to the relationship between the vehicle speed and the change in the position of the center of mass of the vehicle and the first vehicle motion equation;

[0179] Step S215: Determine the single-vehicle motion model according to the first vehicle motion equation and the second vehicle motion equation.

[0180] In steps S211 to S215 of some embodiments, considering the complexity of the mechanical structure of a single vehicle, the following assumptions are made for the vehicle: (1) The structures of each intelligent reconfigurable vehicle are the same; (2) The structures on both sides of the vehicle are symmetrical, and the center of mass is the geometric center of the vehicle; (3) The front wheels are responsible for steering, and the rear wheels are responsible for driving; (4) The vehicle will not experience side shift during driving. A single-vehicle autonomous vehicle motion model can be established, as Figure 3 shown.

[0181] Figure 3Among them, P = (x, y) represents the centroid coordinates of the autonomous vehicle, and P 1 = (x 1 , y 1 ), P 2 = (x 2 , y 2 ) represent the coordinates of the rear axle center and the front axle center respectively. a and b are the distances from the front and rear wheels to the centroid, and δ and θ represent the steering angle and the heading angle of the vehicle respectively.

[0182] It can be known from Figure 3 that the relationship between the front and rear wheels and the centroid position of the vehicle (the first structural position relationship and the second structural position relationship) can be expressed as shown in Equation (53) (the vehicle structure constraint equation). Taking the derivative of Equation (53) gives Equation (54).

[0183]

[0184] The constraint relationship between the front and rear wheels of the vehicle is as shown in Equation (55).

[0185]

[0186] From Equation (54) and Equation (55), the motion constraint equation of the single-vehicle autonomous vehicle (the first vehicle motion equation) is as shown in Equation (56).

[0187]

[0188] In order to better analyze the motion state of the vehicle in the x-axis and y-axis directions in the global coordinate system, the speed of the vehicle in the global coordinate system can be expressed by the change rate of the centroid coordinates of the vehicle (the relationship between the vehicle speed and the change of the centroid position of the vehicle), as shown in Equation (57).

[0189]

[0190] Among them, v x and v y represent the longitudinal speed and the lateral speed of the vehicle respectively.

[0191] Equation (58) is obtained from (56) and (57).

[0192]

[0193] From Equation (57) and Equation (58), the motion state equation of the single-vehicle autonomous vehicle (the second vehicle motion equation) can be obtained as Equation (59).

[0194]

[0195] From equation (59), the constraint relationships among the longitudinal speed, lateral speed, steering angle, and heading angle of a single vehicle in a driving state can be obtained. According to the acquisition processes of the first vehicle motion equation and the second vehicle motion equation described above, the motion model of a single vehicle can be determined.

[0196] In step S220 of some embodiments, please refer to Figure 4 , p l =(x l , y l ), p f =(x f , y f ) respectively represent the centroid coordinates of the leading vehicle and the following vehicle; D is the relative distance between the centroids of the leading vehicle and the following vehicle; ω l is the angle between the line connecting the centroids of the leading vehicle and the following vehicle and the central axis of the leading vehicle; θ l , θ f respectively represent the heading angles of the leading vehicle and the following vehicle; δ l , δ f respectively represent the front-wheel steering angles of the leading vehicle and the following vehicle; α f is the angle between the line connecting the centroids of the leading vehicle and the following vehicle and the x-axis. Decompose the distance D between the two vehicles into the coordinate system, and its components on the x-axis and y-axis are D x and D y respectively, as shown in equation (60). From the coordinate relationship, equation (61) can be obtained.

[0197] D 2 =D x 2 +D y 2 , (60);

[0198] The angle between the driving direction of the leading vehicle body and the line connecting the centroids of the two vehicles is as shown in equation (62). Differentiating both sides of equation (61) can obtain the expression of the constraint relationship between the following distance change rate component and the vehicle driving parameters as shown in equation (63).

[0199]

[0200] According to the vehicle constraint relationship shown in equation (57), equation (63) can be expressed as equation (64).

[0201]

[0202] Among them, v x,l , v y,l respectively represent the longitudinal speed and lateral speed of the leading vehicle; v x,f , v y,frespectively represent the longitudinal speed and lateral speed of the following vehicle.

[0203] From Figure 4 Equation (65) can be obtained.

[0204]

[0205] Differentiating both sides of Equation (60) gives Equation (66).

[0206]

[0207] Substituting Equation (64) and Equation (66) into Equation (65), the constraint relationship equation between the following distance, the vehicle driving speed, the vehicle heading angle and the overall heading angle of the two vehicles can be obtained as shown in Equation (67).

[0208]

[0209] Such as Figure 4 , let the angle between the lateral speed of the following vehicle and the line connecting the centers of mass be γ, then there is Equation (68).

[0210]

[0211] From Equation (67) and Equation (68), it can be obtained that:

[0212]

[0213] From Figure 4 the angular relationship of

[0214]

[0215] Equation (70) can be obtained, and differentiating Equation (70) gives Equation (71).

[0216]

[0217] Equation (72) is the equation of the leader-follower motion model, which can be used for the longitudinal control and lateral control of the leader-follower vehicles during driving.

[0218] In step S300 of some embodiments, the leader vehicle and the following vehicle need to complete the docking operation according to a certain docking distance. To achieve this goal, the leader-follower vehicle motion model obtained in the previous steps is used, combined with a feedback linearization speed controller to adjust the position of the following vehicle. The feedback linearization speed controller is a control method based on the system model. By linearizing the system, it can more effectively control the system, enabling the following vehicle to dynamically adjust its position according to the motion state of the leader vehicle, and finally making the docking distance equal to the desired distance.

[0219] In some embodiments, step S300 may include, but is not limited to, steps S310 to S330:

[0220] Step S310, obtaining an error equation between the leading vehicle and the following vehicle according to the desired distance and the leader-follower vehicle motion model;

[0221] Step S320, constructing a feedback linearized speed controller according to the error equation and the speed information of the following vehicle, where the state information includes speed information;

[0222] Step S330, controlling the speed of the following vehicle according to the feedback linearized speed controller to change the position of the following vehicle.

[0223] In steps S310 to S330 of some embodiments, for vehicle following in a driving scenario, it is necessary to perform a correlation analysis on three variables: the longitudinal error, the lateral error, and the angular error between the leading vehicle and the following vehicle.

[0224] On the basis of the original coordinate system, a new auxiliary coordinate system is established with the centroid of the following vehicle as the origin, the lateral speed direction as the X-axis, and the longitudinal speed direction as the Y-axis. The two-vehicle motion error equations of the driving scenario are established for the following vehicle on the X-axis, Y-axis, and heading angle of the auxiliary coordinate system respectively, as shown in Equation (73). Figure 4 Where D

[0225]

[0226] where D des,X and D des,Y respectively represent the components of the desired distance D des in the X-axis and Y-axis directions; e X and e Y are respectively the lateral error and longitudinal error of the two vehicles on the auxiliary coordinate system, and e θ is the heading angle error between the two vehicles.

[0227] Combined with Figure 4 it can be seen that the error between the desired position and the actual position of the center point of the following vehicle is as shown in Equation (74). Substituting Equation (74) into (73) gives Equation (75).

[0228]

[0229] where D des and γ des are respectively the expected values of D and γ. From Equation (68) and Equation (75), Equation (76) is obtained, and differentiating Equation (75) gives Equation (77).

[0230]

[0231] Equation (78) can be obtained from Equation (58), and substituting Equation (78) into Equation (77) gives Equation (79).

[0232]

[0233] Substituting Equation (76) into Equation (77) gives Equation (80).

[0234]

[0235] Substituting Equation (75) and Equation (76) into Equation (80) and arranging gives Equation (81).

[0236]

[0237] As can be seen from Equation (81), the position error and heading angle error of the vehicle under dynamic driving are related to the positions and speeds of the two vehicles. By calculating and setting the longitudinal speed and lateral speed of the following vehicle, the following error can be made to approach 0. According to the feedback linearization theory, let The selected speed input of the following vehicle is the control input of the system, and the expression is as shown in Equation (82).

[0238]

[0239] where, k 1 > 0, k 2 > 0, K is the supplementary term of the lateral speed v y,f of the following vehicle. The Lyapunov function of the system is selected as shown in Equation (83).

[0240]

[0241] As can be seen from Equation (83), when V = 0; that is, when both the leading vehicle and the following vehicle are in the desired positions, the system error is equal to zero, and the system is in a stable state at this time.

[0242] Taking the derivative of Equation (83) gives Equation (84), and substituting Equation (81) into Equation (84) gives Equation (85).

[0243]

[0244] Substituting Equation (82) into Equation (83) gives Equation (86).

[0245]

[0246] In Equation (86), Let Moreover, there is |cosθ| ≤ 1, |sinθ| ≤ 1, |sinγ| ≤ 1, then there is Equation (87). θ |cosθ| ≤ 1, |sinθ| ≤ 1 θ |sinθ| ≤ 1, |sinγ| ≤ 1 des |sinγ| ≤ 1, then there is Equation (87).

[0247]

[0248] Therefore, there is In the error equation expressed by Equation (79), when t → ∞, e X → 0; therefore, by adjusting the value of K in Equation (85), so that Thus, it can be ensured that the system error model equation, that is, Equation (81) has Lyapunov stability under the action of the control input of the system. Select the expression of K as shown in Equation (88), and substitute Equation (88) into Equation (82), and Equation (89) can be obtained.

[0249]

[0250] Equation (89) is the speed controller for the following vehicle with feedback linearization. By controlling the magnitude of the input quantity, the control of the system error can be realized, and the stability of the relative position of the leading vehicle and the following vehicle in the dynamic driving scenario can be maintained.

[0251] In step S400 of some embodiments, when the docking distance between the following vehicle and the leading vehicle is equal to the desired distance through the previous control steps, it means that the two vehicles are already in a relative position suitable for docking. At this time, it is necessary to accurately identify and locate the docking device of the leading vehicle, and then complete the docking of the two vehicles.

[0252] In some embodiments, step S400 may include but is not limited to steps S410 to S450:

[0253] Step S410, through the millimeter-wave radar and vision sensor of the following vehicle, perform target recognition on the docking device of the leading vehicle to obtain the first image data collected by the millimeter-wave radar and the second image data collected by the vision sensor. Among them, both the first image data and the second image data include time tags and position tags;

[0254] Step S420, according to the time tags, synchronize the first image data and the second image data in time to obtain the first image data and the second image data after time alignment;

[0255] Step S430, fuse the first image data and the second image data after time and space alignment to obtain the image data coincidence ratio;

[0256] Step S440, determine the position of the docking device of the leading vehicle according to the image data coincidence ratio and the position tag

[0257] Step S450: Based on the position of the docking device of the leading vehicle through the target detection algorithm, control the docking device of the following vehicle to dock with the docking device of the leading vehicle.

[0258] In steps S410 to S450 of some embodiments, in order to achieve precise docking between the docking components of the following vehicle and the leading vehicle, the millimeter-wave radar and visual sensor data are fused to achieve high-precision tracking of the target leading vehicle ahead. At the same time, the high-precision distance data provided by the sensors are used to estimate the target state and correct the self-state.

[0259] To ensure that the data collected by the millimeter-wave radar and visual sensor can be fused and processed, it is necessary to perform spatial alignment and time alignment on the data of the two different sensors.

[0260] Normally, due to the inconsistent data acquisition frequencies of the millimeter-wave radar and visual sensor, it is necessary to perform time synchronization alignment processing on the millimeter-wave radar and visual sensor data. Using the precise timestamp of GNSS, the GNSS time can be used as the unified time source to provide the reference time for the millimeter-wave radar and visual sensor. The system controller can then send data acquisition commands to both of them uniformly to achieve the time synchronization of the millimeter-wave radar and visual sensor data acquisition.

[0261] Next, it is necessary to perform spatial alignment processing on the millimeter-wave radar and visual sensor data, which is specifically as follows:

[0262] 1) Association between the radar coordinate system and the world coordinate system

[0263] Refer to Figure 5 , Figure 5 which shows the relationship between the millimeter-wave radar coordinate system and the three-dimensional world coordinate system. Among them, the coordinate systems O r -X r Y r Z r and O w -X w Y w Z w are the millimeter-wave radar coordinate system and the three-dimensional world coordinate system respectively. P 1 is the position of the target, and the distance from the origin O r of the millimeter-wave radar coordinate system is R. The coordinates of the origin of the millimeter-wave radar in the world coordinate system are O r (x r ,y r ,z r ), and β is the angle between the radar scanning plane and the horizontal plane in the world coordinate system.

[0264] The target point P 1The coordinates in the world coordinate system are as shown in Equation (90).

[0265]

[0266] 2) Association between the visual sensor coordinate system and the world coordinate system

[0267] For a measurement target, the coordinates in the world coordinate system are represented as P(x w , y w , z w ), and the coordinates of this target point in the visual sensor coordinate system are represented as P(x c , y c , z c ). Then the coordinate system transformation relationship is as shown in Equation (91).

[0268]

[0269] Among them, R is the rotation matrix, which is an orthogonal matrix of size 3×3; T is the translation matrix.

[0270] 3) Association between the visual sensor and the image coordinate system

[0271] According to the pinhole imaging principle, the coordinate formula expression of the target point in the pixel coordinate system can be obtained as shown in Equation (92).

[0272]

[0273] Among them, (x pic , y pic ) are the coordinates corresponding to the target point in the image coordinate system, and f is the focal length of the visual sensor. The matrix expression of the conversion relationship between the three-dimensional point P(x c , y c , z c ) in the visual sensor coordinate system and the two-dimensional point (x pic , y pic ) in the corresponding image coordinate system is as shown in Equation (93).

[0274]

[0275] Among them, x and y are the horizontal and vertical axes of the image coordinate system respectively.

[0276] 4) Association between the image and the pixel coordinate system

[0277] The image coordinate system and the pixel coordinate system are in the same plane, and there are scaling transformation and translation transformation between them. The conversion relationship between the pixel coordinate system and the image coordinate system is as shown in Equation (94).

[0278]

[0279] Among them, (x pic , y pic ) is the coordinate of the target point in the image coordinate system, and (c x , c y ) is the coordinate of the origin of the image coordinate system in the pixel coordinate system; dx and dy are the physical sizes of each pixel point in the x-axis and y-axis directions of the image coordinate system respectively. (u p , v p ) is the corresponding coordinate of the target point in the pixel coordinate system.

[0280] By performing time and space alignment on the millimeter-wave radar and vision sensor data through the above steps, the target detection points of the millimeter-wave radar are projected onto the image, and the detection points are two-dimensional data points. Assume that the position of the object detected by the radar in its coordinate system is (L, β), and the corresponding position in the image coordinate system is (x pic , y pic ). According to the spatial coordinate conversion relationship, the relationship between the pixel size of the detection box detected by the radar and its position can be obtained, and the expression is shown in Equation (95).

[0281]

[0282] Among them, w r , h r are the width and height of the region of interest respectively; W and H are the width and height of the actual target respectively; f is the focal length of the vision sensor; x 0 , y 0 are the regions of interest of the millimeter-wave radar, that is, the coordinate values of the upper left corner of the rectangular box; dx and dy are the ratios of the picture pixel size to the size of the photosensitive chip of the vision sensor respectively.

[0283] Through Equation (95), the region of interest of the millimeter-wave radar can be projected onto the image, that is, the visualization rectangular box of the millimeter-wave radar detection and tracking result is obtained. Next, the rectangular boxes of the two need to be fused according to a certain fusion rule to determine the final output result. The decision-level fusion rule adopted in the embodiment of the present invention is the intersection over union (IOU) fusion. Through the relevant information calculated from the millimeter-wave radar data, the size and position pixel information of the radar detection box are mapped into the image. Define the millimeter-wave radar detection box (the first image data) as W r , the vision sensor detection box (the second image data) as W c , and define the area of the overlapping region of the two detection boxes as W s , then the calculation formula expression of IOU is shown in Equation (96).

[0284]

[0285] Among them, W c=(pic_xc - pic_x1)*(pic_yc - pic_y1), W r =(w_xr - w_x1)*(w_yr - w_y1), where pic_xc and pic_yc are the pixel coordinates of the center point of the detection frame of the vision sensor, and pic_x1 and pic_y1 are the pixel coordinates of the upper left corner of the detection frame of the vision sensor; w_xr and w_yr are the coordinates of the center point of the detection frame of the millimeter-wave radar in the image coordinate system, and w_x1 and w_y1 are the coordinates of the upper left corner of the detection frame of the millimeter-wave radar in the image coordinate system.

[0286] When it is necessary to determine whether the target detection results of the vision sensor and the millimeter-wave radar come from the same actual target object, it is only necessary to judge according to whether the IOU (image data coincidence ratio) calculated by formula (96) exceeds the threshold. After determining the same target, combining the high-precision distance measurement of the millimeter-wave radar and the image features provided by the vision sensor, more accurate target positioning can be achieved to determine the position of the docking device.

[0287] In some embodiments, step S450 may include but is not limited to steps S451 to S453:

[0288] Step S451, obtain the target of the leading vehicle;

[0289] Step S452, based on the target detection algorithm, obtain the relative pose of the docking devices between the leading vehicle and the following vehicle according to the target and the position of the docking device of the leading vehicle;

[0290] Step S453, according to the relative pose, adjust the pose of the following vehicle and dock the docking device of the following vehicle to the docking device of the leading vehicle.

[0291] In steps S451 to S453 of some embodiments, by installing a target such as an ArUco code at a specific position of the leading vehicle, and using the vision sensor of the following vehicle to scan and identify during the precise docking detection stage, the target information of the leading vehicle is obtained. Based on a deep learning target detection algorithm such as YOLOv4-tiny, combined with the position of the target and the docking device, the pre-calibrated external parameters, and the internal parameters of the vision sensor, the relative pose of the docking devices of the two vehicles is obtained through coordinate transformation and calculation. Then, the relative pose information is transmitted to the vehicle motion and docking device control system. The motion control system adjusts the speed and angle of the following vehicle accordingly to make it approach the leading vehicle. The docking device control system starts the docking action at the appropriate time, extends the connecting component and locks it to achieve precise docking between the vehicles.

[0292] When the following vehicle approaches the target of the leading vehicle and the distance between them is shortened to the desired distance, the system will switch to the precise matching mode. At this stage, the vision sensor starts to detect the ArUco markers located on the docking target. By analyzing the positions of these markers on the image plane of the vision sensor and their deformation conditions, the system can accurately calculate the spatial geometric information of the target object surface. Further, the relative pose between the following vehicle and the target is determined by using the external parameter solution technology, so as to provide the necessary speed and angle reference data for the motion control system.

[0293] In the local precise pose solution part, visual assistance detection is selected to help the reconstruction vehicle estimate the pose of the docking interface. The embodiment of the present invention adopts an efficient and low-computation-cost method to realize fast relative pose estimation by detecting binary fiducial markers: specifically, the ArUco codes fixed on the main frame of the vehicle to be reconstructed and docked. This method first uses the ArUco codes for preliminary pose detection, and then further accurately calculates the relative position and attitude between the two through the external parameter solution interface between the pre-calibrated ArUco codes and the passive docking mechanism.

[0294] Refer to Figure 6 , the adopted ArUco code consists of two parts, namely the internal binary matrix and the black outer border. The internal matrix can verify the unique ID of this two-dimensional code and perform verification, and the external black border can enhance its features, which helps to quickly detect the ArUco. Taking Figure 6 as an example, the ArUco target adopted in the embodiment of the present invention is a 5*5 size. After removing the outer border, black represents 0 and white represents 1; among them, the matrices in the 1st, 3rd, and 5th rows are parity bits, and the 2nd and 4th rows are data bits, with a total of 10 data bits.

[0295] A deep learning object detection algorithm based on the regression method is adopted, and a fast object detection framework based on YOLOv4-tiny is used. This framework uses a feature pyramid network to replace the spatial pyramid pooling and path aggregation network to extract feature maps of different scales, so as to improve the object detection speed. The loss function is an important index to evaluate the network training situation. The loss function not only measures the performance of the current model, ensures that the model is gradually improved until it reaches the best fitting state, but also guides the network to adjust the parameter learning through the error backpropagation generated by the network prediction result and the real sample label.

[0296] The algorithm loss function consists of three parts, as shown in Equation (97).

[0297] loss = loss 1 + loss 2 + loss 3 , (97)

[0298] The first part is the confidence loss function loss 1 The expressions are shown in Equations (98) and (99).

[0299]

[0300] Among them, s 2 represents the number of grids into which the image is divided. Assume the image is divided into S×S grids; represents the weight indicating whether the j-th bounding box in the i-th grid contains the target object; represents the predicted confidence of the j-th bounding box in the i-th grid; represents the true confidence of the j-th bounding box in the i-th grid; λ noobj is a hyperparameter used to balance the loss contribution of the bounding boxes without target objects; P i,j represents the probability that the j-th bounding box in the i-th grid is a target object, represents the intersection over union between the predicted bounding box and the true bounding box.

[0301] The second part is the classification loss function loss 2 The expression is shown in Equation (100).

[0302]

[0303] Among them, and are respectively the predicted probability and the true probability that the object belongs to class c in the j-th bounding box of the i-th grid.

[0304] The third part is the bounding box regression loss function as shown in Equation (101).

[0305]

[0306] Among them, IOU represents the degree of overlap between the predicted bounding box and the true bounding box; ρ 2 (b, b gt ) represents a certain distance or difference measure between the predicted bounding box b and the true bounding box b gt ; c is a constant used to adjust the influence of ρ 2 (b, b gt ); w and h respectively represent the width and height of the predicted bounding box; w gt and h gt respectively represent the width and height of the true bounding box.

[0307] The relative pose between the image solution and the target is obtained through a depth vision sensor. The target is fixed at a specific position on the lead vehicle, and the relative pose between the target and the docking interface is equivalent to the fixed external parameters. Therefore, an accurate relative pose estimation can be obtained through the target, and finally a fast, robust, and highly stable pose estimation of the docking interface can be achieved. The poses of the active and passive docking mechanisms are estimated through the planar pose of the ArUco target. The image perspective transformation formula is shown in Equation (102).

[0308] s*p i =A[R G |T G q i , (102)

[0309] Where:

[0310]

[0311] Among them, s* represents a scale factor, which is used to represent the scaling ratio from the world coordinate system to the image coordinate system; p i represents the projection coordinates of the i-th point in the image coordinate system; A represents the intrinsic parameter matrix of the vision sensor; R G represents the rotation matrix; T G represents the translation vector; q i represents the coordinates of the i-th point in the world coordinate system; r ij represents the components in the rotation matrix; t i represents the components in the translation vector; X G 、Y G 、Z G respectively represent the X, Y, and Z axis coordinates of the point in the world coordinate system.

[0312] In the embodiment of the present invention, the RGB image and the depth image are registered through the vision sensor, and then the three-dimensional spatial coordinates corresponding to all pixels in each detection frame can be directly calculated by combining the depth information in the depth image. Finally, the centroid of the three-dimensional point set corresponding to each cluster of detection frames is used as the three-dimensional spatial coordinates of the object, and the calculation method is shown in Equation (106).

[0313]

[0314] Among them, T z represents the depth factor, which is used to represent the depth scaling ratio from the world coordinate system to the image coordinate system; u and v represent the pixel coordinates in the image coordinate system, usually a two-dimensional vector, which represents the position of the point on the image; f x and f y represent the focal lengths of the vision sensor in the x-axis and y-axis directions; γ represents the slope factor, usually 0; u 0 and v0 Indicates the position of the principal point, i.e., the pixel coordinates of the image center.

[0315] Regarding the center point of the square target as the origin of the target coordinate system, the three-dimensional information of multiple points of the target can be obtained. Using the expression of the visual sensor coordinate system (102), equation (107) can be obtained.

[0316]

[0317] Among them, fX c and fY c respectively represent the pixel coordinates in the visual sensor coordinate system; Z c represents the depth coordinate in the visual sensor coordinate system; respectively represent the transposes of the first row, second row, and third row of the rotation matrix; T x , T y , T z respectively represent the translation transformations of the X, Y, and Z axes from the world coordinate system to the visual sensor coordinate system; X G , Y G , Z G The coordinates of the i-th point in the world coordinate system.

[0318] When the origin of the world coordinate system is near the center of the target, the average depth can be considered as the T z component in the translation vector T, that is, the average value of the Z G of each point, Z G ≈T z , and formula (108) can be obtained.

[0319]

[0320] Among them: wu and wv respectively represent the u and v coordinates of the point in the image coordinate system; w represents the depth factor, used to represent the depth scaling ratio from the world coordinate system to the image coordinate system; s represents the scale factor, used to represent the ratio from the visual sensor coordinate system to the image coordinate system; R 11 , R 12 , R 13 are the elements of the first row of the rotation matrix; R 21 , R 22 , R 23 are the elements of the second row of the rotation matrix; R 31 , R 32 , R 33 are the elements of the third row of the rotation matrix; T x , T y respectively represent the translation amounts of the translation vector on the x-axis and y-axis; T zRepresents the depth factor, which is used to represent the depth scaling ratio from the world coordinate system to the visual sensor coordinate system.

[0321] Next, the PnP method is adopted to obtain the rotation vector of 12 unknowns. Through the Rodrigues equation, the rotation vector is converted into a 3*3 rotation matrix R, that is, the external parameters are solved, and the three-dimensional coordinates of the marker in the visual sensor coordinate system are calculated through Equation (108).

[0322] The pose of the docking component mechanism solved in this way is for the ArUco code reference coordinate system. Since the position of the ArUco code is known, the pose of the visual sensor in the world coordinate system can be solved through coordinate conversion, and the pose of the interface on the component to be docked can also be obtained. Further, through the known pose of the visual sensor, the transformation matrix of the visual sensor actively docking with the component interface can be solved. Therefore, the relative pose between the active and passive docking interfaces of the two reconstructed vehicles (the relative pose of the docking device between the leading vehicle and the following vehicle) can be solved.

[0323] Through the above analysis, combined with the error equation expression between the two vehicles, the two vehicles can maintain a stable dynamic driving at the desired distance; on this basis, the hardware docking device realizes a hard connection and is locked by the locking device, thereby realizing the intelligent reconstruction between the vehicles.

[0324] In some embodiments, refer to Figure 7 , Figure 7A multi-vehicle docking process flow chart is provided as follows: First, establish the IMU specific force equation, determine the velocity update equation, and obtain the vehicle position data at each moment during driving; establish the pseudorange observation equation and carrier phase observation equation of GNSS, and determine the carrier phase observation equation and double-difference pseudorange equation; use the LAMBDA algorithm to solve and fix the integer ambiguity, so as to obtain the positioning result under the GNSS system; establish the state equation and measurement equation of integrated navigation; use the Kalman filtering algorithm to realize the data fusion of the two, and obtain the estimated value of the optimal state of the own vehicle; construct a single-vehicle autonomous motion model, and determine the state equation of the single-vehicle autonomous vehicle motion model; on this basis, construct a two-vehicle motion model, and determine the motion constraint relationship between the two vehicles; and construct the position error equation and tracking error model of the model system; according to the vehicle position error and tracking error equations, design a following vehicle feedback linearization controller, and realize the control of the relative position stability of the two vehicles in the model system by controlling the magnitude of the input quantity; when the distance between the rear vehicle's head and the front vehicle's tail is relatively close, align the millimeter-wave radar and vision sensor data in space and time; realize the data fusion of the millimeter-wave radar and vision sensor data; combine the information after the data fusion of the millimeter-wave radar and vision sensor, and based on the following vehicle feedback linearization controller, realize the precise reconstruction of the two-vehicle system. This process is flexible, real-time, efficient, and robust.

[0325] In some embodiments, referring to Figure 8 , the combination relationship of vehicle units is changed through the automatic connection and positioning between vehicles to realize the reconstruction of its configuration. The multi-vehicle intelligent reconfigurable system includes, but is not limited to, the following different structural forms: chain structure, diamond structure, T-shaped structure, parallel structure, so as to flexibly adapt to the carrying requirements such as the size, shape, and weight of the goods, achieve system reconstruction and cascade expansion, and further improve the carrying capacity; it provides the possibility to expand the application range of intelligent reconfigurable vehicles from the current heavy-load low-speed transportation to the high-speed heavy-load transportation field, breaking through the carrying limit of traditional heavy-duty vehicles.

[0326] The intelligent reconstruction docking forms include, but are not limited to, the following reconstruction forms: Figure 8 In (a) of Figure 8 , the chain structure, the most basic form is "1*2", and it can be expanded to "1*3", "1*4", and so on; Figure 8 In (b) of Figure 8 , the diamond structure, which extends the most basic diamond structure on the basis of (a); Figure 8The most basic form is a "1*2" chain structure, which reconstructs two vehicles into an integrated system, and different structural combinations can be continuously stacked, extended, and expanded on this basis.

[0327] In this embodiment, referring to Figure 9 , Figure 9 is a schematic diagram of the intelligent reconfigurable vehicle multi-vehicle collaborative docking system device provided by the embodiment of the present invention. The intelligent reconfigurable system device for a dynamic driving scenario includes multiple sensor acquisition modules, a processor module, and a memory module. The connections between the modules can connect the data acquisition module, the processor module, and the memory module in a wired or wireless manner. The system device controls the vehicle control unit in a wired or wireless manner to achieve control of the vehicle's motion state.

[0328] The embodiment of the present invention also provides a multi-vehicle collaborative docking system, including:

[0329] A first module for obtaining the position information, status information, and expected distance of vehicle docking of several vehicles, where the several vehicles are divided into a leading vehicle and following vehicles;

[0330] A second module for constructing a leading-following vehicle motion model based on the position information and status information;

[0331] A third module for using a feedback linearization speed controller to change the position of the following vehicle according to the leading-following vehicle motion model with the goal of controlling the docking distance between the following vehicle and the leading vehicle to be equal to the expected distance;

[0332] A fourth module for controlling the docking device of the following vehicle and the docking device of the leading vehicle to dock when the docking distance is equal to the expected distance.

[0333] It can be understood that the content in the above embodiments of the multi-vehicle collaborative docking method is applicable to the embodiments of this system. The functions specifically implemented by the embodiments of this system are the same as those of the above embodiments of the multi-vehicle collaborative docking method, and the beneficial effects achieved are also the same as those of the above embodiments of the multi-vehicle collaborative docking method.

[0334] Next, in combination with Figure 10 the electronic device of the embodiments of the present application will be introduced in detail.

[0335] As Figure 10 , Figure 10 shows the hardware structure of an electronic device in another embodiment. The electronic device includes:

[0336] The processor 1100 can be implemented in the form of a general-purpose central processing unit (CPU), a microprocessor, an application-specific integrated circuit (ASIC), or one or more integrated circuits, etc., and is used to execute relevant programs to implement the technical solutions provided in the embodiments of the present disclosure; the memory 1200 can be implemented in the form of a read-only memory (ROM), a static storage device, a dynamic storage device, or a random access memory (RAM), etc. The memory 1200 can store an operating system and other application programs. When implementing the technical solutions provided in the embodiments of this specification through software or firmware, the relevant program codes are stored in the memory 1200 and are called by the processor 1100 to execute the multi-vehicle collaborative docking method in the embodiments of the present disclosure; the input / output interface 1300 is used to implement information input and output; the communication interface 1400 is used to implement communication interaction between this device and other devices, and can communicate through wired means (such as USB, network cable, etc.) or can also communicate through wireless means (such as mobile network, WIFI, Bluetooth, etc.); the bus 1500 transmits information between various components of the device (such as the processor 1100, the memory 1200, the input / output interface 1300, and the communication interface 1400); among them, the processor 1100, the memory 1200, the input / output interface 1300, and the communication interface 1400 are communicatively connected to each other inside the device through the bus 1500.

[0337] The embodiments of the present disclosure also provide a storage medium, which is a computer-readable storage medium. The computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to cause a computer to execute the above-mentioned multi-vehicle collaborative docking method. As a non-transitory computer-readable storage medium, the memory can be used to store non-transitory software programs and non-transitory computer-executable programs. In addition, the memory can include a high-speed random access memory, and can also include non-transitory memory, such as at least one magnetic disk storage device, a flash memory device, or other non-transitory solid-state storage devices. In some embodiments, the memory can include a memory remotely set relative to the processor, and these remote memories can be connected to the processor through a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an enterprise intranet, a local area network, a mobile communication network, and combinations thereof.

[0338] The preferred embodiments of the present disclosure have been described above with reference to the accompanying drawings, which do not limit the scope of rights of the embodiments of the present disclosure. Any modifications, equivalent substitutions, and improvements made by those skilled in the art without departing from the scope and essence of the embodiments of the present disclosure shall fall within the scope of rights of the embodiments of the present disclosure.

Claims

1. A multi-vehicle collaborative docking method, characterized in that: The following steps are involved: Acquiring location information, status information, and expected distances for vehicle docking of a plurality of vehicles, wherein the plurality of vehicles are divided into a pilot vehicle and a following vehicle; Constructing a pilot-follower vehicle motion model according to the position information and the state information; With the goal of controlling the docking distance between the following vehicle and the pilot vehicle to be equal to the desired distance, the position of the following vehicle is changed according to the pilot-following vehicle motion model using a feedback linearized speed controller; When the docking distance is equal to the expected distance, the docking device of the following vehicle and the docking device of the pilot vehicle are controlled to dock.

2. The multi-vehicle collaborative docking method according to claim 1, characterized in that: The position information and status information of the vehicle are determined by the following steps: Obtain error data of the inertial measurement unit; Obtaining first speed information and first position information of the vehicle according to the motion state of the vehicle by means of the inertial measurement unit; obtaining second speed information and second position information of the vehicle according to the motion state of the vehicle through a global navigation satellite system; Establishing a state equation for integrated navigation of the inertial measurement unit and the global navigation satellite system according to the error data of the inertial measurement unit; Establishing a measurement equation for the integrated navigation of the inertial measurement unit and the global navigation satellite system according to the first speed information, the first position information, the second speed information and the second position information; Based on the Kalman filter algorithm, the vehicle position information and state information are obtained according to the state equation and the measurement equation.

3. The multi-vehicle collaborative docking method according to claim 1, characterized in that: The step of constructing a pilot-follower vehicle motion model according to the position information and the state information comprises the following steps: Obtain a single vehicle motion model; A leading-following vehicle motion model is obtained according to the position information, the state information and the single vehicle motion model.

4. The multi-vehicle collaborative docking method according to claim 3, characterized in that: The single vehicle motion model is determined by the following steps: Acquire structural data of the vehicle, the structural data including a first structural position relationship between the front wheels and the center of mass of the vehicle, a second structural position relationship between the rear wheels and the center of mass, a front and rear wheel constraint relationship, and a relationship between the vehicle speed and the position change of the vehicle center of mass; Obtaining a vehicle structure constraint equation according to the first structure position relationship and the second structure position relationship; According to the front and rear wheel constraint relationship and the vehicle structure constraint equation, a first motion equation of the vehicle is obtained; According to the relationship between the vehicle speed and the position change of the vehicle center of mass and the first vehicle motion equation, a second vehicle motion equation is obtained; A single vehicle motion model is determined according to the first vehicle motion equation and the second vehicle motion equation.

5. The multi-vehicle collaborative docking method according to claim 1, characterized in that: The method aims to control the docking distance between the following vehicle and the pilot vehicle to be equal to the expected distance, and according to the pilot-following vehicle motion model, uses a feedback linearized speed controller to change the position of the following vehicle, including the following steps: Obtaining an error equation between the pilot vehicle and the following vehicle according to the expected distance and the pilot-following vehicle motion model; constructing the feedback linearization speed controller according to the error equation and the speed information of the following vehicle, wherein the state information includes the speed information; According to the feedback linearization speed controller, the position of the following vehicle is changed by controlling the speed of the following vehicle.

6. The multi-vehicle collaborative docking method according to claim 1, characterized in that: The controlling the docking device of the following vehicle to dock with the docking device of the pilot vehicle comprises the following steps: The docking device of the pilot vehicle is identified by the millimeter-wave radar and the visual sensor of the following vehicle to obtain first image data collected by the millimeter-wave radar and second image data collected by the visual sensor, wherein the first image data and the second image data both include a time tag and a location tag; Synchronizing the first image data and the second image data in time according to the time tag to obtain the first image data and the second image data after time alignment; According to the position tag, spatially aligning the time-aligned first image data and the second image data to obtain the time-aligned and spatially aligned first image data and the second image data; Fusing the first image data and the second image data after being aligned in time and space to obtain an image data overlap ratio; Determining the position of the docking device of the pilot vehicle according to the image data overlap ratio and the position tag; By means of a target detection algorithm, the docking device of the following vehicle is controlled to dock with the docking device of the pilot vehicle according to the position of the docking device of the pilot vehicle.

7. The multi-vehicle collaborative docking method according to claim 6, characterized in that: The method of controlling the docking device of the following vehicle to dock with the docking device of the pilot vehicle according to the position of the docking device of the pilot vehicle through a target detection algorithm comprises the following steps: Acquiring a target of the pilot vehicle; Based on the target detection algorithm, according to the positions of the target and the docking device of the pilot vehicle, the relative position and posture of the docking device between the pilot vehicle and the following vehicle are obtained; According to the relative position and posture, the posture of the following vehicle is adjusted and the docking device of the following vehicle is docked with the docking device of the pilot vehicle.

8. A multi-vehicle collaborative docking system, characterized in that: include: The first module is used to obtain the position information, state information and expected distance of vehicle docking of a plurality of vehicles, wherein the plurality of vehicles are divided into a pilot vehicle and a following vehicle; The second module is used to construct a pilot-follower vehicle motion model according to the position information and the state information; A third module is used to change the position of the following vehicle by using a feedback linearized speed controller according to the pilot-following vehicle motion model with the goal of controlling the docking distance between the following vehicle and the pilot vehicle to be equal to the desired distance; The fourth module is used to control the docking device of the following vehicle and the docking device of the pilot vehicle to dock when the docking distance is equal to the expected distance.

9. An electronic device, characterized in that: include: at least one processor; at least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor implements the multi-vehicle collaborative docking method as described in any one of claims 1-7.

10. A computer-readable storage medium storing a program executable by a processor, characterized in that: The program executable by the processor is used to implement the multi-vehicle collaborative docking method as described in any one of claims 1-7 when executed by the processor.

Images