自動運転における車両制御の役割
「自動運転」という単語を聞いたときに，皆様は何を思い浮かべるでしょうか？ おそらく大部分のエンジニアの方は「AI」や「次世代センサ」といったワードを思い浮かべるのではないでしょうか。AIによる高度な外界認識・運転行動決定はもはや自動運転実現には不可欠な技術です。昨今は，Transformerのような新しい学習モデル，走行データを効率的に蓄積可能なデータセンター，大規模な学習モデルを実装可能なAI半導体，といった革新的技術が多数登場しております。これらの技術により，すでに自動運転開発は次のフェーズに進んだと考えられています。
一方，次世代の自動運転車においても，「車両運動制御」の技術は必要不可欠だと考えられます。いかに強力なAIを構築したとしても，AIの指示通りに自動車が動いてくれなければ自動運転を実現できません。また，自動運転時代を迎えるころには，自動車の車体が現在と全く異なる構造になっている可能性もあります。人間による操縦が不要となれば，ハンドルレスの操舵アクチュエータやアクセル・ブレーキペダルの無いアクチュエータになるとされています。UXの拡充や搭乗人数確保のために車体が大型化する場合，車体の走破性を確保するために4WSや4WDの採用も十分考えられます。車体の構造・アクチュエータが変われば，それにあった運動制御が必要となることでしょう。自動車がメカトロニクス製品であり，最終的に走る・曲げる・止まるを実現することを考えると，車両運動制御の技術は依然として重要だと考えられます。
以上の背景により，ここでは車両運動制御の中でも「曲がる」の技術の代表である「経路追従制御」に関して解説します。特に，様々な実証実験で活用されているPath following制御と呼ばれる手法に関して説明します。今回の記事を見た方にとって少しでも参考になる内容が含まれていれば幸いです。
2024/11/3追記：
Path following制御のROS2オープンソースを作りました→Github
ROS2の環境さえあればどなたでも運動シミュレーションを体感できます。
問題設定
今回対象とする車両はFig.1の前輪操舵車両です。$δ$は前輪の操舵角です。$θ$と$ω$はそれぞれ車両の姿勢角とヨーレートです。$V$は車両の車速です。本システムでは車両を曲率$ρ$の目標経路へ経路追従させることを目指します。Fig.2が自動運転システムのシステム構成図です。本システムでは外界・内界情報に基づいて運転行動が決定され，車両の目標経路が計画されます。上位のシステム(経路計画・運転行動決定)では目標経路情報を経路の曲率$ρ$と目標車速として計算します。目標経路と自車間の横偏差・方位角偏差が経路誤差として計算・測定できるとします。以上の条件のもと，目標経路と自車間の横偏差と方位角偏差が小さくなるような自動操舵を実現することにより，車両を目標経路に追従させて自動走行することを目指します。ただし，車速に関しては車速制御により，目標車速と実際の車速がほぼ一致しているとみなします。本来は車速制御の設計もすべきですが，ここでは経路追従制御のみに焦点を当てて解説します。
Fig.1 前輪操舵車両
Fig.2 システム構成の概略
Path followoing制御
経路追従制御に関して様々な手法が提案されていますが，今回は国内自動運転の様々な実証で用いられてきたPath following制御に関して解説していきます。Path following制御はモデル予測制御のような実時間最適化を用いた手法ではありませんが，制御則が比較的シンプルな方法のため，いくつかの実証で用いられています。この章ではPath following制御の手法に関して解説していきます。
経路追従制御則
Path following制御における考え方として，目標経路上で自車と並走している仮想的な車両が存在するとします。この仮想車両に対して実車両を追従させることにより，経路追従実現を目指します。車体の挙動を運動学モデルとして表現すると，運動方程式を
\frac{d}{dt} \begin{bmatrix}
x \\
y \\
\theta
\end{bmatrix} = \begin{bmatrix}
\cos(\theta) & 0 \\
\sin(\theta) & 0 \\
0 & 1
\end{bmatrix} \begin{bmatrix}
V \\
\omega
\end{bmatrix}
と表現できます。($x$,$y$)は車両の自己位置です。目標経路における仮想車両の運動方程式を
\frac{d}{dt} \begin{bmatrix}
x_r \\
y_r \\
\theta_r
\end{bmatrix} = \begin{bmatrix}
\cos(\theta_r) & 0 \\
\sin(\theta_r) & 0 \\
0 & 1
\end{bmatrix} \begin{bmatrix}
V_r \\
\omega_r
\end{bmatrix}
と表現できます。$r$は仮想車両のパラメータを意味する添え字としています。それぞれの変数の意味は自車の運動方程式と同じです。仮想車両のヨーレートは
\omega_r = \rho V_r
と定義します。ここで，Fig.3のように仮想車両に対する誤差を方位角偏差$e_{3}$と横偏差$e_{2}$として考えます。
Fig.3 仮想車両に対する偏差の定義
Fig.3のように仮想車両の座標系に原点をおいて自車の動作を考える場合，幾何学的な関係から自車と仮想車両間の相対誤差を
\begin{bmatrix}
e_1 \\
e_2 \\
e_3
\end{bmatrix} = \begin{bmatrix}
\cos(\theta_r) & \sin(\theta_r) & 0 \\
-\sin(\theta_r) & \cos(\theta_r) & 0 \\
0 & 0 & 1
\end{bmatrix} \begin{bmatrix}
x - x_r \\
y - y_r \\
\theta - \theta_r
\end{bmatrix}
と表現できます。ここで，仮想車両に対する縦方向の誤差に関して$e_{1}$としています。さらに，この式を微分することにより，相対誤差の微分方程式を
\frac{d}{dt} \begin{bmatrix}
e_1 \\
e_2 \\
e_3
\end{bmatrix} = \begin{bmatrix}
V \cos e_3 - V_r + e_2 \omega_r \\
V \sin e_3 - e_1 \omega_r \\
\omega - \omega_r
\end{bmatrix}
と表現できます。ただし，先行文献と同じく$ω=dθ/dt$、$ω_r=dθ_r/dt$としています。ここでは，仮想車両の定義として自車と並走しているため，$e_{1}=0$かつ$de_1/dt=0$となるように自車と並走することを考えると，仮想車両の速度を
V_r = V \cos e_3 + e_2 \omega_r
と表現できます。このとき，仮想車両のヨーレートは
\omega_r =\rho V_{r}= \rho \left(\frac{V \cos e_3}{1 - e_2 \rho}\right)
と表現できます。仮想車両の速度が以上のように与えられるとき，相対誤差の微分方程式を
\frac{d}{dt} \begin{bmatrix}
e_2 \\
e_3
\end{bmatrix} = \begin{bmatrix}
V \sin e_3 \\
\omega - \omega_r
\end{bmatrix}
と表現できます。以上のように，自車と仮想車両に対する誤差方程式を導出できました。この誤差方程式に基づいてリアプノフの安定性理論を満たすような経路追従則がPath following制御となります。リアプノフの安定性理論とは，制御対象に対するリアプノフ関数に基づいて制御系の安定性を保証する論理です。よくわからない方は，「Path following制御で車両を経路追従させる際に，横偏差と方位角偏差を発散させずに収束させるための理論的な裏付け」だと思っていただけると結構です。導出方法の詳細は割愛しますが，リアプノフ関数が存在するようなPath follwoing制御の経路追従則は
\omega_c = \omega_r - K_2 e_2 V - K_3 \sin e_3
となります。$K_{2}$と$K_{3}$はフィードバックゲインであり， $ω_{c}$が車体の目標ヨーレートとなります。$ω_{c}$はPath following制御上からみた制御入力であり，実際の車体への入力は前輪の操舵角$δ$であることに注意してください。$ω_{c}$を車両のヨーレート$ω$として車両へ入力することにより，自車を目標経路に対して追従させることが可能となります。以上がPath Following制御による目標経路への経路追従則となります。
ヨーレート制御則
前節の経路追従則では，$ω_{c}$を車体へ入力する必要があります。しかしながら，実際の車両は前輪操舵での駆動しかできないため，$ω_{c}$に対して実際のヨーレート$ω$を追従させるような前輪の操舵角$δ$を計算する必要があります。Path following制御における文献では，車体の動力学モデルを導出し，フィードフォワード制御則としてヨーレートを制御する方法が提案されています。この方法では車体の動力学モデルの精度がよければ，$ω_{c}$に対してヨーレート$ω$を高精度に追従させることができます。しかしながら，この方法ではタイヤの正確なコーナリング係数や慣性モーメントといった測定困難な物理パラメータが必要となる上に，姿勢滑り角情報の測定も必要となってしまいます。車体の動力学に関わるパラメータが未知であることも多いため，ここでは運動学モデルにおけるヨーレート制御則を考えます。
車体の運動特性に関して，左右輪の特性差がなく，平面運動以外の特性を無視できると仮定します。具体的には，Fig.4のような等価二輪モデルを考えます。Fig.4に関して機構学的な関係から，車体のヨー方向の運動方程式を
\omega= \frac{ V \tan \left( \delta \right)}{l}
と導出できます。$l$はホイールベースとなります。等価二輪モデルの詳細な導出手順に関してはこちらの記事をご確認ください。
Fig.4 等価二輪モデル
本式の逆関数を計算することにより，ヨーレート$ω$を発生させるために必要な操舵角を
\delta = \tan^{-1} \left( \frac{l}{V} \omega \right)
と計算できます。上式が成り立つため，目標ヨーレート$ω_{c}$を発生させるために必要な操舵角をフィードフォワード制御則
\delta = \tan^{-1} \left( \frac{l}{V} \omega_c \right)
として表現できます。つまり，この式にて操舵角を計算して車両へ入力することにより，ヨーレート$ω$を目標ヨーレート$ω_{c}$に近づけることができます。一方，この方法では車体の動力学的な特性が全く考慮されていないため，車速が増加するにつれてヨーレート制御の精度が悪くなってしまう可能性があります。また，実際の車両走行時に様々な外乱が発生するため，フィードフォワード制御則のみでは外乱に対して脆弱な制御となってしまいます。そこで，ヨーレート制御則を2自由度制御系に拡張します。具体的には並列補償器としてPI制御を挿入し，制御則を
\delta = \tan^{-1} \left( \frac{l}{V} \omega_c \right)+K_P e_\omega + K_I e_\omega
e_\omega = \omega_c - \omega
と拡張します。$K_P$と$K_I$はPI制御器のゲインであり，制御系が安定化するようなパラメータとして設計します。一般的な2自由度制御系ではフィードフォワード制御器がノンプロパーな伝達関数となることを避けるため，参照モデル(目標値波形を整形する伝達関数)を内包します。このヨーレート制御則ではノンプロパーな伝達関数計算が生じず，通常の経路追従走行をする分には目標ヨーレートの整形が不要のため，参照モデルを内包しない構成としています。
以上がPath following制御における制御系の構成となります。ブロック線図にすると，下記のような構成となります。Path following制御は経路追従制御とヨーレート制御からなるカスケード制御の構成となっています。メジャーループにあたる経路追従制御がリアプノフの安定性理論を満たすため，目標ヨーレートと実際のヨーレートがマイナーループの制御によって一致していれば制御系の安定性が保証されているとされています。
Fig.5 制御系のブロック線図
参考：Path following制御の自動運転での歴史
ここまで紹介してきたPath following制御ですが，元々は東京大学の深尾隆則先生がご研究されていたロボット向けの制御技術です。この技術は，日本の代表的な自動運転プロジェクトであるNEDO エネルギー ITS 推進事業の「協調走行 (自動運転) にむけた研究開発」で活用されました。このプロジェクトは10年以上前に実施されていた非常に大規模な自動運転プロジェクトであり，様々な自動車関係メーカー・大学研究機関が参画してトラックの隊列走行自動運転に関わる技術開発を実施していました。深尾先生もプロジェクトへ参画し，トラックのレーンキープ/レーンチェンジ制御等の横運動制御の研究に取り組んでいたようです。研究成果に関していくつかの論文で発表されており，自動運転トラックでの自動操舵実現まで検証終了しております。プロジェクトが終了後も深尾先生は自動運転に関する研究を続けており，企業との共同研究を通じて乗用車のようなトラック以外の車両に対する適用も実施していたようです。近年は不整地向けの無人車両への適用に関する研究にも取り組んでおり，様々な実験検証等にも注力しております。下記に参考文献を記載しておきましたので，もしご興味あればこちらも参考にしていただくよいかもしれません。
NEDO自動運転プロジェクトでのPath following制御に関する文献
・吉田順, 杉町敏之, & 深尾隆則. (2011). トラックの Path Following 制御に基く自動運転. 日本機械学会論文集 C 編, 77(783), 4125-4135.
・杉町敏之, 深尾隆則, & 鈴木儀匡. (2013). トラックの Path Following 制御に基づく自動レーンチェンジ. 自動車技術会論文集, 44(1), 193-198.
・有尾拓誠, 杉町敏之, 深尾隆則, & 河島宏紀. (2013). 大型トラックの自動操舵制御ゲインのファジィ推論に基づくセルフチューニング. 自動車技術会論文集, 44(2), 485-490.
乗用車でのPath following制御に関する文献
・吉本達也, 深尾隆則, 横小路泰義, 伊能寛, & 松本平樹. (2016). 乗り心地と追従性を両立する自動車の自動操舵制御. 自動車技術会論文集, 47(4), 997-1002.
商用ビークルに関するPath following制御に関する文献
・植月悠記, 渥美友喜, 倉鋪圭太, 菅原宏, 大場優人, 前田元気, ... & 深尾隆則. (2021). オフロード環境下における無人運搬トラックのための自動操舵制御. 自動車技術会論文集, 52(5), 1155-1160.
・Kurita, H., Oku, M., Nakamura, T., Yoshida, T., & Fukao, T. (2022). Localization method using camera and LiDAR and its application to autonomous mowing in orchards. Journal of Robotics and Mechatronics, 34(4), 877-886.
Path following制御に関する補足・注意事項
ここでは，著者が思いつく限りの補足・注意事項に関して説明をしていきます。
経路追従制御則に関して
ここまで読んだ読者の中に歴戦の制御スペシャリストがいれば，「わりと制御則がシンプルだが，この方法でちゃんと制御できるんだろうか？」と思われることでしょう。走行環境や車両のタイプにもよりますが，上記で解説したPath following制御をそのまま実装しても乗り心地や制御性能が十分でないこともあります。実際の車両では操舵アクチュエータの応答特性や路面外乱・センサノイズの影響等により，目標ヨーレートと実際のヨーレートを常に一致させることはできません。ヨーレート制御の誤差が過大な値になると，経路追従性能に悪影響を及ぼすことがあります。Path followoing制御では車両の平面運動のみを考慮していますが，実際の車体は6自由度運動します。車体のロールやピッチングの影響が無視できないケースにおいても，制御性能に影響を与えてしまうこともあります。さらに，実際の自動運転では経路追従の精度だけでなく，搭乗者の官能評価等も重要になってきます。目標経路に対して高精度に追従できたとしても，大きな横加速度やヨーレートが発生してしまうと，搭乗者に強いショックを与えてしまいます。Path following制御はあくまで経路追従制御でしかないため，横加速度やヨーレートに対して制約を設定しながら制御することはできません。
以上のように，実際の自動車で使用するには様々な課題があるため，先行文献では自動運転の目標品質や車両タイプに応じた様々な改良が施されています。例えば，NEDOのトラック自動運転における論文をみると，車速に応じて経路追従則のゲインをスケジューリングする構成となっています。自動車は車速が変化すると，操舵角から横運動応答までの応答特性が変化することが知られています。車速変化に応じて異なるゲインを使用することにより，車両の時変特性と乗り心地に対応した経路追従を実現できるというわけです。乗り心地という点では，乗用車でのPath following制御に関する文献では，経路追従誤差に応じてゲイン可変とすることにより，乗り心地と追従性を実現できることが報告されています。
ヨーレート制御則に関して
Path following制御では，ヨーレート制御則の性能が悪いと，目標ヨーレートと実際のヨーレートが十分一致せず，経路追従に悪影響を与えてしまいます。しかしながら，前節で述べたように自動車の特性は車速に応じて変化するため，単純なPID制御のみでは十分な制御精度を実現できないことがあります。さらに，自動車の操舵アクチュエータには様々な制約条件が存在します。操舵アクチュエータのモータトルクの上限もありますし，タイヤの操舵角にも機械的な上限があります。加えて，タイヤの特性は路面の状況によっても変動してしまい，それがヨーレートの応答特性にも当然影響を与えます。本記事では非常にシンプルなヨーレート制御則を紹介しましたが，問題設定や車両によっては十分な制御性能を発揮できないことも十分ありえます。
Path folloiwing制御以外の経路追従でもヨーレート制御を使うことがあるため，上記の課題は車両運動制御に関する全般的な課題でもあります。昨今は自動運転や先進安全技術の取り組みが活発になってきた影響により，自動運転車両のヨーレート制御に関する研究論文もいくつか発表されています。Path following制御に限定した方法ではありませんが，いくつかの文献では非常に高精度なヨーレート制御ができています。データ駆動制御のような手法を組み合わせた方法や周波数応答ベースでの設計事例もあるため，本記事で紹介したヨーレート制御手法がうまくいかない場合は，下記の文献の手法も試してみるのも手かもしれません。
ヨーレート制御則に関する文献
・鈴木元哉. (2022). データ駆動制御と部分モデルマッチング法によるビークルのゲインスケジュールド制御. 電気学会論文誌 C (電子・情報・システム部門誌), 142(5), 607-615.
・鈴木元哉. (2022). 時変フィードフォワード制御とデータ駆動制御によるビークルの自動走行. 電気学会論文誌 C (電子・情報・システム部門誌), 142(12), 1313-1320.
・藤井聖也, 宮腰穂, 脇谷伸, 和田信敬, 足立智彦, 矢野康英, & 山本透. (2022). スマートモデルベース開発 (S-MBD) アプローチに基づく制御系の一設計とその自動車のヨーレート制御への応用. 計測自動制御学会論文集, 58(3), 186-193.
・Yahagi, S., & Kajiwara, I. (2024). Data-driven design of model-free control for reference model tracking based on an ultra-local model: Application to vehicle yaw rate control. Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering, 09544070231215636.
・鈴木元哉. (2022). 制御入力速度飽和した初期実験データによるビークルのデータ駆動予測型制御器調整. 電気学会論文誌 C (電子・情報・システム部門誌), 142(8), 959-970.
・蛯名浩志, 牧野統真, 藪井将太, 鈴木元哉, 深沢慎一郎, & 野中謙一郎. (2023). 階層構造により計算コストの削減を実現する自動運転トラックの非線形モデル予測制御. 自動車技術会論文集, 54(3), 485-491.
数値シミュレーションによる検証
それでは，車両シミュレーションにて経路追従制御ができるかを検証してみましょう。せっかくなので，昨年のMATLABのadvent calendarの記事で紹介した14自由度の高精度運動ミュレーション環境を使って検証してみます。ここでは，一定曲率での目標経路に対する経路追従のシミュレーションを実施します。
Fig.6 数値シミュレーションにおける走行条件
本シミュレーションでは，Path following制御の経路追従則のゲインを$K_2=0.1$, $K_3=0.1$と設定しています。ヨーレート制御のPIゲインは$K_{P}=0.3$, $K_{I}=0.3$としています。この条件下にて，車速5m/sで目標経路への経路追従を試みます。Fig.7が経路追従結果です。経路追従における横偏差の最大値が約0.3m程度となっており，よほど狭い道路を走行するわけでないなら問題にならない程度の経路追従ができています。ヨーレート制御の結果を見ても，ヨーレートが目標値に対しておおむね追従していることがわかります。
(a) 目標経路に対する横偏差(縦軸：時間[s]、横軸：横偏差[m]
(b) ヨーレート制御結果(縦軸：時間[s]、横軸：ヨーレート[rad/s])(青線：目標ヨーレート、赤線：ヨーレート)
Fig.7 車速5m/sでの経路追従結果
qiita用まとめ：車速5m/s
— 制御系エンジニア(いつもこころにAD/ADAS推進) (@KariControl) February 24, 2024
比較的滑らかに一定曲率旋回ができている pic.twitter.com/fFdFBIqn2z
最後に，車速10m/sで目標経路への経路追従を試みます。Fig.8が経路追従結果です。経路追従誤差の最大値が0.4m付近まで悪化しており，車両の挙動がやや振動的になっています。車体のヨーレート制御の挙動をみても，やや振動的な挙動になっていることがわかります。注意事項・補足の章でも述べたように，実際の車体は車速によって応答特性が変化するため，車速が変化すると経路追従の制御結果が悪化してしまうことがあります。また，今回のシミュレーションでは14自由度の車両運動モデルをプラントモデルとして使っているため，車速増加によってローリングやピッチングの影響も受けていると考えられます。しかしながら，それでも横偏差が0.4m以下に収まっており，乗用車クラスであればよほど狭い道路を走行するわけでないなら問題にならない程度の精度を実現できています。以上の結果から，Path following制御にて経路追従できることを確認できました。
(a) 目標経路に対する横偏差(縦軸：時間[s]、横軸：横偏差[m]
(b) ヨーレート制御結果(縦軸：時間[s]、横軸：ヨーレート[rad/s])(青線：目標ヨーレート、赤線：ヨーレート)
Fig.8 車速10m/sでの経路追従結果
qiita用まとめ：車速10m/s
— 制御系エンジニア(いつもこころにAD/ADAS推進) (@KariControl) February 24, 2024
車速増加によりやや振動的な挙動 pic.twitter.com/LXaCAyAk90
まとめ
本記事ではPath following制御を解説しました。本手法は比較的シンプルな方法ですが，ある程度の精度の経路追従を実現できます。実際に使う際は車両や目標品質に応じた作りこみが必要な場合もありますが，MPCのような最適化計算が不要のため非常にリーズナブルな方法です。本記事の内容が自動運転に関わるエンジニア・研究者の制御技術開発に少しでも参考になると幸いです。また，自動運転の根幹技術である車両運動制御の発展に少しでもつながると幸いです。それではまた次の記事でお会いしましょう。
謝辞
車のイラスト素材をいらすとやさんからお借りしましたので，ここでクレジット明記させていただきます