add unicycle simulator

steer_vehicle_model/unicycle_model.py

@@ -0,0 +1,67 @@
+#! /usr/bin/python
+# -*- coding: utf-8 -*-
+"""
+
+
+author Atsushi Sakai
+"""
+
+import math
+
+dt = 0.1  # [s]
+L = 5.0  # [m]
+
+
+class State:
+
+    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.v = v
+
+
+def update(state, a, delta):
+
+    state.x = state.x + state.v * math.cos(state.yaw) * dt
+    state.y = state.y + state.v * math.sin(state.yaw) * dt
+    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
+    state.v = state.v + a * dt
+
+    return state
+
+
+if __name__ == '__main__':
+    print("start unicycle simulation")
+    import matplotlib.pyplot as plt
+
+    T = 100
+    a = [1.0] * T
+    delta = [math.radians(1.0)] * T
+    #  print(a, delta)
+
+    state = State()
+
+    x = []
+    y = []
+    yaw = []
+    v = []
+
+    for (ai, di) in zip(a, delta):
+        state = update(state, ai, di)
+
+        x.append(state.x)
+        y.append(state.y)
+        yaw.append(state.yaw)
+        v.append(state.v)
+
+    flg, ax = plt.subplots(1)
+    plt.plot(x, y)
+    plt.axis("equal")
+    plt.grid(True)
+
+    flg, ax = plt.subplots(1)
+    plt.plot(v)
+    plt.grid(True)
+
+    plt.show()

steer_vehicle_model/vehicle_model_note.ipynb

@@ -7,7 +7,7 @@
    },
    "source": [
     "# Table of Contents\n",
-    " <p><div class=\"lev1\"><a href=\"#Unicycle-Model\"><span class=\"toc-item-num\">1&nbsp;&nbsp;</span>Unicycle Model</a></div>"
+    " <p><div class=\"lev1\"><a href=\"#Unicycle-Model\"><span class=\"toc-item-num\">1&nbsp;&nbsp;</span>Unicycle Model</a></div><div class=\"lev1\"><a href=\"#Kinematic-bicycle-Model\"><span class=\"toc-item-num\">2&nbsp;&nbsp;</span>Kinematic bicycle Model</a></div><div class=\"lev1\"><a href=\"#Dynamic-bicycle-Model\"><span class=\"toc-item-num\">3&nbsp;&nbsp;</span>Dynamic bicycle Model</a></div>"
    ]
   },
   {
@@ -20,12 +20,40 @@
     "シンプルだが、速度ベクトルが車両の進行方向と同じなので少し無理がある\n",
     "\n",
     "$$\n",
-    "x_{k+1} = x_{k} + v_k cos(\\phi_k)dt \\\\\n",
-    "y_{k+1} = y_{k} + v_k sin(\\phi_k)dt \\\\\n",
-    "\\phi_{k+1} = \\phi_{k} + \\frac{v_{k}}{L}tan(\\delta_t) \\\\\n",
+    "x_{k+1} = x_{k} + v_k cos(\\phi_k)dt\n",
+    "$$\n",
+    "$$\n",
+    "y_{k+1} = y_{k} + v_k sin(\\phi_k)dt\n",
+    "$$\n",
+    "$$\n",
+    "\\phi_{k+1} = \\phi_{k} + \\frac{v_{k}}{L}tan(\\delta_t)$$\n",
+    "$$\n",
     "v_{k+1} = v_{k} + a_k dt \\\\\n",
     "$$"
    ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Kinematic bicycle Model\n",
+    "Unicycle Modelと比べて、車両のすべり角は考慮しているが、車輪のすべり角は考慮していない。\n",
+    "速度が15km/h以下の時には有効らしい (Parking などでは有効らしい)\n",
+    "\n",
+    "$$ x_{k+1} = x_{k} + v_k cos(\\phi_k+\\beta_k)dt $$\n",
+    "$$ y_{k+1} = y_{k} + v_k sin(\\phi_k+\\beta_k)dt $$\n",
+    "$$ \\phi_{k+1} = \\phi_{k} + \\frac{v_{k}}{L_r}sin(\\beta_t) $$\n",
+    "$$ v_{k+1} = v_{k} + a_k dt $$\n",
+    "\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Dynamic bicycle Model"
+   ]
   }
  ],
  "metadata": {