DEPARTMENT OF ELECTRICAL ENGINEERING UNIVERSITY OF MINNESOTA EE 4237 State Space Control Laboratory Experiment 10: Balancing robot using LEGO Mindstorms NXT Objective: 1. To balance a small LEGO robot using a gyroscope sensor 2. To study the dynamics of the inverted pendulum Apparatus: 1. LEGO robot 2. PC 3. RobotC software Prelab Report: 1. What is an inverted pendulum? 2. What is the application area of a self-balancing robot? Postlab Report: • Answer the questions asked within or at the end of the procedure.21. Modeling of Balancing Robot This section provides a modeling of two-wheeled balancing robot. First, the equations of motion for the wheels are obtained. The free body diagram of the robot is shown below. The lists of used symbols are shown in Appendix A. According to Newton’s law of motion, the sum of forces on the horizontal x direction gives ∑= MaFx HHxMf−=&&ω (1) Since there is no acceleration in the y direction, the sum of Moments around the center of the wheel gives ∑= IaM0 rHCIf−=ωωθ&& (2) From the DC motor dynamics, the output torque to the wheels can be obtained ammmemVRkRkkdtdIC +−==ωθω& (3) Thus, plugging equation (3) into (2), PHθωMωxCHf Fig. 1: The free body diagram of wheeled balancing robot3ωωθθ&&&rIVrRkrRkkHwammmemf−+−= (4) Substituting equation (4) into (1) to get the equation for the wheels, HrIVrRkrRkkxMwammmem−−+−=ωωωθθ&&&&& (5) The angular rotation can be transformed into linear motion by simple transformation, xrxr&&&&&&==ωωθθ It yields, HVrRkxrRkkxrIMammmemw−+−=⎟⎠⎞⎜⎝⎛+&&&22ω (6) And next, the robot chassis can be modeled as an inverted pendulum using Newton’s law of motion. The free body diagram of the chassis is shown below. The sum of forces in the horizontal direction: ]sincos[2θθθθ∑+−==&&&&&lMlMHxMFpppx (7) x&&∑xpFθ&&l2θ& Fig. 2: The free body diagram of the chassis4The sum of forces perpendicular to the pendulum: θθθθθ&&&&lMgMPHxMFpppxp−−+==∑sinsincoscos (8) The sum of moment around the center of mass of pendulum: θθθ&&pIlPlH =−− sincos (9) Combining equation (8) and (9) the final equation is shown as below θθθθcossin2xlMlMglMIpppp&&&&&&−=++ (10) Using equation (6) and (7), θθθθsincos222&&&&&&&&lMlMxMVrRkxrRkkxrIMpppammmemww+−−+−=⎟⎠⎞⎜⎝⎛+ (11) Rearranging these equations (10) and (11), θθθcossin)(2xlMglMlMIpppp&&&&−=++ (12) ammppmempwwVrRklMlMxrRkkxMrIM =−++⎟⎠⎞⎜⎝⎛++θθθθsincos222&&&&&& (13) Linearized by assuming φπθ+=, where φ represents a small angle from the vertical upward direction. Therefore, φθθ−=−= sin,1cos and 02=⎟⎠⎞⎜⎝⎛dtdθ The linearized equation of motion is φφ)()(22lMIglMxlMIlMpppppp+++=&&&& (14) apwwmmpwwmempwwpVMrIMrRkxMrIMrRkkMrIMlMx⎟⎠⎞⎜⎝⎛+++⎟⎠⎞⎜⎝⎛++−+⎟⎠⎞⎜⎝⎛++=2222&&&&&φ (15) The continuous state space equation is obtained as: ()()aeqmmpeqmmppeqpwwpeqmempeqpeqmemppVrQRlkMrQRklMIxxQMrIMglMQrRkklMQglMQrRkklMIxx⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎣⎡++⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡⎟⎠⎞⎜⎝⎛++−+−=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡000010000000102222222φφφφ&&&&&&&& (16)5aVxxy⎥⎦⎤⎢⎣⎡+⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎦⎤⎢⎣⎡=0001000001φφ&& (17) where, ()⎥⎦⎤⎢⎣⎡++⎟⎠⎞⎜⎝⎛+=ppppwweqIMlMIrIMQ22 2. Experiment with robot Two different experiments are to be performed in this section. First experiment is to control the self–balancing robot while maintaining its position. The following measurements are used to control the robot; angular velocity, angle, position, and velocity of the robot. Angular velocity is obtained from gyroscopic sensor and the tilt can be calculated by numerical integration. Position of the robot is measured by encoder of the motor and velocity of robot can be computed by numerical differentiation. All of these four measurements are being used to determine driving power for the two motors. Second experiment is to make the robot navigate while avoiding obstacles. The Robot is supposed to move forward with constant speed and, when it meets obstacle, it is supposed to turn to the right. Ultrasonic sensors are used in this application to measure the distance between the robot and the obstacle. Here is the functional block diagram of the software flow for the robot. φφ&xx&dtd Fig. 3: Functional block diagram of wheeled balancing robot6Questions / Exercises: A. Plot the angle fluctuations in the first experiment of stationary self-balancing and measure the position trajectory in the second experiment of navigation with obstacle avoidance. B. Develop MATLAB code to simulate the step response of stationary self-balancing robot given the state equation model. C. Change the source code to do the following jobs. z In navigation mode, modify the code so that it turns to left when it meets an obstacle. z Navigate through rectangular/rectilinear trajectory.7Appendix A. List of symbols x : Displacement x& : Displacement velocity φ : Angle φ& : Angular velocity θ : Tilted angle of the chassis wθ : Rotation angle of the wheel r : Radius of wheel aV : Applied torque mk : Torque constant ek : Back emf constant mR : Motor resistance P : Reaction force between the wheel and the chassis of y-component of the force H : Reaction force between the wheel and the chassis of x-component fH : Friction force between the ground and the wheel C : Output torque wM : Mass of the wheel pM : Mass of the robot’s chassis g : Gravitational acceleration, 9.81 m/s2 l : Distance between the center of the wheel and the robot’s center of gravity pI : Inertia of the robot’s chassis wI : Inertia of the wheel8Appendix B. Source code for stationary self-balancing robot // two-wheeled balancing robot // k1, k2, k3, k4 feedback gains are specified below const tSensors GyroSensor = (tSensors) S1; //gyro sensor// #define GyroScale 4 #define half_h 2 // Increment used in Runge-Kutta integration #define t_scale 500 task main () { float a=0.2, aa=0.001; // aa is
View Full Document