初版

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

# 概要 新型コロナウイルスによりスーパーマーケットなどの商業施設では入口付近にアルコールが設置されている。大型商業施設となると、施設内で手指の消毒をしたいときには入口まで戻らないとならない。そこで、授業の一環としてアルコールを噴射する機構を搭載したロボットを自立移動させてみた。これにより、商業施設内でもアルコール消毒をすることができる。 # 機能 - 自立移動機能 - 人検知・接近機能 - 消毒呼びかけ機能 - 消毒液噴射機能 今回は、副次的な機能である消毒呼びかけ以外の機能を紹介する。消毒呼びかけ機能については、ディスプレイでの表示と音声によって、近くの人に消毒を呼びかけるようにしている。 # 主な材料 - 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度、側方に障害物がある場合には障害物を避ける方向にカーブして障害物を避ける。 避け方に関しては以下のソースコードに記載している。 ```cpp 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軸の中心座標を利用し、これがディスプレイの中心座標の左にあるか右にあるかで機体のカーブの方向を決めている。実装例は以下の通り。 ```py 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() ``` # 消毒液噴射機能 名前の通り消毒液を噴射する機能。以下のような機構を作り、ボトル上部の支柱を回転させることによって、取り付けられた勾玉のような形の部品がボトルの上部を押してアルコール噴射を実現している。また、手の検知はアルコールボトル前方のボックスに囲われた焦電センサによって行われている。  # 制作物 以下のようなロボットができた。  https://www.youtube.com/watch?v=oEbD1FwUOm8&feature=youtu.be # 感想 アルコールの残量検知の機能を足して、なくなったら帰還するような機能も増やしたかった。