Webots講座の７回目です．引き続きWebotsの自動運転シミュレータを使っていきます．今回はLIDAR(Laser Detection and Ranging)センサの使い方とそれを使った簡単な障害物回避を実現します．
レファレンス
- Lidar (Cyberbotics Ltd.)
- Webots for Automobiles (Cyberbotics Ltd.)
- 参考にしたコード
- Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.c
LIDARの使い方
- LIDARセンサの使い方は次のとおりです．
- デバイスの取得
- lidar = driver.getDevice(“LIDARデバイス名”)でLIDARデバイスを取得する．この例ではLIDARデバイス名は”Sick LMS 291″．
- 有効化
- lidar.enable(TIME_STEP)メソッドでLIDARを有効化する．TIME_STEPはサンプリングの間隔[ms]．
- 各パラメータの取得
- lidar.getHorizontalResolution(), lidar.getMaxRange(), lidar.getFov()などでLIDARのデータ処理に必要なパラメータを取得する．
- データの取得
- lidar_data = lidar.getRangeImage()でLIDARの距離データを取得する.
- データ処理
- 障害物までの方位や距離など必要なデータ処理を行う．
- デバイスの取得
API
- class Lidar (Device):
- def enable(self, samplingPeriod):有効化．samplingPeriodはサンプリングの間隔[ms]．
- def disable(self):無効化
- def getFov(self):水平方向の視野角(Fov) [rad]を取得
- def getHorizontalResolution(self):水平方向解像度の取得
- def getMaxRange(self):最大検出距離の取得[m]
- def getMinRange(self):最小検出距離の取得[m]
- def getRangeImage(self):距離データ[m]の取得．距離データはOpenGLレンダリングのデプスバッファから計算される．1次元の単精度浮動小数点のリストを返す．走査線のデータは左から右へと順番に並んでいる．
- def getSamplingPeriod(self):サンプリング時間の取得[ms]
サンプルコード
- このサンプルコードは次のコードをPythonとクラス化し入門講座に合うように改変しています．元のコードとは，障害物検出が異なっており，このコードでは，長さが進行方向20[m]，幅が車幅+衝突余裕の矩形領域内を障害物検出範囲としています．
- Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.c
- まず，34行目でBMW X5に搭載されているLIDARである”Sick LMS 291″のデバイスを取得し，35行目で有効化し，36行目で水平方向の解像度（走査線数），37行目で検出最大距離[m]，38行目でFOV[rad]を取得しています．
- 160行目のlidar.getRangeImage()メソッドでLIDARのデータを取得し，171行目のcalcObstacleAngleDist(lidar_data)メソッドでデータを処理しています．
- calcObstacleAngleDist(lidar_data)メソッドは117行～141行で定義されています．125行～131行で障害物とその距離を計測しています．125行目のself.lidar_witdh/2はロボット正面のLIDAR走査線の番号なので，ロボットの左から正面までのOBSTACLE_HALF_ANGLE[°]と正面から右OBSTACLE_HALF_ANGLE[°]まで障害物を探しています．この場合は左右20[°]です．障害物がある場合は，ロボットからみたその方位obstacle_angle[°]と距離obstacle_dist[m]を返し，障害物がないときはそれぞれself.UNKNOWNを返しています．
"""robot_car_lidar.py
Webots/projects/vehicle/controllers/autonomous_vehile/autnomous_vehicle.cを参考にしています．
"""
import cv2
import math
import numpy as np
from vehicle import Driver
from controller import GPS, Node
""" ロボットカークラス """
class RobotCar():
SPEED = 20 # 巡行速度
UNKNOWN = 99999.99 # 黄色ラインを検出できない時の値
FILTER_SIZE = 3 # 黄色ライン用のフィルタ
TIME_STEP = 30 # センサのサンプリング時間[ms]
CAR_WIDTH = 2.015 # 車幅[m]
CAR_LENGTH = 5.0 # 車長[m]
""" コンストラクタ """
def __init__(self):
# Welcome message
self.welcomeMessage()
# Driver
self.driver = Driver()
self.driver.setSteeringAngle(0)
self.driver.setCruisingSpeed(self.SPEED)
self.driver.setDippedBeams(True) # ヘッドライト転倒
# LIDAR (SICK LMS 291)
self.lidar = self.driver.getDevice("Sick LMS 291")
self.lidar.enable(self.TIME_STEP)
self.lidar_width = self.lidar.getHorizontalResolution()
self.lidar_range = self.lidar.getMaxRange()
self.lidar_fov = self.lidar.getFov();
# GPS
self.gps = self.driver.getDevice("gps")
self.gps.enable(self.TIME_STEP)
# Camera
self.camera = self.driver.getDevice("camera")
self.camera.enable(self.TIME_STEP)
self.camera_width = self.camera.getWidth()
self.camera_height = self.camera.getHeight()
self.camera_fov = self.camera.getFov()
print("camera: width=%d height=%d fov=%g" % \
(self.camera_width, self.camera_height, self.camera_fov))
# Display
self.display = self.driver.getDevice("display")
self.display.attachCamera(self.camera) # show camera image
self.display.setColor(0xFF0000)
""" ウエルカムメッセージ """
def welcomeMessage(self):
print("*********************************************")
print("* Welcome to a simple robot car program *")
print("*********************************************")
""" 移動平均フィルタを実装しよう．"""
def maFilter(self, new_value):
"""
ここに移動平均フィルタのコードを書く
"""
return new_value
""" ステアリングの制御： wheel_angleが正だと右折，負だと左折 """
def control(self, steering_angle):
LIMIT_ANGLE = 0.5 # ステアリング角の限界 [rad]
if steering_angle > LIMIT_ANGLE:
steering_angle = LIMIT_ANGLE
elif steering_angle < -LIMIT_ANGLE:
steering_angle = -LIMIT_ANGLE
self.driver.setSteeringAngle(steering_angle)
""" 画素と黄色の差の平均を計算 """
def colorDiff(self, pixel, yellow):
d, diff = 0, 0
for i in range (0,3):
d = abs(pixel[i] - yellow[i])
diff += d
return diff/3
""" 黄色ラインを追従するための操舵角の計算 """
def calcSteeringAngle(self,image):
YELLOW = [95, 187, 203] # 黄色の値 (BGR フォーマット)
sumx = 0 # 黄色ピクセルのX座標の和
pixel_count = 0 # 黄色の総ピクセル数
for y in range(int(1.0*self.camera_height/3.0),self.camera_height):
for x in range(0,self.camera_width):
if self.colorDiff(image[y,x], YELLOW) < 30: # 黄色の範囲
sumx += x # 黄色ピクセルのx座標の和
pixel_count += 1 # 黄色ピクセル数の和
# 黄色ピクセルを検出できない時は no pixels was detected...
if pixel_count == 0:
return self.UNKNOWN
else:
y_ave = float(sumx) /(pixel_count * self.camera_width)
steer_angle = (y_ave - 0.5) * self.camera_fov
# print("dev=%g y_ave=%g sumx=%d pixel_count=%d" % (dev, y_ave, sumx, pixel_count))
return steer_angle
""" 障害物の方位と距離を返す. 障害物を発見できないときはUNKNOWNを返す．
ロボットカー正面の矩形領域に障害物がある検出する
"""
def calcObstacleAngleDist(self, lidar_data):
OBSTACLE_HALF_ANGLE = 20.0 # 障害物検出の角度．中央の左右20[°]．
OBSTACLE_DIST_MAX = 20.0 # 障害物検出の最大距離[m]
OBSTACLE_MARGIN = 0.1 # 障害物回避時の横方向の余裕[m]
sumx = 0
collision_count = 0
obstacle_dist = 0.0
for i in range(int(self.lidar_width/2 - OBSTACLE_HALF_ANGLE), \
int(self.lidar_width/2 + OBSTACLE_HALF_ANGLE)):
dist = lidar_data[i]
if dist < OBSTACLE_DIST_MAX:
sumx += i
collision_count += 1
obstacle_dist += dist
if collision_count == 0 or obstacle_dist > collision_count * OBSTACLE_DIST_MAX: # 障害物を検出できない場合
return self.UNKNOWN, self.UNKNOWN
else:
obstacle_angle = (float(sumx) /(collision_count * self.lidar_width) - 0.5) * self.lidar_fov
obstacle_dist = obstacle_dist/collision_count
if abs(obstacle_dist * math.sin(obstacle_angle)) < 0.5 * self.CAR_WIDTH + OBSTACLE_MARGIN:
return obstacle_angle, obstacle_dist # 衝突するとき
else:
return self.UNKNOWN, self.UNKNOWN
""" 実行 """
def run(self):
stop = False
step = 0
while self.driver.step() != -1:
step +=1
BASIC_TIME_STEP = 10 # シミュレーションの更新は10[ms]
if stop == True:
print("Stop!")
#self.driver.setBrakeIntensity(1.0) # ブレーキをかける．
self.driver.setCruisingSpeed(0) # 非常停止
# センサの更新をTIME_STEP[ms]にする．この例では60[ms]
if step % int(self.TIME_STEP / BASIC_TIME_STEP) == 0:
# Lidar
lidar_data = self.lidar.getRangeImage()
# GPS
values = self.gps.getValues()
# Camera
camera_image = self.camera.getImage()
# OpenCVのPythonは画像をnumpy配列で扱うので変換する．
cv_image = np.frombuffer(camera_image, np.uint8).reshape((self.camera_height, self.camera_width, 4))
# 障害物回避
obstacle_angle, obstacle_dist = self.calcObstacleAngleDist(lidar_data)
STOP_DIST = 5 # 停止距離[m]
if obstacle_dist < STOP_DIST:
print("%d:Find obstacles(angle=%g, dist=%g)" % \
(step, obstacle_angle, obstacle_dist))
stop = True
else:
# 操作量（ステアリング角度）の計算
steering_angle = self.maFilter(self.calcSteeringAngle(cv_image))
if steering_angle != self.UNKNOWN:
print("%d:Find the yellow line" % step)
self.driver.setCruisingSpeed(self.SPEED) # 走行速度の設定
self.control(steering_angle) # ステアリングの制御(比例制御の簡易版)
stop = False
else:
print("%d:Lost the yellow line" % step)
self.driver.setCruisingSpeed(0) # 追従する黄色線をロストしたので停止する．
# self.driver.setBrakeIntensity(0.5) # 追従する黄色線をロストしたので停止する．
stop = True
""" メイン関数 """
def main():
robot_car = RobotCar()
robot_car.run()
""" このスクリプトをモジュールとして使うための表記 """
if __name__ == '__main__':
main()
ハンズオン
- Webots講座３：自動運転シミュレータ (Python)を実施した要領で，新規プロジェクトrobot_car，コントローラrobot_car_lidar.pyを作成し，サンプルコードを実行しましょう．
- import cv2でエラーが出る場合はopencvがインストールされていません．次のコマンドでインストールしてください．
- pip install opencv-python
- import cv2でエラーが出る場合はopencvがインストールされていません．次のコマンドでインストールしてください．
- サンプルコードを実行すると初めに遭遇するドラム缶２個は通り過ぎますが，３番目のドラム缶は正面にあるので急停止します．それを避けて，元の黄色ラインを追従するプログラムを何も調べずに自分で良く考えて実装しよう．
- 障害物回避にポテンシャル法を実装してみよう．
終わり
コメント