python - 我无法让我的 3 DOF 自行车模型工作

Question

我正在尝试使用动态自行车模型来模拟车辆，但我似乎无法让它工作。如果我设置一个恒定的转向角，横向速度会呈指数增长并产生不可能的结果。

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

和是前后轴到车辆重心的距离，a是质量和惯性。是全局位置，是航向，是转向角。我从这个文件中得到了这些方程。请注意，我使用简化的轮胎模型来计算侧向力，并假设摩擦力无限大，因此不需要摩擦圆。bmIxythetadelta

有什么我缺少的东西来完成这项工作吗？

标签： pythonsimulation