Three-stage control method for course trajectory tracking of unmanned bicycle

Abstract

The invention discloses a three-stage control method for course trajectory tracking of an unmanned bicycle, and aims to provide a feasible strategy scheme for solving the problems of low course trajectory control precision, slow response speed and the like of the unmanned bicycle in a complex environment. According to the method, high-precision trajectory tracking is realized through three-stage closed-loop control based on a collaborative framework of a course trajectory active-disturbance-rejection controller, a lateral inclination angle active-disturbance-rejection controller and a handlebar corner active-disturbance-rejection controller. The method specifically comprises the steps that an outer ring designs a course trajectory active-disturbance-rejection tracker based on course errors, generates a reference signal # imgabs0 # of a lateral inclination angle, and rapidly compensates initial course deviation by dynamically adjusting the reference signal of the lateral inclination angle, so that it is ensured that the unmanned bicycle can be in an expected trajectory form; the middle ring is used for generating a reference signal delta * (t) of a handlebar rotation angle through a lateral inclination angle active-disturbance-rejection controller in combination with a kinetic model, compensating lateral inclination angle deviation by dynamically adjusting the reference signal of the handlebar rotation angle, and maintaining dynamic balance of a vehicle body; according to the inner ring, a handlebar rotation angle controller is designed based on handlebar rotation angle errors to generate torque T delta (t) applied to a handlebar, the deviation of a lateral inclination angle and a course track is compensated by dynamically adjusting the handlebar rotation angle, and high-precision control is ensured. Through the synergistic effect of the three-stage controller, the unmanned bicycle can keep lateral balance and realize high-precision course trajectory tracking at the same time, and the movement performance of the unmanned bicycle is improved.

Description

Technical Field

[0001] The present invention relates to the field of heading trajectory tracking of unmanned bicycles, and specifically to a three-stage control method for heading tracking of unmanned bicycles. Background Art

[0002] An unmanned bicycle is a product of the combination of a bicycle mechanism and intelligent control technology, with outstanding features such as a narrow body, high maneuverability, energy conservation and high efficiency, and is expected to develop into a new type of intelligent transportation tool in the future. In the research and development of unmanned bicycles, heading trajectory tracking control is one of the key technologies to ensure the stable and accurate driving of the bicycle. Existing heading trajectory tracking control methods for unmanned bicycles mainly include traditional control strategies such as partial feedback linearization control (PFLC), fuzzy control (FC), sliding mode control (SMC), linear quadratic regulator (LQR), model predictive control (MPC), and probabilistic inference learning control (PILCO). These methods can achieve heading trajectory tracking to a certain extent, but there are some problems in practical applications, such as difficulty in dealing with the strong coupling and complex nonlinearity between the handlebar and the body tilt of the unmanned bicycle, slow response time of the system, and poor robustness. In addition, traditional control methods are usually designed based on accurate mathematical models. For example, during the actual driving process of an unmanned bicycle, due to external disturbances such as load changes and road surface unevenness, its dynamic model will change, resulting in a decline in the performance of model-based control methods, and further leading to a reduction in the accuracy of heading trajectory tracking. Generally speaking, traditional control methods are difficult to achieve high-precision heading tracking while ensuring fast response.

[0003] Active disturbance rejection control (ADRC) belongs to an important branch of modern control theory. Its basic principle is to start from the internal and external disturbances of the system and achieve precise control of complex uncertain systems through active estimation and compensation techniques. The core idea of active disturbance rejection control is to use an extended state observer (LESO) to estimate and compensate the state and lumped disturbances of the system in real time, thereby significantly improving the robustness and control accuracy of the system. This method includes two parts: disturbance estimation and error feedback control. The former realizes the efficient observation of the total disturbance through an extended state observer, and the latter realizes the precise adjustment of the system state through a nonlinear state error feedback control law. The key point of active disturbance rejection control is that it does not depend on an accurate system model, can adapt to system parameter changes and external disturbances, and has strong anti-disturbance ability and adaptability. Active disturbance rejection control has been successfully applied in fields such as motor control, aircraft navigation, and industrial process control, showing broad application prospects and important engineering value. Summary of the Invention

[0004] Aiming at the deficiencies of the existing technology, the present invention provides a heading trajectory tracking control method for unmanned bicycles based on three-stage control.

[0005] The overall technical solution of the three-stage control method for tracking the heading trajectory of an unmanned bicycle that can solve the above-mentioned technical problems is as follows.

[0006] Description of the motion variables of the unmanned bicycle: ν is the forward speed of the vehicle body; is the lateral tilt angle (tilt velocity) of the vehicle body; is the heading angle (tilt velocity) of the vehicle body; is the handlebar turning angle (inclination velocity); τ δ Input torque for the control of the handlebars.

[0007] Step 1: Refer to the classic LPV model in the field of unmanned bicycle research given in the reference "Linearized dynamics equations for the balance and steer of a bicycle: a benchmark and review", and rewrite its model formula into a state space equation;

[0008] Step 2, referring to the description of the lateral inclination angle, handlebar angle, and heading angle of the unmanned bicycle in the reference "A robust two-stage active disturbance rejection control for the stabilization of a riderless bicycle", derive the related expression of the lateral inclination angle and the heading angle through simultaneous equations, and differentiate it to obtain the second-order derivative expression;

[0009] Step 3, combining the second-order derivative expressions of the lateral inclination angle and the heading angle obtained in step 2 with the handlebar angle dynamics to obtain the heading angle dynamics formula;

[0010] Step 4: The heading angle dynamics formula obtained in step 3 is designed according to the principle of self-disturbance rejection and uses the augmented system to construct an observer and control law for the heading angle, so that the roll angle and heading angle states can be tracked to the desired trajectory curve within a limited time.

[0011] Step 5, the stability controller, tracking controller and forward speed controller proposed in the reference "A robust two-stage active disturbance rejection control for the stabilization of a riderless bicycle" realize the balance stability of the unmanned bicycle;

[0012] Furthermore, the LPV mechanical model in step 1 is:

[0013]

[0014] wherein, and δ are the lateral inclination angle of the vehicle frame and the steering angle of the handlebar respectively; M is the mass inertia matrix of the system, v(t)C1 is the equivalent damping matrix of the system, (gK0 + v 2 (t)K2) is the equivalent stiffness matrix of the system, v(t) is the horizontal linear velocity of the vehicle body centroid, and g is the gravitational acceleration constant; τ = (0, τ δ ) T is the control torque vector of the system, and τ δ is the control torque of the handlebar.

[0015] Furthermore, the state space equation in step 1 is

[0016]

[0017] wherein, the state variable A and B are the state transition matrix and the control input matrix of the system respectively, and u(t) is the control input.

[0018] In step 2, the kinematic equation that relates the lateral inclination angle to the heading angle ψ(t) is obtained: Its second derivative formula:

[0019] wherein, ρ = V m cos(π / 2 - η) / ω,

[0020] In step 3, the second derivative formula is combined with the corner dynamics formula to obtain the heading angle dynamics formula:

[0021]

[0022] wherein,

[0023] In step 4, from the tracking error e ψ (t) = ψ(t) - ψ * (t), the following is obtained:

[0024]

[0025] wherein,

[0026] Assuming that the change of the heading angle is slow, the augmented system is obtained:

[0027]

[0028] Among them, B ψ =(0 1 0) T , D ψ =(0 - n2 0) T , E ψ =(0 n3 0) T , F ψ =(0 0 1) T , C ψ =(1 0 0) T .

[0029] The observer of the course trajectory tracking controller is set according to the augmented system and the course angle dynamics formula obtained according to error tracking as:

[0030]

[0031] Among them The gain vector L is appropriately selected ψ such that the eigenvalues of (A ψ - L ψ C ψ ) are located on the left side of the complex plane s.

[0032] The auxiliary control law obtained according to the system is:

[0033]

[0034] Assume that the estimates of T δ (t) and are accurate, and the selection of the control gains β0 and β1 makes the roots of the characteristic polynomial s 2 +(β0 + n1)s + β1 = 0 located on the left side of the complex plane s.

[0035] Substituting the auxiliary control law into the system gives:

[0036]

[0037] Among them, is the estimation error. converges to a small neighborhood of ξ ψ (t), converges to a small neighborhood of, indicating that the estimation error will eventually be within a bounded interval near zero. Then the course error dynamics e ψ (t) = ψ(t) - ψ * (t) is determined by the following differential equation:

[0038]

[0039] The auxiliary control law can be converted into the actual control law of the heading trajectory. Therefore, the set value of the heading trajectory control law is:

[0040]

[0041] Furthermore, in step 5, the tilt angle dynamics formula is obtained according to the bicycle model:

[0042]

[0043] where the auxiliary control input the disturbance signal

[0044] uses the first-order linear approximation as the internal model of Therefore, an augmented system is obtained, where the states of the tilt angle dynamics and the states of the disturbance model are part of the model:

[0045]

[0046] where: B o =(0 1 0) T , F o =(0 0 1) T , C ο =(1 0 0) T .

[0047] Based on this augmented state-space system, the observer of the stable controller is set as:

[0048]

[0049] where, L o is the gain vector of the observer.

[0050] The auxiliary control law of the stable controller is set as:

[0051]

[0052] The steering angle dynamics formula is obtained according to the bicycle model:

[0053]

[0054] where According to the tracking error e of the steering system δ(t) = δ(t) - δ * (t) gives:

[0055] where and define to obtain the augmented system:

[0056]

[0057] where, F i2 = (0 0 0 0 0 0 1) T ,

[0058] Based on the augmented system and the steering angle dynamics formula obtained according to the error tracking, the observer of the tracking controller is set as:

[0059]

[0060] where, L i is the gain vector of the observer.

[0061] The control law of the tracking controller is set as:

[0062]

[0063] The forward speed controller uses a PI controller to control the output v(t). Define v * (t) as the desired forward speed, and the PI control law is expressed as:

[0064] where, e v (t) = v(t) - v * (t). And after the bicycle stabilizes at the nominal constant forward speed V m , its dynamics can be approximated as:

[0065]

[0066] Advantages of the present invention:

[0067] 1. By introducing the active disturbance rejection control strategy, the present invention uses a linear extended state observer to realize the precise tracking of the heading trajectory of the unmanned bicycle through the real-time observation and estimation of internal and external disturbances of the system, enhances the adaptability of the system to unknown or changing disturbances, and improves the robustness and accuracy of trajectory tracking.

[0068] 2. The present invention simplifies the controller design process and reduces the dependence on the system model through the active disturbance rejection control strategy. Meanwhile, the debugging process is relatively intuitive and convenient, facilitating quick adjustment and optimization. Description of the Drawings

[0069] Figure 1 It is the system control block diagram of a three-stage control method for the heading trajectory tracking of an unmanned bicycle;

[0070] Figure 2 It is the schematic diagram of the mechanism of an unmanned bicycle;

[0071] Figure 3 It is the lateral tilt angle of the s trajectory, the handlebar angle, the heading angle, the actual and expected values of the trajectory, and the response curves of the heading angle and trajectory error;

[0072] Figure 4 It is the lateral tilt angle of the circular trajectory, the handlebar angle, the heading angle, the actual and expected values of the trajectory, and the response curves of the heading angle and trajectory error; Detailed Embodiment

[0073] To make the objectives, technical solutions, and advantages of the present invention clearer and easier to understand, the present invention will be further described in detail below in conjunction with the following specific embodiments, but the present invention is not limited to the following embodiments.

[0074] The overall technical solution block diagram of this embodiment is shown in the appendix Figure 1 , and the unmanned bicycle used is as shown in the appendix Figure 2 .

[0075] The present invention is applied to the problem of heading trajectory tracking of an unmanned bicycle based on three-stage control. Its purpose is to achieve the stability of the unmanned bicycle while implementing a robust control strategy, and at the same time, enable the unmanned bicycle to track a simple path on a flat road surface. The specific steps are as follows:

[0076] Step 1: Establish an LPV model based on the physical parameters of the entire unmanned bicycle shown in Table 1 and rewrite it into a state-space equation:

[0077]

[0078] In the formula, it is calculated from the physical parameters under no-load conditions

[0079]

[0080] Among them, v in matrix A is the vehicle speed.

[0081] Table 1 Physical parameters of the entire unmanned bicycle

[0082] parameter symbol numerical value Total mass of the vehicle body (kg) <![CDATA[m t > 14.85 Wheelbase between the front and rear wheels (m) ω 0.885 Height of the vehicle body's center of mass (m) h 0.547 Horizontal distance from the vehicle body's center of mass to the origin of the coordinate system (m) b 0.344 Rear offset of the front wheel contact point (m) c 0.078 Fork rake angle (°) η 75 <![CDATA[Acceleration due to gravity (m / s 2 )]]> g 9.81

[0083] Step 2, Lateral Inclination Angle The kinematic second - derivative expression related to the heading angle ψ(t):

[0084] where ρ = V m cos(π / 2 - η) / ω, η = 75°, V m = 1.5m / s, g = 9.81m / s 2 , ω = 0.885m.

[0085] Step 3, Take the second - derivative of the expression of the lateral inclination angle and the heading angle obtained in Step 2, and combine it with the dynamics of the handlebar angle to obtain the heading - angle dynamics formula;

[0086] Step 4, The observer of the heading - trajectory tracking controller is set as:

[0087]

[0088] where the observer gain L is selected ψ =(450, 28, 24) T , and at this time, the three eigenvalues of A ψ -L ψ C ψ (-4.4994, -0.0005 + 0.0023j, -0.0005 - 0.0023j) all have negative real parts, satisfying the boundary conditions for the convergence of the observer.

[0089] Its control law:

[0090] where the gains β0 = 0.03 and β1 = 0.35 are selected, so that the heading - error dynamics e ψ (t)=ψ(t)-ψ * (t) approaches zero.

[0091] Step 5, The forward - speed controller is obtained through the closed - loop characteristic polynomial:

[0092]

[0093] where the natural frequency ω0 = 4.651 and the relative damping ζ = 1.

[0094] The linear extended - state observer in the stability controller:

[0095]

[0096] where the observer gain L is selected o =(1557, 538600, 41480000) T , and at this time, Ao -L o C o The three eigenvalues of (-1102.6, -345.4, -108.9) all have negative real parts, satisfying the boundary conditions for the convergence of the observer.

[0097] Its control law:

[0098] Among them, the gains α0 = 22.412 and α1 = 5.367 are selected to make the lateral tilt angle of the bicycle near zero.

[0099] The linear state extended observer of the tracking controller:

[0100]

[0101] Among them, the observation gain L i =(109.9, 4693, 109400, 22000, 2200) T , at this time A i -L i C i The six eigenvalues of (-60.065, -26.038 + 33.566j, -26.038 - 33.566j, -0.051 + 0.088j, -0.051 - 0.88j, -0.101) all have negative real parts, satisfying the boundary conditions for the convergence of the observer.

[0102] Control law:

[0103] Among them, the gains γ0 = 448.438 and γ1 = 41.538 are selected to make the tracking error dynamics e δ (t)=δ(t)-δ * (t) approach zero.

[0104] In order to verify the high-precision tracking control of this embodiment, on the premise of fully considering the absence of other interferences, the heading trajectory tracker of the present invention is verified for the heading tracking control effect based on the MATLAB simulation environment.

[0105] Simulation group one:

[0106] Set the simulation duration to 120s and the step size to 0.005s. Other settings are as follows:

[0107] (1) Set the initial state: Set the lateral tilt angle, handlebar angle and desired heading angle of the initial state to 0 rad;

[0108] (2) Set the simulation conditions: Set the vehicle speed v d = 4.5 m / s, set the desired trajectory: xd = 2R*n + 2R + Rcos((-1) n *ω*t), y d = R*sin((-1) n *ω*t).

[0109] From the simulation curve of Simulation Group 1 in the embodiment (attached Figure 3 ), it can be seen that:

[0110] (1) In general, taking the data of the first 100 s to obtain the simulation curve, the actual lateral inclination angle and the desired lateral inclination angle basically coincide, and the error is within the range of ±0.2 rad, indicating that the system can quickly track the desired lateral inclination angle and has good dynamic response; in addition, the error between the actual handlebar angle and the desired handlebar angle is small and basically coincides, and the system can quickly track the desired handlebar angle, indicating that the control accuracy is relatively high;

[0111] (2) In general, taking the data of the first 100 s to obtain the simulation curve, the overall error of the heading angle shows periodic fluctuations throughout the simulation process, and the error range is between [-0.9, 0.8] rad, and the absolute value of the error throughout the process is less than 1 rad, basically meeting the stability requirements; when the time is greater than 60 s, its error stabilizes at ±0.2 rad, indicating that the system has entered the steady state stage;

[0112] (3) Taking the data of the first 100 s to obtain the trajectory error curve, since within the first 20 s of the initial stage, the actual trajectory has not yet tracked the desired trajectory, the maximum error trajectory reaches 30 m; however, as time goes by, the error gradually decreases and tends to be stable. When the time is greater than 60 s, the entire error curve fluctuates near the straight line of y = 0, and the fluctuation range is within [-4, 2] m, indicating that the system can quickly respond and reduce the initial error and can meet the requirements of high-precision tracking control.

[0113] Simulation Group 2:

[0114] (1) Set the simulation duration to 120 s and the step size to 0.005 s, and other settings are as follows:

[0115] Set the initial state: Set the lateral inclination angle, handlebar angle, and desired heading angle of the initial state to 0 rad;

[0116] (2) Set the simulation conditions: Set the vehicle speed v d = 4.5 m / s, and set the desired trajectory:

[0117] From the simulation curve of Simulation Group 2 in the embodiment (attached Figure 4 ), it can be seen that:

[0118] (1) Under the overall condition, the simulation curve is obtained by taking the data of the first 100 s. In the initial stage, when the time is less than 20 s, the lateral inclination angle error oscillates within ±0.5 rad. After 20 s, the error converges to within ±0.15 rad. After 20 s, the handlebar angle oscillates near the straight line of y = 0, and the maximum tracking error of the handlebar angle is less than 0.03 rad, indicating that the unmanned bicycle has achieved good lateral balance;

[0119] (2) Under the overall condition, the simulation curve is obtained by taking the data of the first 100 s. In the initial stage, when the time is less than 20 s, the heading angle error quickly converges to within ±0.5 rad. When the time is greater than 60 s, the entire error curve fluctuates near the straight line of y = 0, and the error stabilizes within ±0.1 rad. Therefore, the system enters the steady state stage, indicating that the unmanned bicycle has achieved the expected trajectory tracking control;

[0120] (3) The trajectory error curve is obtained by taking the data of the first 100 s. Since the actual trajectory has not tracked the desired trajectory within the first 40 s of the initial stage, the maximum error trajectory reaches 18 m. However, as time goes by, the error gradually decreases and tends to be stable. After 60 s, the entire error curve fluctuates near the straight line of y = 0, and the fluctuation range is within [-0.7, 0.1] m, indicating that the system can quickly respond and reduce the initial error, and can meet the requirements of high-precision heading tracking.

[0121] In summary, the proposed three-stage control method shows good performance in the heading trajectory tracking of the unmanned bicycle and can effectively achieve high-precision trajectory tracking. According to the data curve graphs of Simulation Group 1 and Simulation Group 2, the simulations of the two different trajectories both show that the system can quickly reduce the initial error and enter the steady state stage, and the final trajectory error curves all fluctuate near the straight line of y = 0, and both can achieve high-precision tracking control.

[0122] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the technical solutions of the present invention, and they should all be covered within the scope of the claims of the present invention.

Claims

1. According to the three-stage control method for heading trajectory tracking of the unmanned bicycle, it is characterized in that: In the said step 3, the second derivative formula is combined with the cornering dynamics formula to obtain the heading angle dynamics formula: Among them, In the said step 4, according to the tracking error e ψ (t) = ψ(t) - ψ * (t), the following is obtained: Among them, Assume that the heading angle changes slowly, The augmented system is obtained: Among them, B ψ =(0 1 0) T , D ψ =(0 - n2 0) T , E ψ =(0 n3 0) T , F ψ =(0 0 1) T , C ψ =(1 0 0) T . The observer of the heading trajectory tracking controller obtained according to the augmented system and the heading angle dynamics formula obtained by error tracking is set as: Among them appropriately select the gain vector L ψ such that the eigenvalues of (A ψ - L ψ C ψ ) are located on the left side of the complex plane s.

2. The three-stage control method for the heading trajectory tracking of an unmanned bicycle according to claim 1, wherein The auxiliary control law designed according to the lumped error observed by the observer is: Assume that for TT δ (t) and are estimated accurately, the control gains β0 and β1 are chosen such that the roots of the characteristic polynomial s 2 +(β0 + n1)s + β1 = 0 lie in the left half of the complex plane s. Substituting the auxiliary control law into the system gives: where, is the estimation error. converges to a small neighborhood of ξ ψ (t), converges to a small neighborhood of, indicating that the estimation error will eventually stay within a bounded interval around zero. Then the heading error dynamics e ψ (t) = ψ(t) - ψ * (t) is determined by the following differential equation:

3. A three-stage control method for the heading trajectory tracking of an unmanned bicycle according to claim 1 or 2, characterized in that It can be converted through into the actual control law of the heading trajectory. Therefore, the set value of the heading trajectory control law is as follows:

Images