The essential states are:
- \(p_3 = L\dot{i}\): flux through the DC motor circuit
- \(p_7 = J_p \omega_p\): pitch angular momentum of the person + platform
- \(q_8 = \theta_p\): absolute pitch angle of the person + platform
- \(p_{17} = J_w \omega_w\): rolling angular momentum of the wheel and rotor assembly
The inputs are:
- \(e(t)\): specified voltage applied to the DC motor circuit
- \(F(t)\): longitudinal wind force
The model is linear and thus can be put into state space form. The following function accepts a structure, p, containing all of the base constant parameters and outputs the state matrix A and input matrix B. Inside the function, the base parameters are converted to the generalized numbered linear constants of each port in the Bond Graph and the linear system needed to convert the \(\dot{p}_7\) and \(\dot{p}_{17}\) into explicit form due to the two derivative causality inertia ports is solved numerically before constructing the two system matrices.
function [A, B] = segway_state_space(p)
% SEGWAY_STATE_SPACE - Returns the state and input matrices of a simple
% Segway longitudinal balancing model.
%
% Syntax: [A, B] = segway_state_space(p)
%
% Inputs:
% p - Structure containing the base constant parameters of the system.
% Outputs:
% A - 4x4 state matrix corresponding to the state x = [p3; p7; q8; p17]
% B - 4x2 input matrix corresponding to the input u = [e(t); F(t)]
% define all of the generalized linear constants of the bond graph
R2 = p.R;
I3 = p.L;
r4_5 = p.T;
I7 = p.Jp;
C8 = 1/p.mp/p.g/p.lp;
m9_10 = p.lp;
I12 = p.mp;
I14 = p.mw;
m16_15 = p.d/2;
I17 = p.Jw;
% solve the derivative casuality system numerically
% M*y = N*w + b*r
% y = [p7dot; p17dot]
% w = [p3; q8]
% r = [F(t)]
% y = M^-1*N*w + M^-1*b*r
M = [ 1 + I12/I7*m9_10^2, -I12/I17*m9_10*m16_15;
-I12/I7*m9_10*m16_15, 1 + I14/I17*m16_15^2 + I12/I17*m16_15^2];
N = [r4_5/I3, 1/C8;
r4_5/I3, 0];
b = [ m9_10;
-m16_15];
invM = inv(M);
% M^-1 * N
MN = invM * N;
% M^-1 * b
Mb = invM * b;
% y = MN * w + Mb * r
% define the state and input matrices
A = [ -R2/I3, -r4_5/I7, 0, -r4_5/I17;
MN(1,1), 0, MN(1,2), 0;
0, 1/I7, 0, 0;
MN(2,1), 0, MN(2,2), 0];
B = [1, 0;
0, Mb(1);
0, 0;
0, Mb(2)];
end
The following m-file computes the two inputs. The wind force is specified as a step function that provides a constant wind force between a start and stop time. The voltage is specified as a linear function of the four states. Each state is multiplied by a gain value and summed together to provide "full state feedback" control. The gain values are chosen using the LQR method in the primary script further below. These gain values are supplied to the function. Note that this assumes it is possible to measure each state directly (which is an slightly unrealistic assumption, but used for the purposes of this demonstration).
function u = eval_segway_input(t, x, p, G)
% EVAL_SEGWAY_INPUT - Returns the input vector, motor voltage and wind
% force, at any given time. The voltage is computed as a full state
% feedback controller adn the gust of wind is a step function with a 1
% second duration.
%
% Syntax: u = eval_segway_input(t, x, p, G)
%
% Inputs:
% t - A scalar value of time, size 1x1.
% x - State vector [p3; p7; q8; p17, xw] at time t, size 5x1.
% p - Constant parameter structure.
% G - Full state feedback gain matrix, size 1x4.
% Outputs:
% u - Input vector [e(t); F(t)] at time t, size 2x1.
% compute the full state feedback controller base on first four
% essential states
% 1x1 = 1x4 * 4x1
e = -G * x(1:4);
% define a gust of wind applied between 1 and 2 seconds
if t > 1.0 && t < 2.0
% magnitude of force from a gust of wind hitting a human at speed v
rho = 1.2; % air density [kg/m^3]
CdA = 0.84; % drag coeff times frontal area [m^2]
v = 18; % wind speed [m/s]
F = rho*CdA*v**2/2;
else
F = 0.0;
end
% pack input into a 2x1 vector
u = [e; F];
end
The function that computes the derivative of the states then becomes fairly simple because we have defined the linear system as a state space. I do add an additional state equation that is necessary if we desire to know the longitudinal position of the wheel center. This state is called an "ignorable" coordinate because the dynamics only depend on the velocity of the wheel center not its position, but it is necessary to integrate the velocity to know this value.
- \(\dot{x}_w = \frac{p_{14}}{I_{14}} = \frac{_{16}r_{15}}{I_{17}}p_{17}\): longitudinal momentum of the wheel assembly
function xdot = eval_segway_rhs(t, x, g, p, A, B, G)
% EVAL_SEGWAY_RHS - Returns the time derivative of the states, i.e.
% evaluates the right hand side of the explicit ordinary differential
% equations.
%
% Syntax: xdot = eval_segway_rhs(t, x, r, p)
%
% Inputs:
% t - Scalar value of time, size 1x1.
% x - State vector [p3; p7; q8; p17; xw], size 5x1.
% g - Anonymous function of form g(t, x, p, G) which computes the
% input at time t.
% p - Constant parameter structure.
% A - State matrix for first four states, size 4x4.
% B - Input matrix, size 4x2.
% G - Full state feedback gain matrix, size 1x4.
% Outputs:
% xdot - Time derivative of the states at time t, size 5x1.
% evaluate the input
u = g(t, x, p, G);
xdot = zeros(5, 1);
xdot(1:4, :) = A * x(1:4) + B * u;
% add an extra non-essential ODE so we can get wheel position
omega_w = x(4) / p.Jw;
vw = omega_w * p.d / 2;
xdot(5, 1) = vw;
end
The next m-file computes several useful outputs. The power draw from the battery is computed and the Cartesian location of the mass center of the wheel assembly and the mass center of the person are computed for use in animating the motion.
function y = eval_segway_output(t, x, g, p, G)
% EVAL_SEGWAY_INPUT - Returns the input vector, motor voltage and wind
% force, at any given time. The voltage is computed as a full state
% feedback controller adn the gust of wind is a step function with a 1
% second duration.
%
% Syntax: u = eval_segway_input(t, x, p, G)
%
% Inputs:
% t - A scalar value of time, size 1x1.
% x - State vector [p3; p7; q8; p17, xw] at time t, size 5x1.
% g - Anonymous function of form g(t, x, p, G) which computes the
% input at time t.
% p - Constant parameter structure.
% G - Full state feedback gain matrix, size 1x4.
% Outputs:
% y - Output vector [xw; yw; xp; yp; P] at time t, size 5x1.
% motor current drawn from source
i = x(1) / p.L;
r = g(t, x, p, G);
e = r(1);
% electrical power
power = e * i;
% plotting
thetap = x(3);
xw = x(5);
yw = p.d / 2;
xp = xw - p.lp * sin(thetap);
yp = yw + p.lp * cos(thetap);
y = [xw; xp; yw; yp; power];
end
Finally, the main script file is shown below. There are these main steps:
- Define the constant parameters.
- Calculate the state and input matrices.
- Calculate the eigenvalues and eigenvectors of the state matrix.
- Try a simple proportional feedback controller.
- Try a simple PID feedback controller.
- Calculate the gains for a full state feedback controller based on LQR.
- Simulate the motion and save data to a file.
- Plot the trajectories of the states, inputs, and outputs.
Type simulate_segway in Octave or Matlab to run the script.
%% define all constant parameters and store in a structure
p.g = 9.81; % acceleration due to gravity [m/s^2]
p.mp = 75.0; % person + platform mass [kg]
p.Jp = 14.0; % person + platform inertia [kg m^2]
p.lp = 0.9; % center of mass [m]
p.d = 0.4; % wheel diameter [m]
p.mw = 6; % wheel mass [kg]
p.Jw = p.mw*(p.d/2)^2; % wheel inertia [kg m^2]
p.T = 12; % dc motor constant [Nm/A]
p.R = 8; % dc motor winding resistance [O]
p.L = 0.5; % dc motor wiinding inductance, [H]
%% calculate the state and input matrices
[A, B] = segway_state_space(p);
%% calculate the eigenvalues and eigenvectors of the state matrix
[evecs, evals] = eig(A);
display('Eigenvalues');
evals
display('Eigenvectors');
evecs
%% check to see if a simple proportional feedback of q8 will stabilize the
%% system
% create a SISO plant input: e, output: q8 and control: e=-k*q8.
C = [0, 0, 1, 0]; % i.e. y = q8
sys = ss(A, B(:, 1), C, 0);
[num, den] = ss2tf(sys);
q8_e = tf(num, den)
% see if proportional feedback can stabilize using root locus graph
figure;
rlocus(q8_e)
%% try a PID controller (manually selected gains)
pid_controller = pid(500, 50, 5);
feedback(q8_e, pid_controller)
display('Eigenvalues of PID controlled system')
eig(feedback(q8_e, pid_controller))
%% try a linear quadratic regulator controller
if rank(ctrb(A, B(:, 1))) == length(A)
display('System is controllable with full state feedback')
end
Q = eye(4);
R = 1;
[G, X, L] = lqr(A, B(:, 1), Q, R);
display('Eigenvalues of PID controlled system')
L
%% simulate the system with the LQR controller
final_time = 6.0;
ts = linspace(0, final_time, num=final_time*60);
f = @(t, x) eval_segway_rhs(t, x, @eval_segway_input, p, A, B, G);
% set the initial forward lean angle to 20 degrees
x0 = [0.0; 0.0; -20*pi/180; 0.0; 0.0];
[ts, xs] = ode45(f, ts, x0);
% calculate the inputs and outputs of the simulation
us = zeros(length(ts), 2);
ys = zeros(length(ts), 5);
for i = 1:length(ts)
us(i, :) = eval_segway_input(ts(i), xs(i, :)', p, G);
ys(i, :) = eval_segway_output(ts(i), xs(i, :)', @eval_segway_input, p, G);
end
% save all simulation input and output data to a file
save('-v6', 'segway-sim-data.mat', 'ts', 'xs', 'ys', 'us', 'p');
%% plot the simulation results
figure(1);
plot(ts, xs);
legend('p3', 'p7', 'q8', 'p17', 'xw');
figure(2);
plot(ts, ys);
legend('xw', 'yw', 'xp', 'yp', 'P');
figure(3);
subplot(611)
plot(ts, us(:, 2));
ylabel('Wind force [N]');
subplot(612)
plot(ts, us(:, 1));
ylabel('Applied Voltage [V]');
subplot(613)
plot(ts, 180/pi*xs(:, 3));
ylabel('Angle [deg]');
subplot(614)
plot(ts, ys(:, 1));
ylabel('Wheel Location [m]');
subplot(615)
plot(ts, 180/pi*xs(:, 4)/p.Jw);
ylabel('Wheel Speed [deg/s]');
subplot(616)
plot(ts, ys(:, 5));
ylabel('Power [Watts]');
The above script generate a .mat file which stores all of the trajectories generated from the simulation. The following Python file loads this data and animates the simulation. The animation can be done in Octave/Matlab but I was having trouble getting a nice result with Octave, so I just wrote it in Python. You'll need to install Python, NumPy, SciPy, and matplotlib for this to run. I recommend installing Anaconda to get them all.
Type python animate_segway.py to run the script from a terminal or command prompt.
#!/usr/bin/env python
import numpy as np
from scipy.io import loadmat
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.patches import Circle
# load the data and extract necessary variables
dat = loadmat('segway-sim-data.mat', squeeze_me=True)
d = float(dat['p']['d'])
ts = dat['ts']
us = dat['us']
xs = dat['xs']
ys = dat['ys']
max_force = us.max()
# create intial plot
fig, ax = plt.subplots()
wheel = Circle((0.0, 0.2), radius=0.2, linestyle='--', linewidth=3,
edgecolor='k')
ax.add_patch(wheel)
person, = ax.plot([], [], 'k', linewidth=2)
wind, = ax.plot([ys[0, 1], ys[0, 1] - us[0, 1]/max_force],
[ys[0, 3], ys[0, 3]],
color='r', linewidth=4, marker="<", markersize=10)
ax.set_ylim(0.0, max(ys[:, 3]) + d/2)
ax.set_xlim(min(ys[:, 1]) - d/2, max(ys[:, 1]) + d/2)
ax.set_aspect('equal')
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
# define a function to update the plot
def update(i, person, wheel, wind):
wheel.center = (ys[i, 0], ys[i, 2])
wheel.angle = -np.rad2deg(xs[i, 4]/(d/2))
person.set_data([ys[i, 0], ys[i, 1]],
[ys[i, 2], ys[i, 3]])
wind.set_data([ys[i, 1], ys[i, 1] - us[i, 1]/max_force],
[ys[i, 3], ys[i, 3]])
return person, wheel, wind
anim = animation.FuncAnimation(fig, update, len(ts),
fargs=(person, wheel, wind),
interval=0.1)
plt.show()
# uncomment to save animation to file
#anim.save('segway.mp4', writer='ffmpeg', fps=60)
#anim.save('segway.gif', writer='imagemagick', fps=30)
The final resulting animation of the simulation is shown in the GIF below:
