離散経路からMPCのターゲット(目標値)を取得する手法

def calc_ref_trajectory(state, cx, cy, cyaw, ck, sp, dl, pind):
    xref = np.zeros((NX, T + 1))
    dref = np.zeros((1, T + 1))
    ncourse = len(cx)

    ind, _ = calc_nearest_index(state, cx, cy, cyaw, pind)

    if pind >= ind:
        ind = pind

    xref[0, 0] = cx[ind]
    xref[1, 0] = cy[ind]
    xref[2, 0] = sp[ind]
    xref[3, 0] = cyaw[ind]
    dref[0, 0] = 0.0  # steer operational point should be 0

    travel = 0.0

    for i in range(T + 1):
        travel += abs(state.v) * DT
        dind = int(round(travel / dl))

        if (ind + dind) < ncourse:
            xref[0, i] = cx[ind + dind]
            xref[1, i] = cy[ind + dind]
            xref[2, i] = sp[ind + dind]
            xref[3, i] = cyaw[ind + dind]
            dref[0, i] = 0.0
        else:
            xref[0, i] = cx[ncourse - 1]
            xref[1, i] = cy[ncourse - 1]
            xref[2, i] = sp[ncourse - 1]
            xref[3, i] = cyaw[ncourse - 1]
            dref[0, i] = 0.0

    return xref, ind, dref

経路

$$ Path=\begin{bmatrix} x_0 & x_1 &\\y_0 & y_1 & …\\ \theta_0 & \theta_1 &\\vx_0 & vx_1 &\\vy_0 & vy_1 & …\\ \omega_0 & \omega_1 &\\\end{bmatrix} $$

目標経路

$$ Xref=\begin{bmatrix} x_{t0} & x_{t1} &\\y_{t0} & y_{t1} & …\\ \theta_{t0} & \theta_{t1} &\\vx_{t0} & vx_{t1} &\\vy_{t0} & vy_{t1} & …\\ \omega_{t0} & \omega_{t1} &\\\end{bmatrix} $$

離散経路Pathから目標値を抜き出す.

制御対象の現在地$x^T=(x_t,y_t,\theta_t)$をもとにPathの中で最も近い点を抜きだし，

$Xref$の$t0$の要素とする．

$t0$の要素から予測ホライゾンの長さまでの目標値を取り出す．

このとき，離散点の間隔は制御周期$dt$になるようにする必要がある．

現在の速度で制御周期時間の間に進むことのできる距離を$dd$経路の離散点の間隔 $dD$とする．

$$ dd=v_{now}*dt\\dD=\sqrt{(x_{t1}-x_{t0})^2+(y_{t1}-y_{t0})^2} $$

dD<dd

離散経路から目標値を取ってくるときのindexの間隔は以下になる．

$$ dind=round(\frac{dd}{dD}) $$

dD>dd

直線補間でdD間の目標値を補間する．

補間点の数は以下になる

$$ dN=round(\frac{dD}{dd}) $$