SimulinkでETロボコン用のシミュレーション環境を構築した話

本記事は、ETロボコン&EV3 Advent Calendar 2018 の記事です。

#はじめに
こんにちは、関西の方でETロボコンの実行委員をやってる者です。
2記事目なので、自己紹介とアドバンストの開発は ↓ の記事へ
https://qiita.com/tko_ji/items/8c262f80c4b4b1d7c068

簡単に述べれば趣味でETロボコンの攻略をしています。

最近、自分の本業が大体MATLABとSimulinkのみになってきていて、自前でHomeライセンスのSimulinkを所持していることもあり、ETロボコンでも使ってみようということで、Simulinkを使ったシミュレーション環境を構築しました。だいぶ有効に活用できたのでその辺りの考え方を書いていきます。

(4月の独自勉強会の資料の図を流用したため、コース画像に古いものが紛れていることに途中で気づきました。内容に問題が生じるわけでもないのでそのままです。)

#やったこと
・走行体のモータ(タイヤ)、カラーセンサ、移動、の挙動を、Simulinkで動作するプラントモデル(数理モデル)で表現した
・実コースの代わりにコースの画像を用い、コースの色情報をSimulinkから呼び出せるようにした
・EV3へ書き込む制御プログラム(C++)をSimulinkから呼び出せるようにした

上記３つを組み合わせて、
・Simulink上で、走行体のプラントモデルを用いて、実機に書き込むCの制御プログラムを動作させる環境を構築し、制御開発を行った。

　業界によってはしばしば耳にする、いわゆるSILS(Software In the Loop Simulation)開発にあたります。
実機で動くCソフトを、シミュレーションで動作確認しながら進める開発です。

#シミュレーションのメリット

###走行体・実コース無しでできる開発が増える
走行体・実コースをSimulinkで再現するので、PC1台でいつでもどこでもETロボコンの開発ができるようになります。
書いたCコードの動作を、コースを広げなくても、例えばファミレスやカフェで、いろいろ開発ができるわけです。

###省時間オペレーション
書いたコードの動作確認ルーチン時間短縮。従来、コンパイルして、EV3を起動して、書き込んで、走行体をコースに設置して、ようやく走れますが、シミュレーションなら走り出すまで大体１クリックで済みます。はやいです。

###外乱がない環境、かつ、内部値がすぐに確認できる環境で開発
デバッグが楽です。
バグの原因が外的要因なのか、アルゴリズムなのか切り分けを頑張らなくてもいい、また、内部値をBluetooth通信してログしてグラフ化して、とかしなくてもすぐ確認できます。

#できないこと、苦労すること

###Simulink上でモデル化できてないことには使えない
・高速走行・倒立走行の調整
　今回は、アドバンストの難所のアルゴリズム検証を目的としているので、慣性とか質量とか無視したモデルにしているので走行制御の調整には使えないです。走行体のプラントモデルをちゃんと作りこめばいけるかもですが。

・ルックアップゲート、シーソー
　立体的な物体との関係をモデル化させるの難しい。

　プライマリの方が、シミュレーションをしようとすると、摩擦とか慣性とか重力とかの考慮が必要で難しく相性悪いです。一方、アドバンストは新幹線も消え、平面での情報処理が主題なので相性がいいです。
　
　相性というと抽象的ですが、表現する必要がある物理現象が、プライマリの方がより細かくなるという感じです。
　今回は、アドバンストの探索や文字認識の動作確認が目的だったのでプラントモデルは「ただ走ればいい」だけでしたので簡素に作っても目的を満たすわけです。

###実機とシミュレーションの平行開発になる可能性
たまーに、しばしば、実機とシミュレーションでCプログラムを変えざるを得なくなるところがあります。実機で調整した値だとシミュレーションでうまく動かないとか。
そんなときは、シミュレーション側でもうまく動くよう調整したりします。調整作業量が倍になってつらい時があります。これを無視するといざというときに使えないので、ちゃんとケアしていく必要があります。

#やったこと詳細

以降、シミュレーション環境を実現するのにやったことを詳細に述べていきます。

ちなみに使用したのはSimulink のhomeライセンスです。
個人の趣味用限定なら、企業で購入するときの数十分の一ぐらいの価格？でライセンスが買えます。
それでも個人のお小遣いに対しては高額ですが・・・。
ちなみにhomeだとSimulinkモデル→Cの変換ツールは使えないので、EV3に書き込むソフトをSimulinkで開発することはできず、SILS開発にせざるを得ないです。

###プラントモデル(物理モデル)を作成
そもそもプラントモデルとは何か。
「ETロボコンで提出するモデル」はソフトウェアの構造や振る舞いを模式化したものです。
それに対して、プラントモデルは現実世界の物体(本件では「走行体」と「コース」)や振る舞い(本件では「走行」)を数学や物理で(＝数理的に)、模式化(＝モデル化)したものです。

以下のようにモデル化しています。

まず「走行体がコースを走行する」という事象を整理します。

これは「走行体」と「コース」という物体と、「移動」という事象の３つの要素で表現できます。

これらの要素間の関係、すなわち入力・出力の関係を整理し、数理的な表現で置き換える、というのがモデル化の流れとなります。

それでは各要素を考えていきます。
「走行体」はカラーセンサと左・右モータと制御プログラムを持ちます。カラーセンサで、コース上の現在位置の色を読み取り、プログラムが演算を行い、左・右モータへ回転の指令を出し、モータが回転します。

　アームを無視すると、アドバンストの走行体の入出力はこれで表現できています。
　
　モータが回転すると走行体は「移動」します。移動すると走行体の位置が変わります。
　走行体は「コース」から変化した位置に応じて、カラーセンサで色を読み取ります。

　結局、図のような関係になり、入出力は以下のように整理されます。

　コース
　　入力：(走行体の)位置
　　出力：コースの色

　走行体
　　入力：コースの色
　　出力：モータ回転

　移動
　　入力：モータ回転
　　出力：位置

これらの入出力関係を数理的に表現してやるとプラントモデルとなります。

すなわち、規定の入力に対し規定の出力をする関数・機能を表現すればいいわけです。

###「コース」を数理モデル化する

実コースの役割(機能)は
　　入力：(走行体の)位置
　　出力：コースの色

でした。

位置に対する色がわかればよいので、モデル化のためコースの画像を使います。
画像を読み込み、データとして保持して、
　　入力：X,Y座標 (ピクセル ⇔ mmの単位変換は必要)、
　　出力：画素のRGB値、

として置き換えてやれば数理的に表現できます。

###「移動」を数理モデル化する

なんか難しい印象を受けますが、ETロボコンのド定番技術で実現できます。

実機では、移動
　　入力：モータ回転
　　出力：位置

となりますが、これはよく用いられる自己位置推定・オドメトリの座標演算を使います。モータの回転数から、X,Y,ヨー角(方位角),走行距離を求めるあれです。

この計算は「移動」という現象を数理で表している、といえるわけなのでそのまま今回のモデル化に利用でき、入出力を以下で置き換えます。

　　入力：モータ回転数
　　出力：X,Y座標

###走行体を数理モデル化する
　走行体はカラーセンサと左右モータと制御プログラムで構成されますが、もう少し細かく書くと、

カラーセンサ
　入力：コースの色
　出力：カラーセンサ値(EV3のAPI)
　
制御プログラム
　入力：カラーセンサ値(EV3のAPI)
　出力：左右モータ指令値(EV3のAPI)

左右モータ
　入力：左右モータ指令値(EV3のAPI)
　出力：モータ回転

となります。

　それぞれモデル化を考えてみます。

###カラーセンサを数理モデル化する

　実機で
　　　入力：コースの色
　　　出力：カラーセンサ値(EV3のAPI)

　となっているのを
　　　入力：コース画像のRGB値
　　　出力：スケール変換したRGB値

　と表現してやればよさそうです。　

実コースに対してEV3のAPIを使って色を読むと
　白＝(100,100,100)
　黒＝(5,5,5)　
ぐらいの値が得られると思います。

コース画像の場合は
　白＝(255,255,255)
　黒＝(0,0,0)
ぐらいが得られると思うので、適当に換スケール変換するか、
制御プログラムでETロボコン定番のキャリブレーション機能があれば、変換さえいらないかもしれません。

さて、実際のカラーセンサは、コース上の1点の色を読んでいるわけではないですね。黒を読んでいて、腺から外れても突然白にはならず、黒→白へ段階的に変化します。これはセンサの検出範囲がLEDで光っている直径20mmぐらいの範囲の色を読んでいるためですね。
モデル化にあたりこの辺りも表現してやる必要があります。

というわけで、
「コース画像のRGB値」は任意の座標点から、直径20mmに相当する画素値の平均値を求めて、カラーセンサ値に替わる値としています。

###モータを数理モデル化する

左右モータの実機は
　入力：左右モータ指令値(EV3のAPI)
　出力：モータ回転(振る舞い)

を
　入力：左右モータ指令値
　出力：モータ回転数

という感じです。
モータの回転という現象を数理で表すのがポイントです。

回転を数理的に表す、というと結局、回転数を数えることになります。

指令値を与え、時間経過に従い、回転数が増える、ということです。

　　モータ回転数 ＝ モータ回転速度 × 時間

で、EV3のモータの指令値はモータの回転速度と比例の関係があります。
そのため、

　　モータ回転速度 = モータ指令値 × 定数

よって
　　モータ回転数 ＝ モータ指令値 × 定数 × 時間

で回転速度が表現できます。
定数は、2018年タイヤで、電池電圧8.7V程度、指令値60のときに612mm/秒 で走行した計測データから換算しています。
（そんな精度いらなかったので結構適当です。）

さて、上記の掛け算だけの処理だと、指令値を与えた瞬間に回転速度がステップ的に変化してしまいます。
実際はモータの指令値を与えたら加減速して徐々に一定速に近づく動きをしますので、ちょっと実際の振る舞いと合ってないです。

というわけで適当にフィルタ挟む等して、加減速の応答要素を盛り込んでみたのですが、
シミュレーション上でのライントレースが無駄に難しくなってしまい、あまりよくなかったです。

結局、指令値を入れた瞬間にその速度で回転する、という簡単なモデルにしてます。
慣性とか遅れとかがないので、ライントレースを失敗しない走行体になります。

この辺りが、何をシミュレーションしたいのか、という目的・用途で変わってくるところです。
例えば、高速ライントレースのシミュレーションがしたいのであればモータの応答性や慣性も考慮した運動方程式を立てて模擬するようなモデル化が必要になります。

###EV3用の制御プログラム(C++)をSimulinkから呼び出す

Simulink上で、MATLAB Functionブロックで coder.ceval を使うと
C/C++の関数を呼び出せるようになります。　
参考：https://jp.mathworks.com/help/simulink/slref/coder.ceval.html

さすがにEV3用のプログラムをただそのまま呼び出しても使えないので、Simulinkの世界用に入出力インターフェイスを変換する必要があります。
といっても直接関係するのはカラーセンサとモータです。

実機の制御プログラム
　入力：カラーセンサ値(EV3のAPI)
　出力：左右モータ指令値(EV3のAPI)

を
　入力：スケール変換したRGB値
　出力：左右モータ指令値

となるようにしてやります。
つまり、ソースコード上、EV3のAPIを呼び出している箇所をSimulinkとやり取りする用の変数に置き換えます。

コードはそのままEV3でもSimulinkでも使いたいので
＃ifdef　を使って、切り替えました。
これをモータとセンサの各APIを使用している場所で行います。

一例でモータの回転数を取得する関数を示します。

/* 回転角度を得る */
#ifndef SIMULINK
S32 Motor::getAngle( void )
{
    return ev3_motor_get_counts(outputPort);
}
#endif
#ifdef SIMULINK
S32 Motor::getAngle( void )
{ 
    return simAngle;
}
#endif

SIMULINKがdefineされていなければAPIを、されていればSimulink用の変数simAngleを返しています。

上記でCの準備ができましたのでSimulinkからの呼び出しです。MATLABFunctionブロックを使います。
実装しているコードをそのままコピペしたので不要なdebug変数が含まれています。

function [pwmR, pwmL, debug0, debug1, debug2, debug3, debug4, debug5, debug6, debug7, y] = c_program( sensGray, sensRed, sensGreen, sensBlue, encR, encL)

pwmR = 0;　pwmL = 0;    
debug0 = 0;debug1 = 0;debug2 = 0;debug3 = 0;debug4 = 0;debug5 = 0;debug6 = 0;debug7 = 0;

y = coder.ceval( 'app',　sensGray, sensRed, sensGreen,sensBlue, encR,  encL, coder.ref(pwmR), coder.ref(pwmL ), coder.ref(debug0), coder.ref(debug1), coder.ref(debug2), coder.ref(debug3), coder.ref(debug4), coder.ref(debug5), coder.ref(debug6), coder.ref(debug7) );

coder.ceval　で 'app'　という関数を呼び出していて

入力が
　sensGray, sensRed, sensGreen, sensBlue, encR, encL

出力が
　pwmR, pwmL（とdebug変数）

となっていることがわかると思います。
使ってないですが(輝度モードを想定したsensGrayも入力しています)

概念の説明だったので省いていましたが、通常と同様、モータのエンコーダ値として回転数もEV3の制御ソフトに入力されます。

以上でモデル化の概要となります。

実際のSimulinkのモデルの図を示します(見せる用の編集を一切していないので大変汚いです)。

概念とは異なり、Cを呼び出す制御プログラム部が一番左になっているので見た目が少し違いますがループのつながりは同様になっています。

#Simulinkモデルを動かしてみる

動かしてみました。
ブロック並べでも経路探索の検証に便利でした。あと走行時間もある程度の精度で得られていたので目安になりました。

AIアンサーでは数字の上の走行経路と、認識アルゴリズムの検証に非常に有用でした。
実機で開発する場合、数字認識が失敗したとき、読み取り走行のずれが原因なのか、認識アルゴリズムに問題があるのか、切り分けになかなか骨が折れると思います。シミュレーションであれば、ずれのある場合・ない場合も試せるので開発がやりやすくなります。

ライントレースはコース形状の取得に使えます。外乱や応答遅れのない走行体を走らせ、X,Y座標と角度と距離をログすればいいです。
Lコースのスタートから駐車場までのシミュレーション結果です(数字解答はその場回転してるので軌跡に現れないですが･･･)。

難所のコースは自作していますが、シミュレーションベースで開発し動作確認したあと、1日でコースを作りその日のうちに実機の走行に成功しています。
家庭環境的にずーっとコース広げっぱなしだと心象があまりよろしくないので、やはりシミュレーションは有用と言えます。

#おわりに
ニッチな領域の話を長々と書きました。
実装に向けた具体的な話がほぼないですし、この駄文で最後まで読む方がどれだけいるのかも疑問ですが、少しでもどこかで何かのお役に立てば幸いです。

世の中ソフトもハードも複雑化の一途ですので、今後、というか既に、シミュレーション技術というのが非常に重要になってきていると思います。

シミュレーションで重要なのはやりたいことに対して「適切な」シミュレーション環境を構築することです。現実世界の事象をモデル化する場合に、目的に応じて抽象化の粒度を適切に決めるのが要点となります。
これはETロボコンで提出するソフトウェアのモデルでも同様といえると覆思います。