MATLAB homeによる自動運転/ADAS運動制御のための車両制御Simと自動レーンチェンジ

# はじめに
今年は国内にて自動運転レベル4の公道解禁がなされ、自動運転に関する様々な取り組みが公表されました。自動運転だけでなく先進運転支援システム(Advanced driving assist system：ADAS)も様々な車両に搭載され、手放し運転や自動駐車等の画期的な機能も普及しつつあります。自動運転やADASは車両制御・センシング・行動計画等を統合することによって成り立つ非常に高度なシステムです。まさに、最先端の技術の粋を集めて生み出されたシステムといっても過言ではありません。特に、システムの根幹を支える車両運動制御技術は車両運動力学・状態推定・制御工学・数値最適化等の知識を要する高度な技術であり、エンジニアや研究者にとって非常に興味深くて面白いテーマです。そのため、「<font color="red">仕事ではなく、趣味や自己啓発で自動運転/ADAS制御開発に取り組みたい！</font>」というMATLABユーザーも大勢いらっしゃるのではないでしょうか。

趣味で自動運転/ADAS運動制御開発に取り組む場合、個人で実験車両や試験設備を作ることができないことが多く、一部の特殊例を除けば基本的には車両シミュレーンベースで楽しむ・勉強することがメインとなります。何らかの方法で車両シミュレーン環境を揃えなければなりませんが、独力で車両プラントモデルや路面モデルを作ることは非常に困難です。そのため、MATLABの力を借りてシミュレーン環境を準備したいのが正直なところです。また，車だけではなくMATLABのスキルをも高めたいという方にとっては，MATLABで環境構築できた方が好都合でしょう。

MATLABには自動運転/ADAS開発用ツールが多く実装されており，専用Toolboxの力を借りればシミュレーン環境構築のハードルが大幅に下がります。しかしながら，趣味や自己啓発の場合では商用ライセンスではなくMATLAB homeライセンスを使うことになります。MATLAB homeでは、自動運転/ADAS開発用のToolboxがほとんど使えないという制約があります。それでは，趣味・自己啓発の範疇でMATLAB homeをどのように使えばよいでしょうか？

本記事では，「趣味・自己啓発目的で自動運転/ADAS運動制御開発に取り組みたい」方向けにMATLAB homeを用いた運動制御シミュレーンの事例をご紹介します。実車レスでの機能検証ができるレベルではないかもしれませんが，趣味・自己啓発目的の運動シミュレータとしては十分な性能といえます。本記事をご覧になった方にとって少しでも役に立つ情報が記載されていれば幸いです。

## 注意
本記事ではMATLAB R2023aでの検証となります。実践いただく場合はMATLAB homeに下記のToolboxが必要なのでご注意ください。
-必須
・MATLAB
・Simulink
・Simscape
・Stateflow
・Simscape multibody

-推奨
・Control system Toolbox(制御系設計するにはあった方がよい)
・Aerospace Toolbox(車載センサモデルを用意するのが手間であればあった方がよい)

# シミュレーン環境構築のハードル
車両制御の検証をする場合，車両プラントモデリング構築が最も重要な要素となります。車両の横運動制御では，車両の横方向の特性を平面運動として扱い，左右輪の特性差が生じないことを前提とした等価二輪モデルが知られています。等価二輪モデルは車両の運動解析・制御系設計にて最も多く使われている運動モデルであり，アカデミア・企業の双方の現場で活躍するモデルです。例えば，下記のような記事でも等価二輪モデルが活用されています。

https://qiita.com/Fuminori_Souma/items/9e0bc9a92515cfffeda1

車両の縦運動制御では，一輪モデルと呼ばれる簡易的なモデルを用いることが多いです。一輪モデルでは車体を縦方向に加減速する質点として扱い，トルクから加減速・速度までの運動方程式として扱ういます。例えば，下記の文献(文献中の2.3節)のように，駆動力orトルクから加減速応答までの運動方程式として表現します。

参考文献：
山内賢, 梅澤結花, 瀬戸洋紀, 今村稔朗, & 滑川徹. (2022). 階層型モデル予測制御による mild HEV の燃費最適化. 計測自動制御学会論文集, 58(4), 203-212.

より簡易的なモデルではエンジンやモータによる損失や走行抵抗の影響を一部無視することにより，簡単な運動方程式として扱います。

参考文献：
髙野毅, 延本秀寿, 岡﨑俊実, & 藤本博志. (2015). 高精度スリップ率制御による駆動力制御技術.

これらの車両モデルは車の簡単な運動解析・制御系設計という観点では，非常に扱いやすくシンプルなモデルです。しかしながら，より詳細な運動解析や制御系の検証をする場合には，少なくとも6自由度(xyz方向の3自由度とロール・ピッチ・ヨーの3自由度)の車両モデルが必要となります。さらに，車体に発生する力や速度をある程度正確にシミュレーンする場合，タイヤやアクチュエータ系の非線形方程式も実装する必要があります。より実際の車両の挙動に近いシミュレーションをする場合は，多自由度の非線形な車両モデルを作る必要があります。

アクチュエータを含めた多自由度の車両モデルを自力で実装するには非常に手間と時間がかかります。また，モデルのパラメータ変更に関しても，UIが悪いと検証における効率がさらに悪くなります。このような事情からMATLABではVehicle dynamics Blocksetと呼ばれるSimulinkライブラリが提供されています。Vehilce dynamics Blocksetでは様々な車両モデル・アクチュエータ・部品のSimulinkモジュールが用意されています。車体の3~6自由度のモデルだけでなく，ステアリングや非線形タイヤモデル・センサ等もモジュールとして提供されています。事前構築済みの車体モデルと部品のブロックをつなぎ合わせるだけで車両シミュレーンモデルを簡単に作成できます。さらに，ブロックのUIもわかりやすく構成されており，パラメータ変更も非常にやりやすくなっています。

https://jp.mathworks.com/products/vehicle-dynamics.html

また，リファレンスアプリケーションもいくつか提供されており，車両制御検証でおなじみのダブルレーンチェンジ試験や旋回試験の環境構築も気軽にできます。自作した車両制御の有効性を検証するという点において非常に強力なツールといえるでしょう。

https://jp.mathworks.com/help/vdynblks/ug/double-lane-change-maneuver.html

このように非常に素晴らしいSimulinkのブロックセットが提供されているので，Vehicle dynamics Blocksetを使えばお手軽に自己啓発・趣味での制御開発ができそうです。ただし，Vehilce dynamics Blocksetには一つだけ致命的な点があります。それは，「<font color="red">MATLAB homeではVehicle dynamics Blocksetが利用できない</font>」という点です。MATLAB homeはあくまで趣味・自己啓発のための特別なMATLABライセンスです。Vehicle dynamics Blocksetのような商用利用をメインとする機能はMATLAB homeでは利用できないようになっています(本記事執筆時点)。非常に残念ですが，MATLAB homeでVehicle dynamics Blocksetが使えない以上別の手段をつかわなければなりません。

# MATLAB HomeでのSimscape multibodyによるシミュレーション構築
結論から述べると，MATLAB homeで多自由度車両運動シミュレーンをする方法は「Simscape Vehicle Templates」を使うことです。Simscape Vehicle Templatesとは，Simscapeとそれのファミリ製品Simscape multibodyを利用して作られている車両モデルのテンプレートセットです。このテンプレートでは，Simscapeのマルチボディダイナミクスのモデリングモジュールを用いて作られたさまざまな車載部品・駆動源・車体モデルが含まれています。このテンプレートはMATLABの公式サイトから無償でダウンロードできます。SimscapeとSimscape multibodyはMATLAB home上でも購入できるため，趣味・自己啓発目的で使うことも可能です。

https://jp.mathworks.com/solutions/physical-modeling/simscape-vehicle-templates.html

さらに，MathWorks様の社員がこの「Simscape Vehicle Templates」を使ったアプリケーションをGithub上で公開しています。公開されている車両モデルは合計14自由度(6自由度車体に加えて，magic formulaのタイヤモデル・サスペンション・ステアリング等)の多自由度モデルとなっています。さらに，車両モデルだけではなく，走行環境(路面や道路)の簡単なモデルも含まれています。つまり，このアプリケーションをベースに小改造をすれば，MATLAB homeでも十分に運動制御のテストができるということです！

https://jp.mathworks.com/matlabcentral/fileexchange/110350-vehicle-dynamics-14-dof-model-in-simscape-multibody

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2981571/ceed8ebb-9fd0-8e08-475f-d6765b684fc0.png)

ビークルモデル内部のブロック構成


さっそくGithub上の14自由度ビークルモデルをダウンロードし，MATLAB homeで動かしてみました。動かし方は非常に簡単で同梱されているプロジェクトファイルを実行すればシミュレーンが実行されます。ここでは，道路のモデルを平坦な道路に設定し，Sin操舵で走行させてみました。実際にシミュレーンを実行した結果が下記の動画です。アニメーションからわかるように車体がSin操舵で動いていることがわかります。車両が6自由度の運動をしており，これなら十分に制御の検証に使えそうです。

<blockquote class="twitter-tweet" data-media-max-width="560"><p lang="en" dir="ltr">Advent calendar用 <a href="https://t.co/q2K63U2A4z">pic.twitter.com/q2K63U2A4z</a></p>&mdash; 制御系エンジニア(元制御系エンジニアリングセンター) (@KariControl) <a href="https://twitter.com/KariControl/status/1732008242463854971?ref_src=twsrc%5Etfw">December 5, 2023</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


<blockquote class="twitter-tweet" data-media-max-width="560"><p lang="en" dir="ltr">Advent calendar用2 <a href="https://t.co/ot7dg61JuO">pic.twitter.com/ot7dg61JuO</a></p>&mdash; 制御系エンジニア(元制御系エンジニアリングセンター) (@KariControl) <a href="https://twitter.com/KariControl/status/1732008393890812026?ref_src=twsrc%5Etfw">December 5, 2023</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

Sin操舵による走行シミュレーション

# 検証例：レーンチェンジ制御設計とテスト

## シミュレーンモデルの改造
このシミュレータを改造して，自作の運動制御のテストをしてみましょう。ここでは，4WS車両の運動制御に関するシミュレーションをしてみます。4WS車両とは一般的な前輪のみが稼働する車両ではなく，前輪と後輪の両方を操舵できる車両を指しています。かつて国内でもいろいろな車両に4WSが搭載されていましたが，現在では一部の高級車を除くと搭載されていません。ただ，昨今は衝突安全や車体大型化に対応するため，欧州では採用している車がいくつかあります。

https://global.honda/jp/tech/serial/Honda_Super_Handling_part01/

https://motor-fan.jp/tech/article/38246/

前章で紹介した14自由度のシミュレーンモデルは前輪操舵車両のモデルです。これを4WS用に改造します。車両モデルの内部を見ていくと，下記の図のように前輪と後輪のサスペンション系のブロックが出てきます。前輪に関してはkinematicなステアリングモデルがサスペンションブロック内に実装されています。後輪も操舵できるように，赤丸の箇所に操舵系のモデルを追加して後輪の操舵角を外部から入力できるようにします。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2981571/2ced2e0c-92d9-c0a5-d915-bd3a7307d22a.png)

次に，センサ系のモデルを追加します。実際の車両では，IMUが実装されていて縦・横加速度とヨーレートを検出できます。ほとんどの車両運動制御では，IMUの測定値を用いて車体の横運動を制御します。IMUによる計測値は，ランダムウォークやオフセット・車体振動の影響を受けます。車両制御のテストをする場合は，当然センサによるノイズの影響も考慮すべきなため，IMUのモデルもシミュレーンに実装する必要があります。ここでは，Aerospace ToolboxのIMUモデルを組み込んで，実際のセンサノイズの挙動を模擬します。Aerospace Toolbox自体は航空宇宙用のTooboxですが，IMU自体の原理は車載向けでも同じなため，ToolboxのIMUモデルを実装します。この車両モデルでは下記の図のようにタイヤや車体の位置・速度・角速度・角度の真値を出力できるため，対応した信号をIMUブロックに入力することにより，センサノイズの挙動を模擬したヨーレート・加速度検出をシミュレーンできるようになります。

https://jp.mathworks.com/help/aeroblks/threeaxisinertialmeasurementunit.html

<img width="420" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2981571/f2a02af3-6155-e575-9010-2c70cbeb4b83.png">


<img width="250" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2981571/a2cab8a1-a8cd-ce75-8c4e-b185a91237a4.png">

左：車体モデルから出力可能な物理量、右：IMUモデル

## 制御系設計とテスト
シミュレーションモデルの準備ができたので，車両運動制御を自作して動作テストをしてみましょう。今年の自動車技術会の秋季大会で4WSの運動制御に関する研究が発表されていたので，その内容を参考にして制御を設計します。

参考文献：
佐藤渉、土屋義明、福川将城：前後同相操舵による新たな価値の創出、自動車技術会2023年秋季大会学術講演会

上記の予稿ではヨーレートを0にするような後輪操舵入力を加えることにより，車体の向きを変えずにレーンチェンジを可能と制御方法が述べられています。予稿にはヨーレートを0にするための制御方法のみ記載されているため，レーンチェンジに必要なほかの制御器・状態推定器に関しては自作しました。オリジナルの方法を色々追加し，IMUで測定した加速度・ヨーレートと車輪速度を用いて自動レーンチェンジを実現する制御系を設計しました。今回は下記のようなブロック線図の制御系を設計しました。この制御系では上位システムからターゲットレーンの情報を受信し，車体がターゲットレーンに追従するように車体の横位置を制御します。実際の車体では横位置を直接測定できないため，IMUの情報から横位置を推定することにより，IMUの情報のみでレーンチェンジを実現します。制御系の詳細に関しては，紙面の都合で別の機会に述べさせていただきます。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2981571/da4c8be8-f850-ba4c-e8c3-83c5b02df891.png)
レーンチェンジシステムのブロック線図

モデルと制御器の実装・改造を施し，4WS車両のレーンチェンジシミュレーションをやってみました。MATLAB上でシミュレーションを実行した結果が下記のとおりです。3つの視点に対して，それぞれの視点から見たアニメーションの動画です。動画を再生していただくとわかるように，車体の姿勢角が変化せずに隣接レーンに車が横移動しています。状態推定器にて車体の横方向の運動を推定し，推定値に基づいたフィードバック制御が適切に動作することでレーンチェンジに成功しています。また，よく見ると後輪が前輪に合わせて変化していることがわかると思います。この後輪の角度変化により，車体の姿勢角を変化させずに車体を横方向に運動させることができます。以上の結果から，MATLAB homeを活用することにより，4WS車両のレーンチェンジシステムの作成・検証ができました。

<blockquote class="twitter-tweet" data-media-max-width="560"><p lang="ja" dir="ltr">Advent calendar用その4 レーンチェンジ <a href="https://t.co/1Phz65KAfy">pic.twitter.com/1Phz65KAfy</a></p>&mdash; 制御系エンジニア(元制御系エンジニアリングセンター) (@KariControl) <a href="https://twitter.com/KariControl/status/1732383812582359428?ref_src=twsrc%5Etfw">December 6, 2023</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
(a) 後方から見た様子
<blockquote class="twitter-tweet" data-media-max-width="560"><p lang="ja" dir="ltr">Advent calendar用その6 レーンチェンジ(上からの様子) <a href="https://t.co/kSUcWCYcck">pic.twitter.com/kSUcWCYcck</a></p>&mdash; 制御系エンジニア(元制御系エンジニアリングセンター) (@KariControl) <a href="https://twitter.com/KariControl/status/1732384496270590197?ref_src=twsrc%5Etfw">December 6, 2023</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
(b) 上空から見た様子
<blockquote class="twitter-tweet" data-media-max-width="560"><p lang="ja" dir="ltr">Advent calendar用その5 レーンチェンジ <a href="https://t.co/2a5d2uBFb9">pic.twitter.com/2a5d2uBFb9</a></p>&mdash; 制御系エンジニア(元制御系エンジニアリングセンター) (@KariControl) <a href="https://twitter.com/KariControl/status/1732384198915457294?ref_src=twsrc%5Etfw">December 6, 2023</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
(c) 右から見た様子

自動レーンチェンジシミュレーション結果

# おわりに
本記事では，「趣味・自己啓発目的で自動運転/ADAS運動制御開発に取り組みたい」方向けにMATLAB homeを用いた運動制御シミュレーンの事例をご紹介しました。オープンソースとなっているSimscape Multibodyの車両シミュレーンを活用することにより，商用向けのライセンスを使わなくても多自由度の車両運動シミュレーションができました。さらに，記事内で紹介したように，シミュレーションを自分で改造することにより，様々な車体・運動制御系のテストまで可能となります。MATLABのスキルを高めつつ，車両運動制御のスキルも高めたいという方が本記事の内容を見て参考になれば幸いです。

# おまけ：さらなる趣味・自己啓発に向けた制御活動紹介
最後まで記事を読んでいただきありがとうございます。実はSNS上で「制御工学勉強会」というエンジニアを中心とした勉強会がdiscord上で結成されました。この会はエンジニアや学生が制御技術に関して気軽に雑談・相談したり，トークを通じて他分野とコミュニティを促進する交流会・勉強会となっております。制御だけでなく，MATLAB等の有益なツール情報もお手軽に入手できたります。

本記事はMATLAB活用や車両のみに焦点を当てたものでしたが，もし制御部分や他のMATLAB活用等で趣味・自己啓発をしたい方はこちらのコミュニティも参考にしていただくとよいかもしれません。私自身も一エンジニアとして参画しており，そのうち今回の記事での制御部分の詳細や他の制御記事の内容も勉強会内で紹介できればと思います。興味のある方はぜひご参加をご検討ください！

https://peatix.com/group/13181263

https://t.co/tBUFbnREkI