Given the above the dynamic equations are found by summing the forces, first in the horizontal direction, then in the vertical direction. The sum of the forces in the horizontal direction is shown by

The sum of the forces in the vertical direction is shown by

Linearizing these equations about pi provides the linear dynamics,

Unfortunately, linear acceleration and angular acceleration are present in both equations. In order to remove algebraic dependence on each other, they are first isolated individually through simple manipulation,

Then substitution is used to ensure linear acceleration and angular acceleration are seen in only one equation each:

where,

Utilizing the linearized dynamic equations produced in Part 1, a scoring function was created, p2_score_segway, in conjunction with the Matlab function fminunc in order to determine the optimum motor gains for the robot. A general outline for the dynamics and scoring algorithm is laid out below:

while time < runtime
	motor torque = k1*(x - x_desired) + k2*(x' - x'_desired) + k3*(theta - theta_desired) + k4*(theta' - theta'_desired);
	update x'';
	update theta'';
	update x';
	update theta';
	update x;
	update theta;
	
	update score;
	time += delta_time;
end

Please see p2_score_segway for full algorithmic details and p2_main for fminunc implementation.

The scoring function utilized was

score += w1*(x - x_desired)^2 + w2*(x' - x'_desired)^2 + w3*(theta - theta_desired)^2 + w4*(theta' - theta'_desired)^2 + w5*(motor torque)^2;

The weights for each scoring variable (w1,w2,w3,w4,w5) were determined experimentally. A test case was arbitrarily set at moving the robot 5 meters in 6 seconds. It was first determined that the most important aspect of the robot is that the pendulum should deviate from the vertical as little as possible. Further, the robot is equally ineffective if it is unable to move to a desired point. As a result, w1 and w3 were first both set equal to 1, while w2, w4, and w5 were set to 0. The absolute value of motor gains obtained were k1=7263, k2=52770, k3=276470, k4=41110, while a final score of 17.7733 was obtained. As can be seen from the graphs below, this test case provided a great deal of deviation from vertical in the pendulum, along with a large overshoot, and angular acceleration. An animation of the robot simulation was also created with function p2_animate_segway and can be found here. The animation, however, does not run in real time, please run p2_main in order to recreate it and the response graphs.

w1=1, w2=0, w3=1, w4=0, w5=0	k1=72630, k2=52770, k3=276470, k4=41110	score = 17.7733

Further experimenting with weights was conducted in order to determine how each weight effects the response. A few random responses can be seen below along with the obtained motor gains and final score.

w1=1, w2=0, w3=1, w4=0, w5=1	k1=0.32, k2=29.82, k3=3.38, k4=42.55	score = 147.6754
w1=1, w2=0, w3=1, w4=0, w5=10-4	k1=98.14, k2=127.82, k3=235.51, k4=201.48	score = 32.3190
w1=1, w2=0, w3=102, w4=0, w5=10-6	k1=994, k2=1611, k3=10015, k4=1965	score = 39.0602
w1=1, w2=0, w3=103, w4=0, w5=10-6	k1=638, k2=1388, k3=25658, k4=1805	score = 64.3525

A number of conclusions can be drawn from these and other experiments conducted. As seen above, increasing the weight on angle ensures there is less of a deviation from theta = 0 or the vertical, meaning a more stable ride. However, this also decreases the motor torque and subsequently linear velocity, meaning a slower ride and a higher probability of linear position overshoot. Decreasing the weight on motor torque seems to have similar effects as decreasing the weight on angle. The linear velocity increases meaning a faster rise time in linear position, however, theta strays much more from 0, meaning more perturbations in the pendulum. Chances of linear position overshoot also seem to decrease. Increasing the weight on linear position also seems to have a similar effect, with the exception of a seemingly higher chance of linear position overshoot. Modifying the weights on linear velocity and angular velocity seem to have the same effects as modifying the weights on their integral couterparts, however with more drastic results. Given a more sophisticated knowledge of controls, weights could be specifically modified in order to obtain exact response parameters such as positional overshoot, angular settling time, and positional rise time.

One thing to note with the current method of control, in all cases the linear and angular accelerations are very large at the beginning of the response, shown by the non-zero starting torques, linear velocities, and angular velocities. This is most likely due to the fact that in both the scoring function and torque function, desired parameters such as theta_desired and x_desired are not time dependent, but in fact are constants throughout the runtime. Such large accelerations in such short periods of time have the potential of not being able to translate to physical motors. Utilizing desired parameters which are time dependent, such as those developed in conjunction with a spline model incorporating knot points, would alleviate this problem.

In order to utilize the linear quadratic regulator method the dynamics first have to be represented in state space form,

Utilizing the dynamics derived in Part 1 and four state variables, linear position, linear velocity, angle, angular velocity, the following representation is produced,

The Matlab function lqr was first utilized in order to obtain motor gains, and then lsim in order to determine the system response. A similar method to the experimentation in Part 2 was utilized in order to determine the Q-matrix utilized in lqr. Further, the input, or motor torque utilized in conjunction with lsim was also experimentally determined. It was found that a constant torque of x_desired*314 allowed the robot to traverse the predetermined 5 meters in 6 seconds, given reasonable enough weights. Please see Matlab function p3_main for full state space representation and algorithm. Various responses to various Q-matrices are shown below, along with the obtained gains.

Q=diag(10,10,10,10)	Q=diag(105,10,10,10)
k1=3.16, k2=19.83, k3=6.27, k4=31.85	k1=316.23, k2=302.94, k3=834.74, k4=425.56
Q=diag(105,105,10,10)	Q=diag(105,105,108,10)
k1=316.2, k2=520.2, k3=2058.3, k4=786.5	k1=316, k2=902, k3=10481, k4=1533

The relationships between weights and responses determined in Part 2 seem to be similar to the ones here, which is expected considering the same dynamic model is utilized, just in a modified representation. Increasing the position weight in the Q-matrix seems to increase the oscillatory nature of the system, but also increases the linear velocity while decreasing the positional rise-time. Increasing the velocity weight actually decreases the velocity and increases the rise-time, but reduces the oscillatory nature and settling time. Increasing the angle weight also reduces the velocity and increases rise-time, but deviation from theta = 0 is reduced. Increasing the angular velocity weight seems to have the same effect as increasing the angle weight.

A comparison between the algorithms of Parts 2 and 3 was also performed. The weights were equally set for both algorithms and responses obtained. An animation of Part 2's response can be seen here and response graphs can be seen below.

w1=105, w2=105, w3=106, w4=106, w5=0	Q=diag(105,105,106,106)
k1=1149, k2=2183, k3=14084, k4=5317	k1=316, k2=611, k3=36514, k4=15578

While the responses look very similar, especially linear position and angle, the obtained gains are very different. This is expected as Part 2's motor torque takes into account constant desired parameters such as x_desired and theta_desired, whereas Part 3's torque does not. As a result, when the same weights are used the same response is seen, but the same motor torque is not necessarily utilized.

No change was deemed necessary from the algorithms in Part 2 and 3 in order to implement a dynamic planner for the robot that moves from standing still at point A to standing still at point B. Both Matlab functions p2_main and p3_main utilize workspaces initialized by minhas_kdc2. In minhas_kdc2 any of the four starting and ending state variables can be modified. As a result, both Part 2's algorithm and Part 3's algorithm can have any starting point A and any ending point B. In fact, the starting and ending conditions can be scaled even further by having different starting angles, angular velocities, and linear velocities. Two examples of differing starting conditions for both algorithms are shown below. In the first example, the robot begins at linear position 2m and travels to linear position 6m, with all other starting and ending conditions being zero. In the second example, the robot begins at linear position 2m, with an angle of 10 degrees and linear velocity of 2 m/s, and travels to 8m with all other starting and ending conditions being zero. Any number of starting and ending combinations can be tested by modifying the initial state variables and desired state variables in minhas_kdc2 and then running p2_main and/or p3_main. An animation for example 1 can be found here and an animation for example 2 can be found here.

The weights used in both examples are the same as those utilized in the comparison of algorithms 2 and 3 in Part 3. In order to optimize travel time, motor torque, or stimulation of the rider's vestibular system, the weights can be modified according to the criteria laid out in the experimentation performed in Parts 2 and 3.

Example 1
Example 2

As implied in Part 4, in order to simulate the robot swinging up the rider from the stable hanging down position to the unstable upright position, the initial conditions simply need to be modified in Matlab function minhas_kdc2. In the example shown below, the starting angle is pi, with all other starting and ending conditions being equal to zero. An animation of the flip-up can be found here.