# Unicycle Model

\begin{align} x_{k+1} &= x_{k} + v_k cos(\phi_k)dt\\ y_{k+1} &= y_{k} + v_k sin(\phi_k)dt\\ \phi_{k+1} &= \phi_{k} + \frac{v_{k}}{L}tan(\delta_t)dt\\ v_{k+1} &= v_{k} + a_k dt \\ \end{align}

# Kinematic bicycle Model

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}

# Dynamic bicycle Model

\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}

# Compare Model



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

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)




0.0
6.0189879062819264e-05
0.00018056963718845778
0.0003611392743769156
0.0006018987906281927
0.0009028481859422891
0.0012639874603192047
0.0016853166137589396
0.0021668356462614937
0.002708544557826867
0.0033104433484550597
0.003972532018146071
0.004694810566899903
0.005477278994716553
0.006319937301596024
0.0072227854875383125
0.00818582355254342
0.009209051496611349
0.010292469319742096
0.011436077021935663
0.012639874603192047
0.013903862063511251
0.015228039402893275
0.01661240662133812
0.018056963718845784
0.019561710695416266
0.021126647551049565
0.022751774285745686
0.024437090899504625
0.026182597392326385
0.027988293764210963
0.02985418001515836
0.03178025614516858
0.03376652215424161
0.03581297804237747
0.03791962380957615
0.04008645945583764
0.042313484981161956
0.044600700385549086
0.04694810566899904
0.04935570083151181
0.0518234858730874
0.05435146079372581
0.05693962559342704
0.05958798027219109
0.062296524830017956
0.06506525926690765
0.06789418358286016
0.07078329777787548
0.07373260185195363
0.07674209580509458
0.07981177963729837
0.08294165334856497
0.08613171693889439
0.08938197040828663
0.09269241375674168
0.09606304698425956
0.09949387009084026
0.10298488307648378
0.10653608594119011
0.11014747868495926
0.11381906130779124
0.11755083380968603
0.12134279619064364
0.12519494845066406
0.12910729058974732
0.1330798226078934
0.13711254450510227
0.141205456281374
0.14535855793670852
0.14957184947110586
0.153845330884566
0.158179002177089
0.1625728633486748
0.1670269143993234
0.17154115532903486
0.17611558613780912
0.1807502068256462
0.1854450173925461
0.19020001783850882
0.19501520816353435
0.1998905883676227
0.20482615845077387
0.20982191841298786
0.21487786825426466
0.2199940079746043
0.22517033757400676
0.23040685705247202
0.2357035664100001
0.241060465646591
0.24647755476224473
0.25195483375696126
0.25749230263074063
0.2630899613835828
0.2687478100154878
0.2744658485264556
0.28024407691648623
0.2860824951855797
0.291981103333736
0.2979399013609551




In [3]:

%timeit dynamic_bicyccle_model.update(dstate,ai,di)




100000 loops, best of 3: 4.47 µs per loop




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.legend()
plt.grid(True)
plt.show()



# linealize Dynamic Bicycle Model memo



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]:

$$vx \cos{\left (\phi \right )} - vy \sin{\left (\phi \right )}$$




In [11]:

sympy.diff(f,x)




Out[11]:

$$0$$




In [12]:

sympy.diff(f,y)




Out[12]:

$$0$$




In [13]:

sympy.diff(f,phi)




Out[13]:

$$- vx \sin{\left (\phi \right )} - vy \cos{\left (\phi \right )}$$




In [14]:

sympy.diff(f,vy)




Out[14]:

$$- \sin{\left (\phi \right )}$$




In [15]:

sympy.diff(f,vx)




Out[15]:

$$\cos{\left (\phi \right )}$$




In [16]:

f2 = vx*sympy.sin(phi) - vy*sympy.cos(phi)
print(f2)




vx*sin(phi) - vy*cos(phi)




In [17]:

sympy.diff(f2,phi)




Out[17]:

$$vx \cos{\left (\phi \right )} + vy \sin{\left (\phi \right )}$$




In [18]:

fvx = a + sympy.sin(d) * cf / m * ((vy+Lf*w)/vx - d) + vy*w
fvx




Out[18]:

$$a + \frac{cf}{m} \left(- d + \frac{1}{vx} \left(Lf w + vy\right)\right) \sin{\left (d \right )} + vy w$$




In [19]:

sympy.diff(fvx,vx)




Out[19]:

$$- \frac{cf}{m vx^{2}} \left(Lf w + vy\right) \sin{\left (d \right )}$$




In [20]:

sympy.diff(fvx,vy)




Out[20]:

$$\frac{cf}{m vx} \sin{\left (d \right )} + w$$




In [21]:

sympy.diff(fvx,w)




Out[21]:

$$\frac{Lf cf}{m vx} \sin{\left (d \right )} + vy$$




In [22]:

sympy.diff(fvx,x)




Out[22]:

$$0$$




In [23]:

sympy.diff(fvx,d)




Out[23]:

$$\frac{cf}{m} \left(- d + \frac{1}{vx} \left(Lf w + vy\right)\right) \cos{\left (d \right )} - \frac{cf}{m} \sin{\left (d \right )}$$




In [24]:

fvy = - cr / m * (vy-Lr*w)/vx + sympy.cos(d)/m*((vy+Lf*w)/vx - d) -vx*w
fvy




Out[24]:

$$- \frac{cr}{m vx} \left(- Lr w + vy\right) - vx w + \frac{1}{m} \left(- d + \frac{1}{vx} \left(Lf w + vy\right)\right) \cos{\left (d \right )}$$




In [25]:

sympy.diff(fvy, vx)




Out[25]:

$$\frac{cr}{m vx^{2}} \left(- Lr w + vy\right) - w - \frac{\cos{\left (d \right )}}{m vx^{2}} \left(Lf w + vy\right)$$




In [26]:

sympy.diff(fvy, vy)




Out[26]:

$$- \frac{cr}{m vx} + \frac{\cos{\left (d \right )}}{m vx}$$




In [27]:

sympy.diff(fvy, w)




Out[27]:

$$\frac{Lf}{m vx} \cos{\left (d \right )} + \frac{Lr cr}{m vx} - vx$$




In [28]:

sympy.diff(fvy,d)




Out[28]:

$$- \frac{1}{m} \left(- d + \frac{1}{vx} \left(Lf w + vy\right)\right) \sin{\left (d \right )} - \frac{1}{m} \cos{\left (d \right )}$$




In [29]:

fw = -cf/Iz*((vy+Lf*w)/vx-d)*Lf*sympy.cos(d)+cr/Iz*(vy-Lr*w)/vx*Lr
fw




Out[29]:

$$- \frac{Lf cf}{Iz} \left(- d + \frac{1}{vx} \left(Lf w + vy\right)\right) \cos{\left (d \right )} + \frac{Lr cr}{Iz vx} \left(- Lr w + vy\right)$$




In [30]:

sympy.diff(fw,vx)




Out[30]:

$$\frac{Lf cf}{Iz vx^{2}} \left(Lf w + vy\right) \cos{\left (d \right )} - \frac{Lr cr}{Iz vx^{2}} \left(- Lr w + vy\right)$$




In [31]:

sympy.diff(fw,vy)




Out[31]:

$$- \frac{Lf cf}{Iz vx} \cos{\left (d \right )} + \frac{Lr cr}{Iz vx}$$




In [32]:

sympy.diff(fw,w)




Out[32]:

$$- \frac{Lf^{2} cf}{Iz vx} \cos{\left (d \right )} - \frac{Lr^{2} cr}{Iz vx}$$




In [33]:

sympy.diff(fw,d)




Out[33]:

$$\frac{Lf cf}{Iz} \left(- d + \frac{1}{vx} \left(Lf w + vy\right)\right) \sin{\left (d \right )} + \frac{Lf cf}{Iz} \cos{\left (d \right )}$$