消毒液を載せて走って回るロボット

新型コロナウイルスによりスーパーマーケットなどの商業施設では入口付近にアルコールが設置されている。大型商業施設となると、施設内で手指の消毒をしたいときには入口まで戻らないとならない。そこで、授業の一環としてアルコールを噴射する機構を搭載したロボットを自立移動させてみた。これにより、商業施設内でもアルコール消毒をすることができる。

自立移動機能
人検知・接近機能
消毒呼びかけ機能
消毒液噴射機能

今回は、副次的な機能である消毒呼びかけ以外の機能を紹介する。消毒呼びかけ機能については、ディスプレイでの表示と音声によって、近くの人に消毒を呼びかけるようにしている。

Raspberry Pi 4 Model B 4GB
Arduino Mega
朱雀技研エンコーダ内蔵ギアードDCモータ
Cytron 13A 5-25V シングルDCモータコントローラ
模型用7.2V ニッケル水素バッテリー
5V モバイルバッテリー
I2C 超音波センサ
アルミorアクリル製シャーシ
消毒用アルコールボトル

自立移動機能では、前方に4つ、左右に2つずつ超音波センサを設置して、前方約40cm、側方約20cmの障害物を検知する。前方に障害物がある場合には45度ないしは135度、側方に障害物がある場合には障害物を避ける方向にカーブして障害物を避ける。
避け方に関しては以下のソースコードに記載している。

typedef enum{
	FRONT_L,
	FRONT_R,
    FRONT_U,
    FRONT_D,
    LEFT_U,
    LEFT_D,
    RIGHT_U,
    RIGHT_D,
	USS_NUM
} uss_t;

typedef enum{
    STRAIGHT,
    BACK,
    TURN,
    CURVE,
    STOP,
    SPRAY,
    RUN_NUM
} run_t;

typedef enum{
	STP = 0,
	STR,
	ROT,
	CRV
} run_state_t;

class Move{
public:
    Move();

    /**
     * @brief 走行関数
     * @details 走行状態をもとにプライベートな走行関数を呼ぶ
     */
    void go();

    /**
     * @brief 走行再開関数
     */
    void resume(int count);


private:
    //! 超音波センサ
    UssMgr ussMgr;
    //! 起動用スイッチ
    Alternate sw_pw;
    //! タッチセンサ左
    Io ts_l;
    //! タッチセンサ右
    Io ts_r;
    //! オーディオ用スイッチ
	Io sw_ad;
    //! 走行状態
    run_t next_state;
    run_state_t run_state;
    // 走行用パラメータ
    int run_param;
    int uss_count;

    /**
     * @brief 直進状態の関数(通常状態)
     * @details 直進する関数をmove.cppに渡す
     */
    void straight();

    /**
     * @brief 後退状態の関数
     * @details 後退する関数をmove.cppに渡す
     */
    void back();

    /**
     * @brief 障害物回避の関数
     * @details 90度回転する関数をmove.cppに渡す
     */
    void turning(int deg);

    /**
     * @brief 曲がる関数
     * @details 曲がる関数をmove.cppに渡す
     */
    void curve(int adjust);

    /**
     * @brief 止まる関数
     */
    void stop();

    /**
     * @brief 噴射するために一時停止する関数
     */
    void spray();
};

void Move::go(){
	int speed, dist;
	request_get_runmode(&run_state, &speed, &dist);
	if(run_state == STP) {
		switch(next_state){
		case STRAIGHT:
			//next_state = STRAIGHT;
			break;
		case BACK:
			next_state = TURN;
			break;
		case TURN:
			next_state = STRAIGHT;
			break;
		case CURVE:
			break;
		case STOP:
			next_state = STRAIGHT;
			break;
		case SPRAY:
			break;
		default:
			break;
		}
	}

	uss_count++;
	if(uss_count % 3 == 0){
		if(next_state == CURVE) next_state = STRAIGHT;
		if(next_state == STRAIGHT){
			int nstate;
			ussMgr.searchUss(&run_state, &nstate, &run_param);
			next_state = (run_t)nstate;
		}
	}
	if(G_dist > 70 && next_state == TURN){
		next_state = STRAIGHT;
	}
	if(G_dist > 80){
		run_state = STP;
		next_state = SPRAY;
		run_param = 0;
	}

	if(ts_l.getSw() || ts_r.getSw()){
		// 衝突していたら強制的に後ろに下げる
		run_state = STP;
		next_state = BACK;
		if(ts_l.getSw()) run_param = -135;
		else run_param = 135;
	}

	run_state_t run;
	int sp, ds;
	request_get_runmode(&run, &sp, &ds);
	//printf("%d,  %d,  %d\n", run, sp, ds);

	G_audio = (int)sw_ad.getSw();
	if(!sw_pw.getSw()) {
		run_state = STP;
		next_state = STOP;
	}

	if (run_state == STP){
		switch(next_state){
		case STRAIGHT:
			straight();
			break;
		case BACK:
			back();
			break;
		case TURN:
			turning(run_param);
			break;
		case CURVE:
			curve(run_param);
			break;
		case STOP:
			stop();
			break;
		case SPRAY:
			spray();
			break;
		default:
			break;
		}
	}
}


void Move::resume(int count){
	if(!ussMgr.isFront() || count > 10000){
		next_state = STRAIGHT;
		G_state = 1;
	}
}


void Move::straight(){
	G_state = 1;
	request_set_runmode(STR, 20, 150);
}

void Move::back(){
	G_state = 2;
	request_set_runmode(STR, -10, -5);
}

void Move::turning(int deg){
	G_state = 3;
	request_set_runmode(ROT, deg > 0 ? 90 : -90, deg);
}

void Move::curve(int adjust){
	G_state = 4;
	request_set_runmode(CRV, 20, adjust);
}

void Move::stop(){
	G_state = 0;
	request_set_runmode(STP, 0, 0);
}

void Move::spray(){
	G_state = 5;
	request_set_runmode(STP, 0, 0);
}

人検知・接近機能ではMobileNet V2 SSDを用いて人を検知している。検知した人のうちの1番横幅が大きいバウンディングボックスのx軸の中心座標を利用し、これがディスプレイの中心座標の左にあるか右にあるかで機体のカーブの方向を決めている。実装例は以下の通り。

import cv2


##
# @brief 人検出クラス
# @details カメラからキャプチャをし、そこから人がいる領域にバウンディングボックスを描画する
class Target():
    ##
    # @brief コンストラクタ
    # @return None
    def __init__(self):
        pass

    ##
    # @brief ターゲット設定メソッド
    # @details 入力されたidに対応するラベルを返す
    # @param target_array 人のバウンディングボックスの中心点のx座標、横縦の大きさ
    # @return power 左右のタイヤのPWM調整値
    def setTarget(self, target_array):
        max_width = 0.0
        target_num = -1
        for i, target in enumerate(target_array):
            if target[1] > max_width:
                target_num = i
        if target_num >= 0:
            power = self.adjustMotor(target_array[target_num][0])
            dist = target_array[target_num][1]
            return power, dist * 100
        else:
            return 0, 0

    ##
    # @brief モータPWM値調整メソッド
    # @details ターゲットのバウンディングボックスの中央座標から左右のモータの調整値を導出する
    # @return power 左右のタイヤのPWM調整値
    def adjustMotor(self, posx):
        print(posx)
        rate = -20
        power = rate * (posx - 0.5)
        return power


##
# @brief 人検出クラス
# @details カメラからキャプチャをし、そこから人がいる領域にバウンディングボックスを描画する
class Detection():
    classNames = {0: 'background', 1: 'person', 2: 'bicycle',  ...省略}

    ##
    # @brief コンストラクタ
    # @details カメラをオープンする
    # @return None
    def __init__(self):
        self.model = cv2.dnn.readNetFromTensorflow('/.../ssd_mobilenet_v2.pb', '/.../ssd_mobilenet_v2.pbtxt')
        self.cap = cv2.VideoCapture(0)
        if not self.cap.isOpened():
            raise IOError('Cannot open camera')
        self.cap.set(cv2.CAP_PROP_FPS, 5)
        self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
        self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('H', '2', '6', '4'))
        _, image = self.cap.read()
        self.image_height, self.image_width, _ = image.shape
        self.target = Target()

    ##
    # @brief ラベル取得メソッド
    # @details 入力されたidに対応するラベルを返す
    # @param class_id ラベルID
    # @return value ラベル名
    def getName(self, class_id):
        for key, value in self.classNames.items():
            if class_id == key:
                return value

    ##
    # @brief 人検出メソッド
    # @details カメラからキャプチャし、人を検出する
    # @return None
    def detect(self):
        _, image= self.cap.read()
        self.model.setInput(cv2.dnn.blobFromImage(image, size=(300, 300), swapRB=True))
        output = self.model.forward()
        target_pos = []
        for detection in output[0, 0, :, :]:
            confidence = detection[2]
            if confidence > .5:
                class_id = detection[1]
                class_name = self.getName(class_id)
                #print(str(str(class_id) + " " + str(detection[2])  + " " + class_name))
                box_x = detection[3] * self.image_width
                box_y = detection[4] * self.image_height
                box_width = detection[5] * self.image_width
                box_height = detection[6] * self.image_height
                if class_name == 'person':
                    target_pos.append([(detection[3] + detection[5]) / 2, detection[5]])
                cv2.rectangle(image, (int(box_x), int(box_y)), (int(box_width), int(box_height)), (23, 230, 210), thickness=1)
                cv2.putText(image, class_name, (int(box_x), int(box_y+.05*self.image_height)), cv2.FONT_HERSHEY_SIMPLEX, (.005*self.image_width), (0, 0, 255))
        power, dist = self.target.setTarget(target_pos)
        print(int(power), int(dist))
        return int(power) + 128, int(dist)

    ##
    # @brief リリース関数
    # @details カメラをリリースする
    # @return None
    def release(self):
        self.cap.release()
        cv2.destroyAllWindows()