はじめに
本ドキュメントは、8/10に行われる一日体験入学に向けて、
標準機を用いてハーフコースを攻略するための戦略、プログラム設計、開発工程について記したドキュメントである。
基本方針
確実に動作しゴールすることを基本方針とする。
そのために各センサーの故障を考慮したプログラム設計を行う。
超音波センサは動作に不安があるので、ロータリーエンコーダによる走行制御の精度を上げて白線・タッチセンサーのみで攻略を行う。
基本戦略
予め公開されているコースの情報は積極的に使う。
したがってDBを確認する必要が無いのでWebカメラは使用しない。
機能定義
| 機能番号 | 機能名 | 機能概要 | 使用する機能 |
|---|---|---|---|
| (1) | 直進走行 | ロータリーエンコーダで進行方向をPID制御で補正する。 また目標距離に合わせて走行速度もPID制御で調整する。 | |
| (2) | 回転走行 | ロータリーエンコーダにより回転軸のズレをPID制御で補正する。 また目標角度に合わせて機体回転速度もPID制御で補正する。 | |
| (3) | 白線補正 | 白線センサーを使用して機体の向きを白線に揃える。 | (2)回転走行 |
| (4) | 通路走行 | 白線検知、正面のタッチセンサが反応、目標距離に到達のいずれかで走行終了。 左右のタッチセンサが反応した場合は一旦後退、回転して回避する。 白線検知した場合は白線補正してから終了する。 | (1)直進走行、(2)回転走行、(3)白線補正 |
プログラム概要
事前に公開されたコースとプログラムの大まかなフローチャートを以下に示す。
公開されたコースに沿って進むので、大きな条件分岐は存在しない。
ロータリーが回避できるため周回走行は使用しない。
|
|
機能詳細
通路走行
関数名 : run_straight_ts_ws
戻り値 : 無し
引数 : 目標距離[mm]
通路走行プログラムのフローチャートを以下に示す。
直進走行
関数名 : run_straight
戻り値 : 現在の走行距離[mm]
引数 : 目標距離[mm]
一回の呼び出しにつきモーターへの指令が一回行われるプログラム。
上限速度と目標距離を指定し、短い時間間隔で繰り返し呼ぶことでモーターの制御を行う。
目標距離と現在の走行距離の差を偏差として走行速度をPID制御し、
左右のモーターの回転回数の差を偏差として左右のモーターの速度比をPID制御する。
単純にI制御を行うと積分項が大きくなりすぎるので、
誤差が減少している間は積分項を0にリセットする。
計算の流れを以下に示す。- x_ref : 目標距離[mm]
- x_l : 左の車輪の走行距離[mm]
- x_r : 右の車輪の走行距離[mm]
- x = (x_l + x_r)/2 : 現在の走行距離[mm]
- e1 = x_ref - x : 目標距離と現在の走行距離の差[mm]
- e2 = x_l - x_r : 左右の走行距離の差[mm]
- e1_last, e1_sum : 前回呼び出し時の e1, 今までの e1 の総和
- e2_last, e2_sum : 前回呼び出し時の e2, 今までの e2 の総和
- v = Kp1 * e1 + Ki1 * e1_sum + Kd1 * e1_last : 目標速度[mm/s]
- dv = Kp2 * e2 + Ki2 * e2_sum + Kd2 * e2_last : 左右の速度の制御入力[mm/s]
- v_l = v + dv : 左のモーターの目標速度[mm/s]
- v_r = v - dv : 右のモーターの目標速度[mm/s]
各ゲインの大きさは
Kp1 = 0.050, Ki1 = 0.003, Kd1 = 0.000
Kp2 = 0.010, Ki2 = 0.000, Kd2 = 0.000
回転走行
関数名 : run_rotate
戻り値 : 現在の回転角度[deg]
引数 : 目標角度[deg]
直進走行同様、一回の呼び出しにつきモーターへの指令が一回行われるプログラム。
上限速度と目標距離を指定し、短い時間間隔で繰り返し呼ぶことでモーターの制御を行う。
目標角度と現在の回転角度の差を偏差として回転速度をPID制御し、
左右のモーターの回転回数の和を偏差として左右のモーターの速度比をPID制御する。
単純にI制御を行うと積分項が大きくなりすぎるので、
誤差が減少している間は積分項を0にリセットする。
計算の流れを以下に示す。- angle_ref : 目標角度[deg]
- x_l : 左の車輪の走行距離[mm]
- x_r : 右の車輪の走行距離[mm]
- angle = (x_r - x_l)/2.0 / 車輪の間隔 * 180/π : 現在の回転角度[deg]
- e1 = angle_ref - angle : 目標角度と現在の回転角度の差[deg]
- e2 = x_l + x_r : 左右の走行距離の差[deg]
- e1_last, e1_sum : 前回呼び出し時の e1, 今までの e1 の総和
- e2_last, e2_sum : 前回呼び出し時の e2, 今までの e2 の総和
- v = Kp1 * e1 + Ki1 * e1_sum + Kd1 * e1_last : 目標速度[deg/s]
- dv = Kp2 * e2 + Ki2 * e2_sum + Kd2 * e2_last : 左右の速度の制御入力[deg/s]
- v_l = -(v + dv) : 左のモーターの目標速度[mm/s]
- v_r = v - dv : 右のモーターの目標速度[mm/s]
各ゲインの大きさは
Kp1 = 0.0100, Ki1 = 0.0005, Kd1 = 0.0000
Kp2 = 0.0100, Ki2 = 0.0000, Kd2 = 0.0000
白線補正
関数名 : seach_line
戻り値 : 無し
引数 : 無し
白線補正プログラムのフローチャートを以下に示す。
このプログラムは、白線が機体の下にある状態で呼ばれることを想定している。
開発日程
| 日付 | ~8/7 | 8/8 | 8/9 | 8/10 朝 |
|---|---|---|---|---|
| 作業内容 | 設計・コーディング | 基本動作(直進走行、回転走行)の試験、デバッグ | PID制御のパラメータ調整 通路走行の試験 | 最終調整 |