無人駕駛算法-使用Stanley method進行路徑跟蹤

 

一、基於幾何追蹤的方法

關於無人車追蹤軌跡，目前的主流方法分爲兩類：基於幾何追蹤的方法和基於模型預測的方法，其中幾何追蹤方法主要包含純跟蹤和Stanley兩種方法，純跟蹤方法已經廣泛應用於移動機器人的路徑跟蹤中，網上也有很多詳細的介紹，本文主要介紹斯坦福大學無人車使用的Stanley方法。

上面的state.yaw根據當前車輛方向盤轉角控制量delta跟速度v、半徑長度L得到當前車輛在dt時間轉過的角度，即，新增的航向角。

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

定義函數用於搜索最臨近的路點：

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

主函數：

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

仿真結果：

通過仿真結果圖，我們看到紅點表示實現規劃好的直線路徑，藍線則表示我們的車輛實際運行的軌跡，綠點表示距離當前位置最近路徑點，在這段代碼中，我們設置了增益參數kkk爲0.3，通過進一步去改變kkk的大小對比結果，可以減小kkk值得到更平滑的跟蹤軌跡，但是更加平滑的結果會引起某些急劇的轉角處會存在轉向不足的情況。

參考

1. Automatic Steering Methods for Autonomous Automobile Path Tracking
2. Stanley_ The robot that won the DARPA grand challenge
3. 無人駕駛汽車系統入門（十八）——使用pure pursuit實現無人車軌跡追蹤(https://blog.csdn.net/adamshan/article/details/80555174)
4. 無人駕駛算法——使用Stanley method實現無人車軌跡追蹤