はじめに

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

動作環境

項目	バージョン
ubuntu	22.04
ROS	ROS2 humble

三次スプライン補間

3次スプライン補間は、与えられた離散的なデータ点を滑らかにつなぐために、各区間で3次多項式を用いる補間手法です。

(x_0, y_0), (x_1, y_1), \dots, (x_{n-1}, y_{n-1})

とします。区間 $[x_i, x_{i+1}]$ごとに次の3次多項式


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)$ を求めます。

スプライン条件

条件１　位置の連続

スプライン補間では以下の条件を満たすよう係数を決定します。

  
  S_i(x_{i+1}) = y_{i+1}, \quad S_{i+1}(x_{i+1}) = y_{i+1}

条件２　１次導関数の連続

  
  S_i'(x_{i+1}) = S_{i+1}'(x_{i+1})

条件３　2次導関数の連続

S_i''(x_{i+1}) = S_{i+1}''(x_{i+1})

さらに、端点の境界条件（自然境界条件や固定境界条件など）を設定して解が一意に定まるようにします。

条件４　自然境界条件

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$は以下のように構成されます。

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$を構築します。

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) を使って、以下の連立方程式を解きます。


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

を解くことで $\mathbf{c}$（各区間の2次項係数）を求めます。
最後に、

a_i = y_i

b_i = \frac{y_{i+1} - y_i}{h_i} - \frac{h_i}{3}(2c_i + c_{i+1})

d_i = \frac{c_{i+1} - c_i}{3h_i}

これで、全ての係数が求まります。

曲率の算出

曲率は以下の式で導出できます。

\kappa = \frac{ y''}{\bigl( 1 + y'^2 \bigr)^{3/2}}

１次微分、２次微分は以下の式で表されます。

\quad y' = b + 2cx + 3dx^2

\quad y'' = 2c + 6dx

３次スプライン補間のソースコード

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

GNSSによって取得した位置データは、ノイズや測定誤差の影響を受けやすく、不規則な軌跡となることがあります。三次スプライン補間を用いることで、より滑らかな経路を生成する手法を紹介します。

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

GNSSの位置データを取得

pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
    "/gnss_pose", 10, std::bind(&SavePath::pose_callback, this, _1));

GNSSセンサーから位置データ（X, Y座標）を取得します。

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）で取得した位置データを保存します。

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点間の補間を行うたびに配列をクリアし、定期的にデータを保存するようにしています。

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.");

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()を呼び出します。

ソースコード

参考文献

謝辞

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