BoRam: Balancing Robot using Arduino and Lego

Kyungjae Baik (9193618) September 10, 2008

Contents
1 Introduction 2 BoRam Description 3 System Modeling
3.1 Actuator Assembly Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Body Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.3 System Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Arduino . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 H-Bridge (Motor Controller) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3 3 4
4 5 6 7 7

4 Microcontroller 5 Sensors

7 8

5.1 IMU Setup with Kalman Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 5.2 Coupling between IMU and Optical Encoder . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 5.3 Kalman Filtering Code Example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 6.1 Identication of Eective Moment of Inertia . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 6.2 Identication of Torque Constant, Choice of PWM Frequency . . . . . . . . . . . . . . . . . . 13

6 Actuator

11 15

7 Digital Mock-Up

7.1 Parametric Design Technique . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 7.2 Draft . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 7.3 Applying Physical Properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 8.1 Digital Controller for Motor Position . . . . . . . . . . . . 8.1.1 Digital Control - Proportional . . . . . . . . . . . . 8.1.2 Digital Control - Root Locus . . . . . . . . . . . . 8.2 LQR Controller for Balancing . . . . . . . . . . . . . . . . 8.2.1 Discrete State-Space . . . . . . . . . . . . . . . . . 8.2.2 Controllability . . . . . . . . . . . . . . . . . . . . 8.2.3 Digital LQR Full-State Feedback Controller design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

8 Controller Design

18

18 18 18 19 19 20 20

9 Simulation & VRML 10 Results and Conclusions 11 Reference A Nomenclature B Arduino Conguration C H-Bridge(L293D) Conguration D Physical Properties of BoRam

21 22 24 24 25 26 26

The design of a low-cost balancing robot is accomplished by using a popular development platform Arduino for the controller and Lego for the construction material. The balancing robot is intended to improve the learning experience of students or hobbyists who are interested in control theory by presenting hands-on experiments and controller design examples that anyone would be able to follow easily and costeectively.

1 Introduction
This paper describes a two-wheel balancing robot which was designed for a project course ENGR6971 under supervision of Prof. Luis Rodrigues. The kit is based on a popular open-architecture development board Arduino. Arduino uses an ATmega168 chip from Atmel Corporation that runs on a clock speed of 16MHz with 1KB of SRAM and 16KB of Flash memory. The overall cost of the total kit excluding the Lego parts sums around $200 which is well below the cost of similar commercially available educational control kits. The robot is named BoRam in short of Balancing Robot using Arduino and Lego plus some extra letters. The procedure used for system modeling, inertial measurements fusion technique, motor identication, and controller design/implementation are described in the following chapters.

2 BoRam Description

Figure 1: Robot Description BoRam is composed of two stages as illustrated in Figure 1. The bottom stage consists of motors, wheels, and two pairs of spur gears that connect the motors and the wheels. The spur gear was added because the motor shaft was not long enough to support the distance required by the optical encoder. The optical encoder requires some space between the striped disk and the optical sensors to read black and white. The top stage consists of a microcontroller and an H-bridge. The microcontroller is the brain of the robot and is responsible to sample and process all the data collected by the sensors. The H-bridge is used with the 3

Part Microcontroller H-bridge DC Motor Encoder Inertial Measurement Unit

Brand Arduino Diecimila LD293 chip LEGO Standard Part # 71427 Nubotics Optical Encoder Sparkfun IMU Combo Board - ADXL203/ADXRS300 Table 1: Part list

Quantity 1 1 2 1 1

microcontroller to control the directions of the motors. There is one more sensor attached to a vertical strut between the two stages. In general it is called a Inertial Measurement Unit, IMU, and this one contains two accelerometers and one gyro to determine the robot's attitude in one angular direction. Table 1 describes which part in particular was used in this project.

3 System Modeling

Figure 2: Reference Frame To make the project simple and manageable, only a two-dimensional movement in which BoRam moves in one plane will be considered. The two motors are assumed to move together symmetrically when an identical input is given. Although this is unlikely the case, it is a good approximation for motors which are manufactured as the same product. A control voltage is applied to the motors, which results in displacement of the wheels and change in angular attitude of the body. A mathematical model for the system is obtained by observing the FBDs in Figure 2. The rst diagram in Figure 2 indicates the inertial frame. All the equations will be derived in this inertial frame. In the second and third diagram, there are a few variables worthy of clarication. h is the friction force between each wheel and surface. H , V are the horizontal and vertical forces acting among the body and the motor shaft respectively. l is the length from the motor shaft to the CG of the Body, and nally is the torque produced by the DC motor.

3.1 Actuator Assembly Dynamics

The observation of the modeling process will start from the third diagram. There are two fundamental equations describing the characteristics of a DC motor. The rst one relates torque on the shaft proportional to the armature current i and the second expresses the back emf voltage e proportional to the shaft's rotational velocity enc , where enc is the velocity measured by the encoder mounted on the same frame as the DC motor. This velocity represents the shaft velocity with respect to the DC motor itself.
= Ki

(1)

e = Ke enc

(2)

The proportionality constant K is called the torque constant, and Ke is called back emf constant. Assume an idealized energy transducer having no power loss in converting electric power into mechanical power
P = ei = enc

(3) (4)

Substituting Equation (1) into (3) and dividing both sides by i yields
e = Kenc Ke is identical to K .

Thus, by comparing Equations (4) and (2) one can conclude that, assuming idealized energy conversion,

BoRam is powered by two Lego 71427 DC motors. Figure 2, third diagram illustrates an electrical circuit in the actuator assembly where Kirchho's voltage law is applied.
v e Ri L di =0 dt

(5)

In deriving the mathematical expression for the motor, shaft viscous friction and inductance are considered negligible to simplify the model. The relative eect of the inductance is negligible compared with the mechanical motion and can be neglected.
v e Ri = 0

(6)

Applying Newton's law in the third diagram of Figure 2 to the horizontal motion of the wheel, and then to the angular motion induced by two torques yields
m = h H x hr = J

(7) (8)

It is assumed that no slip occurs between the wheels and the surface, so the following relationship must hold.
x r x = r =

(9) (10)

Note that is the angular velocity of the wheel in the inertial frame and it is dierent from enc that was used in the DC motor identication section. This will be explained in Section 5.2 Coupling between IMU and optical encoder. It is important to know this relationship especially when it comes to implementing the microcontroller.

3.2 Body Dynamics

The second diagram in Figure 2 illustrates the body dynamics. The rotational motion of the pendulum body about its center of gravity can be described by
I = 2 + 2Hl cos + 2V l sin

(11)

where I is the moment of inertia of the pendulum body about its center of gravity. The horizontal force, vertical force and torque are applied as pairs on both sides and each pair is assumed to be equal because only two-dimensional motion was considered. The horizontal motion is given by 5

2H = M

d2 (x + l sin ) dt2 d2 (l cos ) dt2

(12)

The vertical motion is given by

2V + M g = M

(13)

Since BoRam must be kept in a vertical attitude, can be assumed to be very small such that sin , cos = 1. Then, Equations (11) through (13) can be linearized. The linearized equations are
I = 2 2Hl + 2V l 2H = M ( + l) x 2V = M g

(14) (15) (16)

3.3 System Dynamics

The objective here is to nd suitable ordinary dierential equations that fully describe the system in terms of input v , the states x, and their derivatives x, . The following describes how to characterize the essence of the system in two dierential equations. With Equations (7), (10), and (15) substituted into Equation (8) yields in terms of x and .
= J rM + rm + r 2 x+ rM l 2

(17)

Substituting Equations (3), (1), (9) and (17) into Equation (5) yields
R K J rM + rm + r 2 x+ RrM l K + x K = v 2K r

(18)

Finally substituting Equations (15), (16), (17) into Equation (14) yields
I + rM l + M l2 + 2 J rM + lM + rm + r 2 x M gl = 0

(19)

Equations (18) and (19) describe the motion of BoRam, and these are the equations that constitute the two-dimensional linearized mathematical model of the system. Finally this set of equations can be expressed in a more convenient state-space form as shown in Equation 20.
x 0 0 x = 0 0 0 0 A32 A42 1 0 A33 A43 x 0 1 A34 x A44 0 0 + B3 v B4

(20)

The unknown properties, J , M , m, r, K and R shall be obtained by direct or experimental measurements. Finally I and l shall be found by conducting a full Digital Mock-Up model in CATIA. All of which shall be shown in subsequent chapters.

4 Microcontroller
The brain of BoRam is actually two stacks of hardware. The bottom part is Arduino, and what sits on top of this is the motor driver which contains the H-bridge.

Figure 3: The robot main controller assembly

4.1 Arduino1
Arduino is an open-source electronics prototyping platform based on exible, easy-touse hardware and software. It is originally intended for artists, designers, hobbyists, and anyone interested in creating interactive objects or environments. Because of this broad range of users group, the foundation software seems to be somewhat limited in low-level customizability and extensibility. This comes at a cost of ease of use. However, there are many examples of well-dened coding practice that one could easily follow and learn from. Moreover, the software has a long history going back to the famous even more powerful Wiring platform that had already gone through numerous revisions proving itself to be reliable. In this project neither Arduino Foundation Library, nor the editor that came with it was used. Instead GNU C compiler for AVR combined with Eclipse integrated development environment and Cygwin was used. Some of the codes in Wiring platform came useful but most of the code was written by myself.

4.2 H-Bridge (Motor Controller)

The L293D is a quadruple high-current half-H driver and is designed to provide bidirectional drive currents of up to 600-mA at voltages from 4.5 V to 36 V. It can also drive inductive loads such as relays, solenoids, dc and bipolar stepping motors, as well as other high-current/high-voltage loads in positive-supply applications. The idea of building this circuit on a Prototype board was originally inspired by the Motor Controller Shield kit from AdaFruit2 but had been modied for only two DC motors control, because otherwise, there would be no pins available for other components in BoRam. All hardware schemes in the website are provided with Creative Common Licenses, therefore there should be no issues with Copyrights.
1 http://www.arduino.cc/ 2 http://ladyada.net/make/mshield/

5 Sensors
5.1 IMU Setup with Kalman Filtering
Kalman lter algorithm that was developed by an open-source community3 was adopted and modied. The algorithm is part of the autopilot onboard code package. All of the software is free; can be distributed and/or modied under the terms of the GNU General Public License as published by the Free Software Foundation. The graphs below illustrate how the Kalman lter can be useful in fusing single-axis angular attitudes collected separately. One way of calculating the attitude is by using two accelerometers. The accelerometer unit gives angular attitude by using arctan2. The orientation ximu , yimu of the IMU is set as ximu pointing to x in inertial frame, but yimu pointing towards y in inertial frame. Thus to get a in accordance to the inertial Equation 21 must hold. = arctan2(x, y) (21) The other way of obtaining the attitude is by integrating the angular rate data from the gyro using Euler integration. Ideally a jig would be required to provide an absolute reference angle at any given moment, but since the data will only be used as an observational reference and not in the core algorithm itself, it is reasonable to do without a costly jig and roughly compare data from the two sources, accelerometer unit and gyro unit, and ultimately with the Kalman ltering result.

Figure 4: Steady motion measurement using Kalman Filter Figure 4 is a response to a steady movement. The Y-axis indicates the angle in degrees, and the X-axis is the step count or sampling count from the microcontroller. Usually when implementing a microcontroller a task is programmed to be executed in a routine basis, but in this experiment the microcontroller was operated in an endless-loop without any delay time allowing the Euler Integration to produce higher quality results. It can be observed that both units reect the angle quite similarly. The experiment was not long enough to catch the drift of the gyro but overall all data seemed to be reliable. So it was necessary to provide a harsh environment to intentionally invoke degradation in data. The next experiment was performed in a hard-shaking circumstance.
3 http://autopilot.sourceforge.net

Figure 5: Unsteady motion measurement using Kalman Filter Figure 5 shows an interesting result. The IMU board was not only rotated radically but also shaken in order to generate lateral acceleration. The lateral acceleration introduces noise to the system so that by using accelerometers alone one cannot measure where about the angular attitude is. At the end of the shaking process the gyro reveals an oset compared to the angle provided by the accelerometer. In this case accelerometer data is more reliable, and the oset can be considered as an integration propagation error or a continuous drift in angle. It is very interesting to see that the Kalman lter is following well the patterns of the gyro in a very noisy environment, whereas, at the end of the shaking process when the sensor is stable, it pertains to the more accurate accelerometer data.

5.2 Coupling between IMU and Optical Encoder

The notation for corresponds to the angular velocity of the wheel relative to the inertial frame. The inertial frame was explained in the rst diagram of Figure 2. However what is being measured by the encoder is enc , and this value is with respect to the Body frame since the encoders are attached to the robot's body. The relationship between and enc can be described as
= enc +

(22)

where is the angular rate of the Body frame relative to the Inertial frame. Therefore this relationship should be implemented in the microcontroller correctly in order to blend the correct angular velocity in the model dynamics. This gives an interesting perspective. The results from two sensors are combined together to give one result, in this case, . Each sensor has a certain characteristics of its own, it may contain various non-linear factors such as back-lash, quantization error, or latency but this depends on each individual sensors and becomes more dicult to lter when the data with dierent kinds of error factors are combined. The following gure illustrates the data that was collected with the wheels xed at a certain position and only the body swinging back and forth. The rst 170 steps were performed in a steady slow motion and afterward fast motion was applied to see the dierence. The color green indicates angular rate, enc , deduced by the encoder, blue indicates Kalman lter rate, , of the body. The color red indicates , and in this situation, since the wheels are xed, should always be zero. This shows that combining dierent sensor noise factors can introduce signicant noise that is dicult to lter out.

Figure 6: conict in dierent sensor dynamics

5.3 Kalman Filtering Code Example

Here is the code that had been successfully used with the project.
/* ********************************************************* * EXTENDED KALMAN FILTER * ********************************************************* * Updates the current angle and rate estimate. * * [Note] * 1. Only compute the terms that are explicitly non-zero * 2. Matrix math to be done in as few steps as possible. */ #include "WProgram.h" float angle; float gyro_bias; float rate; // update rate // static const float dt = ( 1024.0 * 256.0 ) / 8000000.0; // covariance matrix (variable) static float P[2][2] = { { 1, 0 }, { 0, 1 } }; // R(1x1) represents measurement covariance noise. // 0.3 rad jitter from the accelerometer expected. static const float R_angle = 0.3; // Q(2x2) represents the process covariance noise. static const float Q_angle = 0.001; static const float Q_gyro = 0.003; void kalman_predict(const float gyro_m, float dt) { // Store our unbias gyro estimate rate = gyro_m - gyro_bias; // Pdot = F*P + P*F' + Q const float Pdot[2 * 2] = { Q_angle - P[0][1] - P[1][0], -P[1][1], -P[1][1], Q_gyro }; // Update angle estimate angle += rate * dt; // Update covariance matrix

10

} void kalman_correct(const float ax_m, const float ay_m) { const float angle_m = atan2(ax_m, -ay_m); const float angle_err = angle_m - angle; // The H matrix const float H_0 = 1; // PH' const float PHT_0 = H_0 * P[0][0]; const float PHT_1 = H_0 * P[1][0]; // S = H P H' + R const float S = R_angle + H_0 * PHT_0; // K = P H' inv(sS) const float K_0 =PHT_0 / S; const float K_1 =PHT_1 / S; // P = P - K H P const float HP_0 = PHT_0; const float HP_1 = H_0 * P[0][1]; P[0][0] -= K_0 * HP_0; P[0][1] -= K_0 * HP_1; P[1][0] -= K_1 * HP_0; P[1][1] -= K_1 * HP_1; // X += K * err angle += K_0 * angle_err; gyro_bias += K_1 * angle_err; } void kalman() { kalman_predict((analogRead(GYRO_PIN) - gyro_zero) * GYRO_SCALE, 0.02); kalman_correct(analogRead(ACCEL_X_PIN) - ax_zero, analogRead(ACCEL_Y_PIN) - ay_zero); }

P[0][0] P[0][1] P[1][0] P[1][1]

+= += += +=

Pdot[0] Pdot[1] Pdot[2] Pdot[3]

* * * *

dt; dt; dt; dt;

6 Actuator
6.1 Identication of Eective Moment of Inertia
The robot under development serves as a multi-purpose test bed replacing the need for a rotary motion apparatus. The rubber tire on the wheel may easily be pushed aside allowing the rim to serve as a pulley. It is possible to suspend a certain amount of weight on a string attached to the pulley to apply a torque against the shaft. The reason for relocating the tire on the wheel instead of taking it o can be found in Section 3. System Modeling. In short, the eort in taking measurements was minimized by only measuring the collective parts that are critical to the model.

11

Figure 7: FBD of Rotary Motion Apparatus The experiment of discovering the moment of inertia of all the rotating parts is conducted by dropping a known weight from the pulley. The string may be loosely attached to the pulley so that it can completely fall o near its end. When the string falls o and when g no longer exists, the pulley will decelerate and gradually stop due to the constant friction torque f . Velocity data is collected by the encoder, and the result is shown in Figure 8.

Figure 8: Rotary motion of the actuator assembly The graph consists of two constant angular accelerations. The point of switching comes when the string falls o from the pulley. The constant deceleration is produced by a constant friction torque f . The acceleration torque, g , is produced by the tension in the string multiplied by r, the moment arm. The friction force acts on both cases. The eective moment of inertia J is a result of all the rotating parts, namely, the rotor and reduction gears inside the motor, the wheel, and nally the shaft that connects them all together. The torque produced by the falling weight, g , can be found by analyzing the tension in the string while the angular velocity accelerates. According to the second diagram in Figure 7, two equations can be described as follows.
J accel = g f g = mraccel mg r

(23) (24)

12

The relationship in the third diagram in Figure 7 is,

J decel = f

(25)

There are two equations in which accel , decel , are known and g can be obtained from Equation (24). Therefore the other two unknowns can be determined easily. Table 2 shows the result. Motor (Right)
accel 26.039 decel 3.496 g 0.0156 f 0.0019 J 0.00053

Table 2: Calculation of J and f using a 0.059[kg] weight and a 0.0293[m] radius pulley

6.2 Identication of Torque Constant, Choice of PWM Frequency

Substituting Equations (1) and (4) into (5) yields
= K K2 v enc R R

(26)

The duty cycle was assumed to approximate the output voltage by v = dutycycle 9V , where 9V is the input voltage and dutycycle varies from 0% to 100%. To nd the appropriate PWM frequency, two distinctive experiments had been carried out to characterize the motor in the best linear fashion ensuring that the derived constants K and R should be as reliable as possible.

Figure 9: BoRam used as a speed-torque apparatus

13

Figure 10: Torque-Speed Characteristics using PWM Frequency of 3.921 KHz

Figure 11: Torque-Speed Characteristics using PWM Frequency of 31.37 KHz This is called the torque-speed characteristics. The legend of Figures 10 and 11 describes the PWM Duty Cycle. The supply voltage is 9V, so the approximate voltage for 33.3%, 55.6%, 77.8%, 100% is 3V, 5V, 7V, 9V respectively. Note that the motor torque increases in proportion to the applied voltage, but the net torque reduces as the angular velocity increases. Figures 10 and 11 illustrates the torque-speed characteristics of the experimental data for dierent PWM Frequencies. The negative slope implies that the voltage-controlled DC motor has an inherent damping in its mechanical behavior. According to the response from two dierent PWM frequencies, it can be seen that the four slopes exists are dierent. This means the DC motors are non-linear. The slopes are supposed to be identical for linear motors. The deviation among four slopes in each graph becomes higher when the PWM frequency is low, this means that lower frequency exhibits more noticeably non-linear responses. Thus, in this case, higher frequency should be favored because it has less deviation among the slopes. 31.37KHz was chosen as the PWM Frequency and among the slopes the curve corresponding to 3V was chosen to be used as a linear reference for the calculation of the constants K and R. The reason of choosing a rather smaller slope is because otherwise the system would interpret the motor torque to be lower than it actually is. This would 14

cause overshoot. By choosing a smaller slope as the reference it may cause slow rise time but overshoot and instability may be prevented. PWM Freq 3.921 KHz 31.37 KHz 31.37 KHz
K 0.730 1.0636 0.4199 R 272.6 290.1 119.1

reference curve 55.6% 100% 33.3%

Table 3: Result of Torque-Speed experiment

7 Digital Mock-Up
7.1 Parametric Design Technique
Since the building blocks for BoRam is basically Lego and many parts are required to build this, parametric design method has been applied in part design level to make similar parts easily just by controlling a few parameters. Lego is a good subject, if not the best, to be represented in a parametric form. All the dimensions in any part are composed of an integer factor of LDU. (1 LDU = 0.397mm) Figure 12 shows that four dierent length of beams can be created by changing only one parameter in the design specication tree.

Figure 12: Example of using Parametric Design on Lego

15

7.2 Draft

Figure 13: Example of generating a draft

7.3 Applying Physical Properties

CATIA has a list of supported materials. Lego is known to be made out of ABS plastic, but the closest material to that in CATIA was found to be Plastic. To make a legitimate approximation, important attributes must match within an acceptable degree. The focus is on static properties, which are essentially mass or moment of inertia, other properties such as Poisson Ratio and Young Modulus may be neglected. As a result only the Specic Gravity should be compared and be similar enough to support the approximation. It seems that Lego does not ocially disclose information about their material but various web resources

16

Figure 14: Properties of ABS Plastic versus Plastic had made available the attributes about Lego Plastic. According to Figure 14, The Specic Gravity 4 for Lego ABS plastic is 1.05 (dimensionless), and CATIA uses 1200kg/m3 for the density of Plastic. Comparing the two after appropriate unit conversion, ABS plastic density stands for 1050kg/m3 which appears to be 15% less than the Plastic CATIA is using. Due to time constraint, this CATIA Plastic approximation value was taken without any further investigation. Figure 15 shows the calculated moment of inertia of the Body, I .

Figure 15: Digital Mock-Up in CATIA Table 4 compares the measured values and the analytic values that was obtained from CATIA. Parameter M body mass I body moment of inertia l length of axis to CG m wheel mass Measured 0.422 0.06 0.0235

CATIA 0.445
0.002 0.06 0.026

Unit
[kg] [kgm2 ] [m] [kg]

Table 4: Physical properties for BoRam

4 http://orionrobots.co.uk/tiki-index.php?page=Lego+Specications

17

8 Controller Design
8.1 Digital Controller for Motor Position

Figure 16: block diagram of motor position control Before jumping into the ultimate goal of controlling BoRam, verications on the parameters must be done. One of the easiest way may be through implementing a controller for the DC Motor. A digital controller for position reference will transform the DC motor to behave like a servo. The code for the controller was generated using the Real timeT oolbox EmbeddedCoder. ATmega168 is not ocially supported as a target system in Real timeT oolbox EmbeddedCoder. Therefore some manual programming is required to port the code into the existing components in the system.

8.1.1 Digital Control - Proportional

To conrm the DC motor model, a simple proportional control was designed and implemented. Figure 17 illustrates the simulation result versus the real data. The rise time is shorter in reality. This is probably because of the sensor dynamics that was not considered in simulation. In reality the encoder has large quantization error. When conducting this experiment 64 lines were used by the encoder. This means 5.6 degrees was the maximum precision the sensors could have which is quite poor. After this experiment the sensitivity was increased by a factor of 2 to achieve greater accuracy in position and velocity values. The gain value used in this experiment was 2, and the step input was 90 degrees.

Figure 17: Simulation vs Acquired Data of Proportional Control

8.1.2 Digital Control - Root Locus

In this experiment Control System Toolbox in Matlab, more specically 'sisotool' was used to design a Root Locus Controller by applying a gain and placing poles and zeros. Several designs were devised that looked robust in theory, but performed not as well. The dierence of theory and practice became prominent as the solution required greater control eort. While the control design technique was intended for linear systems, the eect of saturation was becoming more outstanding in the real world. 18

One of the working design is presented. The control eort was designed to be kept below the saturation point, however the same quantization error that occurred in Proportional Control experiment was also observable here as well.

Figure 18: Root Locus Design Parameter Gain Zeros Poles Value 7.26864330434832 [-0.973326359832636;0.90376569037657] [0.71;-0.98]
7.2686(z + 0.9733)(z 0.9038) (z 0.71)(z + 0.98)

Controller =

Figure 19: Simulation vs Acquired Data of Root Locus Design

8.2 LQR Controller for Balancing

8.2.1 Discrete State-Space
To obtain a discrete model of the system, rst one must nd the state-space representation in continuoustime domain and then convert it to discrete domain. In this case a conversion method called zero-order hold 19

was used. It is a common practice to have 25~30 times of the bandwidth frequency of the closed-loop system as the sampling time but there is another constraint that needs to be considered, which is the processing power of the microcontroller. Not knowing exactly the bandwidth, 0.02[s] was chosen as the sampling time out of a random pick. It should be observed whether the microcontroller is capable of processing the data without being over-run. Since the microcontroller has a limited amount of resource, reducing the period of sampling time can be achieved by trial and error until the microcontroller begins to over-run and fail. The two linearized ordinary dierential Equations (18), (19) representing the system can be modeled in Simulink as shown below. Although having a non-linear system model for the simulation and

using the linearized model for linear controller design is a recommended practice, however time was not allowed to model a non-linear model within Simulink.

Figure 20: linearized system model

8.2.2 Controllability
The design criteria for the system was simply to make BoRam balance permanently. Criteria for settling time, rise time, overshoot and steady-state error were just too much for the initial controller design phase. Moreover, having done a rough linearization on highly non-linear toy parts and the lack of mental conviction of the possibility of ne tuning had also contributed to settling down on a rather simplied goal. In theory, there is a way to gain assurance on whether a system is controllable or not. For the system to be completely state controllable, the controllability matrix must have the rank of n. Consider the linear time invariant system given by
x = Ax + Bu

The controllability matrix of the above LTI system is dened by the pair (A,B) as follows
Cr (A, B) = [B, AB, A2 B, A3 B]

The rank of the matrix is the number of independent rows. Applying this condition to our model gave 4. Thus the system is controllable! This proves that the current discrete system is completely state controllable.

8.2.3 Digital LQR Full-State Feedback Controller design

The next step and the most critical step is to nd the control matrix(K). The linear quadratic regulator method is used to nd the control matrix. This method allows to nd the optimal control matrix that results in some balance between system errors and control eort. To use LQR method, three parameters should be found: Performance index matrix(R), state-cost matrix(Q), and weighting factors. For the sake of simplicity, performance index matrix shall be equal to 1. The weighting factor shall be chosen by trial and error. Finally the control input shall be a step input of 0.2[m]. 20

Unlike other design methods, the full-state feedback system does not compare the output to the reference, instead, it compares all states multiplied by the control matrix, K x, to the reference input. Thus, it is not expected to have the output equal to the reference input. To obtain the desired output, a scaling factor of the reference input should be found to match the output. This is done by introducing a feedforward scaling factor called Nbar. Below is the response after tuning the state-cost matrix and Nbar values to achieve a favorable response for a step input of 0.2[m].

Figure 21: Closed-loop output of the states using LQR controller The block diagram of the full-state feedback system is shown below. A periodic pulse disturbance was added to the state that represents the angular velocity of the Body, this was to simulate any collision or intentional push motion. A step disturbance was added to the state representing the body angle term, this was performed to observe the robustness of the system when the Center of Gravity varies. In addition, a saturation block was added to simulate the non-linear factor of the DC motors. According to the response, this system could reject all disturbance that had been applied.

Figure 22: block diagram of BoRam control

9 Simulation & VRML

To get a VRML representation on the robot, BoRam was modeled in CATIA, then exported to VRML, with which Simulink VRML Toolbox is able to visualize. By using CATIA one can not only design a complex 21

model quite easily but also apply physical material properties to the individual parts and get information regarding CG, or moment of inertia of the full assembly. Due to the amount of details of the VRML model the real-time rendering was tediously slow, so another simplied model was designed for testing purposes. The testing procedure includes dening joints and coinciding coordinate systems. After all is well working the simplied model is swapped to the complex model, then by using the video recording option in VRML Toolbox the simulation can be recorded and can be viewed smoothly.

Figure 23: block diagram for VRML simulation

Figure 24: simplied VRML Model v.s. complex VRML Model

10 Results and Conclusions

Figure 25 illustrates two scope graphs as a result of the Simulation. The scope on the top shows the output states; x in yellow and in pink, the one on the bottom shows the input voltage to the motors. These are results of a step input of 20cm starting at 1 sec, then when the time-line reaches 0:02, a periodic impulse in angular velocity is added to the body with magnitude of 0.1 rad/s, width of 0.2 seconds, for every 2 seconds. Finally a change in attitude to the body angle, 0.1 rad is added to mimic the change in CG. The result was a success in terms of stability.

22

Figure 25: Scope graph The next step after a satisfactory in simulation will be, of course, putting BoRam into a real test. The robot was able to balance for as long as possible while being able to reject mild disturbance applied by pushing the robot back and forth. Figure 26 illustrates BoRam in action.

Figure 26: BoRam in Action BoRam was built thoroughly by using toy parts and hobbyist-graded parts which are inexpensive compared to industrial parts. The quality dierence was observed in the non-linearity and the quantization error 23

characteristics that had been carried out in this project. In spite of massive assumptions on linearity and neglecting many hard-to-dene parameters, the control algorithm that was applied seemed to work ne. This proves the robustness of the systematic approach that was taken to solve this control problem and opens a wide possibility that one can achieve even more with less.

11 Reference
Franklin, Gene F.,Powell, J. D. and Emami-Naeini, Abbas. Feedback control of dynamic systems 2006 Ogata, Katsuhiko. Modern control engineering Mohinder S. Grewal, Angus P. Andrews, Kalman Filtering: Theory and Practice Using Matlab IEEE Transaction on Industrial Electronics, Vol. 49, No. 1, Feb 2002, JOE: A Mobile, Inverted

Pendulum

Tim Wescott, Applied Control Theory for Embedded Systems H. Harry Asada, Department of Engineering, MIT, Introduction to Robotics Experiment09: Angular Momentum, Physics Department, MIT, 2003 www.library.cmu.edu/ctms www.arduino.cc www.wiring.org.co www.geology.smu.edu/~dpa-www/robo/nbot/index.html www.nongnu.org/avr-libc autopilot.sourceforge.net

A Nomenclature
attributes M - mass of the body m - mass of the wheels J - moment of inertia of the actuator assembly I - moment of inertia of the body - torque generated from the motor's armature e - back emf voltage R - DC motor windings resistance K - torque constant Ke - back emf constant - wheel velocity in reference frame - wheel acceleration in reference frame enc - wheel velocity from encoder (body frame) h - friction force r - wheel radius H - horizontal force between body and wheel V - vertical force between body and wheel Table 5: Nomenclature 24 SI unit
kg kg kg m2 kg m2 N m V
N m A V rads rad s rad s2 rad s

N m N N

B Arduino Conguration
ATmega168 pin 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 port PC6 PD0 PD1 PD2 PD3 PD4 VCC GND PB6 PB7 PD5 PD6 PD7 PB0 PB1 PB2 PB3 PB4 PB5 AVCC AREF GND PC0 PC1 PC2 PC3 PC4 PC5 function RESET RXD TXD INT0 OC2B/INT1 Arduino pin RESET D00(RX) D01(TX) D02 D03 D04 VCC GND CRYSTAL CRYSTAL D05 D06 D07 D08 D09 D10 D11 D12 D13 VCC AREF GND A00 A01 A02 A03 A04 A05 interface R R R ENCODER_2A ENCODER_1A L293/02 R R R R L293/01 L293/09 L293/10 L293/15 L293/07 ENCODER_2B ENCODER_1B DEBUG LED 5V GND 2.5REF ACCEL X ACCEL Y GYRO POT

OC0B OC0A OC1A OC1B OC2A

ADC0 ADC1 ADC2 ADC3 ADC4 ADC5

Table 6: Arduino pin mapping Summary of Arduino specication: Microcontroller Operating Voltage Input Voltage (recommended) Digital I/O Pins Analog Input Pins DC Current per I/O Pin DC Current for 3.3V Pin Flash Memory SRAM EEPROM Clock Speed ATMega168 5V 7~12V 14 (including 6 PWM outputs) 6 (10bit ADC) 40mA 50mA 16KB 1KB 512 bytes 16MHz

25

C H-Bridge(L293D) Conguration
LD293 pin 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 port 1,2EN 1A 1Y HSK/GND HSK/GND 2Y 2A VCC2 3,4EN 3A 3Y HSK/GND HSK/GND 4Y 4A VCC1 interface ARDUINO/D05 ARDUINO/D04 MOTOR1_RED MOTOR1_BLK ARDUINO/D09 9V ARDUINO/D06 ARDUINO/D07 MOTOR2_RED MOTOR2_BLK ARDUINO/D08 5V description MOTOR1PWM MOTOR_1A

MOTOR_1B EXTERAL MOTOR2PWM MOTOR_2A

MOTOR_2B INTERNAL

Table 7: H-Bridge pin mapping

D Physical Properties of BoRam

Param
M I l m r

Measured 0.418 (body w/ electronics) + 0.044 (battery) 0.06 0.0295 0.041

Catia 0.353 (body w/o electronics)

0.002 0.06 0.026 0.041

Unit
[kg] [kgm2 ] [m] [kg] [m]

Table 8: Physical properties for BoRam mass 42 [g] gear ratio 13.2583 : 1

Table 9: Lego Motor 71427 Specication

26