Testbed for Trajectory Control of a Two-Wheeled Robot

This thesis develops a test platform for a control problem. The inverted pendulum is selected as a well-established control problem. It is representative of an unstable nonlinear system which may remain balanced using any of several methods. Once balancing is achieved, multidimensional maneuvering is added as a supplemental control objective. To approach the control problem hardware is selected which is then characterized and simulated, and then operated while communicating operational data. The thesis provides a detailed description of the approaches to: • Selecting the hardware. • Characterizing the hardware. • Developing a functioning controller. • Simulating the results. Additionally, a modular test platform is developed such that additional control approaches or characterization models could be implemented and hot-swapped.


List of Figures
The intent of this thesis is as follows: 1. Select a well-established control problem as a focal point.
[Inverted Pendulum: Two-wheeled Robot] 2. Develop a modular test platform, such that differing control methods could be independently applied to the selected control problem, in real-time and in simulation.
[MinSeg Two-Wheeled Robot and Mathworks Software Suite] 3. Select or derive the dynamic equations for the physical model, and populate it.
[Yamamoto [1]] 4. Select the first controller design to address the selected control problem on the test platform, and implement it in simulation.
[Optimal Controller ] 5. Time permitting, implement the same controller design on the hardware.

Statement of the Problem
The two-wheeled robot is a well-established control problem. The robot is topheavy and must continually work to balance itself. The robot is able to move freely on a two-dimensional plane; however, any movements performed by the robot create additional disturbances against its ability to balance itself.
Numerous command regulator approaches ( PID, pole-placement, optimal) have been developed to control such a device; however, no one approach has been determined as a clear choice. Additional functionalities other than command regulators which significantly improve performance may also be implemented in a controller.
This study therefore intends to comparatively study multiple control approaches involving optimalcontrol-focused command regulators and to study the effects of additional functionalities which may be beneficial in general control cases. As a prerequisite to this work, a test platform must be developed for which to design the controllers. This study intends to design the test platform such that: • Studies could be performed on actual hardware.
• Studies could be performed in simulation (using a hardware-equivalent model).
• Similar work involving alternate control methods could easily be incorporated.
The intent of the latter is to significantly diminish several barriers to entry to perform a control study relating to hardware (initial implementation, interfacing/communication, and theoretical/simulation modeling).
This would ideally encourage future studies as well as draw them to a common platform, which would allow for effective comparisons between those studies.

Methodology
The methodology of this thesis is as follows: 1. Select a well-established control problem as a focal point.
[Inverted Pendulum: Two-wheeled Robot] • Select compatible hardware and software. • Implement basic hardware-software interfaces.
• Process signals input to hardware drivers.
• Process raw signals output from hardware sensors. • Establish infrastructure.
• Develop a unified, modular Simulink model which is capable of representing any desired system configuration.
[Variant subsystems used.] • Create a Matlab script hierarchy which is able to: · Configure the Simulink model to any desired system configuration.
· Configure the Simulink model to any desired build/run state.
· Organize the relatively large number of parameters involved in such a system.
· Minimize the effort required for the user to incorporate additional system configurations.
· Minimize the effort required for the user to transition between any system configurations.
• Establish robust methods of signal routing.
• Implement serial communication between hardware and development computer.
· Minimimize sampling interval within the limits of the board hardware.
· Process transmitted signals prior to sending and reconstruct after receiving.
• Develop theoretical plant model.
• Linearize the physical equations.
· Develop a state-space model.
• Acquire linear plant model parameters.
• Implement linear plant model into unified test platform.

Design and develop and controller for the test platform.
· Implement dynamic reference tracking to mitigate bias on the body angular velocity φ sensors.
· Determine control gains using LQR.

Selection of Control Problem
The focal control problem was designated to be the two-wheeled robot, a special case of the inverted pendulum.

Inverted Pendulum
In control theory, the balancing of an inverted pendulum is a well-established problem [2].
In such a problem, a rigid, column-like mass is used as a pendulum. One end of the pendulum is mounted to a motoring device. The mounted end of the pendulum is granted a degree of freedom to rotate. If the pendulum is inverted (positioned in a standing position), any disturbance will ultimately tip it such that it falls.
One such system is depicted in Figure 2.1. In this simple case, the wheels of the cart allow it to move along a one-dimensional plane (a linear path). The motoring device is used in such a setup to provide counterforces to the mounted end of the pendulum. These counterforces are intended to ultimately return the top of the pendulum to its inverted (standing) position.
For the actuator to successfully perform these actions, a controller (calculation device) is required.
The controller, with the assistance of sensory data, is able to dynamically calculate (in real time) the exact forces needed to reestablish the positioning of the pendulum to a standing equilibrium. The controller then communicates the magnitude and direction of these forces to the motoring device which actuates the forces in the physical space. This in turn changes the state of the system, requiring that the controller continually recalculate the forces needed to return to equilibrium.
This problem may be further complicated by implementing trajectory control, in which the operator may command the device to move to one or more different locations. In such a scenario, the device must maintain its control of the balance of the inverted pendulum during and after moving.

Two-Wheeled Robot
The two-wheeled robot is a special case of the inverted pendulum model. In this case, the inverted pendulum model is reduced to only the pendulum and the wheels. The entirety of the robot hardware forms the pendulum, and the pendulum is coupled directly to the wheels.
One such device is depicted in Figure 2.2. In this case, the robot is being used in a medical application. The significance of the two-wheeled robot is not related to any one application; rather its ability to balance allows the added inclusion of top-heavy architectures in design options.
The robot has two wheels, each of which is coupled to an individual motoring device (included in the robot hardware). The motoring devices are able to act independently; therefore, the device is capable of turning and moving across a two-dimensional plane. This transition from one shaft to two shafts creates additional complexity in the system which must be considered in the design of the controller.

Selection of Hardware
The selection of hardware consists of selecting: • A specific device to serve as the plant with respect to the designated control problem.
[MinSeg two-wheeled robot.] • A specific device to serve as the controller with respect to the designated control problem.
[Arduino Mega2560 single-board microcontroller, included with MinSeg two-wheeled robot.] • A specific computer to be used to interface with the controller.
[Available laptop installed with Mathworks Software Suite and supporting software.]

MinSeg M2V3 Two-Wheeled Robot
The MinSeg two-wheeled-robot was selected as the designated hardware platform due to: • Its standard inclusion of several components which are considered desirable with respect to performing a control study.
[Howard and Bushnell [5]] [This highlighted the device as a suitable hardware platform for control studies.] • Its existing driver support [for the Mathworks software environment.].
[This highlighted a significant level of support for a principal component.] • Its relatively affordable cost.
[~$300 ] Specifically, the MinSeg Model M2V3 [8] was selected as the designated hardware platform, due to: • Its standard inclusion of two [equivalent but independent] motoring axes. In Figure 2

Components
As stated in Section 2.2 the MinSeg M2V3 two-wheeled robot was selected due to its inclusion of all of the desired components to perform a control study. These components are defined in Table 2.1. Where beneficial, the components in Table 2.1 are described in greater detail in the sections which follow.

Arduino Single-Board Microcontroller
The MinSeg M2V3 is primarily built upon an Arduino Mega 2560 single-board microcontroller.
Arduino is company, project, and user-community which focuses on the development of open-source computer-hardware and software with respect to single-board microcontrollers [9]. A major boon of Arduino products is the relatively high level of support which has manifested with their popularity, including (but not limited to) the company itself, academic communities, hobbyist communities, as well as third-party private supporters such as math-software company Mathworks.
A brief comparison between the Arduino Mega 2560 and the more standard Arduino Uno is provided in

. Power Source
The MinSeg M2V3 offers two independent sources of power: • External power via a USB port • Internal power via an embedded battery holster A physical switch exists on the MinSeg device to alternate between the two modes of power sourcing.

External-Sourced Power (USB-Cable Connection)
Externally-sourcing power via the USB port offers a constant 5 [V], per the USB standard; however, the cable must be consistently connected to the robot body during use.

Internal-Sourced Power (Battery Pack)
As an alternative to externally-sourced power, power may be sourced from a battery holster embedded within the MinSeg. The battery holster permits the installation of 6 AA-sized batteries.
A typical Alkaline AA-sized battery carries 1.5 [V] at maximum charge. During use, this voltage will rapidly diminish to~1.25 [V], and more slowly diminish from then on to~1.00 [V] before rapidly becoming completely discharged, as depicted in Figure 2.8. To use the battery holster as a power source, all six AA batteries must be installed. The batteries are connected in series and therefore cumulatively offer up to 9.00 [V] when at full charge. During typical operation, the batteries will more likely offer a reduced voltage,~7.50 [V].
Therefore, sourcing power from the battery holster offers consistently greater voltage than external USB-connected sources, (so long as the batteries are not completely discharged), and additionally precludes the use of any wiring which could obstruct testing and operation.

Motor Driver
The MinSeg M2V3 uses a Texas Instruments (TI) SN754410 : Quadruple Half-H Driver chip as a motor driver. Supplementary information from the SN754410 datasheet is depicted in Figure 2.9 and Tables 2.3 -2.6.  Table 2.3 exhibit that the chip has four inputs A and four corresponding outputs Y . Table 2.5 provides a simplified description of the behavior of any one input with respect to its corresponding output: Input A acts a switch for corresponding output Y : • If the input pin A is enabled V IH , then the corresponding output Y will output V CC2 [V ].
• If the input pin A is disabled V IL , then the corresponding output Y will output 0 [V ].      (1) The algebraic convention, in which the least positive (most negative) limit is designated as minimum, is used in this data sheet for logic voltage levels.   As stated in Section 2.2.1.1, there are two DC motors. Each has a positive and negative lead.

Motor, Gearbox, and Encoder
The MinSeg implements two Lego NXT servo motors. Each Lego NXT servo motor contains a DC traction motor, a gearbox, and an encoder. A three-dimensional model of the component is depicted in Figure 2.10. Although Lego did not publicly disclose all of the characteristic parameters of their components, the hobbyist community reverse engineered several of these values by performing various tests and also by (irreversibly) dismantling a spare [16]. The dismantled component is depicted in Figure 2.11.   Eqn. [2.1], where k represents a ratio and n represents an integer count.
The number of teeth in each gearing is depicted in Figure 2.12. Teeth counts in the same row are coupled by teeth. Teeth counts in the same column are coupled by axle, (and therefore rotate at the same rate, independent of teeth count).

Development PC
Designations pertaining to the development PC with respect to the test platform are exhibited in

Alternative Operating System Compatibility
Although the macOS operating system was used, alternative operating systems (Windows and/or Linux ) would be equally acceptable.
Such a transition would primarily require an alternative Mathworks-supported compiler [18] which would be compatible with the new operating system. Slight alterations to the method of determining the test platform serial communication channelwould also be required.
It is not expected that such a transition would be preventatively difficult.

Designated Hardware-Interfacing Software
The Mathworks Software Suite was selected as the designated hardware-interfacing software due to: • Its first-party support for programming real-time hardware.
• Its first-party support for simulating real-time hardware.
• Its first-party driver support for Arduino-brand microcontrollers.
• Its third-party driver support for the MinSeg.
• Its first-party support for serial communication with hardware in real-time.
• Its relatively user-friendly language and interfaces.
• Its relative commonality among students and academic institutions.

 
The software environment was already relatively familiar to the author and to the advising professor prior to performing this study.
  • Its relatively affordable cost.

Selection of a Hardware Model
The physical plant model developed in Yamamoto [1] was used, due to: • Use of state variables involving: -Body pitch angle α -Body yaw angle Ψ -Wheel angle θ • Existing familiarity of the work by the advising professor.
• Existing knowledge of methods to measure nonintuitive model parameters by the advising professor.
The physical plant model is discussed in greater detail in Section ??.

Selection of Controller Design
Since pole-placement methods had been researched relatively recently under the advising professor, optimal control techniques were researched.
This is discussed in greater detail in Section 5.

Test Platform
The test platform consists of the designated hardware, [ The Simulink model is capable of: • Acting as an algorithm with which to program the hardware, such that it may: • Simulate an equivalent model of "the hardware when loaded with the same algorithm".
The Matlab script hierarchy is capable of: • Initialize model parameters.
• Initialize a build or simulate event.
• Initialize a read or write event.
• Post-process raw read data.
• Save processed read data as well as other configuration data.
• Plot processed read data. The Simulink model is hierarchical. The sections which follow will describe the model and will be similarly organized, as depicted in the extended-precision List of Contents below.

Root
The top level of the model, also known as the model root, is depicted in Figure 3.1.
The model root is contains the three primary components of the system:  This grants the user the ability to call any significant signal wherever it is needed using bus selectors,     • Reformatting the raw hardware-output data on receipt.

Variant Subsystems
• Saving the initialization parameters and output data.
• Plotting the output data.
The script is hierarchal, and is therefore only the root or master file to a series of subfiles. The subfiles are broken up into principal segments of the scripting process:

Hardware-Equivalent Dynamics Model
To simulate the dynamics of the hardware, a hardware-equivalent dynamics model was selected.
The model was originally derived by Yamamoto [1] and has been successfully used in other control studies [19]. • All mass geometries are uniform.
• All masses are uniformly distributed.
• The hardware consists of three principal masses: · A rectangular cuboid Tables 4.1 -4.2, define the variables and the parameters, respectively, that the physical model of the two-wheeled inverted pendulum will use.

Two-Wheeled Inverted Pendulum Model
NXTway-GS can be considered as a two wheeled inverted pendulum model shown in Figure

NXTway-GS Modeling
This chapter describes mathematical model and motion equations of NXTway-GS.

Two-Wheeled Inverted Pendulum Model
NXTway-GS can be considered as a two wheeled inverted pendulum model shown in Figure

Nonlinear model
The dynamic motion equations of the two-wheeled robot are derived using the Lagrangian method.
The equations are based on the coordinate system provided in Figure 4.2.

Coordinate System
The coordinate system is explicitly defined in Equations (4.1) -(4.6).

Differential Equations
After creating a nonlinear model using the Lagrangian method, and then linearizing that model,  .7) corresponds to wheel angular position θ and body pitch φ x .

State-Space Representation
The general form of state-space representation is exhibited in Equation (4.23).
The designated x states and p inputs are exhibited in Equations (4.24) -(4.25).
The derivation of indices for the system matrices A and B which are nonintuitive are derived from Equations (4.7) -(4.18). in Equations (4.26) -(4.27).
Note that K 1.ẍ must be invertible to perform the second step in in Equation (4.26). The derivation for matrix invertibility and the proof that K 1.ẍ is nonsingular [and is therefore invertible], are exhibited in Equations (4.28) -(4.31).
The A matrix and the state vector x are exhibited in Equation (4.32).
The B matrix and the input vector u are exhibited in Equation (4.33).
The C matrix and the state vector x are exhibited in Equation (4.34).
The D matrix and the input vector u are exhibited in Equation (4.35).
were publicly available [5,16], or were intuitive to obtain [Example: l h , l w , l d ].
Methods to determine those parameters which were not considered easily obtained are defined in the following sections.
The moment of inertia of the body with respect to pitch, is assumed to be sufficiently equivalent to the moment of inertia of "an ideal thin rectangular plate with length l h , width l w = 0, an axis of rotation at one end of the plate".
This relation is exhibited in Equation (4.36).
The moment of inertia of the body with respect to pitch, is assumed to be sufficiently equivalent to the moment of inertia of "an ideal thin rectangular plate with length l h , width l w = 0, an axis of rotation at one end of the plate".
This relation is exhibited in Equation (4.37).
The length from the body center of mass to the body axis of rotation l b.c2a may be determined using more than one method.

Yamamoto Method
As seen in Figure 4.1 [on page 38], Yamamoto [1] assumes that the geometries of the wheels and the body are uniform. He also assumes that the masses of these geometries are uniform. He therefore defines length from the body center of mass to the body axis of rotation l b.c2a , as exhibited in Equation (4.38) Since the geometries of the actual hardware are assumed to significantly deviate from the assumption of uniform mass distribution, an alternative method is instead used to calculate length from the body center of mass to the body axis of rotation l b.c2a , as exhibited in Equation (4.38) If the hardware is mounted at both wheel axles along the axis which is shared by both wheel axles, and if the hardware is given a degree of freedom to rotate about the wheel axle axis, without rotating the actual wheel axles, then the hardware may be lifted slightly and then released to swing freely like a pendulum along that axis.
Allowing the hardware to freely swing like a pendulum along the wheel axle axis significantly simplifies the dynamic equations of motion of the hardware. Furthermore, if friction at the newly added mount coupling points is negligible, then there will not be a need to model and implement the friction into the dynamics equations.
If the hardware is freely swung like a pendulum along the wheel axle axis as described above, then the relations exhibited in Equations (4.39) -(4.40) become true. .
Of the two resulting relations in Equation (4.41), the former cannot be true while the hardware is in motion; thus, the latter is selected, as depicted on the right with a left-facing arrow.
The coefficient term, abbreviated as k w , is expanded in Equation (4.43). It may be expanded further with the use of Equation (4.36), as exhibited in Equation (4.44).

Harmonic Oscillator
Notably, the selected relation in Equation (4.41) This allows for the relation of the abbreviated term representing the system dynamics, k w , to the natural angular frequency of the hardware [a pendulum] ω p , as is exhibited in Equation (4.46).
This proves significant since ω p represents the angular frequency of the pendulum, which is a measurable value, and since k ω includes the desired unknown term l b.c2a . [All other terms are known]. The relation may rewritten to solve for length from the body center of mass to the body axis of rotation l b.c2a , as is exhibited as Equation (4.47).
Control Design

Additional Dynamics
Additional dynamics may be incorporated into a state feedback regulating system in order to beneficially alter the response of the system in various respects.

Background
A state feedback regulating system is depicted in Figure 5 Thus, the larger system [which includes the plant and the additional dynamics] may temporarily be considered as new plant, as depicted in Figure 5.3. It may therefore be expressed as a single state-space representation containing both systems. Thus, state-feedback regulation techniques may be used to control the system; however, the system response will now include any benefits which the additional dynamics provide. A representation of the larger system is depicted in Figure 5 From this point, the system depiction may be rearranged such that the input to the controller is on the left, while the plant outputs remain on the right. A sequential description of the process, and the figure number which corresponds to each step is provided below: 1. The feedback gains are separated into those with respect to the original plant outputs x and those with respect to the additional dynamics x.a.
[ Figure 5.5] 2. The gains are shifted such that they are forward facing.
[ Figure 5.6] • In this configuration, the original plant may be separated from the components used to control it, including the additional dynamics.
[ Figure 5.7] 3. The plant is shifted to the right side of the system depiction.

Reference Signal
Recall that a standard state-feedback regulator simply brings its inputs, [in this case, system states x and x a ], to zero. If it is desired that a controller input be brought to a value other than zero, a reference signal may be implemented.
In these cases, rather than input the controller with a state which the controller will bring to zero, the controller is input with the difference between the state value and the reference [desired] value.
This difference is commonly known as the error signal. Once the error signal is brought to zero for a given state, the state will be equivalent to the desired reference value.
Returning to system depiction, a reference command is implemented, as depicted in Figure 5.9.
Note that the reference signal receives the negative. Inverting the system state alters the system equation, and could cause the system to become unstable.
Despite this fact, it is sometimes more common to see the system state subtracted from the reference signal. To correctly achieve this, once the reference signal is implemented, either side of the difference Recall that all state-space representations are linear; therefore, the input and the output may be multiplied by the same value. In this case, the negative may be divided out on both sides.
These changes are depicted in Figure 5.10.

Tracking System
Additional dynamics may be incorporated to improve reference tracking. When implemented for this purpose, the additional dynamics are known as a tracking system.
In the case of a tracking system, an integrator may be implemented as the additional dynamics to track a constant reference exactly, or to track a slowly varying reference approximately.
Integrators are also able to mitigate constant disturbances. Incidentally, the MinSeg M2V3 system uses gyroscopes as body angular velocity ψ sensors. Bias is inherent in the output of a gyroscope; therefore, the use of such an integrator as a tracking system has an additional benefit: it will mitigate the effects of bias from a gyroscope output, whether directly or within terms which are derivative of the gyroscope output.

Discrete Additional Dynamics
Since the additional dynamics will be processed on a microcontroller, the additional dynamics will be digital; thus, a continuous-to-discrete conversion will be necessary. An integrator is an established case which is exhibited in Equation (5.2).

Control Gains
Once the additional dynamics are established, state feedback gains must be calculated. Multiple methods exist to calculate these gains. The most established methods involve optimization or poleplacement.

Optimal
Several optimal control techniques exist [20]. This section will focus on linear quadratic regulation techniques.

Implementation
In order to determine the feedback gains of the system, the state-space representation of the system,

[the plant and the additional dynamics], is input into a discrete linear quadratic regulator gain-calculation
Matlab function, dlqr, which outputs state-feedback gains which best minimize the quadratic cost function. The Matlab function also requires quadratic cost function matrices Q and R as inputs.
The quadratic cost matrices Q and R were determined through trial and error; however, some constraints existed. The Q and R matrices were both diagonal matrices; [thus, all indices which are not on the diagonal are equal to zero]. Also, the R matrix was left as an identity matrix until the Q matrix established desirable behavior. Once desirable behavior was established, the option of multiplying the R matrix by a scalar value [greater than one] became a consideration.
Multiplying the R matrix by a scalar value decreases the response time of the controller; however, this also decreases the peak magnitude of the control output, [in this case, motor voltage]. While a decreased response time is generally undesirable, the reduction of the control output can be necessary in certain circumstances. For example, the maximum permissible value for the control output, motor voltage, is limited by the nominal voltage provided by the hardware power source.

Results: Simulation
To demonstrate the capabilities of the device, a dynamic command is provided which attempts to move the device in the shape of an eight 8 on the ground while maintaining balance.
Additionally, the device starts at a body angular position (pitch) φ x of 0.03 [rad]. This represents the inability to start the device at a perfect angle. This causes additional transients in the initial milliseconds of operation.  While moving in accordance with the dynamic commands provided to the wheel angular position θ and body angular position (yaw) φ y states, the robot remains balances and does not deviate significantly from the commands at any time.

Future work
The following is a non-comprehensive list of potential future work which could be performed to improve the capabilities of the test platform or supplement the control studies already performed on the test platform.
• Optimize sample interval of hardware.
• Determine limiting factors in the reduction of the board sampling interval.
• Improve upon these factors, if possible.
• Determine alternative model algorithms such that processing (per sample interval) is significantly minimized.
• Implement bluetooth wireless transmission.
• Determine limiting factors in the reduction of the serial transmission interval.
sample interval.
This is already performed when sending a high number of signals, but improved performance has not been verified.
· Determine if increasing the BAUD frequency on the board will remove limits on the serial transmission interval.
• Determine how to begin a read without resetting the hardware.
• Determine if dynamic/real-time plotting is worthwhile. If so, implement.
[Where N is an integer or infinity.] • Implement a nonlinear plant model.
• Demonstrate operation in nonlinear states.
Example: Operating in a state with a significantly increased component of horizontal pitch.
• Improve model parameters measurements.
· Use scale with improved precision.
[Angular velocity vs. Input Voltage] Hardware must remain perfectly upright while in motion to perform this measurement.
The original measurement was taken while balancing the in-motion device by hand.
Since a pitch controller has been developed, the measurement may be taken more accurately.
· Reduce number of batteries to less than 6.
[Requires use of USB cable for power.] • Alternate geometry.
[Search for Lego tires with differing radius, mass, and/or coefficient of friction. See [21] ] · Incorporation of a second mass on the pendulum.
• Perform movement on an uneven surface.
• Determine tradeoffs between no filter vs 1 st to 6 th order bessel filters.
• Determine tradeoffs between state-space and transfer function blocks, if any.
• Determine tradeoffs between Matlab besself and bessel poles, if any?
• Optimization of observable data • Implement voltage sensor across battery holster.
Use this voltage reading to determine the true voltage of the power source in operation.
• Incorporate use of accelerometer?
Incorporate use of Kalman filter?
Compare effects.
• Test Windows and Linux compatibility.
If the board cannot complete all of its processes before the sampling interval completes, then it performs incorrectly. Detection of this is possible and desirable for the user.
Currently, overrun detection requires that the the user manually view an LED on the board.
The LED is very small and almost entirely masked by the bluetooth module.
(Simulink also currently prevents status reads of the overrun LED pin.) An alternative method should exist which alert the user more conveniently.      2 p l a n t . s u p p l y . mode = u i . p l a n t . s u p p l y . mode ; 3 p l a n t . dynamics . mode = u i . p l a n t . dynamics . mode ; 4 5 p l a n t . n . b a t t e r i e s = u i . p l a n t . n . b a t t e r i e s ;

24
' B a t t e r y power i s e n a b l e d ( p l a n t . s u p p l y . mode == 0 ) ; \ n ' . . .      21 % p l a n t . motor . m = 0 . 2 2 0 * k . l b 2 k g ; % ( q u a n t i t y : 1 ) [ kg ] 22 % p l a n t . b a t t e r y . m = 1 . 0 0 0 * k . l b 2 k g ; % ( q u a n t i t y : 1 ) [ kg ] [ kg ] 26 27 % p l a n t . body .m = p l a n t . board .m . . . 28 % + p l a n t . motor .m * 2 . . . 29 % + p l a n t . motorCable .m * 2 . . . 30 % + p l a n t . b a t t e r y .m * p l a n t . n . b a t t e r y . . . p l a n t . body . w . n a t u r a l = 2 * p i * p l a n t . body . f . n a t u r a l ; 64 % n a t u r a l a n g u l a r f r e q u e n c y [ rad / s ] 65 66 p l a n t . body . l . c = 3 * ( a . g r a v i t y − p l a n t . body . w . n a t u r a l^2 * p l a n t . wheel . r ) . . .

67
/ ( 4 * p l a n t . body . w . n a t u r a l^2 ) ; 69 70 p l a n t . body . J . x = p l a n t . body .m * p l a n t . body . l . c^2 . . . 144 p l a n t . q ( 1 , 1 ) = p l a n t . n e t . m * p l a n t . wheel . r^2 + p l a n t . wheel . J ; 145 p l a n t . q ( 2 , 1 ) = p l a n t . body .m * p l a n t . wheel . r^2 * p l a n t . body . l . c ; 146 p l a n t . q ( 3 , 1 ) = p l a n t . body .m * p l a n t . body . l . c^2 + p l a n t . wheel . J ; 147 p l a n t . q ( 4 , 1 ) = mtr . k . t o r q u e * mtr . k . dlambda / mtr . R + mtr . k . f r i c t i o n ; 148 p l a n t . q ( 5 , 1 ) = p l a n t . body .m * a . g r a v i t y * p l a n t . body . l . c ; 149 p l a n t . q ( 6 , 1 ) = mtr . k . t o r q u e / mtr .R ; 150 151 p l a n t .Q{ 1 , 1 } = [ +p l a n t . q ( 1 ) +p l a n t . q ( 2 ) +p l a n t . q ( 2 ) +p l a n t . q ( 3 ) ] ; 153 p l a n t .Q{ 2 , 1 } = 2 * [ +p l a n t . q ( 4 ) −p l a n t . q ( 4 ) 154 −p l a n t . q ( 4 ) +p l a n t . q ( 4 ) ] ; 155 p l a n t .Q{ 3 , 1 } = [ +0 +0 156 +0 −p l a n t . q ( 5 ) ] ; 157 p l a n t .Q{ 4 , 1 } = [ +p l a n t . q ( 6 ) +p l a n t . q ( 6 ) 158 −p l a n t . q ( 6 ) −p l a n t . q ( 6 ) ] ; 159 160 % body . t h e t a . y ( yaw ) ( p h i ) 161 p l a n t . r ( 1 , 1 ) = p l a n t . body . l . w / p l a n t . wheel . r ; 162 163 p l a n t .R{ 1 , 1 } = 0 . 5 * p l a n t . wheel .m * p l a n t . body . l . w^2 . . .

165
+ 0 . 5 * p l a n t . r ( 1 )^2 * p l a n t . wheel . J ; 166 p l a n t .R{ 2 , 1 } = 0 . 5 * p l a n t . r ( 1 )^2 * p l a n t . q ( 4 ) ; 236 237 %% [ I n i t ] : P l a n t S t a t e Space Model : C, D 238 239 p l a n t . C = e y e ( s i z e ( p l a n t .A ) ) ; 240 p l a n t .D = z e r o s ( s i z e ( p l a n t . A, 1 ) , s i z e ( p l a n t . C, 2 ) ) ;    ' mdl . mode '

12
' mdl . c a s e ' 13 ' mdl . T . sample ' 14 15 ' p l a n t . dynamics . mode ' 16 ' p l a n t . s u p p l y . v '