[0069] The following describes in detail the trajectory tracking control method of an unmanned bicycle proposed by the present invention with reference to the accompanying drawings.
[0070] The invention uses the model predictive control algorithm to carry out the trajectory tracking control of the bicycle. First, for the bicycle, establish its balance dynamics model, and establish the system state equation after adding the self-balance controller; secondly, establish the bicycle kinematics model and linearize it; then compare the kinematics model with the self-balance control system Combined, after corresponding simplification processing, a seven-dimensional state prediction model is established; finally, the linear quadratic index function is selected, the weight matrix parameters are set, and the optimal control input at each sampling time is solved online. Picture 8 It is a flowchart of the trajectory tracking control method of the unmanned bicycle of the present invention.
[0071] 1. Establish the discrete state equation of the bicycle self-balancing control system:
[0072] First, give a schematic diagram of the bicycle structure, such as figure 1 Shown.
[0073] Establish a bicycle balance dynamics model, as shown in formula (1):
[0074]
[0075] among them, Is the inclination angle of the bicycle body, δ is the steering angle of the handlebar of the bicycle, and a is the center of gravity of the bicycle G and the location of the rear wheel P 1 H is the distance from the center of gravity of the bicycle to the ground when the body is not tilted, λ is the fork angle of the bicycle, v x Is the forward speed of the rear wheel of the bicycle, c is the tail of the bicycle, namely P 2 P 3 , B is the wheelbase of the bicycle, namely C 1 C 2 , G is the acceleration due to gravity, and s is the Laplace operator.
[0076] The structure diagram of the bicycle's self-balancing control system is as follows figure 2 Shown. The self-balancing control of bicycle adopts the control structure of proportional-derivative control plus feedforward, and the control law expression is shown in formula (2):
[0077]
[0078] Where δ d Is the steering angle command of the handlebar of the bicycle, which is the output of the self-balancing controller, Is the target body tilt angle of the bicycle, that is, the input of the self-balancing controller, k 1 Is the scale factor, k 2 Is the differential coefficient, 1/k 3 Is the feedforward coefficient.
[0079] Among them, such as figure 2 As shown, the steering angle command of the handlebar δ d There is a low-pass filter link between the actual handlebar steering angle δ This link is a first-order inertial link, T S Is the time constant.
[0080] In summary, the state equation of the bicycle self-balancing control system is expressed in the following form:
[0081]
[0082] A D = 0 1 0 ( K 3 - K 2 · k 1 T s ) / K 1 - K 2 · k 2 K 1 · T s ( K 4 - K 2 T s ) / K 1 - k 1 T s - k 2 T s - 1 T s , B D = 0 K 2 · k 1 K 1 · T s - K 2 · K 3 K 4 · K 1 · T s k 1 T s - K 3 K 4 · T s
[0083] Among them, the intermediate variable K1=mh 2 ,Intermediate variables Intermediate variable K3 = mgh, intermediate variable Feedforward coefficient m is the quality of the bicycle, Is the tilt angle of the bicycle body, For about The first derivative of For about The second derivative of, δ is the steering angle of the handlebar of the bicycle, Is the first derivative with respect to δ;
[0084] Use Euler's method to discretize the above-mentioned state equation to obtain the discrete state equation of the bicycle self-balancing control system:
[0085]
[0086] A D = 1 T 0 ( K 3 - K 2 · k 1 T s ) T / K 1 1 - K 2 · k 2 K 1 · T s T ( K 4 - K 2 T s ) T / K 1 - k 1 T s T - k 2 T s T 1 - 1 T s T , B D = 0 K 2 · k 1 K 1 · T s T - K 2 · K 3 K 4 · K 1 · T s T k 1 T s T - K 3 K 4 · T s T
[0087] Among them, T is the sampling period of the discrete system, and k is the sampling time sequence number.
[0088] 2. Establish the discrete kinematics model of bicycle:
[0089] The structure of an ordinary electric bicycle is that the front wheel controls the steering and the rear wheel controls the speed. Among them, the establishment of the kinematics model is based on the following two assumptions:
[0090] Does not consider wheel side slip. In this case, the forward speed of the bicycle is the forward speed of the rear wheel, namely v=v x; Due to the inclination of the body Very small, so the impact on kinematics is negligible.
[0091] Therefore, contrast figure 1 , Get the kinematics model of bicycle:
[0092] x · a = vcosψ a y · a = vsinψ a ψ · a = v / R = vtanδ f / b - - - ( 5 )
[0093] Where ψ a Is the yaw angle of the bicycle, Regarding ψ a The first derivative of, δ f Is the effective handlebar steering angle of the bicycle, that is, the projection of the handlebar steering angle δ on the ground, b is the bicycle wheelbase, R is the turning radius when the bicycle is moving, x a Is the drop point P for the rear wheel of the bicycle 1 The coordinates in the X direction on the ground coordinate system O-XYZ, For about x a The first derivative of, y a Is the drop point P for the rear wheel of the bicycle 1 The coordinates in the Y direction on the ground coordinate system O-XYZ, For about y a Because the bicycle only moves on the ground plane, the coordinates of the bicycle in the Z direction are not considered.
[0094] Discretize the above continuous kinematics model to obtain the discrete kinematics model of bicycle:
[0095] x a ( k + 1 ) = v ( k ) X c o s ( ψ a ( k ) ) X T + x a ( k ) y a ( k + 1 ) = v ( k ) X s i n ( ψ a ( k ) ) X T + y a ( k ) ψ a ( k + 1 ) = v ( k ) X t a n ( δ f ( k ) ) X T / b + ψ a ( k ) - - - ( 6 )
[0096] Since the bicycle is moving at a constant speed, v(k)=v.
[0097] Handlebar steering angle δ of bicycle and effective handlebar steering angle δ f There are the following relationships:
[0098]
[0099] Substituting formula (6) into:
[0100] x a ( k + 1 ) = v cos ( ψ a ( k ) ) T + x a ( k ) y a ( k + 1 ) = v sin ( ψ a ( k ) ) T + y a ( k ) ψ a ( k + 1 ) = v sin λ tan ( δ ( k ) ) T / b + ψ a ( k ) - - - ( 8 )
[0101] The bicycle kinematics model shown in formula (8) is nonlinear, and the Taylor formula is used to directly expand with the target trajectory as a reference to establish a bicycle linear kinematics model with respect to errors.
[0102] The target trajectory is expressed as x r (k)=[x r (k),y r (k),ψ r (k)) T , U r (k)=[δ r (k)) T. Where x r (k) is the reference state variable of the target trajectory, u r (k) is the reference input variable of the target trajectory, x r Is the drop point P for the rear wheel of the bicycle 1 X direction on the ground coordinate system O-XYZ, y r Is the drop point P for the rear wheel of the bicycle 1 The target coordinate value in the Y direction on the ground coordinate system O-XYZ. ψ r Is the target yaw angle of the bicycle, δ r For the target handlebar steering angle of the bicycle, the expression of the first-order Taylor formula after omitting the high-order terms is as follows:
[0103] x ~ · = f x , r x ~ + f u , r u ~ - - - ( 9 )
[0104] f x,r Is the partial derivative of the bicycle kinematics model with respect to x at x = x r Time value; f u,r Is the partial derivative of the bicycle kinematics model with respect to u at u = u r Time value Indicates the deviation between the actual bicycle state and the reference state; Indicates the deviation between the actual bicycle input and the reference input.
[0105] Since only the first-order term is retained, the bicycle kinematics model derived from this formula is a linear model. Substitute formula (8) into (9) to obtain:
[0106] x ~ ( k + 1 ) = A K ( k ) x ~ ( k ) + B K ( k ) u ~ ( k ) - - - ( 10 )
[0107] among them, A K ( k ) = 1 0 - v sin ( ψ r ( k ) ) · T 0 1 - v cos ( ψ r ( k ) ) · T 0 0 1 , B K ( k ) = 0 0 v sin λ [ 1 + tan 2 ( δ r ( k ) ) ] · T / b ,
[0108] x ~ ( k ) = [ x a ( k ) - x r ( k ) , y a ( k ) - y r ( k ) , ψ a ( k ) - ψ r ( k ) ] T , u ~ ( k ) = [ δ ( k ) - δ r ( k ) ] T .
[0109] 3. Establish a bicycle state prediction model:
[0110] Combine the bicycle dynamics state equation with the balance controller and the bicycle kinematics state equation, and list the following differential equations:
[0111]
[0112] The sixth differential equation is transformed into:
[0113]
[0114] Arrange the above equations to get the following seven-dimensional state prediction model:
[0115] x(k+1)=A·x(k)+B·u(k)(11)
[0116] x(k) is the state variable of the bicycle prediction model, and u(k) is the input variable of the bicycle prediction model. The specific definitions are marked below,
[0117] A = 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 ) δ r ( k ) - δ r ( k + 1 ) 0 0 0 0 0 0 1 , B 0 0 0 0 B D ( 2 , 1 ) B D ( 3 , 1 ) 0 ,
[0118]
[0119] 4. Bicycle trajectory tracking control algorithm based on model predictive control
[0120] Model predictive control, also known as rolling time domain control or rolling optimal control, originated in the 1960s. It uses the explicit model of the controlled object to predict the state of the controlled object in the future from the current state of the controlled object. This predictive ability can calculate a control sequence that optimizes the target control index in real time online, thereby optimizing the behavior of the controlled object in the future. The optimized result will act on the system according to the principle of rolling time domain. Therefore, the core of model predictive control is model prediction, rolling optimization and feedback correction.
[0121] Because model predictive control can clearly integrate control objectives and operating constraints into optimization problems and solve them online in each control cycle, it has been widely used in industrial process control for decades. Due to the need for online calculation and slow time, model predictive control is generally used in factories and other areas with low control frequency at first. However, in recent years, with the tremendous increase in computer computing speed, model predictive control has gradually been applied to high-frequency control fields such as mobile robots.
[0122] The principle of model predictive control is as follows image 3 Shown. For discrete systems, at a certain time t, the model predictive control algorithm calculates a certain future time series t,t+T,...,t+(N-1 according to the system model, index function and the current state of the system ) System optimal control input u at T t ,u t+T ,....,u t+(N-1)T. Among them, T is the system sampling period, and N is the prediction range of model predictive control. But only the input value u at the current moment t Is adopted as the input of the system at the current moment, and the remaining input value u t+T ,....,u t+(N-1)T It is discarded, and the calculation at the previous time is repeated at the next sampling time t+T. Circulate according to this principle to obtain the optimal input value of each control cycle of the system. Therefore, model predictive control is an on-line real-time control method, which can automatically adjust according to the current state of the system and the predicted future conditions to obtain the most suitable control strategy.
[0123] The index function J of model predictive control generally adopts linear quadratic form, and its expression is as follows:
[0124] J ( t 0 ) = 1 2 ∫ t 0 t f [ x T Q ( t ) x + u T R ( t ) u ] d t + 1 2 x T ( t f ) Q 0 x ( t f ) - - - ( 12 )
[0125] Where t 0 Is the starting time of model predictive control, t f Is the termination time of model predictive control, Q(t) is n×n-dimensional positive semi-definite matrix, R(t) is r×r-dimensional positive definite matrix, Q 0 Is n×n-dimensional positive semi-definite matrix;
[0126] In actual engineering, since the components of the state variable and the input variable are generally independent of each other, the cross term is meaningless, so Q(t) and R(t) often take diagonal matrices. Under normal circumstances, the purpose of linear quadratic is to make J take a minimum value, then its practical significance is to maintain a small state error by using a small input, so as to optimize the integration of error indicators and energy consumption.
[0127] Since the control system is a discrete system in the bicycle trajectory tracking problem, the index function (12) becomes the following form:
[0128] J = 1 2 X k = k 0 k 0 + N - 1 [ x T ( k ) Q ( k ) x ( k ) + u T ( k ) R ( k ) u ( k ) ] d t + 1 2 x T ( k + N ) Q 0 x ( k + N ) - - - ( 13 )
[0129] Where k 0 Is the current moment, and N is the prediction range.
[0130] By solving the minimum value of the index function J, the current optimal input is obtained. The bicycle prediction model is a linear model, and the solution steps are as follows:
[0131] Step 1: Initially, let k=k 0 +N, P(k 0 +N)=Q 0
[0132] Step 2: Input u(k)=G(k)·x(k) at the k-th moment, where G(k) is the state feedback coefficient matrix, and its expression is
[0133] G(k)=-(R(k)+B(k) T P(k+1)B(k)) -1 B(k) T P(k+1)A(k)
[0134] Among them, P(k+1) is the positive solution of the Riccati equation of the discrete time-varying system, and the expression of the Riccati equation of the discrete time-varying system is:
[0135] P(k)=Q(k)+A(k) T (P(k+1))-P(k+1))B(k)(R(k)
[0136] +B(k) T P(k+1)B(k)) -1 B(k) T P(k+1))A(k)
[0137] Step 3: All k are decremented by 1, when k=k 0 When the calculation ends, otherwise, return to step 2.
[0138] In this way, after N reverse iterations, the optimal input sequence u(k 0 ),u(k 0 +1),.....u(k 0 +N-1), but only the input value u(k 0 ) Is adopted as the input at the current time of the system, and the remaining input value is discarded, and the above calculation is repeated at each sampling time thereafter. Cycle according to this principle to get the optimal input value of each control cycle of the system.
[0139] It should be noted that the goal of bicycle trajectory tracking 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) tends to 0, and does not care about other state variables in the state vector x and the value of input u. Therefore, when selecting the weight matrix Q and R, it is necessary to set the weights of the above four deviations to be much larger than other states the amount. In addition, because the optimal target body tilt angle of the bicycle is unconstrained due to dynamic programming, but in actual situations, the target body tilt angle of the bicycle cannot be too large, otherwise the bicycle will not be able to maintain balance. Therefore, it is necessary to limit the calculated inclination angle of the target body.
[0140] Figure 4 It is the structure diagram of the bicycle trajectory tracking simulation system. Select several typical target trajectories for dynamic simulation of bicycle trajectory tracking control. The simulation results are as follows: Figure 5 The simulation results of bicycle tracking straight trajectory, Image 6 Bicycle tracking circular trajectory simulation results, Figure 7 From the simulation result of bicycle tracking straight-circle combined trajectory, it can be seen that the unmanned bicycle trajectory tracking control method designed in the present invention can make the bicycle track the target trajectory well.
