quaternion の z を yaw だと思っていませんか: pure pursuit の後輪位置が最大 0.95 m ずれていた話

Last updated at 2026-09-13Posted at 2026-09-12

Team Hayes（Ajayaditya Lokchandra, Nithisha Venkatesh）です。自動運転AIチャレンジ2026 のスターターキットの pure pursuit 制御（simple_pure_pursuit）で、姿勢の quaternion の z 成分をそのまま yaw 角として使っている箇所を見つけました。ROS のコードでときどき見かける間違いなので、なぜ間違いなのか、どれくらいずれるのかを数値でまとめます。

結論: orientation.z は yaw ではなく sin(yaw/2) です。そのため後輪位置の計算が最大 0.95 m ずれ、スターターキットのレースラインを走るだけで操舵指令が最大 15.8° ずれていました。

quaternion の z は何か

z 軸まわりに yaw だけ回転した姿勢の quaternion は次のとおりです。

q = (x, y, z, w) = (0, 0, sin(yaw/2), cos(yaw/2))

つまり z = sin(yaw/2) です。よくある誤解は「yaw が小さいときは z ≈ yaw なので近似として使える」ですが、実際には yaw が小さいときも z ≈ yaw/2 で、最初から半分です。yaw = 30° のとき z をそのまま角度（ラジアン）として使うと 14.8° 相当になり、15.2° ずれます。

スターターキットのコード

simple_pure_pursuit.cpp の 85-88 行目（aichallenge-racingkart dev ac3f929）で、後輪中心の位置を次のように計算していました。

double rear_x = odometry_->pose.pose.position.x -
                wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z);
double rear_y = odometry_->pose.pose.position.y -
                wheel_base_ / 2.0 * std::sin(odometry_->pose.pose.orientation.z);

面白いのは、同じ関数の 109 行目では tf2::getYaw(odometry_->pose.pose.orientation) と正しく yaw を取り出していることです。<tf2/utils.h> もすでに include されています。同じファイルの中で、1 箇所だけ取り出し方が違っていました。

どれくらいずれるか

後輪位置のずれは、正しい yaw と z との角度差を Δ として wheel_base · |sin(Δ/2)| です（ホイールベース 1.087 m）。

車の向き	角度のずれ	後輪位置のずれ
10°	5.0°	0.05 m
30°	15.2°	0.14 m
90°	49.5°	0.46 m
180°	122.7°	0.95 m

周回コースでは車はあらゆる向きを取るので、1 周のどこかで必ず大きくずれます。スターターキットのレースライン上に車を置き、正しい yaw と z のそれぞれで pure pursuit の操舵指令を計算して比べると、差は次のとおりでした。

citycircuit（既定ライン, 131 点）: 最大 15.81°、平均 0.88°
kashiwanoha（377 点）: 最大 9.75°、平均 1.07°

pure pursuit のゲインを調整しているチームは、知らないうちにこのずれを打ち消す方向に調整しているかもしれません。

直し方

同じ関数で使っている tf2::getYaw に揃えるだけです。

-  double rear_x = odometry_->pose.pose.position.x -
-                  wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z);
-  double rear_y = odometry_->pose.pose.position.y -
-                  wheel_base_ / 2.0 * std::sin(odometry_->pose.pose.orientation.z);
+  const double yaw = tf2::getYaw(odometry_->pose.pose.orientation);
+  double rear_x = odometry_->pose.pose.position.x - wheel_base_ / 2.0 * std::cos(yaw);
+  double rear_y = odometry_->pose.pose.position.y - wheel_base_ / 2.0 * std::sin(yaw);

修正 PR: https://github.com/AutomotiveAIChallenge/aichallenge-racingkart/pull/327（運営の判断でクローズされました。実車・シミュレータでは yaw の値が最も信頼できるため、変更で走行が不安定になることを懸念されたためです。記事の内容は、コードが orientation.z を角度として使っている点についての分析です）
同じ修正は、先に出ていた PR #213（高速化のための大きな PR, 未マージ）の中にも含まれています。単独の小さな PR にしたのは、すぐにマージできるようにするためです。

自分のコードを確認する方法

grep -rnE "(cos|sin|atan2)\([^)]*orientation\.z" your_ws/src

C++ なら tf2::getYaw()、Python なら tf_transformations.euler_from_quaternion() か 2 * atan2(z, w)（x, y が 0 のとき）で yaw を取り出します。

作り方について

調査とパッチ作成には AI コーディングエージェントを使いました。数値は、スターターキットのレースラインに対して修正前後の計算を比べるテストハーネスで確認しています。

English

Team Hayes (Ajayaditya Lokchandra, Nithisha Venkatesh). The pure-pursuit controller in the JSAE AI Challenge 2026 starter kit (simple_pure_pursuit) used the quaternion's z component directly as the yaw angle. It is a mistake that turns up in ROS code now and then, so here is why it is wrong and how much it costs, with numbers.

In one line: orientation.z is sin(yaw/2), not yaw. The rear-axle point was off by up to 0.95 m, and on the starter kit's own racelines the steering command was off by up to 15.8°.

What the quaternion's z is

For a pose rotated by yaw about the z axis, q = (0, 0, sin(yaw/2), cos(yaw/2)), so z = sin(yaw/2). The usual excuse, "for small yaw, z ≈ yaw", is wrong: for small yaw, z ≈ yaw/2, half the angle from the start. At 30° of heading, using z as an angle gives 14.8°, an error of 15.2° (figure above).

The starter-kit code

Lines 85-88 of simple_pure_pursuit.cpp (dev ac3f929) computed the rear-axle centre with std::cos(orientation.z) and std::sin(orientation.z) (code above). Line 109 of the same function already extracts yaw correctly with tf2::getYaw(...), and <tf2/utils.h> is already included. One spot in the file did it differently from the rest.

How large the error is

With Δ the difference between the true yaw and z, the rear-axle point moves by wheel_base · |sin(Δ/2)| (wheelbase 1.087 m): 0.05 m at 10° of heading, 0.14 m at 30°, 0.46 m at 90° and 0.95 m at 180° (table above). A car on a circuit takes every heading, so somewhere on each lap the error is large. Placing the car on each point of the shipped racelines and computing the pure-pursuit steering with the true yaw and with z, the difference is up to 15.81° (mean 0.88°, 131 points) on citycircuit and up to 9.75° (mean 1.07°, 377 points) on kashiwanoha. Teams tuning the pure-pursuit gains may have been tuning around this error without knowing it.

The fix

Use tf2::getYaw, as the same function already does (diff above).

Fix PR: https://github.com/AutomotiveAIChallenge/aichallenge-racingkart/pull/327 (closed by the maintainer, who was concerned the change could make driving less stable on the real vehicle and in simulation; this article is our analysis of the code using orientation.z as an angle)
The same change is also inside the earlier, larger PR #213 (unmerged); we sent it as a small separate PR so it can be merged quickly.

Check your own code

grep -rnE "(cos|sin|atan2)\([^)]*orientation\.z" your_ws/src. Extract yaw with tf2::getYaw() in C++, or tf_transformations.euler_from_quaternion() (or 2 * atan2(z, w) when x and y are zero) in Python.

How this was made

We used AI coding agents for the investigation and the patch. The numbers come from a test harness that compares the calculation before and after the fix on the starter kit's racelines.

Team Hayes の自動運転AIチャレンジ2026 シリーズ / series

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up