CN107901917B - A Trajectory Tracking Control Method for Unmanned Vehicles Based on Slip-Slip Coupling Estimation - Google Patents

Abstract

The invention discloses the automatic driving vehicle Trajectory Tracking Control methods based on sliding coupling estimation of trackslipping, the data that this method is detected according to the coefficient that trackslips, the motion mathematical model expression formula of slip rate and GPS-INS, desired trajectory, desired speed and the expectation yaw velocity information provided again according to automatic driving vehicle decision-making level, calculate the coefficient that trackslips, the numerical value of slip rate, then counter to be updated in the kinematics model of automatic driving vehicle, the vehicle wheel rotational speed for realizing track following is compensated and calculated, achievees the purpose that accurately to track desired trajectory.The invention has the advantages that slippage of trackslipping can constantly be calculated, the actual motion state of automatic driving vehicle more really, is accurately described and characterizes, so as to effectively accurately follow desired trajectory.

Description

A Trajectory Tracking Control of Unmanned Vehicles Based on Slip-Slip Coupling Estimation method

Technical field:

The invention relates to the technical field of unmanned vehicles, in particular to a trajectory tracking control method for unmanned vehicles based on slip-slip coupling estimation.

Background technique:

As an important direction and trend of future vehicle development, driverless vehicles have attracted the attention and development of many research institutions and enterprises around the world. Among the key technologies of vehicle control of unmanned vehicles, trajectory tracking control is the core technology method to realize unmanned vehicles to travel according to the trajectory planned by the trajectory, and its control accuracy and control robustness determine whether unmanned vehicles can follow expectations The trajectory is expected to reach the specified destination.

At present, in the field of unmanned vehicle trajectory tracking, the control method is based on the vehicle kinematics model in a rational state, that is, the vehicle kinematics model assuming that the unmanned vehicle wheels are under the condition of no slip and no slip. In the actual road conditions of human-driven vehicles, wheel slippage generally exists, especially when unmanned vehicles drive on gravel roads, icy and snowy roads, so the kinematics model based on the ideal state cannot be calculated from time to time. The amount of slip is out of slip, so it is impossible to effectively and accurately track the desired trajectory.

Invention content:

The technical problem to be solved by the present invention is to provide a system that can calculate the slippage from time to time, more truly and accurately describe and characterize the actual motion state of the unmanned vehicle, so as to effectively and accurately follow the desired trajectory. A trajectory tracking control method for unmanned vehicles based on slip-slip coupling estimation.

The technical solution of the present invention is to provide an unmanned vehicle trajectory tracking control method based on slip-slip coupling estimation, the method comprising the following steps:

Step 1: Receive the desired trajectory and the desired trajectory tracking speed signal planned by the decision-making level of the unmanned vehicle, set the initial preview distance d, and select the point in the expected trajectory whose distance from the vehicle is the preview distance d as the preview point q _d , Read the current state data of the vehicle collected by the GPS-INS combined positioning system;

Step 2: Establish a kinematic model based on wheel slip and body slip of the unmanned vehicle:

Define the geodetic inertial coordinate system ∑I, the vehicle body coordinate system ∑b,

The pose of the car body in the inertial coordinate system: q _I = [x ₁ y ₁ θ ₁ ] ^T

The pose of the car body in the car body coordinate system: q _b =[x _b y _b θ _b ] ^T

And θ _I = θ _b = θ, is the vehicle heading angle,

The speed conversion relationship between the inertial coordinate system and the vehicle body coordinate system is:

Assume

but

In the vehicle coordinate system, define the longitudinal direction of the body as x, the width of the body as lateral y, the slip coefficient of the left wheel as s _l , the slip coefficient of the right wheel as s _r , the wheel radius as r, and the left side of the vehicle body as s l . The wheel speed ω _l , the linear speed v _l , the wheel speed ω _r on the right side of the vehicle body, the linear speed v _r , the vehicle longitudinal speed v _bx , the vehicle yaw rate ω, the wheel center width is 2L,

The slip coefficient of the vehicle slip is i, the lateral speed of the vehicle is v _by ,

In the inertial coordinate system, the vehicle kinematics model based on slip-slip is established:

Step 3: According to the vehicle kinematics model based on slip and slip, solve the expressions of the slip coefficient s _l of the left wheel and the slip coefficient s _r of the right wheel:

Step 4: Establish a trajectory tracking error model in vehicle coordinates:

which is

in, represents the trajectory error in the vehicle body coordinate system, Represents the pose of the desired trajectory point in the inertial coordinate system, that is, the pose of the preview point q _d ; Represents the current pose of the vehicle in the inertial coordinate system;

Step 5: Derive the tracking error model to obtain the tracking error state equation:

Step 6: According to the state equation of trajectory tracking error in step 5, the control law of trajectory tracking control based on slip and slip coefficient is adopted:

where v1 is the speed control input _of the right wheel, _v2 is the speed control input of the left wheel,

The control gain coefficients k ₁ , k ₂ , and k ₃ are greater than zero and have an upper bound;

Step 7: Control the driving of the vehicle according to the input of the control rate in Step 6, and then obtain the current pose of the vehicle in the inertial coordinate system according to the data detected and recorded by GPS-INS That is, q _c = q _I , the velocity in the inertial coordinate system The yaw angular velocity ω of the vehicle body, the left and right wheel speeds ω ₁ and ω _r are measured according to the encoder;

Step 8: According to the relationship between v _bx and v _by in the vehicle body coordinate system and v _Ix and v _Iy in the inertial coordinate system:

Calculate v _bx , v _by and slip rate and Then set i, ω ₁ , ω _r , Substitute into the calculation formula of s _l and s _r in step 3, and calculate s _l and s _r ;

Step 9: Substitute s _l , s _r , desired velocity v _d , and desired yaw angular velocity ω _d calculated in step 8 into the control law in step 6 , and select the control gain coefficients k ₁ , k ₂ , k ₃ , the calculated Substitute step 2 to solve the required wheel speeds on both sides of the control vehicle under the slip-slip coupling estimation of the driving wheels marked as

Step 10: According to the speed of the wheels on both sides of the vehicle calculated in step 9 The vehicle controller sends the calculated wheel speed signal to the actuator that drives the wheel and controls the wheel to move at this speed;

Step 11: Repeat the actions from Step 4 to Step 10, and finally achieve the desired speed and accurately track the desired trajectory.

Preferably, according to the method for tracking and controlling the trajectory of an unmanned vehicle based on slip-slip coupling estimation according to the present invention, the pose q _d of the desired trajectory, the desired velocity v _d and the desired yaw angular velocity ω _d are all is the data output by the decision layer.

Preferably, according to the method for tracking and controlling the trajectory of an unmanned vehicle based on slip-slip coupling estimation according to the present invention, the unmanned vehicle is a two-wheel, four-wheel or six-wheel vehicle.

Preferably, according to the method for tracking and controlling the trajectory of an unmanned vehicle based on slip-slip coupling estimation according to the present invention, the unmanned vehicle is driven by an engine or a motor.

Preferably, according to the method for tracking and controlling the trajectory of an unmanned vehicle based on slip-slip coupling estimation according to the present invention, the encoder is an absolute encoder.

The beneficial effects of the present invention are:

1. The slip coefficient and slip rate model of the unmanned vehicle is introduced into the kinematic model of the unmanned vehicle, which can more realistically and accurately describe and characterize the actual motion state of the unmanned vehicle;

2. The unmanned vehicle kinematics model established based on slip-slip coupling estimation can be used to solve the mathematical relationship expression of slip coefficient and slip rate. Therefore, it is the calculation of slip coefficient and slip rate. models are provided;

3. The proposed trajectory tracking control method based on slip-slip coupling estimation is based on the mathematical expression of slip coefficient, slip rate and the data detected by GPS-INS, and then based on the expectations given by the decision-making layer of the unmanned vehicle From the trajectory, expected speed and expected yaw rate information, the slip coefficient and slip rate can be calculated, and then substituted into the kinematics model of the unmanned vehicle to compensate and calculate the wheel speed for trajectory tracking to achieve The purpose of precisely tracking the desired trajectory.

4. The trajectory tracking control method based on slip-slip coupling estimation proposed by the present invention can calculate the real situation of vehicle slippage and wheel slip from time to time, and improve the environmental adaptability of trajectory tracking of unmanned vehicles, such as ice, snow, slippery Wet, soft and slippery roads can still accurately track the desired trajectory. Therefore, this trajectory tracking control method greatly improves the trajectory tracking accuracy of unmanned vehicles in complex road environments.

Description of drawings:

Fig. 1 is the coordinate representation schematic diagram in the present invention;

2 is a schematic diagram of a tracking error model in the present invention;

Fig. 3 is the slip model schematic diagram in the present invention;

FIG. 4 is a schematic diagram of a control block of the present invention.

Specific examples:

A kind of unmanned vehicle trajectory tracking control method based on slip-slip coupling estimation of the present invention will be further described below in conjunction with the accompanying drawings and specific embodiments:

The unmanned vehicle involved in the present invention is an unmanned vehicle with independent full drive, no active steering degree of freedom of the wheels, and the same rotation speed of the wheels on the same side of the vehicle body. The unmanned vehicle includes a GPS-INS combined positioning system, The encoder used to collect wheel speed data and the vehicle controller that sends the drive motor speed to the vehicle controller.

As shown in Fig. 1, Fig. 2, Fig. 3 and Fig. 4, a method for tracking and controlling the trajectory of an unmanned vehicle based on slip-slip coupling estimation of the present invention includes the following steps:

Step 1: Receive the desired trajectory and the desired trajectory tracking speed signal planned by the decision-making level of the unmanned vehicle, set the initial preview distance d, and select the point in the expected trajectory whose distance from the vehicle is the preview distance d as the preview point q _d , , read the current state data of the vehicle collected by the GPS-INS combined positioning system;

Step 2: Establish a kinematic model based on wheel slip and body slip of the unmanned vehicle:

Define the geodetic inertial coordinate system ∑I, the vehicle body coordinate system ∑b,

The pose of the car body in the inertial coordinate system: q _I = [x _I y _I θ _I ] ^T

The pose of the car body in the car body coordinate system: q _b =[x _b y _b θ _b ] ^T

And θ _I = θ _b = θ, is the vehicle heading angle,

The speed conversion relationship between the inertial coordinate system and the vehicle body coordinate system is:

Assume

but

In the vehicle coordinate system, define the longitudinal direction of the body as x, the width of the body as lateral y, the slip coefficient of the left wheel as s _l , the slip coefficient of the right wheel as s _r , the wheel radius as r, and the left side of the vehicle body as s l . The wheel speed ω _l , the linear speed v _l , the wheel speed ω _r on the right side of the vehicle body, the linear speed v _r , the vehicle longitudinal speed v _bx , the vehicle yaw rate ω, the wheel center width is 2L,

The slip coefficient of the vehicle slip is i, the lateral speed of the vehicle is v _by ,

In the inertial coordinate system, the vehicle kinematics model based on slip-slip is established:

Step 3: According to the vehicle kinematics model based on slip and slip, solve the expressions of the slip coefficient s _l of the left wheel and the slip coefficient s _r of the right wheel:

Step 4: Establish a trajectory tracking error model in vehicle coordinates:

which is

in, represents the trajectory error in the vehicle body coordinate system, Represents the pose of the desired trajectory point in the inertial coordinate system, that is, the pose of the preview point q _d ; Represents the current pose of the vehicle in the inertial coordinate system;

Step 5: Derive the tracking error model to obtain the tracking error state equation:

Step 6: According to the state equation of trajectory tracking error in step 5, the control law of trajectory tracking control based on slip and slip coefficient is adopted:

where v1 is the speed control input _of the right wheel, _v2 is the speed control input of the left wheel,

The control gain coefficients k ₁ , k ₂ , and k ₃ are greater than zero and have an upper bound;

Step 7: Control the driving of the vehicle according to the input of the control rate in Step 6, and then obtain the current pose of the vehicle in the inertial coordinate system according to the data detected and recorded by GPS-INS That is, q _c = q _I , the velocity in the inertial coordinate system The yaw angular velocity ω of the vehicle body, the left and right wheel speeds ω _l and ω _r are measured according to the encoder;

Step 8: According to the relationship between v _bx and v _by in the vehicle body coordinate system and v _lx and v _Iy in the inertial coordinate system:

Calculate v _bx , v _by and slip rate and Then set i, ω _l , ω _r , Substitute into the calculation formula of s _l and s _r in step 3, and calculate s _l and s _r ;

Step 9: Substitute the s _l , s _r , desired velocity v _d , and desired yaw angular velocity ω _d calculated in step 8 into the control law in step 6 , and select the control gain coefficients k ₁ , k ₂ , k ₃ , the calculated Substitute step 2 to solve the required wheel speeds on both sides of the control vehicle under the slip-slip coupling estimation of the driving wheels marked as

Step 10: According to the speed of the wheels on both sides of the vehicle calculated in step 9 The vehicle controller sends the calculated wheel speed signal to the actuator that drives the wheel and controls the wheel to move at this speed;

Step 11: Repeat the actions from Step 4 to Step 10, and finally achieve the desired speed and accurately track the desired trajectory.

Preferably, in the present invention, the pose q _d of the desired trajectory, the desired velocity v _d and the desired yaw angular velocity ω _d are all data output by the decision-making layer.

Preferably, in the present invention, the unmanned vehicle is a two-wheel or four-wheel or six-wheel vehicle.

Preferably, in the present invention, the unmanned vehicle is driven by an engine or a motor.

Preferably, in the present invention, the encoder is an absolute encoder.

The above-mentioned embodiments merely describe the preferred embodiments of the present invention, and do not limit the scope of the present invention. On the premise of not departing from the design spirit of the present invention, those of ordinary skill in the art can make various Such deformations and improvements shall fall within the protection scope determined by the claims of the present invention.

Claims (5)

1. A track tracking control method of an unmanned vehicle based on slip-slip coupling estimation is characterized by comprising the following steps: the method comprises the following steps:

step 1: receiving an expected track and an expected track tracking speed signal planned by a decision layer of the unmanned vehicle, setting an initial pre-aiming distance d, and selecting a point which is away from the vehicle by the pre-aiming distance d in the expected track as a pre-aiming point q_dReading current state data of the vehicle collected by the GPS-INS combined positioning system;

step 2: establishing a kinematics model based on wheel slip and body slip of the unmanned vehicle:

defining a ground inertia coordinate system sigma I, a vehicle body coordinate system sigma b,

pose of the vehicle body under an inertial coordinate system: q. q.s_I＝[x_I y_I θ_I]^T

Pose of the vehicle body under the vehicle body coordinate system: q. q.s_b＝[x_b y_b θ_b]^T

And theta_I＝θ_bθ, is the vehicle heading angle,

the speed conversion relation between the inertial coordinate system and the vehicle body coordinate system is as follows:

is provided with

Then

Under a vehicle coordinate system, defining the length direction of a vehicle body as a longitudinal direction x, the width direction of the vehicle body as a transverse direction y, and the slip coefficient of a left wheel as s_lThe slip coefficient of the right wheel is s_rRadius of wheel r, left wheel rotation speed omega_lLinear velocity v_lRotation speed omega of right wheel of vehicle body_rLinear velocity v_rThe longitudinal speed of the vehicle is v_bxThe yaw rate of the vehicle is omega, the width of the center of the wheel is 2L,

the sliding coefficient of the whole vehicle sliding is i, and the vehicle transverse speed is v_by，

Establishing a vehicle kinematic model based on slip, spin and slip under an inertial coordinate system:

and step 3: according to the vehicle kinematic model based on the slip and slip, the slip coefficient s of the left wheel is solved_lCoefficient of slip s of the right wheel_rExpression (c):

and 4, step 4: establishing a track tracking error model under vehicle coordinates:

namely, it is

Wherein,representing the track error in the vehicle body coordinate system,representing the pose of the expected track point in an inertial coordinate system, i.e. the pre-aiming point q_dThe pose of (a);representing the current pose of the vehicle in an inertial coordinate system;

and 5: and (3) carrying out derivation on the tracking error model to obtain a tracking error state equation:

step 6: according to the state equation of the track tracking error in the step 5, adopting a control law of track tracking control based on slip coefficients:

wherein v is₁For the speed control input of the right wheel, v₂For the speed control input of the left wheel, v_cFor vehicle longitudinal speed control input, ω_cIs a vehicle yaw rate control input,

wherein the gain factor k is controlled₁、k₂、k₃Greater than zero and having an upper bound;

and 7: controlling the vehicle to run according to the input of the control rate in the step 6, and then obtaining the vehicle according to the data detected and recorded by the GPS-INSCurrent position and attitude of vehicle under inertial coordinate systemI.e. q_c＝q_IVelocity in inertial frameThe yaw angular speed omega of the vehicle body is obtained by measuring the rotating speeds omega of the left and the right wheels according to the encoder_l、ω_r；

And 8: according to v under the vehicle body coordinate system_bx、v_byV and under the inertial coordinate system_Ix、v_IyThe relationship is as follows:

calculate v_bx、v_byAnd slip ratioAnd isThen i is put in,ω_l、ω_r、S in step 3_l、s_rA calculation formula of s_l、s_r；

And step 9: s calculated in step 8₁、s_rDesired velocity v_dDesired yaw rate ω_dControl law substituting in step 6And selecting a control gain coefficient k₁、k₂、k₃Will countCalculatingSolving the required rotation speeds of the wheels on both sides of the control vehicle under the estimation of the slip-slip coupling of the driving wheels in the substitution step 2Is marked as

Step 10: the rotating speeds of the wheels on the two sides of the vehicle calculated according to the step 9The vehicle control unit sends the calculated wheel rotating speed signal to an actuator for driving the wheel and controls the wheel to move at the speed;

step 11: and (5) repeatedly executing the actions from the step 4 to the step 10, and finally realizing the accurate tracking of the expected track at the expected speed.

2. The unmanned vehicle trajectory tracking control method based on slip-slip coupling estimation of claim 1, wherein: pose q of the expected trajectory_dDesired velocity v_dAnd desired yaw rate ω_dAre all data output by the decision layer.

3. The unmanned vehicle trajectory tracking control method based on slip-slip coupling estimation of claim 1, wherein: the unmanned vehicle is a two-wheel vehicle, a four-wheel vehicle or a six-wheel vehicle.

4. The unmanned vehicle trajectory tracking control method based on slip-slip coupling estimation of claim 1, wherein: the unmanned vehicle is driven by an engine or a motor.

5. The unmanned vehicle trajectory tracking control method based on slip-slip coupling estimation of claim 1, wherein: the encoder is an absolute encoder.