碼上快樂

相關內容    簡體    繁體

【路徑規划】 Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame (附python代碼實例)

本文轉載自   查看原文   2021-07-13 16:14   187    論文閱讀

參考與前言

  1. 2010年，論文 Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame 地址：https://www.researchgate.net/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame
  2. Python代碼示意地址：https://gitee.com/mirrors/PythonRobotics/tree/master/PathPlanning/FrenetOptimalTrajectory
  3. csdn上matlab解釋：https://blog.csdn.net/caokaifa/article/details/108015374
  4. 格式更好看的notion外鏈版：https://www.notion.so/kinzhang/Frenet-Optimal-Trajectory-Generation-96d5bced98c146d2a9036a1d5fc2b21d
    后續更新也主要在notion上

主要是自己一直聽過這個frenet的規划... 但是一直沒了解到底是個啥，趁着這次傑哥的指點，順便套娃一下基礎知識好了。關於規划的使用中，19年的時候做二維grid map 小小圓盤二輪車用的Dijkstra/A*做的全局，然后局部是DWA（但是當時實驗時車子總是蛇皮走位）照現在的感覺應該是膨脹系數給大了，DWA更新快了？然后路徑規划完哎往牆走，哎躲個牆，哎再往牆走，就這種蛇皮 emmm 這么說好像還是不是很清楚原因，放着萬一啥時候想通了呢 hhhh。這次這個主要是基礎系列補... 補... 補

基礎知識

因為基礎補充，所以就... 慢慢來了，就當把自己的理解總結一下好了..

Frenet坐標系

首先是關於這個坐標系的，一般呢我們用的是笛卡爾坐標系（大白話 xy坐標軸），然后這個的不同之處就是按照你的那個車的路徑定的，縱向距離叫s，橫向距離叫d。例如此手畫圖：[這里的橫縱是指車子在開的方向，車頭前方是縱，方向盤的就是橫]

在這里插入圖片描述

手畫示意

python代碼運行示意

  • [x] 所以這個在一開始定全局路徑規划線的時候就是定frenet坐標系的時候嘛？中途還會有變化嘛？

    中途會有變換，比如這里提到的換道等..

感覺這個在生成軌跡后的切換方面很舒服噢

SO(2)和SE(2)

來源：《現代機器人學》

在這里插入圖片描述

在這里插入圖片描述

也就是說SO是只關注於對比的旋轉，SE添加了向量坐標的變換

論文部分

摘要 Abstract

主要是為了解決高速公路的動態規划問題，同時對於市中心交通堵塞問題下的自動駕駛車的軌跡要求等。提出了 semi-reactive 軌跡生成方法，是結合了行為規划層的一種方法。方法上實現了在常規街道 使用Frenet坐標系采用最優控制策略以使得車輛能夠實現速度保持、匯入車道、跟車、停車、反應式避障等的長期目標

看上去感覺就像... emmm 實現了基本無人小車的功能？可能多列舉出來以行為動作形式展現

  • [x] semi-reactive是什么意思？怎么體現的？

    就是對周圍物體能做交互，做出反應

介紹 Introduction

Motivation : 現有的方法(DAPRA 2007里提出的 18 heuristics and conservative estimates)沒辦法在交通堵塞和高速公路上提供好的表現，所以需要引入軌跡，更清楚的解釋在時間t的規划和控制決策；方法主要是生成速度不變運動，然后再把速度和距離控制發到規划(planning)

Contributions : 策略上耦合了規划里面的反饋，這樣可以更好將 導航任務層 分子任務到 實時的軌跡運行中，隨后穩定由軌跡給出的跟蹤響應控制；對比與其他方法由系統觀測閉環控制，本文的方法主要在生成軌跡階段

  • [ ] [18] heuristics and conservative estimates 這個是啥方法？

  • [ ] velocity invariant movement

    雖然在后面的注釋中有，但是... 我還是看不懂這個注釋表達的velocity invariant movement是什么：It is highly desirable to generate lane change and merging maneuvers, which are timed completely independently from the absolute travelling speed.

方法 Methods

最優控制方法 Optimal Control Approach

首先將最優控制理論應用到軌跡生成方面的工作，有很多人提出了[13] [3]。而我們的工作主要是確保一旦找到這個最優軌跡，使用Bellman's 優化原則去保持住，也就是在每一步都跟隨之前定好的最優軌跡

相反的有些工作是直接拿樣條曲線去做軌跡擬合，然后主要去滿足一些確定的限制，優化問題的形式主要針對於曲線參數的選擇，但是這種方法下的缺點比較明顯，容易超調甚至是不穩定，比如這樣

在這里插入圖片描述

在最優控制方面，我們主要是遵循Bellman的最優化原則去選取cost function，軌跡在最小化cost的基礎上也能最大程度的接近車輛理想的開車形式

對於理想的開車形式定義：假設車輛有明確的關於離期望軌跡的橫向offset，在最初的變道或避障后應該要能返回原來的最優軌跡，權衡一些指標等去實現這一點

  1. compromise: 如果車輛距前車快行/減速，應慢慢減速下來而不是急剎
  2. ease and comfort: 這一點在數學上我們用jerk來指明，也就是加速度的導數，加速度隨時間變化的程度
  3. need time: 就是從起點到終點的時長

總的來說通用的限制優化問題不會受限於車輛場景變化，能在數值上求最優，但是問題總的來說復雜。而我們的方法給這個限制優化問題提供了合理的假設，然后得到一系列的優化解給到無限制的優化問題上，在這上面取滿足限制條件的解

Frenet坐標系下的Motion Planning

在跟蹤控制理論比較好用的方法是：Frenet坐標方法，他能在 \(S E(2):=S O(2) \times \mathbb{R}^{2}\) 特殊歐式子群里有很好的跟蹤性能(invariant tracking performance)

本文中在這種方法去結合橫向和縱向cost function以適應不同的場景去模仿人的開車行為，首先整體的圖如下。移動的參考坐標系由那一點的切向量 \(\vec{t}_{r}\) 和法向量 \(\vec{n}_{r}\) 來給出，其中左邊的center line是指在無障礙物空曠道路的理想軌跡

在這里插入圖片描述

先對比於直接在笛卡爾坐標系中生成軌跡，本文主要是在這個動態的參考系下對於 在center line的 \(\vec{r}\) 點和垂直offset距離是\(d\) 的軌跡 尋找一維的軌跡，這兩者的關系式為

\[\vec{x}(s(t), d(t))=\vec{r}(s(t))+d(t) \vec{n}_{r}(s(t)) \tag{1} \]

對於人類感知中我們也存在對縱向和橫向加速度變換的不同重視程度。因為上面的切向量和法向量變換比較快，所以我們就用前文提到的jerk \(s, d\) 來進行約束。五次多項式是用jerk-optimal 來連接 初始狀態\(P_{0}=\left[p_{0}, \dot{p}_{0}, \ddot{p}_{0}\right]\) 和 結束狀態 \(P_{1}=\left[p_{1}, \dot{p}_{1}, \ddot{p}_{1}\right]\) 之間的一維求解問題，更准確的說是 最小化在時間內最小化jerk平方的cost function

在這里插入圖片描述

這里進行了一個證明：在給定初始狀態和結束狀態，求解下面這個最小cost functional的結果也是一個五次多項式

\[C=k_{j} J_{t}+k_{t} g(T)+k_{p} h\left(p_{1}\right) \]

其中 \(g\) 和 \(h\) 是隨機的函數，\(k_j,k_t,k_p>0\)

  • [ ] invariant tracking performance這個是其他什么跟蹤性能指標嗎？不變性？是指穩定性嘛？

  • [x] 具體為什么能在frenet坐標系下 跟蹤控制理論好用呢？好像沒有證明？

    看完橫縱向大概知道了，因為對於求導速度的處理來說都很方便，這個沒有什么數值上的證明，就是方法上的便捷性吧

  • [x] 這里的表示？公式 感覺怪怪的

    是以center line作為坐標系原點，因為只有法向量方向發生改變所以是加上法向量的，然后帶上他現在的方向上

  • [ ] quintic polynomials are the jerk-optimal connection between x and x，這個connection是指？五次多項式的本質嘛

  • [x] 感覺這一塊沒有講清楚frenet坐標系下的Motion planning問題啊... 主要在於限制條件太單一，只有最小化jerk function

    然后再代入計算cd和cv的最小

橫向運動 Lateral Movement

高速下軌跡

因為要最小化這個jerk平方 在優化時我們在軌跡 \(s\) 中，我們選取最初狀態為\(D_{0}=\left[d_{0}, \dot{d}_{0}, \ddot{d}_{0}\right]\)從后面部分，可知沒有不連續的情況發生。對於優化本身來說，我們令\(\dot d_1=\ddot d_1=0\)，因為我們期望是能平行於center line的，接着確認 \(g(T)=T\) 和 \(h(d_1)=d_1^2\) 得到以下cos functional:

\[C_{d}=k_{j} J_{t}(d(t))+k_{t} T+k_{d} d_{1}^{2} \tag{2} \]

對於偏離center \(d=0\) 的我們需要加懲罰並使其慢慢收斂到求解，通過前面我們知道最優解是五次多項式，所以我們只需求出五次多項式的各個參數即可，然后最小化(2)中的 \(T\) ，最后檢查碰撞

相對比於直接顯式的計算最優軌跡然后再調整其系數，我們在第一步仿照[16] 整個軌跡是結合不同的結束狀態 \(d_i\) 和 \(T_j\) 來計算多條的，又因為前面的得出求導=0，對於多項式的求解中的狀態空間就變成了這樣

\[\left[d_{1}, \dot{d}_{1}, \ddot{d}_{1}, T\right]_{i j}=\left[d_{i}, 0,0, T_{j}\right] \]

在這里插入圖片描述

得出這上面的這么多條軌跡后，我們再取最小的那個cost。因為每一步我們都取的是最優解，那么剩下的每一個時間點也都會是最優解。那么整條軌跡就是最優的了

低速下軌跡

因為在高速下，可以認為 \(d(t)\) 和 \(s(t)\) 是獨立的，但是在低速下，這樣的假設忽略了車輛的non-holonomic性質，所以就算按照上面生成了最優軌跡也沒法被車輛所執行 曲率不達標，在這樣的情況下，我們將行為層換成了有速度閾值到考慮橫向的同時也考慮縱向運動

  • [x] 所以實際的計算是 多條軌跡，然后計算每條軌跡cost 選一條？
  • [ ] 這個non-holonomic性質體現在哪里？

縱向運動 Longitudinal Movement

不同於前面我們將時間/距離作為主要的評判標准，此處的評判標注主要是comfort和safety，所以我們引入了縱向的jerk 作為優化問題的考慮

因為保持距離，匯入車道和停車都是對於軌跡的特定未知的要求，也就是從現在狀態轉到縱向位置的變換 \(s_{target}(t)\) ，類似於橫向的 我們生成縱向的軌跡，從\(S_0=[s_0,\dot s_0,\ddot s_0]\) 到通過差分 \(\Delta s_i\) 和 \(T_j\) 得到結束狀態

\[\left[s_{1}, \dot{s}_{1}, \ddot{s}_{1}, T\right]_{i j}=\left[\left[s_{\text {target }}\left(T_{j}\right)+\Delta s_{i}\right], \dot{s}_{\text {target }}\left(T_{j}\right), \ddot{s}_{\text {target }}\left(T_{j}\right), T_{j}\right] \]

得出來的圖如下，最后每條軌跡通過cost function進行計算：

\[C_{t}=k_{j} J_{t}+k_{t} T+k_{s}\left[s_{1}-s_{d}\right]^{2} \]

在這里插入圖片描述

Following

安全跟車操作，安全距離也就是constant time gap law，所以車輛的立項s狀態就是：

\[s_{\text {target }}(t):=s_{l v}(t)-\left[D_{0}+\tau \dot{s}_{l v}(t)\right] \]

  • 其中\(D_0\) 和 \(\tau\) 都是常數項
  • \(s_{lv}\)和速度\(\dot s_{lv}\) 都是前車的位置和速度

其中前車的一些數據都是需要通過感知預測來獲得的，其中我們假設前車的加速度保持不變，\(\ddot{s}_{l v}(t)=\ddot{s}_{l v}\left(t_{0}\right)=\text { const. }\)

根據時間整合，關於前車的一些公式：

\[\begin{aligned}\dot{s}_{l v}(t) &=\dot{s}_{l v}\left(t_{0}\right)+\ddot{s}_{l v}\left(t_{0}\right)\left[t-t_{0}\right] \\s_{l v}(t) &=s_{l v}\left(t_{0}\right)+\dot{s}_{l v}\left(t_{0}\right)\left[t-t_{0}\right]+\frac{1}{2} \ddot{s}_{l v}\left(t_{0}\right)\left[t-t_{0}\right]^{2}\end{aligned} \]

然后時間求導，自身狀態的變換：

\[\begin{aligned}&\dot{s}_{\text {target }}(t)=\dot{s}_{l v}(t)-\tau \ddot{s}_{l v}(t) \\&\ddot{s}_{\text {target }}(t)=\ddot{s}_{l v}\left(t_{1}\right)-\tau {s}^{'''}_{l v}(t)=\ddot{s}_{l v}\left(t_{1}\right) .\end{aligned} \]

Merging and Stopping

此項為匯入車道和停車操作。根據上面的一系列分析，這個就比較好定了，首先我們的目標點位置：

\[s_{\text {target }}(t)=\frac{1}{2}\left[s_{a}(t)+s_{b}(t)\right] \]

Velocity Keeping

速度跟隨，主要是針對前方沒有車的場景，車輛的目的就應該不是去到某個點，而是速度保持，根據位置公式 只需要最后一項換一下，就是縱向的速度保持公式了

\[C_{v}=k_{j} J_{t}(s(t))+k_{t} T+k_{\dot{s}}\left[\dot{s}_{1}-\dot{s}_{d}\right]^{2} \]

在時間\(t_0\)初始位置：\(S_0=[s_0,\dot s_0, \ddot s_0]\)，在時間\(t_1=t_0+T\)的結束狀態\(S_1\)，得到的狀態變化就是這樣的：

\[\left[\dot{s}_{1}, \ddot{s}_{1}, T\right]_{i j}=\left[\left[\dot{s}_{d}+\Delta \dot{s}_{i}\right], 0, T_{j}\right] \text { , } \]

在這里插入圖片描述

  • [x] 咦 上面的橫向也是jerk呀，那不 就 沒 啥 區別了嘛？ emmm

    jerk的對象不一樣，一個是jerk 位置，一個是jerk 速度

  • [x] 前車加速度保持不變的操作有點學術了 不過10年的方法嘛 正常

結合縱向和橫向

最后，我們需要滿足的最小化的cost就是由縱向和橫向給出響應權重后的求和：

\[C_{tot}=k_{lat}C_{lat}+k_{lon}C_{lon} \]

注意，對於每一步我們都使用一個新的參考作為center line，也就是說每一次的初始化/變換車道/在低速和高速間切換，我們需要將現在的重點投影到新的center line上

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-9vPcVYYJ-1625538790176)(%5BFrenet%5D%20Optimal%20Trajectory%20Generation%20c1760b481c6d43e0ba976cca452668e3/Untitled%207.png)]

  • [x] 這里的每一步是指每一次的整體運行吧，一旦定好center line后只有到達了終點才需要重新再選吧 → 不是

代碼示意

整體簡述

  1. 指定幾個路徑點的xy坐標和障礙物的坐標

  2. 然后通過2D cubic spline生成順滑的曲線，此時保存的[x, y, yaw, k] xy是坐標，yaw是朝向，k是曲率

    生成方式是，從xy到frenet然后再用四次多項式生成result → 后面發現這個主要是生成給顯示笛卡爾用的result

    result = self.a[i] + self.b[i] * dx + self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0

    但是還是生成了縱向的s的累加項的只是對象，可能再后面調用吧

  3. 初始化車輛狀態，速度，橫向位置，橫向速度，橫向加速度

  4. 循環仿真時間內LOOP，做frenet最優規划，

    1. 計算frenet路徑
    2. 計算全局路徑
    3. 檢查是否與障礙物相碰
    4. 選出最小的cost對應的路徑

接着我們走到每個小步的細節處理：

1. 計算frenet路徑

  1. 這里有采樣空間的說法，首先是道路寬度大小，然后是采樣長度：

    # generate path to each offset goal
for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):

  2. 然后是橫向motion planning循環時間T，在這之中有縱向的循環線，首先是最小的預測時間，最大預測時間，時間間隔

    # Lateral motion planning
for Ti in np.arange(MIN_T, MAX_T, DT):

  3. 循環內增加五次多項式的東西，首先是時間上的(0,Ti)以DT為間隔的時間列表

    • 橫向的是五次多項式，因為只考慮一個方向，所以此時其實是一維的公式，解釋在代碼中clone下來有ipynb版的解釋 此處引入於那邊

      輸入的是輸出狀態：

      # initial state
c_speed = 10.0 / 3.6  # current speed [m/s]
c_d = 2.0  # current lateral position [m]
c_d_d = 0.0  # current lateral speed [m/s]
c_d_dd = 0.0  # current lateral acceleration [m/s]

      首先我們假設在 \(x(t)\) 是指在時間t ，軌跡按五次多項式應該長這樣（注意只考慮橫向 只有一維）

      \[x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5 \tag{1} \]

      • 然后在開始的狀態是都是0嘛或者是初始點:

        \[x(0) = a_0 = x_s \tag{2} \]

        \[x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4 \tag{3}\]

        \[x'(0) = a_1 = v_s \tag{4} \]

        \[x''(t) = 2a_2+6a_3t+12a_4t^2 \tag{5} \]

        \[x''(0) = 2a_2 = a_s\tag{6} \]

      最后當求解系數時，我們就有以下的的公式：

      \[\begin{aligned} x(T)=a_0+a_1T+a_2T^2+a_3T^3+a_4T^4+a_5T^5&=x_e\\ x'(T)=a_1+2a_2T+3a_3T^2+4a_4T^3+5a_5T^4&=v_e\\ x''(T)=2a_2+6a_3T+12a_4T^2+20a_5T^3&=a_e\\ \end{aligned}\]

      其中根據初始狀態信息我們已知以下系數

      a0 = c_d         # current lateral position [m]
a1 = c_d_d       # current lateral speed [m/s]
a2 = c_d_dd/ 2.0 # current lateral acceleration [m/s]

      剩余的轉成矩陣求解就是里面的計算是按着這個

      \[\begin{bmatrix} T^3 & T^4 & T^5 \\ 3T^2 & 4T^3 & 5T^4 \\ 6T & 12T^2 & 20T^3 \end{bmatrix}\begin{bmatrix} a_3\\ a_4\\ a_5\end{bmatrix}=\begin{bmatrix} x_e-x_s-v_sT-0.5a_sT^2\\ v_e-v_s-a_sT\\ a_e-a_s\end{bmatrix} \]

      那么接下再返回橫向的各種計算就比較明顯了，畢竟五次多項式的系數都有了

      fp.t = [t for t in np.arange(0.0, Ti, DT)]
fp.d = [lat_qp.calc_point(t) for t in fp.t]
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]

    • 縱向的是四次多項式，主要是保證速度跟隨，循環次數是根據設定的速度等

      TARGET_SPEED = 30.0 / 3.6  # target speed [m/s]
D_T_S = 5.0 / 3.6  # target speed sampling length [m/s]
N_S_SAMPLE = 1  # sampling number of target speed

      其中四次多項式的求解就和上面五次多項式系數求解差不多，就不做重復敘述了

      # Longitudinal motion planning (Velocity keeping)
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
                    TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
    tfp = copy.deepcopy(fp)
    lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)

    tfp.s = [lon_qp.calc_point(t) for t in fp.t]
    tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
    tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
    tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

		Jp = sum(np.power(tfp.d_ddd, 2))  # square of jerk
    Js = sum(np.power(tfp.s_ddd, 2))  # square of jerk

    # square of diff from target speed
    ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2

    tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1] ** 2
    tfp.cv = K_J * Js + K_T * Ti + K_D * ds
    tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv

    frenet_paths.append(tfp)

      后半部分呢，就是論文中講的最小化jerk的二次方，真直接；然后代入上面提到的公式，復制到這里吧 直接對着代碼非常明顯，不過讀論文的時候不知道原來 \(k_j, k_t, k_d\) 都是自選，雖然我記得好像是constant 常數

      \[C_{d}=k_{j} J_{t}(d(t))+k_{t} T+k_{d} d_{1}^{2} \tag{2} \]

      \[C_{v}=k_{j} J_{t}(s(t))+k_{t} T+k_{\dot{s}}\left[\dot{s}_{1}-\dot{s}_{d}\right]^{2} \]

      \[C_{tot}=k_{lat}C_{lat}+k_{lon}C_{lon} \]

所以總結來看frenet主要是五次多項式擬合生成軌跡，然后再計算每條軌跡的按公式來的cost值，自此還沒有進行最優道路的選擇工作

2. 計算全局路徑

這個感覺問題不大，主要就是有了五次多項式后，得到的已經是計算了各個軌跡的cost了，然后從列表里一個個循環

這里回看發現，計算的橫向距離 \(d\) 和他的小伙伴求導 都僅僅是作為cost的計算進入frenet path → 口誤，后面看到原來是deepcopy了的，然后再看看后面有沒有用上其他的計算

  1. 循環整個列表，列表長度 210，單個叫 fp
  2. 循環列表里有的s，每個fp 有20個s
  3. 通過每個fp 內的s計算全局下的位置，因為以前存過路徑點轉換為順滑的那個對象，然后就阿巴阿巴 說起來太復雜了 對着cubic_spline_planner.py看一下吧... 但是大概說一下主要功能是啥：

在這里插入圖片描述

從點到spline

首先這里是幾個點，然后用spline的方法畫成了順滑的線，然后呢 我們返回的就是對應s下順滑線的那個點的xy值，然后呢 這還沒完，我們還要轉成frenet坐標系下的s和d

然后還計算了這個點和下一個點的yaw角和`ds`到 `fp` 里

最后是曲率兩個yaw叫相減除以現在兩點間的`ds`

```python
def calc_global_paths(fplist, csp):
    for fp in fplist:

        # calc global positions
        for i in range(len(fp.s)):
            ix, iy = csp.calc_position(fp.s[i])
            if ix is None:
                break
            i_yaw = csp.calc_yaw(fp.s[i])
            di = fp.d[i]
            fx = ix + di * math.cos(i_yaw + math.pi / 2.0)
            fy = iy + di * math.sin(i_yaw + math.pi / 2.0)
            fp.x.append(fx)
            fp.y.append(fy)

        # calc yaw and ds
        for i in range(len(fp.x) - 1):
            dx = fp.x[i + 1] - fp.x[i]
            dy = fp.y[i + 1] - fp.y[i]
            fp.yaw.append(math.atan2(dy, dx))
            fp.ds.append(math.hypot(dx, dy))

        fp.yaw.append(fp.yaw[-1])
        fp.ds.append(fp.ds[-1])

        # calc curvature
        for i in range(len(fp.yaw) - 1):
            fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])

    return fplist
```

3. 碰撞檢測

emmm 這個很簡單的判斷 十分簡單，計算現在路徑里有的所有點對障礙物的坐標點進行距離計算如果小於機器人的半徑則整條路徑都定義為False，同時也對最大速度，最大加速度和最大曲率都有判斷的。最后返回留下的都是滿足條件的路徑

def check_paths(fplist, ob):
    ok_ind = []
    for i, _ in enumerate(fplist):
        if any([v > MAX_SPEED for v in fplist[i].s_d]):  # Max speed check
            continue
        elif any([abs(a) > MAX_ACCEL for a in
                  fplist[i].s_dd]):  # Max accel check
            continue
        elif any([abs(c) > MAX_CURVATURE for c in
                  fplist[i].c]):  # Max curvature check
            continue
        elif not check_collision(fplist[i], ob):
            continue

        ok_ind.append(i)

    return [fplist[i] for i in ok_ind]

def check_collision(fp, ob):
    for i in range(len(ob[:, 0])):
        d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)
             for (ix, iy) in zip(fp.x, fp.y)]

        collision = any([di <= ROBOT_RADIUS ** 2 for di in d])

        if collision:
            return False

    return True

  • [x] 咦 但是這里我... 有問題，返回fplist直接是False了還... 怎么玩 后面的計算也完全不需要了吧，而且不是按照橫縱分了很多線嘛？為啥判斷這一簇里有一個線上有就不算了？都是False 這樣的表達有點小問題 emmm

    不好意思看岔了 顯示check path然后跳到check collision

4. 選取最小的cost

如題，非常easy，然后再做出這個path動作，回到主循環，繼續復制初始狀態，循環下去 OVER

  • [x] 跳轉 不過我比較好奇為啥不命名成fd fs 難道是我理解錯了？

    是的 不是fd fs而是d,s一直加上center line的xy，得出d,s的笛卡爾fx, fy，其中因為yaw可以超90度，然后運用cos(x+pi/2)替代sin(x) 就可以又負軸

    在這里插入圖片描述

運行示例

以下為運行時的gif：

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-eEAYRPFH-1625538790178)(https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif)]

https://img-blog.csdnimg.cn/20210706102149936.gif

其中藍綠色是center line，黑色x是障礙物位置，紅色是預測的行駛軌跡；左邊是我單獨修改了畫圖，把所有的路線給用顏色標出來了，cost的顏色從大到小如圖例所示

畫圖修改后的代碼見此次gitee commit 點擊即可跳轉

總結

以上，所以生成軌跡用的是貝塞爾，然后跟隨軌跡 橫向距離是按五次多項式、縱向速度保持是四次多項式，然后再按論文中的cost 公式代入計算，所以理解cost公式的意義比較重要，並且各個參數的調整意味着什么。另外有一點在論文體現，代碼里忽略的是關於跟隨前車，超車都是假設前車加速度不變，感覺這樣實際高速操作還是有安全隱患的。

對於jerk的做法，我確實沒感覺出什么不妥，可能是計算速度 更新率方面需要進一步評估這個。

后面想到一個，關於實際車輛運動時的反饋，比如就算你給出了車輛運動的軌跡 甚至是那個速度，跟隨問題也是需要考慮的，但是可能這就不在軌跡規划器里考慮了，而是控制跟隨，比如LQR、PID、Stanley控制器等（其中LQR可能會“預習”一下，后兩者 當時畢設寫過了）


免責聲明！

本站轉載的文章為個人學習借鑒使用，本站對版權不負任何法律責任。如果侵犯了您的隱私權益，請聯系本站郵箱yoyou2525@163.com刪除。



猜您在找 【路徑規划】 The Dynamic Window Approach to Collision Avoidance (附python代碼實例) 運動（motion）規划、路徑（path）規划和軌跡（trajectory）規划之區別 干貨 | 蟻群算法求解帶時間窗的車輛路徑規划問題詳解（附Java代碼） 動態規划實例（四）最優二叉搜索樹（Optimal Binary Search Tree） Python進階：函數式編程實例（附代碼） 機器人中的軌跡規划（Trajectory Planning ） 以計算斐波那契數列為例說說動態規划算法（Dynamic Programming Algorithm Overlapping subproblems Optimal substructure Memoization Tabulation） 轉 | 禁忌搜索算法（Tabu Search）求解帶時間窗的車輛路徑規划問題詳解（附Java代碼） HttpContext的dynamic包裝器DynamicHttpContext （附原代碼） 【路徑規划】OSQP曲線平滑 公式及代碼
 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM