Lane keeping control method, device, equipment and readable storage medium

Abstract

The present application relates to a lane keeping control method, device, equipment and readable storage medium, and relates to the field of automobile intelligent driving technology, including detecting whether an IMU signal and / or a camera signal is received; when the IMU signal and the camera signal are not received, the state quantity prediction value and the error covariance prediction value of the IMU and the camera are updated based on the time property through the Kalman filter algorithm; the updated state quantity prediction value and the error covariance prediction value of the IMU and the camera are data-fused based on the multi-layer Kalman filter data fusion algorithm to obtain a fusion target; the fusion target is input as an observation quantity into the vehicle lateral error dynamics model, and the vehicle lateral error dynamics model is solved based on a linear quadratic regulator to obtain the input control quantity of the LKA. The present application can obtain an accurate input control quantity of the LKA and achieve more stable and accurate lane keeping motion control.

Description

Technical Field

[0001] The present application relates to the field of automobile intelligent driving technology, and in particular to a lane keeping control method, device, equipment and readable storage medium. Background Art

[0002] In the current intelligent driving field, the mainstream solution to realize the unmanned driving function of vehicles is to use multiple sensors to perceive environmental information, fuse the multi-sensor information, and finally perform road planning and vehicle body motion control based on the fused information. Among them, LKA (Lane Keeping Assist) is a technology used to assist the driver to keep the vehicle within the lane line. It is a lateral motion control ADAS (Advanced Driving Assistance System) function developed from the LDW (Lane Departure Warning) function.

[0003] Among them, LKA mainly uses a camera to identify the boundary line of the lane, and controls the opening and closing of the lane keeping system based on whether the boundary line of the lane where the vehicle is located is identified; when the camera identifies that the car deviates from the direction based on the boundary line, the corresponding white line displayed on the instrument panel turns red, and the steering wheel will vibrate to remind the driver. At the same time, it will automatically adjust the steering wheel to correct the direction back to the lane. That is, when the distance between the car and the lane line on the deviated side is less than a certain threshold or some wheels have deviated from the lane line, the electronic control unit calculates the auxiliary steering force and deceleration, controls the steering wheel and brake operating modules according to the degree of deviation, and applies steering force and braking force to make the car stably return to the normal track; however, if the driver turns on the turn signal and changes lanes normally, the system will not make any prompts.

[0004] It can be seen that the current LKA is more based on visual sensors such as cameras to assist in the lateral motion control of the vehicle. However, since the transmission cycle of the vehicle control signal is generally 10-20ms, and the transmission cycle of the visual sensor signal is generally greater than the control cycle, that is, about 50-100ms, it is impossible to receive real-time visual sensor signals in some control cycles, and thus it is impossible to stably and accurately control the lateral motion of the vehicle. Therefore, how to ensure the stability and accuracy of LKA motion control when no perception signal input is received is a problem that needs to be solved urgently. Summary of the invention

[0005] The present application provides a lane keeping control method, device, equipment and readable storage medium to solve the problem in the related art that the stability and accuracy of LKA motion control cannot be effectively guaranteed.

[0006] In a first aspect, a lane keeping control method is provided, comprising the following steps:

[0007] Detect whether IMU signal and / or camera signal is received;

[0008] When the IMU signal and the camera signal are not received, the state quantity prediction value and the error covariance prediction value of the IMU and the state quantity prediction value and the error covariance prediction value of the camera are updated based on the time property through the Kalman filter algorithm;

[0009] Based on the multi-layer Kalman filter data fusion algorithm, the updated IMU state quantity prediction value and error covariance prediction value and the updated camera state quantity prediction value and error covariance prediction value are fused to obtain the fusion target;

[0010] The fusion target is input as an observation quantity into a vehicle lateral error dynamics model, and the vehicle lateral error dynamics model is solved based on a linear quadratic regulator to obtain an input control quantity of the LKA.

[0011] In some embodiments, before the step of detecting whether the IMU signal and the camera signal are received, the method further includes:

[0012] The lateral dynamics model of the vehicle is determined based on the mechanical equilibrium state equation of the vehicle's center of mass and the moment equilibrium state equation of the vehicle body rotating around the Z axis;

[0013] The lateral position deviation and the heading angle deviation between the center of mass of the vehicle and the center line of the lane are input into the vehicle lateral dynamics model to generate a vehicle lateral error dynamics model.

[0014] In some embodiments, the state space equation of the vehicle lateral error dynamics model is:

[0015]

[0016] Where e1 represents the lateral position deviation between the actual lateral displacement from the center of mass of the vehicle to the center line of the lane and the expected displacement, represents the first-order derivative of the lateral position deviation, e2 represents the heading angle deviation between the actual heading angle and the expected heading angle, Represents the first-order derivative of the heading angle deviation, V x represents the longitudinal velocity of the vehicle's center of mass along the X-axis, m represents the vehicle's mass, The yaw rate of the vehicle's center of mass around the center of rotation, C af Indicates the vehicle front axle lateral stiffness, C ar Indicates the vehicle rear axle lateral stiffness, l f Represents the distance from the front axle to the center of mass of the vehicle, lr represents the distance from the rear axle to the center of mass of the vehicle, δ represents the front wheel turning angle, I z Represents the rotational inertia of the vehicle.

[0017] In some embodiments, the multi-layer Kalman filter data fusion algorithm includes a first layer data fusion algorithm and a second layer data fusion algorithm.

[0018] In some embodiments, the multi-layer Kalman filter data fusion algorithm performs data fusion on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a fusion target, including:

[0019] Based on the first layer data fusion algorithm, data fusion is performed on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a cross-correlation covariance matrix between each sensor;

[0020] Based on the second-layer data fusion algorithm, data fusion is performed on the cross-correlation covariance matrix between the sensors, the updated state quantity prediction value and error covariance prediction value of the IMU, and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a fusion target, which includes the system state quantity and the error covariance matrix.

[0021] In some embodiments, after the step of detecting whether an IMU signal and / or a camera signal is received, the method further includes:

[0022] When an IMU signal and a camera signal are received, the state quantity measurement value and the error covariance measurement value of the IMU and the state quantity measurement value and the error covariance measurement value of the camera are updated based on the time property and the measurement property through the Kalman filter algorithm;

[0023] Based on the multi-layer Kalman filter data fusion algorithm, the updated state quantity measurement values ​​and error covariance measurement values ​​of the IMU and the updated state quantity measurement values ​​and error covariance measurement values ​​of the camera are fused to obtain the fusion target.

[0024] In some embodiments, after the step of detecting whether an IMU signal and / or a camera signal is received, the method further includes:

[0025] When an IMU signal is received but a camera signal is not received, the state quantity measurement value and the error covariance measurement value of the IMU are updated based on the time property and the measurement property through the Kalman filter algorithm, and the state quantity prediction value and the error covariance prediction value of the camera are updated based on the time property;

[0026] Based on the multi-layer Kalman filter data fusion algorithm, the updated IMU state quantity measurement value and error covariance measurement value and the updated camera state quantity prediction value and error covariance prediction value are fused to obtain the fusion target;

[0027] When the IMU signal is not received and the camera signal is received, the state quantity prediction value and the error covariance prediction value of the IMU are updated based on the time property through the Kalman filter algorithm, and the state quantity measurement value and the error covariance measurement value of the camera are updated based on the time property and the measurement property;

[0028] Based on the multi-layer Kalman filter data fusion algorithm, the updated IMU state quantity prediction value and error covariance prediction value and the updated camera state quantity measurement value and error covariance measurement value are fused to obtain the fusion target.

[0029] In a second aspect, a lane keeping control device is provided, comprising:

[0030] A detection unit, which is used to detect whether an IMU signal and / or a camera signal is received;

[0031] An updating unit, which is used to update the state quantity prediction value and error covariance prediction value of the IMU and the state quantity prediction value and error covariance prediction value of the camera based on the time property through a Kalman filter algorithm when the IMU signal and the camera signal are not received;

[0032] A fusion unit is used to perform data fusion on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera based on a multi-layer Kalman filter data fusion algorithm to obtain a fusion target;

[0033] A calculation unit is used to input the fusion target as an observation quantity into a vehicle lateral error dynamics model, and solve the vehicle lateral error dynamics model based on a linear quadratic regulator to obtain an input control quantity of the LKA.

[0034] In a third aspect, a lane keeping control device is provided, comprising: a memory and a processor, wherein at least one instruction is stored in the memory, and the at least one instruction is loaded and executed by the processor to implement the aforementioned lane keeping control method.

[0035] In a fourth aspect, a computer-readable storage medium is provided, wherein the computer storage medium stores a computer program, and when the computer program is executed by a processor, the aforementioned lane keeping control method is implemented.

[0036] The beneficial effects brought about by the technical solution provided by the present application include: being able to effectively ensure the stability and accuracy of LKA motion control.

[0037] The present application provides a lane keeping control method, apparatus, device and readable storage medium. For scenarios where it is impossible to receive IMU signals and camera signals at the same time, the present application updates the state quantity prediction values ​​and error covariance prediction values ​​of the IMU and the camera based on the temporal properties of the Kalman filter algorithm, and uses a multi-layer Kalman filter data fusion algorithm to perform data fusion on the updated state quantity prediction values ​​and error covariance prediction values, and inputs the fusion target as an observation quantity into a vehicle lateral error dynamics model, and uses a linear quadratic regulator to solve the error dynamics model to obtain an accurate LKA input control quantity, so that the LKA can more stably and accurately achieve lane keeping motion control based on the input control quantity. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0039] Figure 1 A schematic flow chart of a lane keeping control method provided in an embodiment of the present application;

[0040] Figure 2 A schematic diagram of a two-layer Kalman filter data fusion algorithm flow chart provided in an embodiment of the present application;

[0041] Figure 3 A schematic diagram of the structure of a lane keeping control device provided in an embodiment of the present application;

[0042] Figure 4 A schematic diagram of the structure of a lane keeping control device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0043] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0044] The embodiments of the present application provide a lane keeping control method, device, equipment and readable storage medium, which can solve the problem in the related art that the stability and accuracy of LKA motion control cannot be effectively guaranteed.

[0045] Figure 1 A lane keeping control method provided in an embodiment of the present application comprises the following steps:

[0046] Step S10: Detecting whether an IMU (Inertial Measurement Unit) signal and / or a camera signal is received;

[0047] Furthermore, before the step of detecting whether the IMU signal and the camera signal are received, the method further includes:

[0048] The lateral dynamics model of the vehicle is determined based on the mechanical equilibrium state equation of the vehicle's center of mass and the moment equilibrium state equation of the vehicle body rotating around the Z axis;

[0049] The lateral position deviation and the heading angle deviation between the center of mass of the vehicle and the center line of the lane are input into the vehicle lateral dynamics model to generate a vehicle lateral error dynamics model.

[0050] Furthermore, the state space equation of the vehicle lateral error dynamics model is:

[0051]

[0052] Where e1 represents the lateral position deviation between the actual lateral displacement from the center of mass of the vehicle to the center line of the lane and the expected displacement, represents the first-order derivative of the lateral position deviation, e2 represents the heading angle deviation between the actual heading angle and the expected heading angle, Represents the first-order derivative of the heading angle deviation, V x represents the longitudinal velocity of the vehicle's center of mass along the X-axis, m represents the vehicle's mass, The yaw rate of the vehicle's center of mass around the center of rotation, C af Indicates the vehicle front axle lateral stiffness, C ar Indicates the vehicle rear axle lateral stiffness, l f Represents the distance from the front axle to the center of mass of the vehicle, l r represents the distance from the rear axle to the center of mass of the vehicle, δ represents the front wheel turning angle, I z Represents the rotational inertia of the vehicle.

[0053] For example, in this embodiment, it is assumed that the vehicle is a rigid body, the driving plane is horizontal, the vehicle steering method is front wheel steering, and the longitudinal speed V x If constant, the process of creating the vehicle lateral error dynamics model is as follows:

[0054] The mechanical equilibrium state of the vehicle's center of mass is analyzed as follows:

[0055]

[0056] Where m represents the vehicle mass, V x represents the longitudinal velocity of the center of mass along the X axis, represents the yaw rate of the center of mass rotating around the center of rotation, They represent the lateral linear velocity and lateral linear acceleration of the center of mass along the Y axis, respectively. f , l r Respectively represent the distance from the front and rear axles of the vehicle to the center of mass of the vehicle, C af , C ar They represent the lateral stiffness of the front and rear axles of the vehicle respectively, and δ represents the front wheel steering angle.

[0057] Then, the moment equilibrium state of the front and rear bodies of the vehicle rotating around the Z axis is analyzed:

[0058]

[0059] Among them, I z represents the moment of inertia of the vehicle, It represents the yaw angular acceleration of the center of mass rotating around the center of rotation.

[0060] According to equations (1) and (2), the state space equation of the vehicle lateral dynamics model can be obtained as follows:

[0061]

[0062] Where y represents the actual lateral displacement of the center of mass, and ψ represents the actual heading angle between the axle and the tangent of the lane line.

[0063] When the vehicle is performing lateral motion control, the deviation signal e y and e ψ They correspond to the system state quantities e1 and e2 respectively, where e y Represents the actual lateral displacement y and the expected displacement y between the center of mass of the vehicle and the center line of the lane des The lateral position deviation between ψ Represents the actual heading angle ψ and the expected heading angle ψ des The heading angle deviation between

[0064]

[0065] Using the deviation signal as the system state quantity of the vehicle lateral dynamics model, the state space equation of the vehicle lateral error dynamics model can be obtained as follows:

[0066]

[0067] Among them, the time-varying parameters involved above are derived from the camera unit, the inertial measurement unit and the vehicle CAN (Controller Area Network) network. Specifically, the lateral linear velocity Transverse linear acceleration and yaw rate Acquired by the camera unit; yaw angular acceleration Obtained through the inertial measurement unit; longitudinal velocity V x The front wheel angle δ is collected through the vehicle CAN network. It can be seen that the yaw rate signal and the lateral acceleration signal are collected through the inertial measurement unit, and there is no need to integrate the time, and there is no cumulative error, so that the measurement accuracy is high; at the same time, the system input signal is obtained through the vehicle CAN network and the smart car sensor, and no additional equipment is required, which is easy to deploy.

[0068] Since the transmission cycle of the vehicle control signal is generally 10-20ms, and the transmission cycle of the camera signal is generally greater than the control cycle, that is, about 50-100ms, so that the real-time visual sensor signal cannot be received in some control cycles, therefore, this embodiment will detect whether the IMU signal and / or camera signal is received before performing lane keeping control.

[0069] Step S20: When the IMU signal and the camera signal are not received, the state quantity prediction value and the error covariance prediction value of the IMU and the state quantity prediction value and the error covariance prediction value of the camera are updated based on the time property by using the Kalman filter algorithm;

[0070] Further, after the step of detecting whether the IMU signal and / or the camera signal is received, the method further includes:

[0071] When an IMU signal and a camera signal are received, the state quantity measurement value and the error covariance measurement value of the IMU and the state quantity measurement value and the error covariance measurement value of the camera are updated based on the time property and the measurement property through the Kalman filter algorithm;

[0072] Based on the multi-layer Kalman filter data fusion algorithm, the updated state quantity measurement values ​​and error covariance measurement values ​​of the IMU and the updated state quantity measurement values ​​and error covariance measurement values ​​of the camera are fused to obtain the fusion target.

[0073] Further, after the step of detecting whether the IMU signal and / or the camera signal is received, the method further includes:

[0074] When an IMU signal is received but a camera signal is not received, the state quantity measurement value and the error covariance measurement value of the IMU are updated based on the time property and the measurement property through the Kalman filter algorithm, and the state quantity prediction value and the error covariance prediction value of the camera are updated based on the time property;

[0075] Based on the multi-layer Kalman filter data fusion algorithm, the updated IMU state quantity measurement value and error covariance measurement value and the updated camera state quantity prediction value and error covariance prediction value are fused to obtain the fusion target;

[0076] When the IMU signal is not received and the camera signal is received, the state quantity prediction value and the error covariance prediction value of the IMU are updated based on the time property through the Kalman filter algorithm, and the state quantity measurement value and the error covariance measurement value of the camera are updated based on the time property and the measurement property;

[0077] Based on the multi-layer Kalman filter data fusion algorithm, the updated IMU state quantity prediction value and error covariance prediction value and the updated camera state quantity measurement value and error covariance measurement value are fused to obtain the fusion target.

[0078] Step S30: performing data fusion on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera based on a multi-layer Kalman filter data fusion algorithm to obtain a fusion target;

[0079] Furthermore, the multi-layer Kalman filter data fusion algorithm includes a first layer data fusion algorithm and a second layer data fusion algorithm.

[0080] Furthermore, the multi-layer Kalman filter data fusion algorithm is used to fuse the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a fusion target, including:

[0081] Based on the first layer data fusion algorithm, data fusion is performed on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a cross-correlation covariance matrix between each sensor;

[0082] Based on the second-layer data fusion algorithm, data fusion is performed on the cross-correlation covariance matrix between the sensors, the updated state quantity prediction value and error covariance prediction value of the IMU, and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a fusion target, which includes the system state quantity and the error covariance matrix.

[0083] For example, see Figure 2 As shown, the IMU and camera units are respectively tested for signal output. Taking the IMU as an example, when the IMU signal is detected, it is first determined whether the IMU signal passes the test. If so, the state quantity measurement value and error covariance measurement value of the IMU are updated based on the time property and measurement property through the Kalman filter algorithm; when the IMU signal is not detected or the IMU signal fails the test, the state quantity prediction value and error covariance prediction value of the IMU are updated based on the time property through the Kalman filter algorithm. Taking the camera unit as an example again, when the camera signal is detected, it is first determined whether the camera signal passes the test. If so, the state quantity measurement value and error covariance measurement value of the camera are updated based on the time property and measurement property through the Kalman filter algorithm; when the camera signal is not detected or the camera signal fails the test, the state quantity prediction value and error covariance prediction value of the camera are updated based on the time property and measurement property through the Kalman filter algorithm.

[0084] Then, based on the first-layer data fusion algorithm, the updated state quantity prediction value, error covariance prediction value, state quantity measurement value and error covariance measurement value are fused to obtain the cross-correlation covariance matrix between each sensor; finally, based on the second-layer data fusion algorithm, the cross-correlation covariance matrix between each sensor, the updated state quantity prediction value, error covariance prediction value, state quantity measurement value and error covariance measurement value are fused to obtain the system state quantity and error covariance matrix.

[0085] Specifically, the classical Kalman filter algorithm is applicable to linear systems whose measurement process can be described by state equations and output equations. The state equations and output equations are in the form of:

[0086]

[0087] Among them, A, B and C are constant matrices, X(t), u(t) and Y(t) represent the system state, system input and system output at time t, respectively, w(t) represents process noise, and z(t) represents measurement noise.

[0088] The update equation of the classic Kalman filter algorithm is:

[0089]

[0090] in, P-(t+1) represents the predicted value of the system state and the predicted value of the error covariance at time t+1, P(t) represents the state estimation value of the system state at time t and the state estimation value of the error covariance, K(t+1) represents the Kalman coefficient at time t+1, I nrepresents the unit matrix, Q represents the variance of process noise, and R represents the variance of measurement noise.

[0091] The calculation formula of the cross-correlation covariance matrix is:

[0092]

[0093] Among them, P ij (t), P ij (t+1) represents the correlation coefficient matrix between sensor i and sensor j at time t and time t+1 respectively, K i (t+1), K j (t+1) represents the Kalman coefficient of sensor i and sensor j at time t+1 respectively.

[0094] The calculation formula of system state quantity and error covariance matrix is:

[0095]

[0096] Where F=[F1,F2,...,F l ] T , F i (i=1,2,...,l) represents the fusion coefficient of the lth sensor, I N =[I n1 ,I n2 ,...,I nl ] T , I nl represents the unit matrix of the lth sensor, and P O denote the system state quantity and error covariance matrix respectively, represents the system state quantity of the lth sensor, P ij - 1 Represents the prediction correlation coefficient matrix of sensor i and sensor j.

[0097] It can be seen that by fusing the IMU signal and the camera signal through equations (7) to (9), the system status information after the fusion of IMU and camera can be obtained.

[0098] Step S40: inputting the fusion target as an observation into a vehicle lateral error dynamics model, and solving the vehicle lateral error dynamics model based on a linear quadratic regulator to obtain an input control variable of the LKA.

[0099] Exemplarily, in this embodiment, the vehicle lateral error dynamics model represented by equation (5) is decomposed into the following two state space equations according to the source of the system state quantity signal:

[0100]

[0101]

[0102] Then equation (5-1) and equation (5-2) can be expressed as:

[0103]

[0104] It can be seen that the form of formula (10) conforms to the output equation form of formula (6).

[0105] Then discretize equation (10) to obtain:

[0106] X e (t+1)=(TA e (t)+I)X e (t)+TB e (t)u e (t) (11)

[0107] It can be seen that the form of formula (11) conforms to the state equation form of formula (6). Therefore, the system state quantity and error covariance matrix obtained in step S30 are input as observation quantities to the vehicle lateral error dynamics model (i.e., formula (10) and formula (11)), and the vehicle lateral error dynamics model is solved based on LQR (Linear Quadratic Regulator) to obtain the input control quantity of LKA.

[0108] For scenarios where the IMU signal and the camera signal cannot be received at the same time, this application updates the state quantity prediction value and the error covariance prediction value of the IMU and the camera based on the time property of the classic Kalman filter algorithm, and uses a multi-layer Kalman filter data fusion algorithm to perform data fusion on the updated state quantity prediction value and the error covariance prediction value, and inputs the fusion target as an observation into the vehicle lateral error dynamics model, and uses a linear quadratic regulator to solve the error dynamics model to obtain an accurate input control quantity of the LKA, so that the LKA can achieve lane keeping motion control more stably and accurately based on the input control quantity. Among them, the use of a two-layer Kalman filter data fusion algorithm to fuse the IMU signal and the camera signal can ensure that when the sensor signal is missing or the signal fails to pass the test, the system state quantity and the error covariance matrix can still be updated to complete the multi-sensor information fusion.

[0109] See also Figure 3 As shown, the embodiment of the present application also provides a lane keeping control device, including:

[0110] A detection unit, which is used to detect whether an IMU signal and / or a camera signal is received;

[0111] An updating unit, which is used to update the state quantity prediction value and error covariance prediction value of the IMU and the state quantity prediction value and error covariance prediction value of the camera based on the time property through a Kalman filter algorithm when the IMU signal and the camera signal are not received;

[0112] A fusion unit is used to perform data fusion on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera based on a multi-layer Kalman filter data fusion algorithm to obtain a fusion target;

[0113] A calculation unit is used to input the fusion target as an observation quantity into a vehicle lateral error dynamics model, and solve the vehicle lateral error dynamics model based on a linear quadratic regulator to obtain an input control quantity of the LKA.

[0114] Furthermore, the device also includes a construction unit, which is used to:

[0115] The lateral dynamics model of the vehicle is determined based on the mechanical equilibrium state equation of the vehicle's center of mass and the moment equilibrium state equation of the vehicle body rotating around the Z axis;

[0116] The lateral position deviation and the heading angle deviation between the center of mass of the vehicle and the center line of the lane are input into the vehicle lateral dynamics model to generate a vehicle lateral error dynamics model.

[0117] The state space equation of the vehicle lateral error dynamics model is:

[0118]

[0119] Where e1 represents the lateral position deviation between the actual lateral displacement from the center of mass of the vehicle to the center line of the lane and the expected displacement, represents the first-order derivative of the lateral position deviation, e2 represents the heading angle deviation between the actual heading angle and the expected heading angle, Represents the first-order derivative of the heading angle deviation, V x represents the longitudinal velocity of the vehicle's center of mass along the X-axis, m represents the vehicle's mass, The yaw rate of the vehicle's center of mass around the center of rotation, C af Indicates the vehicle front axle lateral stiffness, C ar Indicates the vehicle rear axle lateral stiffness, l f Represents the distance from the front axle to the center of mass of the vehicle, l r represents the distance from the rear axle to the center of mass of the vehicle, δ represents the front wheel turning angle, I z Represents the rotational inertia of the vehicle.

[0120] Furthermore, the multi-layer Kalman filter data fusion algorithm includes a first layer data fusion algorithm and a second layer data fusion algorithm.

[0121] Furthermore, the fusion unit is specifically used for:

[0122] Based on the first layer data fusion algorithm, data fusion is performed on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a cross-correlation covariance matrix between each sensor;

[0123] Based on the second-layer data fusion algorithm, data fusion is performed on the cross-correlation covariance matrix between the sensors, the updated state quantity prediction value and error covariance prediction value of the IMU, and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a fusion target, which includes the system state quantity and the error covariance matrix.

[0124] Furthermore, the update unit is also used to: when receiving the IMU signal and the camera signal, update the state quantity measurement value and error covariance measurement value of the IMU and update the state quantity measurement value and error covariance measurement value of the camera through the Kalman filter algorithm and based on the time property and the measurement property; the fusion unit is also used to: based on the multi-layer Kalman filter data fusion algorithm, perform data fusion on the updated state quantity measurement value and error covariance measurement value of the IMU and the updated state quantity measurement value and error covariance measurement value of the camera to obtain a fusion target.

[0125] Further, when an IMU signal is received but a camera signal is not received, the updating unit is further used to: update the state quantity measurement value and the error covariance measurement value of the IMU based on the time property and the measurement property through the Kalman filter algorithm, and update the state quantity prediction value and the error covariance prediction value of the camera based on the time property; the fusion unit is further used to: perform data fusion on the updated state quantity measurement value and the error covariance measurement value of the IMU and the updated state quantity prediction value and the error covariance prediction value of the camera based on the multi-layer Kalman filter data fusion algorithm to obtain a fusion target;

[0126] When the IMU signal is not received and the camera signal is received, the update unit is also used to: update the state quantity prediction value and error covariance prediction value of the IMU based on the time property through the Kalman filter algorithm, and update the state quantity measurement value and error covariance measurement value of the camera based on the time property and measurement property; the fusion unit is also used to: perform data fusion on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity measurement value and error covariance measurement value of the camera based on the multi-layer Kalman filter data fusion algorithm to obtain a fusion target.

[0127] The embodiment of the present application uses a two-layer Kalman filter data fusion algorithm to fuse the IMU signal and the camera signal. The fused signal is input into the vehicle lateral error dynamics model as an observation quantity, and the LQR is used to solve the vehicle lateral error dynamics model to obtain the control quantity of the system input, thereby realizing the vehicle's LKA lane keeping.

[0128] It should be noted that those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the above-described device and each unit can refer to the corresponding process in the aforementioned lane keeping control method embodiment, and will not be repeated here.

[0129] The lane keeping control device provided in the above embodiment can be implemented in the form of a computer program. The computer program can be used in Figure 4 Lane keeping control device shown in operation.

[0130] An embodiment of the present application also provides a lane keeping control device, comprising: a memory, a processor and a network interface connected via a system bus, wherein at least one instruction is stored in the memory, and the at least one instruction is loaded and executed by the processor to implement all or part of the steps of the aforementioned lane keeping control method.

[0131] The network interface is used for network communication, such as sending assigned tasks, etc. Those skilled in the art will understand that Figure 4 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0132] The processor may be a CPU, or other general-purpose processors, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor, or the processor may be any conventional processor, etc. The processor is the control center of the computer device, and uses various interfaces and lines to connect various parts of the entire computer device.

[0133] The memory can be used to store computer programs and / or modules. The processor realizes various functions of the computer device by running or executing the computer programs and / or modules stored in the memory and calling the data stored in the memory. The memory can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system, an application required for at least one function (such as a video playback function, an image playback function, etc.), etc.; the data storage area can store data created according to the use of the mobile phone (such as video data, image data, etc.), etc. In addition, the memory can include a high-speed random access memory, and can also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (SmartMediacard, SMC), a secure digital (Secure digital, SD) card, a flash card (Flash Card), at least one disk storage device, a flash memory device or other volatile solid-state storage device.

[0134] An embodiment of the present application also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, all or part of the steps of the aforementioned lane keeping control method are implemented.

[0135] The embodiment of the present application implements all or part of the aforementioned process, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of each of the above methods when executed by the processor. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form, etc. The computer-readable medium may include: any entity or device capable of carrying computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium, etc. It should be noted that the content contained in the computer-readable medium can be appropriately increased or decreased 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 electric carrier signals and telecommunication signals.

[0136] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, servers or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage and optical storage, etc.) containing computer-usable program codes.

[0137] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.

[0138] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems) and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the process in the flowchart. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0139] The above description is only a specific implementation of the present application, so that those skilled in the art can understand or implement the present application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to the embodiments shown herein, but will conform to the widest range consistent with the principles and novel features applied for herein.

Claims

1. A lane keeping control method, characterized in that: The following steps are involved: Detect whether IMU signal and / or camera signal is received; When the IMU signal and the camera signal are not received, the state quantity prediction value and the error covariance prediction value of the IMU and the state quantity prediction value and the error covariance prediction value of the camera are updated based on the time property through the Kalman filter algorithm; Based on the multi-layer Kalman filter data fusion algorithm, the updated IMU state quantity prediction value and error covariance prediction value and the updated camera state quantity prediction value and error covariance prediction value are fused to obtain the fusion target; Inputting the fusion target as an observation into a vehicle lateral error dynamics model, and solving the vehicle lateral error dynamics model based on a linear quadratic regulator to obtain an input control variable of the LKA; The multi-layer Kalman filter data fusion algorithm includes a first layer data fusion algorithm and a second layer data fusion algorithm. The multi-layer Kalman filter data fusion algorithm performs data fusion on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a fusion target, including: Based on the first layer data fusion algorithm, data fusion is performed on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a cross-correlation covariance matrix between each sensor; Based on the second-layer data fusion algorithm, data fusion is performed on the cross-correlation covariance matrix between the sensors, the updated state quantity prediction value and error covariance prediction value of the IMU, and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a fusion target, which includes the system state quantity and the error covariance matrix.

2. The lane keeping control method according to claim 1, characterized in that: Before the step of detecting whether the IMU signal and the camera signal are received, the method further includes: The lateral dynamics model of the vehicle is determined based on the mechanical equilibrium state equation of the vehicle's center of mass and the moment equilibrium state equation of the vehicle body rotating around the Z axis; The lateral position deviation and the heading angle deviation between the center of mass of the vehicle and the center line of the lane are input into the vehicle lateral dynamics model to generate a vehicle lateral error dynamics model.

3. The lane keeping control method according to claim 2, characterized in that: The state space equation of the vehicle lateral error dynamics model is: Where e1 represents the lateral position deviation between the actual lateral displacement from the center of mass of the vehicle to the center line of the lane and the expected displacement, represents the first-order derivative of the lateral position deviation, e2 represents the heading angle deviation between the actual heading angle and the expected heading angle, Represents the first-order derivative of the heading angle deviation, V x represents the longitudinal velocity of the vehicle's center of mass along the X-axis, m represents the vehicle's mass, The yaw rate of the vehicle's center of mass around the center of rotation, C af Indicates the vehicle front axle lateral stiffness, C ar Indicates the vehicle rear axle lateral stiffness, l f Indicates the distance from the front axle to the center of mass of the vehicle, l r represents the distance from the rear axle to the center of mass of the vehicle, δ represents the front wheel turning angle, I z Represents the vehicle's moment of inertia.

4. The lane keeping control method according to claim 1, characterized in that: After the step of detecting whether the IMU signal and / or the camera signal is received, the method further includes: When an IMU signal and a camera signal are received, the state quantity measurement value and the error covariance measurement value of the IMU and the state quantity measurement value and the error covariance measurement value of the camera are updated based on the time property and the measurement property through the Kalman filter algorithm; Based on the multi-layer Kalman filter data fusion algorithm, the updated state quantity measurement values ​​and error covariance measurement values ​​of the IMU and the updated state quantity measurement values ​​and error covariance measurement values ​​of the camera are fused to obtain the fusion target.

5. The lane keeping control method according to claim 1, characterized in that: After the step of detecting whether the IMU signal and / or the camera signal is received, the method further includes: When an IMU signal is received but a camera signal is not received, the state quantity measurement value and the error covariance measurement value of the IMU are updated based on the time property and the measurement property through the Kalman filter algorithm, and the state quantity prediction value and the error covariance prediction value of the camera are updated based on the time property; Based on the multi-layer Kalman filter data fusion algorithm, the updated IMU state quantity measurement value and error covariance measurement value and the updated camera state quantity prediction value and error covariance prediction value are fused to obtain the fusion target; When the IMU signal is not received and the camera signal is received, the state quantity prediction value and the error covariance prediction value of the IMU are updated based on the time property through the Kalman filter algorithm, and the state quantity measurement value and the error covariance measurement value of the camera are updated based on the time property and the measurement property; Based on the multi-layer Kalman filter data fusion algorithm, the updated IMU state quantity prediction value and error covariance prediction value and the updated camera state quantity measurement value and error covariance measurement value are fused to obtain the fusion target.

6. A lane keeping control device, characterized in that: include: A detection unit, which is used to detect whether an IMU signal and / or a camera signal is received; An updating unit, which is used to update the state quantity prediction value and error covariance prediction value of the IMU and the state quantity prediction value and error covariance prediction value of the camera through a Kalman filter algorithm based on the time property when the IMU signal and the camera signal are not received; A fusion unit is used to perform data fusion on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera based on a multi-layer Kalman filter data fusion algorithm to obtain a fusion target; a calculation unit, which is used to input the fusion target as an observation quantity into a vehicle lateral error dynamics model, and solve the vehicle lateral error dynamics model based on a linear quadratic regulator to obtain an input control quantity of the LKA; The multi-layer Kalman filter data fusion algorithm includes a first layer data fusion algorithm and a second layer data fusion algorithm, and the fusion unit is specifically used for: Based on the first layer data fusion algorithm, data fusion is performed on the updated state quantity prediction value and error covariance prediction value of the IMU and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a cross-correlation covariance matrix between each sensor; Based on the second-layer data fusion algorithm, data fusion is performed on the cross-correlation covariance matrix between the sensors, the updated state quantity prediction value and error covariance prediction value of the IMU, and the updated state quantity prediction value and error covariance prediction value of the camera to obtain a fusion target, which includes the system state quantity and the error covariance matrix.

7. A lane keeping control device, characterized in that: include: A memory and a processor, wherein the memory stores at least one instruction, and the at least one instruction is loaded and executed by the processor to implement the lane keeping control method according to any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the lane keeping control method according to any one of claims 1 to 5 is implemented.

Images