CN105320129A - Method for tracking and controlling locus of unmanned bicycle - Google Patents

Abstract

The invention belongs to the technical field of movement controlling of unmanned vehicles, and especially relates to a method for tracking and controlling a locus of an unmanned bicycle. The method comprises the steps of: firstly, establishing a balance dynamic model of the bicycle, and establishing discrete state equations of a bicycle self-balance control system into which a self-balance controller is added; secondly, establishing a discrete kinetic model of the bicycle, and carrying out linearization; then combining the kinetic model with the self-balance control system, carrying out corresponding simplification processing, and establishing a seven-dimension state predicting model of the bicycle; and finally, using the seven-dimension state predicting model as a prediction model of a bicycle locus tracking algorithm, carrying out locus tracking and controlling on the bicycle, and solving optimal control input of each sampling moment on line. The bicycle locus tracking and controlling method based on the seven-dimension state predicting model is capable of accurately predicting the state of the bicycle in the future time, the calculation amount is small, and a good control effect is ensured by the on-line real-time performance.

Description

Method for tracking and controlling track of unmanned bicycle

Technical Field

The invention belongs to the technical field of motion control of an unmanned bicycle, and particularly relates to a method for tracking and controlling a track of the unmanned bicycle.

Background

Compared with an automobile, the unmanned bicycle has the advantages of low price, lightness, portability, flexibility, mobility, environmental protection, energy conservation and the like. However, the two-wheel structure of the bicycle has instability relative to the four-wheel structure of the automobile, and the bicycle cannot maintain balance without human control. Therefore, in order to achieve the unmanned function of the bicycle, the bicycle must first be balanced to control itself. Therefore, the difficulty of the unmanned bicycle is far greater than that of an unmanned automobile.

On the basis of realizing self-balancing control of the bicycle, the bicycle can be completely driven by controlling the movement of the bicycle and matching with the functions of machine vision, trajectory planning and the like on the upper layer. Path tracking means that in an inertial coordinate system, a mobile robot must start from a given initial state to reach and follow a given target track, and an initial point of the robot may be on the track or not, which is an important problem in the research field of mobile robots. At present, most research objects of the track tracking problem are wheel-type mobile robots, unmanned vehicles and unmanned aerial vehicles, and the research on bicycles is less.

Many researchers have adopted many different approaches to trajectory tracking control. Among these methods, the model predictive control algorithm is an online real-time control method, which can achieve a good control effect with a small amount of calculation required. The most important factor in the algorithm is the choice of the prediction model. Because the balance problem does not need to be considered in automobiles and other wheeled mobile robots, only a kinematic model of the automobile and other wheeled mobile robots needs to be established. However, in the case of a bicycle, the balance dynamics model of the bicycle causes a relatively complicated dynamic process when the handlebar of the bicycle is turned to the target turning angle, which has a large influence on the movement track of the bicycle. Therefore, in the bicycle trajectory tracking problem, it is not reasonable to predict the bicycle trajectory only by using a kinematic model, and thus the optimal tracking effect cannot be obtained.

For bicycles, it is impossible to achieve the best effect by using a kinematic model as a prediction model of a model prediction control algorithm to perform trajectory tracking control.

Disclosure of Invention

The invention provides a method for tracking and controlling a track of an unmanned bicycle, which is characterized in that a complex multi-dimensional kinematic model is established for the bicycle, the kinematic model is added with the dynamic characteristic that the bicycle turns under a self-balancing controller, the state of the bicycle at the future moment can be more accurately predicted by using the model, and the good effect can be achieved by adopting a model prediction control algorithm to track and control the bicycle.

A trajectory tracking control method of an unmanned bicycle is characterized by comprising the following steps:

step 1, establishing a balance dynamic model of a bicycle, and establishing a discrete state equation of a self-balancing control system of the bicycle after a self-balancing controller is added;

step 2, establishing a discrete kinematics model of the bicycle, and carrying out linearization;

step 3, combining the kinematic model with a self-balancing control system to establish a seven-dimensional state prediction model of the bicycle;

and 4, adopting the seven-dimensional state prediction model in the step 3 as a prediction model of a bicycle track tracking algorithm to perform bicycle track tracking control, and solving the optimal control input of each sampling moment on line.

The process for establishing the discrete state equation of the bicycle self-balancing control system in the step 1 is as follows:

the balance dynamic model of the bicycle is established as follows:

wherein,the inclination angle of the bicycle body and the steering angle of the handlebar of the bicycle, a is the gravity center G and the rear wheel landing point P of the bicycle₁H is the distance from the center of gravity of the bicycle to the ground when the bicycle body is not inclined, lambda is the front fork angle of the bicycle, v_xThe forward speed of the rear wheel of the bicycle, c the trailing of the bicycle, b the wheel base of the bicycle, g the acceleration of gravity, and s the laplacian operator;

the self-balancing control of the bicycle adopts a control structure of proportional-differential control and feedforward, and the expression of a control law is as follows:

wherein,_dis the steering angle command of the handlebar of the bicycle, namely the output of the self-balancing controller,the target body inclination angle for the bicycle, i.e. the input to the self-balancing controller,to relate toFirst derivative of (k)₁Is a proportionality coefficient, k₂Is a differential coefficient, 1/k₃Is a feedforward coefficient;

steering angle command for handlebar_dA low-pass filtering link exists between the steering angle of the actual handlebarThe link is a first-order inertia link, T_SIs a time constant;

the discrete state equation of the bicycle self-balancing control system is as follows:

matrix array $A_D=\left[\begin{array}{ccc}1& T& 0\\ (K3-\frac{K2\·k_1}{T_s})T/K1& 1-\frac{K2\·k_2}{K1\·T_s}T& (K4-\frac{K2}{T_s})T/K1\\ -\frac{k_1}{T_s}T& -\frac{k_2}{T_s}T& 1-\frac{1}{T_s}T\end{array}\right],$

Matrix array $B_D=\left[\begin{array}{c}0\\ \frac{K2\·k_1}{K1\·T_s}T-\frac{K2\·K3}{K4\·K1\·T_s}T\\ \frac{k_1}{T_s}T-\frac{K3}{K4\·T_s}T\end{array}\right]$

Wherein the intermediate variable K1 ═ mh²Intermediate variablesIntermediate variable K3 ═ mgh, intermediate variableCoefficient of feedforwardm is the mass of the bicycle,is the inclination angle of the bicycle body of the bicycle,to relate toIs related to the handlebar steering angle of the bicycle, T is the sampling period of the discrete system, and k is the sampling time sequence number.

The specific process of the step 2 is as follows:

the forward speed v of the bicycle is the forward speed v of the rear wheel of the bicycle_xI.e. v ═ v_x(ii) a Vehicle body inclination angleNeglecting the influence on the kinematics, obtaining a kinematic model of the bicycle:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_a=vcos{\mathrm{\ψ}}_a\\ {\stackrel{\·}{y}}_a={\mathrm{vsin\ψ}}_a\\ {\stackrel{\·}{\mathrm{\ψ}}}_a=v/R={\mathrm{vtan\δ}}_f/b\end{array}\right.$

wherein psi_aIs the yaw angle of the bicycle,to about psi_aThe first derivative of (a) is,_fis the effective steering angle of the bicycle, i.e. the projection of the steering angle on the ground, b is the wheel base of the bicycle, R is the turning radius of the bicycle during its movement, x_aFor the rear wheel landing point P of a bicycle₁The coordinate of X direction on the ground coordinate system O-XYZ,to relate to x_aFirst derivative of, y_aFor the rear wheel landing point P of a bicycle₁The coordinate of Y direction on the ground coordinate system O-XYZ,as to y_aFirst derivative of (2) since the bicycle is only moving at ground levelMoving, therefore, the bicycle's coordinates in the Z direction are not considered;

handlebar steering angle and effective handlebar steering angle for bicycle_fHave the following relationship between:

the body inclination angle of the bicycle is shown, and lambda is the front fork angle of the bicycle;

the bicycle moves at a constant speed v (k) ═ v, and the kinematic model is discretized to obtain:

$\left\{\begin{array}{c}{x}_a(k+1)=v\cos \left({\mathrm{\ψ}}_a\right(k\left)\right)T+x_a\left(k\right)\\ y_a(k+1)=v\sin \left({\mathrm{\ψ}}_a\right(k\left)\right)T+y_a\left(k\right)\\ {\mathrm{\ψ}}_a(k+1)=v\sin \mathrm{\λ}\tan \left(\mathrm{\δ}\right(k\left)\right)T/b+{\mathrm{\ψ}}_a\left(k\right)\end{array}\right.$

wherein, T is the sampling period of the discrete system, and k is the sampling time sequence number;

the discrete kinematic model is nonlinear, and the kinematic model is linearized near a reference trajectory to obtain a linear kinematic model:

$\tilde{x}(k+1)=A_K\left(k\right)\tilde{x}\left(k\right)+B_K\left(k\right)\tilde{u}\left(k\right)$

wherein, the matrix $A_K\left(k\right)=\left[\begin{array}{ccc}1& 0& -vsin\left({\mathrm{\ψ}}_r\right(k\left)\right)\·T\\ 0& 1& -vcos\left({\mathrm{\ψ}}_r\right(k\left)\right)\·T\\ 0& 0& 1\end{array}\right],$ Matrix array $B_K\left(k\right)=\left[\begin{array}{c}0\\ 0\\ vsin\mathrm{\λ}\[1+ta{n}^2\left({\mathrm{\δ}}_r\left(k\right)\right)\]\·T/b\end{array}\right],$

$\tilde{x}\left(k\right)={\[x_a\left(k\right)-x_r\left(k\right),y_a\left(k\right)-y_r\left(k\right),{\mathrm{\ψ}}_a\left(k\right)-{\mathrm{\ψ}}_r\left(k\right)\]}^T,\tilde{u}\left(k\right)={\[\mathrm{\δ}\left(k\right)-{\mathrm{\δ}}_r\left(k\right)\]}^T;$

Indicating the deviation amount of the actual state of the bicycle from the reference state;indicating the amount of deviation of the bicycle actual input from the reference input.

The seven-dimensional state prediction model of the bicycle in the step 3 is as follows:

combining a discrete state equation of a bicycle self-balancing control system with a kinematic model of a bicycle, and obtaining the following seven-dimensional state prediction model by sorting:

x(k+1)＝A·x(k)+B·u(k)

matrix array $A=\left[\begin{array}{ccccccc}1& 0& A_K(1,3)& 0& 0& 0& 0\\ 0& 0& A_K(2,3)& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& B_K(3,1)& 0\\ 0& 0& 0& 1& T& 0& 0\\ 0& 0& 0& A_D(2,1)& A_D(2,2)& A_D(2,3)& 0\\ 0& 0& 0& A_D(3,1)& A_D(3,2)& A_D(3,3)& A_D(3,3){\mathrm{\δ}}_r\left(k\right)-{\mathrm{\δ}}_r(k+1)\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right],$

Matrix array

x (k) are the state variables of the bicycle prediction model, and u (k) are the input variables of the bicycle prediction model.

The bicycle track tracking control algorithm in the step 4 comprises the following specific steps:

the index function J of model predictive control generally adopts a linear quadratic form, and in the track tracking problem of the bicycle, the control system is a discrete system, and the index function becomes:

$J=\frac{1}{2}\underset{k=k_0}{\overset{k_0+N-1}{\Σ}}\[x^T\left(k\right)Q\left(k\right)x\left(k\right)+u^T\left(k\right)R\left(k\right)u\left(k\right)\]dt+\frac{1}{2}{x}^T(k+N)Q_{0}x(k+N)$

wherein k is₀For the current time, N is the prediction range，t₀For the model predictive control of the starting time, t_fQ (t) is a semi-positive definite matrix with n × n dimensions, R (t) is a positive definite matrix with r × r dimensions, and Q is the end time of model predictive control₀Is a n × n-dimensional semi-positive definite matrix;

the current optimal input is obtained by solving the minimum value of the index function J, and the solving steps are as follows:

step 401: initially, let k equal to k₀+N，P(k₀+N)＝Q₀

Step 402: the input u (k) at the k-th time is g (k) · x (k), where g (k) is a state feedback coefficient matrix, and the expression is

G(k)＝-(R(k)+B(k)^TP(k+1)B(k))^-1B(k)^TP(k+1)A(k)

Wherein P (k +1) is the positive solution of the discrete time-varying system Riccati equation, and the expression of the discrete time-varying system Riccati equation is

P(k)＝Q(k)+A(k)^T(P(k+1))-P(k+1))B(k)(R(k)

+B(k)^TP(k+1)B(k))^-1B(k)^TP(k+1))A(k)

Step 403: k is all self-decreasing by 1, when k is k₀If so, the calculation is finished, otherwise, the step 402 is returned to;

after N times of reverse iteration, the optimal input sequence u (k) is obtained₀),u(k₀+1),.....u(k₀+ N-1), using the input value u (k) at the current time₀) The residual input value is discarded as the input of the current time of the system, and the calculation is repeated at each sampling time; and circulating according to the principle to obtain the optimal input value of each control period of the system.

Advantageous effects

The seven-dimensional state prediction model provided by the invention combines a bicycle kinematic model with the balance dynamics characteristic of a bicycle under a self-balancing controller, so that the control input of the bicycle is changed from the target handlebar steering angle of the bicycle to the target body inclination angle of the bicycle, the model can accurately reflect the actual situation of the running movement, and the state of the bicycle at the future moment is accurately predicted; therefore, the model predictive control algorithm can be better ensured to exert the advantages, and the bicycle can obtain good effect of tracking the target track; because the model is linear, the calculated amount is much smaller than that of a nonlinear model, and therefore the model is very suitable for the online and high-frequency control method of model predictive control. The bicycle trajectory tracking control is carried out by utilizing a model predictive control algorithm, the optimal control input is obtained through matrix iterative calculation, the calculated amount is small, the characteristic of online real-time control of the algorithm is ensured, and the online real-time performance of the algorithm also ensures the good control effect of the algorithm.

Drawings

FIG. 1 is a schematic view of a bicycle

FIG. 2 is a block diagram of a self-balancing control system for a bicycle

FIG. 3 is a schematic diagram of a model predictive control algorithm

FIG. 4 is a schematic diagram of a bicycle tracking simulation system

FIG. 5 is a simulation result of a bicycle tracking straight-line trajectory

FIG. 6 shows the simulation result of the circular track tracing of the bicycle

FIG. 7 is a simulation result of a bicycle tracking straight line-circle combined track

FIG. 8 is a flow chart of a method for controlling the trajectory tracking of an unmanned bicycle according to the present invention

Detailed Description

The following describes a trajectory tracking control method for an unmanned bicycle according to the present invention in detail with reference to the accompanying drawings.

The invention carries out the track following control of the bicycle by a model predictive control algorithm. Firstly, aiming at a bicycle, establishing a balance dynamic model of the bicycle, and establishing a system state equation after a self-balancing controller is added; secondly, establishing a kinematic model of the bicycle and carrying out linearization; then combining the kinematic model with a self-balancing control system, and establishing a seven-dimensional state prediction model after corresponding simplification processing; and finally, selecting an index function of a linear quadratic form, setting weight matrix parameters of the index function, and solving the optimal control input at each sampling moment on line. Fig. 8 is a flowchart of a trajectory tracking control method of the unmanned bicycle of the present invention.

1. Establishing a discrete state equation of a bicycle self-balancing control system:

first, a schematic structural view of a bicycle is given, as shown in fig. 1.

Establishing a balance dynamic model of the bicycle as shown in formula (1):

wherein,the inclination angle of the bicycle body and the steering angle of the handlebar of the bicycle, a is the gravity center G and the rear wheel landing point P of the bicycle₁H is the distance from the center of gravity of the bicycle to the ground when the bicycle body is not inclined, lambda is the front fork angle of the bicycle, v_xForward speed of the rear wheel of the bicycle, c trailing of the bicycle, i.e. P₂P₃B is the bicycle wheelbase, i.e. C₁C₂G is the acceleration of gravity and s is the laplace operator.

The self-balancing control system of a bicycle is illustrated in fig. 2. The self-balancing control of the bicycle adopts a control structure of proportional-differential control and feedforward, and the expression of a control law is shown as a formula (2):

wherein,_dis the steering angle command of the handlebar of the bicycle, namely the output of the self-balancing controller,for a target body inclination of the bicycle, i.e. input from a self-balancing controller, k₁Is a proportionality coefficient, k₂Is a differential coefficient, 1/k₃Is a feed forward coefficient.

Wherein, as shown in FIG. 2, the steering angle command of the handlebar_dA low-pass filtering link exists between the steering angle of the actual handlebarThe link is a first-order inertia link, T_SIs a time constant.

In summary, the equation of state of the bicycle self-balancing control system is expressed in the form:

$A_D=\left[\begin{array}{ccc}0& 1& 0\\ (K3-\frac{K2\·k_1}{T_s})/K1& -\frac{K2\·k_2}{K1\·T_s}& (K4-\frac{K2}{T_s})/K1\\ -\frac{k_1}{T_s}& -\frac{k_2}{T_s}& -\frac{1}{T_s}\end{array}\right],B_D=\left[\begin{array}{c}0\\ \frac{K2\·k_1}{K1\·T_s}-\frac{K2\·K3}{K4\·K1\·T_s}\\ \frac{k_1}{T_s}-\frac{K3}{K4\·T_s}\end{array}\right]$

wherein the intermediate variable K1 ═ mh²Intermediate variablesIntermediate variable K3 ═ mgh, intermediate variableCoefficient of feedforwardm is the mass of the bicycle,is the inclination angle of the bicycle body of the bicycle,to relate toThe first derivative of (a) is,to relate toThe second derivative of (a), with respect to the handlebar steering angle of the bicycle,is the first derivative of interest;

discretizing the state equation by using an Euler method to obtain a discrete state equation of the bicycle self-balancing control system:

$A_D=\left[\begin{array}{ccc}1& T& 0\\ (K3-\frac{K2\·k_1}{T_s})T/K1& 1-\frac{K2\·k_2}{K1\·T_s}T& (K4-\frac{K2}{T_s})T/K1\\ -\frac{k_1}{T_s}T& -\frac{k_2}{T_s}T& 1-\frac{1}{T_s}T\end{array}\right],B_D=\left[\begin{array}{c}0\\ \frac{K2\·k_1}{K1\·T_s}T-\frac{K2\·K3}{K4\·K1\·T_s}T\\ \frac{k_1}{T_s}T-\frac{K3}{K4\·T_s}T\end{array}\right]$

wherein, T is the sampling period of the discrete system, and k is the sampling time sequence number.

2. Establishing a discrete kinematic model of the bicycle:

the structure of a common electric bicycle is that a front wheel controls steering and a rear wheel controls speed. Wherein, the establishment of the kinematic model is based on the following two assumptions:

wheel side slip is not considered. In this case, the bicycle forward speed is the rear wheel forward speed, i.e. v ═ v_x(ii) a Due to the angle of inclination of the vehicle bodySmall and therefore has negligible effect on kinematics.

Thus, referring to fig. 1, a kinematic model of the bicycle is derived:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_a={\mathrm{vcos\ψ}}_a\\ {\stackrel{\·}{y}}_a={\mathrm{vsin\ψ}}_a\\ {\stackrel{\·}{\mathrm{\ψ}}}_a=v/R={\mathrm{vtan\δ}}_f/b\end{array}\right.---\left(5\right)$

wherein psi_aIs the yaw angle of the bicycle,to about psi_aThe first derivative of (a) is,_fis the effective steering angle of the bicycle, i.e. the projection of the steering angle on the ground, b is the wheel base of the bicycle, R is the turning radius of the bicycle during its movement, x_aFor the rear wheel landing point P of a bicycle₁The coordinate of X direction on the ground coordinate system O-XYZ,to relate to x_aFirst derivative of, y_aFor the rear wheel landing point P of a bicycle₁The coordinate of Y direction on the ground coordinate system O-XYZ,as to y_aThe first derivative of (c), since the bicycle only moves at ground level, does not take into account the coordinates of the bicycle in the Z direction.

Discretizing the continuous kinematic model is obtained from a discrete kinematic model of the vehicle:

$\left\{\begin{array}{c}{x}_a(k+1)=v\left(k\right)\×cos\left({\mathrm{\ψ}}_a\right(k\left)\right)\×T+x_a\left(k\right)\\ y_a(k+1)=v\left(k\right)\×sin\left({\mathrm{\ψ}}_a\right(k\left)\right)\×T+y_a\left(k\right)\\ {\mathrm{\ψ}}_a(k+1)=v\left(k\right)\×tan\left({\mathrm{\δ}}_f\right(k\left)\right)\×T/b+{\mathrm{\ψ}}_a\left(k\right)\end{array}\right.---\left(6\right)$

since the bicycle is moving at a constant speed, v (k) ═ v.

Handlebar steering angle and effective handlebar steering angle for bicycle_fHave the following relationship between:

substituting the formula (6) to obtain:

$\left\{\begin{array}{c}{x}_a(k+1)=v\cos \left({\mathrm{\ψ}}_a\right(k\left)\right)T+x_a\left(k\right)\\ y_a(k+1)=v\sin \left({\mathrm{\ψ}}_a\right(k\left)\right)T+y_a\left(k\right)\\ {\mathrm{\ψ}}_a(k+1)=v\sin \mathrm{\λ}\tan \left(\mathrm{\δ}\right(k\left)\right)T/b+{\mathrm{\ψ}}_a\left(k\right)\end{array}\right.---\left(8\right)$

the bicycle kinematic model shown in the formula (8) is nonlinear, and is developed by directly taking the target track as a reference by using a Taylor formula to establish a bicycle linear kinematic model related to errors.

Target trajectory is denoted x_r(k)＝[x_r(k),y_r(k),ψ_r(k)]^T，u_r(k)＝[_r(k)]^T. Wherein x is_r(k) Reference state variable, u, for the target trajectory_r(k) Reference input variable, x, for the target trajectory_rFor the rear wheel landing point P of a bicycle₁In the X direction, y, of the ground coordinate system O-XYZ_rFor the rear wheel landing point P of a bicycle₁And target coordinate values in the Y direction on the ground coordinate system O-XYZ. Psi_rIs the target yaw angle of the bicycle,_rfor a target handlebar steering angle of a bicycle, the expression of the first order Taylor formula after omitting the high order term is as follows:

$\stackrel{\·}{\tilde{x}}=f_{x,r}\tilde{x}+f_{u,r}\tilde{u}---\left(9\right)$

f_x,rpartial derivative of bicycle kinematics model with respect to x is x_rThe value of time; f. of_u,rPartial derivative of bicycle kinematics model with respect to u in u ═ u_rThe value of time;indicating the deviation amount of the actual state of the bicycle from the reference state;indicating the amount of deviation of the bicycle actual input from the reference input.

Since only the first order term is retained, the bicycle kinematic model derived from this equation is a linear model, and equation (8) is substituted into (9):

$\tilde{x}(k+1)=A_K\left(k\right)\tilde{x}\left(k\right)+B_K\left(k\right)\tilde{u}\left(k\right)---\left(10\right)$

wherein, $A_K\left(k\right)=\left[\begin{array}{ccc}1& 0& -v\sin \left({\mathrm{\ψ}}_r\right(k\left)\right)\·T\\ 0& 1& -v\cos \left({\mathrm{\ψ}}_r\right(k\left)\right)\·T\\ 0& 0& 1\end{array}\right],B_K\left(k\right)=\left[\begin{array}{c}0\\ 0\\ v\sin \mathrm{\λ}\[1+{\tan }^2\left({\mathrm{\δ}}_r\right(k\left)\right)\]\·T/b\end{array}\right],$

$\tilde{x}\left(k\right)={\[x_a\left(k\right)-x_r\left(k\right),y_a\left(k\right)-y_r\left(k\right),{\mathrm{\ψ}}_a\left(k\right)-{\mathrm{\ψ}}_r\left(k\right)\]}^T,\tilde{u}\left(k\right)={\[\mathrm{\δ}\left(k\right)-{\mathrm{\δ}}_r\left(k\right)\]}^T.$

3. establishing a bicycle state prediction model:

combining the bicycle dynamic state equation with the balance controller and the bicycle kinematic state equation, the following set of differential equations is listed:

transforming the sixth differential equation into the form:

the equation set is arranged to obtain the following seven-dimensional state prediction model:

x(k+1)＝A·x(k)+B·u(k)(11)

x (k) are the state variables of the bicycle prediction model, u (k) are the input variables of the bicycle prediction model, the specific definitions are indicated below,

$A=\left[\begin{array}{ccccccc}1& 0& A_K(1,3)& 0& 0& 0& 0\\ 0& 0& A_K(2,3)& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& B_K(3,1)& 0\\ 0& 0& 0& 1& T& 0& 0\\ 0& 0& 0& A_D(2,1)& A_D(2,2)& A_D(2,3)& 0\\ 0& 0& 0& A_D(3,1)& A_D(3,2)& A_D(3,3)& A_D(3,3){\mathrm{\δ}}_r\left(k\right)-{\mathrm{\δ}}_r(k+1)\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right],B\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ B_D(2,1)\\ B_D(3,1)\\ 0\end{array}\right],$

4. bicycle track tracking control algorithm based on model predictive control

Model predictive control, also known as rolling horizon control or rolling optimal control, originated in the last 60 s. The method utilizes the explicit model of the controlled object to predict the state of the controlled object at a future moment from the current state of the controlled object. The prediction capability can calculate a control sequence which enables a target control index to be optimal on line in real time, so that the behavior of a controlled object at a future moment is optimized. The result of the optimization will act on the system according to the principle of the rolling time domain. Therefore, the core of the model prediction control is three parts of model prediction, rolling optimization and feedback correction.

Model predictive control has been widely used for decades in industrial process control because it can explicitly integrate control objectives and operational constraints into an optimization problem and solve it online in each control cycle. Since on-line calculation is required and the time is slow, model predictive control is generally applied to a field such as a factory where the control frequency is low. However, in recent years, with the rapid increase in computer computation speed, model predictive control has been increasingly applied to the field of high-frequency control such as mobile robots.

The principle of model predictive control is shown in fig. 3. For a discrete system, at a certain time T, a model predictive control algorithm calculates the optimal control input u of the system at a certain time sequence T, T + T, a_t,u_t+T,....,u_t+(N-1)T. Wherein T is a system sampling period, and N is a prediction range of model prediction control. But only the input value u at the current time_tIs adopted as the input of the current time of the system, and the rest of the input value u_t+T,....,u_t+(N-1)TIs discarded and at the next sampling instant T + T the calculation at the previous instant is repeated. And circulating according to the principle to obtain the optimal input value of each control period of the system. Therefore, model predictive control is an on-line real-time control method that automatically adjusts to the current state of the system and to predicted future conditionsAnd (6) outputting the most appropriate control strategy.

The index function J of model predictive control generally takes the form of a linear quadratic form, and the expression is as follows:

$J\left(t_0\right)=\frac{1}{2}\underset{t_0}{\overset{t_f}{\∫}}\[x^{T}Q\left(t\right)x+u^{T}R\left(t\right)u\]dt+\frac{1}{2}{x}^T\left(t_f\right)Q_{0}x\left(t_f\right)---\left(12\right)$

in the formula t₀For the model predictive control of the starting time, t_fQ (t) is a semi-positive definite matrix with n × n dimensions, R (t) is a positive definite matrix with r × r dimensions, and Q is the end time of model predictive control₀Is a n × n-dimensional semi-positive definite matrix;

in practical engineering, since the components of the state variable and the input variable are generally independent of each other, and the cross terms are meaningless, q (t) and r (t) often take diagonal matrices. In general, the objective of the linear quadratic form is to minimize J, and the practical significance is to optimize the combination of the error index and energy consumption by keeping the state error small with a small input.

Since in the bicycle tracking problem the control system is a discrete system, the indicator function (12) becomes of the form:

$J=\frac{1}{2}\underset{k=k_0}{\overset{k_0+N-1}{\Σ}}\[x^T\left(k\right)Q\left(k\right)x\left(k\right)+u^T\left(k\right)R\left(k\right)u\left(k\right)\]dt+\frac{1}{2}{x}^T(k+N)Q_{0}x(k+N)---\left(13\right)$

wherein k is₀And N is the prediction range at the current moment.

And solving the minimum value of the index function J to obtain the current optimal input. The bicycle prediction model is a linear model, and the solving steps are as follows:

step 1: initially, let k equal to k₀+N，P(k₀+N)＝Q₀

Step 2: the input u (k) at the k-th time is g (k) · x (k), where g (k) is a state feedback coefficient matrix, and the expression is

G(k)＝-(R(k)+B(k)^TP(k+1)B(k))^-1B(k)^TP(k+1)A(k)

Wherein, P (k +1) is the positive solution of the discrete time varying system Riccati equation, and the discrete time varying system Riccati equation expression is as follows:

P(k)＝Q(k)+A(k)^T(P(k+1))-P(k+1))B(k)(R(k)

+B(k)^TP(k+1)B(k))^-1B(k)^TP(k+1))A(k)

and step 3: k is all self-decreasing by 1, when k is k₀If so, the calculation is finished, otherwise, the step 2 is returned.

Thus, after N reverse iterations, the optimal input sequence u (k) is obtained₀),u(k₀+1),.....u(k₀+ N-1), but only the input value u (k) at the current time₀) Is taken as input to the system at the current time, and the remaining input values are discarded, after which the above calculations are repeated at each sampling time. Circulating according to the principle to obtain each control period of the systemThe optimum input value of (2).

It should be noted that the target for the bicycle track following control is to make four deviations x_a(k)-x_r(k)，y_a(k)-y_r(k)，ψ_a(k)-ψ_r(k) And (k) -_r(k) And the weight value tends to 0, and the values of other state quantities and the input u in the state vector x are not concerned, so that the weight values of the four deviation quantities are required to be set to be far larger than the other state quantities when the weight matrix Q and the weight matrix R are selected. Furthermore, since dynamic programming is unconstrained to solve for the optimal target body-tilt angle of the bicycle, in practical situations, the target body-tilt angle of the bicycle cannot be too large, otherwise the bicycle will not be able to remain balanced. Therefore, the obtained target body lean angle needs to be limited.

FIG. 4 is a diagram of a bicycle trajectory tracking simulation system. Several typical target tracks are selected for carrying out the dynamics simulation of bicycle track tracking control, and the simulation results are respectively shown as a bicycle tracking straight line track simulation result in fig. 5, a bicycle tracking circular track simulation result in fig. 6 and a bicycle tracking straight line-circle combined track simulation result in fig. 7.

Claims (5)

1. A trajectory tracking control method of an unmanned bicycle is characterized by comprising the following steps:

step 1, establishing a balance dynamic model of a bicycle, and establishing a discrete state equation of a self-balancing control system of the bicycle after a self-balancing controller is added;

step 2, establishing a discrete kinematics model of the bicycle, and carrying out linearization;

step 3, combining the kinematic model with a self-balancing control system to establish a seven-dimensional state prediction model of the bicycle;

and 4, adopting the seven-dimensional state prediction model in the step 3 as a prediction model of a bicycle track tracking algorithm to perform bicycle track tracking control, and solving the optimal control input of each sampling moment on line.

2. The method for controlling the trajectory tracking of the unmanned bicycle according to claim 1, wherein the discrete state equation of the bicycle self-balancing control system in step 1 is established by:

the balance dynamic model of the bicycle is established as follows:

wherein,the inclination angle of the bicycle body and the steering angle of the handlebar of the bicycle, a is the gravity center G and the rear wheel landing point P of the bicycle₁H is the distance from the center of gravity of the bicycle to the ground when the bicycle body is not inclined, lambda is the front fork angle of the bicycle, v_xThe forward speed of the rear wheel of the bicycle, c the trailing of the bicycle, b the wheel base of the bicycle, g the acceleration of gravity, and s the laplacian operator;

the self-balancing control of the bicycle adopts a control structure of proportional-differential control and feedforward, and the expression of a control law is as follows:

wherein,_dis the steering angle command of the handlebar of the bicycle, namely the output of the self-balancing controller,the target body inclination angle for the bicycle, i.e. the input to the self-balancing controller,to relate toFirst derivative of (k)₁Is a proportionality coefficient, k₂Is a differential coefficient, 1/k₃Is a feedforward coefficient;

steering angle command for handlebar_dA low-pass filtering link exists between the steering angle of the actual handlebarThe link is a first-order inertia link, T_SIs a time constant;

the discrete state equation of the bicycle self-balancing control system is as follows:

matrix array $A_D=\left[\begin{array}{ccc}1& T& 0\\ (K3-\frac{K2\·k_1}{T_s})T/K1& 1-\frac{K2\·k_2}{K1\·T_s}T& (K4-\frac{K2}{T_s})T/K1\\ -\frac{k_1}{T_s}T& -\frac{k_2}{T_s}T& 1-\frac{1}{T_s}T\end{array}\right],$

Matrix array $B_D=\left[\begin{array}{c}0\\ \frac{K2\·k_1}{K1\·T_s}T-\frac{K2\·K3}{K4\·K1\·T_s}T\\ \frac{k_1}{T_s}T-\frac{K3}{K4\·T_s}T\end{array}\right]$

Wherein the intermediate variable K1 ═ mh²Intermediate variablesIntermediate variable K3 ═ mgh, intermediate variable $K4=\frac{({{\mathrm{mv}}_x}^{2}h-macgsin\mathrm{\λ})sin\mathrm{\λ}}{b},$ Coefficient of feedforward $\frac{1}{k_3}=-\frac{K3}{K4},$ m is the mass of the bicycle,is the inclination angle of the bicycle body of the bicycle,to relate toIs related to the handlebar steering angle of the bicycle, T is the sampling period of the discrete system, and k is the sampling time sequence number.

3. The method for controlling the trajectory tracking of the unmanned bicycle as claimed in claim 2, wherein the specific process of step 2 is as follows:

the forward speed v of the bicycle is the forward speed v of the rear wheel of the bicycle_xI.e. v ═ v_x(ii) a Vehicle body inclination angleNeglecting the influence on the kinematics, obtaining a kinematic model of the bicycle:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_a=vcos{\mathrm{\ψ}}_a\\ {\stackrel{\·}{y}}_a={\mathrm{vsin\ψ}}_a\\ {\stackrel{\·}{\mathrm{\ψ}}}_a=v/R={\mathrm{vtan\δ}}_f/b\end{array}\right.$

wherein psi_aIs the yaw angle of the bicycle,to about psi_aThe first derivative of (a) is,_fis the effective steering angle of the bicycle, i.e. the projection of the steering angle on the ground, b is the wheel base of the bicycle, R is the turning radius of the bicycle during its movement, x_aFor the rear wheel landing point P of a bicycle₁The coordinate of X direction on the ground coordinate system O-XYZ,to relate to x_aFirst derivative of, y_aFor the rear wheel landing point P of a bicycle₁The coordinate of Y direction on the ground coordinate system O-XYZ,as to y_aThe first derivative of (a), since the bicycle only moves at ground level, the coordinates of the bicycle in the Z direction are not taken into account;

handlebar steering angle and effective handlebar steering angle for bicycle_fHave the following relationship between:

the body inclination angle of the bicycle is shown, and lambda is the front fork angle of the bicycle;

the bicycle moves at a constant speed v (k) ═ v, and the kinematic model is discretized to obtain:

$\left\{\begin{array}{c}{x}_a(k+1)=v\cos \left({\mathrm{\ψ}}_a\right(k\left)\right)T+x_a\left(k\right)\\ y_a(k+1)=v\sin \left({\mathrm{\ψ}}_a\right(k\left)\right)T+y_a\left(k\right)\\ {\mathrm{\ψ}}_a(k+1)=v\sin \mathrm{\λ}\tan \left(\mathrm{\δ}\right(k\left)\right)T/b+{\mathrm{\ψ}}_a\left(k\right)\end{array}\right.$

wherein, T is the sampling period of the discrete system, and k is the sampling time sequence number;

the discrete kinematic model is nonlinear, and the kinematic model is linearized near a reference trajectory to obtain a linear kinematic model:

$\tilde{x}(k+1)=A_K\left(k\right)\tilde{x}\left(k\right)+B_K\left(k\right)\tilde{u}\left(k\right)$

wherein, the matrix $A_K\left(k\right)=\left[\begin{array}{ccc}1& 0& -vsin\left({\mathrm{\ψ}}_r\right(k))\·T\\ 0& 1& -v\cos \left({\mathrm{\ψ}}_r\right(k))\·T\\ 0& 0& 1\end{array}\right],$ Matrix array $B_K\left(k\right)=\left[\begin{array}{c}0\\ 0\\ vsin\mathrm{\λ}\[1+ta{n}^2\left({\mathrm{\δ}}_r\right(k\left)\right)\]\·T/b\end{array}\right],$

$\begin{array}{cc}\tilde{x}\left(k\right)={\[x_a\left(k\right)-x_r\left(k\right),y_a\left(k\right)-y_r\left(k\right),{\mathrm{\ψ}}_a\left(k\right)-{\mathrm{\ψ}}_r\left(k\right)\]}^T,& \tilde{u}\left(k\right)={\[\mathrm{\δ}\left(k\right)-{\mathrm{\δ}}_r\left(k\right)\]}^T\end{array};$

Indicating the deviation amount of the actual state of the bicycle from the reference state;indicating the amount of deviation of the bicycle actual input from the reference input.

4. The method as claimed in claim 3, wherein the seven-dimensional state prediction model of the bicycle in step 3 is:

combining a discrete state equation of a bicycle self-balancing control system with a kinematic model of a bicycle, and obtaining the following seven-dimensional state prediction model by sorting:

x(k+1)＝A·x(k)+B·u(k)

matrix array $A=\left[\begin{array}{ccccccc}1& 0& A_K(1,3)& 0& 0& 0& 0\\ 0& 0& A_K(2,3)& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& B_K(3,1)& 0\\ 0& 0& 0& 1& T& 0& 0\\ 0& 0& 0& A_D(2,1)& A_D(2,2)& A_D(2,3)& 0\\ 0& 0& 0& A_D(3,1)& A_D(3,2)& A_D(3,3)& A_D(3,3){\mathrm{\δ}}_r\left(k\right)-{\mathrm{\δ}}_r(k+1)\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right],$

Matrix array $B=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ B_D(2,1)\\ B_D(3,1)\\ 0\end{array}\right],$

x (k) are the state variables of the bicycle prediction model, and u (k) are the input variables of the bicycle prediction model.

5. The method for controlling the trajectory tracking of the unmanned bicycle according to claim 4, wherein the bicycle trajectory tracking control algorithm in the step 4 comprises the following specific steps:

the index function J of model predictive control generally adopts a linear quadratic form, and in the track tracking problem of the bicycle, the control system is a discrete system, and the index function becomes:

$J=\frac{1}{2}\underset{k=k_0}{\overset{k_0+N-1}{\mathrm{\Σ}}}\[x^T\left(k\right)Q\left(k\right)x\left(k\right)+u^T\left(k\right)R\left(k\right)u\left(k\right)\]dt+\frac{1}{2}{x}^T(k+N)Q_{0}x(k+N)$

wherein k is₀Is the current time, N is the prediction range, t₀For the model predictive control of the starting time, t_fQ (t) is a semi-positive definite matrix with n × n dimensions, R (t) is a positive definite matrix with r × r dimensions, and Q is the end time of model predictive control₀Is a n × n-dimensional semi-positive definite matrix;

the current optimal input is obtained by solving the minimum value of the index function J, and the solving steps are as follows:

step 401: initially, let k equal to k₀+N，P(k₀+N)＝Q₀

Step 402: the input u (k) at the k-th time is g (k) · x (k), where g (k) is a state feedback coefficient matrix, and the expression is

G(k)＝-(R(k)+B(k)^TP(k+1)B(k))^-1B(k)^TP(k+1)A(k)

Wherein P (k +1) is the positive solution of the discrete time-varying system Riccati equation, and the expression of the discrete time-varying system Riccati equation is

P(k)＝Q(k)+A(k)^T(P(k+1))-P(k+1))B(k)(R(k)

+B(k)^TP(k+1)B(k))^-1B(k)^TP(k+1))A(k)

Step 403: k is all self-decreasing by 1, when k is k₀If so, the calculation is finished, otherwise, the step 402 is returned to;

after N times of reverse iteration, the optimal input sequence u (k) is obtained₀),u(k₀+1),.....u(k₀+ N-1), using the input value u (k) at the current time₀) The residual input value is discarded as the input of the current time of the system, and the calculation is repeated at each sampling time; and circulating according to the principle to obtain the optimal input value of each control period of the system.