ライントレーサの製作
H21.11.3
ライントレーサの製作U kunioホームページに戻る
いつか作ろうと思っていたライントレーサをPID制御の勉強を兼ねて作りました。
ちょっと車体が傾いている
制御の概要
@8個の反射型フォトセンサーでライン(黒線)を8ビット値として読取る。センサーの間隔(7.5mm)とセンサー幅から、
ライン幅は2つのセンサーより少し広くして12.5mmとし、センサーが直線であれば1つまたは2つが検出するようにした。
ライン部は0なので、次のようなデータとなり、それを15段階(−7〜7の整数)の数値に変換し偏差とする。
右端 01111111 → -7 00111111 → -6 10111111 → -5 10011111 → -4
以下同様
中央 11100111 → 0 左端 11111110 → 7 (注:センサーの配置が左右逆のため、ビットの並びと反対)
また、カーブでセンサーがラインを三つにまたがった場合は、検知した中央のセンサのみが検知した場合と同じとした。
例 00011111→10111111 → -5
A偏差をもとにPID処理(もどき?)し制御量を決め、その制御量をもとに左右モータのPMW制御パルス幅を変え、進行方向を
変える。
BPID制御の細かい説明は省略するが、次式で制御量が定義される。(ここに分かりやすい説明があります。)
制御量=K1・e1+K2・覇+K3・ed
第一項は比例制御 K1:比例ゲイン、e1:偏差
第二項は積分制御 K2:積分ゲイン
第三項は微分制御 K3:微分ゲイン ed:偏差の変化量 e1-e0 (
e0:前回サンプリングの偏差)
今回は、偏差が1単位であり、残留偏差による制御は不要と思われるため比例制御と微分制御のみ行う。(PD制御)
制御量=K1・e1+K3・(e1-e0)
PWM制御は、タイマカウンタ0の8ビット高速PMW制御を使い、OC0A(PD5)とOC0B(PD6)出力をそのままモータ駆動に用いる。
8ビット高速PMW制御では、パルス幅をOC0A/OC0Bの設定値(0〜255)で可変できるので、OC0A/OC0Bの値は128
基準(偏差0)として、偏差相当分だけ片側を加算、逆側を減算することによって進行方向を制御する。
具体的には、OC0Aが右側モータ、OC0Bが左側モータを駆動し、偏差による制御量の計算結果がプラス(ラインが左)になれば、
OC0Aは加算、OC0Bが減算され、PWMパルス幅は右側モータが大きくなり左側が小さくなり左に旋回する。(右の場合は逆)
偏差に対する制御量は、8ビット符号付計算のため、128に対して±127が最大となり、ゲインが大きくとれない。
127÷7≒18 比例と微分が同符号の場合は18で制限されても問題ないが、逆符号のことも考えるとゲインを大きくしたい
ので、偏差による制御量計算は16ビットで行い、結果を絶対値127以下に制限し、偏差の制御量とする。
回路図・主要部品
CPU ATmega88
8MHz動作、1つのタイマカウンタで8bitPWM出力が2つとれるもの
反射型フォトセンサー rohm RPR-220 x 8
マブチモータ FA-130
x2
モータ駆動用Tr 2SC3964
x2 Icが大きく(2A)、hfeも大きいものを選定し、1段でモータ駆動できるものとした
ダイオード 1S2076A x2 フライホイール効果(PWM制御によるモータ電流を円滑化)
電源 モータ駆動用(単三x2)と制御用(単四x2)を分け、制御系へのノイズや電圧変動を防止し、電源容量も確保
メカのパーツ
駆動系(ギヤ等)は、タミヤの「楽しい工作シリーズ」のパーツを使った。
ギヤ
ダブルギアボックス 左右独立4速タイプ、ギヤ比は1/114.7とした。
タイヤ トラックタイヤセット
シャーシ ユニバーサルプレートセット
前輪 キャスター(ホームセンターで入手)
ソフトウェア処理 ソース(16_8)
制御の概要を元にして作成。
処理の概要
@初期化処理で、I/Oポートの設定とタイマカウンタ0を設定
タイマカウンタ0:8Bit高速PWMモード、前置分周1でPWM周波数31250Hz(8MHz/256)
OC0A/OC0B非反転動作
Aメインルーチンは空の無限ループ
Bタイマカウンタ0のオーバーフロー割込による制御量設定ルーチン(PWM周期に同期、32us周期)
省エネのためフォトセンサーのLED発光は、必要時のみとした。よって、まずフォトセンサーのLEDをオンし、
検出できるまで待ち時間。
当初、フォトセンサーのデータシートで検知時間(ある条件下で)10usだったので余裕をみて、倍の20us程度で行けると踏んで
いたが、予想以上に時間がかかり正確にI/Oで読み込むためには、約50us必要なことが判明。PWM周期時間32usを大きく
上回ってしまうが、PWM周期は微調整できないので、PWMと非同期で2〜2.5周期の時間となるが良しとした。
(仕上がりでLED発光比率は全時間の70−80%となり、2-3割の省エネ効果)
50us後にセンサーを読み取って、フォトセンサーのLED発光停止。
センサーの検知パターンから偏差を設定する。
(有効なセンサーのデータでない場合はスキップ、スキップが一定時間続いたら、コースアウトと判断し停止)
偏差量から、PD制御処理による制御量を計算し、モータ駆動量(OC0A/OC0B値)を設定する
(OC0A/OC0Bの値は、次の周期のスタート時に反映される)
割り込み処理を終了し、メインルーチンに戻る。
(すでに、オーバーフロー割込条件が出来ているので、直ぐに制御量設定ルーチンへ行く)
製作過程
◇センサー感度確認
ライン読取感度とI/Oポート検出レベルを確認するため、センサ入力を仮接続したLEDで表示した。
適当に設定した、発光ダイオード電流および出力抵抗値で特に問題はなさそうだ。
写真は入力11110011 (LEDが逆向きなっている)
◇PWM信号・フォトセンサーLED点灯波形(比例ゲイン8倍にて)
左上から、右側偏差、偏差ほぼ0、左側偏差
グリーンが左車輪のPWM(Hでon)、イエローがLED(Lでon)
LEDの点灯周期は、PWM周期の2〜2.5倍(65−74us)となっている。
センサーの検出パターンから、偏差に変換する処理ルーチン(順次比較)
によって、処理時間(LEDoff時間)に差が出ている。
◇比例・微分ゲインの調整
コースを作り走らせ試行錯誤で調整した。比例ゲインは16倍にすればほぼ思ったような動きになった。それにプラスして、微分ゲインは大きな効果はわかり
ずらいが、長時間走らせた安定性から8倍ぐらいが適当と思われる。
まがりなりにもPD制御(多分)が、実現できたと思っています。
ちょっと追加
今回のものには電源SWがなく、制御系の電池をSW代わりにしているので、スタートはともかくストップはコースアウトしない
限り動き続ける。文化祭などの展示では、走りっぱなしだと電池が直ぐに消耗してしまうし、見た目もずっと走っていても飽きて
しまう。
そこで、ある指令(キック)で動き出し一定時間走らせ停止(スリープモードで待機)させることにした。
キックは直接車体に触らない方が面白いので、フォトセンサーを使い遮光することで、レベル変化を起こしそれをI/Oピンの
変化として割り込みを掛け、スリープモードから復帰させ、再び走らせるようにした。
暗い部屋では照射が必要となるが、適当な明るさがあればセンサー部分に手をかざすと動くようになります。
回路図(PC5にセンサー入力を接続した)とソース(約20秒走行して停止)
中央部の黒い円柱の筒のなかにフォトセンサー
(ON2160のフォトTrのみを使用)がある。
ライントレーサの製作Uへ