Processing math: 100%
Home / YouTube /

Segway Simulation [gnuplot]

Wednesday, October 10, 2018

gnuplot Mechanics YouTube

t f B! P L
　

Contents[hide]

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

segway_anime.plt

  1. # Setting --------------------
  2. reset
  3. set nokey
  4. set origin -0.25,0.0
  5. unset grid
  6. L = 1.0
  7. x = 2.0
  8. y = 4.0
  9. set size ratio y/(2.0*x)
  10. set xr[-x*L:x*L]
  11. set yr[-0.5:3.5]
  12. set xtics 2
  13. set ytics 1
  14. set xl "{/Times:Italic=24 x} {/Times:Normal=24 [m]}"
  15. set yl "{/Times:Italic=24 y} {/Times:Normal=24 [m]}"
  16. set tics font 'Times, 20'
  17.  
  18. # Parametr --------------------
  19. ww  = 0.1  # Width of wheel  [m]
  20. pw  = 0.1  # Width of pendulum  [m]
  21. wg  = 1000  # Density of wheel  [kg/m3]
  22. pg  = 1000  # Density of pendulum [kg/m3]
  23. r  = 0.5  # Radius of wheel  [m]
  24. rsen = 0.03   # Radius of sensor  [m]
  25. l  = 2.5  # Length of pendulum [m]
  26. m  = pw*2*l*pg # Weight of pendulum [kg]
  27. M  = pi*r**2*ww*wg # Weight of wheel  [kg]
  28. a  = 1.0  # Distance from Center of pendulum to Center of rotation [m]
  29. g  = 9.81  # Gravitational acceleration  [m/s2]
  30.  
  31. dt  = 0.002  # Time step  [s]
  32.  
  33. # Proportional gain
  34. kp1  = 5000.0 # pendulum [Nm/rad]
  35. kp2  = 100.0  # wheel [Nm/rad]
  36.  
  37. # Differential gain
  38. kd1  = 100.0  # pendulum [Nms/rad]
  39. kd2  = 100.0  # wheel [Nms/rad]
  40.  
  41. lim1 = 120    # Stop time
  42. lim2 = 6000   # Loop limit
  43. cut  = 10  # Decimation
  44.  
  45. # coefficient
  46. dx  = dt/6
  47. apl  = a+l/2
  48. lma  = l/2-a
  49.  
  50. # Set filename and output
  51. filename = sprintf("anime %04d-%04d-%04d-%04d.gif", kp1, kp2, kd1, kd2)
  52. set term gif animate delay 2 size 600,480
  53. set output filename
  54.  
  55. # Function --------------------
  56. # Coefficient matrix
  57. A    = m * (a**2 + (l**2)/12)  # (1，1)
  58. B    = m*a*r                   # (1，2) (2, 1)
  59. C    = (m + 3/2*M) * r**2      # (2, 2)
  60.  
  61. D    = A * C                   # Coefficient
  62. E    = B**2
  63. F    = m * g * a               # Torque
  64.  
  65. # Torque generated by motor installed in wheel
  66. Tr(s, t, u, v) = kp1*s + kd1*t + kp2*u + kd2*v
  67.  
  68. # theta' (pendulum angular velocity)
  69. f1(s, t, u, v) = t
  70.  
  71. # theta'' (pendulum angular acceleration)
  72. f2(s, t, u, v) = \
  73.  ( 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)
  74.  
  75. # phi' (wheel angular velocity)
  76. f3(s, t, u, v) = v
  77.  
  78. # phi'' (wheel angular acceleration)
  79. f4(s, t, u, v) = \
  80.  ( 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)
  81.  
  82. # Label displaying gains
  83. gain(a, b, c, d) = sprintf("\
  84. {/Times:Italic K_{/Times:Normal p1}} = %d\n\
  85. {/Times:Italic K_{/Times:Normal p2}} = %d\n\
  86. {/Times:Italic K_{/Times:Normal d1}} = %d\n\
  87. {/Times:Italic K_{/Times:Normal d2}} = %d", \
  88. a, b, c, d)
  89.  
  90. # Time
  91. time(t) = sprintf("{/Times:Italic t} = %.2f [s]", t) 
  92.  
  93. # Plot --------------------
  94. # Initiate value
  95. x1 = pi/180*10  # theta (pendulum)
  96. x2 = pi/180*100  # theta'
  97. x3 = 0.0  # phi   (wheel)
  98. x4 = 0.0  # phi'
  99. t  = 0.0  # time
  100.  
  101. # Draw initiate state (t = 0) for lim1 steps
  102. do for [i = 1:lim1] {
  103.     # Gain and time
  104.     set label 1 left at graph 1.02, 0.9 gain(kp1, kp2, kd1, kd2) font 'Times:Normal, 18'
  105.     set label 2 center at graph 0.5, 0.93 time(t) font 'Times:Normal, 18'
  106.  
  107.     # Center of wheel G (x-coordinate)
  108.     xg =r*x3
  109.  
  110.     # Wheel
  111.     set object 1 circle at xg, r fc rgb 'grey50' size r fs solid
  112.  
  113.     # Sensor
  114.     set object 2 circle at xg+r*sin(x3), r+r*cos(x3) fc rgb 'black' size rsen+0.03 fs solid
  115.     set object 3 circle at xg+r*sin(x3), r+r*cos(x3) fc rgb 'white' size rsen fs solid 
  116.  
  117.     # Pendulum
  118.     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
  119.  
  120.     # Center of wheel
  121.     set object 4 circle at xg, r fc rgb 'black' size rsen fs solid front
  122.  
  123.     set arrow 2 nohead lw 4 from -x*L, 0 to x*L, 0 lc -1 
  124.   
  125.     plot 1/0
  126. }
  127.  
  128. # Update for lim2 steps
  129. do for [i = 1:lim2] {
  130.     t = t + dt
  131.  
  132.     # Calculate using Runge-Kutta 4th
  133.     k11 = f1(x1, x2, x3, x4)
  134.     k12 = f2(x1, x2, x3, x4)
  135.     k13 = f3(x1, x2, x3, x4)
  136.     k14 = f4(x1, x2, x3, x4)
  137.     k21 = f1(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
  138.     k22 = f2(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
  139.     k23 = f3(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
  140.     k24 = f4(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
  141.     k31 = f1(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
  142.     k32 = f2(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
  143.     k33 = f3(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
  144.     k34 = f4(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
  145.     k41 = f1(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
  146.     k42 = f2(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
  147.     k43 = f3(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
  148.     k44 = f4(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
  149.  
  150.     x1 = x1 + dx * (k11 + 2*k21 + 2*k31 + k41)
  151.     x2 = x2 + dx * (k12 + 2*k22 + 2*k32 + k42)
  152.     x3 = x3 + dx * (k13 + 2*k23 + 2*k33 + k43)
  153.     x4 = x4 + dx * (k14 + 2*k24 + 2*k34 + k44)
  154.  
  155.     # Center of wheel G (x-coordinate)
  156.     xg = r * x3
  157.   
  158.     # Wheel
  159.     set object 1 at xg, r
  160.  
  161.     # Sensor
  162.     set object 2 at xg+r*sin(x3), r+r*cos(x3)
  163.     set object 3 at xg+r*sin(x3), r+r*cos(x3)
  164.  
  165.     # Pendulum
  166.     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)
  167.  
  168.     # Center of wheel
  169.     set object 4 circle at xg, r
  170.   
  171.     # Title
  172.     set label 4 center at graph 0.5, 0.93 time(t) font 'Times:Normal, 18'
  173.  
  174.     # Decimate and draw
  175.     if(i%cut==0){
  176.         frx = xg-x*L           # Set x-range
  177.         tox = xg+x*L
  178.         set arrow 2 nohead lw 4 from frx, 0 to tox, 0 lc -1  # Draw ground
  179.         plot [frx:tox] [-0.5:3.5] 1/0
  180.     }
  181. }
  182. set out

sanime 5000-0100-0100-0100.gif


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

segway_graph.plt

  1. # Setting --------------------
  2. reset
  3. set key right top
  4. set size ratio 1.0
  5. set grid
  6. set xl "{/Times:Italic=24 t}     {/Times:Normal=24 [rad]}"
  7. set yl "{/symbol:Italic=24 q  f}     {/Times:Normal=24 [s]}"
  8. set xtics 2
  9. set ytics 1
  10. set tics font 'Times, 20'
  11.  
  12.  
  13. # Patameter--------------------
  14. ww  = 0.1  # Width of wheel  [m]
  15. pw  = 0.1  # Width of pendulum  [m]
  16. wg  = 1000  # Density of wheel  [kg/m3]
  17. pg  = 1000  # Density of pendulum [kg/m3]
  18. r  = 0.5  # Radius of wheel  [m]
  19. rsen = 0.03   # Radius of sensor  [m]
  20. l  = 2.5  # Length of pendulum [m]
  21. m  = pw*2*l*pg # Weight of pendulum [kg]
  22. M  = pi*r**2*ww*wg # Weight of wheel  [kg]
  23. a  = 1.0  # Distance from Center of pendulum to Center of rotation [m]
  24. g  = 9.81  # Gravitational acceleration  [m/s2]
  25.  
  26. dt  = 0.002  # Time step  [s]
  27. dx  = dt/6
  28.  
  29. # Proportional gain
  30. kp1  = 5000.0 # pendulum [Nm/rad]
  31. kp2  = 100.0  # wheel [Nm/rad]
  32. # Differential gain
  33. kd1  = 100.0  # pendulum [Nms/rad]
  34. kd2  = 100.0  # wheel [Nms/rad]
  35.  
  36. lim1 = 120   # Stop time
  37. lim2 = 6000   # Time limit
  38. cut  = 10  # Decimation
  39. graphr = 0.5   # Radius of point plot
  40.  
  41. # Set filename and output
  42. filename = sprintf("graph %04d-%04d-%04d-%04d.gif", kp1, kp2, kd1, kd2)
  43. set term gif animate delay 2 size 1280,480
  44. set output filename
  45.  
  46. # Function --------------------
  47. # Coefficient matrix
  48. A    = m * (a**2 + (l**2)/12)  # (1，1)
  49. B    = m*a*r                   # (1，2) (2, 1)
  50. C    = (m + 3/2*M) * r**2      # (2, 2)
  51.  
  52. D    = A * C                   # Coefficient
  53. E    = B**2
  54. F    = m * g * a               # Torque
  55.  
  56. # Torque generated by motor installed in wheel
  57. Tr(s, t, u, v) = kp1*s + kd1*t + kp2*u + kd2*v
  58.  
  59. # theta' (pendulum angular velocity)
  60. f1(s, t, u, v) = t
  61.  
  62. # theta'' (pendulum angular acceleration)
  63. f2(s, t, u, v) = \
  64.  ( 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)
  65.  
  66. # phi' (wheel angular velocity)
  67. f3(s, t, u, v) = v
  68.  
  69. # phi'' (wheel angular acceleration)
  70. f4(s, t, u, v) = \
  71.  ( 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)
  72.  
  73. # Label displaying gains
  74. gain(a, b, c, d) = sprintf("\
  75. {/Times:Italic K_{/Times:Normal p1}} = %d\n\
  76. {/Times:Italic K_{/Times:Normal p2}} = %d\n\
  77. {/Times:Italic K_{/Times:Normal d1}} = %d\n\
  78. {/Times:Italic K_{/Times:Normal d2}} = %d", \
  79. a, b, c, d)
  80.  
  81. # Plot --------------------
  82. # Initiate value
  83. x1 = pi/180*10     # theta   (pendulum)
  84. x2 = pi/180*100    # theta'
  85. x3 = 0.0           # phi     (wheel)
  86. x4 = 0.0           # phi'
  87. t  = 0.0           # time
  88.  
  89. # Draw initiate state (t = 0) for lim1 steps
  90. do for [i = 1:lim1] {
  91.     set xr[0:dt*lim2]
  92.     set yr[-3:3]
  93.  
  94.     # Point plot
  95.     set label 1 at t, x1 point lc 6 ps graphr pt 7
  96.     set label 2 at t, x3 point lc 0 ps graphr pt 7
  97.  
  98.     # X-Axis (t:time)
  99.     set arrow 1 nohead from 0, 0 to 12, 0 lc -1 lw 2
  100.  
  101.     # Set key
  102.     plot 1/0 lw 0 lc rgb 'white' t " ",\
  103.          1/0 lw 4 lc 6 t "{/symbol:italic=24 q} ",\
  104.          1/0 lw 0 lc rgb 'white' t " ",\
  105.          1/0 lw 4 lc 0 t "{/symbol:italic=24 f} "
  106. }
  107.  
  108. # Update for lim2 steps
  109. do for [i = 1:lim2] {
  110.     t = t + dt
  111.   
  112.     # Calculate using Runge-Kutta 4th
  113.     k11 = f1(x1, x2, x3, x4)
  114.     k12 = f2(x1, x2, x3, x4)
  115.     k13 = f3(x1, x2, x3, x4)
  116.     k14 = f4(x1, x2, x3, x4)
  117.     k21 = f1(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
  118.     k22 = f2(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
  119.     k23 = f3(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
  120.     k24 = f4(x1+dt/2*k11, x2+dt/2*k12, x3+dt/2*k13, x4+dt/2*k14 )
  121.     k31 = f1(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
  122.     k32 = f2(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
  123.     k33 = f3(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
  124.     k34 = f4(x1+dt/2*k21, x2+dt/2*k22, x3+dt/2*k23, x4+dt/2*k24 )
  125.     k41 = f1(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
  126.     k42 = f2(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
  127.     k43 = f3(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
  128.     k44 = f4(x1+dt*k31,  x2+dt*k32,  x3+dt*k33,  x4+dt*k34 )
  129.  
  130.     x1 = x1 + dx * (k11 + 2*k21 + 2*k31 + k41)
  131.     x2 = x2 + dx * (k12 + 2*k22 + 2*k32 + k42)
  132.     x3 = x3 + dx * (k13 + 2*k23 + 2*k33 + k43)
  133.     x4 = x4 + dx * (k14 + 2*k24 + 2*k34 + k44)
  134.  
  135.  
  136.     # Point plot
  137.     set label 2*i+3 at t, x1 point lc 6 ps grar pt 7
  138.     set label 2*i+4 at t, x3 point lc 0 ps grar pt 7
  139.  
  140.     # Decimate and draw
  141.     if(i%cut==0){
  142.         plot 1/0 lw 0 lc rgb 'white' t " ",\
  143.              1/0 lw 4 lc 6 t "{/symbol:italic=24 q} ",\
  144.              1/0 lw 0 lc rgb 'white' t " ",\
  145.              1/0 lw 4 lc 0 t "{/symbol:italic=24 f} "
  146.     }
  147. }
  148. set out

graph 5000-0100-0100-0100.gif

Reference

Wikipedia
Segway

Search This Blog

Translate

QooQ