hệ xe 2 bánh tự cân bằng là hệ phi tuyến dễ dàng bị mất ổn định khi không có tín hiệu điều khiển tích cực tác động vào. Các bộ điều khiển như PID, Fuzzy hoạt động rất tốn năng lượng để giữ được hệ thống ổn định tĩnh. Vì vậy việc thiết kế 1 bộ điều khiển tối ưu là rất cần thiết để tối ưu hóa năng lượng trong quá trình điều khiển
Hanoi University of Science and Technology School of Electrical Engineering ********** PROJECT II TOPIC: MODELLING AND DESIGN OPTIMIZATION CONTROLLER FOR 2-WHEELS SELF BALANCING ROBOT Doctor: Nguyen Thanh Huong Student: Nguyen Tuan Hung 20176941 Vu Duc Phuc 20170145 Class: CTTT-HTĐ.TDH-K62 Hanoi, 1/2021 CONTENTS I Introduction………………………………………………………… …… 1.1 Role of the wheeled balance robot……………………… … …… 1.2 Aim of project…………………………………………………… … II Mathematics model ……………………………………………… …….4 2.1 Reference system and object parameters………………… …… ……4 2.2 Dynamics model……………… …………………………………… III Controller design………………………………….………………………8 3.1 LQR controller … ……………………………….………………… 3.2 Simulation………………………………………… ………………….10 3.2.1 Linearization……………………………………………….……….10 3.2.2 Define the K control matrix.……………………………………… 10 IV Results.……………………………………………………………………11 4.1 The reference value is set to 0……………………………………… 11 4.2 The reference value is set 𝜃 = 𝜋 𝜋 and 𝜙 = ………………………….13 4.3 When the system has disturbance…………………………………….15 4.4 Discussion…………………………………………………………….18 V Conclusion………………………………………………………………….18 REFERENCE………………………………………………………………….18 Table of Figure: Page Figure 1: 3D model of the wheeled balance robot Figure 2: Mathematical model of object in MATLAB/Simulink 10 Figure : Closed loop model of LQR controller in MATLAB/Simulink 11 Figure : The medium angle of the left and right wheels when the reference value is set to 12 Figure : The inclined angle of robot’s body when the reference value is set to 12 Figure : The robot's rotation angle in the xy plane when the reference value is set to 13 Figure : The medium angle of the left and right wheels when the reference value is set 𝜋 𝜋 𝜃 = and 𝜙 = 14 Figure : The inclined angle of robot’s body when the reference value is set 𝜃 = 𝜋 𝜋 and 𝜙 = 14 Figure : The robot's rotation angle in the xy plane when the reference value is set 𝜃 = 𝜋 𝜋 and 𝜙 = 15 Figure 10 : The medium angle of the left and right wheels when the system has disturbance 15 Figure 11 : The inclined angle of robot’s body when the system has disturbance 16 Figure 12 : The robot's rotation angle in the xy plane when the system has disturbance 16 Figure 13 : The medium angle of the left and right wheels when change Q matrix 17 Figure 14 : The inclined angle of robot’s body when change Q matrix 17 Figure 15 : The robot's rotation angle in the xy plane when change Q matrix 18 I Introduction 1.1.Role of the wheeled balance robot Nowadays, self balancing robots are widely used in many fields of the life Self balancing robots are self-moving and balancing in 2-wheels vehicles and versatile with many indoor and outdoor application However, the control of 2-wheels balancing robot is not easy because the 2WBR is an underactuated system This unstable causes the PID and Fuzzy controllers to use a lot of energy to keep the system stable An optimized control algorithm is designed (LQR) to optimize control energy so that the energy loss while keeping the system stable is minimal The following parts are presented as follows: Section II describes the object modeling method Next, section III will present the LQR control algorithm, section VI will present the simulation results Finally, section V concludes on the optimal control method for 2-wheelers selfbalancing robot 1.2.Aim of project This project aims to model and simulate to find the optimal control method as well as parameters for the controller to save energy for the robot during the control process II Mathematics model 2.1.Reference system and object parameters Diagram and reference system corresponding to the robot's coordinate axis Figure 3D model of the wheeled balance robot TABLE 1: SPECIFICATION OF THE ROBOT PARAMETER DESCRIPTION VALUE UNITS M Mass of the robot kg m Mass of the wheel 0.05 kg R Radius of the wheel 0.05 m W The width of robot 0.15 m D The thickness of robot 0.03 m H The height of robot 0.3 m L The distance from the robot's center of mass to the wheel shaft 0.15 m 𝑓𝑤 Coefficient of friction of wheel with moving surface 0.18 𝑓𝑚 Coefficient of friction of robot with wheel shaft 0.002 𝐽𝑚 Moment of inertia of DC motor 0.1 𝑘𝑔𝑚2 𝑅𝑚 Resistor of DC motor 50 Ω 𝐾𝑏 Coefficient of EMF of DC motor 0.468 Vs/rad 𝐾𝑡 Torque of DC motor 0.317 Nm/A N Deceleration ratio 40 g Gravitational acceleration 9.81 𝜃 The medium angle of the left and right wheels rad 𝜃 𝑙,𝑟 The angle of the left and right wheels rad 𝜓 The inclined angle of robot’s body rad 𝜙 The robot's rotation angle in the xy plane rad 𝑥𝑙 , 𝑦𝑙 , 𝑧𝑙 Coordinate of left wheel m 𝑥𝑟 , 𝑦𝑟 , 𝑧𝑟 Coordinate of left wheel m 𝑥𝑚 , 𝑦𝑚 , 𝑧𝑚 Medium coordinate m 𝐹𝜃 , 𝐹𝜓 , 𝐹𝜙 Torque of actuation according to each coordinate system Nm 𝐹𝑙,𝑟 Torque of actuation according to each left and right wheel Nm 𝑖𝑙 , 𝑖𝑟 Current of left and right motor A 𝑚/𝑠 𝑣𝑙 , 𝑣𝑟 Voltage of left and right motor V 𝐽𝜓 Moment of inertial of inclined angle of robot’s body 𝑘𝑔𝑚2 𝐽𝜙 Moment of inertial of the robot's rotation angle in the xy plane 𝑘𝑔𝑚2 2.2.Dynamics model The medium angle of the left and right wheels: 𝜃 = 1/2 × (𝜃𝑙 + 𝜃𝑟 ) (1) The robot's rotation angle in the xy plane: ∅= 𝑅 𝑊 × (𝜃𝑙 − 𝜃𝑟 ) (2) Medium coordinate in inertial reference system: 𝑥𝑚 ∫ 𝑥̇ 𝑚 ∫ 𝑅𝜃̇ cos 𝜙 [𝑦𝑚 ] = [∫ 𝑦̇𝑚 ] = [ ∫ 𝑅𝜃̇ sin 𝜙 ] 𝑧𝑚 𝑅 𝑅 (3) Coordinate of left wheel in inertial reference system: 𝑊 𝑥𝑚 − sin 𝜙 𝑥𝑙 [𝑦𝑙 ] = [𝑦 + 𝑊 cos 𝜙] 𝑚 𝑧𝑙 𝑧𝑚 (4) Coordinate of right wheel in inertial reference system: 𝑊 𝑥𝑚 + sin 𝜙 𝑥𝑟 [𝑦𝑟 ] = [𝑦 − 𝑊 cos 𝜙] 𝑚 𝑧𝑟 𝑧𝑚 (5) Symmetrical center coordinates between two motors in inertial reference system: 𝑥𝑏 𝑥𝑚 + 𝐿 sin 𝜓 cos 𝜙 [𝑦𝑏 ] = [ 𝑦𝑚 + 𝐿 sin 𝜓 cos 𝜙 ] 𝑧𝑏 𝑧𝑚 + 𝐿 cos 𝜓 (6) Torque of actuation according to each coordinate system 𝐹𝑙 + 𝐹𝑟 𝐹𝜃 ̇ ̇ ̇ ̇ [ 𝐹𝜓 ] = [−𝑛𝐾𝑡 𝑖𝑙 − 𝑛𝐾𝑡 𝑖𝑟 − 𝑓𝑚 (𝜓 − 𝜃𝑙 ) − 𝑓𝑚 (𝜓 − 𝜃𝑟 )] 𝑊 𝐹𝜙 (𝐹𝑙 − 𝐹𝑟 ) 𝑅 (7) Torque of actuation according to each left and right wheel 𝐹𝑙 = 𝑛𝐾𝑡 𝑖𝑙 + 𝑓𝑚 (𝜓̇ − 𝜃̇𝑙 ) − 𝑓𝑤 𝜃̇𝑙 (8) 𝐹𝑟 = 𝑛𝐾𝑡 𝑖𝑟 + 𝑓𝑚 (𝜓̇ − 𝜃̇𝑟 ) − 𝑓𝑤 𝜃̇𝑟 (9) Using PWM pulse to control the motor torque, so a formula of current and voltage relation is derived: 𝐿𝑚 𝑖𝑙,𝑟 = 𝑣𝑙,𝑟 + 𝐾𝑏 (𝜓̇ − 𝜃̇𝑙,𝑟 ) − 𝑅𝑚 𝑖𝑙,𝑟 (10) Ignore inductance of armature of DC motor: 𝑖𝑙,𝑟 = 𝑣𝑙,𝑟 +𝐾𝑏 (𝜓̇−𝜃̇𝑙,𝑟 ) (11) 𝑅𝑚 Torque of actuation according to each coordinate system: 𝛼(𝑣𝑙 + 𝑣𝑟 ) − 2(𝛽 + 𝑓𝑤 )𝜃̇ + 2𝛽𝜓̇ 𝐹𝜃 [ 𝐹𝜓 ] = [ −𝛼(𝑣𝑙 + 𝑣𝑟 ) + 2𝛽𝜃̇ − 2𝛽𝜓̇ ] 𝑊 𝑊2 𝐹𝜙 𝛼(𝑣𝑙 − 𝑣𝑟 ) − (𝛽 + 𝑓𝑤 )𝜙̇ 2𝑅 With 𝛼 = 𝑛𝐾𝑡 𝑅𝑚 (12) 2𝑅 and 𝛽 = 𝑛𝐾𝑡 𝐾𝑏 𝑅𝑚 + 𝑓𝑚 Kinetic equation of translational motion: 1 𝐾1 = 𝑚(𝑥̇ 𝑙 + 𝑦̇ 𝑙 + 𝑧̇𝑙 ) + 𝑚(𝑥̇ 𝑟 + 𝑦̇𝑟 + 𝑧̇𝑟 ) + 𝑀(𝑥̇ 𝑏 + 𝑦̇ 𝑏 + 𝑧̇𝑏 ) (13) Kinetic equation of rotation: 2 2 1 1 1 𝐾2 = 𝐽𝑤 𝜃̇𝑙 + 𝐽𝑤 𝜃̇𝑟 + 𝐽𝑤 𝜓̇ + 𝐽𝜓 𝜓̇ + 𝐽𝜙 𝜙̇ + 𝑛2 𝐽𝑚 (𝜃̇𝑙 − 𝜓̇) + 𝑛2 𝐽𝑚 (𝜃̇𝑟 − 𝜓̇) (14) Potential energy equation: U = 𝑚𝑔𝑧𝑙 + 𝑚𝑔𝑧𝑟 + 𝑀𝑔𝑧𝑏 (15) Lagrange equation: L = 𝐾1 + 𝐾2 − 𝑈 (16) 𝑑 𝜕𝐿 ( ) 𝑑𝑡 𝜕𝜃̇ − 𝜕𝜃 = 𝐹𝜃 (17) 𝑑 𝜕𝐿 ( ) 𝑑𝑡 𝜕𝜓̇ − 𝜕𝜓 = 𝐹𝜓 𝑑 𝜕𝐿 ( ) 𝑑𝑡 𝜕𝜙̇ − 𝜕𝜙 = 𝐹𝜙 𝜕𝐿 𝜕𝐿 𝜕𝐿 (18) (19) Derivative L according to the variables Obtain dynamic equations describing robot motion: [(2𝑚 + 𝑀)𝑅 + 2𝐽𝑤 + 2𝑛2 𝐽𝑚 ]𝜃̈ + (𝑀𝐿𝑅 cos Ψ − 2𝑛2 𝐽𝑚)Ψ̈ − 𝑀𝐿𝑅Ψ̇ sin Ψ = 𝛼(𝑣𝑙 + 𝑣𝑟) − 2(𝛽 + 𝑓𝑤)𝜃̇ + 2𝛽Ψ̇ (20) (𝑀𝐿𝑅 cos Ψ − 2𝑛2 𝐽𝑚)𝜃̈ + (𝑀𝐿2 + 𝐽Ψ + 2𝑛2 𝐽𝑚)Ψ̈ − 𝑀𝑔𝐿 sin Ψ − 𝑀𝐿2 Φ2 sin Ψ cos Ψ = −𝛼(𝑣𝑙 + 𝑣𝑟) + 2𝛽𝜃̇ − 2𝛽Ψ̇ (21) 𝑊2 [2 𝑚𝑊 + 𝐽Φ + 2𝑅2 (𝐽𝑤 + 𝑛2 𝐽𝑚) + 𝑀𝐿2 sin Ψ2 ] Φ̈2 + 2𝑀𝐿2 Ψ̇Φ̇ sin Ψ cos Ψ = 𝑊 𝑊 (𝛽 + 𝑓𝑤)Φ̇ 𝛼(𝑣𝑟 − 𝑣𝑙) − 2𝑅 2𝑅 III (22) Control Design 3.1 LQR controller The Linear Quadratic Regulator (LQR) is a well-known method that provides optimally controlled feedback gains to enable the closed-loop stable and high performance design of systems Plant must be linearized about the origin to obtain the optimal control, linear time system to form a continuous time system From dynamic equations, we have equations describing robot motion: 𝑥1̇=𝑥2 𝑥2̇=𝑓1(𝑥1,𝑥2,𝑥3,𝑥4,𝑥5,𝑥6,𝑣𝑟,𝑣𝑙) 𝑥4̇=𝑥5 𝑥5̇=𝑓2(𝑥1,𝑥2,𝑥3,𝑥4,𝑥5,𝑥6,𝑣𝑟,𝑣𝑙) 𝑥7̇=𝑥8 ̇ {𝑥8=𝑓3(𝑥1,𝑥2,𝑥3,𝑥4,𝑥5,𝑥6,𝑣𝑟,𝑣𝑙) (23) With: 𝑥1 = 𝜃, 𝑥2 = 𝜃̇, 𝑥3 = 𝜃̈, 𝑥4 = Ψ, x5 = Ψ̇, 𝑥6 = Ψ̈, 𝑥7 = Φ, x8 = Φ̇, 𝑥9 = Φ̈ (24) Plant must be linearized about the origin to obtain the optimal control, choose balance control point: 𝑥0 = [0 0 0 ]𝑇 and 𝑢0 = [0 0]𝑇 Linearization of the system, the state space model: 𝑥̇ = 𝐴𝑥 + 𝐵𝑢 (25) (26) A , B is the matrix of states 𝐴= 0 0 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 𝜕𝑥1 𝜕𝑥2 𝜕𝑥4 𝜕𝑥5 𝜕𝑥7 𝜕𝑥8 0 0 𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 𝜕𝑓4 𝜕𝑥1 𝜕𝑥2 𝜕𝑥4 𝜕𝑥5 𝜕𝑥7 𝜕𝑥8 0 0 𝜕𝑓6 𝜕𝑓6 𝜕𝑓6 𝜕𝑓6 𝜕𝑓6 𝜕𝑓6 𝜕𝑥2 𝜕𝑥4 𝜕𝑥5 𝜕𝑥7 [𝜕𝑥1 (27) 𝜕𝑥8 ] 𝐵= 0 𝜕𝑓2 𝜕𝑓2 𝜕𝑣𝑙 𝜕𝑣𝑟 0 𝜕𝑓4 𝜕𝑓4 𝜕𝑣𝑙 𝜕𝑣𝑟 0 𝜕𝑓6 𝜕𝑓6 [𝜕𝑣𝑙 (28) 𝜕𝑥1 ] With 𝑥 = 𝑥0 , 𝑢 = 𝑢0 ‘x’ is the all state of plant 𝑇 𝑥 = [𝜃 𝜃̇ Ψ Ψ̇ Φ Φ̇ ] (29) ‘u’ is the control signal of LQR controller 𝑣𝑙 𝑢 = [𝑣 ] 𝑟 (30) State feedback law gives the cost function for optimal control approach ∞ J = ∫0 (x T Qx + U T RU)dt → minimal (31) Here: Q is symmetric positive semi definite matrix 𝑄1 0 0 0 𝑄2 0 0 𝑄 𝑄= 0 30 0 0 𝑄4 0 0 𝑄5 [ 0 0 𝑄6 ] (32) R is symmetric positive definite matrix 𝑅=[ 𝑅1 0 ] 𝑅2 (33) Optimal control input is given as U = - Kx (34) Where ‘K’ is state feedback gain Parameters 𝑄1 , 𝑄2 , 𝑄3 , 𝑄4 , 𝑄5 , 𝑄6 are optimal parameters for each state 𝜃 𝜃̇ Ψ Ψ̇ Φ Φ̇ , depending on the stability priority of which state we will adjust the corresponding parameter on the larger Q matrix, 𝑅1 , 𝑅2 are parameters for two motor, 𝑅1 and 𝑅2 must be equal The K feedback gain matrix is constructed so that the energy consumed to pull the system in steady state is minimal when the system is knocked out of steady state by noise To find the matrix K we replace the values into the Riccati equation or we can find K through the lqr(A, B, Q, R) command supported by MATLAB 3.2 Simulation 3.2.1 Linearization Based on the mathematical model built in 2.2 we conduct object simulation on MATLAB/Simulink Object is robot with input signal PWM voltage to motor and target output is robot states using Fcn function in MATLAB to describe the dynamic model of 2-wheels balancing robot Figure Mathematical model of object in MATLAB/Simulink With system parameter in TABLE 1, the matrix of state A, B: 0 0 −3.7875 30.8417 0.0059 0 0 0 𝐴= −3.7711 30.8613 −0.0016 0 0 0 [0 0 0 −0.0188] 0 0.0062 0.0062 0 𝐵= −0.0017 −0.0017 0 [−0.0053 0.0053 ] 3.2.2 Define K control matrix Choose matrix Q and R: 10 𝑄= 0 [0 0 0 0 0 0 0 0 0 0 0 0 ; 0 1] 𝑅=[ ] Find matrix K using command K=lqr(A,B,Q,R) in MATLAB −0.7 415.1 −3483.7 −864.8 −0.7 −9.9 𝐾=[ ] −0.7 415.1 −3483.7 −864.8 0.7 9.9 Figure Closed loop model of LQR controller in MATLAB/Simulink IV Result 4.1.The reference value is set to The system was initially unstable, but was immediately pulled back to the set state by the LQR controller Steady state error 𝑒∞ ≈ Over shoot 𝑃𝑂𝑇 = 0.521% Rise time 𝑡𝑟 = 6.140𝑠 Settling time 𝑡𝑥𝑙 = 47.498𝑠 11 Figure The medium angle of the left and right wheels Steady state error 𝑒∞ ≈ Over shoot 𝑃𝑂𝑇 = 0.0153% Settling time 𝑡𝑥𝑙 = 28.363𝑠 Figure The inclined angle of robot’s body 12 Steady state error 𝑒∞ ≈ Over shoot 𝑃𝑂𝑇 = 1.732% Settling time 𝑡𝑥𝑙 = 30 Figure The robot's rotation angle in the xy plane 4.2.The reference value is set 𝜽 = 𝝅 𝟐 and 𝝓 = 𝝅 𝟐 The robot moves a distance of half the circumference of the wheel and the robot rotates 90 degrees around its axis Steady state error 𝑒∞ ≈ Over shoot 𝑃𝑂𝑇 = 3.646% Rise time 𝑡𝑟 = 20.527𝑠 Settling time 𝑡𝑥𝑙 = 29.491𝑠 13 Figure The medium angle of the left and right wheels Steady state error 𝑒∞ ≈ Over shoot 𝑃𝑂𝑇 = 1.892% Settling time 𝑡𝑥𝑙 = 32.451𝑠 Figure The inclined angle of robot’s body Steady state error 𝑒∞ ≈ 14 Over shoot 𝑃𝑂𝑇 = 3.958% Rise time 𝑡𝑟 = 25.232𝑠 Settling time 𝑡𝑥𝑙 = 36.493𝑠 Figure The robot's rotation angle in the xy plane 4.3.When the system has disturbance (noise power 0.001) The system is still sticking to the reference value but the effect of high frequency interference on the medium angle of the left and right wheels is relatively large steady state error 𝑒∞ ≈ 0.02 15 Figure 10 The medium angle of the left and right wheels Noise impacted on the inclined angle of robot’s body is relatively small, steady state error 𝑒∞ ≈ Figure 11 The inclined angle of robot’s body Almost no effect of noise on the robot's rotation angle in the xy plane so, parameter chosen for ∅ angle is optimal Figure 12 The robot's rotation angle in the xy plane 16 Because of the maximum effect of noise on 𝜃 angle, Q matrix should be change 20 0 0 0 0 0 𝑄= 0 0 0 0 0 0 0 [ 0 0 1] −3.2 421.5 −3629.2 −890.3 −0.7 −9.9 𝐾=[ ] −3.2 421.5 −3629.2 −890.3 0.7 9.9 The effect of strongly reduced interference shows that 𝜃 angle is very easily unstable by harmonic, 𝜃 angle should be controlled priority over Ψ and ∅ Figure 13 The medium angle of the left and right wheels when change Q matrix 17 Figure 14 The inclined angle of robot’s body when change Q matrix Figure 15 The robot's rotation angle in the xy plane when change Q matrix 4.4.Discussion The LQR controller responds well, the robot always adheres to the set signal even if it is affected by system noise However, the settling time is long and overshoot is still large V Conclusion 18 From the construction of the mathematical description of the 2-wheels self balance model, the paper proves that the self-balancing vehicle is the control nonlinear object Linearization around balance operation point, robot model is unstable system in vertical position Using the LQR optimal controller allows stabilizing the rotation angle of the vehicle body and the robot's rotation angle on the yz/xz and xy coordinate systems Simulation results show that the system is kept in balance after using the LQR controller However, the settling times are long and susceptible to measurement disturbance To overcome this shortcoming, the development direction of the topic is using GA (Genetic Algorithms) to find the matrix K and the Kalman filter algorithm to filter high harmonic References [1] Book Nguyen Doan Phuoc, Book Title: Lý thuyết điều khiển tự động, 150−60−4/2/2005, NXB Văn hóa dân tộc, Hà Nội, Việt Nam, 7/2005 [2] Book Huỳnh Thái Hồng, Nguyễn Thị Phương Hà, Mơ tả hệ rời rạc dùng MATLAB In Lý thuyêt điều khiển tuyến tính, 2005, NXB ĐHQGTPHCM, Hồ Chí Minh, Viet Nam, pp 272, 2005 [3] Conference Paper and Symposium Conference Paper in Print Proceedings Nguyễn Minh Tâm, Lê Văn Tuấn, Nguyễn Văn Đông Hải, Modelling and Optimal Control for Two-wheeled self-balancing robot, journal for research, ResearchGate, TPHCM, Việt Nam, December 2015 Online (Nguyễn Văn Đông Hải) Nguyễn Văn Đông Hải, Hướng dẫn điều khiển LQR cho hệ xe hai bánh tự cân bằng, 12/01/2016 Retrieved from https://www.youtube.com/watch?v=eeaqPCHMAXg&list=LL&index=3&t=3s 19 ...