vehicle_model_note


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

高速で走行するモデルには基本的にこれをつかう 分母に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}

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
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)


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.ylabel("Yaw[rad]")
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 )}$$