0
$\begingroup$

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?

Improve this question
$\endgroup$
1
$\begingroup$

I think this question doesn't belong here, it is more suitable to the stackoverflow. To the problem now, first of all I assume you are making a mistake by mixing degrees with radians. In this part of the code 

 if delta > 180:
        delta = delta - math.radians(360)

delta seems to be in degrees and then you are trying to normalize it by subtracting radians (hint you might get angles that are even bigger than 360 so you should make a loop to check it out).

While here:

  sf = delta - (a*dtheta)/vlon

you are using it as a number(radians).

Please check it out, and upload the source where you got the equations from because it is realy hard to follow your code.

Improve this answer
$\endgroup$
7
  • $\begingroup$ I changed the if statement to radians but it did not fix my problem. I got the equations from this document - vtechworks.lib.vt.edu/bitstream/handle/10919/36615/…. Please note I used a simplified tyre model to calculate the lateral forces and I assumed infinite friction so the friction circle was not needed. $\endgroup$ – Tyrone Sep 27 '19 at 7:09
  • $\begingroup$ When computing ddtheta you don't use the exact formula, you made a mistake it should be ddtheta = (a*pf*delta + b *ff - b*fr)/I according to the paper. Also dvlon = (pf + pr + ff*delta)/m - vlat*dtheta. Added to this I think you first have to update the dtheta and with the new value the theta. dtheta = dtheta + ddtheta*dt theta = theta + dtheta*dt + (1/2)*ddtheta*dt**2. This was without getting too deep in the paper, hope it works, and please be a bit more careful with how you copy paste the code. $\endgroup$ – nionios Sep 29 '19 at 21:12
  • $\begingroup$ Yes, for the ddtheta = (a*pf*delta + b *ff - b*fr)/I this is an error in the paper, if you derive the equation it comes to what I use, he uses the same formula at the bottom of the document, I referenced the equations I used because they are easier to follow. $\endgroup$ – Tyrone Oct 4 '19 at 12:57
  • $\begingroup$ @Tyrone but still do you update the dtheta before making the other calculations? $\endgroup$ – nionios Oct 6 '19 at 20:06
  • $\begingroup$ I tried changing them around but it only made things worse. To test it I just set the steering angle, delta to a constant and give it a starting velocity vlon. The scales of my axes when testing for 30 secongs goes to 1e140 somehow. This is the same for when the theta calculations were the other way around. $\endgroup$ – Tyrone Oct 7 '19 at 13:58

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.