Stanley Controllerという操舵則

Stanley Controllerに出会った日

1. 使い慣れた操舵則

　筆者の研究室では，トラクタの自動操舵に長らく次の操舵則を使ってきた（木瀬ら，2001）。

\delta = a_1 d + a_2 \Delta\phi

　 $\delta$ は操舵角， $d$ は目標経路からの横方向偏差， $\Delta\phi$ は方位偏差， $a_1$ ・ $a_2$ は一定の制御ゲインである。横偏差と方位偏差にそれぞれ係数を掛けて足す，2入力1出力のP制御である。 $d$ と $\Delta\phi$ は，pure pursuitと同じく車両の前方に置いた注視点で評価してきた。長年きちんと動いてきたので，筆者はいつからかこの式を疑わなくなっていた。

2. Stanley controller

　Stanleyとは，2005年のDARPA Grand Challengeで優勝したスタンフォード大学の自動運転車の名前である。その横位置制御則を，Hoffmannらが2007年に定式化した（Hoffmann et al., 2007）。式は次の通りである。

\delta = K_\phi \Delta\phi + \arctan\left(\frac{k\,d}{|v| + v_0}\right)

　 $K_\phi$ は方位ゲイン， $k$ は横偏差ゲイン， $v$ は車速， $v_0$ は低速で過敏になるのを防ぐオフセットである。 $\arctan$ があり，分母に車速がある。最初に見たとき，うちの式とは別物に見えた。

3. 単一入力の比例制御ともよく似ている

　横偏差 $d$ が小さいとき， $\arctan(x) \approx x$ なので，

\arctan\left(\frac{k\,d}{|v| + v_0}\right) \approx \frac{k}{|v| + v_0}\,d

　となる。つまり横偏差項は $\dfrac{k}{|v|+v_0}\,d$ であり，うちの $a_1 d$ と同じ形である。違いは，一定だった $a_1$ が車速に反比例するゲインに置き換わり，さらに $\arctan$ で飽和するようになった点だけである。

4. 車速で割る意味

　車両のヨー応答は

\dot{\psi} = \frac{v}{L}\tan\delta

　であり（ $L$ はホイールベース），同じ操舵角でも速度が高いほど向きは速く変わる。一定ゲインでは，1.0 m/s でも 3.0 m/s でも同じ横偏差に同じ操舵を返してしまう。高速では補正を弱め，低速では強めたい。横偏差項を車速で割るのは，この性質に合っている。 $\arctan$ の飽和は，横偏差が大きいときに操舵が一気に振り切れるのを防ぐ。ただのP制御に無かった配慮である。

5. 前方注視距離が要らなくなる

　もう一つ，筆者が良いと思った点がある。うちの式はpure pursuitの注視点に頼っていた。前方注視距離を長く取るほど動きは滑らかになるが，曲線経路では内側を通り抜けてしまう。短く取れば経路には忠実だが振動しやすい。この注視距離は経路の曲率に合わせて調整が必要である。

　Stanleyは注視点を置かないのは画期的に見えた。 $\Delta\phi$ は最近傍点における経路接線に対する方位偏差， $d$ は前輪位置での横偏差であり，二つを別々に評価してから足し合わせる。pure pursuitでは，注視点への視線角度という一つの角度に方位偏差と横偏差が混ざっていて，その混ぜ方を決めていたのが注視距離だった。Stanleyは混ぜ方を式の側で固定しているので，注視距離というパラメータが消える。

　ただし，調整が不要になるわけではない。 $K_\phi$ ， $k$ ， $v_0$ は残る。速度依存性が式に入ったぶん各ゲインの意味は明確になるが，数は増える。「調整量が経路形状に依存しにくくなる」と言うのが正確だろう。

　また，内回りが消える代わりに，逆の問題が生じる。Stanleyは最近傍点の誤差しか見ないので，先の曲がりを予見しない。急旋回では外側に膨らむ方向に誤差が出る。Hoffmannらの原論文でも，このために経路曲率のフィードフォワード項とヨーレートの減衰項を加えている。

　もう一点，方位偏差を注視点で薄めずにそのまま操舵角に入れるため，GNSS方位のノイズが操舵に出やすい。pure pursuitの注視距離は，実はローパスフィルタのように働いていた。直線部はStanleyの得意分野で，枕地では曲率のフィードフォワードが欲しくなる，という整理になりそうだ。

6. 今後

　パラメータは $a_1, a_2$ から $K_\phi, k, v_0$ へと増え，注視距離が減る。速度や経路が変わるたびの再調整が減ることを期待して，試してみるつもりである。曲線での挙動やノイズへの感度は，実験で確かめたい。

参考文献

木瀬道夫，野口伸，石井一暢，寺尾日出男 (2001). RTK-GPSとFOGを使用したほ場作業ロボット（第2報）—作業計画マップによるほ場作業の自律化. 農業機械学会誌, 63(5), 80–85. https://doi.org/10.11357/jsam1937.63.5_80

Thrun, S., Montemerlo, M., Dahlkamp, H., et al. (2006). Stanley: The Robot that Won the DARPA Grand Challenge. Journal of Field Robotics, 23(9), 661–692.

Hoffmann, G. M., Tomlin, C. J., Montemerlo, M., & Thrun, S. (2007). Autonomous Automobile Trajectory Tracking for Off-Road Driving: Controller Design, Experimental Validation and Racing. American Control Conference (ACC), 2296–2301. https://doi.org/10.1109/ACC.2007.4282788

The Day I Met the Stanley Controller

1. A familiar steering law

In our lab, we have long used the following steering law for tractor auto-steering (Kise et al., 2001).

\delta = a_1 d + a_2 \Delta\phi

$\delta$ is the steering angle, $d$ is the lateral deviation from the target path, $\Delta\phi$ is the heading deviation, and $a_1$ and $a_2$ are constant control gains. It multiplies the lateral deviation and the heading deviation by a coefficient each and adds them: a two-input, one-output P controller. As in pure pursuit, $d$ and $\Delta\phi$ have been evaluated at a look-ahead point placed ahead of the vehicle. It had worked well for years, so at some point I stopped questioning it.

2. The Stanley controller

Stanley is the name of the Stanford University autonomous car that won the 2005 DARPA Grand Challenge. Its lateral control law was formalized by Hoffmann and colleagues in 2007 (Hoffmann et al., 2007). The law is as follows.

\delta = K_\phi \Delta\phi + \arctan\left(\frac{k\,d}{|v| + v_0}\right)

$K_\phi$ is the heading gain, $k$ is the lateral gain, $v$ is the vehicle speed, and $v_0$ is an offset that keeps the controller from becoming too sensitive at low speed. There is an $\arctan$ , and the speed sits in the denominator. When I first saw it, it looked like something different from our law.

3. It also looks a lot like single-input proportional control

When the lateral deviation $d$ is small, $\arctan(x) \approx x$ , so

\arctan\left(\frac{k\,d}{|v| + v_0}\right) \approx \frac{k}{|v| + v_0}\,d

In other words, the lateral term is $\dfrac{k}{|v|+v_0}\,d$ , which has the same shape as our $a_1 d$ . The only differences are that the constant $a_1$ is replaced by a gain inversely proportional to speed, and that the term saturates through $\arctan$ .

4. What dividing by speed means

The yaw response of the vehicle is

\dot{\psi} = \frac{v}{L}\tan\delta

where $L$ is the wheelbase. For the same steering angle, the heading changes faster at higher speed. With a constant gain, the same lateral deviation gets the same steering at 1.0 m/s and at 3.0 m/s. We want weaker correction at high speed and stronger correction at low speed. Dividing the lateral term by speed matches this property. The $\arctan$ saturation keeps the steering from swinging to full lock when the lateral deviation is large. This is a consideration that plain P control never had.

5. No more look-ahead distance

There is one more thing I liked. Our law relied on the look-ahead point of pure pursuit. A longer look-ahead gives smoother motion but cuts the inside of curved paths, while a shorter one follows the path faithfully but tends to oscillate. This look-ahead distance has to be adjusted to the curvature of the path.

That Stanley places no look-ahead point struck me as groundbreaking. $\Delta\phi$ is the heading deviation from the path tangent at the nearest point, $d$ is the lateral deviation at the front axle, and the two are evaluated separately and then added. In pure pursuit, heading deviation and lateral deviation are mixed into a single angle, the line of sight to the look-ahead point, and the look-ahead distance is what decides the mixing. Stanley fixes the mixing inside the law, so the look-ahead distance disappears as a parameter.

That said, tuning does not disappear. $K_\phi$ , $k$ , and $v_0$ remain. With speed dependence built into the law, each gain has a clearer meaning, but there are more of them. It is more accurate to say that the tuning knobs become less dependent on the shape of the path.

Also, in exchange for no longer cutting corners, the opposite problem appears. Stanley sees only the error at the nearest point, so it does not anticipate the curve ahead. In sharp turns, the error tends to swing outward. The original paper by Hoffmann and colleagues adds a feedforward term for path curvature and a yaw-rate damping term for exactly this reason.

One more point: the heading deviation goes straight into the steering angle without being diluted by a look-ahead point, so GNSS heading noise shows up in the steering more easily. The look-ahead distance in pure pursuit was, in effect, working like a low-pass filter. Straight sections are where Stanley excels; on the headlands, we will want curvature feedforward. That seems to be the picture.

6. What comes next

The parameters increase from $a_1, a_2$ to $K_\phi, k, v_0$ , and the look-ahead distance goes away. I plan to try it, hoping that it reduces re-tuning every time the speed or the path changes. The behavior on curves and the sensitivity to noise are things I want to check in experiments.

References

Kise, M., Noguchi, N., Ishii, K., & Terao, H. (2001). Field Work Robot Using RTK-GPS and FOG (2nd Report): Autonomous Field Operation by Work Planning Map. Journal of the Japanese Society of Agricultural Machinery, 63(5), 80–85. https://doi.org/10.11357/jsam1937.63.5_80

Thrun, S., Montemerlo, M., Dahlkamp, H., et al. (2006). Stanley: The Robot that Won the DARPA Grand Challenge. Journal of Field Robotics, 23(9), 661–692.