Chassis prediction control system and method based on binocular vision and multi-domain cooperation

Abstract

The invention relates to the technical field of vehicle electric control suspensions, in particular to a chassis prediction control system and method based on binocular vision and multi-domain cooperation, and constructs the chassis prediction control system based on binocular vision and multi-domain cooperation based on an ADAS binocular camera. IMU data of a binocular camera is obtained by using a data acquisition module of the chassis prediction control system, visual observation data of a vehicle body is obtained by using a binocular visual processing module, and then the IMU data and the visual observation data are fused and converted into a three-dimensional image by using modules such as an extended Kalman filtering fusion module and a motion conversion module. The sprung mass vertical acceleration and sprung mass vertical speed of the vehicle four-corner suspension provide accurate suspension four-corner vertical motion state input for an electric control suspension controller, and the accuracy and real-time performance of the electric control suspension for vehicle body posture control are guaranteed. Meanwhile, the problems that a special IMU is difficult to arrange, high in cost and insufficient in precision and IMU resources of an ADAS system are wasted are solved.

Description

Technical Field

[0001] This invention relates to the field of vehicle electronic suspension technology, specifically to a chassis predictive control system and method based on binocular vision and multi-domain collaboration. Background Technology

[0002] Skyhook control theory is widely used in semi-active suspension systems (such as CDC continuously variable damping suspension) and active suspension systems (such as air suspension), utilizing the vertical acceleration α of the sprung mass. z and the vertical velocity v of the sprung mass z Key parameters such as these are used to accurately sense the motion state of the vehicle's four-corner suspension, so as to adjust the damping or stiffness parameters of the suspension in real time and achieve precise control of the vehicle's attitude.

[0003] To obtain the vertical acceleration 'a' of the sprung mass z Vertical velocity v z For key parameters such as these, those skilled in the art typically add a dedicated 6-axis IMU (Inertial Measurement Unit) to the vehicle floor, or four acceleration sensors at the connection between the suspension and the vehicle body. These dedicated IMUs or acceleration sensors are used to measure the motion near the vehicle's center of gravity or at local connection points, and then, combined with a rigid body kinematics model, the dynamic parameters of the suspension at the four corners are calculated. Although this solution based on dedicated IMUs or acceleration sensors has been mass-produced, those skilled in the art have found that this approach has many shortcomings in practical use, such as:

[0004] ① Difficult installation and layout: In order to avoid mechanical interference, the installation positions of the dedicated IMU and acceleration sensor must strictly avoid internal mechanical components, pipelines and other structures of the vehicle body, which makes the integration of the sensor with the whole vehicle difficult and greatly increases the complexity and implementation cost of the whole vehicle engineering integration.

[0005] ② Increased cost and weight: The additional sensors require supporting components such as independent wiring harnesses and dedicated mounting brackets, which will not only directly increase the total cost of system hardware, but also increase the weight of the vehicle, thus restricting the optimization of vehicle energy consumption and hindering the achievement of vehicle energy efficiency targets.

[0006] ③Limited measurement accuracy: Single-point measurement using a dedicated IMU or local measurement using an accelerometer cannot fully reflect the motion characteristics of the vehicle's four-corner suspension under complex dynamic conditions. This can easily introduce accumulated errors during the state estimation process of the electronic suspension controller, resulting in low accuracy of the final calculated state estimate. This can lead to deviations in vehicle dynamic control-related decision-making and induce traffic safety accidents such as rollovers and rear-end collisions.

[0007] Therefore, how to accurately obtain the vertical motion state of the vehicle's four corner suspensions and provide precise input to the electronic suspension controller has always been a problem that needs to be solved by those skilled in the art. Summary of the Invention

[0008] The purpose of this invention is to address the shortcomings of existing technologies by providing a chassis predictive control system and method based on binocular vision and multi-domain collaboration. This system utilizes an ADAS binocular camera to construct a chassis predictive control system based on binocular vision and multi-domain collaboration. The system's data acquisition module acquires IMU data from the binocular camera, while the binocular vision processing module acquires visual observation data of the vehicle body. Furthermore, modules such as an extended Kalman filter fusion module, motion conversion module, and time delay compensation module are used to fuse the IMU data and visual observation data, converting them into the vertical acceleration and vertical velocity of the sprung mass at the four corners of the vehicle's suspension. This provides precise vertical motion state input for the electronic suspension controller at the four corners of the suspension, helping to ensure the accuracy and real-time performance of the electronic suspension's control over the vehicle's attitude, thereby improving vehicle ride comfort and driving stability.

[0009] The objective of this invention is achieved through the following approach:

[0010] A chassis predictive control system based on binocular vision and multi-domain collaboration includes a data acquisition module, a binocular vision processing module, an extended Kalman filter fusion module, a motion conversion module, a time delay compensation module, and an output module.

[0011] The data acquisition module is used to acquire road surface images captured by the binocular camera, as well as measurement data from the binocular camera's built-in IMU. The measurement data from the binocular camera's built-in IMU includes the binocular camera's three-axis acceleration measurement values ​​and three-axis angular velocity measurement values, and to acquire the vehicle speed from the vehicle bus signal.

[0012] The binocular vision processing module is used to extract, match, and reconstruct 3D features from road images, perform planar fitting on the ground, and calculate the binocular vision attitude data, including the vehicle's pitch and roll angles.

[0013] The extended Kalman filter fusion module is used to estimate the motion state at the camera mounting point by fusing measurement data from the built-in IMU of the binocular camera and attitude data from binocular vision using an extended Kalman filter.

[0014] The motion state conversion module is used to convert the motion state of the binocular camera into the vertical velocity and vertical acceleration of the four corner suspensions of the vehicle based on the spatial positional relationship between the binocular camera and the four corners of the vehicle suspension, combined with rigid body kinematics. The four corner suspensions of the vehicle include the left front suspension, the right front suspension, the left rear suspension, and the right rear suspension.

[0015] The time delay compensation module is used to calculate the time delay based on the current vehicle speed and wheelbase, and extract the state at the corresponding moment from the historical state based on the time delay to compensate for the time delay of the vertical acceleration of the left rear suspension and the right rear suspension.

[0016] The output module is used to input the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension to the electronic suspension controller.

[0017] The first data output terminal of the data acquisition module is connected to the first data input terminal of the extended Kalman filter fusion module, the second data output terminal of the data acquisition module is connected to the data input terminal of the binocular vision processing module, the data output terminal of the binocular vision processing module is connected to the second data input terminal of the extended Kalman filter fusion module, the data output terminal of the extended Kalman filter fusion module is connected to the data input terminal of the motion state conversion module, the data output terminal of the motion state conversion module is connected to the data input terminal of the time delay compensation module, and the data output terminal of the time delay compensation module is connected to the data input terminal of the data output module.

[0018] A chassis predictive control method based on binocular vision and multi-domain collaboration includes the following steps:

[0019] 1) Based on the binocular camera in the ADAS system, a chassis predictive control system based on binocular vision and multi-domain collaboration is constructed. The chassis predictive control system includes a data acquisition module, a binocular vision processing module, an extended Kalman filter fusion module, a motion conversion module, a time delay compensation module, and an output module.

[0020] 2) Use the data acquisition module to collect road surface images, triaxial acceleration measurements from the binocular camera, triaxial angular velocity measurements from the binocular camera, and vehicle speed;

[0021] 3) Based on the road surface image, the binocular vision processing module is used to perform plane fitting on the road surface to obtain the plane normal vector. The pitch angle and roll angle of the vehicle body are calculated based on the plane normal vector.

[0022] 4) Based on the vehicle's pitch angle, roll angle, three-axis acceleration measurement value of the binocular camera, and three-axis angular velocity measurement value of the binocular camera, the state of the binocular camera is estimated using the extended Kalman filter fusion module to obtain the motion state of the binocular camera.

[0023] 5) Based on the motion state of the binocular camera and combined with rigid body kinematics, the vertical velocity and vertical acceleration of the left front suspension, the vertical velocity and vertical acceleration of the right front suspension, the vertical velocity and vertical acceleration of the left rear suspension, and the vertical velocity and vertical acceleration of the right rear suspension are calculated using the motion conversion module.

[0024] 6) Based on the vehicle speed and wheelbase, use the time delay compensation module to perform time delay compensation on the vertical acceleration of the left and right rear suspensions of the vehicle.

[0025] 7) Using the output module, the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension are output to the electronic suspension controller.

[0026] Preferably, in step 3), the pitch angle and roll angle of the vehicle body are calculated in the following manner:

[0027] 3-1) At the same moment, acquire the left road surface image captured by the left eye camera and the right road surface image captured by the right eye camera in the binocular camera;

[0028] 3-2) Preprocess the left and right road surface images respectively to eliminate image noise and correct distortion;

[0029] 3-3) Extract feature points from the left road surface image to form a left feature point set, extract feature points from the right road surface image to form a right feature point set, and pair each feature point in the left feature point set with each feature point in the right feature point set to form a feature point pair set;

[0030] 3-4) Based on the set of feature points, calculate the three-dimensional coordinates of each feature point pair in the camera coordinate system to form a ground point cloud in the camera coordinate system;

[0031] 3-5) Preprocess the ground point cloud to remove non-ground points and avoid fitting an incorrect ground plane;

[0032] 3-6) Based on the ground point cloud, fit the plane equation of the ground in the camera coordinate system, and transform the plane equation to the vehicle coordinate system to obtain the ground plane equation and plane normal vector in the vehicle coordinate system;

[0033] 3-7) Calculate the pitch angle and roll angle of the vehicle body based on the plane normal vector.

[0034] Preferably, in step 4), the specific process of state estimation using the extended Kalman filter fusion module is as follows:

[0035] 4-1) Define the state vector for the extended Kalman filter;

[0036] 4-2) Based on the three-axis acceleration measurement value and the three-axis angular velocity measurement value of the binocular camera, the state vector is predicted to obtain the state prediction value;

[0037] 4-3) Construct the state transition Jacobian matrix, and calculate the covariance matrix used for predicting the state based on the state transition Jacobian matrix according to the following formula:

[0038] P pred =FP k-1 F T +Q

[0039] In the formula, P pred Let F be the covariance matrix, F be the state transition Jacobian matrix, and P be the covariance matrix. k-1 Let Q be the state covariance matrix at time k-1, Q be the process noise covariance matrix, and T be the matrix transpose.

[0040] 4-4) Combine the pitch angle and roll angle of the vehicle body to calculate the Kalman gain, and use the Kalman gain to correct the state prediction value to obtain the optimal state estimate and covariance matrix.

[0041] Preferably, in step 4-2), the specific method for predicting the state vector is as follows:

[0042] In the formula, p k p is the predicted position of the camera at time k. k-1 v is the optimal estimate of the camera position at time k-1. k-1 Δt represents the optimal estimate of the camera velocity at time k-1, where Δt is the sampling time interval.

[0043] 4-2-2) Calculate the predicted value of the stereo camera speed using the following formula:

[0044] v k =v k-1 +(R(q k-1 (a) imu -b a，k-1 )+g k-1 )Δt

[0045] In the formula, v k Let R(q) be the predicted camera velocity at time k. k-1 ) is the attitude quaternion q at time k-1 k-1 The rotation matrix formed, a imu b is the raw measurement value from the IMU accelerometer. a,k-1 For the optimal estimate of the accelerometer bias at time k-1, g k-1 Δt is the optimal estimate of the gravity vector at time k-1, where Δt is the sampling time interval.

[0046] 4-2-3) Calculate the predicted value of the camera attitude quaternion according to the following formula:

[0047]

[0048] In the formula, The derivative of a quaternion, q k-1 Let k-1 be the attitude quaternion. For quaternion multiplication, ω imu b represents the raw measurement value from the IMU gyroscope. g,k-1 q is the optimal estimate of the gyroscope bias at time k-1. k Let k be the attitude quaternion at time k, Δt be the sampling time interval, and ‖·‖ be the modulus calculation operation;

[0049] 4-2-4) Calculate the gyroscope zero-bias prediction value, the accelerometer zero-bias prediction value, and the gravity vector prediction value according to the following formulas:

[0050] b g,k =b g,k-1

[0051] b a,k =b a,k-1

[0052] g k =g k-1

[0053] In the formula, b g,k Let b be the predicted value of the gyroscope bias at time k. g,k-1 b is the predicted value of the gyroscope bias at time k-1. a,k Let b be the predicted value of the accelerometer bias at time k. a,k-1 The predicted value of the accelerometer bias at time k-1, g k Let g be the predicted value of the gravity vector at time k. k-1 This is the predicted value of the gravity vector at time k-1.

[0054] Preferably, in step 5), the vertical velocity and vertical acceleration of the suspension point are calculated in the following manner:

[0055] 5-1) Based on the installation position of the binocular camera in the vehicle coordinate system and the position of the target suspension in the vehicle coordinate system, calculate the position difference vector:

[0056] Δr i =r i -r cam =[Δx i Δy i Δz i ] T

[0057] r cam =[x cam ,y cam ,z cam ] T

[0058] In the formula, Δr i Let Δx be the vector from the camera mounting point to the suspension point i. i For Δr i The component on the x-axis, Δy i For Δr i The component on the y-axis, Δz i For Δr i The component on the z-axis, r cam r represents the mounting position of the binocular camera in the vehicle body coordinate system. i Let x be the position of the target suspension point i in the vehicle coordinate system. cam For r cam The components on the x-axis, y cam For r cam The component on the y-axis, z cam For r cam The components on the z-axis, T is the matrix transpose;

[0059] 5-2) Based on rigid body kinematics, calculate the vertical velocity at suspension point i using the following formula:

[0060] v z,i =v z,cam +ω x Δy i -ω y Δx i

[0061] In the formula, v z,i Let v be the vertical velocity at suspension point i. z,cam For the vertical velocity of the camera mounting point estimated by the extended Kalman filter, ω x To extend the Kalman filter to estimate the angular velocity of the vehicle body about the X-axis, ω y To extend the Kalman filter to estimate the angular velocity of the vehicle body around the Y-axis, Δx i Let Δy be the x-component of the vector from the camera mounting point to the suspension point i. i Let i be the y-component of the vector from the camera mounting point to the suspension point i.

[0062] 5-3) According to rigid body kinematics, calculate the vertical acceleration at suspension point i using the following formula:

[0063] a z,i =a z,cam +α x Δy i -α y Δx i +ω x (ω y Δz i -ω z Δyi )-ω y (ω x Δz i -ω z Δx i )+ω z (ω x Δy i

[0064] -ω y Δx i )

[0065] In the formula, a z,i Let a be the vertical acceleration at suspension point i. z,cam To estimate the vertical acceleration at the camera mount point using the extended Kalman filter, α x Let α be the angular acceleration of the vehicle body about the X-axis. y Let ω be the angular acceleration of the vehicle body about the Y-axis. z Let ω be the angular velocity of the vehicle body around the Z-axis.

[0066] Preferably, in step 6), time delay compensation is performed on the vertical acceleration of the rear suspension, specifically including:

[0067] 6-1) Calculate the time delay based on the vehicle speed signal and the vehicle's wheelbase using the following formula:

[0068]

[0069] In the formula, τ is the time delay, and v x The vehicle speed signal is represented by L, and the wheelbase is represented by L.

[0070] 6-2) Based on the time delay τ, compensate for the time delay of the vertical acceleration of the left and right rear suspensions according to the following formulas:

[0071] a z,rear (t)=a z,rear，calc (t-τ)

[0072] In the formula, a z,rear (t) represents the vertical acceleration of the rear suspension after time delay compensation at time t, a z,rear,calc (t-τ) is the vertical acceleration value of the rear suspension without time delay compensation, calculated according to step 5).

[0073] Preferably, step 2) further includes a data preprocessing procedure to remove the gravity component from the three-axis acceleration measurements of the binocular camera to obtain the true acceleration at the camera mounting point. The specific process is as follows:

[0074] a cam =a imu -g body

[0075]

[0076] g word = [0, 0, -9.81] T

[0077] In the formula, a cam Let a be the actual acceleration at the camera mounting point. imu The raw measurement value from the IMU accelerometer, g body This represents the gravity vector in the vehicle body coordinate system. Let g be the rotation matrix from the world coordinate system to the vehicle coordinate system. world Let T be the gravity vector in the world coordinate system, and T be the matrix transpose.

[0078] A computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the steps of the aforementioned method.

[0079] The beneficial effects of this invention are as follows:

[0080] A chassis predictive control system based on binocular vision and multi-domain collaboration includes a data acquisition module, a binocular vision processing module, an extended Kalman filter fusion module, a motion conversion module, a time delay compensation module, and an output module.

[0081] The data acquisition module is used to acquire road surface images captured by the binocular camera, as well as measurement data from the binocular camera's built-in IMU. The measurement data from the binocular camera's built-in IMU includes the binocular camera's three-axis acceleration measurement values ​​and three-axis angular velocity measurement values, and to acquire the vehicle speed from the vehicle bus signal.

[0082] The binocular vision processing module is used to extract, match, and reconstruct 3D features from road images, perform planar fitting on the ground, and calculate the binocular vision attitude data, including the vehicle's pitch and roll angles.

[0083] The extended Kalman filter fusion module is used to estimate the motion state at the camera mounting point by fusing measurement data from the built-in IMU of the binocular camera and attitude data from binocular vision using an extended Kalman filter.

[0084] The motion state conversion module is used to convert the motion state of the binocular camera into the vertical velocity and vertical acceleration of the four corner suspensions of the vehicle based on the spatial positional relationship between the binocular camera and the four corners of the vehicle suspension, combined with rigid body kinematics. The four corner suspensions of the vehicle include the left front suspension, the right front suspension, the left rear suspension, and the right rear suspension.

[0085] The time delay compensation module is used to calculate the time delay based on the current vehicle speed and wheelbase, and extract the state at the corresponding moment from the historical state based on the time delay to compensate for the time delay of the vertical acceleration of the left rear suspension and the right rear suspension.

[0086] The output module is used to input the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension to the electronic suspension controller.

[0087] The first data output terminal of the data acquisition module is connected to the first data input terminal of the extended Kalman filter fusion module, the second data output terminal of the data acquisition module is connected to the data input terminal of the binocular vision processing module, the data output terminal of the binocular vision processing module is connected to the second data input terminal of the extended Kalman filter fusion module, the data output terminal of the extended Kalman filter fusion module is connected to the data input terminal of the motion state conversion module, the data output terminal of the motion state conversion module is connected to the data input terminal of the time delay compensation module, and the data output terminal of the time delay compensation module is connected to the data input terminal of the data output module.

[0088] A chassis predictive control method based on binocular vision and multi-domain collaboration includes the following steps:

[0089] 1) Based on the binocular camera in the ADAS system, a chassis predictive control system based on binocular vision and multi-domain collaboration is constructed. The chassis predictive control system includes a data acquisition module, a binocular vision processing module, an extended Kalman filter fusion module, a motion conversion module, a time delay compensation module, and an output module.

[0090] 2) Use the data acquisition module to collect road surface images, triaxial acceleration measurements from the binocular camera, triaxial angular velocity measurements from the binocular camera, and vehicle speed;

[0091] 3) Based on the road surface image, the binocular vision processing module is used to perform plane fitting on the road surface to obtain the plane normal vector. The pitch angle and roll angle of the vehicle body are calculated based on the plane normal vector.

[0092] 4) Based on the vehicle's pitch angle, roll angle, three-axis acceleration measurement value of the binocular camera, and three-axis angular velocity measurement value of the binocular camera, the state of the binocular camera is estimated using the extended Kalman filter fusion module to obtain the motion state of the binocular camera.

[0093] 5) Based on the motion state of the binocular camera and combined with rigid body kinematics, the vertical velocity and vertical acceleration of the left front suspension, the vertical velocity and vertical acceleration of the right front suspension, the vertical velocity and vertical acceleration of the left rear suspension, and the vertical velocity and vertical acceleration of the right rear suspension are calculated using the motion conversion module.

[0094] 6) Based on the vehicle speed and wheelbase, use the time delay compensation module to perform time delay compensation on the vertical acceleration of the left and right rear suspensions of the vehicle.

[0095] 7) Using the output module, the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension are output to the electronic suspension controller.

[0096] This invention utilizes the hardware architecture of an ADAS binocular camera, eliminating the need for dedicated sensors, simplifying layout, and reducing deployment costs. A data acquisition module provides basic data, while a binocular vision processing module extracts attitude features, providing visual reference for state estimation. An extended Kalman filter fusion module integrates visual and IMU data, improving the accuracy of camera motion state estimation. A motion conversion module accurately calculates the four-corner suspension parameters based on a rigid body model, and a time delay compensation module corrects for rear wheel lag, ensuring consistency between front and rear wheel states. Finally, an output module provides reliable input to the electronically controlled suspension controller, achieving end-to-end optimization from data acquisition to control output. This effectively solves the problems of difficult deployment, high cost, and insufficient accuracy associated with traditional solutions, improving the real-time performance and accuracy of suspension control.

[0097] Preferably, in step 4), the specific process of state estimation using the extended Kalman filter fusion module is as follows:

[0098] 4-1) Define the state vector for the extended Kalman filter;

[0099] 4-2) Based on the three-axis acceleration measurement value and the three-axis angular velocity measurement value of the binocular camera (i.e. the measurement value of the IMU built-in accelerometer), the state vector is predicted to obtain the state prediction value;

[0100] 4-3) Construct the state transition Jacobian matrix, and calculate the covariance matrix used for predicting the state based on the state transition Jacobian matrix according to the following formula:

[0101] P pred =FP k-1 F T +Q

[0102] In the formula, P pred Let F be the covariance matrix, F be the state transition Jacobian matrix, and P be the covariance matrix. k-1 Let Q be the state covariance matrix at time k-1, Q be the process noise covariance matrix, and T be the matrix transpose.

[0103] 4-4) Combine the pitch angle and roll angle of the vehicle body (i.e. visual observation data) to calculate the Kalman gain, and use the Kalman gain to correct the state prediction value to obtain the optimal state estimate and covariance matrix.

[0104] While the accelerometers and gyroscopes built into the IMU in a binocular camera have fast response times, they are susceptible to noise and drift, and are prone to accumulating errors when used alone. Pitch and roll angles calculated through binocular vision provide absolute attitude references, but their dynamic response is slow. This invention integrates visual attitude data with IMU measurements using an extended Kalman filter (EKF). Visual attitude data corrects the accumulated drift of the IMU, while high-frequency IMU data compensates for the lag in vision during fast-moving scenes, achieving complementary advantages. The resulting camera motion state (position, velocity, attitude, etc.) is more accurate and reliable, providing high-quality foundational data for subsequent conversion of the vertical motion state of the suspension at the four corners based on rigid body kinematics models, ensuring the accuracy of the state parameters required for suspension control.

[0105] Preferably, in step 6), time delay compensation is performed on the vertical acceleration of the rear suspension, specifically including:

[0106] 6-1) Calculate the time delay based on the vehicle speed signal and the vehicle's wheelbase using the following formula:

[0107]

[0108] In the formula, τ is the time delay, and v x The vehicle speed signal is represented by L, and the wheelbase is represented by L.

[0109] 6-2) Based on the time delay p, compensate for the time delay of the vertical acceleration of the left and right rear suspensions according to the following formulas:

[0110] a z,rear (t)=a z,rear，calc (t-τ)

[0111] In the formula, a z,rear (t) represents the vertical acceleration of the rear suspension after time delay compensation at time t, a z,rear,calc (t-τ) is the vertical acceleration value of the rear suspension without time delay compensation, calculated according to step 5).

[0112] The rear wheel suspension excitation exhibits lag during vehicle operation, affecting control timeliness. This invention calculates the rear wheel time delay based on wheelbase and vehicle speed, extracts corresponding moment data from historical states for compensation, effectively corrects the time deviation of the rear wheel suspension state, ensures consistency in the estimation of the front and rear wheel suspension states, and improves the real-time performance and accuracy of suspension control.

[0113] Preferably, step 2) further includes a data preprocessing procedure to remove the gravity component from the three-axis acceleration measurements of the binocular camera to obtain the true acceleration at the camera mounting point. The specific process is as follows:

[0114] a cam =a imu -g body

[0115]

[0116] g word = [0, 0, -9.81] T

[0117] In the formula, a cam Let a be the actual acceleration at the camera mounting point. imu The raw measurement value from the IMU accelerometer, g body This represents the gravity vector in the vehicle body coordinate system. Let g be the rotation matrix from the world coordinate system to the vehicle coordinate system. world Let T be the gravity vector in the world coordinate system, and T be the matrix transpose.

[0118] Raw IMU measurements contain gravitational interference, which can distort acceleration data and affect the accuracy of subsequent motion state estimation if used directly. This invention introduces a transformation calculation of gravity in the binocular camera coordinate system and combines it with the gravity vector in the world coordinate system to achieve precise separation of gravity components. This allows for the acquisition of the true acceleration of the binocular camera, providing a reliable input for subsequent extended Kalman filter (EKF) fusion of visual and IMU data to estimate the motion state of the binocular camera. It avoids state estimation errors caused by gravitational interference, ensures the accuracy of vertical motion state transformation at the four corners of the suspension, and provides high-quality raw data support for the electronic suspension control system.

[0119] The advantages of this invention are as follows:

[0120] ① Traditional dedicated IMUs must strictly avoid mechanical components of the vehicle body, limiting their installation location and making layout difficult. This invention utilizes the existing installation location of ADAS binocular cameras, eliminating the need for additional sensor installation space planning, avoiding complex mechanical avoidance designs and wiring harness layout adjustments, greatly simplifying the overall vehicle engineering layout process, and improving production and assembly efficiency;

[0121] ② Traditional suspension control systems require additional dedicated IMUs or sensors, as well as wiring harnesses for these IMUs or sensors. This not only increases hardware costs but also adds to the overall vehicle weight. This invention reuses the built-in IMU of the binocular camera, which is already widely used in ADAS systems. This eliminates the need for additional sensors, directly saving the procurement and installation costs of dedicated IMUs and their wiring harnesses. Simultaneously, it reduces the increase in vehicle weight due to additional hardware, thereby lowering the overall system cost and energy consumption.

[0122] ③ Traditional single-point IMU measurements are difficult to accurately reflect the dynamic characteristics of the suspension at the four corners. This invention establishes a data channel between the traditional ADAS system and the chassis control system, fully explores the data value of the IMU in the traditional ADAS system (the IMU data of the traditional ADAS system is only used for visual stabilization, which is a significant waste of resources), and fuses the IMU data with binocular visual attitude information through extended Kalman filtering. Combined with rigid body kinematics, it accurately calculates the vertical velocity and acceleration of the vehicle suspension at the four corners and inputs them into the electronic suspension controller. This achieves cross-domain reuse of hardware resources, greatly improves the accuracy of vehicle state estimation, and makes the decision-making related to vehicle dynamic control more in line with the actual suspension control needs, which can reduce the occurrence of driving safety accidents such as rollovers and rear-end collisions.

[0123] Definitions:

[0124] Canopy control theory is a classic control strategy for vehicle suspension systems. Its core logic is to simulate the mechanical effect of "fixing the upper end of the damper on a 'canopy' (fixed reference point) with no relative motion to the ground". By adjusting the suspension damping force or control force in real time, the vehicle body motion is decoupled from the road excitation, thereby optimizing the suspension performance.

[0125] ORB algorithm: short for Oriented FAST and Rotated BRIEF, is a real-time feature detection and description algorithm used in the field of computer vision.

[0126] Rigid body kinematics is the study of the geometric properties of rigid body motion. It falls under the scope of analytical mechanics and its research objects include translation, rotation about a fixed axis, planar motion, rotation about a fixed point, and general forms of motion. This discipline establishes equations of rigid body motion and analyzes the distribution of instantaneous velocity and acceleration, primarily employing analytical methods (deriving equations) or geometric methods (directly establishing velocity relationships) for research.

[0127] Four-corner suspension: This refers to a vehicle where each of the four wheels is equipped with an independent suspension system. Each wheel is connected to the vehicle body via its own suspension structure (such as springs, shock absorbers, and control arms), rather than multiple wheels sharing the same suspension assembly (such as the solid axle structure of a traditional non-independent suspension). This invention includes the left front suspension, left rear suspension, right front suspension, and right rear suspension of the vehicle. Attached Figure Description

[0128] Figure 1 This is a flowchart of the method of the present invention;

[0129] Figure 2 This is a schematic diagram of the core control algorithm of the system of the present invention;

[0130] Figure 3 This is a schematic diagram of the hardware architecture of the system of the present invention;

[0131] Figure 4 This is a schematic diagram illustrating the simulation effect of the present invention;

[0132] Figure 5 This is a block diagram of the present invention. Detailed Implementation

[0133] like Figures 1 to 5 As shown, a chassis predictive control method based on binocular vision and multi-domain collaboration includes the following steps:

[0134] 1) Based on the binocular camera in the ADAS system, a chassis predictive control system based on binocular vision and multi-domain collaboration is constructed. The chassis predictive control system includes a data acquisition module, a binocular vision processing module, an extended Kalman filter fusion module, a motion conversion module, a time delay compensation module, and an output module.

[0135] 2) Use the data acquisition module to collect road surface images, triaxial acceleration measurements from the binocular camera, triaxial angular velocity measurements from the binocular camera, and vehicle speed;

[0136] 3) Based on the road surface image, the binocular vision processing module is used to perform plane fitting on the road surface to obtain the plane normal vector. The pitch angle and roll angle of the vehicle body are calculated based on the plane normal vector.

[0137] 4) Based on the vehicle's pitch angle, roll angle, three-axis acceleration measurement value of the binocular camera, and three-axis angular velocity measurement value of the binocular camera, the state of the binocular camera is estimated using the extended Kalman filter fusion module to obtain the motion state of the binocular camera.

[0138] 5) Based on the motion state of the binocular camera and combined with rigid body kinematics, the vertical velocity and vertical acceleration of the left front suspension, the vertical velocity and vertical acceleration of the right front suspension, the vertical velocity and vertical acceleration of the left rear suspension, and the vertical velocity and vertical acceleration of the right rear suspension are calculated using the motion conversion module.

[0139] 6) Based on the vehicle speed and wheelbase, use the time delay compensation module to perform time delay compensation on the vertical acceleration of the left and right rear suspensions of the vehicle.

[0140] 7) Using the output module, the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension are output to the electronic suspension controller.

[0141] Based on the above method, the following is an example:

[0142] 1) Based on the binocular camera in the ADAS system, construct a chassis predictive control system based on binocular vision and multi-domain collaboration;

[0143] In this embodiment, the chassis predictive control system includes a data acquisition module, a binocular vision processing module, an extended Kalman filter fusion module, a motion conversion module, a time delay compensation module, and an output module.

[0144] The data acquisition module is used to acquire road images captured by the vehicle-mounted binocular camera, and read measurement data from the binocular camera's built-in IMU, including raw accelerometer measurements (three-axis acceleration measurements) and raw gyroscope measurements (three-axis angular velocity measurements). It also obtains the vehicle speed from the bus signal, providing road image data to the visual attitude estimation module, raw IMU measurement data to the extended Kalman filter fusion module, and vehicle speed signals to the rear wheel time delay compensation module, serving as the basic input for subsequent data processing and state estimation.

[0145] The binocular vision processing module is used to extract, match, and reconstruct 3D features from road images. Combined with ground plane fitting, it calculates the pitch and roll angles of the vehicle body, providing visual attitude data for the extended Kalman filter fusion module.

[0146] The extended Kalman filter fusion module is used to fuse the built-in IMU measurement data and the attitude data of the binocular vision using the extended Kalman filter (EKF) to estimate the motion state at the binocular camera mounting point.

[0147] The motion state conversion module is used to convert the motion state of the binocular camera mounting point into the vertical velocity and vertical acceleration of the four corners of the suspension based on the spatial positional relationship between the binocular camera mounting point and the four corners of the vehicle suspension (left front suspension, right front suspension, left rear suspension, right rear suspension) using a rigid body kinematics model.

[0148] The time delay compensation module is used to calculate the time delay based on the current vehicle speed and wheelbase, and extract the state at the corresponding moment from the historical state based on the time delay to compensate for the time delay of the vertical acceleration of the left rear suspension and the right rear suspension.

[0149] The output module is used to output the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension to the electronic suspension controller.

[0150] The first data output terminal of the data acquisition module is connected to the first data input terminal of the extended Kalman filter fusion module, the second data output terminal of the data acquisition module is connected to the data input terminal of the binocular vision processing module, the data output terminal of the binocular vision processing module is connected to the second data input terminal of the extended Kalman filter fusion module, the data output terminal of the extended Kalman filter fusion module is connected to the data input terminal of the motion state conversion module, the data output terminal of the motion state conversion module is connected to the data input terminal of the time delay compensation module, the data output terminal of the time delay compensation module is connected to the data input terminal of the data output module, and the data output terminal of the output module is connected to the data input terminal of the electronically controlled suspension controller.

[0151] 2) Use the data acquisition module to acquire road surface images and read the three-axis acceleration measurement values, three-axis angular velocity measurement values, and vehicle speed from the binocular camera;

[0152] In this embodiment, the IMU built into the binocular camera measures the three-axis acceleration and angular velocity at the camera mounting point. Since the IMU measures specific force (including gravity), the gravity component needs to be removed; therefore, a data preprocessing process is also involved, specifically including:

[0153] The expression for gravity in the camera coordinate system (fixed to the vehicle body) is as follows:

[0154]

[0155] g word = [0, 0, -9.81] T

[0156] In the formula, g body This represents gravity in the camera coordinate system. The rotation matrix from the world coordinate system to the vehicle coordinate system (determined by the vehicle attitude (pitch angle θ, roll angle φ, yaw angle ψ)) is g. word Let T be the gravity vector in the world coordinate system, and T be the transpose of the vector.

[0157] Therefore, the actual acceleration (excluding gravity) at the camera mounting point is:

[0158] a cam =a imu -g body

[0159] In the formula, a cam a represents the actual acceleration at the mounting point of the binocular camera. imu The raw measurement value from the IMU accelerometer, g body This represents gravity in the camera coordinate system.

[0160] 3) The binocular vision processing module is used to extract, match and reconstruct the road surface image in three dimensions. Combined with ground plane fitting, the pitch angle and roll angle of the vehicle body are calculated. The specific process is as follows:

[0161] 3-1) At the same moment, acquire the left road surface image captured by the left eye camera and the right road surface image captured by the right eye camera in the binocular camera.

[0162] 3-2) Preprocess the left and right road surface images respectively, including distortion correction, grayscale conversion, noise reduction, and contrast enhancement, to eliminate image noise;

[0163] 3-3) Extract feature points from the left road surface image to form a left feature point set, extract feature points from the right road surface image to form a right feature point set, and pair each feature point in the left feature point set with each feature point in the right feature point set to form a feature point pair set;

[0164] 3-4) Based on the set of feature point pairs, calculate the three-dimensional coordinates (X, Y, F, Z) of each feature point pair in the camera coordinate system using triangulation. c Y c Z c This serves as the initial road surface point cloud in the camera coordinate system. The triangulation formula is based on the camera calibration parameters (focal length f, baseline b, principal point (c)). u c v The disparity d between feature point pairs:

[0165]

[0166] Among them, u L and v L Z represents the x-coordinate and y-coordinate of the feature point in the left eye camera image coordinate system, respectively. c X represents the z-axis coordinates of the feature point pair in the camera coordinate system. c Let x and y be the x-axis coordinates of the feature point pair in the camera coordinate system. c Here, f is the y-axis coordinate of the feature point pair in the camera coordinate system, f is the focal length (effective focal length of the camera), b is the baseline length, d is the disparity of the feature point pair, and u is the line length. L v represents the x-coordinate of the feature point in the left eye camera image coordinate system. L c represents the pixel ordinate of the feature point in the left eye camera image coordinate system. u c is the pixel x-coordinate of the principal point in the left eye camera image coordinate system. v The ordinate of the pixel of the principal point in the left eye camera image coordinate system.

[0167] 3-5) Preprocess the initial road point cloud by using height filtering and statistical outlier removal algorithms to remove non-ground points (such as vehicles, obstacles, etc.) and noise points in the point cloud to obtain a clean ground point cloud.

[0168] 3-6) Based on the clean ground point cloud, the plane equation AX of the ground in the camera coordinate system is fitted using the least squares method based on singular value decomposition (SVD). c +BYc c +CZ c +D = 0, and calculate the unit normal vector. Subsequently, the mounting extrinsic parameters (rotation matrix) from the camera to the vehicle body were used. Translation vector Transform the plane normal vector to the vehicle body coordinate system:

[0169]

[0170] In the formula, n body This is the ground plane normal vector in the vehicle coordinate system. Let be a rotation matrix. It is a translation vector.

[0171] Obtain the ground plane normal vector n in the vehicle body coordinate system body =[n x n y n z ] T .

[0172] 3-7) Based on the plane normal vector n body Calculate the vehicle's pitch angle θ and roll angle φ:

[0173]

[0174] In the formula, θ is the pitch angle of the vehicle body, φ is the roll angle of the vehicle body, and n x n is the plane normal vector body The projection component on the X-axis of the vehicle coordinate system, n y n is the plane normal vector body The projection component on the Y-axis of the vehicle coordinate system, n z n is the plane normal vector body The projection component on the Z-axis of the vehicle body coordinate system.

[0175] 4) Based on the pitch angle of the vehicle body, the roll angle of the vehicle body, the three-axis acceleration measurement value of the binocular camera, and the three-axis angular velocity measurement value of the binocular camera, the state estimation is performed using the extended Kalman filter fusion module to obtain the motion state of the binocular camera. The motion state of the binocular camera includes the position, velocity, attitude of the binocular camera in the world coordinate system, as well as the bias and gravity vector of the IMU sensor.

[0176] The specific process of state estimation using the extended Kalman filter fusion module is as follows:

[0177] 4-1) Define the state vector of the extended Kalman filter according to the following formula:

[0178]

[0179] p = [p x ,p y ,p z ] T

[0180] v = [v x ,v y ,v z ] T

[0181] q = [q w ,q x ,q y ,q z ] T

[0182] b g =[b gx ,b gy ,b gz ] T

[0183] b a =[b ax ,b ay ,b az ] T

[0184] g = [g x ,g y ,g z ] T

[0185] In the formula, x is the state vector, p is the position of the camera mounting point in the world coordinate system, and p x Let p be the x-component of p. y Let p be the component of p on the y-axis. z Let p be the z-component of p, and v be the velocity of the camera mounting point in the world coordinate system. xLet v be the x-component of v. y Let v be the y-component of v. z Let v be the component of v on the z-axis, and q be the quaternion representation of the vehicle's attitude. w For the real part, q x For the imaginary part, q y For the imaginary part, q z b is the imaginary part. g For the bias of the IMU gyroscope, b gx For b g The component on the x-axis, b gy For b g The component on the y-axis, b gz For b g The component on the z-axis, b a For the bias of the IMU accelerometer, b ax For b a The component on the x-axis, b ay For b a The component on the y-axis, b az For b a The component along the z-axis, g, is the gravity vector in the world coordinate system. x Let g be the x-component of g. y Let g be the component of g on the y-axis. z Let g be the component of g on the z-axis, and T be the transpose operator;

[0186] 4-2) Based on the IMU measurements (three-axis acceleration measurements and three-axis angular velocity measurements from the stereo camera), the state vector is predicted to obtain the predicted state value, specifically including:

[0187] 4-2-1) Calculate the predicted position of the stereo camera in the world coordinate system using the following formula:

[0188] p k =p k-1 +v k-1 Δt

[0189] In the formula, p k Let p be the predicted position of the camera at time k. k-1 v is the optimal estimate of the camera position at time k-1. k-1 Let Δt be the optimal estimate of the camera velocity at time k, where Δt is the time interval.

[0190] 4-2-2) Calculate the predicted value of the stereo camera velocity in the world coordinate system according to the following formula:

[0191] v k =v k-1 +(R(q k-1 (a)imu -b a，k-1 )+g k-1 )Δt

[0192] In the formula, v k Let R(q) be the predicted camera velocity at time k. k-1 ) is the attitude quaternion q at time k-1 k-1 The rotation matrix formed, a imu b is the raw measurement value from the IMU accelerometer. a,k-1 For the optimal estimate of the accelerometer bias at time k-1, g k-1 This is the optimal estimate of the gravity vector at time k-1;

[0193] Where R(q) is the rotation matrix corresponding to the quaternion:

[0194]

[0195] In the formula, R(q) is the rotation matrix corresponding to the quaternion, and q w q is the real part of the quaternion. x Let q be the imaginary part of the quaternion along the x-axis. y Let q be the imaginary component of the quaternion along the y-axis. z The imaginary component (q) of the quaternion along the z-axis x ,q y ,q z ,q w (For IMU quaternion components).

[0196] 4-2-3) Calculate the predicted pose of the stereo camera in the world coordinate system using the following formula:

[0197]

[0198] In the formula, The derivative of a quaternion. ω represents quaternion multiplication. imu b represents the raw measurement value from the IMU gyroscope. g,k-1 is the optimal estimate of the gyroscope bias at time k-1, and ‖·‖ is the modulus calculation;

[0199] 4-2-4) Calculate the gyroscope zero-bias prediction value, the accelerometer zero-bias prediction value, and the gravity vector prediction value according to the following formulas:

[0200] b g,k =b g,k-1

[0201] b a,k =b a,k-1

[0202] g k=g k-1

[0203] In the formula, b g,k Let b be the predicted value of the gyroscope bias at time k. g,k-1 b is the predicted value of the gyroscope bias at time k-1. a,k Let b be the predicted value of the accelerometer bias at time k. a,k-1 The predicted value of the accelerometer bias at time k-1, g k Let g be the predicted value of the gravity vector at time k. k-1 This is the predicted value of the gravity vector at time k-1.

[0204] 4-3) Construct the state transition Jacobian matrix, and calculate the covariance matrix used for predicting the state based on the state transition Jacobian matrix according to the following formula:

[0205] P pred =FP k-1 F T +Q

[0206] In the formula, P pred Let F be the covariance matrix, F be the state transition Jacobian matrix, and P be the covariance matrix. k-1 Let Q be the state covariance matrix at time k-1, and let Q be the process noise covariance matrix.

[0207] The state transition Jacobian matrix:

[0208]

[0209] In the formula, F is the state transition Jacobian matrix, f is the state transition function, x is the state vector, I3 is the 3×3 identity matrix, Δt is the time interval, and F vq F is the derivative of velocity with respect to attitude. vba F is the zero partial derivative of velocity with respect to acceleration. qq F is the derivative of attitude with respect to attitude. qbg The derivative of attitude with respect to the zero bias of the gyroscope;

[0210] 4-4) Combining visual observation data (pitch angle and roll angle of the vehicle body), calculate the Kalman gain K, and correct the state prediction value to obtain the optimal state estimate x at time k. k With covariance matrix P k The specific process is as follows:

[0211] 4-4-1) Construct the observation model according to the following formula:

[0212]

[0213] In the formula, z is the observation vector, h(x) is the observation function, v is the observation noise, p is the position observation, θ(q) is the attitude Euler angle observation, and v θ For attitude observation noise, v a This is noise from location observations;

[0214] It is worth noting that in the Extended Kalman Filter (EKF) update step, the core function of the observation model is to compare the actual observation values ​​(specifically, the vehicle pitch and roll angles) output by the binocular vision processing module with the predicted observation values ​​derived from the state prediction values ​​through the observation function h(x), thereby generating observation residuals (i.e., the difference between the two). These observation residuals will work in conjunction with the Kalman gain to iteratively correct the state prediction values ​​of the EKF, ultimately improving the accuracy of state estimation.

[0215] 4-4-2) Calculate Euler angles using the following formula:

[0216]

[0217] θ=arcsin(-2(q x q z -q ω q y ))

[0218] In the formula, φ is the tilt angle measured by the camera's built-in IMU, θ is the pitch angle measured by the camera's built-in IMU, and q w q is the real part of the quaternion. x Let q be the imaginary part of the quaternion in the x-direction. y Let q be the imaginary part of the quaternion in the y-direction. z The imaginary part (q) in the z-direction of the quaternion x ,q y ,q z ,q w (For IMU quaternion components).

[0219] 4-4-3) In the update step of the extended Kalman filter, the observation Jacobian matrix H needs to be calculated by incorporating Euler angles. The calculation method of the observation Jacobian matrix H is as follows:

[0220]

[0221] In the formula, H is the observation Jacobian matrix, h is the observation function, x is the state vector, I3 is the 3×3 identity matrix, and H θq Let be the matrix of partial derivatives of Euler angles with respect to quaternions, and its specific form is:

[0222]

[0223] In the formula, φ is the roll angle calculated from the quaternion q, and θ is the roll angle calculated from the quaternion q. w q is the real part of the quaternion. x Let q be the imaginary part of the quaternion in the x-direction. y Let q be the imaginary part of the quaternion in the y-direction. z The imaginary part (q) in the z-direction of the quaternion x ,q t ,q z ,q w (For IMU quaternion components);

[0224] 4-4-4) Calculate the Kalman gain based on the observed Jacobian matrix, the covariance matrix of the predicted state, and the observation noise covariance matrix pre-set according to the characteristics of the base IMU sensor:

[0225] K = P pred H T HP pred H T +R) -1

[0226] In the formula, P pred H is the covariance matrix of the predicted state; H is the observation Jacobian matrix; R is the observation noise covariance matrix.

[0227] 4-4-5) Using Kalman gain, the state prediction is corrected in the following way to obtain the optimal state estimate:

[0228] ① The observation residual is the difference between the actual observed value and the predicted observed value, representing the prediction deviation. In this embodiment, the observation residual Δz is calculated according to the following formula:

[0229] Δz=zh(x pred )

[0230] In the formula, z represents the current actual visual observation value (pitch angle, roll angle), and h(x) represents the actual visual observation value. pred To utilize the state prediction value, x pred Δz represents the predicted observation value calculated using the observation model, and Δz represents the observation residual.

[0231] ② Using the Kalman gain K, calculate the state correction Δx (i.e., the correction by which the Kalman gain K maps the observation residuals to the state space) according to the following formula:

[0232] Δx=KΔz

[0233] In the formula, K is the Kalman gain, Δz is the observation residual, and Δx is the state correction.

[0234] ③ Apply the state correction to the state estimate according to the following formula to update the state estimate x. k The optimal state estimate at time k is obtained:

[0235] x k =x pred +Δx

[0236] In the formula, x k x is the updated state estimate. pred The state estimate before the update is given, and Δx is the state correction.

[0237] ④ Update the covariance matrix P according to the following formula. k Used to quantify and correct the uncertainty of state estimation:

[0238] P k =(I-KH)P pred

[0239] In the formula, P k Let I be the covariance matrix, K be the identity matrix, and P be the Kalman gain. pred Let H be the covariance matrix of the predicted state, and let H be the observation Jacobian matrix.

[0240] It is worth noting that relying solely on the IMU for state prediction without correction will lead to accumulated errors in the state estimate over time due to IMU noise, drift, and integration errors, resulting in distorted motion states. For example, attitude angle errors may diverge over time, affecting the accuracy of suspension control.

[0241] 5) Based on the motion state of the binocular cameras, the motion conversion module is used to calculate the vertical velocity and acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension, specifically including:

[0242] The position of the stereo camera mounting point in the vehicle body coordinate system is defined as follows:

[0243] r cam =[x cam y cam , z cam ] T

[0244] In the formula, r cam Let x be the position of the binocular camera mounting point in the vehicle body coordinate system. cam Let x and y coordinates be the camera mounting positions. cam Let the camera mounting position be y-coordinate, z-coordinate cam The z-coordinate of the camera mounting position.

[0245] Define the position of vehicle suspension point i (e.g., front left) in the vehicle body coordinate system as follows:

[0246] r i =[x i y i , z i ] T

[0247] In the formula, r i Let x be the position of suspension point i in the vehicle coordinate system. i Let x be the x-coordinate of the suspension point in the vehicle coordinate system, and y be the y-coordinate of the suspension i Let z be the y-coordinate of the suspension point in the vehicle coordinate system. i Let z be the z-coordinate of the suspension point in the vehicle coordinate system, and T be the matrix transpose operator;

[0248] The vertical velocity and vertical acceleration of the vehicle's left front, right front, left rear, and right rear suspensions are calculated as follows:

[0249] 5-1) Based on the position of the binocular camera in the vehicle coordinate system, and the positions of the left front suspension, right front suspension, left rear suspension, and right rear suspension in the vehicle coordinate system, calculate the vector from the binocular camera to each suspension in the vehicle coordinate system according to the following formulas.

[0250] Δr i =r i -r cam =[Δx i Δy i Δz i ] T

[0251] r cam =[x cam ,y cam ,z cam ] T

[0252] In the formula, Δr i Let Δx be the vector from the camera mounting point to the suspension point i. i For Δr i The component on the x-axis, Δy i For Δr i The component on the y-axis, Δz i For Δr i The component on the z-axis, r cam r represents the mounting position of the binocular camera in the vehicle body coordinate system. i Let x be the position of the target suspension point i in the vehicle coordinate system. cam For r cam The components on the x-axis, y cam For r camThe component on the y-axis, z cam For r cam Components on the z-axis;

[0253] 5-2) According to rigid body kinematics, calculate the velocity of suspension point i (left front suspension, right front suspension, left rear suspension, right rear suspension) according to the following formulas;

[0254] v i =v cam +ωΔr i

[0255] ω=[ω x ω y ω z ] T

[0256] In the formula, v z,i Let v be the vertical velocity at suspension point i. z,cam For the vertical velocity of the camera mounting point estimated by the extended Kalman filter, ω x To extend the Kalman filter estimate of the vehicle's angular velocity around the X-axis (tilt), ω y To extend the Kalman filter estimate of the vehicle's angular velocity about the Y-axis (pitch), Δx i Let Δy be the x-component of the vector from the camera mounting point to the suspension point i. i Let i be the y-component of the vector from the camera mounting point to the suspension point i.

[0257] It is worth noting that ω=[ω x ω y ω z ] T It is the angular velocity of the vehicle body, a gyroscope measurement, which has been compensated for the offset and transformed into the vehicle body coordinate system.

[0258] Vertical velocities (z-direction component) of the left front suspension, right front suspension, left rear suspension, and right rear suspension:

[0259] v z,i =v z,cam +ω x Δy i -ω y Δx i

[0260] In the formula, v z,i Let v be the vertical velocity at suspension point i. z,cam For the vertical velocity of the camera mounting point estimated by the extended Kalman filter, ω x ,ω y To extend the Kalman filter to estimate the angular velocities of the vehicle body around the X-axis (roll) and Y-axis (pitch);

[0261] 5-3) The acceleration at suspension point i consists of the acceleration at the camera mounting point, the tangential acceleration (caused by angular acceleration), and the centripetal acceleration (caused by angular velocity). Therefore, the vertical accelerations of the left front suspension, right front suspension, left rear suspension, and right rear suspension are calculated according to the following formulas:

[0262] a i =a cam +α×Δr i +ω×(ω×Δr i )

[0263] α=[α x α y α z ] T

[0264] In the formula, a i Let a be the vertical acceleration at each suspension-to-body connection point. cam Let α be the vertical acceleration at the camera mounting point, α be the angular acceleration (obtained by differentiating the gyroscope data), and Δr be the vertical acceleration. i Let ω be the vector from the binocular camera mounting point to suspension point i, ω be the angular velocity of the vehicle body, and α be the vector. x Let α be the x-component of angular acceleration. y Let α be the y-component of angular acceleration. z This represents the z-direction component of angular acceleration.

[0265] After unfolding, the vertical acceleration (z-component) is:

[0266] a z,i =a z,cam +α x Δy i -α y Δx i +ω x (ω y Δz i -ω z Δy i )-ω y (ω x Δz i -ω z Δx i )+ω z (ω x Δy i

[0267] -ω y Δx i )

[0268] In the formula, a z,i Let a be the vertical acceleration at suspension point i. z,camTo estimate the vertical acceleration at the camera mount point using the extended Kalman filter, α x The angular acceleration of the vehicle body about the X-axis (via angular velocity ω) x (obtained by numerical differentiation), α y The angular acceleration of the vehicle body about the Y-axis (via angular velocity ω) y (obtained by numerical differentiation), ω z Let ω be the angular velocity of the vehicle body around the Z-axis.

[0269] 6) Because the vehicle is in motion, the rear wheels lag behind the front wheels in passing the same point on the road surface. Therefore, the excitation received by the rear wheel suspension points has a time delay relative to the front wheels, and the magnitude of this time delay depends on the wheelbase L and the vehicle speed v. x Therefore, based on the vehicle speed and wheelbase, a time delay compensation module is used to compensate for the vertical acceleration of the left and right rear suspensions. Specifically, this includes:

[0270] 6-1) Calculate the time delay according to the vehicle speed and wheelbase using the following formula:

[0271]

[0272] In the formula, τ i For time delay, L is the vehicle's wheelbase, v x The speed of the vehicle;

[0273] 6-2) Based on the time delay, compensate for the time delay of the vertical acceleration of the left and right rear suspensions of the vehicle according to the following formulas:

[0274] a z,rear (t)=a z,rear，calc (t-τ i )

[0275] In the formula, a z,rear (t) represents the vertical acceleration after time delay compensation, a z,rear，calc (t-τ) represents the uncompensated vertical acceleration, where t is the current time and τ is the current acceleration. i For time delay.

[0276] 7) Using the output module, the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension are output to the electronic suspension controller.

[0277] It is worth noting that in this embodiment, a binocular stereo camera installed on the vehicle's windshield is used to collect road information and estimate the state of the built-in IMU. The vertical acceleration at the connection points between the vehicle's suspension and the body is observed by simulating the vehicle's passage over a speed bump. The results are compared between the experimental data and the simulation data. Figure 4 As shown.

[0278] In summary, this invention reuses the built-in IMU of an ADAS binocular camera and combines it with a data acquisition module and a binocular vision processing module to determine the three-axis acceleration and three-axis angular velocity measurements of the binocular camera, as well as the pitch and roll angles of the vehicle body. Then, an extended Kalman filter (EKF) fusion module fuses these data to estimate the motion state of the camera mounting point. Finally, a motion conversion module and a time delay compensation module calculate the vertical motion state of the vehicle's four corner suspensions—the vertical acceleration α of the sprung mass. z,i Vertical velocity v of the sprung mass z,i (where i∈FL,FR,RL,RR represent four suspension positions, which are basically consistent with the actual values ​​collected from the actual vehicle, meet the needs of practical engineering applications, solve the problems of difficult layout, high hardware cost and insufficient measurement accuracy of traditional dedicated IMUs, and solve the problem of wasted IMU resources in ADAS systems, providing precise control input for electronic suspension systems to improve vehicle ride quality.)

[0279] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications made to the present invention by those skilled in the art without departing from the spirit of the present invention shall fall within the protection scope of the present invention.

Claims

1. A chassis predictive control system based on binocular vision and multi-domain collaboration, characterized in that, It includes a data acquisition module, a binocular vision processing module, an extended Kalman filter fusion module, a motion conversion module, a time delay compensation module, and an output module. The data acquisition module is used to acquire road surface images captured by the binocular camera, as well as measurement data from the binocular camera's built-in IMU. The measurement data from the binocular camera's built-in IMU includes the binocular camera's three-axis acceleration measurement values ​​and three-axis angular velocity measurement values, and to acquire the vehicle speed from the vehicle bus signal. The binocular vision processing module is used to extract, match, and reconstruct 3D features from road images, perform planar fitting on the ground, and calculate the binocular vision attitude data, including the vehicle's pitch and roll angles. The extended Kalman filter fusion module is used to estimate the motion state at the camera mounting point by fusing measurement data from the built-in IMU of the binocular camera and attitude data from binocular vision using an extended Kalman filter. The motion state conversion module is used to convert the motion state of the binocular camera into the vertical velocity and vertical acceleration of the four corner suspensions of the vehicle based on the spatial positional relationship between the binocular camera and the four corners of the vehicle suspension, combined with rigid body kinematics. The four corner suspensions of the vehicle include the left front suspension, the right front suspension, the left rear suspension, and the right rear suspension. The time delay compensation module is used to calculate the time delay based on the current vehicle speed and wheelbase, and extract the state at the corresponding moment from the historical state based on the time delay to compensate for the time delay of the vertical acceleration of the left rear suspension and the right rear suspension. The output module is used to input the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension to the electronic suspension controller.

2. A chassis predictive control method based on binocular vision and multi-domain collaboration, characterized in that, Includes the following steps: 1) Based on the binocular camera in the ADAS system, a chassis predictive control system based on binocular vision and multi-domain collaboration is constructed. The chassis predictive control system includes a data acquisition module, a binocular vision processing module, an extended Kalman filter fusion module, a motion conversion module, a time delay compensation module, and an output module. 2) Use the data acquisition module to collect road surface images, triaxial acceleration measurements from the binocular camera, triaxial angular velocity measurements from the binocular camera, and vehicle speed; 3) Based on the road surface image, the binocular vision processing module is used to perform plane fitting on the road surface to obtain the plane normal vector. The pitch angle and roll angle of the vehicle body are calculated based on the plane normal vector. 4) Based on the vehicle's pitch angle, roll angle, three-axis acceleration measurement value of the binocular camera, and three-axis angular velocity measurement value of the binocular camera, the state of the binocular camera is estimated using the extended Kalman filter fusion module to obtain the motion state of the binocular camera. 5) Based on the motion state of the binocular camera and combined with rigid body kinematics, the vertical velocity and vertical acceleration of the left front suspension, the vertical velocity and vertical acceleration of the right front suspension, the vertical velocity and vertical acceleration of the left rear suspension, and the vertical velocity and vertical acceleration of the right rear suspension are calculated using the motion conversion module. 6) Based on the vehicle speed and wheelbase, use the time delay compensation module to perform time delay compensation on the vertical acceleration of the left and right rear suspensions of the vehicle. 7) Using the output module, the vertical velocity and vertical acceleration of the vehicle's left front suspension, right front suspension, left rear suspension, and right rear suspension are output to the electronic suspension controller.

3. The chassis predictive control method according to claim 2, characterized in that, In step 3), the pitch angle and roll angle of the vehicle body are calculated as follows: 3-1) At the same moment, acquire the left road surface image captured by the left eye camera and the right road surface image captured by the right eye camera in the binocular camera; 3-2) Preprocess the left and right road surface images respectively to eliminate image noise and correct distortion; 3-3) Extract feature points from the left road surface image to form a left feature point set, extract feature points from the right road surface image to form a right feature point set, and pair each feature point in the left feature point set with each feature point in the right feature point set to form a feature point pair set; 3-4) Based on the set of feature points, calculate the three-dimensional coordinates of each feature point pair in the camera coordinate system to form a ground point cloud in the camera coordinate system; 3-5) Preprocess the ground point cloud to remove non-ground points and avoid fitting an incorrect ground plane; 3-6) Based on the ground point cloud, fit the plane equation of the ground in the camera coordinate system, and transform the plane equation to the vehicle coordinate system to obtain the ground plane equation and plane normal vector in the vehicle coordinate system; 3-7) Calculate the pitch angle and roll angle of the vehicle body based on the plane normal vector.

4. The chassis predictive control method according to claim 2, characterized in that, In step 4), the specific process of state estimation using the extended Kalman filter fusion module is as follows: 4-1) Define the state vector for the extended Kalman filter; 4-2) Based on the three-axis acceleration measurement value and the three-axis angular velocity measurement value of the binocular camera, the state vector is predicted to obtain the state prediction value; 4-3) Construct the state transition Jacobian matrix, and calculate the covariance matrix used for predicting the state based on the state transition Jacobian matrix according to the following formula: P pred =FP k-1 F T +Q In the formula, P pred Let F be the covariance matrix, F be the state transition Jacobian matrix, and P be the covariance matrix. k-1 Let Q be the state covariance matrix at time k-1, Q be the process noise covariance matrix, and T be the matrix transpose. 4-4) Combine the pitch angle and roll angle of the vehicle body to calculate the Kalman gain, and use the Kalman gain to correct the state prediction value to obtain the optimal state estimate and covariance matrix.

5. The method according to claim 4, characterized in that, In step 4-2), the specific method for predicting the state vector is as follows: 4-2-1) Calculate the predicted camera position using the following formula: p k =p k-1 +v k-1 Δt In the formula, p k p is the predicted position of the camera at time k. k-1 v is the optimal estimate of the camera position at time k-1. k-1 Δt represents the optimal estimate of the camera velocity at time k-1, where Δt is the sampling time interval. 4-2-2) Calculate the predicted value of the stereo camera speed using the following formula: v k =v k-1 +(R(q k-1 )(a imu -b a，k-1 )+g k-1 )Δt In the formula, v k Let R(q) be the predicted camera velocity at time k. k-1 ) is the attitude quaternion q at time k-1 k-1 The rotation matrix formed, a imu b is the raw measurement value from the IMU accelerometer. a,k-1 For the optimal estimate of the accelerometer bias at time k-1, g k-1 Δt is the optimal estimate of the gravity vector at time k-1, where Δt is the sampling time interval. 4-2-3) Calculate the predicted value of the camera attitude quaternion according to the following formula: In the formula, The derivative of a quaternion, q k-1 Let k-1 be the attitude quaternion. For quaternion multiplication, ω imu b represents the raw measurement value from the IMU gyroscope. g,k-1 q is the optimal estimate of the gyroscope bias at time k-1. k Let k be the attitude quaternion at time k, Δt be the sampling time interval, and ‖·‖ be the modulus calculation operation; 4-2-4) Calculate the gyroscope zero-bias prediction value, the accelerometer zero-bias prediction value, and the gravity vector prediction value according to the following formulas: b g,k =b g,k-1 b a,k =b a,k-1 g k =g k-1 In the formula, b g,k Let b be the predicted value of the gyroscope bias at time k. g,k-1 b is the predicted value of the gyroscope bias at time k-1. a,k Let b be the predicted value of the accelerometer bias at time k. a,k-1 The predicted value of the accelerometer bias at time k-1, g k Let g be the predicted value of the gravity vector at time k. k-1 This is the predicted value of the gravity vector at time k-1.

6. The method according to claim 2, characterized in that, In step 5), the vertical velocity and vertical acceleration of the suspension point are calculated as follows: 5-1) Based on the installation position of the binocular camera in the vehicle coordinate system and the position of the target suspension in the vehicle coordinate system, calculate the position difference vector: Δr i =r i -r cam =[Δx i ,Δy i ,Δz i ] T r cam =[x cam ,y cam ,z cam ] T In the formula, Δr i Let Δx be the vector from the camera mounting point to the suspension point i. i For Δr i The component on the x-axis, Δy i For Δr i The component on the y-axis, Δz i For Δr i The component on the z-axis, r cam r represents the mounting position of the binocular camera in the vehicle body coordinate system. i Let x be the position of the target suspension point i in the vehicle coordinate system. cam For r cam The components on the x-axis, y cam For r cam The component on the y-axis, z cam For r cam The components on the z-axis, T is the matrix transpose; 5-2) Based on rigid body kinematics, calculate the vertical velocity at suspension point i using the following formula: v z,i =v z,cam +oh x Δy i -oh y Δx i In the formula, v z,i Let v be the vertical velocity at suspension point i. z,cam For the vertical velocity of the camera mounting point estimated by the extended Kalman filter, ω x To extend the Kalman filter to estimate the angular velocity of the vehicle body about the X-axis, ω y To extend the Kalman filter to estimate the angular velocity of the vehicle body around the Y-axis, Δx i Let Δy be the x-component of the vector from the camera mounting point to the suspension point i. i Let i be the y-component of the vector from the camera mounting point to the suspension point i. 5-3) According to rigid body kinematics, calculate the vertical acceleration at suspension point i using the following formula: a z,i =a z,cam +a x Δy i -a y Δx i +oh x (oh y Δz i -oh z Δy i )-oh y (oh x Δz i -oh z Δx i )+ω z (oh x Δy i -oh y Δx i ) In the formula, a z,i Let a be the vertical acceleration at suspension point i. z,cam To estimate the vertical acceleration at the camera mount point using the extended Kalman filter, α x Let α be the angular acceleration of the vehicle body about the X-axis. y Let ω be the angular acceleration of the vehicle body about the Y-axis. z Let ω be the angular velocity of the vehicle body around the Z-axis.

7. The method according to claim 2, characterized in that, In step 6), time delay compensation is performed on the vertical acceleration of the rear suspension, specifically including: 6-1) Calculate the time delay based on the vehicle speed signal and the vehicle's wheelbase using the following formula: In the formula, τ is the time delay, and v x The vehicle speed signal is represented by L, and the wheelbase is represented by L. 6-2) Based on the time delay, compensate for the time delay of the vertical acceleration of the left and right rear suspensions according to the following formulas: a z,rear (t)=a z,rear，calc (t-τ) In the formula, a z,rear (t) represents the vertical acceleration of the rear suspension after time delay compensation at time t, a z,rear,calc (t-τ) is the vertical acceleration value of the rear suspension without time delay compensation calculated according to step 5), where τ is the time delay.

8. The method according to claim 2, characterized in that, Step 2) also includes a data preprocessing procedure to remove the gravity component from the three-axis acceleration measurements of the stereo camera to obtain the true acceleration at the camera mounting point. The specific process is as follows: a cam =a imu -g body g word ＝[0，0，-9.81] T In the formula, a cam Let a be the actual acceleration at the camera mounting point. imu The raw measurement value from the IMU accelerometer, g body This represents the gravity vector in the vehicle body coordinate system. Let g be the rotation matrix from the world coordinate system to the vehicle coordinate system. world Let T be the gravity vector in the world coordinate system, and T be the matrix transpose.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the method as described in any one of claims 2 to 8.

Images