I'm trying to simulate a vehicle using a dynamic bicycle model but I cannot seem to get it working. If I set a constant steering angle the lateral velocity grows exponentially and creates impossible results.
a = 0.34284
b = 0.40716
m = 155
I = 37.29
def f_DynBkMdl(x, y, delta, theta, dt, states):
dtheta = states[0]
vlat = states[1]
vlon = states[2]
if delta > math.radians(180):
delta = delta - math.radians(360)
if delta<0:
j = 1
else:
j = 0
if dtheta<0:
q = 1
else:
q = 0
dtheta = abs(dtheta)
delta = abs(delta)
sf = delta - (a*dtheta)/vlon
ff = 30.77*math.degrees(sf)
pf = 0
sr = (b*dtheta)/vlon
fr = 30.77*math.degrees(sr)
pr = 0
if j == 1:
fr = -fr
ff = -ff
if q == 1:
dtheta = -dtheta
theta = theta%math.radians(360)
ddtheta = (a*pf*delta + a*ff - b*fr)/I
dvlat = (pf*delta + ff + fr)/m - vlon*dtheta
dvlon = (pf + pr - ff*delta)/m - vlat*dtheta
dx = -vlat*np.sin(theta) + vlon*np.cos(theta)
dy = vlat*np.cos(theta) + vlon*np.sin(theta)
theta = theta + dtheta*dt + (1/2)*ddtheta*dt**2
dtheta = dtheta + ddtheta*dt
vlat = vlat + dvlat*dt
vlon = vlon + dvlon*dt
vabs = np.sqrt(vlat**2 + vlon**2)
x = x + dx*dt
y = y + dy*dt
states = [dtheta, vlat, vlon]
array = np.array([x, y, theta, vabs, states])
return array
With a
and b
being the distance between the front and rear axle to the vehicle's centre of gravity, m
being the mass and I
the inertia.
x
and y
are the global position and theta
is the heading with delta
being the steering angle.
I obtained my equations from this document https://vtechworks.lib.vt.edu/bitstream/handle/10919/36615/Chapter2a.pdf under the headings 2.3.1, 2.3.2 and 2.3.3. I used a simplified tyre model and assumed infinite friction so the friction circle is not required.
Is there something I am missing to make this work?