A trajectory coordination optimization control method in a dual-vehicle cooperative operation scene

Abstract

The application discloses a kind of trajectory coordination optimization control methods under the scene of double-vehicle cooperative operation, it is related to intelligent vehicle cooperative control technical field, by constructing double-vehicle six degrees of freedom relative pose and real-time dynamics mapping model of operating load, load gravity center offset, inertia tensor variation and stress disturbance are identified online;Further, the collaborative optimization framework of "load-pose-trajectory coupling" is constructed, the dynamic characteristics of operating load are directly embedded into the objective function and hard constraint condition of trajectory optimization, the deep coupling of load characteristics and trajectory, pose optimization is realized, at the same time, using the rolling horizon optimization method, the optimal expected trajectory of double-vehicle, collaborative pose adjustment amount and load disturbance feedforward compensation amount are solved synchronously in each control period, the global optimization of double-vehicle trajectory and the synchronous promotion of tracking accuracy are realized.

Description

Technical Field

[0001] This invention relates to the field of intelligent vehicle cooperative control technology, specifically to a trajectory cooperative optimization control method for dual-vehicle cooperative operation scenarios. Background Technology

[0002] Dual-vehicle collaborative operation is a key operational method in industrial and special operation scenarios such as heavy-duty handling, large-item hoisting, precision assembly, and emergency rescue. By coordinating the work of two vehicles, it effectively overcomes the limitations of single-vehicle operation in terms of load, size, and space, significantly improving operational capabilities and flexibility. In many complex scenarios with extremely high operational requirements, dual-vehicle collaborative operation can fully leverage its advantages to accomplish tasks that are difficult for a single vehicle, becoming an important means of ensuring the smooth operation of various tasks.

[0003] However, existing dual-vehicle cooperative control technologies still have the following core limitations when dealing with the above-mentioned complex working conditions. First, most existing technologies adopt a static approach to the load model, treating the weight, center of gravity, and inertia tensor of the working load as fixed constraints or static boundary conditions, ignoring the dynamic characteristics of the load caused by factors such as the relative pose of the two vehicles and the progress of the operation. This results in the inability to establish a real-time dynamic mapping relationship between the load and the pose of the two vehicles, causing the control model to be seriously out of touch with the actual working conditions and failing to accurately reflect the real situation during the operation. Second, existing control processes generally adopt a decoupling and separation approach, executing trajectory planning, pose coordination adjustment, and load disturbance compensation as three independent steps in sequence. First, a baseline trajectory is planned under fixed constraints, then pose deviations are corrected, and finally, post-event feedback compensation is performed for load disturbances. This step-by-step execution approach has obvious control lag problems, and decoupling errors will continue to accumulate. Under dynamic working conditions, the accuracy of cooperative control drops significantly, seriously affecting the quality and safety of the operation. Finally, existing technologies do not directly embed the dynamic characteristics of the load into the objective function and hard constraints of trajectory optimization, but only treat the load constraints as the outer boundary of trajectory planning. This makes it impossible to achieve balanced control of load stress throughout the entire process at the source of trajectory optimization, which can easily lead to problems such as load overload, local stress concentration, and operational instability. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and provide a trajectory collaborative optimization control method for dual-vehicle cooperative operation scenarios. By constructing a real-time dynamic mapping model of the six-degree-of-freedom relative pose of the two vehicles and the operating load, online identification of load center of gravity shift, inertial tensor changes, and force disturbances is achieved. Based on this, a load-pose-trajectory coupled collaborative optimization framework is constructed, directly embedding the dynamic characteristics of the operating load into the objective function and hard constraints of trajectory optimization, achieving deep coupling between load characteristics and trajectory and pose optimization. At the same time, a rolling time-domain optimization method is adopted to simultaneously solve the optimal expected trajectory of the two vehicles, the collaborative pose adjustment amount, and the load disturbance feedforward compensation amount in each control cycle, realizing the global optimization of the dual-vehicle trajectory and the synchronous improvement of tracking accuracy.

[0005] To solve the above-mentioned technical problems, the present invention provides the following technical solution: a trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario, the method comprising:

[0006] S1: Construct the basic dynamic model of the dual-vehicle cooperative operation coupling system, including the six-degree-of-freedom rigid body dynamic model of the dual vehicles, the rigid body dynamic model of the operating load, and the coupling constraint relationship between the dual vehicles and the operating load;

[0007] S2: Construct a real-time dynamic mapping model of the relative pose of two vehicles with six degrees of freedom and the working load, and establish a real-time mapping relationship between the relative pose of the two vehicles, the working process, the spatial pose and the center of gravity position, inertial tensor and force state of the working load.

[0008] S3: Based on real-time collected data on the position and posture of the two vehicles and the stress state of the working load, combined with the real-time dynamic mapping model, the center of gravity offset, inertia tensor change and real-time force disturbance of the working load are identified online, and the full dynamic characteristic parameters of the working load are obtained.

[0009] S4: Construct a load-pose-trajectory coupled collaborative optimization objective function and real-time hard constraints, and embed the acquired full dynamic characteristic parameters of the work load into the objective function and hard constraints, wherein the hard constraints include the work load force balance constraint and the allowable load limit constraint.

[0010] S5: Based on the rolling time-domain optimization framework, in each control cycle, the optimal expected trajectory of the two vehicles, the cooperative pose adjustment amount and the load disturbance feedforward compensation amount are solved simultaneously with the current dual-vehicle pose state and the full dynamic characteristic parameters of the load as initial conditions, so as to realize the synchronous solution of trajectory optimization, pose adjustment and load compensation.

[0011] S6: Decompose the optimal control quantity obtained by synchronous solution into the underlying motion control commands of the master vehicle and the slave vehicle, and combine them with closed-loop feedback control to ensure the trajectory tracking accuracy and posture coordination accuracy of the two vehicles, while realizing the full-process balanced control of the working load force.

[0012] S7: Repeatedly execute collaborative control optimization in each control cycle to complete closed-loop real-time collaborative optimization control of the entire operation process.

[0013] Furthermore, in S1, the specific steps for constructing the basic dynamic model of the dual-vehicle cooperative operation coupling system are as follows:

[0014] Using the geodetic coordinate system as the reference and the vehicle body local coordinate system as an aid, six-degree-of-freedom rigid body dynamic models of two working vehicles are established respectively. Integrating the inertia, damping and driving characteristics of the chassis running mechanism and the vehicle-mounted working mechanism, dynamic equations including three-axis translation (X / Y / Z) and three-axis rotation (roll / pitch / yaw) are constructed to characterize the dynamic mapping relationship between vehicle posture, motion state and driving torque and external disturbances.

[0015] By combining the mass, geometric dimensions and mass distribution parameters of the working load, a rigid body dynamics model of the load itself is established. For flexible deformation loads, equivalent stiffness and inertia correction terms are introduced to characterize the relationship between the translational and rotational states of the load and the external forces and moments it is subjected to.

[0016] By defining the rigid binding relationship between the position and orientation of the dual-vehicle operation execution end and the load support point, as well as the balance relationship between the transmission of force and torque, motion coupling constraints and mechanical coupling constraints between the dual vehicles and the load are established, integrating the dual-vehicle dynamic model and the load dynamic model into a unified coupled system basic dynamic model.

[0017] Furthermore, in S2, using the geodetic coordinate system as a reference, a six-degree-of-freedom relative pose vector for the two vehicles is defined:

[0018]

[0019] These correspond to the relative translational displacement, relative roll, pitch, and yaw attitude angles of the two vehicles along the three axes, respectively.

[0020] Combined with the work process time With the global spatial pose and motion state vectors of the two vehicles A real-time dynamic mapping model between the two vehicles and the operating load is constructed through recursive derivation of multibody dynamics:

[0021]

[0022] This is the real-time composite force matrix of the load, including the supporting force, inertial force, and environmental disturbance force. The load inertia tensor matrix is ​​dynamically transformed with the relative pose and spatial attitude of the two vehicles. The model represents the real-time center of gravity position vector of the load due to the relative displacement of the support points. It also incorporates the changes in support constraints caused by the relative motion of the two vehicles, the load attitude deflection caused by the attitude adjustment of the actuator during the operation process, and the mass distribution projection correction term caused by the spatial attitude transformation. This fully realizes the real-time quantitative mapping of the relative attitude of the two vehicles, the operation process, the global spatial attitude, and the load center of gravity position, inertial tensor, and stress state.

[0023] Furthermore, in S3, the global pose, velocity, and acceleration status of the two vehicles are collected in real time through the vehicle-mounted GNSS and IMU combined navigation module. The real-time force and torque data of the load support point are obtained according to the multi-dimensional force / torque sensor on the operation execution end. The pose feedback signal of the operation mechanism is collected simultaneously. The collected real-time data is input into the constructed real-time dynamic mapping model of the two vehicles and the load. The recursive least squares algorithm with forgetting factor is used to carry out online identification cycle by control cycle. The current center of gravity offset of the load is obtained by inversion fitting of the dynamic equation. The real-time change of the inertia tensor is calculated iteratively by combining the spatial pose transformation relationship. The deterministic components of the support force and inertial force are separated from the composite force. The real-time force disturbance caused by the environment and motion is extracted. Finally, the full dynamic characteristic parameters covering the load mass distribution, inertia characteristics, and external disturbances are obtained.

[0024] Furthermore, in S4, the collaborative optimization objective function is:

[0025]

[0026] For trajectory tracking error cost term, .in, To predict the length of the time domain, To predict the first in the time domain The reference pose and motion state vectors of the two vehicles at each moment. To predict the first in the time domain The optimized pose and motion state vectors of the two vehicles at each moment. The L2 norm is used to calculate the sum of squares of the differences between the actual and reference states of the two vehicles, which characterizes the magnitude of the trajectory tracking error.

[0027] The cost term for the relative pose deviation between the two vehicles. ;in, To predict the first in the time domain The expected relative pose vectors of the two vehicles at each moment. To predict the first in the time domain The actual optimized relative pose vectors of the two vehicles at each time point are used to characterize the degree of pose coordination deviation between the two vehicles by calculating the sum of squared differences between the reference values ​​and the actual values ​​of the relative poses of the two vehicles.

[0028] This refers to the cost of load fluctuations and imbalances. ;in, To predict the first in the time domain Real-time composite force matrix of load at each moment This represents the theoretical equilibrium force value at each support point of the load. To predict the first in the time domain The standard deviation of the load force at each support point at each moment is used to characterize the load force fluctuation and the degree of imbalance by summing the sum of the squares of the force deviations and the standard deviation of the force.

[0029] This is an energy consumption cost item for dual-vehicle operation. ;in, To predict the first in the time domain The driving torque vector of the master or slave actuator at any given moment Corresponding to the main vehicle, Corresponding to the slave vehicle, the sum of the squares of the driving torques of the actuators is used to characterize the driving energy consumption during the dual-vehicle operation process;

[0030] Weighting coefficients The calculation method is as follows:

[0031] First, determine the baseline value for each cost item, denoted as follows: , , , The baseline values ​​are obtained through offline simulation or statistical analysis of historical operation data, representing the typical value range of each cost item;

[0032] Secondly, based on the priority requirements of the work scenario, the weight allocation ratio of each cost item is set, denoted as . , , , ,satisfy ;

[0033] Finally, the weighting coefficients are obtained through normalization, using the following formula:

[0034]

[0035] ; corresponding in sequence , , , Furthermore, in S4, the real-time hard constraint conditions include at least:

[0036] Load constraint: The real-time force value at each load point does not exceed the preset allowable maximum value, and the force imbalance at each load point does not exceed the preset threshold.

[0037] Dual-vehicle kinematic constraints: The speed, acceleration, and jerk of both vehicles shall not exceed their respective limit thresholds;

[0038] Dual-vehicle posture coordination constraint: The relative posture deviation of the two vehicles in six degrees of freedom does not exceed the preset safe operating range;

[0039] Operating space constraints: The movement trajectories of the two vehicles and the operating load do not exceed the preset operating space and meet obstacle avoidance constraints;

[0040] System dynamics constraint: The dynamic response of the dual-vehicle and load coupled system shall not exceed the output limit of the actuator.

[0041] Furthermore, in S5, the rolling time-domain optimization framework adopts the Model Predictive Control (MPC) framework, and the prediction time domain is set to... Control time domain set to ,in Within each control cycle, the expected trajectory sequence, pose adjustment sequence, and feedforward compensation sequence of the two vehicles are simultaneously optimized in the prediction time domain. The first set of control variables in the control time domain is taken as the optimal control variable output for the current cycle.

[0042] Furthermore, the desired trajectory sequence includes position, velocity, and acceleration parameters at each moment; the dual-vehicle cooperative pose adjustment sequence includes relative pose deviation correction and pose adjustment parameters of the operating actuator; and the feedforward compensation sequence for adapting to load dynamic disturbances includes compensation for load center of gravity shift, inertia tensor change, and force disturbance.

[0043] Furthermore, in S6, the underlying motion control commands include drive / brake commands, steering commands, and position adjustment commands for the dual vehicles and the work execution mechanism; the closed-loop feedback control adopts one of PID control, sliding mode control, or adaptive robust control, and corrects the control commands in real time based on the deviation between the actual position feedback of the dual vehicles and the desired trajectory.

[0044] Beneficial effects

[0045] Compared with existing technologies, this invention has the following advantages: First, by constructing a real-time dynamic mapping model of the relative pose and working load of the two vehicles with six degrees of freedom, it realizes online identification of load center of gravity shift, inertial tensor change and force disturbance, solving the control mismatch problem caused by the static processing of the load model in traditional methods; Second, it innovatively embeds the dynamic characteristics of the load directly into the trajectory optimization objective function and hard constraints, forming a three-in-one collaborative optimization framework of load-pose-trajectory, ensuring the balance of load force throughout the entire process from the source; Third, by realizing the synchronous solution of trajectory planning, pose adjustment and load feedforward compensation through the rolling time domain optimization framework, it breaks through the bottleneck of decoupling error accumulation and control lag in the traditional step-by-step control mode, and improves the global optimality of the dual-vehicle trajectory and tracking accuracy simultaneously while ensuring operational stability.

[0046] Other advantages, objectives and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination or study, or may be learned from the practice of the invention. Attached Figure Description

[0047] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0048] Figure 1 A flowchart of a trajectory collaborative optimization control method in a dual-vehicle collaborative operation scenario;

[0049] Figure 2 This is a flowchart of step S4 of a trajectory collaborative optimization control method in a dual-vehicle collaborative operation scenario. Detailed Implementation

[0050] To further illustrate the technical means and effects of the present invention in achieving its intended purpose, the following detailed description of the specific implementation methods, structures, features, and effects of the present invention, in conjunction with the accompanying drawings and preferred embodiments, is provided below.

[0051] The purpose of this invention is to overcome the shortcomings of existing technologies and provide a trajectory collaborative optimization control method for dual-vehicle cooperative operation scenarios. By constructing a real-time dynamic mapping model of the six-degree-of-freedom relative pose of the two vehicles and the operating load, online identification of load center of gravity shift, inertial tensor changes, and force disturbances is achieved. Based on this, a load-pose-trajectory coupled collaborative optimization framework is constructed, directly embedding the dynamic characteristics of the operating load into the objective function and hard constraints of trajectory optimization, achieving deep coupling between load characteristics and trajectory and pose optimization. At the same time, a rolling time-domain optimization method is adopted to simultaneously solve the optimal expected trajectory of the two vehicles, the collaborative pose adjustment amount, and the load disturbance feedforward compensation amount in each control cycle, realizing the global optimization of the dual-vehicle trajectory and the synchronous improvement of tracking accuracy.

[0052] S1: Construct the basic dynamic model of the dual-vehicle cooperative operation coupling system, including the six-degree-of-freedom rigid body dynamic model of the dual vehicles, the rigid body dynamic model of the operating load, and the coupling constraint relationship between the dual vehicles and the operating load;

[0053] A two-level coordinate system is defined: the global coordinate system O-XYZ is used as the global inertial coordinate system, with the origin O selected as a fixed reference point within the work area. The X-axis points to the preset direction of travel, the Y-axis points to the horizontal direction, and the Z-axis is perpendicular to the horizontal plane and upwards. Local coordinate systems O1-X1Y1Z1 and O2-X2Y2Z2 are defined for the master vehicle and slave vehicle, respectively, with the origin set at the centroid of the two vehicles. The three axes of the local coordinate system are fixed to the vehicle body and undergo pose transformation synchronously with the vehicle body. A local coordinate system for the load is defined synchronously, with the origin set at the theoretical centroid of the load, used for the characterization and transformation of the load's dynamic characteristics.

[0054] For both the main vehicle and the slave vehicle, a six-degree-of-freedom rigid body dynamics model is constructed. The driving / braking characteristics and steering characteristics of the chassis running mechanism, as well as the inertia, damping and stiffness characteristics of the on-board operation actuator, are integrated to fully characterize the full-dimensional dynamic characteristics of the vehicle body in translation along the X / Y / Z axes, and in roll, pitch and yaw rotation around the three axes.

[0055] This embodiment uses the Lagrange method to construct the rigid body dynamics equations for the two vehicles, with the general form as follows:

[0056]

[0057] Representing the main vehicle, Represents the vehicle; The vehicle body is a six-degree-of-freedom generalized coordinate vector, which includes three-axis translational displacement and three-axis rotational attitude angle; The vehicle body inertia matrix, The matrix of Coriolis force and centrifugal force. The gravity term matrix, Here is the damping coefficient matrix. The driving torque / driving force vector of the actuator. This represents the external environmental disturbance vector. Through this equation, a complete dynamic mapping relationship is established between the vehicle's pose, motion state, driving input, and external disturbances.

[0058] For the operational load, a six-degree-of-freedom rigid body dynamics model is constructed based on its mass, geometric dimensions, and mass distribution parameters. The equations are as follows:

[0059]

[0060] in, The six-degree-of-freedom generalized coordinate vector of the load. Let be the inertia matrix of the load. The matrix of Coriolis force and centrifugal force. The gravity term matrix, This represents the vector of supporting force and torque exerted on the load by the dual-vehicle operating end. This is the vector of environmental disturbance forces and moments experienced by the load.

[0061] For operational loads with flexible deformation, equivalent stiffness correction terms and equivalent inertia correction terms are introduced into the inertia matrix to provide an equivalent characterization of the inertia change and stiffness characteristics caused by the flexible deformation of the load. This corrects the influence of flexible deformation on the dynamic characteristics and accurately describes the correlation between the translational and rotational states of the load and the external forces and torques it is subjected to.

[0062] By defining the pose rigid binding relationship between the dual-vehicle operation execution end and the load support point, and the force and torque transmission balance relationship, motion coupling constraints and mechanical coupling constraints between the dual vehicles and the load are established, specifically:

[0063] Motion coupling constraint: The pose of the load support point is consistent with the pose of the corresponding actuator of the working vehicle, that is... ,in Let be the pose vector of the load support point in the geodetic coordinate system. This is the homogeneous transformation matrix from the vehicle body local coordinate system to the load coordinate system. This is the pose vector of the working vehicle's actuator in the local coordinate system of the vehicle body;

[0064] Mechanical coupling constraint: The resultant force and torque exerted on the load by the dual-vehicle operating end maintain torque balance with the load's inertial force, gravity, and environmental disturbance force, i.e. .

[0065] Based on this coupling constraint, the dynamic models of the master vehicle and slave vehicle are integrated with the load dynamic model into a unified basic dynamic model of the dual-vehicle-load coupled system, which fully characterizes the dynamic characteristics of the coupled system in all dimensions.

[0066] S2: Construct a real-time dynamic mapping model of the relative pose of two vehicles with six degrees of freedom and the working load, and establish a real-time mapping relationship between the relative pose of the two vehicles, the working process, the spatial pose and the center of gravity position, inertial tensor and force state of the working load.

[0067] Using the geodetic coordinate system as a reference, the six-degree-of-freedom relative pose vector of the two vehicles as defined in equation (1) is constructed. The relative pose vector is obtained by solving the homogeneous transformation of the global pose of the two vehicles.

[0068] Define job process time The normalized time parameter for the entire job process, with a value range of [value range missing]. , Corresponding to the start time of the task. Corresponding to the completion time of the task; define the global spatial pose and motion state vectors of the two vehicles. It includes generalized coordinates, velocity, and acceleration of both the master vehicle and the slave vehicle, providing comprehensive state information.

[0069] Using the multibody dynamics recursive method, a real-time dynamic mapping model of the two vehicles and the working load defined in equation (2) is constructed. During the construction of this mapping model, three types of correction terms are simultaneously incorporated: the first type is the support constraint change term caused by the relative motion of the two vehicles, characterizing the influence of the load support point spacing and support angle changes caused by the relative posture changes of the two vehicles on the load's force and inertia characteristics; the second type is the load posture deflection correction term caused by the actuator posture adjustment during the working process, which, combined with the working process time, characterizes the influence of the load posture change caused by the working actuator's actions on the dynamic characteristics; the third type is the mass distribution projection correction term caused by spatial posture transformation, which, through homogeneous coordinate transformation, completes the real-time projection correction of the load's inertia tensor and center of gravity position under different postures. Through this model, a complete real-time quantitative mapping relationship is established between the relative posture of the two vehicles, the working process, and the global spatial posture, and the load's center of gravity position, inertia tensor, and force state.

[0070] S3: Based on real-time collected data on the position and posture of the two vehicles and the stress state of the working load, combined with the real-time dynamic mapping model, the center of gravity offset, inertia tensor change and real-time force disturbance of the working load are identified online, and the full dynamic characteristic parameters of the working load are obtained.

[0071] Within each control cycle, the global pose, three-axis velocity, and three-axis acceleration data of the master vehicle and slave vehicle are collected in real time through the vehicle-mounted GNSS and IMU integrated navigation module. The real-time support force and torque data of each support point of the load are obtained through the multi-dimensional force / torque sensor mounted on the work execution end. The pose feedback signal of the vehicle-mounted work execution mechanism is collected simultaneously to complete the synchronous collection and timestamp alignment of the real-time status data of the coupled system. The data sampling frequency is consistent with the system control cycle.

[0072] The collected and aligned real-time status data is input into the constructed real-time dynamic mapping model. A recursive least squares algorithm with a forgetting factor is used to perform online identification of the dynamic characteristics of the operational load on a control cycle basis. The specific process is as follows:

[0073] Based on the fundamental dynamic model of the coupled system, a linear regression model of the load dynamic characteristics is constructed. The load center of gravity offset, inertia tensor change, and force disturbance to be identified are used as the parameter vector to be estimated, and the collected dual vehicle posture, motion state, and load force data are used as the regression input vector.

[0074] Introducing the forgetting factor The range of values ​​is Historical data is weighted to reduce the impact of outdated data on the identification results and improve the algorithm's ability to track changes in the dynamic characteristics of the load.

[0075] Within each control cycle, based on the data collected in the current cycle, the estimated value of the parameter to be estimated is updated through a recursive formula to complete one identification iteration;

[0076] By inverting and fitting the dynamic equations, the center of gravity offset of the load at the current moment relative to the theoretical design value is obtained. Combined with the spatial pose transformation relationship, the real-time change of the inertia tensor is calculated iteratively. At the same time, the deterministic components of the support force and inertial force are extracted from the load composite force matrix, and the real-time force disturbance caused by environmental disturbance and motion impact is extracted.

[0077] Through the above online identification process, the full dynamic characteristic parameters of the coverage load mass distribution characteristics, inertia characteristics and external disturbances are finally obtained, and the identification results are synchronously updated to the subsequent collaborative optimization module.

[0078] S4: Construct a load-pose-trajectory coupled collaborative optimization objective function and real-time hard constraints, and embed the acquired full dynamic characteristic parameters of the work load into the objective function and hard constraints, wherein the hard constraints include the work load force balance constraint and the allowable load limit constraint.

[0079] A load-pose-trajectory coupled collaborative optimization objective function is constructed, and the identified full dynamic characteristic parameters of the load are embedded into the cost term of the objective function, expressed as (3), and the weighting coefficient formula is (4). The dimensional differences of each cost term are eliminated through normalization to ensure that the weighting coefficient can accurately reflect the priority of each objective.

[0080] Simultaneously construct real-time hard constraints for collaborative optimization, embedding the identified full dynamic characteristic parameters of the load into the constraints, specifically including:

[0081] Load constraint: The real-time force value of each support point does not exceed the preset allowable maximum value, and the force imbalance of each support point does not exceed the preset threshold. The force imbalance is defined as the ratio of the difference between the maximum and minimum force values ​​of each support point to the average force value.

[0082] Dual-vehicle kinematic constraints: The travel speed, steering angular velocity, three-axis acceleration, and three-axis jerk of both vehicles shall not exceed their respective mechanism limit thresholds;

[0083] Dual-vehicle posture coordination constraint: The relative posture deviation of the six degrees of freedom of the two vehicles does not exceed the preset safe operating range;

[0084] Workspace constraints: The movement trajectories of the two vehicles and the work load do not exceed the preset workspace boundaries and meet obstacle avoidance constraints, ensuring that the movement envelope of the two vehicles and the load maintains a safe distance from obstacles in the work area;

[0085] System dynamics constraints: The dynamic response of the dual-vehicle and load coupled system shall not exceed the limit threshold of the driving torque and output force of the actuator.

[0086] S5: Based on the rolling time-domain optimization framework, in each control cycle, the optimal expected trajectory of the two vehicles, the cooperative pose adjustment amount and the load disturbance feedforward compensation amount are solved simultaneously with the current dual-vehicle pose state and the full dynamic characteristic parameters of the load as initial conditions, so as to realize the synchronous solution of trajectory optimization, pose adjustment and load compensation.

[0087] A rolling time-domain optimization framework is constructed using the Model Predictive Control (MPC) framework, with the prediction time domain set as... Control time domain is ,in The lengths of the prediction time domain and the control time domain are set according to the system control cycle and the dynamic characteristics of the operation scenario. The shorter the control cycle and the faster the dynamic changes of the operation scenario, the smaller the values ​​of the prediction time domain and the control time domain.

[0088] Within each control cycle, the following rolling optimization solution process is executed:

[0089] Initialize optimization parameters: Use the dual vehicle position and attitude state and load full dynamic characteristic parameters acquired and identified in the current control cycle as the initial conditions for optimization, and use the current system state as the initial state in the prediction time domain.

[0090] Constructing the optimization problem: Taking the above-mentioned collaborative optimization objective function as the objective of the optimization problem, and the real-time hard constraints as the inequality constraints and equality constraints of the optimization problem, a constrained finite-time quadratic programming optimization problem is constructed.

[0091] Synchronous optimization solution: The above quadratic programming problem is solved using the effective set method or interior point method. In the prediction time domain, the expected trajectory sequence, cooperative pose adjustment sequence, and load disturbance feedforward compensation sequence of the two vehicles are obtained simultaneously through optimization. The expected trajectory sequence includes the position, velocity, and acceleration parameters of the two vehicles at each time point in the prediction time domain. The cooperative pose adjustment sequence includes the relative pose deviation correction of the two vehicles and the pose adjustment parameters of the operating actuator. The feedforward compensation sequence includes the compensation for load center of gravity shift, inertia tensor change, and real-time force disturbance, thus realizing the simultaneous solution of trajectory optimization, pose adjustment, and load compensation.

[0092] Control output: The first set of optimized control quantities in the control time domain is taken as the optimal control output for the current control cycle. The remaining optimized sequences are only used for prediction and solution and are not directly output.

[0093] S6: Decompose the optimal control quantity obtained by synchronous solution into the underlying motion control commands of the master vehicle and the slave vehicle, and combine them with closed-loop feedback control to ensure the trajectory tracking accuracy and posture coordination accuracy of the two vehicles, while realizing the full-process balanced control of the working load force.

[0094] The optimal control quantity for the current cycle obtained by rolling optimization is decomposed into the underlying motion control commands of the master vehicle and slave vehicle according to the control authority and actuator configuration of the master vehicle and slave vehicle. Specifically, these commands include the drive / braking commands and steering commands of the dual chassis, as well as the position adjustment commands and force control compensation commands of the on-board work actuators.

[0095] For the decomposed low-level control commands, a closed-loop feedback control strategy is adopted for execution. The closed-loop feedback control can be any of PID control, sliding mode control, or adaptive robust control. This embodiment adopts an adaptive robust control strategy. The specific execution process is as follows: the actual pose, motion state, and load force feedback data of the two vehicles are collected in real time by on-board sensors. The deviation values ​​between the actual state and the optimized expected trajectory, expected pose, and expected force are calculated. Based on the deviation values, the low-level control commands are corrected in real time by the adaptive robust controller to compensate for the influence of model errors, unmodeled dynamics, and external disturbances, so as to ensure the trajectory tracking accuracy and pose coordination accuracy of the two vehicles, and at the same time achieve the full-process balanced control of the working load force.

[0096] S7: The collaborative control optimization is repeatedly executed in each control cycle to complete the closed-loop real-time collaborative optimization control of the entire operation process. In the entire operation process, the collaborative control optimization is repeatedly executed in each control cycle, that is, the system status data acquisition, online identification of the full dynamic characteristics of the load, construction of collaborative optimization problems, rolling time domain optimization solution, decomposition and closed-loop execution of the underlying control instructions are completed simultaneously to form the closed-loop real-time collaborative optimization control of the entire operation process until the operation process is completed.

[0097] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some modifications or alterations to the above-disclosed technical content to create equivalent embodiments without departing from the scope of the present invention. Any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the scope of the present invention.

Claims

1. A trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario, characterized in that, The method includes: S1: Construct the basic dynamic model of the dual-vehicle cooperative operation coupling system, including the six-degree-of-freedom rigid body dynamic model of the dual vehicles, the rigid body dynamic model of the operating load, and the coupling constraint relationship between the dual vehicles and the operating load; S2: Construct a real-time dynamic mapping model of the relative pose of two vehicles with six degrees of freedom and the working load, and establish a real-time mapping relationship between the relative pose of the two vehicles, the working process, the spatial pose and the center of gravity position, inertial tensor and force state of the working load. S3: Based on real-time collected data on the position and posture of the two vehicles and the stress state of the working load, combined with the real-time dynamic mapping model, the center of gravity offset, inertia tensor change and real-time force disturbance of the working load are identified online, and the full dynamic characteristic parameters of the working load are obtained. S4: Construct a load-pose-trajectory coupled collaborative optimization objective function and real-time hard constraints, and embed the acquired full dynamic characteristic parameters of the work load into the objective function and hard constraints, wherein the hard constraints include the work load force balance constraint and the allowable load limit constraint. S5: Based on the rolling time-domain optimization framework, in each control cycle, the optimal expected trajectory of the two vehicles, the cooperative pose adjustment amount and the load disturbance feedforward compensation amount are solved simultaneously with the current dual-vehicle pose state and the full dynamic characteristic parameters of the load as initial conditions, so as to realize the synchronous solution of trajectory optimization, pose adjustment and load compensation. S6: Decompose the optimal control quantity obtained by synchronous solution into the underlying motion control commands of the master vehicle and the slave vehicle, and combine them with closed-loop feedback control to ensure the trajectory tracking accuracy and posture coordination accuracy of the two vehicles, while realizing the full-process balanced control of the working load force. S7: Repeatedly execute collaborative control optimization in each control cycle to complete closed-loop real-time collaborative optimization control of the entire operation process.

2. The trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario according to claim 1, characterized in that, In S1, the specific steps for constructing the basic dynamic model of the dual-vehicle cooperative operation coupling system are as follows: Using the geodetic coordinate system as the reference and the vehicle body local coordinate system as an aid, six-degree-of-freedom rigid body dynamic models of two working vehicles are established respectively. Integrating the inertia, damping and driving characteristics of the chassis running mechanism and the vehicle-mounted working mechanism, dynamic equations including three-axis translation (X / Y / Z) and three-axis rotation (roll / pitch / yaw) are constructed to characterize the dynamic mapping relationship between vehicle posture, motion state and driving torque and external disturbances. By combining the mass, geometric dimensions and mass distribution parameters of the working load, a rigid body dynamics model of the load itself is established. For flexible deformation loads, equivalent stiffness and inertia correction terms are introduced to characterize the relationship between the translational and rotational states of the load and the external forces and moments it is subjected to. By defining the rigid binding relationship between the position and orientation of the dual-vehicle operation execution end and the load support point, as well as the balance relationship between the transmission of force and torque, motion coupling constraints and mechanical coupling constraints between the dual vehicles and the load are established, integrating the dual-vehicle dynamic model and the load dynamic model into a unified coupled system basic dynamic model.

3. The trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario according to claim 1, characterized in that, In S2, the six-degree-of-freedom relative pose vector of the two vehicles is defined based on the geodetic coordinate system. ,in Characterizes the relative translational displacement of the two vehicles along the three axes. These correspond to the relative roll, pitch, and yaw attitude angles of the two vehicles, respectively, combined with the operation progress time. With the global spatial pose and motion state vectors of the two vehicles A real-time dynamic mapping model between the two vehicles and the operating load is constructed by recursive derivation of multibody dynamics. In the formula This is the real-time composite force matrix of the load, including the supporting force, inertial force, and environmental disturbance force. Let be the load inertia tensor matrix that dynamically changes with the relative pose and spatial attitude of the two vehicles. The model is a real-time center of gravity position vector of the load caused by the relative displacement of the support points. It also incorporates the changes in support constraints caused by the relative motion of the two vehicles, the load attitude deflection caused by the attitude adjustment of the actuator during the operation process, and the mass distribution projection correction term caused by the spatial attitude transformation. It fully realizes the real-time quantitative mapping of the relative attitude of the two vehicles, the operation process, the global spatial attitude and the load center of gravity position, inertial tensor, and stress state.

4. The trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario according to claim 1, characterized in that, In step S3, the global pose, velocity, and acceleration status of the two vehicles are collected in real time through the vehicle-mounted GNSS and IMU combined navigation module. The real-time force and torque data of the load support point are obtained based on the multi-dimensional force / torque sensor on the operation execution end. The pose feedback signal of the operation mechanism is collected simultaneously. The collected real-time data is input into the constructed real-time dynamic mapping model of the two vehicles and the load. The recursive least squares algorithm with forgetting factor is used to carry out online identification cycle by control cycle. The current center of gravity offset of the load is obtained by inverse fitting of the dynamic equation. The real-time change of the inertia tensor is calculated iteratively by combining the spatial pose transformation relationship. The deterministic components of the support force and inertial force are separated from the composite force. The real-time force disturbance caused by the environment and motion is extracted. Finally, the full dynamic characteristic parameters covering the load mass distribution, inertia characteristics, and external disturbances are obtained.

5. The trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario according to claim 1, characterized in that, In S4, the collaborative optimization objective function is: ,in, For trajectory tracking error cost term, The cost term for the relative pose deviation between the two vehicles. This refers to the cost of load fluctuations and imbalances. This is an energy consumption cost item for dual-vehicle operation. These are the weighting coefficients for each cost item.

6. The trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario according to claim 1, characterized in that, In S4, the real-time hard constraint conditions include at least: Load constraint: The real-time force value at each load point does not exceed the preset allowable maximum value, and the force imbalance at each load point does not exceed the preset threshold. Dual-vehicle kinematic constraints: The speed, acceleration, and jerk of both vehicles shall not exceed their respective limit thresholds; Dual-vehicle posture coordination constraint: The relative posture deviation of the two vehicles in six degrees of freedom does not exceed the preset safe operating range; Operating space constraints: The movement trajectories of the two vehicles and the operating load do not exceed the preset operating space and meet obstacle avoidance constraints; System dynamics constraint: The dynamic response of the dual-vehicle and load coupled system shall not exceed the output limit of the actuator.

7. The trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario according to claim 1, characterized in that, In S5, the rolling time-domain optimization framework adopts the Model Predictive Control (MPC) framework, and the prediction time domain is set to... Control time domain set to ,in Within each control cycle, the expected trajectory sequence, pose adjustment sequence, and feedforward compensation sequence of the two vehicles are simultaneously optimized in the prediction time domain. The first set of control variables in the control time domain is taken as the optimal control variable output for the current cycle.

8. The trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario according to claim 7, characterized in that, The desired trajectory sequence includes position, velocity, and acceleration parameters at each moment; the dual-vehicle cooperative pose adjustment sequence includes relative pose deviation correction and pose adjustment parameters of the operating actuator; and the feedforward compensation sequence for adapting to load dynamic disturbances includes compensation for load center of gravity shift, inertial tensor change, and force disturbance.

9. The trajectory cooperative optimization control method for a dual-vehicle cooperative operation scenario according to claim 1, characterized in that, In S6, the underlying motion control commands include drive / brake commands, steering commands, and position adjustment commands for the dual vehicles and the work execution mechanism; the closed-loop feedback control adopts one of PID control, sliding mode control, or adaptive robust control, and corrects the control commands in real time based on the deviation between the actual position feedback of the dual vehicles and the desired trajectory.

Images