Segway Simulation [gnuplot]

Animeation (YouTube 0:41-0:54 Left side)

segway_anime.plt

# Setting --------------------
reset
set nokey
set origin -0.25,0.0
unset grid
L = 1.0
x = 2.0
y = 4.0
set size ratio y/(2.0*x)
set xr[-x*L:x*L]
set yr[-0.5:3.5]
set xtics 2
set ytics 1
set xl "{/Times:Italic=24 x} {/Times:Normal=24 [m]}"
set yl "{/Times:Italic=24 y} {/Times:Normal=24 [m]}"
set tics font 'Times, 20'

# Parametr --------------------
ww  = 0.1  # Width of wheel  [m]
pw  = 0.1  # Width of pendulum  [m]
wg  = 1000  # Density of wheel  [kg/m3]
pg  = 1000  # Density of pendulum [kg/m3]
r  = 0.5  # Radius of wheel  [m]
rsen = 0.03   # Radius of sensor  [m]
l  = 2.5  # Length of pendulum [m]
m  = pw*2*l*pg # Weight of pendulum [kg]
M  = pi*r**2*ww*wg # Weight of wheel  [kg]
a  = 1.0  # Distance from Center of pendulum to Center of rotation [m]
g  = 9.81  # Gravitational acceleration  [m/s2]

dt  = 0.002  # Time step  [s]

# Proportional gain
kp1  = 5000.0 # pendulum [Nm/rad]
kp2  = 100.0  # wheel [Nm/rad]

# Differential gain
kd1  = 100.0  # pendulum [Nms/rad]
kd2  = 100.0  # wheel [Nms/rad]

lim1 = 120    # Stop time
lim2 = 6000   # Loop limit
cut  = 10  # Decimation

# coefficient
dx  = dt/6
apl  = a+l/2
lma  = l/2-a

# Set filename and output
filename = sprintf("anime %04d-%04d-%04d-%04d.gif", kp1, kp2, kd1, kd2)
set term gif animate delay 2 size 600,480
set output filename

# Function --------------------
# Coefficient matrix
A    = m * (a**2 + (l**2)/12)  # (1，1)
B    = m*a*r                   # (1，2) (2, 1)
C    = (m + 3/2*M) * r**2      # (2, 2)

D    = A * C                   # Coefficient
E    = B**2
F    = m * g * a               # Torque

# Torque generated by motor installed in wheel
Tr(s, t, u, v) = kp1*s + kd1*t + kp2*u + kd2*v

# theta' (pendulum angular velocity)
f1(s, t, u, v) = t

# theta'' (pendulum angular acceleration)
f2(s, t, u, v) = \
 ( C*(F*sin(s)-Tr(s, t, u, v)) - B*cos(s)*( Tr(s, t, u, v)+B*(t**2)*sin(s) ) ) / (D-E*(cos(s))**2)

# phi' (wheel angular velocity)
f3(s, t, u, v) = v

# phi'' (wheel angular acceleration)
f4(s, t, u, v) = \
 ( A*(F*sin(s)-Tr(s, t, u, v)) - B*cos(s)*( Tr(s, t, u, v)+B*(t**2)*sin(s) ) ) / (D-E*(cos(s))**2)

# Label displaying gains
gain(a, b, c, d) = sprintf("\
{/Times:Italic K_{/Times:Normal p1}} = %d\n\
{/Times:Italic K_{/Times:Normal p2}} = %d\n\
{/Times:Italic K_{/Times:Normal d1}} = %d\n\
{/Times:Italic K_{/Times:Normal d2}} = %d", \
a, b, c, d)

# Time
time(t) = sprintf("{/Times:Italic t} = %.2f [s]", t) 

# Plot --------------------
# Initiate value
x1 = pi/180*10  # theta (pendulum)
x2 = pi/180*100  # theta'
x3 = 0.0  # phi   (wheel)
x4 = 0.0  # phi'
t  = 0.0  # time

# Draw initiate state (t = 0) for lim1 steps
do for [i = 1:lim1] {
    # Gain and time
    set label 1 left at graph 1.02, 0.9 gain(kp1, kp2, kd1, kd2) font 'Times:Normal, 18'
    set label 2 center at graph 0.5, 0.93 time(t) font 'Times:Normal, 18'

    # Center of wheel G (x-coordinate)
    xg =r*x3

    # Wheel
    set object 1 circle at xg, r fc rgb 'grey50' size r fs solid
 
    # Sensor
    set object 2 circle at xg+r*sin(x3), r+r*cos(x3) fc rgb 'black' size rsen+0.03 fs solid
    set object 3 circle at xg+r*sin(x3), r+r*cos(x3) fc rgb 'white' size rsen fs solid 

    # Pendulum
    set arrow 1 nohead lw 7 from xg-lma*sin(x1), r-lma*cos(x1) to xg+apl*sin(x1), r+apl*cos(x1) lc 6
 
    # Center of wheel
    set object 4 circle at xg, r fc rgb 'black' size rsen fs solid front
 
    set arrow 2 nohead lw 4 from -x*L, 0 to x*L, 0 lc -1 
  
    plot 1/0
}

# Update for lim2 steps
do for [i = 1:lim2] {
    t = t + dt
 
    # Calculate using Runge-Kutta 4th
    k11 = f1(x1, x2, x3, x4)
    k12 = f2(x1, x2, x3, x4)
    k13 = f3(x1, x2, x3, x4)
    k14 = f4(x1, x2, x3, x4)
    k21 = f1(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
    k22 = f2(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
    k23 = f3(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
    k24 = f4(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
    k31 = f1(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
    k32 = f2(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
    k33 = f3(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
    k34 = f4(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
    k41 = f1(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
    k42 = f2(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
    k43 = f3(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
    k44 = f4(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )

    x1 = x1 + dx * (k11 + 2*k21 + 2*k31 + k41)
    x2 = x2 + dx * (k12 + 2*k22 + 2*k32 + k42)
    x3 = x3 + dx * (k13 + 2*k23 + 2*k33 + k43)
    x4 = x4 + dx * (k14 + 2*k24 + 2*k34 + k44)
 
    # Center of wheel G (x-coordinate)
    xg = r * x3
  
    # Wheel
    set object 1 at xg, r
 
    # Sensor
    set object 2 at xg+r*sin(x3), r+r*cos(x3)
    set object 3 at xg+r*sin(x3), r+r*cos(x3)

    # Pendulum
    set arrow 1 from xg-lma*sin(x1), r-lma*cos(x1) to xg+(a+l/2)*sin(x1), r+(a+l/2)*cos(x1)
 
    # Center of wheel
    set object 4 circle at xg, r
  
    # Title
    set label 4 center at graph 0.5, 0.93 time(t) font 'Times:Normal, 18'
 
    # Decimate and draw
    if(i%cut==0){
        frx = xg-x*L           # Set x-range
        tox = xg+x*L
        set arrow 2 nohead lw 4 from frx, 0 to tox, 0 lc -1  # Draw ground
        plot [frx:tox] [-0.5:3.5] 1/0
    }
}
set out

sanime 5000-0100-0100-0100.gif

Graph (YouTube 0:41-0:54 Right side)

segway_graph.plt

# Setting --------------------
reset
set key right top
set size ratio 1.0
set grid
set xl "{/Times:Italic=24 t}     {/Times:Normal=24 [rad]}"
set yl "{/symbol:Italic=24 q  f}     {/Times:Normal=24 [s]}"
set xtics 2
set ytics 1
set tics font 'Times, 20'


# Patameter--------------------
ww  = 0.1  # Width of wheel  [m]
pw  = 0.1  # Width of pendulum  [m]
wg  = 1000  # Density of wheel  [kg/m3]
pg  = 1000  # Density of pendulum [kg/m3]
r  = 0.5  # Radius of wheel  [m]
rsen = 0.03   # Radius of sensor  [m]
l  = 2.5  # Length of pendulum [m]
m  = pw*2*l*pg # Weight of pendulum [kg]
M  = pi*r**2*ww*wg # Weight of wheel  [kg]
a  = 1.0  # Distance from Center of pendulum to Center of rotation [m]
g  = 9.81  # Gravitational acceleration  [m/s2]

dt  = 0.002  # Time step  [s]
dx  = dt/6

# Proportional gain
kp1  = 5000.0 # pendulum [Nm/rad]
kp2  = 100.0  # wheel [Nm/rad]
# Differential gain
kd1  = 100.0  # pendulum [Nms/rad]
kd2  = 100.0  # wheel [Nms/rad]

lim1 = 120   # Stop time
lim2 = 6000   # Time limit
cut  = 10  # Decimation
graphr = 0.5   # Radius of point plot

# Set filename and output
filename = sprintf("graph %04d-%04d-%04d-%04d.gif", kp1, kp2, kd1, kd2)
set term gif animate delay 2 size 1280,480
set output filename

# Function --------------------
# Coefficient matrix
A    = m * (a**2 + (l**2)/12)  # (1，1)
B    = m*a*r                   # (1，2) (2, 1)
C    = (m + 3/2*M) * r**2      # (2, 2)

D    = A * C                   # Coefficient
E    = B**2
F    = m * g * a               # Torque

# Torque generated by motor installed in wheel
Tr(s, t, u, v) = kp1*s + kd1*t + kp2*u + kd2*v

# theta' (pendulum angular velocity)
f1(s, t, u, v) = t

# theta'' (pendulum angular acceleration)
f2(s, t, u, v) = \
 ( C*(F*sin(s)-Tr(s, t, u, v)) - B*cos(s)*( Tr(s, t, u, v)+B*(t**2)*sin(s) ) ) / (D-E*(cos(s))**2)

# phi' (wheel angular velocity)
f3(s, t, u, v) = v

# phi'' (wheel angular acceleration)
f4(s, t, u, v) = \
 ( A*(F*sin(s)-Tr(s, t, u, v)) - B*cos(s)*( Tr(s, t, u, v)+B*(t**2)*sin(s) ) ) / (D-E*(cos(s))**2)

# Label displaying gains
gain(a, b, c, d) = sprintf("\
{/Times:Italic K_{/Times:Normal p1}} = %d\n\
{/Times:Italic K_{/Times:Normal p2}} = %d\n\
{/Times:Italic K_{/Times:Normal d1}} = %d\n\
{/Times:Italic K_{/Times:Normal d2}} = %d", \
a, b, c, d)

# Plot --------------------
# Initiate value
x1 = pi/180*10     # theta   (pendulum)
x2 = pi/180*100    # theta'
x3 = 0.0           # phi     (wheel)
x4 = 0.0           # phi'
t  = 0.0           # time

# Draw initiate state (t = 0) for lim1 steps
do for [i = 1:lim1] {
    set xr[0:dt*lim2]
    set yr[-3:3]
 
    # Point plot
    set label 1 at t, x1 point lc 6 ps graphr pt 7
    set label 2 at t, x3 point lc 0 ps graphr pt 7
 
    # X-Axis (t:time)
    set arrow 1 nohead from 0, 0 to 12, 0 lc -1 lw 2

    # Set key
    plot 1/0 lw 0 lc rgb 'white' t " ",\
         1/0 lw 4 lc 6 t "{/symbol:italic=24 q} ",\
         1/0 lw 0 lc rgb 'white' t " ",\
         1/0 lw 4 lc 0 t "{/symbol:italic=24 f} "
}

# Update for lim2 steps
do for [i = 1:lim2] {
    t = t + dt
  
    # Calculate using Runge-Kutta 4th
    k11 = f1(x1, x2, x3, x4)
    k12 = f2(x1, x2, x3, x4)
    k13 = f3(x1, x2, x3, x4)
    k14 = f4(x1, x2, x3, x4)
    k21 = f1(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
    k22 = f2(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
    k23 = f3(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
    k24 = f4(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
    k31 = f1(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
    k32 = f2(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
    k33 = f3(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
    k34 = f4(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
    k41 = f1(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
    k42 = f2(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
    k43 = f3(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
    k44 = f4(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )

    x1 = x1 + dx * (k11 + 2*k21 + 2*k31 + k41)
    x2 = x2 + dx * (k12 + 2*k22 + 2*k32 + k42)
    x3 = x3 + dx * (k13 + 2*k23 + 2*k33 + k43)
    x4 = x4 + dx * (k14 + 2*k24 + 2*k34 + k44)
 

    # Point plot
    set label 2*i+3 at t, x1 point lc 6 ps grar pt 7
    set label 2*i+4 at t, x3 point lc 0 ps grar pt 7

    # Decimate and draw
    if(i%cut==0){
        plot 1/0 lw 0 lc rgb 'white' t " ",\
             1/0 lw 4 lc 6 t "{/symbol:italic=24 q} ",\
             1/0 lw 0 lc rgb 'white' t " ",\
             1/0 lw 4 lc 0 t "{/symbol:italic=24 f} "
    }
}
set out

graph 5000-0100-0100-0100.gif

Reference

Wikipedia
Segway