無人運転アルゴリズム-パストラッキングにスタンレー法を使用

1.幾何学的追跡に基づく方法

無人機の軌道に関して、現在の主流の方法は、幾何学的追跡ベースの方法とモデル予測ベースの方法の2つのカテゴリに分けられます。幾何学的追跡方法には、主に純粋な追跡とスタンレーの方法が含まれます。純粋な追跡方法は、モバイルで広く使用されています。ロボットの経路追跡に関する多くの詳細な紹介があります。この記事では、主にスタンフォード大学の無人機で使用されているスタンレー法を紹介します。

スタンレー法幾何モデル図

上記のstate.yawは、現在のハンドル角度制御デルタ、速度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値を小さくして追跡軌道を滑らかにすることができますが、結果が滑らかになると、一部の鋭いコーナーでアンダーステアが発生します。

参照

自動運転経路追跡のための自動操舵方法
Stanley_DARPAグランドチャレンジで優勝したロボット
無人車両システムの概要（18）-純粋な追跡を使用して、無人車両の軌道追跡を実現します（https://blog.csdn.net/adamshan/article/details/80555174）
無人運転アルゴリズム-スタンレー法を使用して無人車両の軌道追跡を実現

無人運転アルゴリズム-パストラッキングにスタンレー法を使用

おすすめ