LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

@tomtomApp

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

Last updated at Posted at 2025-02-24

はじめに

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

動作環境

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

三次スプライン補間

スクリーンショット 2025-02-24 23.19.20.png

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で作成した経路を補間する

スクリーンショット 2025-02-24 23.20.29.png
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(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?