形式仕様記述言語VDMによるライントレーサ制御のモデリングの試み( 1/5): 基本概念整理
ライントレーサの構造
ここでは、ライントレーサの構造を、以下のように単純化して考えます。
wheelDistance
左右量車輪間の距離
単位はmm(ミリメートル)
sensorDistance
ライントレーサの中心点からセンサーまでの距離
単位はmm(ミリメートル)。
このモデルでは、ライントレーサの動きを、車体の速度と旋回角度の組み合わせで表現することにします。
車体速度と旋回半径を指定されると、左右車輪間の間隔から左右両車輪それぞれの速度を求めることができます。(
ここで「車輪の速度」とは、車輪の中心が走行コース上を水平に移動する速度のことを言います。この意味での車輪の
速度を実現するための駆動モータへの出力の計算については、今後の課題とします。)
右旋回を例に取ると、左右の車輪それぞれの旋回半径は以下のようになります。
左車輪の旋回半径 = radius + (wheelDistance / 2)
右車輪の旋回半径 = radius - (wheelDistance / 2)
左右それぞれの車輪の旋回角度と車体の旋回角度との比は、左右それぞれの車輪の速度と車体の速度との比と等
しくなります。
左車輪の速度 / 車体の速度 = 左車輪の旋回半径 / 車体の旋回半径
右車輪の速度 / 車体の速度 = 右車輪の旋回半径 / 車体の旋回半径
従って、左車輪の速度は以下のように計算できます。
ライントレーサの座標系
ライントレーサの位置・動きを表すための座標系は、以下のように決めます。進行方向などの角度は、単位をラジアンとし、時計
回り(右回り)を正とします。また、右水平方向を進行方向=0と定義します。
X座標
左から右に向かう方向を正とします。
Y座標
上から下に向かう方向を正とします。
左車輪の速度 = 車体速度 * (radius + (wheelDistance / 2)) / radius
右車輪の速度 = 車体速度 * (radius - (wheelDistance / 2)) / radius
左旋回の場合に旋回半径を負の値とすることにより、上記の右旋回と同じ計算式で、左旋回の場合の両車輪の速度
を求めることができます。
ある瞬間のライントレーサの進行方向は、両車輪を結ぶ車軸と垂直に前方に伸びる線の座標系内での向きを、水平
右方向を0とする角度(ラジアン単位)で表します。
ライントレーサの制御
ライントレーサの制御は、以下の処理を一定間隔で繰り返すことで実現します。
1.センサーで走行コースの色を読み取る。黒ならば車体はコース上、白ならばコース外と見なす。
2.車体がコース上であれば、車体を「コース外」に向けて旋回させる。車体がコース外であれば、車体をコ
ースに向けて旋回させる。
ここで、車体の旋回については、ライントレーサをコースの右側の縁に沿って走らせるか、左側の縁に沿っ
て走らせるかによって、旋回の向きが変わります。
制御パラメータとしては、以下のようになります。
コースの縁
ライントレーサの動き
ライントレーサの走行は、以下の3通りの場合があります。
直進
左右両車輪を同じ速度で回転させることにより、直進します。
右旋回
左車輪を右車輪よりも早い速度で回転させることにより、ライントレーサを右に旋回させることができます。
左旋回
右車輪を左車輪よりも早い速度で回転させることにより、ライントレーサを左に旋回させることができます。
旋回の緩急は、左右両車輪の速度差を調節することで制御できます。左右両車輪の速度差が大きい程、急な旋回となり、速度
差が小さいほど緩い旋回となります。
このモデルでは、旋回の緩急を表すパラメータとして旋回の半径(車体の中心点が描く円周の半径)を考えます。
このモデルでは、旋回半径が正の値を取るときには右旋回を、負の値を取るときには左旋回を表すものとします。
右側
左側
センサー値
旋回方向
コース上
右旋回
コース外
左旋回
コース上
左旋回
コース外
右旋回
intervalTime
処理の周期間隔時間
(単位は「秒」)
speed
車体中心の速度
(単位は「mm/秒」)
radius
旋回半径の大きさ
(単位はmm)
座標(posX,posY)で進行方向directionのライントレーサが、1周期の後に移動する先の座標(nextX,nextY)
は、以下のように計算されます。
旋回の中心点X座標 centerX = posX - radius * sin( direction )
旋回の中心点Y座標 centerY = posY + radius * cos( direction )
旋回の回転角度 angle = speed * intervalTime / radius
移動後の進行方向 nextDirection = (direction + angle) mod (pi * 2)
移動先のX座標 nextX = centerX + radius * sin( nextDirection )
移動先のY座標 nextY = centerY - radius * cos( nextDirection )
形式仕様記述言語VDMによるライントレーサ制御のモデリングの試み( 2/5): モデルの構造化
ライントレーサ本体のクラス階層と状態遷移
走行コースへのセグメント分割
外側コースの状態遷移図
内側コースの状態遷移図
ライントレーサ本体としては、走行コースのセグメントの変化を
追い、セグメントに適した制御方式を選択して制御を実行する
ことになります。
走行コースのセグメントの境界には灰色のマーカがついている
ので、それを目印に、セグメントの変化を追って行きます。
このセグメントをライントレーサの「状態」と見なして、ライントレ
ーサを状態遷移モデルで表現してみます。
この状態遷移には、外側コースと内側コースとで2通りあること
になります。
このようにコースによって異なる状態遷移を取るため、それぞ
れのコース毎にLineTracerモデルのサブクラスを定義し、走行
コースの状態遷移と各状態に対応する制御クラスの選択を、こ
れらのサブクラスでモデル化するようにします。
前回のモデルでは、ライントレーサの制御方式としてごく単純な1種
類の制御しかモデル化しませんでした。
実際の走行コースは、いくつかの「難関」を含むため、1種類の制御
で全コースを乗り切るのは困難です。
今回のコンテストの走行コースは以下のようなものです。
ここでは、走行コース全体をいくつかの「セグメント」に分割し、それぞれの
セグメントに適した制御方式に順次切り替えていく形を取ることにします。
上図の通り、外側のコースと内側のコースとでは、セグメントの構成が異な
ります。
外側コース(右サイド走行)
1
通常セグメント1
なだらかなカーブの1本道
2
点線
点線カーブ(近道)と実線カーブ(遠回り)の2通りの経路を持つ
3
通常セグメント2
なだらかなカーブの1本道
4
行き止まり
行き止まりの枝道(袋小路)を持つ
5
坂道
上りと下りの坂道
内側コース(左サイド走行)
1
通常セグメント1
やや急なカーブの1本道
2
ループ付き
実線カーブの両側に1個ずつ円形(ループ)の枝道を持つ
3
通常セグメント2
なだらかなカーブの1本道
4
坂道
上りと下りの坂道
走行制御部の分離と多様化
このように、セグメントによって制御を変えていくためには、まず、ライントレーサの本体から制御部分を分離して、取り替え可能にす
る必要があります。分離した制御部分は、ライントレーサ側から見ると同じインターフェースを持ち、その内部処理は、種類ごとに異
なる制御を行うことになります。
ここでは、VDM++ の「オブジェクト指向性」を生かし、以下のような継承関係をもつクラスとして、ライントレーサの制御をモデル化し
ます。
ライントレーサのベースクラス LineTracer では、以下の2つの操作を定義します。
これらの操作は、コースのセグメント毎の制御の内容に関係なく、同一の処理となる部分をモデル化します。
startRun
走行開始の処理
doOneCycle
周期処理1回の制御を実行する。
さらに、LineTracer クラスでは、そのサブクラスが定義するべき2つの操作について、そのインターフェースを定
義します。
changeState
ライントレーサの状態を、サブクラスが定義する状態遷移上の次の状態(セグメント)に進めます。
currentController
ライントレーサの現在の状態(セグメント)に対応する制御オブジェクトを返します。
LineTracer は、他に以下の2つのクラスを参照します。
ベースクラス LineTracerController は、制御クラスの共通インターフェースとして、以下の2つのメソッドを定義します。
他のクラスは、これらのメソッドの詳細を、自身の制御処理内容に合わせて定義します。
上記のセグメントと制御クラスの対応は右表のようになります。
セグメント種別
制御クラス
メソッド
内容
UsualController
通常セグメント
init
制御開始時の初期設定を行う
DottedLineController
点線セグメント
controll
周期処理1回の制御を実行する
DeadEndController
行き止まりセグメント
ループセグメント
LoopLineController
坂道セグメント
SlopeController
クラス名
内容
LineTracerCourse
ライントレーサの走行コースを表します。
インターフェースとしては、走行コースの特定の点の「色」を返す関数 check を持ちます。
MarkerFinder
走行コース上の灰色のマーカを検出する役割を持ちます。
走行コースから読み取られたコースの「色」の変化から、マーカの有無を検出します。
形式仕様記述言語VDMによるライントレーサ制御のモデリングの試み( 3/5): モデルの構造化(2)
ライントレーサの処理シーケンス
ここではライントレーサの処理内容と上記のクラス間の相互作用のイメージをつかむために、主要な処理をシーケンスで
表現してみます。
最初に、ひとつのセグメント内を走行する際の周期処理1回の処理内容についてみると、処理のステップとしては以下のよ
うになります。
走行開始時の初期処理は以下の2段階から成ります。
1.ライントレーサのオブジェクトを作成し、状態を初期化する。
2.走行を開始する。
初期化処理は、さらに以下のステップとなります。
1.LineTracer のオブジェクトを作成する。
2.LineTracer のインスタンス変数を初期化する。
3.MarkerFinder のインスタンスを作成する。
4.ライントレーサの状態遷移に応じて、必要となる制御オブジェクトを作成する。
5.ライントレーサの初期状態を設定する。
上記の1から3はベースクラス LineTracer の処理、4~5はサブクラスの処理となります。
走行開始は、以下の処理シーケンスとなります。
1.最初の状態に対応する制御オブジェクトを初期化する。
2.走行コース LineTracerCourse から、現在のライントレーサの位置の色を読み取る。
3.MarkerFinder に読み取った色を指定してマーカの検知を依頼する。
4.MarkerFinder は「マーカ検知せず」の回答を返す。
5.現在の状態(セグメント)に対応する制御オブジェクト(LineTracerController のサブクラス)に読み取った色を指定
して、制御情報の計算を依頼する。
6.制御オブジェクトは制御情報を計算して返す。
7.受け取った制御情報に基づいて、制御パラメータを出力する。
以上の処理シーケンスを図に表すと以下のようになります。
次に、マーカを検知した場合の処理シーケンスを見てみます。
形式仕様記述言語VDMによるライントレーサ制御のモデリングの試み( 4/5): VDMによるモデリング(1)
-- LineTracerモデルで使用するデータ型の定義
class LineTracerTypes
-- 通常セグメントの制御クラス
class UsualController is subclass of LineTracerController
-- 外側コース走行用のライントレーサモデル
class OutSideTracer is subclass of LineTracer
types
public Side
-- コースの右側・左側のどちらを走行するかの選択
= <rightSide>
-右側走行
| <leftSide>;
-左側走行
public Turn
-- 回転方向(直進、右回り、左回り)の選択
= <rightTurn>
-右回り
| <leftTurn>;
-左回り
public SensorInput
-- センサー入力値。白・灰色・黒の3階調を区別する。
= <white>
-白(コース内)
| <gray>
-灰色(マーカ)
| <black>;
-黒(コース外)
public Speed = real;
-- 走行スピード(実数)
public Length = real;
-- 距離(実数)
public Time
= real;
-- 時間(実数)
public Angle
= real
-- 方向(角度)(実数、ラジアン単位)
inv val == (0 <= val) and (val < (2 * MATH`pi));
public ControllInfo::
-- 制御情報レコード
turn
: Turn
-回転方向
speed
: Speed
-進行速度
radius : Length;
-旋回半径
operations
types
-- 走行コースの状態(セグメント)
State = <usual1> | <dottedLine> | <usual2> | <deadEnd> | <slope> | <usual3>
end LineTracerTypes
-- ライントレーサ制御のベースクラス
class LineTracerController
instance variables
-- ライントレーサの制御に関するパラメータ
protected side
: LineTracerTypes`Side;
protected intervalTime : LineTracerTypes`Time;
protected defaultSpeed : LineTracerTypes`Speed;
protected defaultRadius : LineTracerTypes`Length;
-- ライントレーサコントローラの名称(テスト用)
public name
: seq of char;
-----
コースの右側・左側の選択
センサー読み取りの時間間隔
デフォルトの進行速度
デフォルトの旋回半径
operations
-- 初期化
public
init :
LineTracerTypes`Side
*
LineTracerTypes`Time
*
LineTracerTypes`Speed *
LineTracerTypes`Length
==> ()
init( sd, it, ds, dr ) == (
side := sd;
intervalTime := it;
defaultSpeed := ds;
defaultRadius := dr
);
-----
コースの右側・左側のどちらを走行するかの選択
センサー読み取りの時間間隔
デフォルトの進行速度
デフォルトの旋回半径
-- センサー値をチェックして次の回転方向と両車輪の速度を決める
-- 実際の定義は各サブクラスの責任となる
public
controll :
LineTracerTypes`SensorInput
-- センサー入力値
==> LineTracerTypes`ControllInfo
-- 制御情報レコードが返る
controll( si ) == is subclass responsibility
end LineTracerController
-- コンストラクタ
public
UsualController : () ==> UsualController
UsualController() == (
name := "UsualController";
);
-- センサー値をチェックして次の回転方向と両車輪の速度を決める
public
controll :
LineTracerTypes`SensorInput
-- センサー入力値
==> LineTracerTypes`ControllInfo
-- 制御情報レコードが返る
controll( sensorInput ) ==
-- 走行サイドとセンサー入力に従って、旋回の方向を決め、
-- 制御情報を返す。
-- スピード、旋回半径はデフォルトのままとする
return
cases decideTurn( side, sensorInput ):
<rightTurn> ->
mk_LineTracerTypes`ControllInfo( <rightTurn>, defaultSpeed, defaultRadius ),
<leftTurn> ->
mk_LineTracerTypes`ControllInfo( <leftTurn>, defaultSpeed, - defaultRadius )
end
functions
-- センサーからの読み取り値とコースの走行サイドから回転方向を決定する
-- 黒・灰色は走行線上と見なし、白は走行線外と見なす
decideTurn :
LineTracerTypes`Side
*
-- 走行サイド
LineTracerTypes`SensorInput
-- センサー入力値
-> LineTracerTypes`Turn
-- 旋回方向が返る
decideTurn( side, sensorInput ) ==
cases side:
<rightSide> ->
cases sensorInput:
<black> -> <rightTurn>,
<gray> -> <rightTurn>,
<white> -> <leftTurn>
end,
<leftSide> ->
cases sensorInput:
<black> -> <leftTurn>,
<gray> -> <leftTurn>,
<white> -> <rightTurn>
end
end;
end UsualController
instance variables
state
usualController
dottedLineController
deadEndController
slopeController
:
:
:
:
:
State;
UsualController;
DottedLineController;
DeadEndController;
SlopeController
operations
-- コンストラクタ
public
OutSideTracer :
LineTracerTypes`Length *
-- 両車輪間の間隔
LineTracerTypes`Length *
-- 車体中心からセンサーまでの距離
LineTracerTypes`Side
*
-- コースの右側・左側のどちらを走行するかの選択
LineTracerTypes`Time
*
-- センサー読み取りの時間間隔
LineTracerTypes`Speed
*
-- 現在の進行速度
LineTracerTypes`Length *
-- カーブの回転半径
LineTracerCourse
*
-- 走行コースの指定
LineTracerTypes`Length *
-- 初期の位置のX座標
LineTracerTypes`Length *
-- 初期の位置のY座標
LineTracerTypes`Angle
-- 初期のの進行方向
==> OutSideTracer
OutSideTracer( wdist, sdist, sd, it, spd, rad, crs, initX, initY, initDir ) ==
(
-- ベースクラスのコンストラクタを呼ぶ
LineTracer( wdist, sdist, sd, it, spd, rad, crs, initX, initY, initDir );
-- 各状態に対応する制御オブジェクトを作成する
usualController
:= new UsualController();
dottedLineController
:= new DottedLineController();
deadEndController
:= new DeadEndController();
slopeController
:= new SlopeController();
-- 状態を初期化する
state := <usual1>;
);
-- 状態をひとつ次に進める
public
changeState : () ==> ()
changeState() ==
state := cases state :
<usual1>
->
<dottedLine>
->
<usual2>
->
<deadEnd>
->
<slope>
->
<usual3>
->
end;
<dottedLine>,
<usual2>,
<deadEnd>,
<slope>,
<usual3>,
<usual3>
-- 現在の状態に対応した制御オブジェクトを返す
public
currentController: () ==> LineTracerController
currentController() ==
return
cases state :
<usual1>
-> usualController,
<dottedLine>
-> dottedLineController,
<usual2>
-> usualController,
<deadEnd>
-> deadEndController,
<slope>
-> slopeController,
<usual3>
-> usualController
end
end OutSideTracer
形式仕様記述言語VDMによるライントレーサ制御のモデリングの試み( 5/5): VDMによるモデリング(2)
-- ライントレーサのモデル
class LineTracer
---------------------------------------------------- メンバー変数の定義
instance variables
-- ライントレーサの構造に関するパラメータ
public wheelDistance
: LineTracerTypes`Length;
public sensorDistance
: LineTracerTypes`Length;
-- ライントレーサの制御に関するパラメータ
public side
: LineTracerTypes`Side;
public intervalTime
: LineTracerTypes`Time;
public defaultSpeed
: LineTracerTypes`Speed;
public defaultRadius
: LineTracerTypes`Length;
-- 走行コース上のマーカ検出
public markerFinder
: MarkerFinder;
-- 走行コース
public course
: LineTracerCourse;
-- ライントレーサの走行状態に関する変数
public posX
: LineTracerTypes`Length;
public posY
: LineTracerTypes`Length;
public direction
: LineTracerTypes`Angle;
public turn
: LineTracerTypes`Turn;
public speed
: LineTracerTypes`Speed;
public radius
: LineTracerTypes`Length;
public beyondLimit
: bool;
-- 走行位置計算用の変数
public centerX
: LineTracerTypes`Length;
public centerY
: LineTracerTypes`Length;
public angle
: LineTracerTypes`Angle;
-- 駆動制御の出力値
public speedRight
: LineTracerTypes`Speed;
public speedLeft
: LineTracerTypes`Speed;
-- 両車輪間の間隔
-- 車体中心からセンサーまでの距離
-----
コースの右側・左側の選択
センサー読み取りの時間間隔
デフォルトの進行速度
デフォルトの旋回半径
-- 走行コース上のマーカ検出器
-- 走行コース
--------
位置のX座標
位置のY座標
進行方向
回転方向
進行速度
旋回半径
コース外に出たかどうかのフラグ
-- 現在の回転中心のY座標
-- 現在の回転中心のX座標
-- 現在の回転角度
-- 右車輪の速度
-- 左車輪の速度
---------------------------------------------------- 操作の定義
operations
-- コンストラクタ
public
LineTracer :
LineTracerTypes`Length *
-- 両車輪間の間隔
LineTracerTypes`Length *
-- 車体中心からセンサーまでの距離
LineTracerTypes`Side
*
-- コースの右側・左側のどちらを走行するかの選択
LineTracerTypes`Time
*
-- センサー読み取りの時間間隔
LineTracerTypes`Speed *
-- 現在の進行速度
LineTracerTypes`Length *
-- カーブの回転半径
LineTracerCourse
*
-- 走行コースの指定
LineTracerTypes`Length *
-- 初期の位置のX座標
LineTracerTypes`Length *
-- 初期の位置のY座標
LineTracerTypes`Angle
-- 初期のの進行方向
==> LineTracer
LineTracer( wdist, sdist, sd, it, spd, rad, crs, initX, initY, initDir ) ==
(
-- 引数の値をメンバー変数に設定する
wheelDistance
:= wdist;
sensorDistance := sdist;
side
:= sd;
intervalTime
:= it;
defaultSpeed
:= spd;
defaultRadius
:= rad;
course
:= crs;
posX
:= initX;
posY
:= initY;
direction
:= initDir;
speed
:= defaultSpeed;
beyondLimit
:= false;
-- 作業用の変数を初期化する
centerX
:= 0;
centerY
:= 0;
angle
:= 0;
-- 補助クラスのインスタンスを用意する
markerFinder
:= new MarkerFinder()
);
-- 処理開始
public
startRun : () ==> ()
startRun() ==
(
-- 初期状態に対応する制御オブジェクトを初期化する
currentController().init( side, intervalTime, defaultSpeed,
defaultRadius );
-- 走行コースの色から最初の旋回方向を決め、両車輪の速度を求める
setupControl()
);
-- 反復処理の1回の処理
public
doOneCycle : () ==> ()
doOneCycle() ==
if beyondLimit = false
then (
newPosition();
setupControl()
);
-- センサー値をチェックして次の回転方向と両車輪の速度を決める
private
setupControl : () ==> ()
setupControl() ==
let
-- センサーの位置座標を求める
sensorX = posX + sensorDistance * MATH`cos( direction ),
sensorY = posY + sensorDistance * MATH`sin( direction ),
-- 走行コースの色をチェックする
sensorInput = senseCourse( sensorX, sensorY, course )
in (
-- 灰色マーカを検知する。
if( markerFinder.check( sensorInput ) )
then(
-- マーカを検知した場合は、次の状態(セグメント)に進める
changeState();
-- 次の状態に対応する制御クラスを初期化する
currentController().init(
side, intervalTime, defaultSpeed, defaultRadius )
);
-- 制御クラスに対し、制御情報の計算を依頼する
let controllInfo = currentController().controll( sensorInput )
in (
-- 制御情報をインスタンス変数に記録する
turn := controllInfo.turn;
speed := controllInfo.speed;
radius := controllInfo.radius;
-- 制御情報から左右両車輪の速度を決定する
speedRight := speed * (radius-(wheelDistance/2)) / radius;
speedLeft := speed * (radius+(wheelDistance/2)) / radius
)
);
-- 周期1回分の移動先の座標を求める
private
newPosition : () ==> ()
newPosition() == (
let
-- 反復一回分の走行距離を計算
distance = speed * intervalTime
in
(
-- 車体の位置、回転角度、進行方向から回転の中心座標を求める
centerX
:= posX - radius * MATH`sin( direction );
centerY
:= posY + radius * MATH`cos( direction );
-- 走行距離と回転半径から、回転角度を求める
angle
:= distance / radius;
-- 進行方向を更新する
direction
:= nextDirection( direction, angle );
-- 移動先の座標を求める
posX
:= centerX + radius * MATH`sin( direction );
posY
:= centerY - radius * MATH`cos( direction );
-- コース外へ出たかどうかを判定する
beyondLimit := (posX < 0) or (course.width <= posX) or
(posY < 0) or (course.height <= posY)
)
);
-- 現在のコントローラを取得する
public
changeState : () ==> ()
changeState() == is subclass responsibility;
-- 現在のコントローラを取得する
public
currentController : () ==> LineTracerController
currentController() == is subclass responsibility
---------------------------------------------------- 関数の定義
functions
-- 進行方向を変更する(0以上、2π未満の範囲に制限する)
nextDirection
: LineTracerTypes`Angle * LineTracerTypes`Angle -> LineTracerTypes`Angle
nextDirection(
dir,
-- 元の進行方向
angle
-- 変化角度
) ==
if( (dir + angle) < 0 ) then
(dir + angle) + (MATH`pi * 2)
elseif( (dir + angle) >= MATH`pi * 2 ) then
(dir + angle) - (MATH`pi * 2)
else
(dir + angle);
-- コースから色を読み取る
senseCourse : LineTracerTypes`Length * LineTracerTypes`Length * LineTracerCourse
-> LineTracerTypes`SensorInput
senseCourse(
sensorX,
-- センサー位置のX座標
sensorY,
-- センサー位置のY座標
course
-- 走行コース
)
==
let senseVal = course.sense( sensorX, sensorY )
in
if senseVal < 0.33 then
<black>
else if senseVal < 0.66 then
<gray>
else
<white>;
end LineTracer
ダウンロード

形式仕様記述言語VDMによるライントレーサ制御のモデリングの試み