Making RTC for EV3 (Python)

移動ロボットの組み立て

EV3 の標準セットを購入すると、移動ロボット (Educator Vehicle) の作成方法のマニュアルが1冊ついてくるはずです。 なければ、以下の URL からダウンロードすることができます。この移動ロボットを例にとり、移動ロボットRTC を作成してみます。

まずは、このマニュアルに従って、移動ロボット (Educator Vehicle) を組み立てます。

それぞれのモーター、センサーはそれぞれ以下のように取り付けます。

モーター右 ポート C
モーター左 ポート B
モーター(M) ポートA
タッチセンサー右 ポート 3
タッチセンサー左 ポート 1
超音波センサー ポート 4
ジャイロセンサー ポート 2

移動ロボットの運動学

さて、上記のコンポーネントは指令値として2次元速度指令を受け取りますが、実際にモータを制御する際には、モーターの角速度指令値を計算しモーターに命令しなければなりません。

移動ロボットの運動学については、東北学院大学の熊谷先生のページが参考になります。

座標系としては、自律移動機能共通インターフェース仕様書に従って、

ロボット進行方向をX軸とした右手系を想定する。速度指令は座標系にならって、(v_x、v_y、v_a)とする。独立二輪駆動式の移動ロボットなので、v_y は常に0となり、実質的に v_x および v_a を指定することになる。

さて、車輪の角速度を math16.png , math17.png として、車輪の半径を r とすると、各車輪の接地点での速度 v_r、v_l はそれぞれ


math3.png
math4.png

となる。また、回転中心からロボット中心の距離を ρ とすると、以下の式が成り立つ。


math5.png

一方、中心から車輪までの距離(トレッドの1/2)を d とすると、


math0.png
math1.png

となる。以上の式から、速度指令(v_x、v_y、v_a)の時に実際に与えるべき左右のモーターの角速度は以下の通りになります。


math6.png
math7.png

Educator Vehicle の車輪の直径 (2r)、トレッド (2d) はそれぞれ

車輪の直径 2r 56mm (0.056m)
車輪の直径 r 28mm (0.028m)
トレッド幅 2d 118.5mm (0.1185m)
トレッド幅 d 59.25mm (0.05925m)
車輪の幅 28.5mm (0.0285m)

なので、

math8.png
math9.png

となる。

自己位置推定(オドメトリ)

次に、ロボットの自己位置同定の方法について考えます。 ロボットの移動速度・角速度を積分することで任意の時点の位置・姿勢を得ることができます。速度 v_x、角速度 v_a とすると、(x、y、θ) の微小変位と v_x、v_a との関係を直線近似すると、


math10.png
math11.png
math12.png

となる。(より厳密には円弧近似する方法もあるが、ここでは簡単に直線近似とした。) これを実際のロボット制御時に、計算機上で求める場合、サンプリング周期 Δt [s] として、

math13.png
math14.png
math15.png

のように求められる。

RTC のひな形作成

作成する RTC の設計

作成する RTC を設計します。

作成する RTC の仕様をいかに示します。RTCBuilder で必要事項を入力し、ひな形コードを生成します。

基本タブ
プロファイル名 名称・指定
モジュール名 EducatorVehicle
バージョン 任意
ベンダ名 任意
カテゴリ Mobilerobot
アクティビティ
有効アクション onInitialize、onActivated、onDeactivated、onExecute
データポート (InPort)
ポート名 意味
velocity2D RTC::TimedVelocity2D 速度指令 (v_x、v_y、v_θ) [m/s、m/s、rad/s]
angle RTC::TimedDouble
データポート (OutPort)
ポート名 意味
odometry RTC::TimedPose2D 現在の位置・姿勢(角度) (x、y、θ) [m、m、rad]
ultrasonic RTC::RangeData 超音波センサーをレンジセンサーと仮定し、要素1の距離データを格納
gyro RTC::TimedDouble ジャイロセンサーを TimedDouble [rad] にて出力
color RTC::TimedString カラーセンサーの値を色名 (none、black、white、blue、green、red、yellow、brown) で出力
touch RTC::TimedBooleanSeq タッチセンサーの値をBoolean[2] で出力
コンフィギュレーション
ポート名 意味
wheelRadius double タイヤの半径 [m]
wheelDistance double タイヤ間距離の1/2 [m]

Download

latest Releases : 2.0.0-RELESE

2.0.0-RELESE Download page

Number of Projects

Choreonoid

Motion editor/Dynamics simulator

OpenHRP3

Dynamics simulator

OpenRTP

Integrated Development Platform

AIST RTC collection

RT-Components collection by AIST

TORK

Tokyo Opensource Robotics Association

DAQ-Middleware

Middleware for DAQ (Data Aquisition) by KEK