Unicycle Modelと比べて、車両のすべり角は考慮しているが、車輪のすべり角は考慮していない。 速度が15km/h以下の時には有効らしい (Parking などでは有効らしい)
\begin{align} x_{k+1} &= x_{k} + v_k cos(\phi_k+\beta_k)dt \\ y_{k+1} &= y_{k} + v_k sin(\phi_k+\beta_k)dt\\ \phi_{k+1} &= \phi_{k} + \frac{v_{k}}{L_r}sin(\beta_k)dt\\ v_{k+1} &= v_{k} + a_k dt\\ \beta_{k} &= tan^{-1}(\frac{L_r}{L_f+L_r}tan(\delta_{f,k})) \end{align}高速で走行するモデルには基本的にこれをつかう 分母にv_xがあるため、速度が0の時に計算できなくなる。
\begin{align} x_{k+1} &= x_{k} + v_{x,k} cos(\phi_k)dt - v_{y,k} sin(\phi_k)dt \\ y_{k+1} &= y_{k} + v_{x,k} sin(\phi_k)dt + v_{y,k} cos(\phi_k)dt\\ \phi_{k+1} &= \phi_{k} + \omega_k dt\\ F_{fy,k} &= -C_f( \frac{v_{y,k}+L_f\omega_k}{v_{x,k}}-\delta_t) \\ F_{ry,k} &= -C_r( \frac{v_{y,k}-L_r\omega_k}{v_{x,k}}) \\ v_{x,k+1} &= v_{x,k} + (a_k-\frac{F_{fy,k}sin(\delta_{k})}{m}+v_{y,k}\omega_k)dt\\ v_{y,k+1} &= v_{y,k} + (\frac{F_{ry,t}}{m}+\frac{F_{fy,k}cos(\delta_{k})}{m}-v_{x,k}\omega_k)dt\\ \omega_{k+1} &= \omega_{k} + \frac{dt}{I_z}(F_{fy,t}L_fcos(\delta_{t})-F_{ry,t}L_r) \end{align}
In [2]:
#%matplotlib inline
import matplotlib.pyplot as plt
import numpy as np
import unicycle_model
import kinematic_bicycle_model
import dynamic_bicyccle_model
import math
T = 100
a = [1.0] * T
delta = [math.radians(1.0)] * T
ustate = unicycle_model.State()
kstate = kinematic_bicycle_model.State()
dstate = dynamic_bicyccle_model.State()
ux,uy,uyaw,uv, = [],[],[],[]
dx,dy,dyaw,dv, = [],[],[],[]
kx,ky,kyaw,kv, kbeta= [],[],[],[],[]
time = []
t = 0.0
for (ai, di) in zip(a, delta):
t = t + unicycle_model.dt
time.append(t)
ustate = unicycle_model.update(ustate, ai, di)
ux.append(ustate.x+kinematic_bicycle_model.Lr*math.cos(ustate.yaw))
uy.append(ustate.y+kinematic_bicycle_model.Lr*math.sin(ustate.yaw))
uyaw.append(ustate.yaw)
uv.append(ustate.v)
kstate = kinematic_bicycle_model.update(kstate, ai, di)
kx.append(kstate.x)
ky.append(kstate.y)
kyaw.append(kstate.yaw)
kv.append(kstate.v)
kbeta.append(kstate.beta)
dstate = dynamic_bicyccle_model.update(dstate, ai, di)
dx.append(dstate.x)
dy.append(dstate.y)
dyaw.append(dstate.yaw)
In [3]:
%timeit dynamic_bicyccle_model.update(dstate,ai,di)
In [6]:
dynamic_bicyccle_model?
In [15]:
plt.plot(ux,uy,label="unicycle_model")
plt.plot(kx,ky,label="kinematic_bicycle_model")
plt.plot(dx,dy,label="dynamic_bicyccle_model")
plt.axis("equal")
plt.xlabel("X[m]")
plt.ylabel("Y[m]")
plt.legend()
plt.grid(True)
plt.show()
In [ ]:
plt.plot(ux,uy,label="unicycle_model")
plt.plot(kx,ky,label="kinematic_bicycle_model")
plt.axis("equal")
plt.xlabel("X[m]")
plt.ylabel("Y[m]")
plt.legend()
plt.grid(True)
plt.show()
In [19]:
plt.plot(time, uyaw,label="unicycle_model")
plt.plot(time, kyaw,label="kinematic_bicycle_model")
plt.xlabel("Time[s]")
plt.ylabel("Yaw[rad]")
plt.legend()
plt.grid(True)
plt.show()
In [9]:
%matplotlib inline
In [10]:
import sympy
from sympy import init_printing
init_printing()
x,y,vx, vy, phi, a, d, cf, cr, m, Lf, Lr, w,Iz = sympy.symbols('x y vx vy phi a d cf cr m Lf Lr, w, Iz')
f = vx*sympy.cos(phi) - vy*sympy.sin(phi)
f
Out[10]:
In [11]:
sympy.diff(f,x)
Out[11]:
In [12]:
sympy.diff(f,y)
Out[12]:
In [13]:
sympy.diff(f,phi)
Out[13]:
In [14]:
sympy.diff(f,vy)
Out[14]:
In [15]:
sympy.diff(f,vx)
Out[15]:
In [16]:
f2 = vx*sympy.sin(phi) - vy*sympy.cos(phi)
print(f2)
In [17]:
sympy.diff(f2,phi)
Out[17]:
In [18]:
fvx = a + sympy.sin(d) * cf / m * ((vy+Lf*w)/vx - d) + vy*w
fvx
Out[18]:
In [19]:
sympy.diff(fvx,vx)
Out[19]:
In [20]:
sympy.diff(fvx,vy)
Out[20]:
In [21]:
sympy.diff(fvx,w)
Out[21]:
In [22]:
sympy.diff(fvx,x)
Out[22]:
In [23]:
sympy.diff(fvx,d)
Out[23]:
In [24]:
fvy = - cr / m * (vy-Lr*w)/vx + sympy.cos(d)/m*((vy+Lf*w)/vx - d) -vx*w
fvy
Out[24]:
In [25]:
sympy.diff(fvy, vx)
Out[25]:
In [26]:
sympy.diff(fvy, vy)
Out[26]:
In [27]:
sympy.diff(fvy, w)
Out[27]:
In [28]:
sympy.diff(fvy,d)
Out[28]:
In [29]:
fw = -cf/Iz*((vy+Lf*w)/vx-d)*Lf*sympy.cos(d)+cr/Iz*(vy-Lr*w)/vx*Lr
fw
Out[29]:
In [30]:
sympy.diff(fw,vx)
Out[30]:
In [31]:
sympy.diff(fw,vy)
Out[31]:
In [32]:
sympy.diff(fw,w)
Out[32]:
In [33]:
sympy.diff(fw,d)
Out[33]: