WO2022237392A1 - 车辆的横向控制方法、装置及车辆 - Google Patents
车辆的横向控制方法、装置及车辆 Download PDFInfo
- Publication number
- WO2022237392A1 WO2022237392A1 PCT/CN2022/085370 CN2022085370W WO2022237392A1 WO 2022237392 A1 WO2022237392 A1 WO 2022237392A1 CN 2022085370 W CN2022085370 W CN 2022085370W WO 2022237392 A1 WO2022237392 A1 WO 2022237392A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- vehicle
- matrix
- current
- distance
- deviation
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 239000011159 matrix material Substances 0.000 claims abstract description 283
- 230000008859 change Effects 0.000 claims abstract description 78
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 52
- 238000004364 calculation method Methods 0.000 claims description 29
- 230000005484 gravity Effects 0.000 claims description 25
- 238000010586 diagram Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 6
- 230000000694 effects Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000012360 testing method Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000003491 array Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000003672 processing method Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/02—Control of vehicle driving stability
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/02—Control of vehicle driving stability
- B60W30/025—Control of vehicle driving stability related to comfort of drivers or passengers
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
- B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
- B60W40/06—Road conditions
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
- B60W60/0013—Planning or execution of driving tasks specially adapted for occupant comfort
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W2050/0001—Details of the control system
- B60W2050/0019—Control system elements or transfer functions
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W2050/0001—Details of the control system
- B60W2050/0019—Control system elements or transfer functions
- B60W2050/0028—Mathematical models, e.g. for simulation
- B60W2050/0031—Mathematical model of the vehicle
Definitions
- the present application relates to the technical field of vehicles, in particular to a vehicle lateral control method, device and vehicle.
- Lateral control generally refers to the tracking control method based on the path, curvature and other information output by the upper-level motion planning module to reduce tracking errors and ensure the stability and comfort of the vehicle at the same time; according to whether the lateral control method uses the same vehicle model , which can be divided into two types: (1) model-free lateral control methods; (2) model-based lateral control methods. Among them, the model-based lateral control method can be further divided into: a lateral control method based on a vehicle kinematics model and a lateral control method based on a vehicle dynamics model.
- the mainstream PID (Proportion Integral Differential, PID algorithm) control algorithm is a model-free lateral control, which takes the current path tracking deviation of the vehicle as an input, and performs proportional (Proportion), integral (Integration) and differential (Differentiation) on the tracking deviation. ) control to get the steering control amount.
- the PID algorithm does not consider the characteristics of the vehicle itself, it is less robust to external disturbances and cannot meet the effective control requirements of the vehicle during high-speed driving.
- the present application provides a vehicle lateral control method, device and vehicle.
- the first aspect of the embodiments of the present application is to provide a lateral control method of a vehicle, including the following steps:
- the calculation of the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculating the state matrix include:
- the target point is the closest point on the trajectory to the current position
- the type is a curve type, when the actual vehicle speed of the vehicle is greater than a preset threshold, the target point is a point away from the preview distance, otherwise the distance is determined by the curvature of the road.
- the formula for calculating the distance determined by the curvature of the road is:
- k is the linear change ratio of the speed
- V is the actual vehicle speed of the vehicle
- lmin is the minimum setting value of the preview distance
- the calculation formula of the state matrix is:
- e1 is the distance deviation
- e2 is the change rate of the distance deviation
- e3 is the heading angle deviation
- e4 is the change rate of the angle deviation.
- the second aspect of the embodiments of the present application is to provide a lateral control device for a vehicle, including:
- the obtaining module is used to obtain the actual coordinates and the current heading angle of the vehicle, and obtain the current pose and position information of the target point;
- Calculation module used to calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculate the state matrix
- the control module is used to determine the first model parameter matrix and the second model parameter matrix by using the vehicle dynamics model, and select the first weight matrix and the second weight matrix at the same time, so as to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator , and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix.
- the device also includes:
- a determining module configured to determine the required weight according to the vehicle's front wheel cornering stiffness, rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, the moment of inertia of the z-axis of the vehicle, and the mass of the vehicle.
- the calculation module includes:
- a judging unit configured to judge the type of the current road
- the first determination unit is configured to, if the type is a straight type, then the target point is a point on the trajectory closest to the current position;
- the second determining unit is configured to: if the type is a curve type, when the actual speed of the vehicle is greater than a preset threshold, the target point is a point that is a distance away from the preview; otherwise, the distance is a distance determined by the curvature of the road point.
- the formula for calculating the distance determined by the curvature of the road is:
- k is the linear change ratio of the speed
- V is the actual vehicle speed of the vehicle
- lmin is the minimum setting value of the preview distance
- the calculation formula of the state matrix is:
- e1 is the distance deviation
- e2 is the change rate of the distance deviation
- e3 is the heading angle deviation
- e4 is the change rate of the angle deviation.
- the third aspect of the embodiments of the present application is to provide a lateral control method of a vehicle, including:
- the target point being a track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track;
- a steering control amount is calculated based on the state matrix and the optimal matrix, and a steering actuator of the vehicle is controlled to execute the steering control amount to perform lateral control on the vehicle.
- the calculation of the state matrix according to the deviation between the current pose and the pose of the target point includes:
- the first weighting matrix is a diagonal matrix
- the diagonal matrix includes a first element, a second element, a third element, and a fourth element located on the main diagonal, and the first element, The second element, the third element and the fourth element respectively correspond to the distance deviation, the range deviation change rate, the heading angle deviation and the angle deviation change rate in the state matrix;
- the first element is the deviation coefficient of the vehicle
- the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation
- the third element is determined according to the vehicle information and the road information
- the Both the second element and the fourth element are 0.
- the vehicle information includes the current speed, and the road information includes the curvature radius of the current road;
- Determining the third element according to the vehicle information and the road information includes:
- the third element is calculated according to the following formula:
- the third element is calculated according to the following formula:
- the first radius of curvature threshold is a boundary condition for distinguishing a straight road from a curve
- the second radius of curvature threshold is a boundary condition for distinguishing a curve with a small curvature from a curve with a large curvature.
- the calculating the steering control amount based on the state matrix and the optimal matrix includes:
- the method before acquiring the pose of the target point, the method further includes:
- the type is a straight track type, determining the track point closest to the vehicle on the tracking track as the target point;
- the type is a curve type
- the determining as the target point a track point on the tracking track that is at an aiming distance from the vehicle includes:
- the aiming distance is determined according to the current vehicle speed, wherein,
- the aiming distance is calculated according to the following formula:
- the aiming distance is calculated according to the following formula:
- Aiming is performed along the traveling direction of the vehicle, and a track point on the tracking track that is apart from the vehicle by the aiming distance is determined as the target point.
- the method before determining the first model parameter matrix and the second model parameter matrix in the linear quadratic regulator LQR algorithm according to the vehicle dynamics model, the method further includes:
- the fourth aspect of the embodiments of the present application provides a lateral control device for a vehicle, including:
- An acquisition module configured to acquire the current pose of the vehicle and the pose of a target point, the target point being the track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track ;
- a calculation module configured to calculate a state matrix according to the deviation between the current pose and the pose of the target point
- a determining module configured to determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm of the linear quadratic regulator according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm , wherein the first weighting matrix is related to vehicle information of the vehicle and road information of the current road;
- the calculation module is also used to determine the optimal matrix according to the LQR algorithm, and calculate the steering control amount based on the state matrix and the optimal matrix;
- a control module configured to control a steering actuator of the vehicle to execute the steering control amount, so as to perform lateral control on the vehicle.
- calculation module is also used for:
- the first weighting matrix is a diagonal matrix
- the diagonal matrix includes a first element, a second element, a third element, and a fourth element located on the main diagonal, and the first element, The second element, the third element and the fourth element respectively correspond to the distance deviation, the range deviation change rate, the heading angle deviation and the angle deviation change rate in the state matrix;
- the first element is the deviation coefficient of the vehicle
- the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation
- the third element is determined according to the vehicle information and the road information
- the Both the second element and the fourth element are 0.
- the vehicle information includes the current speed, and the road information includes the curvature radius of the current road;
- the calculation module is also used for:
- the third element is calculated according to the following formula:
- the third element is calculated according to the following formula:
- the first radius of curvature threshold is a boundary condition for distinguishing a straight road from a curve
- the second radius of curvature threshold is a boundary condition for distinguishing a curve with a small curvature from a curve with a large curvature.
- the calculation module is further configured to multiply the state matrix and the optimal matrix to obtain the steering control amount.
- the device further includes a judging module, and the judging module is used for:
- the type is a straight track type, determining the track point closest to the vehicle on the tracking track as the target point;
- the type is a curve type
- the obtaining module is also used to obtain the current speed of the vehicle
- the judgment module is also used for:
- the aiming distance is determined according to the current vehicle speed, wherein,
- the aiming distance is calculated according to the following formula:
- the aiming distance is calculated according to the following formula:
- Aiming is performed along the traveling direction of the vehicle, and a track point on the tracking track that is apart from the vehicle by the aiming distance is determined as the target point.
- the determining module is further configured to: according to the vehicle's front wheel cornering stiffness, rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, and the z-axis rotation of the vehicle Inertia and overall vehicle mass determine the vehicle dynamics model.
- a fifth aspect of the embodiments of the present application is to provide a vehicle, including a controller configured to execute the vehicle lateral control method described in the first aspect and/or the third aspect of the embodiments of the present application.
- a sixth aspect of the embodiments of the present application provides a vehicle, which includes the vehicle lateral control device described in the second aspect and/or the fourth aspect of the embodiments of the present application.
- FIG. 1 is a flowchart of a first vehicle lateral control method provided in an embodiment of the present application
- Fig. 2 is an example diagram of the LQR horizontal and vertical error provided by the embodiment of the present application.
- Fig. 3 is the flowchart of the LQR algorithm that the embodiment of the present application provides
- Fig. 5 is a schematic diagram of the tracking effect of the curve at different speeds after optimization provided by the embodiment of the present application.
- Fig. 6 is a schematic block diagram of a lateral control device of a first vehicle provided in an embodiment of the present application
- FIG. 7 is a flow chart of a second vehicle lateral control method provided by an embodiment of the present application.
- FIG. 8 is a flow chart of a third vehicle lateral control method provided by an embodiment of the present application.
- Fig. 9 is a schematic block diagram of a second vehicle lateral control device provided by an embodiment of the present application.
- the LQR algorithm uses a two-degree-of-freedom dynamic model to design the lateral controller.
- the advantage of the LQR algorithm is that it can effectively reduce the steady-state tracking error when driving on a part of the curve by effectively combining it with the steering feedforward, so that When the vehicle is traveling on a medium-speed curve, its steady-state error approaches zero, which greatly improves the tracking performance.
- the tracking effect will be significantly reduced, and the dependence on the environment and parameter selection is high, that is, when the environment changes suddenly, it cannot be well adapted to the trajectory tracking under the new state conditions.
- the adjustment of LQR parameters is complicated. It not only needs to obtain the model parameters of the vehicle itself, but also needs to adjust the QR matrix of the LQR objective function (including the first weighting matrix Q and the second weighting matrix R). If the selection of the QR matrix is inaccurate, then the LQR The tracking performance of the algorithm will be greatly reduced, resulting in control failure, and the LQR algorithm in the related art basically uses a fixed QR matrix, resulting in poor system self-adaptability. This problem needs to be solved urgently.
- the present application provides a lateral control method for an autonomous vehicle.
- the actual coordinates and current heading angle of the vehicle can be obtained, the current pose and the position information of the target point can be obtained, and according to the current pose of the vehicle Calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point at the current moment based on the position information of the target point, calculate the state matrix, and use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix Model parameter matrix, and select the first weighting matrix and the second weighting matrix at the same time, to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator, and control the steering actuator of the vehicle to perform the multiplication of the optimal matrix and the state matrix to get Steering control amount, so as to ensure the stability and comfort of vehicle tracking on complex roads with rapid changes in curvature and speed, and improve the control accuracy and adaptability of the LQR controller.
- FIG. 1 is a schematic flowchart of a lateral control method for an autonomous vehicle provided in an embodiment of the present application.
- the lateral control method of the self-driving vehicle includes the following steps:
- step S101 the actual coordinates and current heading angle of the vehicle are obtained, and the current pose and position information of the target point are obtained.
- the method of obtaining the actual coordinates and current heading angle of the vehicle, and obtaining the current pose and position information of the target point according to the actual coordinates and the current heading angle can adopt the processing methods in related technologies. In order to avoid redundancy, in This will not be described in detail.
- step S102 calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculate the state matrix.
- calculating the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculating the state matrix includes: judging the current road type; if the type is a straight road type, the target point is the point on the track closest to the current position; if the type is a curve type, when the actual speed of the vehicle is greater than the preset threshold, the target point is a point away from the preview distance , otherwise a point at a distance determined by the curvature of the road.
- the preset threshold may be a threshold preset by a user, may be a threshold obtained through a finite number of experiments, or may be a threshold obtained through a finite number of computer simulations.
- the preset threshold is 60km/h.
- Vx is the longitudinal speed of the vehicle
- Vy is the lateral speed of the vehicle
- ⁇ is the current heading angle of the vehicle
- ⁇ des is the heading angle of the target point
- ⁇ is the deflection angle of the front wheels.
- the ratio to the front wheel deflection angle is defined as ratio. It can be seen from Figure 2 that the calculation of the horizontal and vertical errors is based on the comparison between the current time point and the target point, so how to obtain the target point will be described in detail below.
- the type of road can generally include straight road type and curve type. If the type of road is straight road type, then the target point is selected as the point closest to the current position on the trajectory, so as to ensure the accuracy of straight line tracking; if the road If the type of curve is a curve type, it can be determined according to the actual speed of the vehicle. For example, when the actual speed of the vehicle is greater than the preset threshold, the selection of the speed threshold (i.e.
- k is the linear change ratio of the speed
- V is the actual speed of the vehicle
- lmin is the minimum setting value of the preview distance
- lmin is selected as twice the minimum turning radius of the vehicle
- lmax lmin/2+v*v/2a
- a is the comfortable deceleration set by the vehicle, usually set to 3
- k ⁇ *kp
- kp is the curvature of the point closest to the current position on the trajectory
- ⁇ is an adjustable coefficient, which can be adjusted according to the actual tracking situation. If the curve is tracked early and the position deviation is large, ⁇ can be reduced, and vice versa. Then, according to the above formula, the preview distance is obtained, and the preview is performed along the forward direction of the vehicle to obtain the corresponding target point. When the preview distance is greater than the distance to the end point, the distance to the end point is selected as the preview distance.
- e1 is the displacement from the current moment to the target point, and the state matrix state is calculated by the following formula
- R is the radius of curvature of the target track point
- e1 is the distance deviation
- e2 is the change rate of the distance deviation
- e3 is the heading angle deviation
- e4 is the change rate of the angle deviation.
- step S103 use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix, and select the first weight matrix and the second weight matrix at the same time, so as to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator , and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix.
- Figure 3 is a flowchart of the LQR algorithm, which mainly includes the following steps:
- the perceived environment and vehicle information include: vehicle coordinates and heading angles, and tracking track target point coordinates and heading angles.
- the processed data is sent to step S309.
- step S303 determining the curvature of the straight road and curve, if it is a straight road, execute step S304, and if it is a curve, execute step S305.
- the target point is the point on the track closest to the current position, and skip to step S308.
- step S305 judging whether the actual vehicle speed of the vehicle is greater than a preset threshold, if yes, execute step S306, otherwise, execute step S307.
- step S308 state quantity, and jump to step S310.
- the distance deviation e1 between the vehicle and the target point, the distance deviation change rate e2, the heading deviation e3, and the angle deviation change rate e4 are calculated according to the real-time pose and target point position information, so as to obtain the state matrix state.
- the model parameter matrices A and B can be determined according to the above dynamic model parameters, and the weighting matrices Q and R can be selected at the same time (selection of QR weights).
- the state matrix can be obtained, and the state matrix and the QR weight matrix selector are input to the LQR controller.
- step S311 the vehicle turns to the actuator, and skips to step S301.
- the steering control amount of the self-driving car is calculated and delivered to the steering actuator for execution.
- it also includes: according to the front wheel cornering stiffness of the vehicle, the rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, and the z-axis moment of inertia of the vehicle Determine the vehicle dynamics model with the vehicle mass.
- the parameters of the vehicle dynamics model mainly include: the front wheel cornering stiffness Cf, the rear wheel cornering stiffness Cr, the distance lf from the front axle to the center of gravity of the vehicle, the distance lr from the rear axle to the center of gravity of the vehicle, and the z-axis rotation of the vehicle Inertia Iz and vehicle mass m.
- the above-mentioned parameters of the vehicle dynamics model can be obtained by querying the basic information of the vehicle, or can be obtained by re-measurement. Specifically, it can be processed by those skilled in the art according to the actual situation, and is not specifically limited here.
- calculation formulas for determining the first model parameter matrix matrix_a_ and the second model parameter matrix matrix_b_ by using the vehicle dynamics model may be as follows:
- Cf and Cr are the front wheel cornering stiffness and rear wheel cornering stiffness
- lf and lr are the distances from the front and rear axles to the center of gravity
- Iz is the moment of inertia of the vehicle's z-axis
- m is the mass of the vehicle.
- the first parameters correspond to the four variables of the state matrix state, and the selection of q1 and q3 is the key to LQR control;
- R1 and R2 are the boundary conditions for distinguishing straight roads and curves, small curvature and large curvature respectively;
- the first weighting matrix Q can be obtained only by determining the initial value of q1 through line tracking of the real vehicle alone.
- the optimal matrix matrix_k is determined according to the numerical iterative solution of the Riccati equation, which is obtained by the following formula;
- matrix_k (matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)
- max_num_iterationa is the maximum number of iterations, exemplarily selected as 150
- matrix_a_T and matrix_b_T are the transpose matrices of matrix_a_ and matrix_b_ respectively
- matrix_p is the process iteration matrix
- the initial value is the Q matrix.
- the front wheel rotation angle is finally multiplied by ratio and output to the actuator to realize tracking.
- the embodiment of the present application also provides a lateral control method of a vehicle, which can be applied to self-driving cars, including fully automatic driving cars and semi-automatic driving cars, and the lateral control method includes steps S701-S706:
- the target point is the track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track.
- pose may also be referred to as position information, which may include coordinate information and direction information.
- the deviation between the current pose and the pose of the target point at least includes the distance deviation between the actual coordinates of the vehicle and the coordinates of the target point, and the heading angle deviation between the current heading angle and the target heading angle.
- the distance deviation refers to the absolute distance deviation between the actual coordinates of the vehicle and the coordinates of the target point, but in some embodiments, the distance deviation can also refer to the lateral distance between the actual coordinates of the vehicle and the coordinates of the target point Distance
- lateral distance refers to the distance in the width direction of the vehicle body.
- the lateral distance can be obtained by orthographically projecting the absolute distance in the vehicle body width direction.
- the deviation between the current pose and the pose of the target point may also include a distance deviation change rate and an angle deviation change rate.
- S703. Determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm of the linear quadratic regulator according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm.
- the first weighting matrix Q is a state weight matrix, and the selection condition of this matrix is related to the vehicle information of the vehicle and the road information of the current road;
- the second weighting matrix R is a control weighting matrix.
- the first weighting matrix Q and the second weighting matrix R can be selected after determining the first model parameter matrix and the second model parameter matrix, or after determining the first model parameter matrix Select the first weighting matrix Q and the second weighting matrix R before and the second model parameter matrix, and select the first weighting matrix Q and the second weighting matrix R while determining the first model parameter matrix and the second model parameter matrix.
- the optimal matrix can be obtained by substituting the first model parameter matrix and the second model parameter matrix into the Riccati equation for iterative solution.
- steps S701-S702 can be executed first, and then steps S703-S704 can be executed; steps S701-S702 can also be executed first, and then steps S703-S704 can be executed; Steps S701-S702 and steps S703-S704.
- step S705 can be executed by the controller of the vehicle or other units with calculation functions.
- the steering command can be sent to the steering actuator of the vehicle, so that the steering actuator that receives the steering command can follow the steering control Quantitative control of steering to achieve lateral control of the vehicle.
- the optimal matrix is obtained by processing; the steering control amount can be obtained by calculating based on the optimal matrix and the state matrix; by controlling the steering actuator of the vehicle to execute the steering control amount, the lateral control of the vehicle can be realized, thus ensuring The tracking stability and comfort of the vehicle on complex roads with large curvature changes and fast-changing speeds have improved the control accuracy and adaptability of the LQR controller.
- an embodiment of the present application also provides a lateral control method for an automatic driving vehicle, which can be applied to automatic driving vehicles, including fully automatic driving vehicles and semi-autonomous driving vehicles. Taking an autonomous vehicle as an example, the lateral control method will be described below.
- the lateral control method includes steps S801-S808:
- the self-driving car will plan the driving route in advance or in real time, and then drive on the current road according to the planned driving route.
- the driving route is the tracking trajectory.
- the target point will be determined according to the type of the current road. Wherein, the target point is the track point corresponding to the vehicle on the tracking track at the current moment.
- step S801 may further include:
- the track point closest to the vehicle on the tracking track is determined as the target point.
- the track point on the tracking track closest to the vehicle may be the point closest to the vehicle in the vertical direction, or the point closest to the vehicle in the lateral direction (ie, in the width direction of the vehicle beam).
- the track point on the tracking track that is separated from the vehicle by the aiming distance is determined as the target point, and the aiming distance is related to the current speed of the vehicle and the curvature of the current road.
- the method for determining the target point may further include:
- the aiming distance is calculated according to the following formula:
- the aiming distance is calculated according to the following formula:
- step S802 is executed.
- the current pose of the vehicle includes the actual coordinates and the current heading angle of the vehicle at the current moment
- the pose of the target point includes the target coordinates and the target heading angle of the target point.
- the manner of acquiring the current pose of the vehicle and the pose of the target point can be any acquisition manner in the related art, and to avoid redundancy, details are not repeated here.
- the deviation between the current pose and the pose of the target point includes at least the distance deviation between the actual coordinates of the vehicle and the target coordinates, and the heading angle deviation between the current heading angle and the target heading angle, and may also include the distance The rate of change of deviation and the rate of change of angular deviation of the heading angle.
- step S803 may further include:
- Step S8031 calculating the distance deviation e1, the distance deviation change rate e2, the heading angle deviation e3 and the angle deviation change rate e4 between the vehicle and the target point.
- the distance deviation between the vehicle and the target point (generally refers to the distance deviation between the center of mass of the vehicle and the target point) is e1
- the current heading angle of the vehicle is ⁇
- the target point The target heading angle of the point is ⁇ des
- the heading angle deviation e3 ⁇ des - ⁇ .
- Step S8032 taking the distance deviation e1, the distance deviation change rate e2, the heading angle deviation e3 and the angle deviation change rate e4 as elements in the state matrix to obtain the state matrix.
- Step S804 determining the vehicle dynamics model.
- the vehicle dynamics model can be based on the front wheel cornering stiffness Cf of the vehicle, the rear wheel cornering stiffness Cr, the distance lf from the front axle to the center of gravity of the vehicle, the distance lr from the rear axle to the center of gravity of the vehicle, the z-axis moment of inertia Iz and The vehicle mass m and other parameters are determined. These parameters can be obtained by querying the basic information of the vehicle, or can be obtained by measurement, and specifically can be processed by those skilled in the art according to actual conditions, and are not specifically limited here.
- Step S805 Determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix.
- the calculation formulas of the first model parameter matrix matrix_a_ and the second model parameter matrix matrix_b_ determined according to the vehicle dynamics model may be as follows:
- Cf is the cornering stiffness of the front wheel
- Cr is the cornering stiffness of the rear wheel
- lf is the distance from the front axle to the center of gravity of the vehicle
- lr is the distance from the rear axle to the center of gravity of the vehicle
- Iz is the moment of inertia of the z-axis of the vehicle
- m is the integer car quality.
- the first weighting matrix usually called Q matrix
- the second weighting matrix usually called R matrix
- the first weighting matrix may choose a diagonal matrix, namely
- the first element q1, the second element q2, the third element q3 and the fourth element q4 located on the main diagonal of the diagonal matrix are respectively related to the distance deviation e1, the distance deviation change rate e2, and the heading angle deviation in the state matrix e3 corresponds to the angular deviation change rate e4.
- the selection of the first element q1 and the third element q3 is the key to the LQR control algorithm.
- the first element q1 in the first weighting matrix is the deviation coefficient of the vehicle
- the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation, which is used to adjust the state weight of the distance deviation e1 in the state matrix.
- the deviation coefficient of the vehicle can be determined based on the individual straight-line tracking of the real vehicle. For example, before the self-driving car leaves the factory, technicians test the deviation generated when the vehicle drives along the straight-line tracking track for many times, and collect the front wheel deflection data in real time during the test.
- the ratio of the front wheel deflection angle to the distance deviation after processing the collected data, the ratio of the front wheel deflection angle to the distance deviation, namely the deviation coefficient, can be stored in the vehicle in advance.
- the third element q3 in the first weighting matrix is determined according to vehicle information and road information, and both the second element q2 and the fourth element q4 are 0.
- the above-mentioned "distance deviation” can generally be regarded as a lateral distance deviation, that is, a distance deviation generated in a direction parallel to the width of the vehicle.
- the first weighting matrix matrix_q_ can be:
- the vehicle information may include the current speed
- the road information may include the curvature radius of the current road.
- the method of determining the third element q3 according to vehicle information and road information may be to compare the curvature radius of the current road with the first curvature radius threshold and the second curvature radius threshold, wherein the first curvature radius threshold is used to distinguish straight roads from curved roads.
- the boundary condition of the road, the second curvature radius threshold is the boundary condition used to distinguish small curvature curves from large curvature curves.
- the first weighting matrix can be obtained.
- the second weighting matrix may choose an identity matrix, namely
- matrix_r_ is the second weighting matrix
- the first model parameter matrix and the second model parameter matrix are calculated according to the LQR algorithm of the linear quadratic regulator to obtain the optimal matrix matrix_k, which may specifically include:
- the optimal matrix matrix_k is determined by the numerical iterative solution of the Riccati equation:
- matrix_p_next matrix_a_T*matrix_p_*matrix_a_-(matrix_a_T*matrix_p_*matrix_b_)*(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)+matrix_q_;
- matrix_k (matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)
- max_num_iterationa is the maximum number of iterations, optionally, max_num_iterationa can be 150; matrix_a_T and matrix_b_T are the transpose matrices of matrix_a_ and matrix_b_ respectively; matrix_p_ is the process iteration matrix, the initial value is the first weighting matrix.
- the optimal matrix matrix_k can be obtained.
- Step S807 calculating the steering control amount based on the state matrix and the optimal matrix.
- the steering control amount can be obtained by multiplying the state matrix and the optimum matrix.
- Step S808 controlling the steering actuator of the vehicle to execute the steering control amount, so as to perform lateral control on the vehicle.
- the steering control amount obtained in step S808 is the steering control amount of the front wheels, and if it is necessary to calculate the steering wheel steering control amount, it needs to be further calculated on this basis.
- the steering control amount of the steering wheel can be obtained by multiplying the steering control amount of the front wheels by the ratio value.
- ratio is the ratio of the steering wheel angle of the vehicle to the front wheel angle, and the ratio can be obtained by consulting the basic information of the vehicle, or by testing the actual vehicle, and can also be pre-stored in the vehicle.
- Figure 4 Schematic diagram of tracking error before optimization
- Fig. 5 is a schematic diagram of tracking error after optimization
- the embodiment of the application has summed up the self-adaptive formula to solve the problem that the Q matrix and the R matrix in the LQR algorithm cause larger errors when selecting, and it can be seen from the results of the simulation comparison verification test that when using this After applying the adaptive formula provided by the embodiment, the tracking effect of the vehicle is significantly improved.
- the actual coordinates and current heading angle of the vehicle can be obtained, the current pose of the vehicle can be obtained, and the pose of the target point can be obtained; then according to the current pose of the vehicle and the target point Point pose, calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point at the current moment, and obtain the state matrix; use the vehicle dynamics model to determine the first model parameter matrix in the LQR algorithm and The second model parameter matrix, and select the first weighting matrix and the second weighting matrix in the LQR algorithm to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator, and finally control the steering actuator of the vehicle to perform the optimal matrix and
- the steering control amount obtained by multiplying the state matrix can ensure the stability and comfort of the vehicle tracking on complex roads with large curvature changes and fast-changing driving scenes, and realize the control accuracy and adaptability of the LQR controller. improve.
- Fig. 6 is a schematic block diagram of a lateral control device for an automatic driving vehicle provided by an embodiment of the present application.
- the lateral control device 10 of the self-driving vehicle includes: an acquisition module 100 , a calculation module 200 and a control module 300 .
- the acquiring module 100 is used to acquire the actual coordinates and the current heading angle of the vehicle, and obtain the position information of the current pose and the target point;
- the calculation module 200 is used to calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculate the state matrix;
- the control module 300 is used to determine the first model parameter matrix and the second model parameter matrix by using the vehicle dynamics model, and select the first weight matrix and the second weight matrix at the same time, so as to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator , and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix.
- the above-mentioned lateral control device 10 of the self-driving vehicle further includes:
- the determining module is used to determine vehicle dynamics according to the front wheel cornering stiffness, rear wheel cornering stiffness, distance from the front axle to the center of gravity of the vehicle, distance from the rear axle to the center of gravity of the vehicle, the moment of inertia of the z-axis of the vehicle, and the mass of the vehicle Model.
- the calculation module 200 includes:
- a judging unit configured to judge the type of the current road
- the first determining unit is used to determine the nearest point on the track from the current position of the vehicle (ie, the vehicle coordinates) as the target point when it is judged that the type of the current road is a straight road type;
- the second determination unit is used to determine the track point that is apart from the current position of the vehicle as the target point if the actual vehicle speed of the vehicle is greater than the preset threshold when it is determined that the type of the current road is a curve type, otherwise the A track point that is at a distance determined by the curvature of the road from the current position of the vehicle is determined as a target point.
- the calculation formula for determining the distance from the curvature of the road is:
- k is the linear change ratio of the speed
- V is the actual speed of the vehicle
- lmin is the minimum setting value of the preview distance
- the calculation formula of the state matrix is:
- e1 is the distance deviation
- e2 is the change rate of the distance deviation
- e3 is the heading angle deviation
- e4 is the change rate of the angle deviation.
- the actual coordinates and current heading angle of the vehicle can be obtained, the current pose and the position information of the target point can be obtained, and the current moment and target can be calculated according to the current pose and position information.
- Point distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate calculate the state matrix, and use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix, and simultaneously select the first weighting matrix and
- the second weighting matrix is to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator, and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix, so as to ensure that the curvature and speed change quickly.
- Fig. 9 is a schematic diagram of another vehicle lateral control device provided by an embodiment of the present application, which can be applied to autonomous driving vehicles, including fully automatic driving vehicles and semi-autonomous driving vehicles.
- the lateral control device 90 includes:
- the acquisition module 901 is configured to acquire the current pose of the vehicle and the pose of a target point, wherein the target point is the track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track.
- the calculation module 902 is configured to calculate the state matrix according to the deviation between the current pose and the pose of the target point.
- the determining module 903 is configured to determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm of the linear quadratic regulator according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm , wherein the first weighting matrix is related to the vehicle information of the vehicle and the road information of the current road.
- the vehicle information may include the current speed
- the road information may include the curvature radius of the current road.
- the calculation module 902 is further configured to determine the optimal matrix according to the LQR algorithm, and obtain the steering control amount based on the state matrix and the optimal matrix.
- the control module 904 is configured to control the steering actuator of the vehicle to execute the steering control amount, so as to perform lateral control on the vehicle.
- the computing module 902 is also configured to:
- the first weighting matrix is a diagonal matrix
- the diagonal matrix includes a first element, a second element, a third element, and a fourth element located on the main diagonal, and the first element, the The second element, the third element and the fourth element respectively correspond to the distance deviation, the range deviation change rate, the heading angle deviation and the angle deviation change rate in the state matrix.
- the first element is the deviation coefficient of the vehicle
- the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation
- the third element is determined according to the vehicle information and road information
- the second element and the fourth element are both 0.
- the determining module 903 is further configured to determine a third element according to vehicle information and road information, wherein,
- the third element is calculated according to the following formula:
- the third element is calculated according to the following formula:
- the third element is calculated according to the following formula:
- q3 is the third element
- q1 is the first element
- V is the current speed of the vehicle, m/s
- the first curvature radius threshold is the boundary condition for distinguishing straight roads and curves
- the second curvature radius threshold is for Boundary conditions that differentiate between small curvature curves and large curvature curves.
- the calculation module 902 is further configured to multiply the state matrix and the optimal matrix to obtain the steering control amount.
- the lateral control device 90 may also include:
- Judging module 905, configured to judge the type of the current road
- the track point closest to the vehicle on the tracking track is determined as the target point
- the track point on the tracking track that is separated from the vehicle by the aiming distance is determined as the target point, and the aiming distance is related to the current speed of the vehicle and the curvature of the current road.
- the obtaining module is also used to obtain the current speed of the vehicle
- the aiming distance is calculated according to the following formula:
- the aiming distance is calculated according to the following formula:
- a is the comfortable deceleration, m/s 2 ;
- Aiming is carried out along the driving direction of the vehicle, and the track point on the tracking track which is far from the vehicle's aiming distance is determined as the target point.
- the determination module 903 is further configured to, according to the vehicle's front wheel cornering stiffness, rear wheel cornering stiffness, distance from the front axle to the center of gravity of the vehicle, distance from the rear axle to the center of gravity of the vehicle, z
- the moment of inertia of the shaft and the mass of the whole vehicle determine the vehicle dynamics model.
- an embodiment of the present application also provides a vehicle, which includes the vehicle lateral control device mentioned in any of the above embodiments.
- the embodiment of the present application also provides another vehicle, the vehicle includes a controller, and the controller is configured to implement the vehicle lateral control method described in any one of the above embodiments.
- the vehicle's actual coordinates and current heading angle can be obtained through the above-mentioned lateral control device of the vehicle, and the current pose and the pose of the target point can be obtained, and according to the current pose and the pose of the target point Calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point at the current moment, calculate the state matrix, and use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix, and Select the first weighting matrix and the second weighting matrix to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator, and then control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix, so that Ensure the stability and comfort of vehicle tracking on complex roads with large curvature changes and fast-changing driving scenarios, and improve the control accuracy and adaptability of the LQR controller.
- first and second are used for descriptive purposes only, and cannot be interpreted as indicating or implying relative importance or implicitly specifying the quantity of indicated technical features.
- the features defined as “first” and “second” may explicitly or implicitly include at least one of these features.
- “N” means at least two, such as two, three, etc., unless otherwise specifically defined.
- Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing a custom logical function or step of a process , and the scope of preferred embodiments of the present application includes additional implementations in which functions may be performed out of the order shown or discussed, including in substantially simultaneous fashion or in reverse order depending on the functions involved, which shall It should be understood by those skilled in the art to which the embodiments of the present application belong.
- each part of the present application may be realized by hardware, software, firmware or a combination thereof.
- the N steps or methods may be implemented by software or firmware stored in memory and executed by a suitable instruction execution system.
- a suitable instruction execution system For example, if implemented in hardware as in another embodiment, it can be implemented by any one or a combination of the following techniques known in the art: a discrete Logic circuits, ASICs with suitable combinational logic gates, Programmable Gate Arrays (PGA), Field Programmable Gate Arrays (FPGA), etc.
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
一种车辆的横向控制方法、装置及车辆,其中方法包括:获取车辆的当前位姿和目标点的位姿;根据当前位姿和目标点的位姿之间的偏差,计算状态矩阵;根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取LQR算法中的第一加权矩阵和第二加权矩阵;根据LQR算法确定最优矩阵;基于状态矩阵和最优矩阵计算转向控制量,控制车辆的转向执行器执行转向控制量,以对车辆进行横向控制。由此,保证了车辆在曲率变化大的复杂道路上和速度变化快的行驶场景下跟踪的稳定性和舒适性,实现了LQR控制器的控制精度和自适应性的提高。
Description
本申请要求于2021年5月11日提交的申请号为202110510779.1、发明名称为“自动驾驶车辆的横向控制方法、装置及车辆”的中国专利申请,以及于2022年3月4日提交的申请号为202210210929.1、发明名称为“自动驾驶车辆的横向控制方法、装置及车辆”的中国专利申请的优先权,其全部内容通过引用结合在本申请中。
本申请涉及车辆技术领域,特别涉及一种车辆的横向控制方法、装置及车辆。
横向控制一般是指根据上层运动规划模块输出的路径、曲率等信息进行跟踪控制,以减少跟踪误差,同时保证车辆行驶的稳定性和舒适性的车辆控制方法;根据横向控制方法是否使用车辆模型同,可以将其分为两种类型:(1)无模型的横向控制方法;(2)基于模型的横向控制方法。其中,基于模型的横向控制方法又可分为:基于车辆运动学模型的横向控制方法以及基于车辆动力学模型的横向控制方法。
主流的PID(Proportion Integral Differential,PID算法)控制算法即为无模型的横向控制,将车辆当前的路径跟踪偏差作为输入量,通过对跟踪偏差进行比例(Proportion)、积分(Integration)和微分(Differentiation)控制得到转向控制量。
然而,由于PID算法没有考虑车辆本身的特性,因此对外界干扰的鲁棒性较差,无法满足车辆在高速行驶过程中的有效控制需求。
发明内容
本申请提供一种车辆的横向控制方法、装置及车辆。
本申请实施例的第一方面是提供一种车辆的横向控制方法,包括以下步骤:
获取车辆的实际坐标和当前航向角,得到当前位姿和目标点的位置信息;
根据所述当前位姿和位置信息计算当前时刻与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵;以及
利用车辆动力学模型确定第一模型参数矩阵和第二模型参数矩阵,并同时选取第一加权矩阵和第二加权矩阵,以根据线性二次型调节器(linear quadratic regulator,LQR)算法 确定最优矩阵,及控制所述车辆的转向执行器执行由所述最优矩阵和所述状态矩阵相乘得到的转向控制量。
可选地,还包括:
根据所述车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量与整车质量确定所述车辆动力学模型。
可选地,所述根据所述当前位姿和位置信息计算当前时刻与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵,包括:
判断当前道路的类型;
若所述类型为直道类型,则所述目标点为轨迹上距离当前位置最近的点;
若所述类型为弯道类型,则在所述车辆的实际车速大于预设阈值时,所述目标点为相距预瞄距离的点,否则相距由道路曲率确定距离的点。
可选地,所述由道路曲率确定距离的计算公式为:
L=kV+lmin,
其中,k为速度的线性变化比例,V为所述车辆的实际车速,lmin为预瞄距离的最小设定值。
可选地,所述状态矩阵的计算公式为:
其中,e1为所述距离偏差,e2为所述距离偏差变化率,e3为所述航向角偏差,e4为所述角度偏差变化率。
本申请实施例的第二方面是提供一种车辆的横向控制装置,包括:
获取模块,用于获取车辆的实际坐标和当前航向角,得到当前位姿和目标点的位置信息;
计算模块,用于根据所述当前位姿和位置信息计算当前时刻与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵;以及
控制模块,用于利用车辆动力学模型确定第一模型参数矩阵和第二模型参数矩阵,并同时选取第一加权矩阵和第二加权矩阵,以根据线性二次型调节器LQR算法确定最优矩阵,及控制所述车辆的转向执行器执行由所述最优矩阵和所述状态矩阵相乘得到的转向控制量。
可选地,所述装置还包括:
确定模块,用于根据所述车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量与整车质量确定所述车辆动力学模型。可选地,所述计算模块,包括:
判断单元,用于判断当前道路的类型;
第一确定单元,用于若所述类型为直道类型,则所述目标点为轨迹上距离当前位置最近的点;
第二确定单元,用于若所述类型为弯道类型,则在所述车辆的实际车速大于预设阈值时,所述目标点为相距预瞄距离的点,否则相距由道路曲率确定距离的点。
可选地,所述由道路曲率确定距离的计算公式为:
L=kV+lmin,
其中,k为速度的线性变化比例,V为所述车辆的实际车速,lmin为预瞄距离的最小设定值。
可选地,所述状态矩阵的计算公式为:
其中,e1为所述距离偏差,e2为所述距离偏差变化率,e3为所述航向角偏差,e4为所述角度偏差变化率。
本申请实施例的第三方面是提供一种车辆的横向控制方法,包括:
获取车辆的当前位姿和目标点的位姿,所述目标点为当前时刻所述车辆在跟踪轨迹上对应的轨迹点,所述车辆按照所述跟踪轨迹在当前道路上行驶;
根据所述当前位姿和所述目标点的位姿之间的偏差,计算状态矩阵;
根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取所述LQR算法中的第一加权矩阵和第二加权矩阵,其中所述第一加权矩阵与所述车辆的车辆信息和所述当前道路的道路信息有关;
根据所述LQR算法确定最优矩阵;
基于所述状态矩阵和所述最优矩阵计算转向控制量,控制所述车辆的转向执行器执行所述转向控制量,以对所述车辆进行横向控制。
可选地,所述根据所述当前位姿和所述目标点的位姿之间的偏差,计算状态矩阵,包 括:
计算所述车辆与所述目标点之间的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率;
将所述距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率作为所述状态矩阵中的元素,以得到所述状态矩阵。
可选地,所述第一加权矩阵为对角矩阵,所述对角矩阵包括位于主对角线上的第一元素、第二元素、第三元素和第四元素,所述第一元素、所述第二元素、所述第三元素和所述第四元素分别与所述状态矩阵中的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率对应;
其中,所述第一元素为所述车辆的偏差系数,所述偏差系数为前轮偏角与所述距离偏差的比值,所述第三元素根据所述车辆信息和所述道路信息确定,所述第二元素和所述第四元素均为0。
可选地,所述车辆信息包括当前速度,所述道路信息包括当前道路的曲率半径;
根据所述车辆信息和所述道路信息确定所述第三元素,包括:
当所述当前道路的曲率半径小于第一曲率半径阈值时,确定所述当前道路为直道,按照如下公式计算所述第三元素:
当所述当前道路的曲率半径大于所述第一曲率半径阈值且小于第二曲率半径阈值时,确定所述当前道路为小曲率弯道,按照如下公式计算所述第三元素:
当所述当前道路的曲率半径大于所述第二曲率半径阈值时,确定所述当前道路为大曲率弯道,按照如下公式计算所述第三元素:
其中,q
3是第三元素;q
1是第一元素;V是所述车辆的当前速度,m/s;
所述第一曲率半径阈值是用于区分直道和弯道的边界条件,所述第二曲率半径阈值是用于区分小曲率弯道和大曲率弯道的边界条件。
可选地,所述基于所述状态矩阵和所述最优矩阵计算转向控制量,包括:
将所述状态矩阵和所述最优矩阵相乘,得到所述转向控制量。
可选地,获取所述目标点的位姿之前,所述方法还包括:
判断当前道路的类型;
当所述类型为直道类型时,将所述跟踪轨迹上距离所述车辆最近的轨迹点确定为所述目标点;
当所述类型为弯道类型时,将所述跟踪轨迹上与所述车辆相距瞄准距离的轨迹点确定为所述目标点,所述瞄准距离与所述车辆的当前车速和所述当前道路的曲率有关。
可选地,所述将所述跟踪轨迹上与所述车辆相距瞄准距离的轨迹点确定为所述目标点,包括:
获取所述车辆的当前车速;
根据所述当前车速确定所述瞄准距离,其中,
当所述当前车速小于或等于速度阈值时,按照如下公式计算所述瞄准距离:
当所述当前车速大于所述速度阈值时,按照如下公式计算所述瞄准距离:
其中,L为瞄准距离,m;V为当前车速,m/s;α为调整系数;kp为所述跟踪轨迹上距离所述车辆最近的轨迹点的曲率;r为所述车辆的最小转弯半径,m;a为舒适减速度,m/s
2;
沿所述车辆的行驶方向进行瞄准,将所述跟踪轨迹上与所述车辆相距所述瞄准距离的轨迹点确定为所述目标点。
可选地,所述根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵之前,所述方法还包括:
根据所述车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量和整车质量确定所述车辆动力学模型。
本申请实施例的第四方面是提供了一种车辆的横向控制装置,包括:
获取模块,用于获取车辆的当前位姿和目标点的位姿,所述目标点为当前时刻所述车辆在跟踪轨迹上对应的轨迹点,所述车辆按照所述跟踪轨迹在当前道路上行驶;
计算模块,用于根据所述当前位姿和所述目标点的位姿之间的偏差,计算状态矩阵;
确定模块,用于根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取所述LQR算法中的第一加权矩阵和第二加权矩阵,其中所述第一加权矩阵与所述车辆的车辆信息和所述当前道路的道路信息有关;
所述计算模块,还用于根据所述LQR算法确定最优矩阵,以及基于所述状态矩阵和所述最优矩阵计算转向控制量;
控制模块,用于控制所述车辆的转向执行器执行所述转向控制量,以对所述车辆进行横向控制。
可选地,所述计算模块,还用于:
计算所述车辆与所述目标点之间的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率;以及,
将所述距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率作为所述状态矩阵中的元素,以得到所述状态矩阵。
可选地,所述第一加权矩阵为对角矩阵,所述对角矩阵包括位于主对角线上的第一元素、第二元素、第三元素和第四元素,所述第一元素、所述第二元素、所述第三元素和所述第四元素分别与所述状态矩阵中的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率对应;
其中,所述第一元素为所述车辆的偏差系数,所述偏差系数为前轮偏角与所述距离偏差的比值,所述第三元素根据所述车辆信息和所述道路信息确定,所述第二元素和所述第四元素均为0。
可选地,所述车辆信息包括当前速度,所述道路信息包括当前道路的曲率半径;
所述计算模块,还用于:
当所述当前道路的曲率半径小于第一曲率半径阈值时,确定所述当前道路为直道,按照如下公式计算所述第三元素:
当所述当前道路的曲率半径大于所述第一曲率半径阈值且小于第二曲率半径阈值时,确定所述当前道路为小曲率弯道,按照如下公式计算所述第三元素:
当所述当前道路的曲率半径大于所述第二曲率半径阈值时,确定所述当前道路为大曲率弯道,按照如下公式计算所述第三元素:
其中,q3是第三元素;q1是第一元素;V是所述车辆的当前速度,m/s;
所述第一曲率半径阈值是用于区分直道和弯道的边界条件,所述第二曲率半径阈值是用于区分小曲率弯道和大曲率弯道的边界条件。
可选地,所述计算模块还用于将所述状态矩阵和所述最优矩阵相乘,得到所述转向控制量。
可选地,所述装置还包括判断模块,所述判断模块用于:
判断所述当前道路的类型;
当所述类型为直道类型时,将所述跟踪轨迹上距离所述车辆最近的轨迹点确定为所述目标点;
当所述类型为弯道类型时,将所述跟踪轨迹上与所述车辆相距瞄准距离的轨迹点确定为所述目标点,所述瞄准距离与所述车辆的当前车速和所述当前道路的曲率有关。
可选地,所述获取模块还用于获取所述车辆的当前车速;
所述判断模块,还用于:
根据所述当前车速确定所述瞄准距离,其中,
当所述当前车速小于或等于速度阈值时,按照如下公式计算所述瞄准距离:
当所述当前车速大于所述速度阈值时,按照如下公式计算所述瞄准距离:
其中,L为瞄准距离,m;V为当前车速,m/s;α为调整系数;kp为所述跟踪轨迹上距离所述车辆最近的轨迹点的曲率;r为所述车辆的最小转弯半径,m;a为舒适减速度,m/s
2;
沿所述车辆的行驶方向进行瞄准,将所述跟踪轨迹上与所述车辆相距所述瞄准距离的轨迹点确定为所述目标点。
可选地,所述确定模块,还用于根据所述车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量和整车质量确定所述车辆动力学模型。
本申请实施例的第五方面是提供一种车辆,包括控制器,所述控制器用于执行本申请实施例的第一方面和/或第三方面所述的车辆的横向控制方法。
本申请实施例的第六方面是提供了一种车辆,其包括本申请实施例第二方面和/或第四方面所述的车辆的横向控制装置。
本申请附加的方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本申请的实践了解到。
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解,其中:
图1为本申请实施例提供的第一种车辆的横向控制方法的流程图;
图2为本申请实施例提供的LQR横纵向误差的示例图;
图3为本申请实施例提供的LQR算法的流程图;
图4为本申请实施例提供的优化前不同速度下弯道的跟踪效果的示意图;
图5为本申请实施例提供的优化后不同速度下弯道的跟踪效果的示意图;
图6为本申请的实施例提供的第一种车辆的横向控制装置的方框示例图;
图7为本申请实施例提供的第二种车辆的横向控制方法的流程图;
图8为本申请实施例提供的第三种车辆的横向控制方法的流程图;
图9为本申请的实施例提供的第二种车辆的横向控制装置的方框示例图。
下面详细描述本申请的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本申请,而不能理解为对本申请的限制。
下面参考附图描述本申请实施例的车辆的横向控制方法、装置及车辆。
在介绍本申请实施例的车辆的横向控制方法之前,先简单介绍下相关技术中的处理方式。
相关技术中,LQR算法使用二自由度动力学模型来设计横向控制器,LQR算法的优点在于,通过与转向前馈进行有效结合,能够有效地减小一部分曲线行驶时的稳态跟踪误差,使得车辆在以中等速度曲线行驶时其稳态误差趋近于零,从而极大提升跟踪性能。
但是,针对一些曲率大和高速情况跟踪效果就会明显降低,对环境和参数选择依赖程度高,即当环境突变的情况下不能很好的适应新状态条件下的轨迹跟踪。同时LQR参数调节复杂,其不仅需要获取车辆自身的模型参数,还需要调节LQR目标函数的QR矩阵(包括第一加权矩阵Q和第二加权矩阵R),若是QR矩阵的选取不准确,那么LQR算法的跟 踪性能会大幅降低,从而导致控制失效,而且相关技术中的LQR算法基本采用固定的QR矩阵,导致***自适应能力差,此问题亟待解决。
因此,本申请提供了一种自动驾驶车辆的横向控制方法,在该方法中,可以获取车辆的实际坐标和当前航向角,得到当前位姿和目标点的位置信息,并根据车辆的当前位姿和目标点的位置信息计算当前时刻车辆与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵,并利用车辆动力学模型确定第一模型参数矩阵和第二模型参数矩阵,并同时选取第一加权矩阵和第二加权矩阵,以根据线性二次型调节器LQR算法确定最优矩阵,及控制车辆的转向执行器执行由最优矩阵和状态矩阵相乘得到的转向控制量,从而保证曲率和速度变化快的复杂道路下车辆跟踪的稳定性和舒适性,实现LQR控制器的控制精度和自适应性的提高。
具体而言,图1为本申请实施例所提供的一种自动驾驶车辆的横向控制方法的流程示意图。
如图1所示,该自动驾驶车辆的横向控制方法包括以下步骤:
在步骤S101中,获取车辆的实际坐标和当前航向角,得到当前位姿和目标点的位置信息。
可以理解的是,获取车辆的实际坐标和当前航向角,以及根据实际坐标和当前航向角得到当前位姿和目标点的位置信息的方式可以采用相关技术中的处理方式,为避免冗余,在此不做详细赘述。
在步骤S102中,根据当前位姿和位置信息计算当前时刻与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵。
可选地,在一些实施例中,根据当前位姿和位置信息计算当前时刻与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵,包括:判断当前道路的类型;若类型为直道类型,则目标点为轨迹上距离当前位置最近的点;若类型为弯道类型,则在车辆的实际车速大于预设阈值时,目标点为相距预瞄距离的点,否则相距由道路曲率确定距离的点。
其中,预设阈值可以是用户预先设定的阈值,可以是通过有限次实验获取的阈值,也可以是通过有限次计算机仿真得到的阈值。可选地,预设阈值为60km/h。
示例性地,如图2所示,Vx为车辆纵向车速,Vy为车辆横向车速,ψ为车辆的当前航向角,ψ
des为目标点的航向角,δ为前车轮偏转角度,将车辆方向盘角度和前轮偏角的比值定义为ratio。由图2可知,横纵向误差计算是根据当前时刻点和目标点进行对比得到的,故下文将进行详细阐述如何获取目标点。
具体而言,道路的类型一般可以包括直道类型和弯道类型两种,如果道路的类型为直道类型,则目标点选择为轨迹上距离当前位置最近的点,从而保证直线跟踪的精度;如果道路的类型为弯道类型,则可以根据车辆的实际车速进行判定。例如,当车辆的实际车速大于预设阈值时,速度阈值(即预设阈值)的选取根据轨迹路段最大限速调整,通常选取为限速,选取目标点的预瞄距离L=lmax;再如,当车辆的实际车速小于或等于预设阈值时,可以根据道路曲率确定距离,计算公式可以为:
L=kV+lmin;
其中,k为速度的线性变化比例,V为车辆的实际车速,lmin为预瞄距离的最小设定值。
需要说明的是,lmin选取为车辆的最小转弯半径的二倍,lmax=lmin/2+v*v/2a,a为车辆设定的舒适减速度,通常设定为3,k=α*kp,kp为轨迹上距离当前位置最近的点的曲率,α为可调整系数,可根据实际跟踪情况调整,弯道跟踪入弯早且位置偏差大,可减小α,反之调大。然后根据上述公式代入得到预瞄距离,沿着车辆前进方向进行预瞄,得到对应目标点,当预瞄距离大于到终点的距离时,选择到终点的距离作为预瞄距离。
进一步地,在一些实施例中,e1即为当前时刻到目标点的位移,同时由下列公式计算得到状态矩阵state;
e3=θ-θ1;
e2=Vx*e3+Vy;
得到:
其中,R为目标轨迹点曲率半径,e1为距离偏差,e2为距离偏差变化率,e3为航向角偏差,e4为角度偏差变化率。
在步骤S103中,利用车辆动力学模型确定第一模型参数矩阵和第二模型参数矩阵,并同时选取第一加权矩阵和第二加权矩阵,以根据线性二次型调节器LQR算法确定最优矩阵,及控制车辆的转向执行器执行由最优矩阵和状态矩阵相乘得到的转向控制量。
其中,如图3所示,图3为LQR算法的流程图,主要包括以下步骤:
S301,感知环境和车辆信息。
其中,感知环境和车辆信息包括:车辆坐标和航向角,跟踪轨迹目标点坐标和航向角。
S302,数据处理。
其中,在数据处理之后,将处理后的数据发送至步骤S309。
S303,直道弯道曲率判定,如果是直道,执行步骤S304,如果是弯道,执行步骤S305。
S304,目标点为轨迹上距离当前位置最近的点,并跳转执行步骤S308。
S305,判断辆的实际车速是否大于预设阈值,如果是,执行步骤S306,否则,执行步骤S307。
S306,目标点选取为预瞄距离L=lmax,并跳转执行步骤S308。
S307,目标点选取为预瞄距离L=kV+lmin。
S308,状态量,并跳转执行步骤S310。
其中,根据实时位姿和目标点位置信息计算当前时刻车辆与目标点的距离偏差e1,距离偏差变化率e2,航向偏差e3,角度偏差变化率e4,从而得到状态矩阵state。
S309,QR权重矩阵选择器。
也就是说,本申请实施例可以根据上述的动力学模型参数确定模型参数矩阵A和B,同时选取加权矩阵Q和R(QR权重的选择)。
由此即可得到状态矩阵,并将状态矩阵和QR权重矩阵选择器输入至LQR控制器。
S310,LQR控制器。
S311,车辆转向执行器,并跳转执行步骤S301。
由此,根据上述确定的控制器参数,计算自动驾驶汽车的转向控制量,将其传递给转向执行器执行。
可选地,在一些实施例中,还包括:根据车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量与整车质量确定车辆动力学模型。
也就是说,车辆动力学模型的参数主要包括:前轮侧偏刚度Cf、后轮侧偏刚度Cr、前轴到车辆重心的距离lf、后轴到车辆重心的距离lr、车辆的z轴转动惯量Iz和整车质量m。
需要说明的是,上述车辆动力学模型的参数可以通过查询车辆的基本信息即可获得,也可以重新测量获得,具体地可以由本领域技术人员根据实际情况进行处理,在此不做具体限定。
进一步地,利用车辆动力学模型确定第一模型参数矩阵matrix_a_和第二模型参数矩阵matrix_b_的计算公式可以如下所示:
其中,Cf和Cr为前轮侧偏刚度和后轮侧偏刚度,lf和lr为前后轴到重心的距离,Iz为车辆z轴转动惯量,m为整车质量。
进一步地,选取第一加权矩阵Q和第二加权矩阵R,第一加权矩阵Q,选取对角矩阵matrix_q_=diag[q1,q2,q3,q4],其中,q1、q2、q3和q4这四个参数分别对应状态矩阵state的四个变量,q1和q3的选择即是LQR控制的关键;第二加权矩阵R选择单位矩阵matrix_r_=[1];由上述图3流程图可知,跟踪***通过感知得到环境信息并进行选择:
(1)首先对道路曲率半径R与R1和R2进行判断,R1和R2分别是区分直道和弯道,小曲率和大曲率的边界条件;
(2)当R<R1时判定跟踪曲线为直线,选择q3=kq*q1,kq=0.1*V;
(3)当R1<R<R2时判定跟踪轨迹为小曲率弯道,选择q3=kq*q1,kq=V;
(4)当R>R2时判定跟踪轨迹为大曲率弯道,选择q3=kq*q1,kq=10*V;
(5)根据上述公式只需实车单独直线跟踪确定q1的初始值即可得到第一加权矩阵Q。
进一步地,根据黎卡提方程的数值迭代求解确定最优矩阵matrix_k,由下列公式得到;
while(num_iteration++<max_num_iteration)
{
matrix_p_next=
matrix_a_T*matrix_p_*matrix_a_-(matrix_a_T*matrix_p_*matrix_b_)*(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)+matrix_q_;
matrix_p_=matrix_p_next;
}
matrix_k=(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_ *matrix_a_)
其中,max_num_iterationa为最大迭代次数,示例性地可选取为150,matrix_a_T和matrix_b_T分别为matrix_a_和matrix_b_的转置矩阵,matrix_p为过程迭代矩阵,初始值为Q矩阵。
进一步地,根据最优矩阵matrix_k和状态矩阵state相乘得到前轮转角最后乘以ratio输出到执行机构,实现跟踪。
如图7所示,本申请实施例还提供了一种车辆的横向控制方法,该方法能够应用于自动驾驶汽车,包括全自动驾驶汽车和半自动驾驶汽车,该横向控制方法包括步骤S701-S706:
S701、获取车辆的当前位姿和目标点的位姿。
目标点为当前时刻车辆在跟踪轨迹上对应的轨迹点,车辆按照跟踪轨迹在当前道路上行驶。其中,位姿也可以称为位置信息,其可以包括坐标信息和方向信息。
S702、根据当前位姿和目标点的位姿之间的偏差,计算状态矩阵。
其中,当前位姿和目标点的位姿之间的偏差,至少包括车辆的实际坐标和目标点的坐标之间的距离偏差,以及当前航向角和目标航向角之间的航向角偏差。通常来讲,距离偏差是指车辆的实际坐标和目标点的坐标之间的绝对距离偏差,但是在一些实施例中,距离偏差也可以是指车辆的实际坐标和目标点的坐标之间的横向距离,横向距离是指在车身宽度方向上的距离。示例性地,横向距离可以通过将绝对距离在车身宽度方向上进行正投影而得到。在本申请的另一些实施例中,当前位姿和目标点的位姿之间的偏差还可以包括距离偏差变化率和角度偏差变化率。
S703、根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取LQR算法中的第一加权矩阵和第二加权矩阵。
其中,使用LQR算法时所用到的第一加权矩阵Q和第二加权矩阵R中,第一加权矩阵Q为状态权重矩阵,该矩阵的选取条件与车辆的车辆信息和当前道路的道路信息有关;第二加权矩阵R为控制权重矩阵。
在本申请实施例中,根据实际情况及需求,可以在确定第一模型参数矩阵和第二模型参数矩阵之后选取第一加权矩阵Q和第二加权矩阵R,也可以在确定第一模型参数矩阵和第二模型参数矩阵之前选取第一加权矩阵Q和第二加权矩阵R,还可以在确定第一模型参数矩阵和第二模型参数矩阵的同时选取第一加权矩阵Q和第二加权矩阵R。
S704、根据LQR算法确定最优矩阵。
其中,可以通过将第一模型参数矩阵和第二模型参数矩阵代入黎卡提方程进行迭代求 解,得到最优矩阵。
需要说明的是,在本申请实施例中,既可以先执行步骤S701-S702,然后再执行步骤S703-S704;也可以先执行步骤S701-S702,然后再执行步骤S703-S704;还可以同时执行步骤S701-S702和步骤S703-S704。
S705、基于状态矩阵和最优矩阵计算转向控制量。
S706、控制车辆的转向执行器执行转向控制量,以对车辆进行横向控制。
其中,步骤S705可以通过车辆的控制器或者其它具有计算功能的单元执行,在得到转向控制量之后,可以向车辆的转向执行器发送转向指令,从而接收到转向指令的转向执行器可以按照转向控制量控制转向,实现对车辆的横向控制。
综上所述,在本申请实施例中,通过获取车辆的当前位姿和目标点的位姿,并根据车辆的当前位姿和目标点的位姿之间的偏差计算状态矩阵;利用车辆动力学模型确定LQR算法中国的第一模型参数矩阵和第二模型参数矩阵,并选取LQR算法的第一加权矩阵和第二加权矩阵;之后根据LQR算法对第一模型参数矩阵和第二模型参数矩阵进行处理,得到最优矩阵;基于最优矩阵与状态矩阵进行计算,即可得到转向控制量;通过控制车辆的转向执行器执行该转向控制量,即可实现对车辆的横向控制,从而保证了车辆在曲率变化大的复杂道路上和速度变化快的行驶场景下跟踪的稳定性和舒适性,实现了LQR控制器的控制精度和自适应性的提高。
参见图8,本申请实施例还提供了一种自动驾驶车辆的横向控制方法,该方法能够应用于自动驾驶汽车,包括全自动驾驶汽车和半自动驾驶汽车。下面以自动驾驶汽车为例,对该横向控制方法进行描述。该横向控制方法包括步骤S801-S808:
S801、判断当前道路的类型,确定目标点;
自动驾驶汽车会预先或者实时规划出行驶路线,然后按照规划出的行驶路线在当前道路上行驶,该行驶路线即为跟踪轨迹。当需要自动驾驶汽车执行横向控制时,会根据当前道路的类型,确定目标点。其中,目标点为当前时刻车辆在跟踪轨迹上对应的轨迹点。
在本申请的实施例中,步骤S801进一步可以包括:
当当前道路的类型为直道类型时,将跟踪轨迹上距离车辆最近的轨迹点确定为目标点。
在一些实施例中,跟踪轨迹上距离车辆最近的轨迹点可以是在垂向上与车辆相距最近的点,也可以是横向上(即车梁的宽度方向上)与车辆相距最近的点。
当当前道路的类型为弯道类型,将跟踪轨迹上与车辆相距瞄准距离的轨迹点确定为目标点,瞄准距离与车辆的当前车速和当前道路的曲率有关。
其中,当当前道路类型为弯道类型时,确定目标点的方法进一步可以包括:
S8011、获取车辆的当前车速;
S8012、根据当前车速确定瞄准距离,其中,
当当前车速小于或等于速度阈值时,按如下公式计算瞄准距离:
当当前车速大于速度阈值时,则按如下公式计算瞄准距离:
其中,L为瞄准距离,m;V为当前车速,m/s;α为调整系数;kp为跟踪轨迹上距离车辆最近的轨迹点的曲率;r为车辆的最小转弯半径,m;a为舒适减速度,m/s
2;
S8013、沿车辆的行驶方向进行瞄准,将跟踪轨迹上与车辆相距瞄准距离的轨迹点确定为目标点。
在确定目标点之后,执行步骤S802。
S802、获取车辆的当前位姿和目标点的位姿。
车辆的当前位姿包括车辆在当前时刻的实际坐标和当前航向角,目标点的位姿包括目标点的目标坐标和目标航向角。获取车辆的当前位姿和目标点的位姿的方式可以采用相关技术中的任意获取方式,为避免冗余,在此不再赘述。
S803、根据当前位姿和目标点的位姿之间的偏差,计算状态矩阵。
其中,当前位姿和目标点的位姿之间的偏差,至少包括车辆的实际坐标和目标坐标之间的距离偏差,以及当前航向角和目标航向角之间的航向角偏差,还可以包括距离偏差变化率和航向角的角度偏差变化率。
在本申请的一些实施例中,步骤S803进一步可以包括:
步骤S8031、计算车辆与目标点之间的距离偏差e1、距离偏差变化率e2、航向角偏差e3和角度偏差变化率e4。
如图2所示,跟踪轨迹上具有目标点,车辆和目标点之间的距离偏差(一般是指车辆的质心和目标点之间的距离偏差)为e1,车辆的当前航向角为ψ,目标点的目标航向角为ψ
des,航向角偏差e3=ψ
des-ψ。
步骤S8032、将距离偏差e1、距离偏差变化率e2、航向角偏差e3和角度偏差变化率e4作为状态矩阵中的元素,以得到所述状态矩阵。
因此,根据上述元素得到的状态矩阵state(1,0)=e1,state(2,0)=e2,state(3,0)=e3,state (4,0)=e4,即
步骤S804、确定车辆动力学模型。
其中,车辆动力学模型可以根据车辆的前轮侧偏刚度Cf、后轮侧偏刚度Cr、前轴到车辆重心的距离lf、后轴到车辆重心的距离lr、车辆的z轴转动惯量Iz和整车质量m等参数进行确定。这些参数可以通过查询车辆的基本信息获得,也可以通过测量获得,具体地可以由本领域技术人员根据实际情况进行处理,在此不做具体限定。
步骤S805、根据车辆动力学模型确定LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取第一加权矩阵和第二加权矩阵。
在本申请实施例中,根据车辆动力学模型确定的第一模型参数矩阵matrix_a_和第二模型参数矩阵matrix_b_的计算公式可以如下所示:
其中,Cf为前轮侧偏刚度,Cr为后轮侧偏刚度,lf为前轴到车辆重心的距离,lr为后轴到车辆重心的距离,Iz为车辆的z轴转动惯量,m为整车质量。
在使用LQR算法进行计算之前,还需要选取第一加权矩阵(通常称为Q矩阵)和第二加权矩阵(通常称作R矩阵),其中,第一加权矩阵与车辆的车辆信息和当前道路的道路信息有关。
在本申请的一些实施例中,第一加权矩阵可以选取对角矩阵,即
matrix_q_=diag[q1 q2 q3 q4];
位于对角矩阵的主对角线上的第一元素q1、第二元素q2、第三元素q3和第四元素q4,分别与状态矩阵中的距离偏差e1、距离偏差变化率e2、航向角偏差e3和角度偏差变化率e4对 应。
在本申请实施例中,第一元素q1和第三元素q3的选择即是LQR控制算法的关键。其中,第一加权矩阵中的第一元素q1为车辆的偏差系数,偏差系数为前轮偏角与距离偏差的比值,用于调整状态矩阵中的距离偏差e1的状态权重。车辆的偏差系数可以根据实车单独直线跟踪确定,例如在自动驾驶汽车出厂之前,由技术人员多次测试车辆沿直线跟踪轨迹行驶时所产生的偏差,并在测试期间实时采集前轮偏角数据和距离偏差数据,在对所采集的数据进行处理后得到前轮偏角与距离偏差的比值,即偏差系数,该偏差系数可以预先存储在车辆中。第一加权矩阵中的第三元素q3根据车辆信息和道路信息确定,第二元素q2和第四元素q4均为0。在本申请的一些实施例中,上述的“距离偏差”一般可以认为是横向距离偏差,即在平行于车辆的宽度方向上所产生的距离偏差。
因此,第一加权矩阵matrix_q_可以为:
在本申请的一些实施例中,车辆信息可以包括当前速度,道路信息可以包括当前道路的曲率半径。根据车辆信息和道路信息确定第三元素q3的方法,可以是将当前道路的曲率半径与第一曲率半径阈值和第二曲率半径阈值进行比较,其中第一曲率半径阈值是用于区分直道和弯道的边界条件,第二曲率半径阈值是用于区分小曲率弯道和大曲率弯道的边界条件。比较之后可能出现以下三种可能的结果之一:
(1)当当前道路的曲率半径小于第一曲率半径阈值时,确定跟踪轨迹为直道,按照如下公式计算第三元素q
3:
(2)当当前道路的曲率半径大于第一曲率半径阈值且小于第二曲率半径阈值时,确定跟踪轨迹为小曲率弯道,按如下公式计算第三元素q3:
(3)当当前道路的曲率半径大于第二曲率半径阈值时,确定跟踪轨迹为大曲率弯道,按如下公式计算第三元素q3:
在上述公式中,q1是第一元素;V是车辆的当前速度,m/s。
基于上述方法,可以得到第一加权矩阵。
在本申请的一些实施例中,第二加权矩阵可以选取单位矩阵,即
matrix_r_=[1];
其中,matrix_r_为第二加权矩阵。
S806、根据线性二次型调节器LQR算法确定最优矩阵。
在选取第一加权矩阵和第二加权矩阵之后,根据线性二次型调节器LQR算法对第一模型参数矩阵和第二模型参数矩阵进行计算,得到最优矩阵matrix_k,具体可以包括:
根据下列公式,由黎卡提方程的数值迭代求解确定最优矩阵matrix_k:
while(num_iteration++<max_num_iteration)
{
matrix_p_next=matrix_a_T*matrix_p_*matrix_a_-(matrix_a_T*matrix_p_*matrix_b_)*(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)+matrix_q_;
matrix_p_=matrix_p_next;
}
matrix_k=(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)
其中,max_num_iterationa为最大迭代次数,可选地,max_num_iterationa可以为150;matrix_a_T和matrix_b_T分别为matrix_a_和matrix_b_的转置矩阵;matrix_p_为过程迭代矩阵,初始值为第一加权矩阵。
基于上述迭代运算,可以得到最优矩阵matrix_k。
步骤S807、基于状态矩阵和最优矩阵计算转向控制量。
在本申请的一些实施例中,在得到状态矩阵和最优矩阵之后,可以通过将状态矩阵和最优矩阵相乘,得到转向控制量。
步骤S808、控制车辆的转向执行器执行转向控制量,以对车辆进行横向控制。
需要说明的是,步骤S808中得到的转向控制量为前车轮的转向控制量,若是需要计算方向盘的转向控制量,还需要在此基础上进一步计算。例如,可以将前车轮的转向控制量乘以ratio的值,得到方向盘的转向控制量。其中,ratio为车辆的方向盘角度和前车轮角度的比值,该比值可以通过查阅车辆基本信息得到,也可以通过实车测试得到,还可以预存在车辆中。
示例性地,加入初始距离偏差0.5m的干扰(模拟环境突变情况)后,车辆按照跟踪轨迹在同一弯道以不同速度行驶的仿真对比验证结果如图4和图5所示,其中图4为优化前 的跟踪误差示意图,图4中的线条1为V=20km/h的情况,线条2为V=30km/h的情况,线条3为V=40km/h的情况,线条4为V=50km/h的情况,线条5为V=60km/h的情况;图5为优化后的跟踪误差示意图,图5中的线条6为V=20km/h的情况,线条7为V=30km/h的情况,线条8为V=40km/h的情况,线条9为V=50km/h的情况,线条10为V=60km/h的情况,显然,得到优化过后的车辆在按照跟踪轨迹行驶时,在受到干扰后所产生的误差波动更小,因此能够更快的回复到稳定状态,提高了车辆的舒适性和稳定性。
由此,通过使用本申请实施例提的横向控制方法对车辆的横向控制性能进行优化,保证了车辆在速度变化快的情况下以及曲率变化大的复杂道路上进行轨迹跟踪的稳定性和舒适性;同时本申请实施例总结出了自适应公式来解决LQR算法中的Q矩阵和R矩阵在选取时所引起的误差较大的问题,并且通过仿真对比验证试验的结果可以看出,在采用本申请实施例所提供的自适应公式后,车辆的跟踪效果有了明显的提升。
因此,根据本申请实施例提出的车辆的横向控制方法,可以获取车辆的实际坐标和当前航向角,得到车辆的当前位姿,并获取目标点的位姿;之后根据车辆的当前位姿和目标点的位姿,计算当前时刻车辆与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,得到状态矩阵;利用车辆动力学模型确定LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取LQR算法中的第一加权矩阵和第二加权矩阵,以根据线性二次型调节器LQR算法确定最优矩阵,最后控制车辆的转向执行器执行由最优矩阵和状态矩阵相乘得到的转向控制量,从而保证车辆在曲率变化大的复杂道路上和速度变化快的行驶场景下进行跟踪的稳定性和舒适性,实现LQR控制器的控制精度和自适应性的提高。
下面,参照附图描述根据本申请实施例提出的自动驾驶车辆的横向控制装置。
图6是本申请实施例提供的一种自动驾驶车辆的横向控制装置的方框示意图。
如图6所示,该自动驾驶车辆的横向控制装置10包括:获取模块100、计算模块200和控制模块300。
其中,获取模块100用于获取车辆的实际坐标和当前航向角,得到当前位姿和目标点的位置信息;
计算模块200用于根据当前位姿和位置信息计算当前时刻与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵;以及
控制模块300用于利用车辆动力学模型确定第一模型参数矩阵和第二模型参数矩阵,并同时选取第一加权矩阵和第二加权矩阵,以根据线性二次型调节器LQR算法确定最优矩阵,及控制车辆的转向执行器执行由最优矩阵和状态矩阵相乘得到的转向控制量。
可选地,在一些实施例中,上述的自动驾驶车辆的横向控制装置10,还包括:
确定模块,用于根据车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量与整车质量确定车辆动力学模型。
可选地,在一些实施例中,计算模块200包括:
判断单元,用于判断当前道路的类型;
第一确定单元,用于在判断出当前道路的类型为直道类型时,将轨迹上距离车辆的当前位置(即车辆坐标)最近的点确定为目标点;
第二确定单元,用于在判断出当前道路的类型为弯道类型时,若车辆的实际车速大于预设阈值,将与车辆的当前位置相距预瞄距离的轨迹点确定为目标点,否则将与车辆的当前位置相距由道路曲率确定距离的轨迹点确定为目标点。
可选地,在一些实施例中,由道路曲率确定距离的计算公式为:
L=kV+lmin,
其中,k为速度的线性变化比例,V为车辆的实际车速,lmin为预瞄距离的最小设定值。
可选地,在一些实施例中,状态矩阵的计算公式为:
其中,e1为距离偏差,e2为距离偏差变化率,e3为航向角偏差,e4为角度偏差变化率。
需要说明的是,前述对自动驾驶车辆的横向控制方法实施例的解释说明也适用于该实施例的自动驾驶车辆的横向控制装置,此处不再赘述。
根据本申请实施例提出的自动驾驶车辆的横向控制装置,可以获取车辆的实际坐标和当前航向角,得到当前位姿和目标点的位置信,并根据当前位姿和位置信息计算当前时刻与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵,并利用车辆动力学模型确定第一模型参数矩阵和第二模型参数矩阵,并同时选取第一加权矩阵和第二加权矩阵,以根据线性二次型调节器LQR算法确定最优矩阵,及控制车辆的转向执行器执行由最优矩阵和状态矩阵相乘得到的转向控制量,从而保证曲率和速度变化快的复杂道路下车辆跟踪的稳定性和舒适性,实现LQR控制器的控制精度和自适应性的提高。
图9是本申请实施例提供的另一种车辆的横向控制装置的示意图,该装置可以应用于自动驾驶车辆,包括全自动驾驶车辆和半自动驾驶车辆。如图9所示,该横向控制装置90 包括:
获取模块901,被配置为获取车辆的当前位姿和目标点的位姿,其中目标点为当前时刻所述车辆在跟踪轨迹上对应的轨迹点,车辆按照跟踪轨迹在当前道路上行驶。
计算模块902,被配置为根据当前位姿和目标点的位姿之间的偏差,计算状态矩阵。
确定模块903,被配置为根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取LQR算法中的第一加权矩阵和第二加权矩阵,其中第一加权矩阵与车辆的车辆信息和当前道路的道路信息有关。
其中,车辆信息可以包括当前速度,道路信息可以包括当前道路的曲率半径。
计算模块902,还被配置为根据LQR算法确定最优矩阵,以及基于状态矩阵和最优矩阵得到转向控制量。
控制模块904,被配置为控制车辆的转向执行器执行该转向控制量,以对车辆进行横向控制。
在本申请的一些实施例中,计算模块902还被配置为:
计算车辆与目标点之间的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率;以及将距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率作为状态矩阵中的元素。
在本申请的一些实施例中,第一加权矩阵为对角矩阵,对角矩阵包括位于主对角线上的第一元素、第二元素、第三元素和第四元素,第一元素、第二元素、第三元素和第四元素分别与状态矩阵中的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率对应。其中,第一元素为车辆的偏差系数,偏差系数为前轮偏角与距离偏差的比值,第三元素根据车辆信息和道路信息确定,第二元素和第四元素均为0。
确定模块903还被配置为根据车辆信息和道路信息确定第三元素,其中,
当当前道路的曲率半径小于第一曲率半径阈值时,确定跟踪轨迹为直道,按照如下公式计算第三元素:
当当前道路的曲率半径大于第一曲率半径阈值且小于第二曲率半径阈值时,确定跟踪轨迹为小曲率弯道,按如下公式计算第三元素:
当当前道路的曲率半径大于第二曲率半径阈值时,确定跟踪轨迹为大曲率弯道,按如下公式计算第三元素:
其中,q3是第三元素;q1是第一元素;V是车辆的当前速度,m/s;第一曲率半径阈值是用于区分直道和弯道的边界条件,第二曲率半径阈值是用于区分小曲率弯道和大曲率弯道的边界条件。
在本申请的一些实施例中,计算模块902还被配置为将状态矩阵和最优矩阵相乘,得到转向控制量。
在本申请的一些实施例中,该横向控制装置90还可以包括:
判断模块905,被配置为判断当前道路的类型;
当当前道路的类型为直道类型时,将跟踪轨迹上距离车辆最近的轨迹点确定为目标点;
当当前道路的类型为弯道类型时,将跟踪轨迹上与车辆相距瞄准距离的轨迹点确定为目标点,瞄准距离与车辆的当前车速和当前道路的曲率有关。
在本申请的一些实施例中,获取模块还用于获取所述车辆的当前车速;
根据当前车速确定瞄准距离,其中,
若当前车速小于或等于速度阈值,则按如下公式计算瞄准距离:
其中,L为瞄准距离,m;V为当前车速,m/s;α为调整系数;kp为跟踪轨迹上距离车辆最近的轨迹点的曲率;r为车辆的最小转弯半径,m;
若当前车速大于速度阈值,则按如下公式计算瞄准距离:
其中,a为舒适减速度,m/s
2;
沿车辆的行驶方向进行瞄准,将跟踪轨迹上与车辆相距瞄准距离的轨迹点确定为目标点。
在本申请的一些实施例中,确定模块903还被配置为根据车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量和整车质量确定车辆动力学模型。
此外,本申请实施例还提供了一种车辆,该车辆包括上述的任一实施例中所提到的车辆的横向控制装置。
本申请实施例还提供了另一种车辆,该车辆包括控制器,该控制器用于执行上述任一实施例所述的车辆的横向控制方法。通过执行上述横向控制方法,保证了车辆在曲率变化 大的复杂道路上和速度变化快的行驶场景下跟踪的稳定性和舒适性,实现了LQR控制器的控制精度和自适应性的提高。
根据本申请实施例提出的车辆,通过上述的车辆的横向控制装置,可以获取车辆的实际坐标和当前航向角,得到当前位姿和目标点的位姿,并根据当前位姿和目标点的位姿计算当前时刻车辆与目标点的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率,计算状态矩阵,并利用车辆动力学模型确定第一模型参数矩阵和第二模型参数矩阵,并选取第一加权矩阵和第二加权矩阵,以根据线性二次型调节器LQR算法确定最优矩阵,之后控制车辆的转向执行器执行由最优矩阵和状态矩阵相乘得到的转向控制量,从而保证曲率变化大的复杂道路上和速度变化快的行驶场景下车辆跟踪的稳定性和舒适性,实现LQR控制器的控制精度和自适应性的提高。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或N个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中,“N个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更N个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本申请的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本申请的实施例所属技术领域的技术人员所理解。
应当理解,本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,N个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。如,如果用硬件来实现和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离 散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,该程序在执行时,包括方法实施例的步骤之一或其组合。
Claims (18)
- 一种车辆的横向控制方法,包括以下步骤:获取车辆的当前位姿和目标点的位姿,所述目标点为当前时刻所述车辆在跟踪轨迹上对应的轨迹点,所述车辆按照所述跟踪轨迹在当前道路上行驶;根据所述当前位姿和所述目标点的位姿之间的偏差,计算状态矩阵;根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取所述LQR算法中的第一加权矩阵和第二加权矩阵,其中所述第一加权矩阵与所述车辆的车辆信息和所述当前道路的道路信息有关;根据所述LQR算法确定最优矩阵;基于所述状态矩阵和所述最优矩阵计算转向控制量,控制所述车辆的转向执行器执行所述转向控制量,以对所述车辆进行横向控制。
- 根据权利要求1所述的方法,所述根据所述当前位姿和所述目标点的位姿之间的偏差,计算状态矩阵,包括:计算所述车辆与所述目标点之间的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率;将所述距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率作为所述状态矩阵中的元素,以得到所述状态矩阵。
- 根据权利要求2所述的方法,所述第一加权矩阵为对角矩阵,所述对角矩阵包括位于主对角线上的第一元素、第二元素、第三元素和第四元素,所述第一元素、所述第二元素、所述第三元素和所述第四元素分别与所述状态矩阵中的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率对应;其中,所述第一元素为所述车辆的偏差系数,所述偏差系数为前轮偏角与所述距离偏差的比值,所述第三元素根据所述车辆信息和所述道路信息确定,所述第二元素和所述第四元素均为0。
- 根据权利要求3所述的方法,所述车辆信息包括当前速度,所述道路信息包括当前道路的曲率半径;根据所述车辆信息和所述道路信息确定所述第三元素,包括:当所述当前道路的曲率半径小于第一曲率半径阈值时,确定所述当前道路为直道,按照如下公式计算所述第三元素:当所述当前道路的曲率半径大于所述第一曲率半径阈值且小于第二曲率半径阈值时,确定所述当前道路为小曲率弯道,按照如下公式计算所述第三元素:当所述当前道路的曲率半径大于所述第二曲率半径阈值时,确定所述当前道路为大曲率弯道,按照如下公式计算所述第三元素:其中,q3是第三元素;q1是第一元素;V是所述车辆的当前速度,m/s;所述第一曲率半径阈值是用于区分直道和弯道的边界条件,所述第二曲率半径阈值是用于区分小曲率弯道和大曲率弯道的边界条件。
- 根据权利要求1所述的方法,所述基于所述状态矩阵和所述最优矩阵计算转向控制量,包括:将所述状态矩阵和所述最优矩阵相乘,得到所述转向控制量。
- 根据权利要求1所述的方法,获取所述目标点的位姿之前,所述方法还包括:判断所述当前道路的类型;当所述类型为直道类型时,将所述跟踪轨迹上距离所述车辆最近的轨迹点确定为所述目标点;当所述类型为弯道类型时,将所述跟踪轨迹上与所述车辆相距瞄准距离的轨迹点确定为所述目标点,所述瞄准距离与所述车辆的当前车速和所述当前道路的曲率有关。
- 根据权利要求1所述的方法,所述根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵之前,所述方法还包括:根据所述车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量和整车质量确定所述车辆动力学模型。
- 一种车辆的横向控制装置,包括:获取模块,用于获取车辆的当前位姿和目标点的位姿,所述目标点为当前时刻所述车辆在跟踪轨迹上对应的轨迹点,所述车辆按照所述跟踪轨迹在当前道路上行驶;计算模块,用于根据所述当前位姿和所述目标点的位姿之间的偏差,计算状态矩阵;确定模块,用于根据车辆动力学模型确定线性二次型调节器LQR算法中的第一模型参数矩阵和第二模型参数矩阵,并选取所述LQR算法中的第一加权矩阵和第二加权矩阵,其中所述第一加权矩阵与所述车辆的车辆信息和所述当前道路的道路信息有关;所述计算模块,还用于根据所述LQR算法确定最优矩阵,以及基于所述状态矩阵和所述最优矩阵计算转向控制量;控制模块,用于控制所述车辆的转向执行器执行所述转向控制量,以对所述车辆进行横向控制。
- 根据权利要求9所述的装置,所述计算模块,还用于:计算所述车辆与所述目标点之间的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率;以及,将所述距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率作为所述状态矩阵中的元素,以得到所述状态矩阵。
- 根据权利要求10所述的装置,所述第一加权矩阵为对角矩阵,所述对角矩阵包括位于主对角线上的第一元素、第二元素、第三元素和第四元素,所述第一元素、所述第二元素、所述第三元素和所述第四元素分别与所述状态矩阵中的距离偏差、距离偏差变化率、航向角偏差和角度偏差变化率对应;其中,所述第一元素为所述车辆的偏差系数,所述偏差系数为前轮偏角与所述距离偏差的比值,所述第三元素根据所述车辆信息和所述道路信息确定,所述第二元素和所述第四元素均为0。
- 根据权利要求11所述的装置,所述车辆信息包括当前速度,所述道路信息包括当前道路的曲率半径;所述计算模块,还用于根据车辆信息和道路信息确定第三元素,其中,当所述当前道路的曲率半径小于第一曲率半径阈值时,确定所述当前道路为直道,按照如下公式计算所述第三元素:当所述当前道路的曲率半径大于所述第一曲率半径阈值且小于第二曲率半径阈值时,确定所述当前道路为小曲率弯道,按照如下公式计算所述第三元素:当所述当前道路的曲率半径大于所述第二曲率半径阈值时,确定所述当前道路为大曲率弯道,按照如下公式计算所述第三元素:其中,q3是第三元素;q1是第一元素;V是所述车辆的当前速度,m/s;所述第一曲率半径阈值是用于区分直道和弯道的边界条件,所述第二曲率半径阈值是用于区分小曲率弯道和大曲率弯道的边界条件。
- 根据权利要求9所述的装置,所述计算模块还用于将所述状态矩阵和所述最优矩阵相乘,得到所述转向控制量。
- 根据权利要求9所述的装置,还包括判断模块,所述判断模块用于:判断所述当前道路的类型;当所述类型为直道类型时,将所述跟踪轨迹上距离所述车辆最近的轨迹点确定为所述目标点;当所述类型为弯道类型时,将所述跟踪轨迹上与所述车辆相距瞄准距离的轨迹点确定为所述目标点,所述瞄准距离与所述车辆的当前车速和所述当前道路的曲率有关。
- 根据权利要求9所述的装置,所述确定模块还用于根据所述车辆的前轮侧偏刚度、后轮侧偏刚度、前轴到车辆重心的距离、后轴到车辆重心的距离、车辆的z轴转动惯量和整车质量确定所述车辆动力学模型。
- 一种车辆,包括控制器,所述控制器用于执行如权利要求1-8任一项所述的车辆的横向控制方法。
- 一种车辆,包括如权利要求9-16任一项所述的车辆的横向控制装置。
Applications Claiming Priority (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110510779.1A CN113120001A (zh) | 2021-05-11 | 2021-05-11 | 自动驾驶车辆的横向控制方法、装置及车辆 |
CN202110510779.1 | 2021-05-11 | ||
CN202210210929.1A CN114655248A (zh) | 2021-05-11 | 2022-03-04 | 自动驾驶车辆的横向控制方法、装置及车辆 |
CN202210210929.1 | 2022-03-04 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2022237392A1 true WO2022237392A1 (zh) | 2022-11-17 |
Family
ID=76781662
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/CN2022/085370 WO2022237392A1 (zh) | 2021-05-11 | 2022-04-06 | 车辆的横向控制方法、装置及车辆 |
Country Status (2)
Country | Link |
---|---|
CN (2) | CN113120001A (zh) |
WO (1) | WO2022237392A1 (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113608530A (zh) * | 2021-07-22 | 2021-11-05 | 武汉理工大学 | 一种带有pid转角补偿的参数自整定lqr路径跟踪方法 |
CN113632033A (zh) * | 2020-02-25 | 2021-11-09 | 华为技术有限公司 | 车辆控制方法及装置 |
CN115675637A (zh) * | 2022-12-28 | 2023-02-03 | 禾多科技(北京)有限公司 | 车辆控制方法、装置、电子设备和计算机可读介质 |
CN116118751A (zh) * | 2023-04-19 | 2023-05-16 | 深圳佑驾创新科技有限公司 | 车辆的控制方法、装置、车辆和存储介质 |
CN117369463A (zh) * | 2023-10-31 | 2024-01-09 | 云创智行科技(苏州)有限公司 | 参数优化方法、装置、电子设备及存储介质 |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113120001A (zh) * | 2021-05-11 | 2021-07-16 | 雄狮汽车科技(南京)有限公司 | 自动驾驶车辆的横向控制方法、装置及车辆 |
CN113771868B (zh) * | 2021-08-11 | 2023-03-07 | 佛山仙湖实验室 | 一种无人驾驶汽车的横向控制方法 |
CN113753080B (zh) * | 2021-08-31 | 2023-09-26 | 的卢技术有限公司 | 一种自动驾驶汽车横向运动自适应参数控制方法 |
CN113741450B (zh) * | 2021-08-31 | 2023-11-21 | 的卢技术有限公司 | 一种车辆自动驾驶横向自适应控制方法 |
CN114001976B (zh) * | 2021-10-19 | 2024-03-12 | 杭州飞步科技有限公司 | 控制误差的确定方法、装置、设备及存储介质 |
CN114275039B (zh) * | 2021-12-27 | 2022-11-04 | 联创汽车电子有限公司 | 智能驾驶车辆横向控制方法和模块 |
CN114312847B (zh) * | 2022-01-13 | 2024-04-12 | 安徽江淮汽车集团股份有限公司 | 一种自动驾驶车辆的横向控制方法及装置 |
CN115071732A (zh) * | 2022-07-14 | 2022-09-20 | 东风商用车有限公司 | 一种基于lqr的smc商用车智能驾驶横向控制方法 |
CN116578100B (zh) * | 2023-07-12 | 2023-11-21 | 城市之光(深圳)无人驾驶有限公司 | 一种基于扫刷误差的二自由度动力模型沿边清扫控制方法 |
CN116749950B (zh) * | 2023-07-25 | 2024-01-26 | 云创智行科技(苏州)有限公司 | 一种自适应转向判定方法及装置 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104571112A (zh) * | 2015-01-14 | 2015-04-29 | 中国科学院合肥物质科学研究院 | 基于转弯曲率估计的无人车横向控制方法 |
CN104670233A (zh) * | 2013-11-28 | 2015-06-03 | 现代摩比斯株式会社 | 车辆转弯控制方法及其装置 |
US20170101094A1 (en) * | 2015-10-13 | 2017-04-13 | Magna Electronics Inc. | Vehicle vision system with lateral control algorithm for lane keeping |
CN107015477A (zh) * | 2017-04-05 | 2017-08-04 | 同济大学 | 基于状态反馈的车辆路径跟踪h∞控制方法 |
CN108791289A (zh) * | 2018-04-28 | 2018-11-13 | 华为技术有限公司 | 一种车辆控制方法和装置 |
CN110568758A (zh) * | 2019-09-12 | 2019-12-13 | 中汽研(天津)汽车工程研究院有限公司 | 一种自动驾驶汽车的参数自适应横向运动lqr控制方法 |
CN111638712A (zh) * | 2020-05-26 | 2020-09-08 | 三一专用汽车有限责任公司 | 自动驾驶车辆横向运动控制方法、装置和自动驾驶车辆 |
CN113120001A (zh) * | 2021-05-11 | 2021-07-16 | 雄狮汽车科技(南京)有限公司 | 自动驾驶车辆的横向控制方法、装置及车辆 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE19812237C1 (de) * | 1998-03-20 | 1999-09-23 | Daimler Chrysler Ag | Verfahren zur Fahrdynamik-Regelung an einem Straßenfahrzeug |
CN111806437B (zh) * | 2020-09-10 | 2021-01-15 | 中汽研(天津)汽车工程研究院有限公司 | 自动驾驶汽车预瞄点确定方法、装置、设备及存储介质 |
-
2021
- 2021-05-11 CN CN202110510779.1A patent/CN113120001A/zh not_active Withdrawn
-
2022
- 2022-03-04 CN CN202210210929.1A patent/CN114655248A/zh active Pending
- 2022-04-06 WO PCT/CN2022/085370 patent/WO2022237392A1/zh active Application Filing
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104670233A (zh) * | 2013-11-28 | 2015-06-03 | 现代摩比斯株式会社 | 车辆转弯控制方法及其装置 |
CN104571112A (zh) * | 2015-01-14 | 2015-04-29 | 中国科学院合肥物质科学研究院 | 基于转弯曲率估计的无人车横向控制方法 |
US20170101094A1 (en) * | 2015-10-13 | 2017-04-13 | Magna Electronics Inc. | Vehicle vision system with lateral control algorithm for lane keeping |
CN107015477A (zh) * | 2017-04-05 | 2017-08-04 | 同济大学 | 基于状态反馈的车辆路径跟踪h∞控制方法 |
CN108791289A (zh) * | 2018-04-28 | 2018-11-13 | 华为技术有限公司 | 一种车辆控制方法和装置 |
CN110568758A (zh) * | 2019-09-12 | 2019-12-13 | 中汽研(天津)汽车工程研究院有限公司 | 一种自动驾驶汽车的参数自适应横向运动lqr控制方法 |
CN111638712A (zh) * | 2020-05-26 | 2020-09-08 | 三一专用汽车有限责任公司 | 自动驾驶车辆横向运动控制方法、装置和自动驾驶车辆 |
CN113120001A (zh) * | 2021-05-11 | 2021-07-16 | 雄狮汽车科技(南京)有限公司 | 自动驾驶车辆的横向控制方法、装置及车辆 |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113632033A (zh) * | 2020-02-25 | 2021-11-09 | 华为技术有限公司 | 车辆控制方法及装置 |
CN113632033B (zh) * | 2020-02-25 | 2023-10-20 | 华为技术有限公司 | 车辆控制方法及装置 |
CN113608530A (zh) * | 2021-07-22 | 2021-11-05 | 武汉理工大学 | 一种带有pid转角补偿的参数自整定lqr路径跟踪方法 |
CN115675637A (zh) * | 2022-12-28 | 2023-02-03 | 禾多科技(北京)有限公司 | 车辆控制方法、装置、电子设备和计算机可读介质 |
CN116118751A (zh) * | 2023-04-19 | 2023-05-16 | 深圳佑驾创新科技有限公司 | 车辆的控制方法、装置、车辆和存储介质 |
CN116118751B (zh) * | 2023-04-19 | 2023-08-29 | 深圳佑驾创新科技有限公司 | 车辆的控制方法、装置、车辆和存储介质 |
CN117369463A (zh) * | 2023-10-31 | 2024-01-09 | 云创智行科技(苏州)有限公司 | 参数优化方法、装置、电子设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN114655248A (zh) | 2022-06-24 |
CN113120001A (zh) | 2021-07-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
WO2022237392A1 (zh) | 车辆的横向控制方法、装置及车辆 | |
US20200278686A1 (en) | Iterative Feedback Motion Planning | |
CN110286681B (zh) | 一种变曲率弯道的动态自动驾驶换道轨迹规划方法 | |
CN109131325B (zh) | 智能驾驶汽车的三维可拓预瞄切换的车道保持控制方法 | |
CN111016893B (zh) | 一种拥堵环境下智能车辆可拓博弈车道保持自适应巡航控制***与控制方法 | |
CN107264534A (zh) | 基于驾驶员经验模型的智能驾驶控制***和方法、车辆 | |
JP2005339241A (ja) | モデル予測制御装置および車両用推奨操作量生成装置 | |
CN108791290B (zh) | 基于在线增量式dhp的双车协同自适应巡航控制方法 | |
CN110789530B (zh) | 一种四轮独立转向-独立驱动车辆轨迹跟踪方法和*** | |
WO2021083860A1 (en) | Real-time performance handling virtual tire sensor | |
CN105480228A (zh) | 增强的车辆速度控制 | |
CN116560371A (zh) | 基于自适应模型预测控制的自动驾驶车辆路径跟踪方法 | |
JP2017061168A (ja) | 車両の自動運転装置 | |
CN117508166A (zh) | 一种车辆行驶控制方法、行驶控制装置、设备及存储介质 | |
CN113815602B (zh) | 一种车辆横向自适应控制方法 | |
CN113311698B (zh) | 车道保持控制方法、控制装置和车辆 | |
Zhou et al. | Optimal lane change analysis for vehicle platooning based on lateral and longitudinal control | |
CN115542899A (zh) | 车辆路径跟踪的方法、装置、车辆、电子设备及介质 | |
JP2020044957A (ja) | 操舵制御システム | |
Casanova et al. | On the optimisation of the longitudinal location of the mass centre of a formula one car for two circuits | |
CN115214715A (zh) | 车辆自动驾驶的横向控制方法和控制装置、车辆 | |
Liu et al. | Path tracking control of automated vehicles based on adaptive MPC in variable scenarios | |
CN113064412B (zh) | 路径跟踪方法、装置、车辆及可读存储介质 | |
CN118348992A (zh) | 自动驾驶轨迹跟踪控制方法、装置、车辆及存储介质 | |
Shen et al. | Stability Analysis and Control Validation of DDEV in Handling Limit via SOSP: A Strategy Based on Stability Region |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 22806362 Country of ref document: EP Kind code of ref document: A1 |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 22806362 Country of ref document: EP Kind code of ref document: A1 |