Webots講座４：自分の位置を知ろう (Python)

2021.06.27

Webots講座の４回目です．今回も前回に引き続き，Webotsの自動運転シミュレータを使います．BMW X5に搭載されているGPSを使い自分の位置（自己位置）を知る方法を学びます．

レファレンス

自己位置の取得

  • 自己位置の取得には内界センサや外界センサを使った方法があります．
    1. 内界センサ：ロボットの車速センサやステアリング角度センサなどの内部状態を知る内界センサを使い，その情報を使って自己位置を計算する方法をデッドレコニング(Dead Reckoning)とよびます．
    2. 外界センサ：GPS，LIDAR，カメラなどの外界センサの情報を使い自己位置を計算する方法．GPSセンサは自己位置を直接出力するので簡単に位置を取得できます．Webotsでは，ロボットにGPSノードを追加します．BMW X5にはGPSノードが組み込まれているので，APIを呼び出すだけ自己位置を取得できます．

GPSに関するAPI

  • enable(samplingPeriod): GPSの測定を可能にする．引数のsamplingPeriodでセンサのサンプリング時間[ms]を指定できる．
  • disable(): GPSの測定を不可能にする．
  • getSamplingPeriod(): サンプリング時間[ms]を取得する．
  • getValues(): GPSの測定結果(3次元ベクトル)を返す．値は座標系によりデカルト座標系の(x, y, z)[m]か世界測地系(WGS84)の(緯度,経度,高度)となる．
  • getSpeed(): 速度[m/s]を返す．
  • getCoordinateSystem(): 座標系を取得する．座標系はWorldInfoノードのgpsCoordinateSystemフィールドで定義されている．戻り値はWB_GPS_LOCAL_COORDINATEなら０，WB_GPS_WGS84_COORDINATEなら1である．

サンプルコード

''' robot_car2.py '''
import math                 # 数学モジュールのインポート
from vehicle import Driver  # vehcleモジュールからDriverクラスをインポート
from controller import GPS  # コントローラモジュールからGPSクラスをインポート

TIME_STEP = 60              # GPSのサンプリング時間 60[ms]  

driver = Driver()             # Driverクラスのインスタンスを生成
driver.setSteeringAngle(0.0)  # ステアリングの角度を0.0[rad]に設定
driver.setCruisingSpeed(20)   # 巡行速度を20[km/h]に設定

# GPS
gps = driver.getDevice("gps") # デバイス名から，そのクラスのインスタンスを生成
gps.enable(TIME_STEP)         # サンプリング間隔TIME_STEPでGPSからデータを取得可能

coordinate = gps.getCoordinateSystem()        # 座標系の取得
print("Coordinate system is ",coordinate)

while driver.step() != -1:
    angle = 0.03 * math.cos(driver.getTime()) # ステアリング角の計算
    driver.setSteeringAngle(angle)            # ステアリング角の設定
    
    # GPS
    values = gps.getValues()                  # GPSからデータを取得
    print("%g[s] position:(%g, %g, %g9[m]" % (driver.getTime(),values[0], values[1], values[2]))

    if driver.getTime() > 5.0:                # 5[s]経過したら
        driver.setCruisingSpeed(0.0)          # 停止
    上のサンプルコードは，ロボットに搭載されているGPSセンサを使いデータを取得するまでの一連の処理が書かれ，ロボットはスタートしてから5[s]でストップします．コードの各行にコメントを入れているので細かい説明は各行を読んでください．なお，このシミュレータでは座標系はデカルト座標系が設定されているのでGPSから取得した値の単位は[m]になります．普通，GPSセンサからデータを取得すると世界測地系で単位は[°]になり，[m]に変換する処理が必要になるケースが多いです．この例では，それが不要なので簡単ですね．

ハンズオン

  1. Webots講座３：自動運転シミュレータ (Python)で実施した要領で，新規プロジェクトrobot_car2を作成して，ワールドcity.wbtをワールドrobot_car2.wbtに上書き保存する．
  2. サンプルコードをコントローラプログラムとして，シミュレーションを実行して動作を確認しよう．
  3. サンプルコードを改良して，10[m]前進して停止させるプログラムを作ろう．
  4.  サンプルコードを改良して，Start地点から目標地点(進行方向に30[m]，左方向に5[m])に走行して停止するプログラムを作ろう．
    • ヒント：GPSセンサからStart地点の座標を取得して，目標地点のGPS座標系で位置を計算する．ロボットが目標地点に近づくようにステアリング角を制御する．制御方法は比例制御で良い．
  5. サンプルコードを改良して，下図の黄色コースを１周する(Start地点から走行し，反時計まわりに１周してStart地点で停止する）プログラムを作ろう．
    • ヒント：現時点ではGPSセンサしか使えないので，黄色コース上で通過する地点（ウエイポイント）を何点か決めて，その点を順番に経由することで１周すれば良い．これをウェイポイントナビゲーションとよぶ．途中で障害物があるので，それを避けるように経由点を設定する．

プチプロジェクト

  • レファレンスを読んで，この自動運転シミュレータで使われているロボットカーBMW X5のモデルを調べて，デッドレコニングを実施しましょう．
    • ヒント：ロボットカーはアッカーマン機構なので，その運動学を調べてデッドレコニングを実装する．

終わり

 

コメント

タイトルとURLをコピーしました