Unmanned driving algorithm-using Stanley method for path tracking

 

1. Method based on geometric tracking

Regarding the trajectory of unmanned vehicles, the current mainstream methods are divided into two categories: geometric tracking-based methods and model prediction-based methods. Geometric tracking methods mainly include pure tracking and Stanley methods. Pure tracking methods have been widely used in mobile There are also many detailed introductions on the path tracking of robots. This article mainly introduces the Stanley method used by Stanford University's unmanned vehicles.

The above state.yaw obtains the angle that the current vehicle has turned in time dt according to the current steering wheel angle control delta, speed v, and radius L, that is, the new heading angle.

def PControl(target, current):
    a = Kp * (target - current)
    return a


def stanley_control(state, cx, cy, ch, pind):
    ind = calc_target_index(state, cx, cy)

    if pind >= ind:
        ind = pind

    if ind < len(cx):
        tx = cx[ind]
        ty = cy[ind]
        th = ch[ind]
    else:
        tx = cx[-1]
        ty = cy[-1]
        th = ch[-1]
        ind = len(cx) - 1

    # 计算横向误差
    if ((state.x - tx) * th - (state.y - ty)) > 0:
        error = abs(math.sqrt((state.x - tx) ** 2 + (state.y - ty) ** 2))
    else:
        error = -abs(math.sqrt((state.x - tx) ** 2 + (state.y - ty) ** 2))
    #此路线节点期望的航向角减去当前车辆航向角(航向偏差)，然后再加上横向偏差角即match.atan()得到的值
    #得到的delta即为控制车辆方向盘的控制量
    delta = ch[ind] - state.yaw + math.atan2(k * error, state.v)

    #  限制车轮转角 [-30, 30]
    if delta > np.pi / 6.0:
        delta = np.pi / 6.0
    elif delta < - np.pi / 6.0:
        delta = - np.pi / 6.0
    return delta, ind

Define the function to search for the nearest waypoint:

def calc_target_index(state, cx, cy):
    # 搜索最临近的路点
    dx = [state.x - icx for icx in cx]
    dy = [state.y - icy for icy in cy]
    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
    ind = d.index(min(d))

    return ind

Main function:

def main():
    #  设置目标路点
    cx = np.arange(0, 50, 1)
    cy = [0 * ix for ix in cx]
    #路径的结点处的航向角，指的是车身整体
    ch = [0 * ix for ix in cx]  
    target_speed = 5.0 / 3.6  # [m/s]
    T = 200.0  # 最大模拟时间
    # 设置车辆的初始状态
    state = VehicleState(x=-0.0, y=-3.0, yaw=-0.0, v=0.0)
    lastIndex = len(cx) - 1
    time = 0.0
    x = [state.x]
    y = [state.y]
    #当前车身的航向角
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    target_ind = calc_target_index(state, cx, cy)

    while T >= time and lastIndex > target_ind:
        ai = PControl(target_speed, state.v)
        di, target_ind = stanley_control(state, cx, cy, ch, target_ind)
        state = update(state, ai, di)
        time = time + dt

        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)

        plt.cla()
        plt.plot(cx, cy, ".r", label="course")
        plt.plot(x, y, "-b", label="trajectory")
        plt.plot(cx[target_ind], cy[target_ind], "go", label="target")
        plt.axis("equal")
        plt.grid(True)
        plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
        plt.pause(0.001)


if __name__ == '__main__':
    main()

Simulation results:

Through the simulation result graph, we see that the red dot represents the planned straight path, the blue line represents the actual trajectory of our vehicle, and the green dot represents the path point closest to the current position. In this code, we set the gain The parameter kkk is 0.3. By further changing the comparison result of kkk, the kkk value can be reduced to obtain a smoother tracking trajectory, but the smoother result will cause understeer at some sharp corners.

reference

1. Automatic Steering Methods for Autonomous Automobile Path Tracking
2. Stanley_ The robot that won the DARPA grand challenge
3. Introduction to Unmanned Vehicle System (18)-Use pure pursuit to realize unmanned vehicle trajectory tracking (https://blog.csdn.net/adamshan/article/details/80555174)
4. Unmanned driving algorithm-using Stanley method to achieve unmanned vehicle trajectory tracking