├── FDL_Trajectary_Tracking_SBR.slx ├── JitendersirSBR.wrl ├── LICENSE ├── README.md ├── ReadME.txt ├── linear_SBR6voltage_A.m ├── linear_SBR6voltage_B.m ├── linear_SBRvoltage_A.m ├── linear_SBRvoltage_B.m └── lqr_control_3DOF.m /FDL_Trajectary_Tracking_SBR.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jitendra825/Self-Balancing-Robot-Simulation/e2d5baac49330d786c8b45c79f72fa5bf52bbb76/FDL_Trajectary_Tracking_SBR.slx -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Jitendra Singh 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Self-Balancing-Robot-Simulation 2 | ![Self_Balancing_Robot](https://user-images.githubusercontent.com/88198964/128762117-d227a156-91b4-4186-8ab5-f131df760192.gif) 3 | ![SBR_8-Shape](https://user-images.githubusercontent.com/88198964/127717390-5d413aef-fe3b-4031-b74a-a5e53549d86b.gif) 4 | 5 | This repository contains simulation for Self Balancing Robot. LQR Control for Balancing the robot in vertical position. Feedback Linearization control for Trajectory Tracking 6 | Steps for Run the Simulation 7 | 1. firstly, Run lqr_control_3DOF.m file in MATLAB to calculate gains & initilize the parameters of system 8 | 2. To simulate Feedback linearization control for trajectory tracking of Self balancing Control open FDL_Trajectary_Tracking_SBR.slx in Simulink 9 | 10 | ![tempsnip](https://user-images.githubusercontent.com/88198964/127717279-8748becb-79af-4e5f-a763-a157682965a1.png) 11 | 12 | 3. To change the Trajectory types Click on block showing in above Image. 13 | 4. Type Number between 1 to 4 such as 14 | 15 | 1 for Circular Trajecctory 16 | 17 | 2 for 8-Shaped Trajecotory 18 | 19 | 3 for Infinite Shaped Trajectory 20 | 21 | 4 for Heart Shaped Trajectory (Just for Fun/ this is not a Continious trajectory) 22 | 23 | 5. Run the simulation by click on RUN Icon in Simulink 24 | 6. To Visualizing the 3D Animation,Double click on VR Sink block and run the simulation in VRML 25 | 26 | Video: https://www.youtube.com/watch?v=gvfqVTKq9_w 27 | -------------------------------------------------------------------------------- /ReadME.txt: -------------------------------------------------------------------------------- 1 | # Self-Balancing-Robot-Simulation 2 | This repository contains simulation for Self Balancing Robot. LQR Control for Balancing the robot in vertical position. Feedback Linearization control for Trajectory Tracking 3 | Steps for Run the Simulation 4 | 1. firstly, Run lqr_control_3DOF.m file in MATLAB to calculate gains & initilize the parameters of system 5 | 2. To simulate Feedback linearization control for trajectory tracking of Self balancing Control open FDL_Trajectary_Tracking_SBR.slx in Simulink 6 | 3. To change the Trajectory types Click on block showing in above Image. 7 | 4. Type Number between 1 to 4 such as 8 | 9 | 1 for Circular Trajecctory 10 | 2 for 8-Shaped Trajecotory 11 | 3 for Infinite Shaped Trajectory 12 | 4 for Heart Shaped Trajectory (Just for Fun/ this is not a Continious trajectory) 13 | 5. Run the simulation by click on RUN Icon in Simulink 14 | 6. To Visualizing the 3D Animation,Double click on VR Sink block and run the simulation in VRML -------------------------------------------------------------------------------- /linear_SBR6voltage_A.m: -------------------------------------------------------------------------------- 1 | function A_handle = linear_SBR6voltage_A(I_W_a,I_y,R_m,g,k_e,k_m,l,m_B,m_W,r) 2 | %LINEAR_SBR6VOLTAGE_A 3 | % A_HANDLE = LINEAR_SBR6VOLTAGE_A(I_W_A,I_Y,R_M,G,K_E,K_M,L,M_B,M_W,R) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 10-May-2020 07:48:55 7 | 8 | t2 = l.^2; 9 | t3 = r.^2; 10 | t4 = I_W_a.*I_y.*2.0; 11 | t5 = I_W_a.*m_B.*t2.*2.0; 12 | t6 = I_y.*m_B.*t3; 13 | t7 = I_y.*m_W.*t3.*2.0; 14 | t8 = m_B.*m_W.*t2.*t3.*2.0; 15 | t9 = t4+t5+t6+t7+t8; 16 | t10 = 1.0./t9; 17 | t11 = 1.0./R_m; 18 | t12 = I_W_a.*2.0; 19 | t13 = m_B.*t3; 20 | t14 = m_W.*t3.*2.0; 21 | t15 = l.*m_B.*r; 22 | A_handle = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-g.*m_B.^2.*t2.*t3.*t10,g.*l.*m_B.*t10.*(t12+t13+t14),0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,k_e.*k_m.*t10.*t11.*(I_y+t15+m_B.*t2).*-2.0,(k_e.*k_m.*t10.*t11.*(t12+t13+t14+t15).*2.0)./r,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0],[6,6]); 23 | -------------------------------------------------------------------------------- /linear_SBR6voltage_B.m: -------------------------------------------------------------------------------- 1 | function B_handle = linear_SBR6voltage_B(I_W_a,I_W_d,I_y,I_z,R_m,d,k_m,l,m_B,m_W,r) 2 | %LINEAR_SBR6VOLTAGE_B 3 | % B_HANDLE = LINEAR_SBR6VOLTAGE_B(I_W_A,I_W_D,I_Y,I_Z,R_M,D,K_M,L,M_B,M_W,R) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 10-May-2020 07:48:56 7 | 8 | t2 = l.^2; 9 | t3 = r.^2; 10 | t4 = 1.0./R_m; 11 | t5 = m_B.*t2; 12 | t6 = l.*m_B.*r; 13 | t7 = I_y+t5+t6; 14 | t8 = I_W_a.*I_y.*2.0; 15 | t9 = I_W_a.*m_B.*t2.*2.0; 16 | t10 = I_y.*m_B.*t3; 17 | t11 = I_y.*m_W.*t3.*2.0; 18 | t12 = m_B.*m_W.*t2.*t3.*2.0; 19 | t13 = t8+t9+t10+t11+t12; 20 | t14 = 1.0./t13; 21 | t15 = k_m.*r.*t4.*t7.*t14; 22 | t16 = I_W_a.*2.0; 23 | t17 = m_B.*t3; 24 | t18 = m_W.*t3.*2.0; 25 | t19 = t6+t16+t17+t18; 26 | t20 = d.^2; 27 | t21 = I_W_a.*t20; 28 | t22 = I_W_d.*t3.*4.0; 29 | t23 = I_z.*t3.*2.0; 30 | t24 = m_W.*t3.*t20; 31 | t25 = t21+t22+t23+t24; 32 | t26 = 1.0./t25; 33 | t27 = d.*k_m.*r.*t4.*t26; 34 | B_handle = reshape([0.0,0.0,0.0,t15,-k_m.*t4.*t14.*t19,t27,0.0,0.0,0.0,t15,-k_m.*t4.*t14.*t19,-t27],[6,2]); 35 | -------------------------------------------------------------------------------- /linear_SBRvoltage_A.m: -------------------------------------------------------------------------------- 1 | function AA_handle = linear_SBRvoltage_A(I_W_a,I_W_d,I_y,I_z,R_m,d,g,k_e,k_m,l,m_B,m_W,n,r) 2 | %LINEAR_SBRVOLTAGE_A 3 | % AA_HANDLE = LINEAR_SBRVOLTAGE_A(I_W_A,I_W_D,I_Y,I_Z,R_M,D,G,K_E,K_M,L,M_B,M_W,N,R) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 27-May-2020 14:52:00 7 | 8 | t2 = l.^2; 9 | t3 = r.^2; 10 | t4 = I_W_a.*I_y.*2.0; 11 | t5 = I_W_a.*m_B.*t2.*2.0; 12 | t6 = I_y.*m_B.*t3; 13 | t7 = I_y.*m_W.*t3.*2.0; 14 | t8 = m_B.*m_W.*t2.*t3.*2.0; 15 | t9 = t4+t5+t6+t7+t8; 16 | t10 = 1.0./t9; 17 | t11 = 1.0./R_m; 18 | t12 = n.^2; 19 | t13 = I_W_a.*2.0; 20 | t14 = m_B.*t3; 21 | t15 = m_W.*t3.*2.0; 22 | t16 = l.*m_B.*r; 23 | t17 = d.^2; 24 | AA_handle = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-g.*m_B.^2.*t2.*t3.*t10,g.*l.*m_B.*t10.*(t13+t14+t15),0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,k_e.*k_m.*t10.*t11.*t12.*(I_y+t16+m_B.*t2).*-2.0,(k_e.*k_m.*t10.*t11.*t12.*(t13+t14+t15+t16).*2.0)./r,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,-(k_e.*k_m.*t11.*t12.*t17)./(I_W_d.*t3.*4.0+I_W_a.*t17+I_z.*t3.*2.0+m_W.*t3.*t17)],[6,6]); 25 | -------------------------------------------------------------------------------- /linear_SBRvoltage_B.m: -------------------------------------------------------------------------------- 1 | function BB_handle = linear_SBRvoltage_B(I_W_a,I_W_d,I_y,I_z,R_m,d,k_m,l,m_B,m_W,n,r) 2 | %LINEAR_SBRVOLTAGE_B 3 | % BB_HANDLE = LINEAR_SBRVOLTAGE_B(I_W_A,I_W_D,I_Y,I_Z,R_M,D,K_M,L,M_B,M_W,N,R) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 8.1. 6 | % 27-May-2020 14:52:01 7 | 8 | t2 = l.^2; 9 | t3 = r.^2; 10 | t4 = 1.0./R_m; 11 | t5 = m_B.*t2; 12 | t6 = l.*m_B.*r; 13 | t7 = I_y+t5+t6; 14 | t8 = I_W_a.*I_y.*2.0; 15 | t9 = I_W_a.*m_B.*t2.*2.0; 16 | t10 = I_y.*m_B.*t3; 17 | t11 = I_y.*m_W.*t3.*2.0; 18 | t12 = m_B.*m_W.*t2.*t3.*2.0; 19 | t13 = t8+t9+t10+t11+t12; 20 | t14 = 1.0./t13; 21 | t15 = k_m.*n.*r.*t4.*t7.*t14; 22 | t16 = I_W_a.*2.0; 23 | t17 = m_B.*t3; 24 | t18 = m_W.*t3.*2.0; 25 | t19 = t6+t16+t17+t18; 26 | t20 = d.^2; 27 | t21 = I_W_a.*t20; 28 | t22 = I_W_d.*t3.*4.0; 29 | t23 = I_z.*t3.*2.0; 30 | t24 = m_W.*t3.*t20; 31 | t25 = t21+t22+t23+t24; 32 | t26 = 1.0./t25; 33 | t27 = d.*k_m.*n.*r.*t4.*t26; 34 | BB_handle = reshape([0.0,0.0,0.0,t15,-k_m.*n.*t4.*t14.*t19,t27,0.0,0.0,0.0,t15,-k_m.*n.*t4.*t14.*t19,-t27],[6,2]); 35 | -------------------------------------------------------------------------------- /lqr_control_3DOF.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all 3 | clc 4 | format short g % Stop exponential answers 5 | 6 | %% Parameters 7 | m_B = 1.15; % Mass of pendulum body 8 | m_W = 0.053; % Mass of wheel 9 | l = 0.045; % COM length of pendulum 10 | r = 0.0425; % Radius of wheel 11 | d = 0.23; % Distance between wheels 12 | I_W_a = 0.0000365; % MOI of wheel wrt its rotating axis 13 | I_W_d = 0.00001825; % MOI of wheel wrt its radial (diameter) 14 | I_y = 0.00429462; % MOI of pendulum body wrt y-axis %I_2 = 0.006548; 15 | I_z = 0.003350; % MOI of pendulum body wrt z-axis 16 | I_x = 0.007402; 17 | C_L = 0; 18 | g = 9.81; % Gravitational Constant 19 | k_m = 0.63; % Motor Torque Constant 20 | k_e = 0.63; % Motor Back Emf Constant 21 | R_m = 1.25; % Motor Resistance 22 | n = 1; 23 | 24 | %% computing all matrix 25 | 26 | A_v = linear_SBR6voltage_A(I_W_a,I_y,R_m,g,k_e,k_m,l,m_B,m_W,r) 27 | B_v = linear_SBR6voltage_B(I_W_a,I_W_d,I_y,I_z,R_m,d,k_m,l,m_B,m_W,r) 28 | 29 | A_test = linear_SBRvoltage_A(I_W_a,I_W_d,I_y,I_z,R_m,d,g,k_e,k_m,l,m_B,m_W,n,r) 30 | B_test = linear_SBRvoltage_B(I_W_a,I_W_d,I_y,I_z,R_m,d,k_m,l,m_B,m_W,n,r) 31 | 32 | C = eye(6); 33 | D = zeros(6,2); 34 | R = diag([0.01 0.01]); % Input Weight Matrix 35 | Q = diag([500 20000 1000 2000 1 0]); % State Weight Matrix 36 | 37 | % Controllability Test 38 | ra = rank(ctrb(A_v,B_v)); 39 | ra_test = rank(ctrb(A_test,B_test)); 40 | %% Continious time control design 41 | K_v = lqr(A_v,B_v,Q,R) % LQR control design 42 | K_test = lqr(A_test,B_test,Q,R) 43 | %% Discrete time control design 44 | sys_c = ss(A_v,B_v,C,D); 45 | sys_test = ss(A_test,B_test,C,D) 46 | Ts = 0.01; 47 | sys_d = c2d(sys_c,Ts,'zoh'); 48 | sys_dtest = c2d(sys_test,Ts,'zoh'); 49 | a = sys_d.A; 50 | b = sys_d.B; 51 | c = sys_d.C; 52 | dd = sys_d.D; 53 | 54 | a_test = sys_dtest.A; 55 | b_test = sys_dtest.B; 56 | c_test = sys_dtest.C; 57 | dd_test = sys_dtest.D; 58 | K_dis = dlqr(a,b,Q,R) % Discrete LQR control design 59 | 60 | K_dis_test = dlqr(a_test,b_test,Q,R) % Discrete LQR control design 61 | 62 | %cl = feedback(sys_c,K_v); 63 | %cl_d = feedback(sys_d,K_dis); 64 | % step(cl,'r') --------------------------------------------------------------------------------