Final Report

Columbia University
Mechanical Engineering
Engineering Design: MECE E3420 & MECE E3430
Fall 2017 - Spring 2018
CatBot: The Self-Righting Robot

Final Report
May 3, 2018
Team 1: The Cat in the Hat

Wing-Sum Law, Robert Kydd, Fabian Stute, Francesco Zampetti
Table of Contents
Table of Contents 1
1. Executive Summary 2
2. Introduction 2
3. Background Information 3
4. Design 4
4.1 Analysis 4
4.1.1 One Dimensional Model and Simulation 5
4.1.2 Three Dimensional Model and Simulation 8
4.2 Assembly Details 12
4.2.1 Structure 12
4.2.2. CAD 12
4.2.3. Electronics 13
4.3 Experiments and Results 15
4.3.1 One Dimensional Experimental Results 15
4.3.2 Three Dimensional Experimental Results 18
5. Conclusions 20
References 20
Appendix 21
A1. One Dimensional Controller Simulation Code 21
A2. Rotation Matrix 24
A3. Three Dimensional Controller Simulation Code 24
A4. Electronics Schematic 30
A5. Video Link 30
A6. One Dimensional Controller Code 31
A7. Three Dimensional Controller Code 34
A8. CAD Drawings 38
1
1. Executive Summary
Conventional orientation adjustment systems rely on aerodynamics and/or using
thrusters, however we wanted to design a system that uses spinning masses to rotate the system
to a desired orientation. We aimed to adjust the attitude of a robot without relying on air
resistance or drag, by spinning up and down steel flywheels to correct and counter any initial
angular displacement or velocity. The design of our system is constrained to both a
1-dimensional (1D) and 3-dimensional (3D) case where the robot is only allowed to rotate
around one and two axes respectively. Inertial measurement units (IMUs) measure the exact
angular orientation of the robot to calculate in real time the corrections that need to be made to
ensure the robot rotates to a desired angle. Similar systems are used in satellites and spaceships
to make fine adjustments to keep them on course. Our robot represents a proof of concept,
proving that flywheels are a viable option to make quick near-real time attitude adjustments with
a completely self-contained system. Due to motor torque constraints, though we were able to
correct the robots orientation in one dimension, the timescale was still rather unsatisfactory. This
may be due to the bearing friction that our 1D constraining jig imposed on the robot. We also
faced issues with weight imbalances that would need to be corrected for future iterations by
altering the geometry and weight distribution of the robot to make it more uniform. This issue
was particularly prominent in the 3D case, which would require weight redistribution in the
future to make it work.
2. Introduction
Orientation detection and correction are topics of importance in an era of rapidly
evolving transportation technology. Unmanned vehicles of all kinds – marine, aerial, etc. – all
require the ability to self-correct orientation and trajectory in order to effectively navigate
without human input. From technical standpoint, orientation correction is not only an interesting
research topic in and of itself, but it also facilitates research into other fields. Space and sea
exploration horizons expand with the use of unmanned vehicles, since vehicles can go further
than any human can safely. For business purposes, with all of the hype surrounding self-driving
cars, it is essential that safe, effective reorientation technology is available at a commercial level
of success and price.
Our design uses a flywheel mechanism. This is in contrast to many of the mechanisms we
found for rapid orientation correction, most of which involved a swinging, weighted tail.1, 2 The
flywheel mechanism is of particular interest for orientation maintenance and processes that
experience less drag or can happen on shorter time scales, such as satellite adjustment in space or
marine vehicle stabilization.3-5 This particular design is important because the flywheel
mechanism tends to be more compact and close to body center of mass than the tail mechanism,
and it has a lower risk of parts snapping off than in a cantilevered tail situation. Thus we
2
constructed a robot with two flywheels – one for adjusting pitch and one for adjusting roll – in
order to both correct and maintain orientation about two axes. Our mechanism would be most
useful in marine or aerial vehicles, as discussed above.
We have multiple levels of success as our goals for this design project. Our first three
goals concentrate on success about one axis: (1) adjust orientation about one axis, (2) correct
orientation about one axis from rest, and (3) correct orientation about one axis from initial
angular velocity. The next three goals essentially mirror the first three goals, but about two axes:
(4) adjust orientation about two axes, (5) correct orientation about two axes from rest, and (6)
correct orientation about two axes from initial angular velocity. The main challenges associated
with accomplishing our objectives are centered about unexpected and unmeasurable factors such
as disturbances during testing and inexact parameter estimation (e.g. bearing friction, machining
asymmetries). An added complication about two axes is that the system becomes nonlinear,
making controls a challenge.
3. Background Information
The best way to alter an object’s orientation midair is to swinging some mass in the
opposite direction of the desired rotation. An initial internet search reveals two useful methods of
doing this. One is to use flywheels, which are giant cylinders (to maximize their moment of
inertia) which can be spun up to alter the rotation of a given object. The other common option is
a more biologically inspired approach which aims to mimic the way lizards and other animals
spin their tails to right themselves in midair. Attaching a large weight to the end of a long rod
which can be rotated has also been found to be an effective way of manipulating an object’s
orientation in midair, which is also similar to the tail mechanisms. In addition we found that
many space shuttles and satellites use similar principles to make small adjustments in orientation
and help the given object maintain a steady orientation.
A series of cursory as well as in-depth searches were performed to attempt to identify
mechanisms that achieve the same objectives as our robot. Internet searches for “self-righting
robot in mid-air” turned up a number of examples of external reorientation and stabilization
mechanisms similar to the tales of lizards. A team at Berkeley used a 2DOF tale mechanism to
achieve mid-air reorientation in pitch and roll.1 Continuing with the search of self-righting robots
yielded a paper from the IEEE journal about a robotic cube from a ETH Zurich team that uses an
internal set of momentum wheels to stabilize itself in 3-axes.6 Our team decided to use an
internal reorientation mechanism. Our main concern with an external mechanism such as a tail is
that our robot will often be dropped to the ground, and there is a serious danger of a tail
snapping. Thus an internal mechanism is more attractive.
Once we had decided to pursue an internal mechanism we looked into existing patented
mechanisms. A 1988 patent by Philip A. Studer served as the basis of research for the patent
searches.7 His system employs a three-flywheel system coupled with three sensors for the yaw,
3
pitch, and roll axes to control the attitude of body stabilized vehicles such as spacecrafts or
satellites. Like the robotic cube, this system uses motors to drive flywheels and generate reaction
torques. The categories for this patent included 244/164 - Attitude Control Patents and 244/165-
Attitude Control by Gyroscope or Flywheel Patents. Conducting a category search yielded a
1996 patent by Thomas Holmes8 about a similar attitude control device for spacecraft; however,
this patent incorporates a tetrahedral system of four wheels as opposed to the three used in other
patents and papers. Three smaller flywheels are orientated orthogonally to one another whereas a
larger momentum wheel sits at the point of intersection of their axes to maintain gyroscopic
stiffness. The four wheels also provided a level of redundancy that will allow the system to
continue to effectively operate even in the event of the failure of one of the wheels. Given that
our project only needs to correct in pitch and roll (not yaw), it is excessive to have all four
flywheels. In the end, we decided just to use the two flywheels necessary to correct about the two
axes.
4. Design
4.1 Analysis
Our design criteria for both the 1D and 3D formulation of this problem are as follows:
● Body angle should settle in less than 5 seconds
● Body angle should be less than 10º at steady state
● Motor torque must stay under 97.1 mNm
From the above design criteria, we conducted a similar analysis procedure for both the
one dimensional problem and the three dimensional problem. First, from first principles
governing equations we determined the equations describing our system. Using these equations,
a state space system was formulated, along with a linear quadratic regulator (LQR).10 We then
simulated this system using Python 3 with the Python Control Systems Toolbox and the SciPy
Signal Processing Toolbox. The details for both one dimension and three dimensions are
described below.
4
4.1.1 One Dimensional Model and Simulation
Figure 4.1: The one-dimensional system with positive rotation defined
Our first level of success involves just correcting around one axis (Figure 4.1). This is a
very easy linear system with a three by one state vector:
(4.1)
(4.2)
In the linear system described above, θ represents part angle, I represents motion of
inertia, T describes motor torque, and B represents bearing friction, for which we have used the
value provided in Gajamohan et al. (2012)6 in our simulations. For all of the calculations, we
made the assumption that air drag was negligible and that our flywheel axis goes through the
body center of mass. We also assumed that the mass is balanced about the body center of mass,
and thus the flywheel axis. While the latter assumption is likely to be inaccurate, our controller
should correct for that problem.
In order to use an LQR, we must first confirm that our system is controllable using a
controllability matrix:
C = [B AB A2 B ... An−1 B] (4.3)
where n is the number of state variables – in our case, three. For a system to be controllable, the
rank of the controllability matrix should be equal to the number of state variables.
Our controllability rank is three, which means we can proceed with an LQR. An LQR
determines an input u that minimizes the following cost function, which can be found in most
standard control systems lecture notes, such as Murray (2006):
5
(4.4)
In equation (4.4), Q weights the difference between the states and the desired state, and R
weights the input. For the Q matrix, what matters is the relative weights. We are interested in
correcting body angle and angular velocity, but do not care what the final wheel angular velocity
is. We are also much more concerned with final body angle rather than final body angular
velocity. Thus we gave body angle, body angular velocity, and wheel angular velocity weights of
1, 0.05, and 0, respectively. We also have a hard constraint of motor torque being less than 97.1
mNm, so we gave R a value of 15 to limit motor torque.
The value P 1 is found by solving the Ricatti ODE shown in equation (4.5).
− Ṗ = P A + AT P − P BR−1 B T P + Q (4.5)
We use the results of this to determine the static gain K of our controller such that our input
u(t) =− R−1 B T P x =− K x . The Python code used to accomplish these tasks can be found in A1.
LQR assumes full state feedback. Technically, this is a requirement that our system
satisfies, but we are anticipating errors that stem from geometrical simplifications and the
presence of disturbances from those at the expo. Thus, we have added an estimator for the
purpose of estimating system trajectory, but with poles that settle much faster than the system
itself (6 times faster than the system’s slowest pole). This will allow us to correct based on state
estimation error. Below in Figure 4.2 is a block diagram illustrating this setup.
Figure 4.2: Block diagram for one dimensional case with linear quadratic recorder and estimator.
One of the disadvantages of LQR is that it returns a static gain. Given how fast the
calculation is to determine the final state (about 0.005 s), we may be able to combat this by
performing the calculation multiple times while the robot is airborne.
Use of an estimator is described by the following equation:
6
x̂˙ = Aˆx + B u + L(y − ŷ ) (4.6)
where the hat denotes the estimated state. Matrix L is determined by placing the poles
determined as settling 6 times faster than those of the actual system. The final state space form of
the system to be simulated, with e = ẋ − x̂˙ being error, is as follows:
(4.7)
We determined our theoretical static gain K and simulated the system coming from
multiple initial conditions using the parameters detailed in Table 4.1 below.
Parameter Value
Bearing Friction, B 0.05 × 10−3 kg m2 s−1
Body Moment of Inertia, I w 0.319 × 10−3 kg m2
Wheel Moment of Inertia, I B 0.005 kg m2

Table 4.1: Parameter values for one-dimensional simulation
These parameters, when subject to the calculations described in the above section,
resulted in a gain matrix K = [− 0.258 − 0.077 0 ] and an estimator value L = 0.367 .
Results of simulation using code in A1 are shown below in Figure 4.3.
(a) (b) (c) (d)

Figure 4.3: Simulation results for (a) unactuated motor and actuated motor cases with initial
conditions (b) body at 180º and angular velocity 0 º/s, (c) body at 180º and angular velocity at
-180 º/s, and (d) body at 0º and angular velocity at -180 º/s
7
Figure 4.3 demonstrates the sufficiency of our control system in one dimension. All
systems have more or less settled in under one second, and the final angle error for all three cases
was less than 3º. However, it is clear that our motors are not sufficiently strong enough to
achieve such fast settling times. Motor torque often exceeded the maximum torque of 97.1 mNm
throughout the tests. This means that for the actual testing, we will have to more heavily penalize
torque in our cost function in order to safely use our motors. Model adjustment and controller
iteration is described in Section 4.4. Finally, as expected, with no motor torque, the systems
position changes linearly with no change in angular velocity, getting further away from its
desired orientation.
4.1.2 Three Dimensional Model and Simulation
Figure 4.4: Three dimensional problem with inertial frame of reference depicted in gray and
body frame of reference depicted in blue. Degrees of freedom in each frame are depicted in the
corresponding colors.
Our robot is composed of a body – containing electronics – attached via motors to two
flywheels, which will be activated to correct the robots orientation. In order to determine the
correct controller for our project, we first describe the dynamics of the system. Our formulation
of system of dynamics uses Lagrangian mechanics, i.e. an energy method:
(4.8)
L=T −V (4.9)
where L is the Lagrangian, T is the kinetic energy of the system, V is the potential energy of
the system, and q is the degrees of freedom. Since both frames of reference are attached to the
body center of mass, we are concerned only with body rotational energy and wheel translational,
rotational, and potential energy. Assembled are the following system equations:
8
(4.10)
(4.11)
where I is moment of inertia, m is mass, and r is the vector connecting each flywheel center of
mass to the center of mass of the robot. The subscripts B and w represent the body and
flywheel, respectively, and the subscripts 1 and 2 denote the flywheel about the body frame
x-axis and y-axis, respectively (Figure 4.4). As pictured in Figure 4.4, θ represents rotations in
the body frame of reference, while γ , β , and α represent rotations in the inertial frame of
reference. The trigonometric functions in equation 4.11 are part of the rotation matrix that rotates
vectors from the inertial frame of reference to the body frame of reference, and vice versa
(A2.1).
Although technically the system has degrees of freedom about the flywheel axes, we are
primarily interested in θBx , θBy , and θBz for correcting orientation. Using equation 4.8 at an
equilibrium position of γ , β , and α = 0 , we determine the equations:
(4.12)
(4.13)
(4.14)
in which we use viscous bearing friction just as we did in the one-dimensional case.
Then using the rotation matrix (A2.1), we can create a state space system that will also
return γ , β , and α . Our goal is only to correct γ and β (roll and pitch, respectively) to zero.
We rearranged equations 4.12 through 4.14, and used the rotation matrix (A2.1) to formulate the
following system equations:
(4.15)
9
Control for the three dimensional problem is far more complicated than that of the one
dimensional problem. This is because with the Coriolis effect, the three dimensional problem
becomes nonlinear. Control for nonlinear systems is quite tricky, especially for one as
complicated as ours. We have decided again to use LQR, since it allows for trajectory tracking.
However, LQR can only be used on linear systems. So we will need to linearize our system using
the following A and B matrices:
(4.16)
where ẋ = f (x, u) and xe and ue are state and input at some equilibrium value. Our state and
input vectors are as follows:
(4.17)
The jacobians that result from equation 4.16 are extremely complex and large, so we have
instead decided to include the numerical values given our assumed parameters in Table 4.2
below:
Matrix Value
Table 4.2: State and input matrices as determined from linearization
Using the same procedure described in equations 4.3 through 4.7 (albeit with larger
matrices), we determined the LQR gain and estimator matrices shown in Table 4.3 below:
Matrix Value
10
Table 4.3: LQR gain (K) and estimator (L) matrices determined using the procedure described in
4.2.1
The gains calculated in Table 4.3 use the following inertial properties:
Parameter Value
Body Moment of Inertia, I w

k g m2
Wheel Moment of Inertia, I B 0.005 kg m2

Table 4.4: Three dimensional parameters
As in one dimension, we assumed that the flywheel axes are aligned with the body center
of mass and associated body frame of reference. Further we assume that the only mass imbalance
is from the flywheels. This assumption is unlikely to hold up as well in multiple dimensions due
to a variety of factors such as machining and assembly error, and we anticipate having to correct
our model for it after running some actual experiments. Nevertheless, we ran a preliminary set of
simulations to assess the controller at a basic level.
(a) (b) (c)

Figure 4.5: Simulation results for [γ β γ˙ β] ˙ = (a) [90º 0º 0 º/s 0 º/s] , (c)
[45º 45º 0 º/s 0 º/s] , and (d) [0º 9 0º 0 º/s − 180 º/s] . Gray bands represent less than
10º of final error.
11
Figure 4.5 demonstrates the controller’s ability to theoretically bring the system to an
acceptable amount of error within one second, but as with the one dimensional case, the motor
torque is far out of the acceptable range. So again, for the actual testing, we will have to more
heavily penalize torque in our cost function in order to safely use our motors. Furthermore, we
expect to have to extend the time allowed for the system to settle: rather than one second, it may
be closer to five. Model adjustment and controller iteration in response to experimental results is
described in Section 4.4.
4.2 Assembly Details
4.2.1 Structure
We wanted our robot to be modular, easy to assemble, and easy to create. We decided to
aim for a roughly cylindrical shape, the flywheels had to be mounted at 90 degrees to each other
so that they can control the robots orientation in pitch and roll. In terms of ease of manufacturing
we switched the material for the frame from aluminum to acrylic so that we were able to laser cut
each of the panels and to also reduce the weight of the frame by roughly 50%. Laser cutting the
panels is cheap and quick which allowed us to also iterate through various designs. The whole
robot is held together with a threaded rod with a nut on either side that tightens to clamp the four
side panels between the two end caps to hold each of them in place. The only parts that we had to
machine were the flywheels because they needed to have a large moment of inertia while being
as light as possible. TO mount the flywheels on the motor shaft we also drilled and tapped holes
into a shaft collar.
The cumulative cost of the three-dimensional system $635. The two motors and their
respective controllers make up 66% of this total. As the design is not manufacture intensive, a
human-work hours are limited to the time to machine the steel flywheels (2 hours per wheel)
and laser-cut the acrylic components (~30 minutes). Accounting for times to tap holes, screw
together components, and soldering adds an additional 12-14 hours of work. This sums to
roughly 20 hours to manufacture the robot.
12
4.2.2. CAD
Figure 4.6: Full exploded assembly
Figure 4.7: Detailed view of motor sub assembly
Detailed CAD drawings can be viewed in Appendix A8.
4.2.3. Electronics
Raspberry Pi 3 Model B+
Our microprocessor of choice is the Raspberry Pi 3 model B+. The 1.2GHz processor
speed of the Pi 3b made it a preferable choice to Arduino or BeagleBone devices. The quad-core
processor also allows us the option to potentially run multiple calculations (like for each of the
dimensions) concurrently if we have the time to do so. Vast online documentation for the Pi also
made it an appealing choice for debugging. The GPIO (general-purpose input/output) pins on the
model 3 are almost twice as responsive as the previous model. It also has 1 GB of LPDDR2 ram
13
which would be useful for collecting and processing large amounts of data rapidly which was
one of our primary criteria.
IMU
The Inertial Measurement Unit (IMU) is an essential component to the robot. It measures
linear and angular motion of the system using gyroscopes and accelerometers. For our purpose,
the angular velocity and acceleration as well as linear acceleration are necessary for the robot
functioning as desired. Based upon desired testing conditions, angular velocity will not exceed
±1000 °/sec and linear acceleration 3g. These conditions would only be reproduced for higher
level of success cases. Level 1 assumes no initial angular velocity. In addition, data output rates
are vital to consider. The IMU selected offers an accelerometer output of 1.0kHz and gyroscope
output of 8.0kHz as well as meeting the desired testing condition requirements. One IMU is
used for data measurementation. This is fed to the Raspberry Pi.
Motors
We opted for the Maxon EC 45 flat Ø42.8 mm, brushless, 50 Watt, with Hall sensors. For
our project we need to maximize the torque output of our motors, a higher torque means more
control of the angular velocity of the robot. For our purposes one of the major constraints is
power consumption because we didn’t want to have to plug our robot into the wall because
running a wire to the robot would drastically affect the movement of our robot.
Because of the precise control needed to alter our robots attitude in midair, we opted for a
brushless DC motor with hall sensors for accurate positional control. We reviewed the brushless
DC motor offerings from each of the approved vendors and found that none of them quite
matched our specifications. After browsing the Maxon website we determined the EC 45 flat
Ø42.8 mm motor to be our ideal choice. An EC motor (Electronically Commutated) is just
another name for a brushless DC motor. It has a flat form factor which allows us to pack the
internals of the robot more tightly reducing the moment of inertia for our robot and this reducing
the torque requirements. 18 V also satisfied our power requirements because it can be run off a
LiPo Battery. Most importantly it had a relatively high torque output (97.1 mNm) with a top
speed of 5190 rpm.
Motor Controllers
Our motor controller is the ESCON Module 24/2, 4-Q servo controller for DC/EC
motors, 2/6 A, 10-24 VDC, it is recommended to be bundled with the motor that we purchased
and is also sold by Maxon. It is rated to control DC motors up to 24V and also comes with
software to fine tune the intricacies of the controller as well as in depth documentation on the
pinout for a variety of cases.
14
LiPo Battery
The robot power requirements will vary significantly between nominal and peak. At rest,
the power usage will be due to the sensors and Raspberry Pi. The two 18.5V motors operating at
full power represent the majority of the power usage for the system. The battery needed to be
sized to have enough capacity for the robot but also deliver enough power at an instant to power
all the components at peak load. A lithium polymer battery (LiPo) was the obvious choice. LiPo
batteries offer a good weight to capacity ratio and high discharge rates--ideal for a system
constrained by weight and power consumption.
LiPo Voltage (V) Capacity (mAh) Discharge Rate (A)
Pulse FPV 18.5 (5S) 1350 101.25 (75C)

Table 4.5: Specifications of the LiPo battery
Buck Converter
A mini-360 buck converter was used to step-down the voltage from the LiPo battery to
power the Raspberry Pi. The converter is rated for 23V and was used to convert 21V from the
fully charged LiPo cells down to 5V.
4.3 Experiments and Results

For both one dimension and three dimensions, we designed a rig to hold the robot while
allowing it to move freely about the degrees of freedom that the controller corrects. We then
chose multiple starting conditions, and allowed the robot to correct itself. During experiments,
we collected orientation, angular velocity, and input torque data. Using that data with the models
described in Section 4.2 we simulated from initial conditions with input torque over time to
determine whether model output was comparable to experimental results. Below are the details
for both one dimensional and three dimensional results.
4.3.1 One Dimensional Experimental Results
Figure 4.8: One dimensional set up
15
Both body moment of inertia and bearing friction for the model had to be adjusted up as a
result of the added material beyond the original design in the final set up (Figure 4.8). We
increased model bearing friction to 0.075 × 10−2 kg m2 s−1 and model body moment of inertia
to 0.007 kg m2 .
(a) (b)
Figure 4.9: One dimensional experimental results (orange dots) from two different initial
conditions. Simulated results were obtained by inputting the torque from experiments (bottom
row) into the model described in Section 4.2.1 (blue lines). From top to bottom, the rows show
body angle, body angular velocity, and motor torque.
From Figure 4.9a it is clear that there is a weight balance issue with the robot, in which a
torque in one direction is applied for longer durations than torque in the other direction in order
to maintain a constant body angle. The actual body angle in Figure 4.9a remains around 0º, while
the simulated body angle steadily rotates as a result of the uneven torque application in the two
different directions. However, as seen in Figure 4.9b, model parameters in terms of bearing
friction and moment of inertia are relatively accurate, since the timing and magnitude of
oscillations in body angle and body angular velocity generally match between experimental and
simulated results.
Experiment (a) Experiment (b)
RMS difference θb (º) 52.77 6.08
16
RMS difference θ̇ b (º/s) 258.99 46.83
Table 4.6: RMS difference between experimental results and simulated results for the two
experiments shown in Figure 4.9. RMS differences were calculated for the time between 2-4 sec,
to negate the effects of code start up and cessation.
Quantitatively, as seen in Table 4.6, Experiment (b) much more closely matched between
experimental and simulated results. We consider an RMS error 6.08º between the two to be
acceptable, given that we considered anything less than 10º of error at the end of our experiments
to be correctly oriented.
However, as a result of the weight balance issue and more extreme starting position in
Experiment (a), the RMS differences were much, much larger. This error can be partially
attributed to the assumption we made in simulation that the axis of the flywheel coincides with
the center of mass of the body and that the moment of inertia is evenly distributed about that
center of mass. The rest of the error can be attributed to the error correction that occurs in actual
experiments. Since the estimator part of our control scheme (described in Section 4.1.1) corrects
for the error in the actual experiment, the actual robot corrects to the correct angle using the
torque from in the bottom row of Figure 4.9. However, without the measurement error in
simulation (since there is no measurement), the experimental torque as an input results in a
lopsided result.
In the one dimensional case, we have met our goal of correcting the body within 10º of
horizontal. It does not happen as rapidly as we would like it to, as a result of the limited motor
torque with which we are working combined with the increased viscous friction from the one
dimensional jig. We conclude from our one dimensional experiments that this current design
works more or less as fast as possible with the motor torque we have available. A link to videos
of the robot operating in both one dimension and three dimensions can be found in Appendix A5,
and a copy of the controller code can be found in Appendix A6.
17
4.3.2 Three Dimensional Experimental Results
Figure 4.10: Three dimensional set up
As a result of the coriolis effect, adding a second axis around which to correct requires
that the robot be free to move in all directions. To allow this to happen without having to actually
toss the robot into the air, we mounted it inside of a polycarbonate sphere and allowed it to roll
around on the ground. It was immediately clear qualitatively that our motor torque was
insufficient to completely reorient the robot to vertical. Although the motors were torquing in the
correct direction to correct the orientation of the robot, they simply were not strong enough to
actual complete robot reorientation.
From Figure 4.11, it is clear that there are start up effects where although the motor
torque remains high, the experimental body angle does not change much. There are a couple of
factors that could explain this. For one, the model described in Section 4.1.2 does not include
rolling friction or static friction between the sphere and the ground, since the model does not
include a spherical case at all. Furthermore, there isn’t an element of body translational kinetic
energy in the system described in Section 4.1.2, since we originally imagined the robot moving
only about its center of mass.
18
Figure 4.11: From top to bottom: roll angle, pitch angle, torque about roll, and torque about pitch
for a three dimensional experiment. Experimental results are noted as orange dots, and simulated
results are the solid blue lines.
Though the motors’ power ended up being insufficient to reorient the robot, it seems as
though the dynamic system described in Section 4.1.2 was, at least at a high-level, accurate. The
factors described in the above paragraph would have to be added to the model to make it more
accurate for its final form, but generally speaking the motors actuated in the correct directions to
reorient the robot. The motor power insufficiency was exacerbated by the weight imbalance of
the robot, since the whole thing tended toward the side that had both of the flywheels present.
One final issue that became apparent with actual experimental testing is that there may be
some cross-talk between the IMU and the two motors being actuated. This is apparent from the
way that the body angle measurements are noisier for the three dimensional case than the one
dimensional case. Continuing the project would require either a filter for the data built into the
control scheme, or rewiring of the entire system to eliminate as much of the noise in the readings
as possible. A link to videos of the robot operating in both one dimension and three dimensions
can be found in Appendix A5, and a copy of the controller code can be found in Appendix A7.
19
5. Conclusions
Our first three goals concentrate on success about one axis: (1) adjust orientation about
one axis, (2) correct orientation about one axis from rest, and (3) correct orientation about one
axis from initial angular velocity. The next three goals essentially mirror the first three goals, but
about two axes: (4) adjust orientation about two axes, (5) correct orientation about two axes from
rest, and (6) correct orientation about two axes from initial angular velocity. We accomplished
goals 1 through 4 in a fairly complete sense, being able to fully correct orientation within 10º in
one dimension and also having a complete robot that adjusts itself about two axes. Goals 5 and 6
were only partially accomplished, since the robot attempted to correct its orientation about two
axes but was ultimately unable to, being limited by the maximum torque of the motors we chose.
There are multiple improvements to be made for both the 1D and the 3D case. In the 1D case, the
biggest issue was weight balance about the flywheel axis. The first improvement to be made in
the 1D case would be to make the flywheel axis align with the center of mass of the actual body,
which could be accomplished by redistributing the weight within the body of the robot.
Improvements to the 3D case are a little more complicated. For one, the noise from the
IMU measurements would need to be cleaned to make controller calculations accurate.
Furthermore, we found that the motors’ maximum torque were insufficient for correcting the
orientation in three dimensions with the weight imbalance issues that we were having. A big next
step for this project would be to do two pairs of coupled flywheels that are balanced across the
center of mass of the body of the robot, which would alleviate the weight imbalance issues and
potentially allow the motors to completely correct the robot’s orientation, since the robot would
no longer tend so heavily to one side. With these changes, the success of the robot may be able to
complete goals 5 and 6.
References
[1] Chang-Siu, E., Libby, T., Brown, M., Full, R. J., & Tomizuka, M. (2013). A nonlinear feedback
controller for aerial self-righting by a tailed robot. In 2013 IEEE International Conference on
Robotics and Automation (pp. 32–39). https://doi.org/10.1109/ICRA.2013.6630553
[2] Zhao, J., Zhao, T., Xi, N., Cintrón, F. J., Mutka, M. W., & Xiao, L. (2013). Controlling aerial
maneuvering of a miniature jumping robot using its tail. In 2013 IEEE/RSJ International Conference
on Intelligent Robots and Systems (pp. 3802–3807). https://doi.org/10.1109/IROS.2013.6696900
[3] Hyneman, J. (2000, September 5). Remote control device with gyroscopic stabilization and directional
control. Retrieved from https://patents.google.com/patent/US6458008B1/en
20
[4] Perkel, H., & Comerford, W. (1967, January 27). Control system for spinning bodies. Retrieved from
https://patents.google.com/patent/US3591108A/en
[5] Townsend, N. C., Murphy, A. J., & Shenoi, R. A. (2007). A new active gyrostabiliser system for ride
control of marine vehicles. Ocean Engineering, 34(11), 1607–1617.
https://doi.org/10.1016/j.oceaneng.2006.11.004
[6] Gajamohan, M., Merz, M., Thommen, I., & D’Andrea, R. (2012). The Cubli: A cube that can jump up
and balance. In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (pp.
3722–3727). https://doi.org/10.1109/IROS.2012.6385896
[7] Studer, P. (1985, November 7). Three axis attitude control system. Retrieved from
https://patents.google.com/patent/US4732353A/en
[8] Holmes, J. (1996, July 15). Spacecraft control system with a trihedral momentum bias wheel
configuration. Retrieved from https://patents.google.com/patent/US5826829A/en
[9] Murray, R. M. (2006, January 11). Lecture 2 – LQR Control. California Institute of Technology.
Appendix
A1. One Dimensional Controller Simulation Code

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#*******************************************************************
# File: mod1d.py
# Name: Wing-Sum Law
# UNI: wal2116
#
# Run 1d model of self-righting robot
#*******************************************************************
import numpy as np

import control.matlab as cm
import matplotlib.pyplot as plt
import scipy.signal as sig
import time
def main():
# Timer
st = time.time()
# Params
b = 0.05 * 10**(-3) # bearing friction, kg m^2/s
J_w = 0.319 * 10**(-3) flywheel inertia, kg m^2
#
21
J_b = 5 * 10**(-3) # body inertia, kg m^2
# Sim times
end = 1
dt = 0.01
# ICs
theta = np.pi
dtheta = 0
dtheta_w = np.pi
x0 = np.hstack((theta,dtheta,dtheta_w,0,0,0))
x0n = np.hstack((theta,dtheta,dtheta_w))
x = []
# Original state space matrices

A = np.matrix([[0, 1, 0],[0, 0, 0],[0, 0, -b/J_w]])
B = np.matrix([[0],[-1/J_b],[1/J_w]])
C = np.matrix([[1, 0, 0],[0, 1, 0],[0, 0, 1]])
D = np.matrix([[0],[0],[0]])
# Weights for LQR (Q weights states, R weights inputs)

Q = np.array([[1, 0, 0],[0, 0.05, 0],[0, 0, 0]])
R = 15
# Calculate LQR gain

K, S, E = cm.lqr(A, B, Q, R)
Knp = np.matrix(K)
# Find observer/estimator poles

Ac = A - B*Knp
poles = np.linalg.eig(Ac)[0]
slowest = min(abs(poles))
# Estimators
P = -6*np.array([slowest,slowest,slowest]) - np.array([0,1,2])
L = (sig.place_poles(A.T, C.T, P).computed_poles).T

Lnp = np.matrix(L)
# New state space matrices w/ observer & LQR gain

Aco = np.vstack((np.hstack((A-B*Knp, B*Knp)),np.hstack((np.zeros(np.shape(A)),
A-Lnp*C))))
Aco = np.matrix(Aco)
Bco = np.vstack((B, np.zeros(np.shape(B))))
Bco = np.matrix(Bco)
Cco = np.hstack((C, np.zeros(np.shape(C))))
Cco = np.matrix(Cco)
22
Dco = np.matrix([[0],[0],[0]])
# Create state space object using control.matlab

sys = cm.ss(Aco, Bco, Cco, Dco)
# Create state space object no control

sysn = cm.ss(A, B, C, D)
# Time and input – when simulating input is r=0 but actual torque should be -Kx
t = np.arange(0,end,dt)
r = np.zeros(np.size(t))
# Linear sim w/o control

[yn,tn,xn] = cm.lsim(sysn,r,t,x0n)
# Linear sim w/control

[y,t,x] = cm.lsim(sys,r,t,x0)
# Calculate torque applied

u = np.einsum('ij,ji->i',-Knp, y[:,0:3].T)
calct = time.time() - st
# Convert output to degrees

th_b = np.degrees(y[:,0])
dth_b = np.degrees(y[:,1])
th_bn = np.degrees(yn[:,0])
dth_bn = np.degrees(yn[:,1])
# Plot no input results

fig0,(ax00,ax10,ax20) = plt.subplots(3,1)
fig0.set_size_inches((4,6),forward=True)
ax00.plot(t[:],th_bn)
ax00.set_ylabel('Body Angle (deg)')
ax00.set_title(r'$\theta_b$')
ax10.plot(t[:],dth_bn)
ax10.set_ylabel('Angular Velocity (deg/s)')
ax10.set_title(r'$\dot{\theta}_b$')
ax20.plot(t[:],r)
ax20.set_ylabel('Torque (mNm)')
ax20.set_xlabel('Time (sec)')
ax20.set_title('Motor Torque')
plt.tight_layout()
plt.show()
23
# Plot results
fig1,(ax0,ax1,ax2) = plt.subplots(3,1)
fig1.set_size_inches((4,6),forward=True)
ax0.plot(t[:],th_b)
ax0.set_ylabel('Body Angle (deg)')
ax0.set_title(r'$\theta_b$')
ax1.plot(t[:],dth_b)
ax1.set_ylabel('Angular Velocity (deg/s)')
ax1.set_title(r'$\dot{\theta}_b$')
ax2.plot(t[:],u[:]*1000)
ax2.set_title('Motor Torque')
plt.tight_layout()
plt.show()
print('End Error')
print(th_b[-1])
print('\nCalculation Time')
print(calct)
main()
A2. Rotation Matrix

The following matrix can be used to transform from the inertial frame of reference to the
body frame of reference for calculating moment arms.
(A2.1
)
The inverse rotation matrix is simply its transpose.
A3. Three Dimensional Controller Simulation Code

# Wing-Sum Law
# 4/17/2018
# Lagrangian try 2
import numpy as np
24
import numdifftools as nd
import scipy.signal a s sig
# for my sanity
def s(x):
return np.sin(x)
def c(x):
return np.cos(x)
# Some values
T1 = 0
T2 = 0
F1 = [-1.0843888715389727, 0.022705981325841555, 0.36500648388028184]
F2 = [0.40207073732124293, -1.705514724407551, 0.452132757415739]
r1 = [0.031+0.021, 0.0, -0.002]
r2 = [0.0, 0.037+0.021, -0.002]
rm1 = np.dot(r1,r1)
rm2 = np.dot(r2,r2)
I = [0.007,0.007,0.005]
I1 = [0.414*10**(-3),0.208*10**(-3),0.208*10**(-3)]
I2 = [0.208*10**(-3),0.414*10**(-3),0.208*10**(-3)]
mb = 1.832
mw = 0.183372
g = 9.8
b = 0.05 * 10**(-3) # bearing friction, kg m^2/s
def rotatemat(gamma, beta, alpha):

'''Returns rotation matrix between body frame and inertial frame based on
states'''
R11 = np.cos(alpha)*np.cos(beta)
R12 = np.cos(alpha)*np.sin(beta)*np.sin(gamma) - np.sin(alpha)*np.cos(gamma)
R13 = np.cos(alpha)*np.sin(beta)*np.cos(gamma) + np.sin(alpha)*np.sin(gamma)
R21 = np.sin(alpha)*np.cos(beta)
R22 = np.sin(alpha)*np.sin(beta)*np.sin(gamma) + np.cos(alpha)*np.cos(gamma)
R23 = np.sin(alpha)*np.sin(beta)*np.cos(gamma) - np.cos(alpha)*np.sin(gamma)
R31 = -np.sin(beta)
R32 = np.cos(beta)*np.sin(gamma)
R33 = np.cos(beta)*np.cos(gamma)
R = np.array([[R11, R12, R13],[R21, R22, R23],[R31, R32, R33]])

return R
# Find Jacobian
25
fun = lambda x: np.r_[ c(x[2])*c(x[1])*x[3] + s(x[2])*c(x[1])*x[4] - s(x[1])*x[5], \
(c(x[2])*s(x[1])*s(x[0]) - s(x[2])*c(x[0]))*x[3] +
(s(x[2])*s(x[1])*s(x[0]) \
+ c(x[2])*c(x[0]))*x[4] + c(x[1])*s(x[0])*x[5], \
(c(x[2])*s(x[1])*c(x[0]) + s(x[2])*s(x[0]))*x[3] +
(s(x[2])*s(x[1])*c(x[0]) \
- c(x[2])*s(x[0]))*x[4] + c(x[1])*c(x[0])*x[5], \
(-b/(I[0] + mw*rm1 + mw*rm2))*x[3] + (1/(I[0] + mw*rm1 +
mw*rm2))*u[0] + \
(c(x[1])*c(x[0])*(r1[1]*mw+r2[1]*mw) - (s(x[2])*s(x[1])*c(x[0]) - \
c(x[2])*s(x[0]))*(r1[2]*mw+r2[2]*mw))*g/(I[0] + mw*rm1 + mw*rm2), \
mw*rm2))*u[1] + \
((c(x[2])*s(x[1])*c(x[0]) + s(x[2])*s(x[0]))*(r1[2]*mw+r2[2]*mw) - \
c(x[1])*c(x[0])*(r1[0]*mw+r2[0]*mw))*g/(I[1] + mw*rm1 + mw*rm2), \
((s(x[2])*s(x[1])*c(x[0]) - c(x[2])*s(x[0]))*(r1[0]*mw+r2[0]*mw) - \
(c(x[2])*s(x[1])*c(x[0]) + \
s(x[2])*s(x[0]))*(r1[1]*mw+r2[1]*mw))*g/(I[2] + mw*rm1 + mw*rm2) ]
funu = lambda u: np.r_[ c(x[2])*c(x[1])*x[3] + s(x[2])*c(x[1])*x[4] - s(x[1])*x[5], \

(c(x[2])*s(x[1])*s(x[0]) - s(x[2])*c(x[0]))*x[3] +
(s(x[2])*s(x[1])*s(x[0]) \
+ c(x[2])*c(x[0]))*x[4] + c(x[1])*s(x[0])*x[5], \
(c(x[2])*s(x[1])*c(x[0]) + s(x[2])*s(x[0]))*x[3] +
(s(x[2])*s(x[1])*c(x[0]) \
- c(x[2])*s(x[0]))*x[4] + c(x[1])*c(x[0])*x[5], \
mw*rm2))*u[0] + \
(c(x[1])*c(x[0])*(r1[1]*mw+r2[1]*mw) - (s(x[2])*s(x[1])*c(x[0]) - \
c(x[2])*s(x[0]))*(r1[2]*mw+r2[2]*mw))*g/(I[0] + mw*rm1 + mw*rm2), \
mw*rm2))*u[1] + \
((c(x[2])*s(x[1])*c(x[0]) + s(x[2])*s(x[0]))*(r1[2]*mw+r2[2]*mw) - \
c(x[1])*c(x[0])*(r1[0]*mw+r2[0]*mw))*g/(I[1] + mw*rm1 + mw*rm2), \
((s(x[2])*s(x[1])*c(x[0]) - c(x[2])*s(x[0]))*(r1[0]*mw+r2[0]*mw) - \
(c(x[2])*s(x[1])*c(x[0]) + \
s(x[2])*s(x[0]))*(r1[1]*mw+r2[1]*mw))*g/(I[2] + mw*rm1 + mw*rm2) ]
x = [0,0,0,0,0,0]
u = [0,0]
A = np.matrix(nd.Jacobian(fun)(x))
B = np.matrix(nd.Jacobian(funu)(u))
C = np.matrix(np.eye(6))
D = np.matrix(np.zeros((6,2)))
26
Q = np.matrix([[0.6, 0, 0, 0, 0, 0],\
[0, 0.6, 0, 0, 0, 0],\
[0, 0, 0.01, 0, 0, 0],\
[0, 0, 0, 0.05, 0, 0],\
[0, 0, 0, 0, 0.05, 0],\
[0, 0, 0, 0, 0, 0.05]])
R = np.matrix(([12, 0],[0, 12]))
K, S, E = cm.lqr(A, B, Q, R)
Knp = np.matrix(K)
# Find observer/estimator poles

Ac = A - B*Knp
poles = np.linalg.eig(Ac)[0]
slowest = min(abs(poles))
# Estimators
P = -6*np.ones((1,6))*slowest - np.arange(6)
P = P[0]
L = (sig.place_poles(A.T, C.T, P).computed_poles).T

Lnp = np.matrix(L)
# New state space matrices w/ observer & LQR gain

Aco = np.vstack((np.hstack((A-B*Knp, B*Knp)),np.hstack((np.zeros(np.shape(A)),
A-Lnp*C))))
Aco = np.matrix(Aco)
Bco = np.vstack((B, np.zeros(np.shape(B))))
Bco = np.matrix(Bco)
Cco = np.hstack((C, np.zeros(np.shape(C))))
Cco = np.matrix(Cco)
Dco = D
np.savez('lagrange_sys',Aco,Bco,Cco,Dco)
np.save('LQRgain',Knp)
# Wing-Sum Law
# 4/17/2018
# Simulate lagrangian try 2 sysdot
import numpy as np

import matplotlib.pyplot as plt
import time
def main():
27
st = time.time()
# Initial state
x0 = np.hstack((0,np.pi/2,0,-np.pi,0,0,0,0,0,0,0,0))
x = []
# Sim times
end = 1
dt = 0.01
# Load sys from file

sysfile = np.load('lagrange_sys.npz')
A = sysfile['arr_0']
B = sysfile['arr_1']
C = sysfile['arr_2']
D = sysfile['arr_3']
# Load gain
K = np.load('LQRgain.npy')

sys = cm.ss(A, B, C, D)
r = np.zeros((np.size(t),2))
# Linear sim w/control


u = np.einsum('ij,jk->ki',-K, y[:,0:6].T)
# Convert output to degrees

g = np.degrees(y[:,0])
b = np.degrees(y[:,1])
# Plot results
fig,(ax0,ax1,ax2,ax3) = plt.subplots(4,1)
fig.set_size_inches((4,8),forward=True)
ax0.plot(t[:],g)
ax0.set_ylabel('Roll (deg)')
ax0.set_title(r'$\gamma$')
ax1.plot(t[:],b)
ax1.set_ylabel('Pitch (deg/s)')
28
ax1.set_title(r'$\beta$')
ax2.plot(t[:],u[:,0]*1000)
ax2.set_title('Motor 1 Torque')
ax3.plot(t[:],u[:,1]*1000)
ax3.set_title('Motor 2 Torque')
plt.tight_layout()
plt.show()
print('End Error')
print(g[-1])
print(b[-1])
print(calct)
main()
29
A4. Electronics Schematic
A5. Video Link

Both one dimensional and three dimensional operation can be viewed at the following link:
https://drive.google.com/drive/u/0/folders/1QPUf2EPtwusk25e7y8ilCNAsJGA18ndq
Should viewing permission be an issue, please email wal2116@columbia.edu to request it.
30
A6. One Dimensional Controller Code
Below is the code used to control the actual robot in one dimension. File with controller
parameters was produced by the code described in Section A1.
# -*- coding: utf-8 -*-
"""
Wing-Sum Law
4/18/2018
Simulate 1d model
need to run "sudo pigpiod" before running code
"""
import os.path
import pigpio
import RPi.GPIO as GPIO
from datetime import datetime
from mpu6050 import mpu6050
import numpy as np
import time
import math
GPIO.setmode(GPIO.BOARD)
imu = mpu6050(0x68)
pi = pigpio.pi()
filename = "/home/pi/data_log.csv"
file = open(filename, "w")
file.write("Time,Output Torque,Accelerometer X, Accelerometer Y, Accelerometer Z,
Gyroscope X, Gyroscope Y, Gyroscope Z\n")
def main():
u = [0]
# 13 High, 15 Low makes motor go CW
# 15 High, 13 Low makes motor go CCW
GPIO.setup(13, GPIO.OUT)
GPIO.setup(15, GPIO.OUT)
# Get motor speed (output from controller through ADC)

GPIO.setup(29, GPIO.IN)
31
# Sim times
end = 0.01
dt = 0.005
# ICs
theta = getAngle(0)
dtheta = getAngular()
dtheta_w = 0
x0 = np.hstack((theta,dtheta,dtheta_w,0,0,0))
x = []
# Load gain
K = np.load('1dGain.npy')
Knp = np.matrix(K)
sysfile = np.load('1d_sys.npz')
Aco = sysfile['arr_0']
Bco = sysfile['arr_1']
Cco = sysfile['arr_2']
Dco = sysfile['arr_3']

sys = cm.ss(Aco, Bco, Cco, Dco)
r = np.zeros(np.size(t))
th_b = x0[0]
while(th_b != 0):
st = time.time()
Linear sim w/control

#
take in x_actual i.e. measure [thb dthb dthw]^T

#
x_a = [getAngle(u[0]), getAngular()] #your measurement here
e = x_actual - y[:,0:3]
#
e = x_a - y[-1,0:2] # no wheel v
# calculate again, with x0 = [x_actual e]^T (should be 6x1)
32
x0 = np.hstack((x_a,y[-1,2],e,0))
th_b = y[-1,0]

u = np.einsum('ij,ji->i',-Knp, y[:,0:3].T) # Nm
setMotorSpeed(u[0]*1000)
print(calct)
def getAngle(torque):
# Returns accelerometer data from IMU
current = imu.get_accel_data()
currentG = imu.get_gyro_data()
file.write(str(datetime.now())+","+str(torque)+","+str(current["x"])+","+str(current[
"y"])+","+str(current["z"])+","+str(currentG["x"])+","+str(currentG["y"])+","+str(cur
rentG["z"])+"\n")
if(current["z"] == 0):
return -np.sign(current["y"])*np.pi/2
elif(current["z"] > 0):
return
math.atan(current["y"]/current["z"])-(np.sign(current["y"])*np.pi)
else:
return math.atan(current["y"]/current["z"])
def getAngular():
# Returns angular velocity of the body (deg/s)
current = imu.get_gyro_data()
return math.radians(current["x"])
def setMotorSpeed(torque):
# Max motor torque is 97.1 mNm
if abs(torque) > 97.1:
torque = np.sign(torque) * 97.1
if torque > 0:
GPIO.output(16,0)
GPIO.output(18,1)
elif torque < 0:
GPIO.output(18,0)
GPIO.output(16,1)
else:
GPIO.output(18,0)
GPIO.output(16,0)
pi.hardware_PWM(12,100,int(abs(torque)*1000000/97.1))
try:
main()
33
except (Exception,KeyboardInterrupt) as e:
print(e)
GPIO.output(13,0)
GPIO.output(15,0)
pi.hardware_PWM(12,0,0)
file.close()
A7. Three Dimensional Controller Code

Below is the code used to control the actual robot in three dimensions. File with controller
parameters was produced by the code described in Section A3.
# -*- coding: utf-8 -*-
"""
Wing-Sum Law
5/2/2018
Run 3d
need to run 'sudo pigpiod' before running code
"""
import os.path
import pigpio
import RPi.GPIO as GPIO
from datetime import datetime
from mpu6050 import mpu6050
import numpy as np
import time
import math
GPIO.setmode(GPIO.BOARD)
imu = mpu6050(0x68)
pi = pigpio.pi()
filename = "/home/pi/data3D_log.csv"
file = open(filename,'w')
file.write('Time,Motor 1 Torque, Motor 2 Torque,Accelerometer X,Accelerometer
Y,Accelerometer Z,Gyroscope X,Gyroscope Y,Gyroscope Z\n')
def main():
u = [0,0]
# 13 High, 15 Low makes motor go CW
15 High, 13 Low makes motor go CCW
#
GPIO.setup(13,GPIO.OUT)
34
# sim times
end = 0.01
dt = 0.005
# ics
g, b, a = getAngle(u)
dg, db, da = getAngular()
x0 = np.hstack((g,b,a,dg,db,da,0,0,0,0,0,0))
x = []
# Load sys from file

sysfile = np.load('lagrange_sys.npz')
A = sysfile['arr_0']
B = sysfile['arr_1']
C = sysfile['arr_2']
D = sysfile['arr_3']
# Load gain
K = np.load('LQRgain.npy')

sys = cm.ss(A, B, C, D)
r = np.zeros((np.size(t),2))
while(g != 0 or b != 0):

st = time.time()
Linear sim w/control

#
# Calc torque applied

u = np.einsum('ij,jk->ki',-K,y[:,0:6].T)
setMotorSpeed(u[0,:]*1000)
# get x_actual i.e. [g,b,a,dg,db,da]^T

x_a = np.hstack((getAngle(u[0,:]),getAngular()))
e = x_a-y[0:6]
#
e = x_a - y[-1,0:6]
35
calc again
#
x0 = np.hstack((x_a,e))
g, b = y[-1,0:2]
print(calct)
def getAngle(torque):
print(torque)
# Returns accelerometer data from IMU
current = imu.get_accel_data()
currentG = imu.get_gyro_data()
file.write(str(datetime.now())+","+str(torque[0])+","+str(torque[1])+","+str(current[
"x"])+","+str(current["y"])+","+str(current["z"])+","+str(currentG["x"])+","+str(curr
entG["y"])+","+str(currentG["z"])+"\n")
gamma = -np.sign(current["y"])*np.pi/2
gamma = math.atan(current["y"]/current["z"])-(np.sign(current["y"])*np.pi)
else:
gamma = math.atan(current["y"]/current["z"])
beta = -np.sign(current["x"])*np.pi/2
beta = math.atan(current["x"]/current["z"])-(np.sign(current["x"])*np.pi)
else:
beta = math.atan(current["x"]/current["z"])
if(current["x"] == 0):
alpha = -np.sign(current["y"])*np.pi/2
elif(current["x"] > 0):
alpha = math.atan(current["y"]/current["x"])-(np.sign(current["y"])*np.pi)
else:
alpha = math.atan(current["y"]/current["x"])
return [gamma,beta,alpha]
def getAngular():
# Returns angular velocity of the body (deg/s)
current = imu.get_gyro_data()
return
[math.radians(current["x"]),math.radians(current["y"]),math.radians(current["z"])]
36
def setMotorSpeed(torque):
ug, ub = torque
# Max motor torque is 97.1 mNm
if abs(ug) > 97.1:
ug = np.sign(ug) * 97.1
if abs(ub) > 97.1:
ub = np.sign(ub) * 97.1
if ug > 0:
GPIO.output(15,0)
GPIO.output(13,1)
else:
GPIO.output(13,0)
GPIO.output(15,1)
if ub > 0 :
GPIO.output(16,0)
GPIO.output(18,1)
else:
GPIO.output(18,0)
GPIO.output(16,1)
pi.hardware_PWM(12,100,int(abs(ug)*1000000/97.1))
pi.hardware_PWM(18,100,int(abs(ub)*1000000/97.1))
try:
main()
except (Exception,KeyboardInterrupt) as e:
print(e)
GPIO.output(16,0)
GPIO.output(18,0)
GPIO.output(13,0)
GPIO.output(15,0)
file.close()
37
A8. CAD Drawings
38
39
40
41
42

Final Report

Hochgeladen von

Dokumentinformationen

Originaltitel

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

Final Report

Hochgeladen von

Copyright:

Verfügbare Formate

Columbia University

CatBot: The Self-Righting Robot

Team 1: The Cat in the Hat

Figure 4.1: The one-dimensional system with positive rotation defined

Bearing Friction, B 0.05 × 10−3 kg m2 s−1

Body Moment of Inertia, I w 0.319 × 10−3 kg m2

Wheel Moment of Inertia, I B 0.005 kg m2

(a) (b) (c) (d)

4.1.2 Three Dimensional Model and Simulation

Table 4.2: State and input matrices as determined from linearization

Body Moment of Inertia, I w

Wheel Moment of Inertia, I B 0.005 kg m2

(a) (b) (c)

4.2 Assembly Details

Figure 4.6: Full exploded assembly

Figure 4.7: Detailed view of motor sub assembly

Detailed CAD drawings can be viewed in Appendix A8.

LiPo Voltage (V) Capacity (mAh) Discharge Rate (A)

Pulse FPV 18.5 (5S) 1350 101.25 (75C)

4.3 Experiments and Results

4.3.1 One Dimensional Experimental Results

Figure 4.8: One dimensional set up

Experiment (a) Experiment (b)

RMS difference θb (º) 52.77 6.08

Figure 4.10: Three dimensional set up

A1. One Dimensional Controller Simulation Code

import​ numpy ​as​ np

​# Original state space matrices

​# Weights for LQR (Q weights states, R weights inputs)

​# Calculate LQR gain

​# Find observer/estimator poles

L = (sig.place_poles(A.T, C.T, P).computed_poles).T

​# New state space matrices w/ observer & LQR gain

​# Create state space object using control.matlab

​# Create state space object no control

​# Linear sim w/o control

​# Linear sim w/control

​# Calculate torque applied

​# Convert output to degrees

​# Plot no input results

A2. Rotation Matrix

The inverse rotation matrix is simply its transpose.

A3. Three Dimensional Controller Simulation Code

import​ numpy ​as​ np

def​ rotatemat(gamma, beta, alpha):

R = np.array([[R11, R12, R13],[R21, R22, R23],[R31, R32, R33]])

funu = ​lambda​ u: np.r_[ c(x[2])*c(x[1])*x[3] + s(x[2])*c(x[1])*x[4] - s(x[1])*x[5], \

# Find observer/estimator poles

L = (sig.place_poles(A.T, C.T, P).computed_poles).T

# New state space matrices w/ observer & LQR gain

# Simulate lagrangian try 2 sysdot

import​ numpy ​as​ np

​# Load sys from file

​# Create state space object using control.matlab

​# Linear sim w/control

​# Calculate torque applied

​# Convert output to degrees

A5. Video Link

Should viewing permission be an issue, please email ​wal2116@columbia.edu​ to request it.

​# Get motor speed (output from controller through ADC)

​# Create state space object using control.matlab

​ Linear sim w/control

​ take in x_actual i.e. measure [thb dthb dthw]^T

import numpy as np

# Original state space matrices

# Weights for LQR (Q weights states, R weights inputs)

# Calculate LQR gain

# Find observer/estimator poles

# New state space matrices w/ observer & LQR gain

# Create state space object using control.matlab

# Create state space object no control

# Linear sim w/o control

# Linear sim w/control

# Calculate torque applied

# Convert output to degrees

# Plot no input results

import numpy as np

def rotatemat(gamma, beta, alpha):

funu = lambda u: np.r_[ c(x[2])c(x[1])x[3] + s(x[2])c(x[1])x[4] - s(x[1])*x[5], \

import numpy as np

# Load sys from file

# Create state space object using control.matlab

# Linear sim w/control

# Calculate torque applied

# Convert output to degrees

Should viewing permission be an issue, please email wal2116@columbia.edu to request it.

# Get motor speed (output from controller through ADC)

# Create state space object using control.matlab

Linear sim w/control

take in x_actual i.e. measure [thb dthb dthw]^T

# calculate again, with x0 = [x_actual e]^T (should be 6x1)

# Calculate torque applied

# Load sys from file

# Create state space object using control.matlab

while(g != 0 or b != 0):

Linear sim w/control

# get x_actual i.e. [g,b,a,dg,db,da]^T