Donkey CarでのセンサーによるHybrid走行

はじめに

海外では、人の操作による走行とAI推論による走行の時間が肉薄しており、RCカーのハードウェア性能に依存するところが高くなってきており、タイムトライアルでの競技はそろそろ終わるかなと感じてきています。最近ではコースにペースカーを走らせておいて、その車を避けながら走行するなど、徐々に来年あたりから、AI推論だけの走行からセンサーを補助的に使ったハイブリッド方式での走行が流行するのでという考えで検討しはじめた内容です。

現行のDonkey Car(3.1.1)では、走行による推論により前輪のアングルと後輪のスロットルを制御しており、各種センサーからの情報を基に、より走行精度をあげるために以下の検討をしました。

• Raspberry PiのGPIOにセンサーをつける方法
  imu.py(MPU6050)と同じアプローチだが、センサーが多くなったり、センサー取り出し時間が長くなることで、全体のサイクル(DRIVE_LOOP_HZ = 20)に影響しそうで、サイクルは今後推論モデルの複雑化による時間のためにキープしておいた方がいいかと思いました。

• Arduinoでセンサー制御
  Arduinoをセンサー全体のコントロールをするコプロセッサとして活用を検討しました。センサー接続事例も多く、アナログ・デジタルセンサーもつなぎやすい。Raspberry Piとの接続は、I2Cをセンサーのマスターとして使うので、USBシリアル通信で接続します。

構成図

・ArduinoとセンサーをI2CまたはSPIで接続し、状態を１バイトに変換して、Serial.writeで定期的にUSB経由でRaspberry Piに送る。
・Raspberry PiでのSerial_Arduino.pyはThreadとして起動され、常にUSBシリアルから最新の状態を読み出し、保持しておく(self.status)。
・Donkeyのメインループで推論の後で、最新状態を使って、新しいスロットル値を求める。

事前準備

pip install pyserial

参考プログラム

Arduino(Donkey_Sensor.ino)

このサンプルは、VL53L1Xを一つI2C接続して測定最大距離をShort(1.3mくらい)にして、距離に応じて速度カテゴリを１バイトの文字にして、それをRaspberry Pi側に送信している。デバッグ用に距離を短く設定してある。

/*
 *   Sensor controller for Donkey car
 *                            2019/12/02
 *   Ver. 0.1  single VL53L1
 *   
 *   command: 'F': Full throttle
 *            'M': Medium
 *            'S': Slow
 *            'E': Emergency Stop
 */
#include <Wire.h>
#include <VL53L1X.h>
#define Threshold_M  450  // mm
#define Threshold_S  250  // mm
#define Threshold_E   50  // mm

VL53L1X sensor;

void setup()
{
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000); // use 400 kHz I2C

  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1);
  }
  // long ... 50ms, medium ... 33ms, short ... 20ms
  sensor.setDistanceMode(VL53L1X::Short);
  sensor.setMeasurementTimingBudget(20000);
  // Start continuous readings at a rate of one measurement every 50 ms (the
  // inter-measurement period). This period should be at least as long as the
  // timing budget.
  sensor.startContinuous(50);
}

void loop()
{
  int range;
  sensor.read();

  if(sensor.ranging_data.range_status == 0) {
    range = sensor.ranging_data.range_mm;

    if(range < Threshold_E) {
      Serial.write("E");
    } else {
      if(range < Threshold_S) {
        Serial.write("S");
      } else {
        if(range < Threshold_M) {
          Serial.write("M");
        } else {
          Serial.write("F");
        }
      }
    }      
  } else {
    Serial.write("F");  // out of range
  }
  delay(50);
}

Raspberry Pi(myconfig.py)

下記のフラグを追加する。

# Sensor from Arduino
SERIAL_ARDUINO = True

Raspberry Pi(parts/serial_arduino.py)

devは/dev/ttyACM0または/dev/ttyUSB0となると思われるので、USBを刺した時に追加されるデバイスを確認しておくといい。baudrateはArduinoのSerial.beginと同じ値にしておく。

import serial
import time

class Serial_sense():
    '''
    get sensor information from Arduino via serial USB interface
    '''

    def __init__(self, dev='/dev/ttyUSB0', baudrate=115200, poll_delay=0.03):
        self.status = b'F'
        self.dev = dev
        self.baudrate = baudrate
        self.serial_port = serial.Serial(self.dev, self.baudrate)
        time.sleep(1)
        self.poll_delay = poll_delay
        self.on = True

    def update(self):
        while self.on:
            self.poll()
            time.sleep(self.poll_delay)
                
    def poll(self):
        try:
            self.status = self.serial_port.read()
        except:
            print('failed to read serial USB interface!!')
            
    def run_threaded(self):
        return self.status

    def run(self):
        self.poll()
        return self.status

    def shutdown(self):
        self.serial_port.close()
        self.on = False

Raspberry Pi(parts/sensor_controller.py)

サンプルではスロットルのパワーを30%ずつ下げるようなコーディングにしているが、この辺は実際の走行により値を決めていくのがいいでしょう。
最小トルクよりも小さくなると停止してしまうので、個体に合わせて最小トルクに戻すといいかも。

class SensorController():
    '''
    This part is to control throttle from ToF sensor on Arduino.
    command: 'F': Full throttle   100%
             'M': Medium           70%
             'S': Slow             40%
             'E': Emergency Stop    0%
    '''
    def __init__(self):
        self.power_dict = {b'\x00':0.0, b'F':1.0, b'M':0.7, b'S':0.4, b'E':0.0}

    def run(self, mode, ai_throttle, status):
        new_throttle = ai_throttle
        # temporary changing for debugging
        if mode == "local":
            power_rate = self.power_dict[status]
            new_throttle = new_throttle * power_rate

        return new_throttle

Raspberry Pi(manage.py)

IMUのThread追加の後に、５行分追加。

    #IMU
    if cfg.HAVE_IMU:
        from donkeycar.parts.imu import Mpu6050
        imu = Mpu6050()
        V.add(imu, outputs=['imu/acl_x', 'imu/acl_y', 'imu/acl_z',
            'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=True)

    #Serial USB interface
    if cfg.SERIAL_ARDUINO:
        from donkeycar.parts.serial_arduino import Serial_sense
        serial_sensor = Serial_sense(dev='/dev/ttyUSB0', baudrate=115200)       
        V.add(serial_sensor, outputs=['serial_sensor/status'], threaded=True)

Drive train setup部の前に、７行追加。

    #Throttle controller from sensors on Arduino   
    if cfg.SERIAL_ARDUINO:
        from donkeycar.parts.sensor_controller import SensorController
        sensor_controller = SensorController()       
        V.add(sensor_controller, 
            inputs=['user/mode', 'throttle', 'serial_sensor/status'],
            outputs=['throttle'])       
    
    #Drive train setup
    if cfg.DONKEY_GYM:
        pass

最後に

これで、センサーから取り入れた情報を加工して、Raspberry Piで制御するまでの基本的な一連の流れが動くと思います。みんなで色々と知恵を出し合って、面白いコントロールができるといいと思っています。