- Thiết kế bộ điều khiển mô hình Ballbot dùng NMPC Nonlinear Model Predictive Control - Mô phỏng và đánh giá kết quả.. In addition, a NonlinearModel Predictive Control NMPC is designed f
MATHEMATICAL MODEL FOR THE PLANAR SYSTEM
Model desCrIPtIOII - .- 199g ng re 5 2.2 ASSUMPTIONS 8n
The three-dimensional system is divided into three planar models where each can be described in two degree of freedom (DoF): e | DoF for the rotation of the ball. e | DoF for the rotation of the body.
The propulsion system is modelled as one virtual wheel which is not the same position and does not have the same speed as the omniwheels in the real system.
In the planar system model, three planar models are treated as three independent model It means there is no coupling effect among three of them Two of them (the model in XOZ plane and YOZ plane) are very similar to each other The third one which is shown on the right side of the Figure 2 describes the rotation about the z- axis.
The system is assumed to consist of three rigid bodies: the ball, the virtual wheel and the body Additionally, these assumptions are made: e The contact points between the ball and the ground and between the wheels and the ball are assumed to be free of slippage. e The friction is neglected except for the rotation of the ball on the ground around the z-axis. e We assume that the Ballbot consist of three rigid bodies: the Body, the actuating Wheel and the Ball.
Figure 2.2: Sketch of the planar model [4]
Model in the YOZ pÌane - - << 1111929 nh 6 2.4 Modeling in the XOY plane - - - HH ng ki 13
The model in XOZ plane and YOZ plane are similar to each other, so we can model the Ballbot in YOZ plane and use it for the other The left side of the Figure 2 shows the parameters of the Ballbot in the YOZ plane The description of the parameters is in the Table 2 The value of them is based on the physical model in [4].
Table 2.1: The parameters of Ballbot [4]
Symbol/Unit Description Value Mx/kg Mass of the Driven Ball 2.29 My/kg Mass of the Omni-Directional Wheel 3 my/kg Mass of the Body 9.2 Tím Radius of the Driven Ball 0.125 Tim Radius of the Omni-Directional 0.06
Wheel r,/m Radius of the Body (Cylinder) 0.1
Lim Gravity Center Height of the Body 0.339 O,/kg.m* | Inertia of the Ball 0.00239 Đyy/ kg.m? Woe of the Omni-Directional 0.00236
@,/kg.m* | Rotation Inertia of the Body 4.76 Dy The orientation of the ball Ủy The orientation of the body
To conduct the equation of motion in YOZ plane, we use the Lagrange method.
The following steps are needed: e Compute kinetic and potential energy of all the rigid bodies as a function of minimal coordinates. e Write non-potential forces as a function of minimal coordinates. e Solve the Lagrange equation for the second order derivative equation of the minimal coordinates.
The minimal coordinates in YOZ plane are chosen as below dyz — [5 (2.1)
The position of the ball, the wheels and the body can be written as follows.
XK = Px TK Xự = X + sind,y (Te + Ty)
The binding equation for t„ is derived by equating the velocities in the contact point between the ball and the actuating wheel.
Figure 2.3: Sketch of the planar model with the contact point [4]
> Vp = [Xx] + |—T.c0SÔy Py | = Km (2.6)0 0
Ve =|#x|+|o | x|ứx +rw).sim9„|+| 0 |x|Crw.sim0y (2.7)
0 fo] LQ + Ty) cosv,, 0 —Ty COSD,
> Vc = Be + |—Ứứk + Ty) cosd,.9,| + |—Tw.c0SÔy x (2.8)
01 | (try) sind, 0, Tự SIND Wy
> Vc = |Xx — (Tr + Tự) C0SÔy Oy — Tựy,COSÔy Wx (2.9)
(Te + Ty) SiNDy ệ„ + Tự sin0x Wy
Because there is no slip between the ball and the virtual wheel, we have vg Vc It means
{** — Tx COSY Oy = Xx — (Te + Ty) COSD, By — Ty COSY y Wy lc wánc6 Two (2 10)Ty SIND Oy = (Te + Tự) sin0y ệy + Tự Sindy W,
From one of two equation above, the tỦ„ can be solved as below
The kinetic and potential of each part. e The Ball
Tk = 5: Mx- (Te Px)? + 2 9y Gx”1 1
= ~ mự, (Tr Py + ệy cos0y (Tr + Tự ))“ + ~ Oy (= (dy — ử„) — 6)
=.T£ | My + vơ] Dy +1 x (cost (re +Tw)—— (= + )) Dye Ủy
+5 cos29„ mựy (Ty + Ty)? + Oy (= + 1) Oy (2.14)
Vw = Thự g (Ty + Ty) cosd, (2.15) e The Body
= 2:Tmạ: (Ty Dy + 0yx.cos0„ )“ + 2: Đạ- Ủy
= 2: mạ Tỉ: xy? + ra.T7 cosôÔy Ì Dy 1 ,By +
V, = m,.g.l.cosv,, (2.17) The kinetic and potential energy of the Ballbot on the YOZ plane is the sum of the kinetic and potential energy of three parts.
According to Lagrange equation d (—) (—) + (—) _ dt \aq aq aq = Ive with fyp is the non-potential forces of the Ballbot.
Each part of equation (2.20) is computed as below.
+ | rx cost, (my (Ty + TW) + mạ L) — Ow.— (= + 1) Dy
Px 2 'k K tre (My (Ty + Tự) +m, 1) (cosð; 3, — sind — Qựy.— “in,
+rx (css, (my (Te + tw) + mạ -— (= + 1) Ủy —
—r+ (My (Te + Mw) + mạ 1) sindằ 02
—— =re.| cosd, (my (Te + Ww) + mạ.) ——” (= + 1) Dy + Ủy Tự Tw r 2 ;
+ (o2 (my (Ty + Tw)* + mạụ l2) + Oy (= + 1) + 0.) 0, (2.25)
W fT) = [xy: (my (Te + Ty) + mạ Đ-cos8, — 6 (= +1) by +TK at \ 56.) Ye (My (Te + Tw) + m,.1).cosd,, wi Dye r, 2 :
+ em (Te + Tw)* + mạ l2) cos^9„ + Oy (= + 1) + 9, 0, —
—T+ (mựy (Ty + Tự) + mạ D sinễy Py ệ„ —
— sin(20,) (my (re + Tw)? + my 17) 82 (2 26)
OT ¢ 7007 —Tx sind, My (Ty + Tw) + mạ 1) 0y ệy — x
— 5 sin(20,) (my (Te + Tw)? + mg 17) 9 (2.27)
The Lagrange equation (2.20) can be written as the matrix form as follows.
The matrices contain masses and inertias M(q,q) = M M
, Coriolis forces C(q, j) = | and Gravitational forces G(q) = a are:
The following substitutions were used in the equations above. Œ = Thự (Ty + Tựy) + mạ Ì B = My - (Tx + Ty)? + THẠ [?
The only non-potential force is the torque from the actuating wheel:
= TT — = _— — 1 j0) 7 (Gx Ủy) + Ủy sứ +1)|-ọ
The counter torque acting on the coordinate J, of the body is express as:
The total non-potential force is the sum of those two:
F Ty T, fyueyz = ÍNP,yz1 t+ ÍNP,„yz2 = Bì = Tx “ (2.48)
2.4 Modeling in the XOY plane
Figure 2.4: Planar model in XOY plane [4]
Table 2.2: The parameters of Ballbot in XOY plane [4]
Symbol/Unit Description Value a Angle between bottom of the Wheel 400 and the center of the Ball Tim Radius of the Driven Ball 0.125 rim Radius of the Omni-Directional 0.06
Ox/kg.m7 | Inertia of the Ball 0.00239
Inertia of the Omni-Directional
O,xy/ kg.m’ | Rotation Inertia of the Body 0.092 QO, The orientation of the ball
0, The orientation of the body
The minimum coordinates are used as below
Because the reference point is on the XOY plane the potential energy of the all three parts are equal to zero The kinetic energy of each part e The Ball
The kinetic and potential energy of the Ballbot on the XOY plane is the sum of the kinetic and potential energy of three parts.
14 an) ~ (Gq) * (Gq) = ủz with fyp is the non-potential forces of the Ballbot.
Each part of equation (2.55) is computed as below.
OT rx 2 a, TK 2 2 ` a0 = x + Ow.ry-(—) Sin“œ 2 ~ Oway-(TM) Sin*a.d, d (OT AM : alga) = Ox + Ow.ay-(—*) Sin“ˆœ |.@„ —
—Ôwxy (=) Sin2dœ 9, lw OT _ ad, - rk 2 2 ˆ
+{ Oaxy + ỉự„y.|——] sin“œ |.é; lw d(9T\_- 6 (9 na G +
FK 2 c2 “ + | Oaxy + éự xy ơ Sin“œ|.éĐ,
The forces apply to the Ballbot on XOY plane e From the actuators
J› =[] (2.65) e Friction between ball and ground
—T; + 7 Sina T, ÍNPxy=lơ- lỡ + ]a.Tạ + J3.T3 = Tx “ (2.68)
From the Lagrange equation we have
@ + Ow xy: (*) in) - Pz — Ow xv (*) Sin?d.ệ„ = —Tr + sina Ty
— Oy xy: () Sn1“ơ Pz + (Gas + Ow xy () Sỉn ô].ð, = TT sina.T, — T;
( —rứ |Tr (n? Oaxy + rẻ 9ụ xy sin?a) + Ty (rẻ Đụ xy sin?œ — Tự Ty O4,xy sina) | Pz = ; ; ; tạ Ox (72 ĐA xy + Tỷ Ow xy Sina) + tệ sin2a Ow xy (2 rể Ow xy Sin? a + 7% ĐA vy)
—Tự |7 (Cw +r sina) (OK r + Ow xy TR sin?œ) + Ow xy Tả sin?a) — Tp Tỉ Tụ Ow xy: sin?a|
Z tạ On xy (Ti Ox + Tỷ Ow xy Sin2a) + Tỷ Oy vy sin2a (2.72 Ow xy sin2a + Hf Ox)
Equation for the motion solve for Ty ơ T, 1h SING (Tự Oaxy — Tr: Ow,xy- Sina)
= Ow xy MK Sin2a + Ty On xy ( )2.72
CONTROLLER DESIGN - HH HH nghe 18 3.1 The Principle of Nonlinear Model Predictive ControẽL ô25 ôôsss+xxcssss 18 3.2 NMPC Mathematical FormulafiOIi .- - - 5-5 E111 193511 1111335311111 se 20 3.3 Parameters CHOICE - - - c1 1199001019990 0g ng gg 23 3.4 Advantages and Disadvantages of NMPC HH ng re 23 3.5 Apply NMPC to the Ball bot - - 1 1321111923119 ng ng ngư 24 3.5.1 Moving from point to point without obstacle - ôS22 sa 24 3.5.2 Moving from Point to Point with an obsfacÌe ôc2 sa 26
3.1 The Principle of Nonlinear Model Predictive Control
Nonlinear model predictive control (NMPC) is one of the most general and flexible approaches to control nonlinear systems based on optimization method.
#=/ 4.0 where x € R” is the state, u € IR" is the control input and y € IR” is the output of interest It is assumed that the state is measured with the sampling time Ts.
A prediction X(t) of the state over the interval [t,t + Tp] is obtained by integration () Tp = Ts is called the prediction horizon.
At atime 7 € [t,t + Tp], the predicted state is the function of the initial state x(t) and the input signal
X(t) = R(t, x(t), u(t: t)) (3.2) where u(t: T) denotes the input signal in the interval [t, z].
Typically, the input signal is assumed constant after a certain time 7, called the control horizon. u(t) = u(t +Tc), + € [t + Tc,t + Tp] (3.3) where 0 < Tẹ < Te < Tp.
4 past | future/prediction predicted state 2 closed-loop R statex - 4 open loop input u closed-loop inputu — f t+Ts t+T, t+T, control horizon 7,
Figure 3.1: The principle of NMPC [7]
At each time £ = tự = k.Ts,k = 0,1,2, we look for an input signal u*(t: T) such that the predicted state
X(t, x(t), u(t: T)) has a desired behavior for 7 € [t, t + Tp] It means the input signal ”(£: £ + Tp) is chosen as one minimizing the following objective function.
J(uŒ:t + Tp)) =| [llZp(z)llệ + u(r) I12] dt + WXp(t+ TPR =GB.4 t+Tp where Xp(t) = x(t) — X(T) is the predicted tracking error and x,(T) is a reference to track.
|| l[y are the weighted norms and their integrals are signal norms.
The term ||#p(£ + Tp) |lệ gives further importance to the final tracking error.
The term ||w(t)||% allows us to manage the trade-off between performance and command activity.
19 e X(t) satisfies the model equation (3.1) It means
A classical example is represented by obstacles. e [nput constraints u(t) CU,, Vr€ Ịt,t + Tp]
A classical example is represented by input saturation.
At each time t = tự, the following optimization problem is solved: u*(t:t + Tp) = arg min J (u(t: t + Tp))
X(t) EX, Vr€ [t,t + Tp] u(t) EU, Vr€ Ịt,t + Tp] u(t) = u(t+Tc), r+€[t+ Tc,t +Tp]
(3.9) (3.10) (3.11) (3.12) where 0 set-point predicted state x closed-loop state x - ˆ
~ a —“ input u ; t i-+T, t+T, control/prediction horizon 7, Figure 3.2: Piecewise constant input signal [7]
As can be seen from figure above, the input signal is represented by a vector with finite-dimension.
Instead using 1 in the optimization problem in (), we can use c for the variable.
After solving that problem, we have c; The optimal input is given by: m w(t) =) ¢f.gi(0) (3.17) i=1
Suppose that at the time t = t,, the optimal input signal ”(£: t + Tp) has been computed by solving the optimization problem u*(t: t + Tp) is applied for the entire time interval [t,t + Tp], so it does not perform a feedback action Therefore, it cannot increase the precision, and reduces error and disturbance effects.
The NMPC feedback control algorithm is obtained by means of a so-called receding horizon strategy: e Attime t = t,,compute u*(t:t + Tp) via optimization. e Apply only the first input value: (7) = u*(t = t„) and keep it constant for
Vt € [tạ,ty+1ẽ. e Repeat two steps above for t = ty44, wy+a, -
In summary, we have the NMPC algorithm. e Ateach time t = t,, solve the optimization problem: c*=arg “đun IC) (3.18) subject to
#=ƒ(,u), Kt) =x(t) (3.19) 2Œ) E Xo, Vr€ [t,t +Tp]Ì (3.20) u(+)€UÚ,, Vre€ [t,t + Tp] (3.21) u(t) = u(t + Tc), TE [t + Tc,t + Tp] (3.22) o Optimal open-loop input: (7) = Ui", c7 6; (1) o Closed-loop control law: u(t) = u*(t;), VT € [ty tsi].
22 e Repeat step above for t = ty44,thi2,- 3.3 Parameters choice
We have to choose some parameters. e 7s: In many situations, the sampling time is given and cannot be chosen.
However, we have to choose it in some cases A trial and error procedure can be adopted T; should be:
> suficently small to deal with the plant dynamics.
> not too small, to avoid the numerical problems and slow computation. e Tp,Tc: can be chosen through a trail and error procedure in simulation It is convenient to set Tc = Tp Observations:
> A large Tp increases the closed-loop stability properties.
> A too large Tp may reduce the tracking accuracy. e m: Usually a low number of m is enough to obtain a satisfatory control performance.
The diagonal matrices Q, R, P We use the trial and error procedure, change the value of Q;;, Riz, P;; untill the requiements are satisfied in simulation.
3.4 Advantages and Disadvantages of NMPC e Advantages
> Constraints and input saturation accounted for.
> Efficient management of performance/input activity trade-off.
> Optimal trajectories (over finite time interval). e Disadvantages
> Possible local minima in optizimation problem.
> High on-line computational cost.
3.5 Apply NMPC to the Ballbot 3.5.1 Moving from point to point without obstacle
From initial position, the Ballbot moves to the desired position and minimizing the cost function J(u) as well as rotate the body 90 degree about the z-axis.
At first, started point and the destination are considered The state is made from the YOZ plane and XOZ plane. x=|ứ Ú ộy Ủy ỉw Ủy go |] (3.23) where
~,, represent the orientation of the ball in the YOZ plane J, represent the orientation of the body in the YOZ plane
@y represent the orientation of the ball in the XOZ plane Ủy represent the orientation of the body in the XOZ plane
The input signal is u=llzy Ty] (3.24)
Due to the same priciple of XOZ and Y OZ planar, the last four state are defined as the first four The first four can be defined from the (2.29) ko = Xq (3.26)
Fì Mạ — Fy My, — Cy.M21, + Co.M,,+G).M ka — 1 21 2 11 1 21 2 11 2 11 (3.28)
Mìa.M¿i — My1.M22 with M,,,M12,Mo21,M22,C,, C2, F,, Fy, and G2 are defined in the modeling section.
There is one condition which is relavant to the orientation of the Body while Ballbot moving need to be added to the controller The orientation angle of the Body the orientation of the body can not be greater than 30 degree (or 77/6 radian). lA X2 lA lA Xs lA œ=i1ola TU 6 (3.29) 6
Next the orientation of the Ball and the Body about z-axis are considered The approach is the same as the one is used above Due to the condition for the stiction, the only variable has to be dealed with is the orientation of the body. x=[9, 9,]' (3.30) with J, represent the orientation of the body in the XOY plane
Figure 3.3: Block diagram of the NMPC
3.5.2 Moving from Point to Point with an obstacle In the case dealing with the obstacles, the goal is moving Ballbot to the destination while avoding the obstacles The Ballbot need to rotate 90 degree about Z-aXIS aS same as above.
The setup for the controller is the same as above, but there is one more condition which is about the obstacle need to be added to the controller For example, there is one round obstacle of radius 0.4 meter which is at (x, y) = (1.5,2) The Ballbot will avoid the obstacle if and only if
Because the radius of the Ball is 0.125 meter, the eqution (3.33) becomes
SIMULATION RESULTS .- ng ngư 27 4.1 Point to Point 2G G9 ng vớ 27 4.2 Point to Point with an obsfaCẽ€ - - 2c Q12 SH ng ng ngu 31
In this section, the controller is tested in the simulation environment The Ballbot moves from (x, y) = (0,0) to (x, y) = (4,5) The initial state is set as
Xo = [000.1 0.100 0.1 0.1]’ (4.1) and the reference is r = [4000032000]’' (4 2)
The parameters which is used in this section is shown in the Table 2.1 and Table 2.2.
_— phi i] ——ef-phi, 25 | ‘ —— ref-phi angle [rad]
Ballbot takes almost 15 seconds to move from (x,y) = (0,0) to (x, y) = (4,5)
Figure 4.2: Simulation of 0, and Dy
As can be seen from Figure , the maximun angle of J, and J, while Ballbot is moving is less than 0.08 rad ( about 4.58 degree) which is acceptable. control input
Figure 4.3: Control input T,, and Ty
Figure 4.4: The trajectory of Ballbot a,
Figure 4.5: The trajectory at the destination
The trajectory of the Ballbot shows that Ballbot goes to exactly the desired point.
Figure 4.6: Simulation of 0, The Body of the Ballbot takes less than 10 seconds to rotate 90 degree about z- l | | | J 0 5 10 15 20 25 time |S]
Figure 4.7: Control input T, The torque in the real system is different from the one in assume in planar model Therefore, the conversion is needed (the details is in the Appendix) The torque which is applied on three omniwheels is illustrated as below.
Figure 4.8: Control input in the real system As can be seen from the Figure 4.8, the control input converge to zero when the Ballbot is at the destination When Ballbot moving, the control torque always oscillate to guide Ballbot on desired trajectory.
4.2 Point to Point with an obstacle
The Ballbot moves to the same destination as the first section, but this time there is one obstacles along the way.
Ballbot also takes about 15 seconds to move to the destination.
Figure 4.10: Simulation of 0y and Ủy
The maximum angle of 0x and J, while Ballbot is moving is less than 0.08 rad (about 4.58 degree) which is acceptable.
Figure 4.11: Control input T,, and Ty sẮ
Figure 4.12: The trajectory of Ballbot
Figure 4.13: The comparison between the trajectory of Ballbot without and with obstacle As can be seen seen from two figures above, the Ballbot has to move in different path when compare to the simulation result in last situation.
Figure 4.15; Control input T, The simulation results in the XOY plane are the same as the last section.
The comparison of the control input in two situations are shown as below
Figure 4.16: The comparison of TÌ
In all the three omniwheels, inputs which are deal with the obstacle are need more energy than the ones move freely All the inputs converge to the zero when Ballbot is at destination The energy reach the peak at about 2.5 seconds when Ballbot try to avoid the obstacle
CONCLUSIONS AND FUTURE WORKS 00000 eeeeeeees 37 5.1 COMCIUSIONS 5 a
Future WOrkS 20 cccccccesssnececcesesneeccesssseeeeecssseceecceseaaeccesesaeeeeeeeseeeeeseeeeaaaeeceneeaas 37
A 3-D model could be built for the further research, so the controller will be more precise More applications may be developed such as tracking a desired trajectory The controller could be improved by adding some noises.
[1] T.B.Lauwers, G.A.Kantor, and R.L.Hollis, “A dynamically stable single-wheeled mobile robot with inverse mouse-ball drive”, Proceedings IEEE International Conference on Robotic and Automation, Orlando, Florida, May 2006.
[2] Xu Su, Cun Wang, Weiwei Su, and Yan Ding, “Control of Balancing Mobile Robot on a Ball with Fuzzy Self-adjusting PID”, 28” Chinese Control and Decision Conference, 2016.
[3] Masaki Kumagai, and Takaya Ochiai, “Development of a robot balancing on a ball”, International Conference of Control, Automation, and System, Seoul, Korea, October 2008.
[4] Peter Fankhauser and Corsin Gwerder, “Modelling and Control of a Ballbot”, Bachelor Thesis, Swiss Federal Institute of Technology Zurich, 2010.
[5] Ching-Chih Tsai, Cheng-Kai Chan, and Lung-Chun Kuo, “LQR Motion Control of a Ball-Riding Robot”, The 2012 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Kaohsiung, Taiwan, July 11-14, 2012.
[6] Justin Fong and Simon Uppill, “Ballbot” Preliminary Report, University of Adelaide, May 22, 2009.
[7] Carlo Novara, “Nonlinear Control and Aerospace Applications”, Lecture Notes, Politecnico di Torino, 2017.
[8] Rolf Findeisen and Frank Allgower, “An Introduction to Nonlinear Model Predictive Control” 2/° Benelux Meeting on System and Control, The Netherlands, March 19-21, 2002.
[9] Eduardo F Camacho and Carlos Bordons, “Nonlinear Model Predictive Control:
An Introductory Review”, Assessment and Future Directions of NMPC (pp I-16), Berlin Heidelberg, 2007.
[10] Lar Grune and Jurgen Pannek, “Nonlinear Model Predictive Control-Theory and Algorithms”, Book, Springer London Dordrecht Heidelberg New York, 2011.
Figure A.1: Torque and tangential forces generated by the real actuating system
The planar model uses a virtual wheel to simplify the system The real system has an actuating structure which differs strongly the one assume in the planar model.
Since the controller is going to be implement to the real system A conversion has to be calculated.
The structure of the real system is on the left side of the Figure A.1 While on the right the top view of the real actuating system is showed The torque of each omniwheel generates a tangential force on the surface of the ball.
Tangential forces of the real configuration
Lever of real configuration cos B sina
Tew = Tr: sn B sin | (A 4) cosa
Ykw.2 = TK] „ 27 (A 5) sin (4 + =) sina cosa
Tkw,3 —“Tg-| „ 27 (A 6) sin (6 — =) sina cosa
Tangential force of the virtual configuration plane
Lever of the virtual configuration plane
Torque on the Ball from the real omniwheels:
Tgw,+ = Tew X Fw (A 12) Trw,2 = Tgw,2 X Fw,2 (A 13) Tgw,3 = Tgw,3 X Fw,3 (A 14)
Torque on the Ball from the virtual omniwheels:
Although there are two different systems of omniwheels, they need to introduce the same torque of the ball
Tew, + Tgw,a + Tgw, = Tgw,x + ẽgw„y + ẽgw„z (A 18)
Solved for the real motor torque
Tạ = a(t + (sin B (V3T, + Ty) + cos 8(—T, + vấn,) (A 21)1 1
B Matlab Code B.1 Main program clear all close all cle
L=0.3339; g=9.81; x=sym('x', [8,1], 'real'); u=sym('u',[2,1], 'real'); anpha=mw* (rk+rw) +ma*L; beta=mw* (rk+rw) *2+ma*L*2; gama=rk/rw+1; mtEot=mk+mw+ma ;
6X M11=(mtot+Jw/rw*2) *rk*24+Jdk;
M12=rk* (cos (x (2) ) *anpha-Jw/rw* gama) ; M21=rk* (cos (x (2) ) *anpha-Jw/rw* gama) ; M22ta*cos (x (2) ) ^2+Jw*qama^2+ưa;z
E2=-rk/rw*u(1); xd (1) =x (3); xd(2)=x(4); xđ(3)=(F1*M22-F2*M12-C1*M22+Œ2*M12+G2*M12) /(M11*M22-M12*M21); xd (4) =(FL*M21-F2*M11-C1*M214+C2*M114+G2*M11) /(M12*M21-M11*M22);
42 ®) o Y M11 y=(mtot+Jw/rw^2) *rk^2+Jk;
M12_ y=rk* (cos (x(6) ) *anpha-Jw/rw*gama) ; M21_ y=rk* (cos (x(6) ) *anpha-Jw/rw*gama) ;
M22 yta*cos (x (6) ) ^2+Jw*gama^2+Ja;
Fl _y=rk/rw*u(2)z F2 y=-rk/rw*u(2); xd (5)=x (7) d(6)=x (8) xd(7)=(El y*M22 y-F2 y*M12 y- Cl y*M22 y+C2 y*M12 y+G2 y*M12 y)/(M11 y*M22 y-M12 y*M21 y); xd(8)=(El y*M21 y-F2 y*MII y- Cl y*M21 y+C2 y*M11 y+G2 y*M11 y)/(M12 y*M21 y-M11 y*M22 y); ° LỄ ° LỄ xd=xd'; matlabFunction(xd,'File','model ballbot','Vars', [u;x]);
S$ om: optimization model om.Ts=0.0001; om Tp=3;
% om.Q=diag([lel5 10 les 0]); om Q=lel8*eye (8); om.R=le3*eye (2);
% om.P=diag([lelO 0 1e8 0]); em P=le20*eye (8);
% Oom.ub=l*ones (3,1); om.m=2; om=nmpc design(xd,om) ;
%simulation x0O=[O0;0;0.1;0.1;0;0;/0.1;0.1]; ref=[32;0;0;0;40;0;0;0]; tinf%; open('nmpc ballbot') 6 sim('nmpc_ ballbot')
B.2 NMPC design function om=nmpc_design(f, om) om.n=size(f,1); i1=1; while any(diff(f,['u' int2str(i)])~=0) om.nu=1;
1Ý ~isfield(om, 'Q') om Qs=eye (om.n) ; else om.Qs=sqrt(om.Q); end 1f ~isfield(om, 'R') em Rs=zeros (om.nu); else em Rs=sgrt (om.R) ; end
1£ ~isfield(om, 'P') om Ps=eye (om.n) ; else om.Ps=sgrt(om.P); end 1£ ~isfield(om, 'm') om.m=1; end 1£ ~isfield(om, 'ub') om.ub=[]; end 1£ ~isfield(om, 'lb') om.lb=[]; end 1£ ~isfield(om, 'nlc') om.nlc=[]; end om.nc=om.nu*om.m; syms t real x=sym('"x',[om.n 1], 'real'); u=sym('u', [om.nu 1],'real'); c=sym('c',[om.nu om.m], 'real'); uu=c* (t.*(O0:om.m-1))'; f=subs(f,u,uu);
C=reshape (c,om.nc, 1); matlabFunction(f,'File','f model','Vars',{t,x,c});
B.3 NMPC law function ustar=nmpc_law(om, w) r=w(l:om.n); x=w(om.n+1:om.n+om.n) z cO=w(om.ntom.nt+l:end) ; °
1£ om.nlc== nic=@(u)nlcon(om, x,u) ; else nic=[]; end op=optimoptions('fmincon', 'Display','off',
'OptimalityTolerance',1le-3, 'StepTolerance',1le-3); ustar=fmincon(@(c)obj fun(om,r,x,c),c0,A,b,Aeq,beq,om.1b,om.u b,nlc,op); end function [J,xhat]=obj fun(om,r,x,u)
[~,xhat]=ode45 (@(t,x)f£ model (t,x,u),[0,om.Tp],x); xtilde=ones (size(xhat,1),1)*r'-xhat; u=reshape(u,om.nu,om.m) ;
J=norm(xtilde*om.Qs)*2+norm(u'*om.Rs) *2+norm(xtilde(end,:)*om PS)^2; end
B.4 nlcon function [con, coneg]=nlcon(om,x,u)
Nc0; tt=linspace(0,om.Tp,Nc) ; [~,xhat]=ode45(@(t,x)f£ model (t,x,u),tt,x); con=[xhat(:,2)-pi/3;
-xhat(:,1).^2-xhat(:,5).^2+xhat(:,1).*24+xhat(:,5).*32- 384]; coneq=[ ]; end
Stability for the closed loop is always important for any control theory, NMPC is not an exception This problem has been tackled from different points of view, and several contributions have appeared in recent years In paper [9], the author introduces some main proposals which is listed below.
Infinite horizon: This solution consists of increasing the control and prediction horizons to infinity In this case, the objective function can be considered as a Lyapunov function, providing nominal stability This is an important concept, but it cannot be directly implemented since an infinite set of decision variables should be computed at each sampling time.
Terminal constraint: This is a solution considering a finite horizon and ensuring stability by adding a state terminal constraint of the form x(t + Tp) = x ; With this constraint, the state is zero Consequently, the system stays at the origin.
Dual control: The idea was to define a region around the final state inside which the system could be driven to the final state by means of a linear state feedback controller The constraint is: x(t + Tp) € ©
The NMPC algorithm is used outside the region in such a way that the prediction horizon is considered as a decision variable and is decreased at each sampling time Once the state enters 0, the controller switches to a previously computed linear strategy.
Quasi-infinite horizon: This idea used the terminal region and stabilizing control, but only for the computation of the terminal cost The control action is determined by solving a finite horizon problem without switching to the linear controller even inside the terminal region The method adds the term
[Xp(t + Tp) || to the cost function This term is upper bound of the cost
47 needed to drive the nonlinear system to the origin starting from a state in the terminal region, so this finite horizon cost function approximates the finite- horizon one.
Stability of NMPC oo —
Stability for the closed loop is always important for any control theory, NMPC is not an exception This problem has been tackled from different points of view, and several contributions have appeared in recent years In paper [9], the author introduces some main proposals which is listed below.
Infinite horizon: This solution consists of increasing the control and prediction horizons to infinity In this case, the objective function can be considered as a Lyapunov function, providing nominal stability This is an important concept, but it cannot be directly implemented since an infinite set of decision variables should be computed at each sampling time.
Terminal constraint: This is a solution considering a finite horizon and ensuring stability by adding a state terminal constraint of the form x(t + Tp) = x ; With this constraint, the state is zero Consequently, the system stays at the origin.
Dual control: The idea was to define a region around the final state inside which the system could be driven to the final state by means of a linear state feedback controller The constraint is: x(t + Tp) € ©
The NMPC algorithm is used outside the region in such a way that the prediction horizon is considered as a decision variable and is decreased at each sampling time Once the state enters 0, the controller switches to a previously computed linear strategy.
Quasi-infinite horizon: This idea used the terminal region and stabilizing control, but only for the computation of the terminal cost The control action is determined by solving a finite horizon problem without switching to the linear controller even inside the terminal region The method adds the term
[Xp(t + Tp) || to the cost function This term is upper bound of the cost
47 needed to drive the nonlinear system to the origin starting from a state in the terminal region, so this finite horizon cost function approximates the finite- horizon one.