Frenet Frameによる軌道最適化を用いて障害物回避(WIP)

記事作成途中です．

## 目的

グローバルプランナーなどで与えられた目標trajectory（**以降，center lineと呼ぶ**）をもとに，障害物を避けるようなtrajectoryをいい感じに生成することを目的としています．イメージは以下の図のようなものです．

<img width="500" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/51f712ed-66bd-bc15-5a51-814a7cd6171b.png">


## 参考

この記事を書くにあたって以下の資料を参考にしました．

- [元論文 : Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame](https://ieeexplore.ieee.org/document/5509799)

- [Python Robotics](https://atsushisakai.github.io/PythonRobotics/index.html)
    - Optimal Trajectory in a Frenet Frame [[ドキュメント](https://atsushisakai.github.io/PythonRobotics/modules/path_planning/frenet_frame_path/frenet_frame_path.html)],[[実装](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py)]
    - Quintic polynomials planning [[ドキュメント](https://atsushisakai.github.io/PythonRobotics/modules/path_planning/quintic_polynomials_planner/quintic_polynomials_planner.html)],[[実装](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py)]

- [記事 fjp.github.io : Trajectory Planning in the Frenet Space](https://fjp.at/posts/optimal-frenet/)

## Frenet座標系について

このアルゴリズムを理解するために重要な概念であるFrenet座標系について説明します．Frenet座標系とは，一言で表すと「center lineに沿った座標系」です．

一般的に座標系は以下のような$(x,y)$座標を考えます．
![](https://raw.githubusercontent.com/fjp/frenet/master/docs/images/cart_refpath.svg?sanitize=true)(fjp.github.ioより)

これに対してFrenet座標系は，center line(青線)を軸$s$とし，それに直交する線を軸$d$とする$(s,d)$座標を考えます．
![](https://raw.githubusercontent.com/fjp/frenet/master/docs/images/frenet_refpath.svg?sanitize=true)(fjp.github.ioより)

trajectoryを計画する際には，最初はFrenet座標系でtrajectoryを考えて，それを直交座標系に戻すというアプローチをとります．

## アルゴリズムの概観

以下のアニメーションは，実際にプランニングをしている様子を示しています．青がcenter lineで，赤が障害物を回避するように最適化されたtrajectoryを表しています．

<img width="500" src="https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif">
(python roboticsより)

これらアルゴリズムは，以下のような手順を踏みます．

1. center lineを求める
    - このcenter lineは，Frenet座標系の$s$軸として用います．

2. Frenet座標系での最適化に用いる初期位置・速度・加速度を求める
    - **最初にプランニングを行う場合**は，現在の直交座標系での状態からFrenet座標系の状態に変換する必要があります．**それ以外の場合**では，前回計算したtrajectoryの次の点を用います．(このアルゴリズムはMPCではないので，毎ステップ，観測された現在の状態からFrenet座標系での状態に変換する必要はありません．)

3. 横方向と縦方向のtrajectoryを計算する
    - 基本的に横方向のtrajectory$d(t)$と縦方向$s(t)$を独立に求めます．(車速が低速の場合は，この記述は当てはまりません．[ここ](https://qiita.com/drafts/e50a1b7e0aaf92111bdf/edit#2-%E4%BD%8E%E9%80%9F%E8%BB%8C%E9%81%93%E3%81%AE%E5%A0%B4%E5%90%88)で詳細を説明します)
    - ここで求めるtrajectoryは，1つではなく，複数のtrajectoryを求めます．最終的に，これらのtrajectoryの中から，制約を満たした最も良いtrajectoryを選びます．
    - これら複数のtrajectoryは，**一つの初期条件(状態)** と，$s$や$d$や時間間隔$T$が異なる**複数の終端条件(状態)** を満たす多項式として表されます．
    - 以下が実際に生成されるFrenet座標系でのtrajectoryです．
    ![Figure_1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/dec48992-f674-f5bc-3eb7-03d90b5f7094.png)

4. 縦方向と横方向のtrajectoryを合わせる
    - ここは，特に難しいとこは無いのですが，$C_{\text {tot }}=$ $k_{\text {lat }} C_{\text {lat }}+k_{\text {lon }} C_{\text {lon }}$のように，横方向でのコスト$C_{\text {lat }}$と縦方向でのコスト$C_{\text {lon }}$に係数$k_{\text {lat }}, k_{\text {lon }}$を掛けて，トータルコスト$C_{\text {tot }}$を求めます．

5. 直交座標系でのtrajectoryを求める
5. 複数のtrajectoryの中から障害物に衝突せず，コスト$C_{\text {tot }}$が最も小さいtrajectoryを選択する

## アルゴリズムの詳細(WIP)

### center lineのプロパティ

center lineはFrenet座標系で求めた多項式で表されるtrajectoryから，直交座標系でのtrajectoryを求める必要があります．詳しくは[Frenet座標系から直交座標系への変換](###Frenet座標系から直交座標系への変換)に記述しますが，

- 最適化したtrajectoryとして，**位置$\vec{x}$のみを求めたい場合**，center lineはtrajectory上で進んだ距離$s$から，以下を求めることができる必要があります．
    - center line上の点$\vec{r}(s)$
    - center lineの傾き$\theta_{r}(s)$
- 最適化したtrajectoryとして，**速度$v_x$や速度の向き$\theta_x$も求めたい場合**，上記に加え，以下のものを求める必要があります．
    - center lineの曲率$\kappa_{r}(s)$
- 最適化したtrajectoryとして，**曲率$\kappa_x$や加速度$a_x$も求めたい場合**，更に以下を求めることが出来る必要があります．
    - center lineの曲率の変化率$\kappa_{r}'(s)$

### 横方向のプランニング

#### 1. High Speed Trajectoryの場合

#### 2. Low Speed Trajectoryの場合

### 縦方向のプランニング

### Frenet座標系から直交座標系への変換


## サンプルコードと実行結果(WIP)