TRAJECTORY CONTROL OF A TWO-WHEELED ROBOT

Robots have been used in many applications in the past three decades. One type of robot is a two-wheeled robot that requires control for both balancing and maneuvering. This thesis shows the design of a controller that can both balance and provide trajectory control for a two-wheeled robot. The controller is a digital tracking system that utilizes pole placement for system stability. The thesis provides a detailed description for modeling the plant, design of the controller, simulating the final system and implementation into hardware. Three pole placement methods are analyzed as well as tracking designs that use steady-state or ramp tracking. Each of these controllers have simulation results and the controller that has the best simulation performance is implemented into the hardware. The final controller is implemented into Lego R © Mindstorm R © hardware using Matlab R © and Simulink R ©. The results of the simulations are compared to the results found in hardware.


LIST OF TABLES
The ability to control robots has been in the forefront of control theory in the past three decades. Robots have been used in many applications. They can be used to make everyday living for the handicapped easier while allowing tasks to be performed more efficiently in industry. These robots are becoming more sophisticated to where they can autonomously perform tasks.
The control of each type of robot becomes its own engineering problem.
Robots can move around using wheels, tracks or belts. They may even stay in place and perform tasks along an assembly line. The actuators can be electromechanical or use hydraulics. All these differences result in different kinematics and dynamics of the robot.
There are many examples of controlling an inverted pendulum. Seul Jung and Sung Su Kim [1] use a Neural Network to provide tracking control for a singleinput-multiple-output (SIMO) two-wheeled robot. The cart is controlled forward and back, tracking a sinusoidal motion. Vaccaro [2] uses digital state feedback tracking to control a SIMO inverted pendulum that uses a cart coupled to a drive screw. In work introduced by S.W. Nawawi et al, [3] a sliding mode controller was used to balance a two-wheeled robot; however, it does not indicate that the controller can be used to control the robot's trajectory.

Problem Identification
The two-wheeled robot is an inverted pendulum that can also move about in a two dimensional plane. The robot contains sensors that provide feedback for balancing and trajectory tracking. Pole placement will be used in the design of the controller. Therefore, the two-wheeled robot will need to be modeled. Since pole placement is used to design the controller the method of placement needs to be scrutinized for both stability and performance. Consideration will also need to be taken when implementing the new controller in real hardware.

Contributions of this Thesis
The two-wheeled robot has some challenges that are not seen in the inverted pendulum problem. In the inverted pendulum problem found in the book by Vaccaro [2], the pendulum is mounted to a cart. The cart is directly coupled to the drive motor, while the pendulum moves freely on a mounted encoder. The two-wheeled robot has the inverted pendulum directly coupled to the drive motor.
Also, the inverted pendulum rotates on a single shaft. The two-wheeled robot does not rotate on one shaft, but uses two shafts, one for each wheel. This problem was examined by Yamamoto [4] by controlling the average of the two wheels.
Yamamoto was successful in creating a closed-loop control for balancing; however, the controller did not provide closed-loop control for trajectory tracking. Rather than control the average of the wheels (a single control input), the controller in this thesis will send independent coordinated signals to each motor (two control inputs).
The design of this system will require the tools of multivariable control theory.
Tests with the hardware robot will demonstrate the applicability of multivariable control theory to a real-world system. Modifications may have to be made for the theoretical results to be implemented in hardware.

Overview of this Thesis
The research for this master's thesis will require a scientific approach. The dynamics of the system need to be carefully scrutinized and also the micro controller architecture, which can introduce delays [5]. A plant model exists [4] and some system parameters will be found directly from measurements of the hardware system. The final plant will be used to design a controller based on Digital Tracking System Theory. The design will include a comparison of multiple methods used to calculate a feedback matrix for the desired closed-loop pole locations, and a stability analysis to determine robustness. Simulations will be used to observe the response of the system. The controller that provides the best stability margins and performance will be loaded into hardware for testing. Finally, collected data from the hardware and simulations will be compared. The two-wheeled robot model used in this thesis was derived by Yamamoto   The state-space equation for the two-wheeled robot is based on the coordinate system in Figure 2.2. The equations that define the motion of the two-wheeled robot in the coordinate system are shown in Equations (2.1) through (2.7) and

List of References
have the initial heading along the positive x-axis at t = 0. These equations show that the final state-space equation only requires the variable θ, Ψ, and φ.

State-Space Equations
The state-space equation is derived from the translational kinetic energy, rotational kinetic energy, potential energy, DC motor torque and the DC motor viscous friction. Equation (2.8) is the motion equation for the two-wheeled robot for the variables θ and Ψ.
The matrices E,F,G and H are defined in Equation (2.9) and the constants are defined in Equation (2.10).
Equation (2.11) is the motion equation for the two-wheeled robot for the variable φ. The constants are defined in Equation (2.12).
The state vector is defined by x and the input vector is defined by u. 14) The matrices A and B are defined in Equation (2.15) along with the definitions of the constants.

Experimental Parameters
The model that was provided in reference [1] had all the required parameters defined. However, it was determined that some of the parameters could be more accurate if they were extracted experimentally. The distance to the center of mass L and the friction coefficient between the body and DC motor f m were two of these parameters. L was chosen since the current model assumed that the body was rectangular. This is clearly not true since the shape of the body is not rectangular. f m was chosen because the friction can easily change from one DC motor to another.
The first parameter that was extracted was L. This was accomplished by using the natural frequency of the freely hanging robot. By suspending the robot upside down by the axle and not rotating the wheels, Equation (2.8) can be simplified.
Since the wheels are not rotating, the angle θ and Ψ are equal and u is equal to 0. This reduces Equation (2.8) to the state Equation (2.16).
The following equation can be used to characterize an inverted pendulum by its natural frequency w p as stated in Vaccaro [2].
Since a speed controller is being used for the experiment,ẋ 3 in Equations (2.20) and (2.21) drops out once the two wheels have reached a steady-state speed. Also, setting v l and v r to u results in the Equation (2.22).
The ratio for the measured output to the measured input is defined as α, where (2.23) Using Equation (2.23) and substituting the constants for C4 and C6 with the constants from Equation (2.10) the value of f m can be determined.

CHAPTER 3 Digital Tracking System Theory
The two-wheeled robot must balance while maneuvering. Balancing the robot is the well known inverted pendulum problem. However, to maneuver the robot with closed-loop control is not an easy task. The robot as described in Equation (2.15) has six state variables which all contribute to the stability of both maneuvering and balancing of the robot. The task also has an added level of difficulty due to the fact that there are two control actuators.
where T is the sampling interval and k is the discrete-time index.
The digital control toolbox contains the function zohe that will convert the continuous state-space matrices A and B to Φ and Γ. The function requires A, B and T as inputs. It uses a single matrix exponential shown in Equation (3.2) that can simultaneously calculate the matrices Φ and Γ. The method is credited to C.

Controllability
The robot must be determined to be controllable prior to the design of the , has a determinant that is not equal to zero or the rank equals n, where n is the number of states. In a single-input-single-output (SISO) system Φ is an n × n matrix and Γ is a n × 1 matrix. This means that W c is an n × n square matrix and the determinant can be calculated. The two-wheeled robot is a multiple-inputmultiple-output (MIMO) system, therefore, Γ is a n × p matrix and p is defined as the number of inputs. In this case controllability of the ZOH models can only be determined by the rank not the determinant of the matrix W c . 3) The ZOH model must also be controllable.

Tracking System
The control of the two-wheeled robot is accomplished by using a digital tracking system as shown in Figure 3.

Additional Dynamics
The additional dynamics are required to have a tracking system. The additional dynamics in a digital tracking system are implemented as a state-space equation as seen in Figure 3.1. The inputs to the additional dynamics are the difference of the measured outputs from the plant and the commanded reference.
The outputs of the additional dynamics are based on the feedback matrix L 2 .
The additional dynamics drive the system to zero steady-state error by adjusting the output. This can be implemented to remove steady-state error only (single integrator) or remove error to a ramp (double integrator).
In hardware the output of a controller can saturate. At the point of saturation, or just before saturation, the additional dynamics need to be disabled. Disabling the additional dynamics prevents the integrators from running away, or "winding up".

Feedback Matrix
The feedback matrix is used to place the closed-loop poles.
where z is the discrete-time domain pole location, s is the continuous-time domain pole and T is the sampling rate.
The command fbg from the controls toolbox can now be used to calculate the feedback matrix. There are also other tools provided by Matlab R such as place and acker that can be used to generate the feedback matrix. The command acker will not be examined in this thesis. It only produces results for a single input system, and the two-wheeled robot is a multiple input system. There will also be another recently developed algorithm called TFBG The closed-loop system matrix Φ d − Γ d L has poles λ i and eigenvectors Ψ i that satisfy This equation can be rewritten as The dimensions of the P (λ i ) are n × (n + p) where n is the number of states and p is the number of inputs. This means that the null-space has a dimension p which is partitioned into an n-vector Ψ i and a p-vector LΨ i . There are up to p linearly independent vectors that can be chosen for each pole location in the closed-loop system. The number of vectors chosen for each pole is equal to the multiplicity of the pole.
To find the value of L, each null-space vector for the desired pole location is partitioned into a "top part" t i and "bottom part" b i . Using Equation (3.10) then t i can be used to solve for L.
Since n is equal to the number of desired poles, this is repeated n times, grouping all the top and bottom null-space vectors. This results in a top matrix T which is n × n and bottom matrix B which is n × p with the equation which a unique L can be solved for by Although this provides a unique solution for L, the construction of B and T are not unique. Different constructions of B and T will result in the desired closedloop poles, however may not produce the optimal performance for the closed-loop system [4]. It should also be noted that to avoid L having complex values, complex poles need to include the complex conjugate pole location.
The algorithms fbg and Matlab's place are based on methods introduced by Kautsky, Nichols, and Van Dooren [5]. This method calculates eigenvectors that are as orthogonal as possible, while achieving the desired closed-loop poles. This is accomplished by using an iterative method. The matrix T is first found by initializing the matrix using the method above. Then, one at a time, each eigenvector in T is projected to be as orthogonal as possible to all other eigenvectors, which produces the optimal eigenvector. Before calculating the next eigenvector in T, the new eigenvector replaces the old one. Once all the eigenvectors of T are calculated, the process is repeated. The iteration that results in the nonsingular matrix T with the best condition number is then used to calculate B. In order to calculate the matrix B a vector α is introduced where The vector α can be found with the knowledge of T (λ i ) and Ψ i by using Equation (3.16).
The knowledge of α i allows b i to be determined using the the following equation and repeating this n times to form the matrices T and B. Then Equation (3.14) can be used to find L.
There are slight differences in both fbg and Matlab's place that will be seen later numerically. The most likely place where they may differ is the criteria that is used to choose the initial null-space vectors.
The new method of calculating L is based on optimizing a stability robustness norm, which is described in Burl [6]. The calculation of L is performed in the function TFBG. TFBG is initialized using the same method as fbg, which is the only commonality between the two algorithms. The new algorithm searches for stability robustness in a system with a MIMO plant. The stability robustness indicates how big ∆(z), the error in the plant, can be before the system goes unstable, where I + ∆(z) is a MIMO transfer function cascaded with the plant.
The inputs and outputs of ∆(z) are y d and w d respectively, and if ∆(z) = 0 then the control system is using the nominal plant model. The condition for stability of the control system is the robustness norm δ max can be found by The objective of TFBG is to find L that results in the largest possible δ max . The two-wheeled robot requires another constraint, where the rows of L have the same magnitude and certain columns have opposite signs.

Stability Margins
The value of δ max is directly related to the gain and phase margin of the control system. In a SISO system the stability margins can be found using q, which represents gain and phase uncertainty of the plant in classical control. In a MIMO system q could be used; however the stability margins are only representative of the individual plant inputs without any consideration of simultaneous errors on each plant input. Therefore, there would be stability margins only for each individual input. Assuming δ max has been calculated for a given control system and ∆(z) is assumed to be a complex number C, the corresponding classical gain margins can be found by using where the upper gain margin (UGM) and lower gain margin (LGM) are U GM = 20 log 10 q max (3.24) LGM = 20 log 10 q min (3.25) The UGM and LGM are to be greater than 3db and less than -3db, which requires δ max to be greater than or equal to 0.4.
In order to find the phase margin, the equation q = e −jφ = 1 + C is used, where 1 is the center of the circle and C is a disk with the radius δ max . Therefore, the phase margin (PM) is To have a PM of at least 30 • , δ max needs to be greater than or equal to 0.5.
Therefore, a δ max that has good PM will always have good gain margins. [2] C. F. V. Loan, "Computing integrals involving the matrix exponential," IEEE Transactions on Automatic Control, vol. AC-23, pp. 395-404, June 1978.

CHAPTER 4
Hardware The two-wheeled robot is built using the LEGO R Mindstorm R NXT system

Design Tools
The controller for the robot is designed using Matlab and Simulink R and is

Bluetooth Interface
The bluetooth interface provides a communications link from a bluetooth enabled device to the Lego Mindstorm NXT main processor. The link contains the information shown in

GUI
The PC has a graphical user interface (GUI) that was designed for this application and can be seen in Figure 4.4. This custom interface provides a method to select and open a communications port. It also provides a command to balance, a command for θ, a command for φ, and a method to record data.
The first step in the operation of the GUI is to make a connection through the bluetooth communications port. This requires the Lego Mindstorm NXT to be paired with the computer by performing the following steps adapted from Chikamasa [3].
1. On the robot, in the main NXT screen scroll to the bluetooth symbol.
2. In the bluetooth menu scroll to the ON/OFF and ensure that it is on.
3. Return to the bluetooth menu.
4. Place the PC's bluetooth into Discovery mode.

5.
In the NXT bluetooth menu scroll and select search, then select the PC that the NXT should be paired to.
6. The NXT will provide a pairing number and once that number is accepted it will attempt to connect to the PC.
7. The PC should indicate that the NXT is trying to connect and requires the pairing number. Provide the number and finalize the connection.
8. Then to determine what communications port will be used, go to the bluetooth settings on th PC and locate the port associated with "outgoing NXT 'Dev B"'. The communications port that was found using the pairing method used above is the port selection used in the GUI. This port will remain the link to the Lego Mindstorm NXT until the device is removed from the PC. Now that the bluetooth is configured, the GUI can be used to control the robot. The following steps are taken to run the robot and assume that the controller with the bluetooth interface block has been preloaded.
1. Turn on the Lego Mindstorm NXT and press the orange button until the nxtOSEK screen flashes.
2. Select "Run" by pressing the right arrow. The robot will beep once the calibration of the Gyro is complete. The robot should not be moved prior to the beep since this will result in an abnormal gyro calibration.
3. Run the GUI in Matlab.
4. In the drop down menu, select the communications port for the bluetooth interface.
5. Click on "Connect". Once a connection is made the "Balance" button will activate.
6. Place the robot in an upright position on a surface and press the "Balance" button. Once the robot starts to balance, let go and let the controller do the work.
7. In the test panel, select from the four tests that can be performed.
8. Clicking "Start" will send the profile and collect data. Once the profile is finished, the data is available in Trial.mat.

Controller Design Considerations
The main processor is a 32 bit fixed point processor. The Real-Time Workshop allows floating point calculations to to be performed on the 32 bit fixed point processor. These operations will consume more processing time. The controller will be implemented in floating point; therefore, the extra computation is a factor.
The data that will be sent back and forth over bluetooth communications also requires processing time. These factors determined that a 10ms sampling interval should be used. The high sampling interval is also much higher than the interval at which the I2C communications are handled with the AVR. Therefore, the I2C delays will not be a factor in the design and analysis of the controller.
The hardware will also affect the design of the controller since the resolution of the sensors is low. This requires the controller to have a slower setting time so that the controller does not respond to the bit changes. This The plant is then examined and determined to be controllable by obtaining W c from Equation (3.3) and verifying that the rank is equal to 6.

Additional Dynamics
The next step is to determine the additional dynamics. The ability to track steady-state or ramps in θ and φ were both examined. The additional dynamics for a steady-state tracking are shown in Equation The closed-loop poles need to be determined before the feedback matrix can be calculated. The two tracking designs, steady-state and ramp, will have their own set of closed-loop pole locations. This is due to the fact that step tracking requires an 8th order closed-loop system and ramp tracking requires a 10th order closed-loop system. In both ramp and step tracking, three of the closed-loop poles from the plant are used.

Feedback Matrix
The design of the controller can utilize one of three algorithms for calculating the feedback matrix. These gains were calculated for both the steady-state and ramp tracking. Therefore, six sets of gains were determined in all. The stability margins are also calculated for the six controller designs. The performance of each of these designs is later examined in the Matlab/Simulink Model.

Stability Margins
The stability margins for each of the closed-loop systems were calculated using the equations in Section 3.5. The system N(z) to calculate δ max is the state-space model in Equation (5.5).
LGM  Table 5.4 do indicate that using TFBG will provide the most robust controller. These controllers are examined further for performance in Chapter 6.

Plant Inputs and Outputs
In order to implement the controller, the feedback and commands of the robot need to be transformed into the states that are shown in Equation (2.14). The feedback signals from the robot are θ m l , θ m r andΨ which are converted from degrees to radians and then used to find the required states. θ and φ are determined by using Equations (2.1) and (2.2). Ψ is calculated by using a forward Euler integration ofΨ.
The state variablesφ andθ require the derivative of θ and φ. The derivative was implemented as a second order filter with a specific settling time in order to reject any frequencies that are higher than the system response.
Estimates of the state variablesφ andθ could have been implemented using a linear observer system. However, modeling has shown that non-linearities due to quantization make it difficult to use the linear observer accurately.
The outputs of the plant require scaling. Since the DC motor voltage is PWM based, the voltage from the controller is scaled and limited by the battery voltage (±V bat ). The commands sent to the motors are PWM l,r = 100 V l,r V bat . The presence of the limiters require that the additional dynamics, which are integrators, be held in order to keep them from rapidly growing while in the limits. The integrators are not held exactly at ±V bat but at a voltage slightly lower. Limiting the integrators at a lower voltage than the battery allows the controller to respond as a regulator for the remaining voltage head room. The regulator is the main component used to maintain the robot's balance, giving a priority to the system, which is first balancing and second maneuvering.

Modeling and Simulations
The controller design is first implemented in a computer model utilizing Matlab and Simulink. The model used can be seen in Figure 6.1.

Performance Testing
The model was used to determine the performance of the controller design for each of the feedback matrix calculation algorithms to both steady-state and ramp tracking. In Chapter 5 the stability margins for each feedback matrix calculation algorithm with steady-state and ramp tracking were calculated. The stability margins are a good indication of how the design will perform. However, even though the stability margins may show the system to be more robust, the dynamics may still not produce the best performance.

Steady-State Tracking
Testing was performed using the steady-state tracking designs utilizing place, fbg, and TFBG. The following figures were generated from data collected from the model. The inputs to the model, in the first test are a ramping profile in φ and a constant zero reference to θ.  Figure 6.4 shows that the place algorithm allows θ to be affected by a command in φ, while the other algorithms hold to the reference. In all cases balancing is achieved; however, Figure 6.6 shows that place introduces disturbances into Ψ. This is an example of where place had higher stability margins than fbg as seen in Table 5.4, and does not have better performance.     The model was also executed with a ramping profile in θ and a constant zero reference to φ. The voltages in Figure 6.7 show that both wheels are provided commands in the same direction which causes the robot to move forward and back.
However, it is evident that the left wheel has much more damping than the right wheel with the place algorithm. Figure 6.8 show that all three of the algorithms produce tracking system with zero steady-state error and all have approximately the same error to a ramp. Figure 6.9 shows that the place algorithm allows φ to be affected by a command in θ, while the other algorithms hold to the reference.
In all cases, balancing is achieved and a disturbance is introduced in Ψ as shown in Figure 6.10.     The steady-state tracking modeling shows that place had the worst performance. TFBG seems to show marginally better performance than fbg. Using place would cause the robot to move very sloppily because the states are highly coupled to each other. This would make it very difficult to coordinate commands in θ and φ that would result in a desired trajectory. In all cases steady-state tracking was achieved. While it is not expected that any of these designs track a ramp perfectly since they were designed for steady-state tracking, it would be desirable that they track with the least amount of error as possible.

Ramp Tracking
Testing was performed using the ramp tracking designs utilizing place, fbg, and TFBG. The following figures were generated from data collected from the model. The inputs to the model, in the first test were a ramping profile in φ and a constant zero reference to θ. Figure 6.11 shows the voltage commanded to both wheels, and as expected, the voltages are equal in magnitude and with opposite direction, which results in the robot turning. The voltage commands show that the TFBG algorithm has a faster response, which can also be seen in Figure   6.13 where TFBG has less overshoot. Figure 6.12 shows that the place and fbg algorithms allow θ to be affected by a command in φ, while the TFBG algorithm hold to the reference. In all cases balancing is achieved. However, Figure 6.14 shows that place and fbg introduces disturbances into Ψ. In this case TFBG from Table 5.4 has the best performance and stability margins.    [rad] State Ψ FBG TFBG Place Figure 6.14. Ramping φ Tracking Measured Ψ State The model was also executed with a ramping profile in θ and a constant zero reference to φ. It is expected that the voltages should have commands with equal magnitude and direction, as in Figure 6.15, for the robot to move forward and back. However, it is evident that the right wheel has much more damping than the left wheel with the place algorithm. It can also be seen that TFBG has a slower response to commands in θ, and therefore has the most overshoot to a commanded θ as seen in Figure 6.16. Figure 6.17 shows that the place and fbg algorithms allow φ to be affected by a command in θ, while TFBG algorithm holds to the reference. The state Ψ shown in Figure 6.18 indicates that during maneuvering the robot remains balanced.     Tracking a figure eight will test the controller's ability to track both φ and θ at the same time. During this testing only the design that uses TFBG will be used.
The performance testing in the previous section shows that using place would cause the system response to be very sloppy. It also shows that fbg is sloppy when performing ramp tracking even with the ramp tracking design. TFBG has better performance due to the fact that it not only optimizes phase and gain margin, it also ensures that the feedback matrix has a certain symmetry which can be seen in Tables 5.2 and 5.3. The fbg algorithm had shown this symmetry in the steady-state tracking design, which had good performance. In the ramp tracking design, fbg did not have this symmetry. This explains why fbg performed better in steady-state tracking than in ramp tracking. Therefore, since neither fbg or place guarantee the symmetry needed to get the best performance, TFBG will be used in the controller design for figure eight tracking. The figure eight will help to determine if steady-state or ramp tracking is better for hardware implementation.

Figure Eight Profile Generation
The figure eight profile generation is calculated in the commands block of the model. A command for θ and φ are calculated from the desired x m and y m . The desired x m and y m are calculated using the following equations where A x is the amplitude of x m , A y is the amplitude of y m and T is the period of the figure eight. This means that a figure eight can be commanded to remain in an area of A x × A y and complete the figure eight in 2πT seconds. In the following tests T is set to 10, A x to 50 cm and A y to 100 cm.
The locations x m and y m are transformed to θ and φ using Equation (2.4) which results in the following equations The tan −1 is calculated by using atan2 in order for all quadrants to be properly calculated. The atan2 function is bound by ± π 2 so an "unwrapper" is used so that the discontinuities in the atan2 function are removed. This is needed because the robot expects absolute position, not relative position. The 2π wrap causes the robot to get a large step in rotational position that would try to spin the robot 1 revolution very rapidly, saturating the voltage commands.
The initial angle φ is a large step. The large command takes time to settle out, so the command generator applies the initial angle φ and then waits for the system to settle before starting the figure eight commands.

Steady-State Tracking
The following plots show how the system responds to the commands to a figure eight with the TFBG steady-state design. Figures 6.20 and 6.21 clearly show that there is a delay in both θ and φ. This delay results in a figure eight that is shifted as seen in Figure 6.23. The figure eight is clearly out of the limits of ± 50 cm on the x axis and slightly out of ± 100 cm in the y axis. Figure 6.23 shows the extent of the error in both x and y. The trend seems to show that as time passes the figure eight would continue to shift to the negative x and y axis. Figure 6.22 shows that the robot will remain balanced while maneuvering the figure eight.

Ramp Tracking
The following plots show how the system responds to the commands to a figure eight with the TFBG ramp tracking design. Figures 6.26 and 6.27 show that there is very little delay in both θ and φ. There is, however, a start-up delay in θ which does cause a misalignment in time between φ and θ. This results in a slightly malformed figure eight as seen in Figure 6.29. The figure eight is out of the limits of ± 50 cm on the x axis and slightly short of the ± 100 cm in the y axis. The figure eight does have less error than the steady-state design which can be seen in Figure 6.30. This design also results in very stable balancing which can be seen in Figure 6.28.  x m vs y m    x m vs y m  offset that needs to be removed. This is accomplished by using a 10 Hz low pass filter for 1 second at the start of the program. The output of the filter is the gyro offset provided that the robot has been stationary. The filter is disabled after 1 second which holds and outputs the offset value.  is the "Controller" block shown in Figure 7.7. This is where the actual controller resides. The block contains the state-space for the additional dynamics that is enabled when the output is in range. When the additional dynamics block is disabled, it holds the last output value until it is enabled once again. The enabling of the subsystem is handled by the "Integrator Logic" which ensures that the magnitude of the voltage commands are less than the battery voltage. The block will have the robot generate a beep whenever the output voltage command is out of range. Also, the "Controller" block generates the error for θ and φ and adds the output of the −L 1 gain to the output of the additional dynamics. from the simulations so that the results can be compared.

Balancing
This test was conducted to see how well the the robot would balance in place.

Theta Ramping
In the θ ramping tests, the voltages in Figure 7.15 generally behave as expected where they have the same direction and magnitude for each wheel. The right wheel does show extra dynamics that the left wheel does not. The voltages are comparable to the voltages that can be seen in Figure 6.15 from simulation, which is a good indication that the model and the real hardware respond very similarly.
This can also be seen in Figure 7.16 where the state variable θ has an overshoot of approximately 4 radians as in Figure 6.16 of the simulation. The response of φ to a ramping θ as shown in Figure 7.18 is not ideal since φ looks to be perturbed, unlike

Phi Ramping
In the φ ramping tests, the voltages in Figure 7.23 are comparable to the voltages that can be seen in Figure 6.11. The voltages are opposite in direction and have equal magnitude for each wheel. Figure 7.25 has an overshoot of approximately 0.5 radians as in Figure 6.13 of the simulation. The response of θ to a ramping φ as shown in Figure 7.26 is not ideal since θ is perturbed, unlike     The figure eight in this test was commanded with an A x = 50 cm, A y = 100 cm and T = 10. The voltages in Figure 7.31 are comparable to the voltages that can be seen in Figure 6.25 of the simulations, except they are much noisier. Figure   7.32 shows that the commanded θ is tracked very well and is comparable to the simulation results in Figure 6.26. This is also seen in the tracking of φ when comparing Figure 7.34 to Figure 6.27 of the simulations. Figure 7.36 shows that Ψ has very little movement so the robot is well balanced. Finally, the XY plot Figure   7.38 shows how the robot moved in the XY plane. The figure eight is slightly deformed but it's deformity can be compared to Figure 6.29. This error is due to how well the states θ and φ match in time. The biggest issue is the start-up of θ which does not track for the first 3 seconds. This causes θ and φ to always be misaligned. The robot does track ramps in both θ and φ very well.     The feedback control algorithms place, fbg and TFBG were all examined.
Analysis and simulations showed that even though a feedback matrix may have better stability margins, it may not have the best performance. These algorithms were all compared to one another in both steady-state tracking and in ramp tracking. The place algorithm did not produce very good results, which has been the case in other studies [2]. TFBG showed the best results. The algorithm searches for the best stability margins for both inputs. It also searches for a desired symmetry in the feedback matrix.
The controller was able to track θ and φ, which was demonstrated in both simulation and in the hardware. The hardware and simulation showed difficulty in tracking a figure eight at high a high speed. In both simulation and hardware, slowing down the commands by changing the parameter T from 10 to 20 allowed the system to track much better. The result was a very slow moving system that tracked.