A robust control method and system for vehicle trajectory tracking with obstacle avoidance

Abstract

The present invention discloses a robust control method for vehicle trajectory tracking with obstacle avoidance considerations, comprising the following steps: Step 1: Establishing a longitudinal and lateral dynamics model for a four-wheel independent drive vehicle; Step 2: Establishing a trajectory tracking equation constraint model; Step 3: Establishing a vehicle collision avoidance constraint model; Step 4: Designing a constraint following controller; Step 5: Decomposing the driving / braking forces; and Step 6: Deploying the controller into a real vehicle. The robust control method for vehicle trajectory tracking with obstacle avoidance considerations of the present invention, through the configuration of Steps 1 to 6, takes into account the longitudinal and lateral dynamics models unique to four-wheel independent drive vehicles, comprehensively addresses the longitudinal and lateral control of the vehicle, has an analytical control law, and requires low hardware computing power.

Description

Technical Field

[0001] The present invention relates to the field of intelligent connected vehicles, and more particularly to a vehicle trajectory tracking robust control method and system taking obstacle avoidance into consideration. Background Art

[0002] Trajectory tracking control is one of the most basic and important functions of autonomous vehicles, which ensures that the vehicle travels stably along a given path and speed. When the vehicle travels along a given trajectory, it is necessary to dynamically adjust the desired trajectory of the vehicle in real time based on the information of surrounding obstacles to ensure that the vehicle can avoid obstacles. At present, dynamic adjustment of the desired trajectory of the vehicle is mostly achieved through local path planning methods such as dynamic window method (DWA), time elastic band method (TEB), artificial potential field method, and fast search random tree. Trajectory tracking control realizes the tracking of the adjusted desired trajectory, which includes two subtasks: speed tracking control and path tracking control. Most of the existing technical solutions handle the two subtasks independently: using PID, feedforward PID, fuzzy control, model predictive control (MPC) and other control methods to achieve vehicle speed control; using Stanley, pure tracking, MPC, linear quadratic regulator (LQR), H ∞ Path tracking control is achieved using control methods such as MATLAB and MATLAB. A small number of technical solutions also comprehensively consider speed tracking and path tracking control tasks, designing a single trajectory tracking controller to achieve coupled longitudinal and lateral control of the vehicle. However, most existing trajectory tracking control solutions rely on local path planning to achieve obstacle avoidance, which requires high hardware computing power. While the MPC algorithm can consider obstacle avoidance during trajectory tracking, it also suffers from the drawback of requiring high computing power for MPC optimization solutions. Summary of the Invention

[0003] In view of the shortcomings of the existing technology, the purpose of the present invention is to overcome the existing trajectory tracking control schemes that mostly rely on local path planning to achieve obstacle avoidance, which requires high hardware computing power. To this end, a trajectory tracking control strategy that takes obstacle avoidance into consideration is proposed.

[0004] To achieve the above objectives, the present invention provides the following technical solution: a vehicle trajectory tracking robust control method taking obstacle avoidance into consideration, characterized in that it comprises the following steps:

[0005] Step 1: Establish a longitudinal and lateral dynamic model of a four-wheel independent drive vehicle;

[0006] Step 2: Establish a trajectory tracking equality constraint model;

[0007] Step 3: Establish a vehicle collision avoidance constraint model;

[0008] Step 4: Design a constrained following controller;

[0009] Step 5: Decomposition of driving / braking force;

[0010] Step 6: Deploy the controller into the actual vehicle.

[0011] As a further improvement of the present invention, the dynamic model in step 1 includes a longitudinal dynamic model, a lateral dynamic model, and a longitudinal and lateral dynamic model in matrix form; wherein the longitudinal dynamic model is as follows:

[0012]

[0013] Where m is the vehicle mass, ρ a is the air density, A x is the equivalent frontal area, C a is the drag coefficient, μ is the rolling resistance coefficient, g is the acceleration of gravity, θ is the road slope, ω1 is the influence of modeling error and external disturbance, and ω1 is a time-varying bounded parameter;

[0014] The lateral dynamics model is as follows:

[0015]

[0016]

[0017] Among them, ω2 and ω3 are the influence of modeling error and external disturbance, which are time-varying and bounded parameters, and a1 to a8 are the vehicle lateral dynamic parameters;

[0018] The matrix form of the longitudinal and lateral dynamics model is as follows:

[0019]

[0020] in, is the vehicle's position vector, are the longitudinal position, lateral position and yaw angle of the vehicle respectively. U=[F x ,δ f ,ΔM] T is the vehicle control input, F x ,δ f ,δ r are the longitudinal driving / braking force, front wheel steering angle, and rear wheel steering angle respectively. The other parameter matrices M, H, B, and Ω can be expressed as:

[0021]

[0022]

[0023] As a further improvement of the present invention, the vehicle lateral dynamics parameter of the lateral dynamics model is calculated by the following formula:

[0024]

[0025]

[0026] Among them, I Z k is the moment of inertia of the vehicle around its center of gravity perpendicular to the horizontal plane, r is the rear wheel cornering stiffness, k f is the front wheel cornering stiffness.

[0027] As a further improvement of the present invention, the trajectory tracking equality constraint model includes a trajectory tracking kinematic model and a trajectory tracking equality constraint, wherein the trajectory tracking motion model is derived by the following steps:

[0028] Step 1: Calculate the desired longitudinal displacement x of the vehicle according to the following formula: d :

[0029]

[0030] Among them, v d (t) is the expected speed;

[0031] Step 2: Calculate the longitudinal displacement tracking error e according to the following formula: x :

[0032]

[0033] Step 3: Calculate the speed following error according to the following formula:

[0034]

[0035] Step 4: Calculate the yaw angle error of the four-wheel independent drive vehicle according to the following formula:

[0036]

[0037] in, is the desired yaw angle, which is determined by the tangent angle at the nearest point on the desired path, and the desired path curvature c at the nearest point R , we can get the expected yaw angular velocity:

[0038]

[0039] Step 5: Calculate the first and second order derivatives of the yaw angle error:

[0040]

[0041]

[0042] The lateral error of the vehicle e yIt is defined as the minimum distance between the vehicle and the desired path. According to the kinematic relationship, its first-order and second-order derivatives can be expressed as:

[0043]

[0044]

[0045] Step 6: Assume that the vehicle continues to move forward in the current direction and the preview distance l p The shortest distance between the vehicle and the desired path at the preview point is the preview lateral error e p The control goal is to ensure e p Approaching 0, we can get e from the geometric relationship p The approximate expression of is:

[0046]

[0047] Step 7: Calculate e according to the formula in step 5 p The first and second derivatives of :

[0048]

[0049]

[0050] The trajectory tracking equality constraints are as follows:

[0051]

[0052] Among them, h1>0, h2>0 are constant parameters, and the longitudinal displacement error e x and preview lateral error e p As a further improvement of the present invention, the collision avoidance constraint model in step 3 includes a collision-free geometric constraint and a collision-free equality constraint, wherein the collision-free set constraint is as follows:

[0053]

[0054]

[0055] Among them, (X o,1 ,Y o,1 )、(X o,2 ,Y o,2 ) are the coordinates of the centers of the two circles describing the vehicle envelope in the geodetic coordinate system. The coordinates can be obtained by the position of the vehicle in the geodetic coordinate system [X, Y] T and yaw angle Calculation, specifically l1 and l2 are the distances between the center of the two circles and the center of gravity of the vehicle, respectively, and are constants. o,1 、Ro,2 are the radii of the two circles, Δ m is the minimum safety distance, (X b,i ,Y b,i ) is the center position of the envelope of the i-th obstacle, R i is the circle radius;

[0056] The collision avoidance equality constraints are as follows:

[0057]

[0058] in, for e a The first derivative of a To define the parameters, specifically f j is an inequality constraint, and f j >0.

[0059] As a further improvement of the present invention, the driving force decomposition in step 5 is to divide F into x and ΔM are decomposed into the driving / braking force F of the four wheels x1 ~F x4 , the allocation rules are as follows:

[0060]

[0061]

[0062] Among them, l w The wheelbase of the vehicle.

[0063] On the other hand, the present invention provides a system for applying the above method, including an environmental perception module, a decision-making and planning module, and a vehicle control module, wherein the environmental perception module is used for collecting vehicle information, obstacle information, and environmental perception, etc., the decision-making and planning module is used to solve obstacle avoidance problems to solve the optimal obstacle avoidance and vehicle trajectory tracking path, and the vehicle execution control module is equipped with software for executing the above method and executes the vehicle's underlying control instructions issued by the decision-making and planning module.

[0064] The beneficial effect of the present invention is that, through the setting of steps one to three, it is possible to effectively realize the comprehensive consideration of the longitudinal and lateral dynamics model unique to the four-wheel independent drive vehicle by establishing a longitudinal and lateral dynamics model of the four-wheel independent drive vehicle, a trajectory tracking equation constraint model and a vehicle collision avoidance constraint model, comprehensively handle the longitudinal and lateral control of the vehicle, have an analytical form control law, and have low requirements on hardware computing power. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 This is a schematic diagram of the patented system architecture of the present invention;

[0066] Figure 2 is a schematic diagram of a four-wheel independent drive vehicle of the present invention;

[0067] Figure 3 is a schematic diagram of the vehicle lateral dynamics and path tracking error of the present invention;

[0068] Figure 4 It is a schematic diagram of obstacle modeling of the present invention;

[0069] Figure 5 This is the controller design process of the present invention. DETAILED DESCRIPTION

[0070] The present invention will be further described below with reference to the embodiments shown in the accompanying drawings.

[0071] Reference Figures 1 to 5 As shown, this embodiment considers Figure 2 The intelligent control of a vehicle with four-wheel independent drive and front-wheel steering configuration is shown. Point o is the center of gravity of the vehicle, and xoy is the vehicle coordinate system, which takes the center of gravity of the vehicle as the origin, the length of the vehicle body as the x-axis, and the axis perpendicular to the length of the vehicle body on the horizontal plane as the y-axis. The distance between the rear wheel and the center of gravity is l r , the distance between the front wheel and the center of gravity is l f The front axle of the vehicle has a wire-controlled steering function, providing a front wheel angle δ f Control interface. All four wheels of the vehicle are driving wheels, providing driving / braking force F x1 、F x2 、F x3 、F x4 Control interface. Similar to conventional autonomous vehicles, four-wheel independent drive intelligent vehicles also have a positioning system (such as a global positioning system (GPS)), a perception module (a combination of motion sensors, lidar, cameras, etc.), a decision-making and planning module, and a control module. This embodiment considers the path tracking control strategy in the control module. Through the planning module, the vehicle can generate a desired path, which can be the lane centerline or a pre-planned global path (without the need for real-time dynamic adjustment). Figure 3 Schematic diagram of vehicle lateral dynamics and definition of error in vehicle path tracking. Four-wheel independent driving / braking force F x1 ~F x4 Equivalent to the active yaw moment ΔM (positive in counterclockwise direction) acting on the vehicle's center of mass and the longitudinal driving / braking force F x After obtaining the desired active yaw moment ΔM and longitudinal driving / braking force F x Then, the expected driving / braking force F of each tire can be obtained according to the preset driving / braking force distribution rule. x1 ~F x4 The curvature information c of the point closest to the vehicle's center of gravity on the desired path RThrough the positioning and perception module, the vehicle can obtain its own motion state: speed v x , yaw angular velocity Lateral velocity v y , the relationship between the vehicle and the desired path: the lateral offset e y (y-distance between the vehicle and the nearest point on the desired path), vehicle yaw angle error (The difference between the vehicle's yaw angle and the desired yaw angle determined by the slope at the nearest point on the desired path.) Through the perception module, the vehicle can obtain information about surrounding obstacles in the geodetic coordinate system. Figure 4 As a schematic diagram of obstacle modeling, the obstacle envelope is described by a circle, and the center position of the obstacle can be obtained (X b,i ,Y b,i ), obstacle radius R i , i = 0, 1,…, n (long and narrow obstacles are described by multiple circles).

[0072] The design steps of the trajectory tracking controller considering obstacle avoidance in this embodiment are as follows:

[0073] 1. Longitudinal and lateral dynamics modeling of four-wheel independent drive vehicles

[0074] 1) Longitudinal dynamics model

[0075] The longitudinal dynamics of a four-wheel independent drive vehicle are similar to those of other vehicles and can be expressed as

[0076]

[0077] Where m is the vehicle mass, ρ a is the air density, A x is the equivalent frontal area, C a is the drag coefficient, μ is the rolling resistance coefficient, g is the acceleration of gravity, θ is the road slope, ω1 is the influence of modeling error and external disturbance, and ω1 is a time-varying bounded parameter.

[0078] 2) Lateral dynamics model

[0079] The lateral dynamics of a four-wheel independent drive vehicle are affected by multiple factors, including tire nonlinearity, road roll, vehicle roll motion, and crosswind. To ensure both model accuracy and theoretical analysis convenience, the following simplifications are made to the vehicle's lateral dynamics:

[0080] (1) Assuming that the vehicle body and suspension system are rigid, the effects of vehicle roll and pitch motion are not considered, that is, the vehicle is assumed to move in a plane parallel to the ground;

[0081] (2) Assuming the vehicle is bilaterally symmetrical, the tires on both sides of the same center axis have the same side slip angles and can be combined into one tire to describe it;

[0082] (3) Assume that the vehicle's longitudinal velocity changes slowly.

[0083] Based on the structural characteristics of the four-wheel independent drive vehicle, Newton's second law and the theorem of rigid body rotation around a fixed axis, the calculation formula for the lateral dynamics model of the four-wheel independent drive vehicle can be obtained as follows:

[0084]

[0085]

[0086] Among them, ω2 and ω3 are the influence of modeling error and external disturbance, which are time-varying and bounded parameters. The calculation formulas of vehicle lateral dynamic parameters a1~a8 are

[0087]

[0088]

[0089] Among them, I Z k is the moment of inertia of the vehicle around its center of gravity perpendicular to the horizontal plane, r is the rear wheel cornering stiffness, k f is the front wheel cornering stiffness.

[0090] 3) Matrix-form longitudinal and transverse dynamics model

[0091] To facilitate subsequent controller design, the aforementioned vehicle longitudinal and lateral dynamics model is rewritten into the following matrix form:

[0092]

[0093] in, is the vehicle's position vector, are the longitudinal position, lateral position and yaw angle of the vehicle respectively. U=[F x ,δ f ,ΔM] T is the vehicle control input, F x ,δ f ,δ r are longitudinal driving / braking force, front wheel steering angle, and rear wheel steering angle respectively. Other parameter matrices M, H, B, and Ω can be expressed as:

[0094]

[0095] 2. Trajectory tracking equality constraint modeling

[0096] 1) Trajectory tracking kinematic model

[0097] Trajectory tracking requires tracking of trajectory speed and path.

[0098] The desired longitudinal displacement of the vehicle x d It can be expressed as:

[0099]

[0100] Among them, v d (t) is the desired velocity. Longitudinal displacement tracking error e x It can be expressed as:

[0101]

[0102] The speed following error is

[0103]

[0104] like Figure 3 As shown in Figure 2, the yaw angle error of a four-wheel independent drive vehicle can be expressed as:

[0105]

[0106] in, is the desired yaw angle, which is determined by the tangent angle at the nearest point on the desired path, and the desired path curvature c at the nearest point R , we can get the expected yaw angular velocity:

[0107]

[0108] Therefore, the first and second derivatives of the yaw angle error are as follows:

[0109]

[0110]

[0111] The lateral error of the vehicle e y It is defined as the minimum distance between the vehicle and the desired path. According to the kinematic relationship, its first-order and second-order derivatives can be expressed as:

[0112]

[0113]

[0114] The controller is designed using the idea of preview. Specifically, assuming the vehicle continues to move forward in the current direction, the preview distance l p The shortest distance between the vehicle and the desired path at the preview point is the preview lateral error e p The control goal is to ensure e p Approaches 0. From the geometric relationship, we can get e p The approximate expression of is:

[0115]

[0116] From formulas (12) to (16), we can get e p The first and second derivatives of :

[0117]

[0118]

[0119] 2) Trajectory tracking equality constraints

[0120] The goal of trajectory tracking control is: longitudinal displacement error e x and preview lateral error e p Approaches 0. This control objective is achieved by ensuring the establishment of the following trajectory tracking equality constraints:

[0121]

[0122] Among them, h1>0, h2>0 are constant parameters. If this equation is strictly true, then When time t approaches infinity, e x and e p Approaching 0.

[0123] 3. Vehicle collision avoidance constraint modeling

[0124] 1) Collision-free geometric constraint modeling

[0125] During driving, the vehicle must maintain a safe distance from obstacles. Existing solutions often use a method that combines local path planning to update the desired path in real time and trajectory tracking control. However, this solution directly guarantees the safety of the vehicle through the designed control law with an analytical form. Figure 4 The obstacle modeling diagram shown in the figure uses a circle to describe the obstacle envelope, and the center position of the obstacle in the geodetic coordinate system (X b,i ,Y b,i ), obstacle radius R i , i = 0, 1, ..., n (slender obstacles are described by multiple circles). Then the geometric constraints for the vehicle to avoid collision are:

[0126]

[0127]

[0128] Among them, (X o,1 ,Y o,1 )、(X o,2 ,Y o,2 ) are the coordinates of the centers of the two circles describing the vehicle envelope in the geodetic coordinate system. The coordinates can be obtained by the position of the vehicle in the geodetic coordinate system [X, Y]T and yaw angle Calculation, specifically l1 and l2 are the distances between the center of the two circles and the center of gravity of the vehicle, respectively, and are constants. o,1 、R o,2 are the radii of the two circles, Δ m is the minimum safety distance, (X b,i ,Y b,i ) is the center position of the envelope of the i-th obstacle, R i is the circle radius.

[0129] From equations (20) and (21), we can obtain 2n inequality constraints, each of which can be uniformly described as follows:

[0130]

[0131] When j=1,2,…,n,

[0132]

[0133] When j=n+1,n+2,…,2n,

[0134]

[0135] 2) Collision avoidance equation constraint design

[0136] As long as the constraint (22) is satisfied, the vehicles will not collide. Define the parameters Because f j >0, so there is e in the safe interval a ∈(-∞,+∞). As long as the initial state f j >0, j=1,2,…,2n, and for any t≥0, e a ∈(-∞,+∞), the vehicle is collision-free during driving. a The first-order derivative of is:

[0137]

[0138] in,

[0139]

[0140] Π can contain the speed information of the obstacle. a The second derivative of is:

[0141]

[0142]

[0143] In the above formula:

[0144]

[0145] like is bounded, then within a finite time, e a ∈(-∞,+∞), the vehicle will not collide with the obstacle, so the following collision avoidance equality constraint is designed:

[0146]

[0147] Integrating the trajectory tracking equality constraint (13) and the collision avoidance equality constraint (25), we obtain the following matrix equality constraint:

[0148]

[0149] Among them, the matrix parameters A, c, and b are

[0150]

[0151]

[0152]

[0153] in,

[0154]

[0155] Constraint (27) is equivalent to the trajectory tracking equality constraint (13) and the collision avoidance equality constraint (25).

[0156] 4. Constrained Following Controller Design

[0157] Steps 1 to 3 have already obtained the vehicle's longitudinal and lateral dynamics model (5) and the equation constraints (27) for the vehicle to complete the tracking task and meet the obstacle avoidance conditions. By designing a constrained following controller to ensure that the vehicle meets the equation constraints, tracking control that ensures vehicle safety can be achieved.

[0158] The constraint following error β is defined as follows:

[0159]

[0160] For the unknown parameter matrix Ω in the dynamic model (5), the following properties exist:

[0161] There exists a positive constant ω M , so that for any (q,t)∈R 3 ×R + And the given positive definite matrix P∈R 3×3 , conditional ‖PAM -1 Ω‖<ω M Established.

[0162] The above properties indicate that the modeling error and external disturbance in the dynamic model (5) are bounded, but the upper bound is often unknown. This embodiment designs the following adaptive law to estimate the parameter ω M Value:

[0163]

[0164] in, is the parameter ω M The estimated value of η ω >0,∈>0 are constant parameters.

[0165] Design the constraint following controller as follows:

[0166] U=p1+p2+p3 (33)

[0167] in,

[0168]

[0169] In the above formula, the symbol “+” represents the Moore-Penrose generalized inverse of the matrix, κ>0, is a constant parameter, P∈R 3×3 And P>0,

[0170]

[0171] Among them, α>0 is a constant parameter close to 0, and its value is usually 0.1.

[0172] Equations (33) to (35) are the designed vehicle trajectory tracking controllers that take obstacle avoidance into consideration. They have an analytical control law, and after obtaining the state parameters in the control law expression, the control variables can be efficiently calculated.

[0173] 5. Driving / braking force decomposition

[0174] The vehicle control quantity U obtained in the constrained following control law (33) to (35) is [F x ,δ f ,ΔM] T In, F x and ΔM are synthesized by the driving / braking forces of the four tires and cannot be directly input into the underlying controller. F x and ΔM are decomposed into the driving / braking force F of the four wheels x1 ~F x4 The specific allocation rules are as follows:

[0175]

[0176]

[0177] Among them, l w The wheelbase of the vehicle.

[0178] 6. Control Law Real Vehicle Deployment Method

[0179] When the controller is deployed to the actual vehicle, the control parameters κ, α, and η need to be determined. ω 、 ∈, h1, h2, Δ, l p , P, and vehicle-related parameters: m, I z 、l f 、l m 、l r 、k l In the actual control process, the vehicle obtains a desired path through the planning module (this path may have the risk of colliding with obstacles); the vehicle state information, path tracking error information, obstacle information, and path curvature information at the nearest point on the path are measured in real time through the on-board sensor equipment and positioning equipment. By substituting this information into the designed control law expression (33), the trajectory tracking control quantity F that takes obstacle avoidance into account can be obtained. x , δ f , ΔM. Then, according to formula (36) (37), the driving / braking force distribution is performed to obtain the expected driving / braking force F of each wheel. x1 ~F x4 Finally, these control variables are input into the corresponding drive system and steering system to achieve vehicle control.

[0180] The above description is merely a preferred embodiment of the present invention. The scope of protection of the present invention is not limited to the above embodiment. All technical solutions based on the concept of the present invention are within the scope of protection of the present invention. It should be noted that for those skilled in the art, various improvements and modifications that do not depart from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A robust control method for vehicle trajectory tracking with obstacle avoidance, characterized by: The steps include: Step 1: Establish a longitudinal and lateral dynamic model of a four-wheel independent drive vehicle; Step 2: Establish a trajectory tracking equality constraint model; Step 3: Establish a vehicle collision avoidance constraint model; Step 4: Design a constrained following controller; Step 5: Decomposition of driving / braking force; Step 6: Deploy the controller into the actual vehicle; The design constraint following controller in step 4 is as follows: U=p1+p2+p3 in, p1=B -1 M 1 / 2 (AM -1 / 2 ) + (b-AM -1 H) p2=-κB -1 MA T (AA T ) -1 P -1 b In the above formula, the symbol "+" represents the Moore-Penrose generalized inverse of the matrix, κ>0, is a constant parameter, P∈R 3×3 And P>0, Among them, α>0 is a constant parameter close to 0, and its value is 0.

1.

2. The vehicle trajectory tracking robust control method with obstacle avoidance according to claim 1, characterized in that: The dynamic model in step 1 includes a longitudinal dynamic model, a lateral dynamic model, and a matrix-form longitudinal and lateral dynamic model; wherein the longitudinal dynamic model is as follows: Where m is the vehicle mass, ρ a is the air density, A x is the equivalent frontal area, C a is the drag coefficient, μ is the rolling resistance coefficient, g is the acceleration of gravity, θ is the road slope, ω1 is the influence of modeling error and external disturbance, and ω1 is a time-varying bounded parameter; The lateral dynamics model is as follows: Among them, ω2 and ω3 are the influence of modeling error and external disturbance, which are time-varying and bounded parameters, and a1 to a8 are the vehicle lateral dynamic parameters; The matrix form of the longitudinal and lateral dynamics model is as follows: in, is the vehicle's position vector, x, y, are the longitudinal position, lateral position and yaw angle of the vehicle respectively; U=[F x ,δ f ,ΔM] T is the vehicle control input, F x ,δ f ,δ r are the longitudinal driving / braking force, front wheel steering angle, and rear wheel steering angle respectively. The other parameter matrices M, H, B, and Ω can be expressed as:

3. The vehicle trajectory tracking robust control method with obstacle avoidance according to claim 2, characterized in that: The vehicle lateral dynamics parameters of the lateral dynamics model are calculated using the following formula: Among them, I Z k is the moment of inertia of the vehicle around its center of gravity perpendicular to the horizontal plane, r is the rear wheel cornering stiffness, k f is the front wheel cornering stiffness.

4. The vehicle trajectory tracking robust control method with obstacle avoidance according to claim 3, characterized in that: The trajectory tracking equality constraint model includes a trajectory tracking kinematic model and a trajectory tracking equality constraint, wherein the trajectory tracking motion model is derived by the following steps: Step 1: Calculate the desired longitudinal displacement x of the vehicle according to the following formula: d : Among them, v d (t) is the expected speed; Step 2: Calculate the longitudinal displacement tracking error e according to the following formula: x : Step 3: Calculate the speed following error according to the following formula: Step 4: Calculate the yaw angle error of the four-wheel independent drive vehicle according to the following formula: in, is the desired yaw angle, which is determined by the tangent angle at the nearest point on the desired path, and the desired path curvature c at the nearest point R , we can get the expected yaw angular velocity: Step 5: Calculate the first and second order derivatives of the yaw angle error: The lateral error of the vehicle e y It is defined as the minimum distance between the vehicle and the desired path. According to the kinematic relationship, its first-order and second-order derivatives can be expressed as: Step 6: Assume that the vehicle continues to move forward in the current direction and the preview distance l p The shortest distance between the vehicle and the desired path at the preview point is the preview lateral error e p The control goal is to ensure e p Approaching 0, we can get e from the geometric relationship p The approximate expression of is: Step 7: Calculate e according to the formula in step 5 p The first and second derivatives of : The trajectory tracking equality constraints are as follows: Among them, h1>0, h2>0 are constant parameters, and the longitudinal displacement error e x and preview lateral error e p Approaching 0.

5. The vehicle trajectory tracking robust control method with obstacle avoidance according to claim 4, characterized in that: The collision avoidance constraint model in step 3 includes a collision-free geometric constraint and a collision-free equality constraint, wherein the collision-free set constraint is as follows: Among them, (X o,1 ,Y o,1 )、(X o,2 ,Y o,2 ) are the coordinates of the centers of the two circles describing the vehicle envelope in the geodetic coordinate system. The coordinates can be obtained by the position of the vehicle in the geodetic coordinate system [X, Y] T and yaw angle Calculation, specifically l1 and l2 are the distances between the center of the two circles and the center of gravity of the vehicle, respectively, and are constants. o,1 、R o,2 are the radii of the two circles, Δ m is the minimum safety distance, (X b,i ,Y b,i ) is the center position of the envelope of the i-th obstacle, R i is the circle radius; The collision avoidance equality constraints are as follows: in, for e a The first derivative of a To define the parameters, specifically f j is an inequality constraint, and f j >0.

6. The vehicle trajectory tracking robust control method with obstacle avoidance according to claim 5, characterized in that: The driving force decomposition in step 5 is to divide F x and ΔM are decomposed into the driving / braking force F of the four wheels x1 ~F x4 , the allocation rules are as follows: Among them, l w The wheelbase of the vehicle.

7. A system using the vehicle trajectory tracking robust control method with obstacle avoidance according to any one of claims 1 to 6, characterized in that: It includes an environmental perception module, a decision-making and planning module, and a vehicle control module. The environmental perception module is used to collect vehicle information, obstacle information, and environmental perception. The decision-making and planning module is used to solve obstacle avoidance problems to solve the optimal obstacle avoidance and vehicle trajectory tracking path. The vehicle execution control module is equipped with software that executes the above method and executes the vehicle's underlying control instructions issued by the decision-making and planning module.

Images