Vehicle and garage cooperative adaptive docking control method and system

Abstract

The application discloses a vehicle and garage cooperative adaptive docking control method and system, which comprises the following steps: establishing a discrete state space model of vehicle and garage coupling based on a unified state vector and a control input vector of mutual correlation between the vehicle and the garage; combining a state estimation vector of the previous time to predict a state estimation vector of the current time; establishing a penalty function about a control input vector sequence of a future prediction time domain based on a difference between the state estimation vector of the current time and a preset docking target state vector to obtain a cooperative control instruction sequence; converting a first control input vector in the cooperative control instruction sequence into an executable control action of the vehicle and the garage; and repeating the above steps until the vehicle and the garage are docked. The application realizes accurate, efficient and flexible automatic docking by establishing a unified mathematical model describing the motion relationship between the vehicle and the garage and dynamically selecting an optimal cooperative strategy through intelligent decision.

Description

Technical Field

[0001] This invention relates to the field of automobile parking technology, specifically to a method and system for adaptive docking control between vehicles and parking garages. Background Technology

[0002] With the continuous growth of urban car ownership, the parking problem has become increasingly prominent, and mechanical automated parking garages have become an effective solution to the low space utilization rate. Existing technologies mainly fall into two categories: one is vehicle single-sided path planning technology, which uses sensors such as lidar and cameras to achieve autonomous vehicle parking; the other is independent control technology for garage equipment, such as Jiangsu Puteng's "Automatic Dynamic Centering System for Elevator Cars in Automated Parking Garages" and the adaptive centering system for vertical circulation automated parking garages. However, these technologies have significant drawbacks: vehicle path planning only considers its own dynamic constraints, ignoring the real-time movement state of the garage; garage control passively waits for vehicles to be in place, lacking an active coordination mechanism. Statistics show that the parking failure rate in traditional garages due to positional deviations is as high as 15%-20%, the average parking time is extended by 30%-45%, and equipment wear increases by more than 25%. Existing patents such as CN114592729B only solve the car centering problem and do not involve active vehicle control; CN223293476U optimizes and improves the response speed of the device, but does not establish a vehicle-garage joint model. Therefore, existing technologies lack a control framework that can unify the modeling of vehicle status and garage motion status, and achieve two-way information interaction and collaborative optimization. Summary of the Invention

[0003] To address the problems existing in the prior art, this invention provides a vehicle-garage collaborative adaptive docking control method and system. Its core lies in constructing an integrated collaborative control framework, treating the vehicle and garage as a jointly controllable "vehicle-garage collaborative entity." By establishing a unified mathematical model describing their motion relationship, multi-source sensor data is fused and high-precision state estimation is performed. Based on model predictive control theory, control commands for both the vehicle and garage are simultaneously optimized. Finally, through intelligent decision-making, the optimal collaborative strategy is dynamically selected to achieve precise, efficient, and smooth automatic docking, forming a rigorous technical closed loop of "unified modeling - joint perception - collaborative optimization - dynamic execution." This fundamentally solves the problems of low efficiency and insufficient reliability caused by the separation of vehicle and garage control in existing technologies.

[0004] Other features and advantages of this application will become apparent from the following detailed description, or may be learned in part from practice of this application.

[0005] According to a first aspect of this application, a vehicle-garage cooperative adaptive docking control method is provided, comprising: A discrete state-space model of vehicle-garage coupling is established based on the unified state vector and control input vector that are interconnected between the vehicle and the garage. Based on the discrete state-space model and the state estimation vector of the previous time step, predict the state estimation vector of the current time step; Based on the difference between the current state estimation vector and the preset docking target state vector, a penalty function is established for the control input vector sequence in the future prediction time domain. Under the physical control constraints corresponding to the vehicle and the garage respectively, the control input vector sequence in the future prediction time domain corresponding to the minimum penalty function value is used as the cooperative control command sequence. The first control input vector in the coordinated control command sequence is converted into an executable control action for the vehicle and the garage; Repeat the steps of building a discrete state-space model to obtain executable control actions until the vehicle docks with the garage.

[0006] In some embodiments of this application, based on the foregoing scheme, the discrete state-space model of vehicle-garage coupling established based on the unified state vector and control input vector that are interrelated between the vehicle and the garage includes: Define a unified state vector and control input vector that relate the vehicle and the garage. The unified state vector includes key vehicle state variables and key garage state vectors, and the control input vector includes vehicle motion vectors and garage motion vectors. Based on a unified state vector and control input vector, and combined with the tire characteristics of the vehicle, a kinematic model of the vehicle and a kinematic model of the garage are established. The kinematic model of the vehicle and the kinematic model of the garage are coupled in the state space to obtain the relative state error, which includes longitudinal error, lateral error and heading angle error. Based on the relative state error and the unified state vector, a discrete state space model is established.

[0007] In some embodiments of this application, based on the foregoing scheme, predicting the current state estimation vector based on the discrete state-space model and the state estimation vector of the previous time step includes: Using the state estimation vector and error covariance matrix from the previous time step, combined with the discrete state space model, the state estimation vector and error covariance matrix for state prediction are obtained. Based on the pre-established observation model, the state estimation vector and error covariance matrix of the state prediction are converted into observable predicted measurements, and the observable predicted measurements are compared with the actual sensor measurement data to obtain the residuals. Based on the residual and the state estimation vector and error covariance matrix of the state prediction, the state estimation vector at the current time is predicted.

[0008] In some embodiments of this application, based on the foregoing scheme, predicting the state estimation vector at the current moment based on the residual and state prediction state estimation vector and the error covariance matrix includes: The state estimation vector and error covariance matrix of the state prediction are set as the state estimation vector and error covariance matrix of the initial iteration point. The adaptive iterative extended Kalman filter algorithm is used to iterate the state estimation vector and error covariance matrix of the initial iteration point until the state estimation vector is less than the first preset threshold or the maximum number of iterations is reached. The output state estimation vector and error covariance matrix at the termination point are the state estimation vector and error covariance matrix of the current time. During the iteration process, based on the state estimation vector and error covariance matrix of the current iteration point, the Jacobian and Kalman gain of the observation matrix of the current iteration point are obtained. Based on the residual and the Jacobian and Kalman gain of the observation matrix of the current iteration point, the state estimation vector and error covariance matrix of the next iteration point are obtained.

[0009] In some embodiments of this application, based on the foregoing scheme, the following further methods are also included: Before each iteration, the standardized innovation square is obtained. If the standardized innovation square is greater than the second preset threshold, the residual data is incorrect, causing the sensor to malfunction. If a sensor fails, an isolation mechanism is triggered, which includes short-term isolation, sensor-level isolation, and adaptive adjustment.

[0010] In some embodiments of this application, based on the aforementioned scheme, the step of establishing a penalty function for the control input vector sequence in the future prediction time domain based on the difference between the current state estimation vector and the preset docking target state vector, and under the physical control constraints corresponding to the vehicle and the garage respectively, using the control input vector sequence in the future prediction time domain corresponding to the minimum penalty function value as the cooperative control command sequence, includes: Based on the difference between the current state estimation vector and the preset docking target state vector, set the state estimation vector sequence and control input vector sequence in the future prediction time domain; Based on the difference between the state estimation vector sequence in the future prediction time domain and the preset docking target state vector, and the change of the control input vector sequence in the future prediction time domain, a penalty function is established. The penalty function consists of a relative state error penalty term, an energy penalty term for the control input vector, a change penalty term for the control input vector, and a prediction terminal error penalty term in the future prediction time domain. Set constraint functions, which include system dynamic constraints, control input constraints, and state constraints; The penalty function is solved by nonlinear optimization to obtain the control input vector sequence in the future prediction time domain, thereby minimizing the penalty function value.

[0011] In some embodiments of this application, a safe distance penalty term is added to the penalty function based on the aforementioned scheme.

[0012] In some embodiments of this application, based on the foregoing scheme, the step of converting the first control input vector in the cooperative control command sequence into an executable control action for the vehicle and the garage includes: Setting condition A: The docking distance in the x-direction is greater than the weighted value of the final docking accuracy threshold in the x-direction, the docking distance in the y-direction is greater than the weighted value of the final docking accuracy threshold in the x-direction, or the attitude angle is greater than the weighted value of the final docking attitude angle, where the weight is greater than 1; Setting condition B1: Extract the current vehicle speed from the first control input vector, where the current vehicle speed is within a reasonable adjustment range; Condition B2: Extract the vehicle control component and the garage control component from the cooperative control instruction sequence, and obtain the cooperative weight ratio based on the vehicle control component and the garage control component. The cooperative weight ratio is greater than the preset weight threshold. Setting condition B3: The required control quantity of the vehicle has not reached the physical limit; Vehicle Active Tracking Mode: If condition A is met and conditions B1, B2, and B3 are met simultaneously, then it is determined that the vehicle is being adjusted and the vehicle control component is executed. Garage Active Adaptation Mode: If condition A is met but any one of conditions B1, B2, or B3 is not met, then it is determined that the garage needs to be adjusted, and the garage control component is executed.

[0013] According to a second aspect of this application, a vehicle-garage cooperative adaptive docking control system is provided, comprising: The model building module is used to build a discrete state-space model of the vehicle and garage coupling based on the unified state vector and control input vector that are related to each other. The prediction module is used to predict the state estimation vector at the current time based on the discrete state-space model and the state estimation vector at the previous time step. The acquisition module is used to establish a penalty function for the control input vector sequence in the future prediction time domain based on the difference between the current state estimation vector and the preset docking target state vector. Under the physical control constraints corresponding to the vehicle and the garage respectively, the control input vector sequence in the future prediction time domain corresponding to the minimum value of the penalty function is used as the cooperative control command sequence. The conversion module is used to convert the first control input vector in the cooperative control instruction sequence into an executable control action for the vehicle and the garage; The docking module is used to repeatedly build a discrete state-space model to obtain executable control actions until the vehicle docks with the garage.

[0014] According to a third aspect of this application, a computer-readable storage medium is provided that stores a computer program thereon, the computer program including executable instructions that, when executed by a processor, implement the method described above.

[0015] According to a fourth aspect of this application, an electronic device is provided, comprising: One or more processors; A memory for storing executable instructions of the processor, which, when executed by the one or more processors, cause the one or more processors to implement the method described above.

[0016] The beneficial effects of this application are as follows: This application provides a vehicle-garage collaborative adaptive docking control method and system, constructing a complete collaborative control system from perception, decision-making to execution. First, it creates the first unified vehicle-garage joint state-space model capable of describing the dynamics of vehicles and heterogeneous garages, laying the theoretical foundation for collaborative control. Second, it designs a joint state estimator based on adaptive iterative extended Kalman filtering, achieving high-precision and robust state synchronization perception under noise and interference. Third, it innovatively treats vehicle and garage control commands as unified optimization variables, solving them within a model predictive control framework, achieving globally optimal collaborative motion planning. Fourth, it proposes a dual-mode dynamic decision-making mechanism of "vehicle tracking" and "garage adaptation," intelligently switching the dominant role to cope with different working conditions. Finally, it incorporates real-time safety boundary constraints based on relative motion, ensuring absolute safety during the collaborative process. These five points together constitute a closed-loop intelligent collaborative docking solution. This application significantly improves the parking docking success rate compared to traditional technologies and achieves precise and reliable docking. Through collaborative path optimization, it shortens the average docking time and reduces ineffective adjustment actions, greatly improving parking efficiency. The smoother and more precise movement reduces wear on garage mechanical components, effectively extending equipment lifespan. Optimizing the overall motion trajectory saves total system energy. Thanks to joint state estimation and real-time safety monitoring, potential risks can be warned in advance, reducing the rate of collisions and other safety incidents, significantly enhancing system robustness and safety.

[0017] It should be understood that the above general description and the following detailed description are merely exemplary and explanatory, and do not limit this application. Attached Figure Description

[0018] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this application, illustrate exemplary embodiments of the invention and are intended to explain the invention, but do not constitute an undue limitation thereof. In the drawings: Figure 1This is a flowchart of a vehicle-garage cooperative adaptive docking control method according to the present invention; Figure 2 This is a schematic diagram of a vehicle-garage cooperative adaptive docking control system according to the present invention; Figure 3 This is a schematic diagram of an electronic device according to the present invention. Detailed Implementation

[0019] To make the objectives, features, and advantages of this invention more apparent and understandable, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the embodiments described below are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0020] It should be understood that the terms "comprising" and other similar expressions in the specification, claims, and accompanying drawings of this invention are intended to cover a non-exclusive inclusion, such as a process, method, system, or apparatus that includes a series of steps or units and is not limited to the listed steps or units. Furthermore, "first" and "second" are used to distinguish different objects and are not intended to describe a specific order.

[0021] According to the first aspect of this application, Figure 1 As shown, this embodiment provides a vehicle-garage cooperative adaptive docking control method, including: Step S1: Based on the unified state vector and control input vector that are interconnected between the vehicle and the garage, establish a discrete state-space model that couples the vehicle and the garage.

[0022] In some embodiments of this example, the method for establishing the discrete state-space model includes: Define a unified state vector and control input vector that relate the vehicle and the garage. The unified state vector includes key vehicle state variables and key garage state vectors, and the control input vector includes vehicle motion vectors and garage motion vectors. Based on a unified state vector and control input vector, and combined with the tire characteristics of the vehicle, a kinematic model of the vehicle and a kinematic model of the garage are established. The kinematic model of the vehicle and the kinematic model of the garage are coupled in the state space to obtain the relative state error; Based on the relative state error and the unified state vector, a discrete state space model is established.

[0023] In this embodiment, The unified state vector at time t for:

[0024] in, These are key state variables for the vehicle. This represents the key state vector of the garage. , These are the coordinates (in meters) of the rear axle center of the vehicle in the geodetic coordinate system. The vehicle's heading angle (rad); This represents the rear axle center speed of the vehicle (m / s), with forward movement being positive. The steering angle of the vehicle's front wheels (rad); , These are the coordinates (in meters) of the geometric center of the garage in the geodetic coordinate system. The attitude angle of the garage (rotation angle about the vertical axis, rad); Let be the translational velocity (m / s) of the garage along its current orientation.

[0025] The control input vector is:

[0026]

[0027] in, Let the vehicle's motion vector be... The motion vector of the garage. The longitudinal acceleration of the vehicle (m / s²); The steering angular velocity of the vehicle's front wheels (rad / s); Let be the translational acceleration of the garage (m / s²).

[0028] In this embodiment, the vehicle's kinematic model considers tire characteristics to describe its steering and acceleration behavior. A vehicle kinematic model based on the rear axle center is used, and its continuous-time dynamic equations are as follows:

[0029]

[0030]

[0031]

[0032]

[0033] in, This refers to the vehicle's wheelbase.

[0034] In this embodiment, the kinematic model of the garage is simplified to a planar rigid body motion model to describe its translation and rotation. The garage is considered as a rigid body capable of translation and rotation within a horizontal plane. For simplicity, its primary motion is considered to be translation along the guide rails, with rotation used for fine-tuning alignment. Its continuous-time dynamic equations are:

[0035]

[0036]

[0037]

[0038] in, This represents the rotational speed of the garage.

[0039] In this embodiment, by defining relative state errors (such as relative position deviation and relative heading deviation), the two independent subsystem models are coupled in the state space to form a complete discrete state space model of "vehicle-garage cooperative" with clear input-output relationship.

[0040] Define the relative state error and the docking point (e.g., the center of the vehicle's rear axle and the center of the target parking area in the garage), and calculate the formula as follows:

[0041]

[0042]

[0043]

[0044] in, This is the relative state error. For longitudinal error, For lateral error, This represents the heading angle error.

[0045] Construct a coupled discrete state-space model of the "vehicle-garage cooperative" system, combining the state equations of the vehicle and the garage with the relative state error equations to build an augmented system state vector. The discrete state-space model can be represented as:

[0046] in, It is based on , and The derived dynamic equation of relative state error, and Let be the discrete-time state transition functions for the vehicle or the garage, respectively. Describes the control input vector Under its influence, the vehicle changes from the state of the previous moment. Current state The dynamic evolution law, Describes the control input vector Under its influence, the garage changes from the state of the previous moment. Current state The dynamic evolution law. Final model. It is a nonlinear function that describes the state of the control input vector. Under the influence of the previous state, the estimated vector is obtained from the previous state. State estimation vector up to the current time step The evolutionary relationship is defined. The discrete state-space model will serve as the predictive model for subsequent state estimation and predictive control. The discrete state-space model clarifies the mathematical relationship between the current state, control inputs (vehicle steering wheel angle, acceleration, and garage acceleration) and the state at the next moment, providing a complete mathematical model of the vehicle-garage coupled nonlinear system that can be used for prediction and control.

[0047] In this embodiment, by utilizing the established discrete state-space model and fusing multiple sensor data from the vehicle and garage, the internal true state of the "vehicle-garage cooperative" can be estimated in real time and accurately, thus solving the problem that some states cannot be directly measured.

[0048] Step S2: Based on the discrete state-space model and the state estimation vector of the previous time step, predict the state estimation vector of the current time step.

[0049] In this embodiment, the raw sensor data streams from the vehicle (e.g., integrated navigation, wheel speed, inertial measurement unit) and the garage (e.g., encoder, tilt sensor, laser rangefinder), along with the state estimation vector from the previous time step, are used as the state estimator by an improved adaptive iterative extended Kalman filter algorithm. Its workflow is as follows: using the discrete state-space model and the state estimation vector from the previous time step, the state estimation vector for the current time step is predicted.

[0050] In some embodiments of this example, predicting the current state estimation vector based on a pre-established discrete state-space model of the vehicle and garage coupling and the state estimation vector of the previous time step includes: Using the state estimation vector from the previous time step And error covariance matrix Combined with the aforementioned discrete state-space model Obtain the state estimation vector for state prediction. And error covariance matrix ; Based on a pre-established observation model, the state estimation vector and error covariance matrix of the state prediction are transformed into observable predicted measurements. These observable predicted measurements are then compared with actual sensor measurement data to obtain the residuals. ; Based on residuals and the state estimation vector of state prediction And error covariance matrix Predict the state estimation vector at the current moment.

[0051] In this embodiment, the state estimation vector for state prediction is obtained. And error covariance matrix The calculation formula is:

[0052]

[0053] in, It is a discrete state-space model in and Jacobian matrix (state transition matrix) at the location. It is the process noise covariance matrix.

[0054] In this embodiment, the observation model State estimation vector and sensor measurements The relationship between them, for example, if the sensor can measure the relative distance between the vehicle and the center of the garage. and azimuth ,but:

[0055] in, To observe the noise, Let x be the docking distance in the x-direction. The docking distance is in the y-direction.

[0056] Obtain the predicted measurement value: .

[0057] Calculate the innovation / residual: This residual That is, the difference between the actual observation and the predicted observation contains information about the uncertainty of state estimation and observation noise.

[0058] In some embodiments of the example, predicting the state estimation vector at the current moment based on the residual and the state prediction state estimation vector and the error covariance matrix includes: The state estimation vector and error covariance matrix of the state prediction are set as the state estimation vector and error covariance matrix of the initial iteration point. The adaptive iterative extended Kalman filter algorithm is used to iterate the state estimation vector and error covariance matrix of the initial iteration point until the state estimation vector is less than the first preset threshold or the maximum number of iterations is reached. The output state estimation vector and error covariance matrix at the termination point are the state estimation vector and error covariance matrix of the current time. During the iteration process, based on the state estimation vector and error covariance matrix of the current iteration point, the Jacobian and Kalman gain of the observation matrix of the current iteration point are obtained. Based on the residual and the Jacobian and Kalman gain of the observation matrix of the current iteration point, the state estimation vector and error covariance matrix of the next iteration point are obtained.

[0059] In this embodiment, the core innovation of the adaptive iterative extended Kalman filter algorithm lies in its adaptive and iterative mechanism. By analyzing the statistical characteristics of the residuals, it automatically adjusts the filtering algorithm's estimation of noise, thus maintaining stable estimation performance even when the sensor signal is contaminated by non-Gaussian noise (such as accidental interference). Through multiple iterative calculations, the state estimation vector at the current moment is continuously corrected until it converges to the optimal solution.

[0060] Specifically, let the maximum number of iterations be... ; initialization: ; In each iteration, the observation matrix Jacobian for the current iteration point is calculated: ; Calculate the new information covariance matrix:

[0061] in, To observe the noise covariance matrix.

[0062] Calculate the Kalman gain:

[0063] State correction: Calculate the new state estimate vector:

[0064] Calculate the new error covariance:

[0065] Iteration terminates: when The iteration will terminate when the number of iterations is less than the preset threshold or when the maximum number of iterations N is reached. Output the final posterior estimate: , .

[0066] In some implementations of this embodiment, the following are also included: Before each iteration, the standardized innovation square is obtained. If the standardized innovation square is greater than the second preset threshold, the residual data is incorrect, causing the sensor to malfunction. If a sensor fails, an isolation mechanism is triggered, which includes short-term isolation, sensor-level isolation, and adaptive adjustment.

[0067] In this way, fault detection logic can identify and isolate untrusted sensor data, ensuring the robustness of state estimation.

[0068] Specifically, the Chi-square test: Before each filter update, the standardized innovation square (NIS) is calculated. Theoretically It follows a chi-square distribution. A second preset threshold is set. (Based on confidence level, such as 95%).

[0069] Judgment: If Then the sensor measurement value A significant discrepancy between the prediction and the model-based forecast may indicate sensor malfunction or unmodeled interference.

[0070] Isolation: When a fault is detected, the isolation mechanism is triggered. Short-term isolation: Temporarily discard the current set of sensor data and use only the predicted values ​​as the state estimate. Or, give the sensor a very large value when updating the state. (This is equivalent to disbelieving the observation.)

[0071] Sensor-level isolation: If a sensor (such as a lidar) triggers a fault alarm multiple times in a row, the system marks it as "untrusted", completely disables its data for several subsequent cycles, and may trigger a degradation strategy (such as using other sensor combinations or a more conservative model).

[0072] Adaptive adjustment: During a fault, the process noise covariance can be adaptively increased. This reflects the increase in model uncertainty.

[0073] This step, through optimal filtering, yields a highly reliable vehicle-garage joint state estimation vector and its uncertainty metric, providing precise initial conditions for the next step of optimized control.

[0074] Step S3: Based on the difference between the current state estimation vector and the preset docking target state vector, establish a penalty function for the control input vector sequence in the future prediction time domain. Under the physical control constraints corresponding to the vehicle and the garage respectively, take the control input vector sequence in the future prediction time domain corresponding to the minimum penalty function value as the cooperative control command sequence.

[0075] In some embodiments of this example, the method for establishing the penalty function is as follows: Based on the difference between the current state estimation vector and the preset docking target state vector, a sequence of state estimation vectors and a sequence of control input vectors are set in the future prediction time domain, wherein the preset docking target state vector is set to the zero-error state. Based on the difference between the state estimation vector sequence in the future prediction time domain and the preset docking target state vector, and the changes in the control input vector sequence in the future prediction time domain, a penalty function is established. This penalty function comprises a relative state error penalty term, an energy penalty term for the control input vector, a change penalty term for the control input vector, and a prediction terminal error penalty term in the future prediction time domain. The calculation formula is as follows:

[0076] in, This is a penalty term for the relative state error in the future prediction time domain, used to drive the system to converge toward the target docking state. This is the first weight matrix. For the first The state estimation vector at time t, This is the preset docking target state vector; To control the energy penalty term of the input vector and avoid excessive acceleration / steering, This is the second weight matrix. For the first The control input vector at each time step; To control the change in the input vector, a penalty term is applied. This makes control smoother. This is the third weight matrix. For the first The control input change vector at each time step, For the first The control input vector at each time step; To predict the terminal error penalty term and ensure stability, This is the fourth weight matrix. For the first The state estimation vector at time t.

[0077] Set constraint functions, which include system dynamic constraints, control input constraints, and state constraints, specifically: System dynamic constraints: ,for , For the first The control input vector at each time step.

[0078] Control input constraints: (such as steering wheel angle and acceleration limits).

[0079] State constraints: These may include velocity constraints, relative position safety boundaries, etc. Based on the current state estimation vector and constraint function, a nonlinear optimization solution is obtained to find the control input sequence in the future prediction time domain, minimizing the penalty function value.

[0080] Specifically, in the solution process, the state estimation vector at the current moment is used. Starting with the initial state, a nonlinear optimization problem is solved for the penalty function, which is a constrained nonlinear programming problem. An online solution is performed using numerical solvers (such as IPOPT and Snopt) based on sequential quadratic programming (SQP) or interior-point methods. The solver iteratively searches for the optimal control sequence U. , making the penalty function Minimum, and satisfies all constraints.

[0081] Take the first control quantity in the coordinated control instruction sequence Used for execution at the current moment, and then re-optimized in the next moment.

[0082] In some embodiments of this example, the penalty item further includes a penalty safety distance, specifically: Define safety boundaries: Define the profiles of the vehicle and garage (typically using multiple circular or rectangular envelopes). Calculate the profiles of the vehicle and garage at every time point in the future prediction time domain.

[0083] Add distance constraints: In the constraints of MPC Problem 1, add a series of inequality constraints to require the minimum distance between the vehicle profile and the garage profile. Always greater than the safety threshold : ,for .

[0084] Relaxation: To avoid optimization failure due to lack of a solution, this safety constraint can be set as a soft constraint, that is, a penalty term is added to the penalty function. ,in It is a very large penalty weight. When the safe distance is violated, the penalty function will increase sharply, and the optimizer will prioritize satisfying the safe constraints.

[0085] This safety constraint, together with the penalty function, constitutes the updated penalty function, which is a more complex constraint optimization problem, solved by the same solver. This ensures that the solved optimal control sequence naturally satisfies the safety requirements.

[0086] To achieve smooth control, the vehicle's steering wheel angle and acceleration commands, along with the garage's translational acceleration commands, are used as decision variables to be optimized. Solving this constrained optimization problem involves finding a set of future control sequences that minimizes the penalty function while satisfying all actuator physical limits. Since the model is known, this problem can be transformed into a standard numerical optimization problem for efficient online solution.

[0087] In the near future, the goal is to develop a sequence of vehicle and garage control commands that will optimize docking performance.

[0088] Step S4: Convert the first control input vector in the cooperative control command sequence into an executable control action for the vehicle and the garage.

[0089] In some embodiments of this example, this step transforms the calculated theoretically optimal control sequence into a practically executable control action capable of responding to emergencies, and a mode of intelligent decision-making-led collaboration.

[0090] Specifically, this is based on the optimal cooperative control sequence obtained from the solution, the state estimation vector at the current moment, and the actual state feedback of the underlying actuator.

[0091] Processing procedure: First, a dynamic collaborative strategy decision is made. The magnitude of the current relative state error, control sequence characteristics, and actuator states are analyzed. If the error is large and the vehicle has sufficient control capability, Judgment logic and quantization threshold: The decision-making logic is based on the current state and the optimization results. A threshold for the desired final docking accuracy is set.

[0092] To determine if the error is "relatively large": calculate the magnitude of the current relative state error or the absolute value of each component, and compare it with the threshold.

[0093] Condition A: The docking distance in the x-direction is greater than the weighted value of the final docking accuracy threshold in the x-direction, the docking distance in the y-direction is greater than the weighted value of the final docking accuracy threshold in the x-direction, or the attitude angle is greater than the weighted value of the final docking attitude angle, where the weight is greater than 1.

[0094] Determine if "the vehicle has sufficient control": Analyze the MPC optimization results from the previous step and the current state of the vehicle.

[0095] Condition B1 (Control Potential): Current vehicle speed Within a reasonable adjustment range (e.g., 0.1m / s to 1.5m / s), and without reaching its minimum turning radius.

[0096] Condition B2 (Optimization outcome tendency): The optimal control sequence obtained from MPC. In the process, vehicle control components are extracted ( , ) and garage control components ( Calculate a collaborative weight ratio. .if A value greater than a threshold (such as 0.7) indicates that the optimal solution itself is more inclined to allow the vehicle to actively adjust.

[0097] Condition B3 (Control quantity not saturated): The vehicle's required control quantity has not reached its physical limit (e.g., the steering wheel angle is not fully turned, or the acceleration has not reached its peak value).

[0098] Decision-making rules: Vehicle Active Tracking Mode: If condition A is met and conditions B1, B2, and B3 are also met, it is determined that the vehicle has sufficient capability to make major adjustments. In this mode, the garage acceleration command is set to zero or a very small value, and the vehicle control command is executed.

[0099] Garage Active Adaptation Mode: If condition A is met but any of conditions B1, B2, or B3 are not, the system is deemed to have difficulty adjusting the vehicle or to be inefficient, and this mode is adopted. The system significantly attenuates the vehicle acceleration and steering commands given by the MPC, and mainly executes the garage control commands, allowing the garage to move quickly to below the next expected docking point on the vehicle's predicted path.

[0100] The system employs a "vehicle active tracking" mode, primarily executing vehicle control commands in the sequence to keep the garage in a ready state. If the vehicle adjustment approaches its limit or the error is mainly caused by the garage not arriving in time, it switches to a "garage active adaptation" mode, primarily executing garage control commands in the sequence to move the garage to "meet" the vehicle. The two modes can be smoothly switched. Next, command smoothing and distribution are performed. The first control variable in the optimized sequence is interpolated and smoothed to generate smooth commands that conform to the actuator communication cycle, which are then sent to the vehicle drive-by-wire system and the garage motion controller. Finally, full-process safety monitoring and robust fault tolerance are implemented. The uncertainty of the state estimate is monitored in real time; if it increases abnormally (indicating potential sensor failure), the system automatically degrades to a safety control mode based on a conservative model. Simultaneously, the relative speed between the vehicle and the garage is continuously monitored; if it exceeds a safety threshold, a coordinated emergency braking procedure involving both is immediately triggered, and feedback compensation is superimposed on the control commands to suppress model errors and external disturbances.

[0101] Outputs: final steering and drive / brake commands sent to the vehicle drive-by-wire system, final garage movement commands sent to the garage controller, and real-time health status indicators of the system.

[0102] Step S5: Repeat the steps of establishing the discrete state-space model to obtain the executable control action until the vehicle is docked with the garage.

[0103] According to a second aspect of this application, this embodiment provides a vehicle-garage cooperative adaptive docking control system, comprising: The model building module is used to build a discrete state-space model of the vehicle and garage coupling based on the unified state vector and control input vector that are related to each other. The prediction module is used to predict the state estimation vector at the current time based on the discrete state-space model and the state estimation vector at the previous time step. The acquisition module is used to establish a penalty function for the control input vector sequence in the future prediction time domain based on the difference between the current state estimation vector and the preset docking target state vector. Under the physical control constraints corresponding to the vehicle and the garage respectively, the control input vector sequence in the future prediction time domain corresponding to the minimum value of the penalty function is used as the cooperative control command sequence. The conversion module is used to convert the first control input vector in the cooperative control instruction sequence into an executable control action for the vehicle and the garage; The docking module is used to repeatedly build a discrete state-space model to obtain executable control actions until the vehicle docks with the garage.

[0104] Specifically, this embodiment corresponds one-to-one with the above method embodiments. The functions of each module have been described in detail in the corresponding method embodiments, so they will not be repeated here.

[0105] According to a third aspect of this application, this embodiment provides a computer-readable storage medium having a computer program stored thereon, the computer program including executable instructions that, when executed by a processor, implement the method described above.

[0106] The present invention can implement all or part of the processes in the above methods, or it can be accomplished by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or system capable of carrying computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, the computer-readable medium does not include electrical carrier signals and telecommunication signals.

[0107] According to a fourth aspect of this application, an electronic device is provided, comprising: One or more processors; Memory is used to store executable instructions for the processor, which, when executed by one or more processors, cause one or more processors to implement the methods described above.

[0108] Electronic devices are manifested in the form of general-purpose computing devices. Components of an electronic device may include, but are not limited to: at least one processor, at least one memory, and a bus connecting different system components (including memory and processor).

[0109] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. The processor is the control center of a computer system, connecting all parts of the computer system through various interfaces and lines.

[0110] Memory can be used to store computer programs and / or modules. The processor implements various functions of the computer system by running or executing the computer programs and / or modules stored in the memory, and by accessing data stored in the memory. Memory can mainly include a program storage area and a data storage area. The program storage area can store the operating system and at least one application program required for a function (e.g., sound playback, image playback, etc.); the data storage area can store data created based on the use of the mobile phone (e.g., audio data, video data, etc.). Furthermore, memory can include high-speed random access memory, and can also include non-volatile memory, such as hard disks, RAM, plug-in hard disks, SmartMedia Cards (SMC), Secure Digital (SD) cards, Flash Cards, at least one disk storage device, flash memory device, or other volatile solid-state storage devices.

[0111] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, servers, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage and memory) containing computer-usable program code.

[0112] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), servers, and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A system that specifies functions in one or more boxes.

[0113] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including an instruction set implemented in a process. Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0114] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0115] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0116] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

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

Claims

1. A vehicle-garage cooperative adaptive docking control method, characterized in that, include: A discrete state-space model of vehicle-garage coupling is established based on the unified state vector and control input vector that are interconnected between the vehicle and the garage. Based on the discrete state-space model and the state estimation vector of the previous time step, predict the state estimation vector of the current time step; Based on the difference between the current state estimation vector and the preset docking target state vector, a penalty function is established for the control input vector sequence in the future prediction time domain. Under the physical control constraints corresponding to the vehicle and the garage respectively, the control input vector sequence in the future prediction time domain corresponding to the minimum penalty function value is used as the cooperative control command sequence. The first control input vector in the coordinated control command sequence is converted into an executable control action for the vehicle and the garage; Repeat the steps of building a discrete state-space model to obtain executable control actions until the vehicle docks with the garage.

2. The method according to claim 1, characterized in that, The discrete state-space model of vehicle-garage coupling, established based on the unified state vector and control input vector that are interrelated between the vehicle and the garage, includes: Define a unified state vector and control input vector that relate the vehicle and the garage. The unified state vector includes key vehicle state variables and key garage state vectors, and the control input vector includes vehicle motion vectors and garage motion vectors. Based on a unified state vector and control input vector, and combined with the tire characteristics of the vehicle, a kinematic model of the vehicle and a kinematic model of the garage are established. The kinematic model of the vehicle and the kinematic model of the garage are coupled in the state space to obtain the relative state error, which includes longitudinal error, lateral error and heading angle error. Based on the relative state error and the unified state vector, a discrete state space model is established.

3. The method according to claim 1, characterized in that, The prediction of the current state estimate vector based on the discrete state-space model and the state estimate vector of the previous time step includes: Using the state estimation vector and error covariance matrix from the previous time step, combined with the discrete state space model, the state estimation vector and error covariance matrix for state prediction are obtained. Based on the pre-established observation model, the state estimation vector and error covariance matrix of the state prediction are converted into observable predicted measurements, and the observable predicted measurements are compared with the actual sensor measurement data to obtain the residuals. Based on the residual and the state estimation vector and error covariance matrix of the state prediction, the state estimation vector at the current time is predicted.

4. The method according to claim 3, characterized in that, The prediction of the state estimation vector at the current moment based on the residual and state prediction state estimation vector and the error covariance matrix includes: The state estimation vector and error covariance matrix of the state prediction are set as the state estimation vector and error covariance matrix of the initial iteration point. The adaptive iterative extended Kalman filter algorithm is used to iterate the state estimation vector and error covariance matrix of the initial iteration point until the state estimation vector is less than the first preset threshold or the maximum number of iterations is reached. The output state estimation vector and error covariance matrix at the termination point are the state estimation vector and error covariance matrix of the current time. During the iteration process, based on the state estimation vector and error covariance matrix of the current iteration point, the Jacobian and Kalman gain of the observation matrix of the current iteration point are obtained. Based on the residual and the Jacobian and Kalman gain of the observation matrix of the current iteration point, the state estimation vector and error covariance matrix of the next iteration point are obtained.

5. The method according to claim 4, characterized in that, Also includes: Before each iteration, the standardized innovation square is obtained. If the standardized innovation square is greater than the second preset threshold, the residual data is incorrect, causing the sensor to malfunction. If a sensor fails, an isolation mechanism is triggered, which includes short-term isolation, sensor-level isolation, and adaptive adjustment.

6. The method according to claim 1, characterized in that, The method establishes a penalty function for the control input vector sequence in the future prediction time domain based on the difference between the current state estimation vector and the preset docking target state vector. Under the physical control constraints corresponding to the vehicle and the garage respectively, the control input vector sequence in the future prediction time domain corresponding to the minimum penalty function value is used as the cooperative control command sequence, including: Based on the difference between the current state estimation vector and the preset docking target state vector, set the state estimation vector sequence and control input vector sequence in the future prediction time domain; Based on the difference between the state estimation vector sequence in the future prediction time domain and the preset docking target state vector, and the change of the control input vector sequence in the future prediction time domain, a penalty function is established. The penalty function consists of a relative state error penalty term, an energy penalty term for the control input vector, a change penalty term for the control input vector, and a prediction terminal error penalty term in the future prediction time domain. Set constraint functions, which include system dynamic constraints, control input constraints, and state constraints; The penalty function is solved by nonlinear optimization to obtain the control input vector sequence in the future prediction time domain, thereby minimizing the penalty function value.

7. The method according to claim 1, characterized in that: Add a safety distance penalty term to the penalty function.

8. The method according to claim 1, characterized in that, The step of converting the first control input vector in the coordinated control command sequence into executable control actions for the vehicle and the garage includes: Setting condition A: The docking distance in the x-direction is greater than the weighted value of the final docking accuracy threshold in the x-direction, the docking distance in the y-direction is greater than the weighted value of the final docking accuracy threshold in the x-direction, or the attitude angle is greater than the weighted value of the final docking attitude angle, where the weight is greater than 1; Setting condition B1: Extract the current vehicle speed from the first control input vector, where the current vehicle speed is within a reasonable adjustment range; Condition B2: Extract the vehicle control component and the garage control component from the cooperative control instruction sequence, and obtain the cooperative weight ratio based on the vehicle control component and the garage control component. The cooperative weight ratio is greater than the preset weight threshold. Setting condition B3: The required control quantity of the vehicle has not reached the physical limit; Vehicle Active Tracking Mode: If condition A is met and conditions B1, B2, and B3 are met simultaneously, then it is determined that the vehicle is being adjusted and the vehicle control component is executed. Garage Active Adaptation Mode: If condition A is met but any one of conditions B1, B2, or B3 is not met, then it is determined that the garage needs to be adjusted, and the garage control component is executed.

9. A vehicle-garage cooperative adaptive docking control system, characterized in that, include: The model building module is used to build a discrete state-space model of the vehicle and garage coupling based on the unified state vector and control input vector that are related to each other. The prediction module is used to predict the state estimation vector at the current time based on the discrete state-space model and the state estimation vector at the previous time step. The acquisition module is used to establish a penalty function for the control input vector sequence in the future prediction time domain based on the difference between the current state estimation vector and the preset docking target state vector. Under the physical control constraints corresponding to the vehicle and the garage respectively, the control input vector sequence in the future prediction time domain corresponding to the minimum value of the penalty function is used as the cooperative control command sequence. The conversion module is used to convert the first control input vector in the cooperative control instruction sequence into an executable control action for the vehicle and the garage; The docking module is used to repeatedly build a discrete state-space model to obtain executable control actions until the vehicle docks with the garage.

10. An electronic device, characterized in that, include: One or more processors; A memory for storing executable instructions of the processor, which, when executed by the one or more processors, cause the one or more processors to perform the method according to any one of claims 1-7.

Images