３次スプライン補間を用いた滑らかな経路設計

# はじめに
安定した走行を実現する経路追従制御において、事前に滑らかな経路を設計することは重要です。しかし、GNSSやSLAMでの自己位置推定を用いて経路を作成すると、信号の受信状況や推定誤差の影響で、経路が不自然な曲率を持つことがあります。そこで、三次スプライン補間を適用することで、各座標を通過しながら位置・傾き・曲率の連続性を保ち、より自然で滑らかな軌道を生成できます。さらに、曲率を計算できるため、ロボットの制御システムに適した速度指令の算出が容易となり、追従制御の精度と安定性の向上が期待できます。

# 動作環境
|項目  | バージョン |
|---|---|
|ubuntu  |22.04  |
|ROS  |ROS2 humble  |

# 三次スプライン補間
![スクリーンショット 2025-02-24 23.19.20.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3480989/0418b6d4-5526-46a3-8173-93e7005d94c2.png)

3次スプライン補間は、与えられた離散的なデータ点を滑らかにつなぐために、各区間で3次多項式を用いる補間手法です。
``` math
(x_0, y_0), (x_1, y_1), \dots, (x_{n-1}, y_{n-1})
```
とします。区間 $[x_i, x_{i+1}]$ごとに次の3次多項式
```math

S_i(x) = a_i + b_i (x - x_i) + c_i (x - x_i)^2 + d_i (x - x_i)^3
\quad (0 \le i \le n-2)

```
を定義し、係数 $(a_i, b_i, c_i, d_i)$ を求めます。
### スプライン条件
#### 条件１　位置の連続
スプライン補間では以下の条件を満たすよう係数を決定します。
```math 
  
  S_i(x_{i+1}) = y_{i+1}, \quad S_{i+1}(x_{i+1}) = y_{i+1}
  
```
#### 条件２　１次導関数の連続
```math
  
  S_i'(x_{i+1}) = S_{i+1}'(x_{i+1})
  
```
#### 条件３　2次導関数の連続
```math
S_i''(x_{i+1}) = S_{i+1}''(x_{i+1})
```

さらに、端点の境界条件（自然境界条件や固定境界条件など）を設定して解が一意に定まるようにします。
#### 条件４　自然境界条件
```math
S_0''(x_0) = 0,
\quad
S_{n-2}''(x_{n-1}) = 0
```
自然境界条件では、両端で2次導関数を0とします。

### 係数の算出方法
条件１から条件４より、行列 $A$ とベクトル$B$を構築し、３次多項式の係数を求めます。各区間の長さを$h_i = x_{i+1} - x_i$とすると、行列$A$は以下のように構成されます。
```math
A = 
\begin{pmatrix}
1 & 0 & 0 & \cdots & 0 \\
h_0 & 2(h_0 + h_1) & h_1 & \cdots & 0 \\
0 & h_1 & 2(h_1 + h_2) & \cdots & 0 \\
\vdots & \vdots & \vdots & \ddots & h_{n-2} \\
0 & 0 & 0 & h_{n-2} & 1
\end{pmatrix}
```
自然境界条件（端点での2次導関数 = 0）を反映し、最初と最後の対角要素が1にしています。
次に、以下の式でベクトル$B$を構築します。
```math
B_i = 3 \left( \frac{y_{i+1} - y_i}{h_i} - \frac{y_i - y_{i-1}}{h_{i-1}} \right), \quad i = 1, \dots, n-2
```
ベクトルの最初と最後の要素はゼロであり、自然境界条件を満たします。
行列 \(A\) とベクトル \(B\) を使って、以下の連立方程式を解きます。
```math

A \cdot \mathbf{c} = \mathbf{B}

```
を解くことで $\mathbf{c}$（各区間の2次項係数）を求めます。
最後に、
``` math
a_i = y_i
```
``` math
b_i = \frac{y_{i+1} - y_i}{h_i} - \frac{h_i}{3}(2c_i + c_{i+1})
```
```math
d_i = \frac{c_{i+1} - c_i}{3h_i}
```
これで、全ての係数が求まります。

### 曲率の算出
曲率は以下の式で導出できます。
```math
\kappa = \frac{ y''}{\bigl( 1 + y'^2 \bigr)^{3/2}}
```
１次微分、２次微分は以下の式で表されます。
```math
\quad y' = b + 2cx + 3dx^2
```
```math
\quad y'' = 2c + 6dx
```
### ３次スプライン補間のソースコード
https://github.com/Arcanain/path_smoother/blob/feature/performance2024/include/path_smoother/cubic_spline_1d.hpp

https://github.com/Arcanain/path_smoother/blob/feature/performance2024/src/cubic_spline_1d_component.cpp

https://github.com/Arcanain/path_smoother/blob/feature/performance2024/src/cubic_spline_1d_node.cpp

# GNSSで作成した経路を補間する

![スクリーンショット 2025-02-24 23.20.29.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3480989/f76000fc-9751-447f-ac0a-eca0437dc86f.png)
GNSSによって取得した位置データは、ノイズや測定誤差の影響を受けやすく、不規則な軌跡となることがあります。三次スプライン補間を用いることで、より滑らかな経路を生成する手法を紹介します。

上の図のように、GNSSの位置データを一定の間隔で取得し、取得したデータ間を三次スプライン補間によって滑らかに接続します。これにより、ノイズや測定誤差の影響を抑えつつ、実際の軌跡に近い滑らかな経路を生成できます。また、一度に大量のデータを処理せず、区間ごとに補間計算を行うことで、計算負荷を軽減し、リアルタイム処理にも対応可能です。

### GNSSの位置データを取得
``` C++
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
    "/gnss_pose", 10, std::bind(&SavePath::pose_callback, this, _1));
```
GNSSセンサーから位置データ（X, Y座標）を取得します。
``` C++
double dist = std::sqrt(std::pow(msg->pose.position.x - pre_x, 2) + std::pow(msg->pose.position.y - pre_y, 2));
if (dist > dist_thread) {
    x.push_back(msg->pose.position.x);
    y.push_back(msg->pose.position.y);
    pre_x = msg->pose.position.x;
    pre_y = msg->pose.position.y;
}
```
一定の間隔（例: 0.5m）で取得した位置データを保存します。

```C++
CubicSpline2D sp(x, y);

std::vector<double> s_values;
double ds = 0.1;
for (double s = 0; s <= sp.get_s_max(); s += ds) {
    s_values.push_back(s);
}

for (double s : s_values) {
    auto [ix, iy] = sp.calc_position(s);
    rx_.push_back(ix);
    ry_.push_back(iy);
    ryaw_.push_back(sp.calc_yaw(s));
    rk_.push_back(sp.calc_curvature(s));
}

x.clear();
y.clear();
rx_.clear();
ry_.clear();
ryaw_.clear();
rk_.clear();

```
３次スプライン補間の関数であるCubicSpline2D(x, y) を初期化し、区間長 ds（例: 0.1m）で補間を行います。

補間結果として以下を取得します。
・rx_: 補間後のX座標
・ry_: 補間後のY座標
・ryaw_: 補間後のヨー角
・rk_: 補間後の曲率

一度に大量の位置データを補間すると計算量が膨大になるため、2点間の補間を行うたびに配列をクリアし、定期的にデータを保存するようにしています。

```C++
output_file_.open(file_path);
if (output_file_.is_open()) {
    output_file_ << "x,y,z,w0,w1,w2,w3\n";
}

for (size_t i = 0; i < path_msg.poses.size(); ++i) {
    std::vector<double> current_point = {
        path_msg.poses[i].pose.position.x,
        path_msg.poses[i].pose.position.y,
        path_msg.poses[i].pose.position.z,
        path_msg.poses[i].pose.orientation.x,
        path_msg.poses[i].pose.orientation.y,
        path_msg.poses[i].pose.orientation.z,
        path_msg.poses[i].pose.orientation.w
    };
    path_.push_back(current_point);
}

if (!output_file_.is_open()) {
    RCLCPP_ERROR(this->get_logger(), "CSV file is not open.");
    return;
}

for (const auto& point : path_) {
    for (size_t i = 0; i < point.size(); ++i) {
        output_file_ << point[i];
        if (i < point.size() - 1) {
            output_file_ << ",";
        }
    }
    output_file_ << "\n";
}
RCLCPP_INFO(this->get_logger(), "CSV file has been saved.");

```

```C++
rclcpp::on_shutdown([this]() {
    RCLCPP_INFO(this->get_logger(), "Shutdown detected. Saving path.");
    if (!save_flag) {
        save_csv();
    }
    if (output_file_.is_open()) {
        output_file_.close();
    }
});
```
補間後の経路、曲率、yaw角をCSVファイルに保存します。
rclcpp::on_shutdown() を使用して、ノードの終了時csvにデータを保存する関数save_csv()を呼び出します。

#### ソースコード
https://github.com/Arcanain/path_smoother/blob/feature/performance2024/src/save_path_gnss.cpp




## 参考文献
https://myenigma.hatenablog.com/entry/2016/10/12/073335

https://qiita.com/khidaka/items/5d476cdf0b4a8ca0ffb5

 


## 謝辞
この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.

https://www.gxp.co.jp/

https://qiita.com/organizations/gxp