CN105320129B - A kind of method of unmanned bicycle Trajectory Tracking Control - Google Patents

A kind of method of unmanned bicycle Trajectory Tracking Control Download PDF

Info

Publication number
CN105320129B
CN105320129B CN201510690834.4A CN201510690834A CN105320129B CN 105320129 B CN105320129 B CN 105320129B CN 201510690834 A CN201510690834 A CN 201510690834A CN 105320129 B CN105320129 B CN 105320129B
Authority
CN
China
Prior art keywords
bicycle
mrow
msub
model
mover
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201510690834.4A
Other languages
Chinese (zh)
Other versions
CN105320129A (en
Inventor
赵明国
余永超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tsinghua University
Original Assignee
Tsinghua University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tsinghua University filed Critical Tsinghua University
Priority to CN201510690834.4A priority Critical patent/CN105320129B/en
Publication of CN105320129A publication Critical patent/CN105320129A/en
Application granted granted Critical
Publication of CN105320129B publication Critical patent/CN105320129B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Feedback Control In General (AREA)

Abstract

The invention belongs to the movement control technology fields of automatic driving vehicle, more particularly to a kind of method of unmanned bicycle Trajectory Tracking Control, it is characterized in that, the equilibrium kinetics model of bicycle is initially set up, and establishes the discrete state equations of the bicycle Self-balance Control System after adding in self-balancing controller；The discrete motion model of bicycle is resettled, and is linearized；Then kinematics model with Self-balance Control System is combined, after carrying out corresponding simplified processing, establishes the 7 degree of freedom State Forecasting Model of a bicycle；The prediction model of the 7 degree of freedom State Forecasting Model as bicycle track following algorithm is finally used, carries out the Trajectory Tracking Control of bicycle, line solver goes out the optimum control input of each sampling instant.The Trajectory Tracking Control method of bicycle based on the 7 degree of freedom State Forecasting Model can predict state of the bicycle in future time instance exactly, and calculation amount is small, and online real-time in turn ensures its good control effect.

Description

A kind of method of unmanned bicycle Trajectory Tracking Control
Technical field
Movement control technology field more particularly to a kind of unmanned bicycle the invention belongs to unmanned bicycle The method of Trajectory Tracking Control.
Background technology
Compared with automobile, unmanned bicycle has cheap, light and handy portable, flexible and environmental protection and energy saving etc. Advantage.But for four wheel constructions of automobile, two wheel constructions of bicycle have unstability, are not artificially controlling Bicycle is unable to maintain that balance in the case of system.Therefore, for bicycle, to realize unpiloted function, first must It must accomplish to control the balance of its own.It can be seen that the realization difficulty of unmanned bicycle will be far longer than unmanned vapour Vehicle.
, it is necessary to carry out motion control to it on the basis of bicycle realizes self-balancing control, the machine on upper strata is coordinated to regard The functions such as feel and trajectory planning, can just fully achieve autonomous driving.Path trace refers in inertial coodinate system, moving machine Device people must reach and follow given target trajectory from a given original state, and the initial point of robot can With on this track, can not also on track, this be in mobile robot research field one it is important the problem of.Rail at present The research object of mark tracking problem is mostly wheeled mobile robot, pilotless automobile and unmanned plane, on grinding for bicycle Study carefully then less.
Many researchers employ many different methods and realize Trajectory Tracking Control.In these methods, model is pre- Control algolithm is surveyed as a kind of online real-time control method, it can obtain good control in the case that required calculation amount is smaller Effect processed.And most important factor is that the selection of prediction model in the algorithm.Due to automobile and other wheel type mobiles Machine is per capita without the concern for equilibrium problem, therefore it may only be necessary to establish its kinematics model.However for bicycle and Speech, the equilibrium kinetics model of bicycle can so that there are one more complicated dynamics when the handlebar of bicycle goes to target rotation angle Process, this can produce bigger effect the movement locus of bicycle.Therefore, in the track following problem of bicycle, only with Kinematics model is predicted and unreasonable, thus can not also obtain optimal tracking effect.
For bicycle, progress rail is removed as the prediction model of Model Predictive Control Algorithm only with kinematics model Mark tracing control can not obtain optimal effect.
The content of the invention
The present invention proposes a kind of method of unmanned bicycle Trajectory Tracking Control, and one kind is established for bicycle Complex multi-dimensional kinematics model adds the dynamic characteristic that bicycle turns under self-balancing controller in the kinematics model, State of the bicycle in future time instance can be more accurately predicted out with this model, carried out voluntarily using Model Predictive Control Algorithm The Trajectory Tracking Control of vehicle can obtain good effect.
A kind of Trajectory Tracking Control method of unmanned bicycle, which is characterized in that comprise the following steps：
Step 1, the equilibrium kinetics model for establishing bicycle, and it is certainly flat to establish the bicycle after adding in self-balancing controller The discrete state equations for the control system that weighs；
Step 2, the discrete motion model for establishing bicycle, and linearized；
Kinematics model is combined by step 3 with Self-balance Control System, establishes the 7 degree of freedom status predication of a bicycle Model；
Step 4, using prediction model of the 7 degree of freedom State Forecasting Model as bicycle track following algorithm in step 3, The Trajectory Tracking Control of bicycle is carried out, line solver goes out the optimum control input of each sampling instant.
The process of establishing of the discrete state equations of bicycle Self-balance Control System is in the step 1：
The equilibrium kinetics model of the bicycle of foundation is：
Wherein,For the body sway angle of bicycle, δ is the handlebar steering angle of bicycle, and a is bicycle center of gravity G with after Take turns pick-up point P1Horizontal distance, h be vehicle body without tilt when bicycle center of gravity arrive ground distance, λ for bicycle front fork angle, vxFor the forward speed of bicycle rear, c is bicycle hangover, and b is acceleration of gravity away from, g for axle for bicycle, s La Pula This operator；
The control structure that the self-balancing control of bicycle is controlled plus feedovered using proportional-plus-derivative, control law expression formula are：
Wherein, δdFor the output of the handlebar steering angle command, i.e. self-balancing controller of bicycle,For the target of bicycle The input at body sway angle, i.e. self-balancing controller,For onFirst derivative, k1For proportionality coefficient, k2For differential system Number, 1/k3For feed-forward coefficients；
Handlebar steering angle command δdThere are a low-pass filtering links between actual handlebar steering angle sigmaThe ring It saves as a first order inertial loop, TSFor time constant；
The discrete state equations of bicycle Self-balance Control System are：
Matrix
Matrix
Wherein, intermediate variable K1=mh2, intermediate variableIntermediate variable K3=mgh, intermediate variableFeed-forward coefficientsM is the quality of bicycle,For the vehicle body of bicycle Inclination angle,For onFirst derivative, δ is handlebar steering angle on bicycle, and T is the sampling period of discrete system, k For sampling instant sequence number.
The detailed process of the step 2 is：
Bicycle forward speed v is bicycle rear forward speed vx, i.e. v=vx；Body sway angleTo kinematic shadow Sound is ignored, and draws the kinematics model of bicycle：
Wherein, ψaFor the yaw angle of bicycle,For on ψaFirst derivative, δfIt is turned to for effective handlebar of bicycle Angle, i.e. handlebar steering angle sigma are in the projection on ground, radius of turn when b for axle for bicycle is cycling away from, R, xaFor voluntarily Back wheels of vehicle pick-up point P1The coordinate of X-direction on earth axes O-XYZ,For on xaFirst derivative, yaAfter bicycle Take turns pick-up point P1The coordinate of Y-direction on earth axes O-XYZ,For on yaFirst derivative, since bicycle only exists Moved on ground level, thus without considering bicycle Z-direction coordinate；
The handlebar steering angle sigma of bicycle and effective handlebar steering angle sigmafBetween have following relation：
For the body sway angle of bicycle, λ is the front fork angle of bicycle；
Bicycle is constant motion v (k)=v, will be obtained after above-mentioned kinematics model discretization：
Wherein, T is the sampling period of discrete system, and k is sampling instant sequence number；
It is above-mentioned it is discrete after kinematics model to be non-linear, which is linearized near reference locus Obtain linear movement model：
Wherein, matrixMatrix
Represent bicycle virtual condition and the departure of reference state；Expression bicycle actually enters and reference input Departure.
The 7 degree of freedom State Forecasting Model of bicycle is in the step 3：
The discrete state equations of bicycle Self-balance Control System with the kinematics model of bicycle are combined, are arranged To following 7 degree of freedom State Forecasting Model：
X (k+1)=Ax (k)+Bu (k)
Matrix
Matrix
The input variable that x (k) is the state variable of bicycle prediction model, u (k) is bicycle prediction model.
Bicycle Trajectory Tracking Control algorithm concretely comprises the following steps in the step 4：
The target function J of Model Predictive Control generally use Linear-Quadratic Problem form, due to the track of bicycle with In track problem, control system is discrete system, and target function becomes：
Wherein, k0For current time, N is estimation range, t0For the initial time of Model Predictive Control, tfFor model prediction The end time of control, Q (t) tie up positive semidefinite matrix for n × n, and R (t) ties up positive definite matrix, Q for r × r0Positive semidefinite square is tieed up for n × n Battle array；
By solving the minimum of target function J, so as to obtain current optimal input, solution procedure is as follows：
Step 401：When initial, k=k is made0+ N, P (k0+ N)=Q0
Step 402：The input u (k) at kth moment=G (k) x (k), wherein G (k) be feedback of status coefficient matrix, table It is up to formula
G (k)=- (R (k)+B (k)TP(k+1)B(k))-1B(k)TP(k+1)A(k)
Wherein, P (k+1) is the normal solution of Discrete Time-Varying Systems Riccati equation, and Discrete Time-Varying Systems Riccati equation is expressed Formula 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 from subtracting 1, works as k=k0When, calculating terminates, otherwise, return to step 402；
After the reverse iteration of n times to get to optimal list entries u (k0),u(k0+1),.....u(k0+ N-1), it adopts With the input value u (k at current time0) input as system current time, and remaining input value is given up, it is each thereafter Sampling instant repeats above-mentioned calculating；It is cycled by this principle to get to the optimal input value of each controlling cycle of system.
Advantageous effect
7 degree of freedom State Forecasting Model proposed by the present invention is by bicycle under cycling model and self-balancing controller Equilibrium kinetics characteristic be combined, so that the control input of bicycle from bicycle target carriage steering angle become from The target vehicle body inclination angle of driving, the model can relatively accurately reflect the actual conditions of cycling, predict exactly Go out state of the bicycle in future time instance；It can so better ensure that Model Predictive Control Algorithm has given play to its advantage, bicycle Tracking target trajectory can obtain good effect；Due to the model be it is linear, calculation amount with nonlinear model compared to small very much, Therefore it is this online to be very suitable for Model Predictive Control, the control method of high frequency.It is carried out certainly using Model Predictive Control Algorithm The Trajectory Tracking Control of driving acquires optimum control by matrix iteration calculating and inputs, and calculation amount is smaller, ensures that the algorithm is online In real time the characteristics of control, online real-time in turn ensures its good control effect.
Description of the drawings
Fig. 1 is the structure diagram of bicycle
Fig. 2 is the Self-balance Control System structure chart of bicycle
Fig. 3 is Model Predictive Control Algorithm schematic diagram
Fig. 4 is bicycle Seam-Tracking Simulation system construction drawing
Fig. 5 tracks straight path simulation result for bicycle
Fig. 6 tracks circular trace simulation result for bicycle
Fig. 7 tracks straight line-circle combined trajectories simulation result for bicycle
Fig. 8 is the flow chart of the Trajectory Tracking Control method of the unmanned bicycle of the present invention
Specific embodiment
Below in conjunction with the accompanying drawings, a kind of Trajectory Tracking Control method of unmanned bicycle proposed by the present invention is carried out detailed It describes in detail bright.
The present invention carries out the Trajectory Tracking Control of bicycle by Model Predictive Control Algorithm.First, for bicycle, build Its equilibrium kinetics model is found, and establishes the system state equation after adding in self-balancing controller；Secondly, the fortune of bicycle is established It is dynamic to learn model, and linearized；Then kinematics model with Self-balance Control System is combined, carried out at corresponding simplify After reason, a 7 degree of freedom State Forecasting Model is established；The target function of Linear-Quadratic Problem is finally selected, sets its weight matrix ginseng Number, line solver go out the optimum control input of each sampling instant.Fig. 8 is the track following of the unmanned bicycle of the present invention The flow chart of control method.
1. establish the discrete state equations of bicycle Self-balance Control System：
First, the structure diagram of bicycle is provided, as shown in Figure 1.
The equilibrium kinetics model of bicycle is established, as shown in formula (1)：
Wherein,For the body sway angle of bicycle, δ is the handlebar steering angle of bicycle, and a is bicycle center of gravity G with after Take turns pick-up point P1Horizontal distance, h be vehicle body without tilt when bicycle center of gravity arrive ground distance, λ for bicycle front fork angle, vxFor the forward speed of bicycle rear, c trails for bicycle, i.e. P2P3Spacing, b for axle for bicycle away from i.e. C1C2Spacing, G is acceleration of gravity, and s is Laplace operator.
The Self-balance Control System structure chart of bicycle is as shown in Figure 2.The self-balancing control of bicycle is using proportional-plus-derivative Control plus the control structure of feedforward, shown in control law expression formula such as formula (2)：
Wherein, δdFor the output of the handlebar steering angle command, i.e. self-balancing controller of bicycle,For the target of bicycle The input at body sway angle, i.e. self-balancing controller, k1For proportionality coefficient, k2For differential coefficient, 1/k3For feed-forward coefficients.
Wherein, as shown in Fig. 2, handlebar steering angle command δdThere are a low-pass filtering between actual handlebar steering angle sigma LinkThe link be a first order inertial loop, TSFor time constant.
To sum up, the state equation of bicycle Self-balance Control System is expressed as following form：
Wherein, intermediate variable K1=mh2, intermediate variableIntermediate variable K3=mgh, intermediate variableFeed-forward coefficientsM is the quality of bicycle,For the body sway angle of bicycle,For onFirst derivative,For onSecond dervative, δ is handlebar steering angle on bicycle,For on δ First derivative；
Discretization is carried out to above-mentioned state equation using Euler method, obtains the discrete state of bicycle Self-balance Control System Equation：
Wherein, T is the sampling period of discrete system, and k is sampling instant sequence number.
2. establish the discrete motion model of bicycle：
The structure of common electric bicycle turns to for front-wheel control, trailing wheel control speed.Wherein, kinematics model is established Based on following 2 points hypothesis：
Without considering wheel side sliding.In the case, bicycle forward speed be trailing wheel forward speed, i.e. v=vx；Due to vehicle Body inclination angleVery little, therefore ignore on kinematic influence.
Therefore, compares figure 1 draws the kinematics model of bicycle：
Wherein, ψaFor the yaw angle of bicycle,For on ψaFirst derivative, δfIt is turned to for effective handlebar of bicycle Angle, i.e. handlebar steering angle sigma are in the projection on ground, radius of turn when b for axle for bicycle is cycling away from, R, xaFor voluntarily Back wheels of vehicle pick-up point P1The coordinate of X-direction on earth axes O-XYZ,For on xaFirst derivative, yaAfter bicycle Take turns pick-up point P1The coordinate of Y-direction on earth axes O-XYZ,For on yaFirst derivative, since bicycle only exists Moved on ground level, thus without considering bicycle Z-direction coordinate.
The discrete motion model that discretization derives from driving is carried out to above-mentioned continuous kinematics model：
Since bicycle is constant motion, so v (k)=v.
The handlebar steering angle sigma of bicycle and effective handlebar steering angle sigmafBetween have following relation：
Formula (6) is substituted into obtain：
Cycling model shown in formula (8) be it is non-linear, using Taylor's formula directly using target trajectory as With reference to expansion, a bicycle linear movement model on error is established.
Target trajectory is expressed as xr(k)=[xr(k),yr(k),ψr(k)]T, ur(k)=[δr(k)]T.Wherein, xr(k) it is mesh Mark the reference state variable of track, ur(k) it is the reference-input variable of target trajectory, xrFor bicycle rear pick-up point P1On ground X-direction on areal coordinate system O-XYZ, yrFor bicycle rear pick-up point P1The coordinates of targets of Y-direction on earth axes O-XYZ Value.ψrFor the target yaw angle of bicycle, δrFor bicycle target carriage steering angle, it is public to omit the first order Taylor after higher order term The expression formula of formula is as follows：
fx,rFor cycling model on x partial derivative in x=xrWhen value；fu,rFor cycling model Partial derivative on u is in u=urWhen value；Represent bicycle virtual condition and the departure of reference state；Represent bicycle Actually enter the departure with reference input.
Due to only retaining single order item, the cycling model that thus formula pushes away is linear model, by formula (8) generation Enter (9) to obtain：
Wherein,
3. establish the State Forecasting Model of bicycle：
Bicycle dynamics state equation with balance controller is combined with the kinestate equation of bicycle, List following differential equation group：
6th differential equation variation is obtained：
Above-mentioned equation group is arranged to obtain following 7 degree of freedom State Forecasting Model：
X (k+1)=Ax (k)+Bu (k) (11)
The input variable that x (k) is the state variable of bicycle prediction model, u (k) is bicycle prediction model, it is specific fixed Justice is indicated below,
4. the bicycle Trajectory Tracking Control algorithm based on Model Predictive Control
Model Predictive Control, be otherwise known as roll stablized loop or rolling optimum control, originating from the sixties in last century.It Using the explicit model of controlled device, prediction controlled device is gone in the state of future time instance by the current state of controlled device.This Kind predictive ability can calculate a control sequence for making target control index optimal in real time online, exist so as to optimize controlled device The behavior of future time instance.The result of optimization will act on system according to the principle of rolling time horizon.Therefore, the core of Model Predictive Control The heart is model prediction, rolling optimization and feedback compensation three parts.
Since control targe and operation constraint can be clearly integrated in optimization problem and each by Model Predictive Control Line solver in controlling cycle, therefore as long as being widely used in industrial stokehold many decades.Due to needing online meter It calculates, the time is slow, therefore Model Predictive Control is generally initially applied to factory etc. and controls the relatively low field of frequency.But in recent years Come, with the significant increase of computer calculating speed, Model Predictive Control is gradually applied to the control of the high frequencies such as mobile robot Field.
The principle of Model Predictive Control is as shown in Figure 3.For discrete system, at a time t, Model Predictive Control For algorithm according to system model, the current state of target function and system calculates following sometime sequence t, t+T ..., t+ (N-1) system optimal control input u during Tt,ut+T,....,ut+(N-1)T.Wherein, T is system communication cycle, and N is model prediction The estimation range of control.But the only input value u at current timetIt is used, as the input at system current time, and it is remaining Input value ut+T,....,ut+(N-1)TIt is given up, in next sampling instant t+T, repeats the calculating of last moment.It is followed by this principle Ring obtains the optimal input value of each controlling cycle of system.Therefore, Model Predictive Control is a kind of online real-time controlling party Method, it can be automatically adjusted according to the current state of system and the future condition of prediction, draw most suitable control strategy.
The target function J of Model Predictive Control generally uses the form of Linear-Quadratic Problem, and expression formula is as follows：
T in formula0For the initial time of Model Predictive Control, tfFor the end time of Model Predictive Control, Q (t) ties up for n × n Positive semidefinite matrix, R (t) tie up positive definite matrix, Q for r × r0Positive semidefinite matrix is tieed up for n × n；
In Practical Project, due to general mutual indepedent between state variable and each component of input variable, cross term is not intended to Justice, therefore Q (t) and R (t) often take diagonal matrix.Under normal circumstances, the purpose of Linear-Quadratic Problem is to make J minimalizations, then its Practical significance is to keep smaller state error by using little input, so that error criterion and energy expenditure Comprehensive realization is optimal.
Since in the track following problem of bicycle, control system is discrete system, therefore, target function (12) becomes Following form：
Wherein, k0For current time, N is estimation range.
By solving the minimum of target function J, so as to obtain current optimal input.The prediction model of bicycle is line Property model, solution procedure are as follows：
Step 1：When initial, k=k is made0+ N, P (k0+ N)=Q0
Step 2：The input u (k) at kth moment=G (k) x (k), wherein G (k) are feedback of status coefficient matrix, are expressed Formula 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 normal solution of Discrete Time-Varying Systems Riccati equation, and Discrete Time-Varying Systems Riccati equation is expressed Formula 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 3：K from subtracting 1, works as k=k0When, calculating terminates, otherwise, return to step 2.
In this way, after the reverse iteration of n times, obtaining optimal list entries u (k0),u(k0+1),.....u(k0+ N-1), But the only input value u (k at current time0) be used, as the input at system current time, and remaining input value is given up Fall, thereafter each sampling instant, repeat above-mentioned calculating.It is cycled by this principle, obtains the optimal of each controlling cycle of system Input value.
It should be noted that since the target of bicycle Trajectory Tracking Control is to make four departure xa(k)-xr(k), ya (k)-yr(k), ψa(k)-ψr(k) and δ (k)-δr(k) 0 is tended to, without concern for other quantity of states and input in state vector x The value of u, therefore when choosing weight matrix Q and R, the weights that need to set aforementioned four departure are much larger than other quantity of states. Further, since the optimal objective body sway angle that Dynamic Programming solves bicycle is unconfined, but in a practical situation, from The target vehicle body inclination angle of driving cannot be excessive, and otherwise bicycle will be unable to keep balance.Therefore, the target vehicle body to solving Inclination angle need to carry out amplitude limit.
Fig. 4 is bicycle Seam-Tracking Simulation system construction drawing.It chooses several typical target trajectories and carries out voluntarily track The dynamics simulation of mark tracing control, simulation result is respectively as Fig. 5 bicycles track straight path simulation result, Fig. 6 bicycles Circular trace simulation result is tracked, Fig. 7 bicycles tracking straight line-circle combined trajectories simulation result can therefrom be found out, in the present invention The unmanned bicycle Trajectory Tracking Control method of design can make bicycle track target trajectory well.

Claims (4)

  1. A kind of 1. Trajectory Tracking Control method of unmanned bicycle, which is characterized in that comprise the following steps：
    Step 1, the equilibrium kinetics model for establishing bicycle, and establish the bicycle self-balancing control after adding in self-balancing controller The discrete state equations of system processed；
    Step 2, the discrete motion model for establishing bicycle, and linearized；
    Kinematics model is combined by step 3 with Self-balance Control System, establishes the 7 degree of freedom status predication mould of a bicycle Type；
    Step 4, using prediction model of the 7 degree of freedom State Forecasting Model as bicycle track following algorithm in step 3, carry out The Trajectory Tracking Control of bicycle, line solver go out the optimum control input of each sampling instant；
    The 7 degree of freedom State Forecasting Model of bicycle is in the step 3：
    The discrete state equations of bicycle Self-balance Control System are combined with the kinematics model of bicycle, arrangement obtain with Under 7 degree of freedom State Forecasting Model：
    X (k+1)=Ax (k)+Bu (k)
    Matrix
    Matrix
    The input variable that x (k) is the state variable of bicycle prediction model, u (k) is bicycle prediction model.
  2. A kind of 2. Trajectory Tracking Control method of unmanned bicycle according to claim 1, which is characterized in that the step The process of establishing of the discrete state equations of bicycle Self-balance Control System is in rapid 1：
    The equilibrium kinetics model of the bicycle of foundation is：
    Wherein,For the body sway angle of bicycle, δ is the handlebar steering angle of bicycle, and a lands for bicycle center of gravity G and trailing wheel Point P1Horizontal distance, h be vehicle body without tilt when bicycle center of gravity arrive ground distance, λ be bicycle front fork angle, vxFor certainly The forward speed of driving trailing wheel, c are that bicycle trails, and b is axle for bicycle away from g is acceleration of gravity, and s is Laplace operator；
    The control structure that the self-balancing control of bicycle is controlled plus feedovered using proportional-plus-derivative, control law expression formula are：
    Wherein, δdFor the output of the handlebar steering angle command, i.e. self-balancing controller of bicycle,For the target vehicle body of bicycle The input at inclination angle, i.e. self-balancing controller,For onFirst derivative, k1For proportionality coefficient, k2For differential coefficient, 1/k3 For feed-forward coefficients；
    Handlebar steering angle command δdThere are a low-pass filtering links between handlebar steering angle sigmaThe link is one one Rank inertial element, TSFor time constant；
    The discrete state equations of bicycle Self-balance Control System are：
    Matrix
    Matrix
    Wherein, intermediate variable K1=mh2, intermediate variableIntermediate variable K3=mgh, intermediate variableFeed-forward coefficientsM is the quality of bicycle,Incline for the vehicle body of bicycle Oblique angle,For onFirst derivative, δ be bicycle handlebar steering angle, T be discrete system sampling period, k for sampling Moment sequence number.
  3. A kind of 3. Trajectory Tracking Control method of unmanned bicycle according to claim 1, which is characterized in that the step Rapid 2 detailed process is：
    Bicycle forward speed v is bicycle rear forward speed vx, i.e. v=vx；Body sway angleKinematic influence is neglected Slightly disregard, draw the kinematics model of bicycle：
    <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <msub> <mover> <mi>x</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>a</mi> </msub> <mo>=</mo> <mi>v</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <msub> <mi>&amp;psi;</mi> <mi>a</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>y</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>a</mi> </msub> <mo>=</mo> <mi>v</mi> <mi> </mi> <msub> <mi>sin&amp;psi;</mi> <mi>a</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>&amp;psi;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>a</mi> </msub> <mo>=</mo> <mi>v</mi> <mo>/</mo> <mi>R</mi> <mo>=</mo> <mi>v</mi> <mi> </mi> <msub> <mi>tan&amp;delta;</mi> <mi>f</mi> </msub> <mo>/</mo> <mi>b</mi> </mrow> </mtd> </mtr> </mtable> </mfenced>
    Wherein, ψaFor the yaw angle of bicycle,For on ψaFirst derivative, δfFor effective handlebar steering angle of bicycle, i.e., Handlebar steering angle sigma is in the projection on ground, radius of turn when b for axle for bicycle is cycling away from, R, xaAfter bicycle Take turns pick-up point P1The coordinate of X-direction on earth axes O-XYZ,For on xaFirst derivative, yaFall for bicycle rear Place P1The coordinate of Y-direction on earth axes O-XYZ,For on yaFirst derivative, since bicycle is only in ground level Upper movement, thus without considering bicycle Z-direction coordinate；
    The handlebar steering angle sigma of bicycle and effective handlebar steering angle sigmafBetween have following relation：
    For the body sway angle of bicycle, λ is the front fork angle of bicycle；
    Bicycle is constant motion v (k)=v, will be obtained after above-mentioned kinematics model discretization：
    <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mi>a</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> <mo>=</mo> <mi>v</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mo>(</mo> <msub> <mi>&amp;psi;</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>)</mo> <mi>T</mi> <mo>+</mo> <msub> <mi>x</mi> <mi>a</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mi>a</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> <mo>=</mo> <mi>v</mi> <mi> </mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mo>(</mo> <msub> <mi>&amp;psi;</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>)</mo> <mi>T</mi> <mo>+</mo> <msub> <mi>y</mi> <mi>a</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;psi;</mi> <mi>a</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> <mo>=</mo> <mi>v</mi> <mi> </mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;lambda;</mi> <mi>tan</mi> <mo>(</mo> <mi>&amp;delta;</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>)</mo> <mi>T</mi> <mo>/</mo> <mi>b</mi> <mo>+</mo> <msub> <mi>&amp;psi;</mi> <mi>a</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mtd> </mtr> </mtable> </mfenced>
    Wherein, T is the sampling period of discrete system, and k is sampling instant sequence number；
    It is above-mentioned it is discrete after kinematics model to be non-linear, which is carried out near reference locus to linearize to obtain line Property kinematics model：
    <mrow> <mover> <mi>x</mi> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>A</mi> <mi>K</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mover> <mi>x</mi> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>B</mi> <mi>K</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mover> <mi>u</mi> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
    Wherein, matrixMatrix
    <mrow> <mover> <mi>x</mi> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>[</mo> <msub> <mi>x</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>x</mi> <mi>r</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>,</mo> <msub> <mi>y</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>y</mi> <mi>r</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>,</mo> <msub> <mi>&amp;psi;</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>&amp;psi;</mi> <mi>r</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mo>]</mo> <mi>T</mi> </msup> <mo>,</mo> <mover> <mi>u</mi> <mo>~</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mrow> <mo>[</mo> <mi>&amp;delta;</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>&amp;delta;</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>]</mo> </mrow> <mi>T</mi> </msup> <mo>;</mo> </mrow>
    Represent bicycle virtual condition and the departure of reference state；It is inclined with reference input to represent that bicycle is actually entered Residual quantity.
  4. A kind of 4. Trajectory Tracking Control method of unmanned bicycle according to claim 1, which is characterized in that the step Bicycle Trajectory Tracking Control algorithm concretely comprises the following steps in rapid 4：
    The target function J of Model Predictive Control generally uses the form of Linear-Quadratic Problem, since the track following in bicycle is asked In topic, control system is discrete system, and target function becomes：
    <mrow> <mi>J</mi> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <msub> <mi>k</mi> <mn>0</mn> </msub> </mrow> <mrow> <msub> <mi>k</mi> <mn>0</mn> </msub> <mo>+</mo> <mi>N</mi> <mo>-</mo> <mn>1</mn> </mrow> </munderover> <mo>&amp;lsqb;</mo> <msup> <mi>x</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>Q</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>x</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mi>u</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mi>u</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mi>d</mi> <mi>t</mi> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msup> <mi>x</mi> <mi>T</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>+</mo> <mi>N</mi> <mo>)</mo> </mrow> <msub> <mi>Q</mi> <mn>0</mn> </msub> <mi>x</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>+</mo> <mi>N</mi> <mo>)</mo> </mrow> </mrow>
    Wherein, k0For current time, N is estimation range, and Q (t) ties up positive semidefinite matrix for n × n, and R (t) ties up positive definite matrix for r × r, Q0Positive semidefinite matrix is tieed up for initial time n × n；
    By solving the minimum of target function J, so as to obtain current optimal input, solution procedure is as follows：
    Step 401：When initial, k=k is made0+ N, P (k0+ N)=Q0
    Step 402：The input u (k) at kth moment=G (k) x (k), wherein G (k) be feedback of status coefficient matrix, expression formula For
    G (k)=- (R (k)+B (k)TP(k+1)B(k))-1B(k)TP(k+1)A(k)
    Wherein, P (k+1) is the normal solution of Discrete Time-Varying Systems Riccati equation, and Discrete Time-Varying Systems Riccati equation expression formula 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 from subtracting 1, works as k=k0When, calculating terminates, otherwise, return to step 402；
    After the reverse iteration of n times to get to optimal list entries u (k0),u(k0+1),.....u(k0+ N-1), using work as Input value u (the k at preceding moment0) input as system current time, and remaining input value is given up, each sampling thereafter Moment repeats above-mentioned calculating；It is cycled by this principle to get to the optimal input value of each controlling cycle of system.
CN201510690834.4A 2015-10-22 2015-10-22 A kind of method of unmanned bicycle Trajectory Tracking Control Active CN105320129B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510690834.4A CN105320129B (en) 2015-10-22 2015-10-22 A kind of method of unmanned bicycle Trajectory Tracking Control

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510690834.4A CN105320129B (en) 2015-10-22 2015-10-22 A kind of method of unmanned bicycle Trajectory Tracking Control

Publications (2)

Publication Number Publication Date
CN105320129A CN105320129A (en) 2016-02-10
CN105320129B true CN105320129B (en) 2018-05-29

Family

ID=55247708

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510690834.4A Active CN105320129B (en) 2015-10-22 2015-10-22 A kind of method of unmanned bicycle Trajectory Tracking Control

Country Status (1)

Country Link
CN (1) CN105320129B (en)

Families Citing this family (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105539679B (en) * 2016-02-19 2017-12-15 桂林电子科技大学 Pulling type more piece modul bicycle machines robot mechanism
CN106080941B (en) * 2016-08-04 2018-12-18 清华大学 A kind of unmanned bicycle for realizing speed change balance control
CN106919177A (en) * 2017-03-31 2017-07-04 深圳市靖洲科技有限公司 A kind of unmanned balance of bicycle control method based on rate gyroscope
CN106997664B (en) * 2017-05-19 2020-01-17 宁波齐心科技股份有限公司 Control method and system for sharing intelligent driving of bicycle
CN107357168B (en) * 2017-06-01 2019-12-31 同济大学 Unmanned vehicle obstacle avoidance method based on opportunity constraint model predictive control
CN107932489A (en) * 2018-01-15 2018-04-20 哈尔滨理工大学 A kind of robot cycling device and control method
CN108334086B (en) * 2018-01-25 2021-06-22 江苏大学 Unmanned vehicle path tracking control method based on soft-constraint quadratic programming MPC
CN108762244B (en) * 2018-01-26 2020-06-23 浙江大学 Behavior-driven self-balancing unmanned bicycle and control method of equivalent mapping thereof
CN108357595B (en) * 2018-01-26 2020-11-20 浙江大学 Model-based self-balancing unmanned bicycle and model driving control method thereof
CN108327841B (en) * 2018-01-26 2020-10-30 浙江大学 Self-balancing unmanned bicycle and control method thereof
CN108646734B (en) * 2018-04-27 2021-04-02 天津大学 Nonlinear model prediction control method based on quantum particle swarm algorithm
CN108762095B (en) * 2018-07-12 2020-05-12 清华大学 Real-time partition stable control method for multi-agent system with discrete control quantity
CN109017808B (en) * 2018-08-16 2020-06-05 天津大学 Design method of model predictive controller of unmanned automobile
CN109948830B (en) * 2019-01-29 2021-03-02 青岛科技大学 Bicycle track prediction method, equipment and medium oriented to self-mixed environment
CN110032187B (en) * 2019-04-09 2020-08-28 清华大学 Unmanned motorcycle static obstacle avoidance path planning calculation method
CN109945885B (en) * 2019-04-16 2021-01-15 清华大学 Unmanned motorcycle dynamic obstacle avoidance path planning and calculating method
CN110134136A (en) * 2019-05-29 2019-08-16 清华大学 Unmanned motorcycle vision navigation system
CN110221611B (en) * 2019-06-11 2020-09-04 北京三快在线科技有限公司 Trajectory tracking control method and device and unmanned vehicle
CN110348140B (en) * 2019-07-15 2021-06-25 清华大学 Method and device for modeling and static balancing of two-wheeled robot based on towing distance
CN111361681B (en) * 2020-03-19 2021-02-09 北京大学 Device and method capable of realizing self-balancing of bicycle and bicycle
CN112445232A (en) * 2020-06-23 2021-03-05 国网浙江温岭市供电有限公司 Portable somatosensory control autonomous inspection robot
CN112477880B (en) * 2020-11-30 2022-05-20 天津大学 Longitudinal control method for unmanned automobile
CN113771837B (en) * 2021-09-03 2023-02-28 长安大学 Unmanned bicycle control method and system

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103085816A (en) * 2013-01-30 2013-05-08 同济大学 Trajectory tracking control method and control device for driverless vehicle
CN103631142A (en) * 2013-12-09 2014-03-12 天津工业大学 Iterative learning algorithm for trajectory tracking of wheeled robot
CN103926839A (en) * 2014-04-23 2014-07-16 浙江师范大学 Movement subdivision control method of wheeled mobile robot
CN104181923A (en) * 2014-08-29 2014-12-03 武汉大学 Intelligent automobile curve tracking method based on linear controller

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8775006B2 (en) * 2011-07-14 2014-07-08 GM Global Technology Operations LLC System and method for enhanced vehicle control

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103085816A (en) * 2013-01-30 2013-05-08 同济大学 Trajectory tracking control method and control device for driverless vehicle
CN103631142A (en) * 2013-12-09 2014-03-12 天津工业大学 Iterative learning algorithm for trajectory tracking of wheeled robot
CN103926839A (en) * 2014-04-23 2014-07-16 浙江师范大学 Movement subdivision control method of wheeled mobile robot
CN104181923A (en) * 2014-08-29 2014-12-03 武汉大学 Intelligent automobile curve tracking method based on linear controller

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Self-Tuning PID Controller for Autonomous Car Tracking in Urban Traffic;Luciano Alonso 等;《Proc of System Theory,Control and Computing(ICSTCC)》;20131219;全文 *
自主驾驶自行车的改进路径跟踪算法;余永超 等;《华中科技大学学报（自然科学版）》;20151016;第43卷;第345-350页 *

Also Published As

Publication number Publication date
CN105320129A (en) 2016-02-10

Similar Documents

Publication Publication Date Title
CN105320129B (en) A kind of method of unmanned bicycle Trajectory Tracking Control
CN106125728B (en) A kind of 4 wheel driven wheeled mobile robot trace tracking and controlling method
Lee et al. Self stabilizing strategy in tracking control of unmanned electric bicycle with mass balance
CN109799814B (en) Design method of track unmanned vehicle track tracking controller based on kinematics model
CN107160398B (en) The safe and reliable control method of Rigid Robot Manipulator is limited based on the total state for determining study
CN103472724B (en) A kind of real-time control dynamics modeling method for multiple freedom parallel mechanism
Dian et al. Modeling and trajectory tracking control for magnetic wheeled mobile robots based on improved dual-heuristic dynamic programming
CN105652869A (en) CMAC and PID-based omnidirectional mobile robot and moving control method
CN105174061B (en) Double pendulum crane length of a game optimal trajectory planning method based on pseudo- spectrometry
CN108227491A (en) A kind of intelligent vehicle Trajectory Tracking Control method based on sliding formwork neural network
CN103240739A (en) Automatic switching control method for decentralization and centralization of mobile manipulators
CN110045598B (en) Method for tracking and controlling underground advancing path of cantilever type heading machine
CN104192713A (en) Time-optimal bridge crane track planning method based on differential flatness and B-spline
CN108502053A (en) A kind of wheeled robot platform accuracy control method
CN109031947A (en) Trajectory Tracking Control and method based on radial base neural net
CN110989597A (en) Adaptive path tracking method of integrated fuzzy neural network
CN114510063B (en) Unmanned tracked vehicle and track tracking control method and system thereof
Zhao et al. PID trajectory tracking control of autonomous ground vehicle based on genetic algorithm
CN107193210A (en) A kind of adaptive learning default capabilities control method of nonlinear system
Raza et al. Balance stability augmentation for wheel-legged biped robot through arm acceleration control
Tian et al. Control of different-axis two-wheeled self-balancing vehicles
CN101794153A (en) Point-stabilized control method of intelligent trolley
CN107085432A (en) A kind of target trajectory tracking of mobile robot
CN107272419A (en) A kind of driver&#39;s adaptive direction control method based on improvement PSO
Zhang et al. Design of agv chassis motion control system based on mworks

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant