MATLAB code

Commonly Used Variables

• state: state is an vector with 8 state variables, in order they are, time step, X position, Y position, Lean Angle, Yaw Angle, Steer Angle, Lean Angle Rate, Velocity
• motCommands - this is the same as ${\vec {u}}$ in the control equation, for forward moving balancing, it is steer angle rate $[{\dot {\delta }}]$
• K - this is the vector of the three gain values k1,k2,k3. They are used to find the value of $[{\dot {\delta }}]$

MATLAB files

lqr_BC_optimizer.m

This script calculates optimal gain values. For a specified set of initial conditions, this file runs the LQR algorith on a Q matrix, and then the scaled values of that Q matrix. Then it calls runBicycleTest on each of the sets of gain values that it calculates. Based on the outputs of runBicycleTest it knows whether the bike balanced or didn't from the success output of runBicycleTest. Then it uses the allStates and applies a balance score equation and path score equation to score how well those gain values worked. Then it prints out the optimal gain values based on the balance score and stores all the data into a csv file.

balanceControlOptimizer.m

This is an older script used to find optimal gain values. Instead of using LQR to find gain values to test, you manually input a range of gain values you want to test. Then the code tests these values the same way lqr_BC_optimizer does.

runBicycleTest.m

Inputs:

• x0,y0 - initial location
• v0 - initial speed
• delta0 - initial steer angle
• phi0 - initial lean angle
• phi_dot0 - initial lean angle ratw
• psi0 - initial yaw angle (heading)
• K - vector of gains (k1, k2, k3)
• delta_offset - either a scalar (a constant offset), or a vector of offsets (the bike will attempt to hit delta_offset(n) at the nth timestep
• numTimeSteps - the number of time steps to run the simulation for
• graph - 1=draws graph, 0=does not

Outputs:

• success - 1 if bike stayed up successfully throughout duration of the test, 0 if the bike falls down(lean angle becomes greater than pi/4 during the test)
• AllStates - matrix with size (number of time steps x 8). Each row of this matrix contains a state, it contains all the states for all the timesteps in the test.

This function uses state variables and gains and passes them to rhs, which produces the derivative of all the state variables(minus timestep) and the motor command then it uses that derivative and euler integrates to find the next state values. It repeats this process for all the time steps in the test. Each time step is 1/50 second apart. Finally it uses all the states and all the motor commands and calls animateBike, to produce an animation of the bike and create all the data graphs for analysis. runBicycleTest produces the p parameter used in rhs and animateBike.

rhs.m

• currentState - contains the 8 current state variables
• p - contains bike parameters gravity, length of wheel base, distance from rear wheel to COM, height of COM, trail, and pause length
• K - Gain values
• delta_offset - desired steer angle
• phi_offset - desired lean angle

Outputs:

• zdot - zdot is a vector of the 7 derivatives of the state variables(minus timestep), it is used in the euler integration in runBicycleTest
• u - u is the same as the motor command, it is the $[{\dot {\delta }}]$ that will be applied to the bike so that the bike can balance itself

rhs works by using the gains values to calculate u, and then using the EOM to calculate the derivatives. It also sets a max steer angle rate to 4.8 rad/s based on the limitations of the motor used. For straight line balancing, delta_offset and phi_offset will be 0. For turning balancing, they will be determined by the navigation controller. rhs calls no other functions.

BikeAndMotorConstants.m

No inputs Output: CONST - A vector containing important constants about the bike

This function calculates important constants including moment of inertia, center of mass, and others. It is called in drawBike.

animateBike.m

Inputs:

• state - this is not one set of state variables, it is actually the allStates variable from runBicycleTest
• p - contains bike parameters gravity, length of wheel base, distance from rear wheel to COM, height of COM, trail, and pause length
• motCommands - steer angle rate. See above.
• delta_offset - desired steer angle
• phi_offset - desired lean angle

No Outputs

animateBike produces graphs and an animation of the bike test being run. Graphs produced: Lean vs Time, Lean Rate vs Time, Steer vs Time, Steer Rate vs Time, Steer Rate vs Time, X Position vs Y Position, Yaw Rate vs Time. animate Bike calls drawBike repeatedly, once for each state. In between states it pauses for timestep length(1/50 sec). Inside a for loop, the 3-d plot containing drawBike is updated repeatedly making the animation. On each timestep, the location of the back wheel is added as a red dot to the ground. These add up to make a line that represents the trajectory of the bike. In the picture below you can see a frame of the animation.

drawBike.m

Inputs:

• x - X position
• y - Y position
• z - Z position
• yaw - Yaw angle (heading)
• roll - Roll angle (lean angle)
• steer - Steer angle
• p - contains bike parameters gravity, length of wheel base, distance from rear wheel to COM, height of COM, trail, and pause length

Outputs:

• COG_hand - plots the center of gravity
• SH_hand - plots the handle bar
• CPfw_hand - plots the front wheel
• CPrq_hand - plots the back wheel

This function uses state values to draw the bike at a single state, animateBike utilizes successions of these frames to make an animation. This function calls BikeAndMotorConstants to get center of mass and size dimensions to proportionally base the drawing on. It also cals CircleAboutZ to draw the circles for the wheels. The picture below shows a drawing of the bike in an animation. The line is the handle bars, the two circles are the wheels, and the blue star is the center of mass. The red line is produced by animateBike and is the trajectory that has already been travelled by the bike.

CircleAboutZ.m

Inputs:

• r - radius of the circle

Outputs:

• CP - vector containing x and y values of a circle of radius r based around the origin. The z values are all 0, so the circle exists in the xy plane.

This function simply uses cosine and sine to find the x and y values for a circle using 100 theta values between 0 and 2pi. These values are used in drawBike to draw the wheels. The radius for the wheels is determined by BikeAndMotorConstants which is called in drawBike.

plotMultipleControllersTogether.m

This script runs runBicycleTest four times using different parameters. The user inputs the different parameters, then it makes plots comparing the four different parameters. Ideally you would only change the gain values on the parameters to compare the gains. It creates plots for Lean vs Time, Lean Rate vs Time, Steer vs Time, & Steer Rate vs Time. Make sure you put 0 as the graph parameter for runBicycleTest so the animation and plots don't appear for every single instance.

testSteerOffset.m

For our straight line balance the steer offset desired is 0. But for navigation it will be constantly changing. This script tests how well a set of gain values performs when the desired steer angle changes sinusoidally. The user inputs four different gains vectors, then the script runs runBicycleTest for each gain vector with the sinusoidal delta_offset. Then it graphs the four controllers against each other for Lean vs Time, Lean Rate vs Time, Steer vs Time, Steer Rate vs Time, Bicycle Trajectory, & Yaw Rate vs Time. The script also creates a score for how well the the bike achieved the desired steer angle. It prints out these scores, or if the bike fell, it prints out that the test failed.

See also

• LQR