M5Stackでできること：移動ロボットに超音波センサをつけてみる

動かして学ぶ簡単ROS入門

前回、Plusという移動走行ロボットをROSを使って動かす方法を記事で紹介しました。Plusに使用されているマイコンはM5Stackというマイコンで、非常にカスタマイズ性に優れています。

今回はPlusに搭載されているM5Stackとセンサを組み合わせてカスタマイズを行うことで、センサを使った移動走行ロボットの制御を行っていきます。センサを組み込むことで周囲の環境に応じて自律的な判断を行う移動ロボットを作ることが可能になります。

1. M5Stackとは？

今回使用する移動走行ロボットPlusは制御マイコンにM5Stackという以下のようなマイコンが使用されています。

M5Stackは液晶やWifi、Bluetooth、スピーカ、バッテリなどの周辺機器が一体となったマイコンです。
IoT機器などの開発において、ハードウェアとして必要となるものがパッケージング化されており、ソフトウェア開発環境も提供されているので簡単に開発を始めることができます。また後述するM5Stack互換のセンサを取り付けて、簡単にセンサを使ったアプリケーションの構築が可能となっています。
移動走行ロボットPlusではモータの制御、外部PCとのWifi通信、ボタン操作がM5Stackによって行われています。

2. 自律移動走行ロボットのために必要なセンサ

自律移動走行ロボットを作る上で、外界を認識するセンサは非常に重要です。よくセンサとして使用されるものとしてカメラがありますが、移動を行う上では、距離情報も極めて重要であり、距離を測るためのLiDARなども使用されることが多いです。

他にも自律移動走行で使用されるセンサを以下にまとめました。

  • カメラ：人や物体を検出し、避けたり動作を停止したりするのに使用します。
  • 距離センサ：人や物体の距離を検出し、避けたり動作を停止したりするのに使用します。
  • 加速度センサ・ジャイロセンサ：車体の傾きや重力方向を認識することで、車体の正しい位置や姿勢を知るために使用します。
  • 熱感知センサ：夜間などのカメラで検出が難しい状況で、人や物体の検出を行います。

照度センサ：昼間と夜間、屋内と屋外などを認識することで、使用するセンサの切り替えなどに使用します。

3. M5Stack互換センサ

M5StackにはI2C通信を使ってセンサとやりとりするためのポートが用意されており、そこにM5Stack互換のセンサを接続することで、様々なセンサを使用することが可能になっています。

以下に実際に販売されているセンサをいくつか紹介します。

  • 超音波測距ユニット：超音波による距離の計測を行うセンサユニットです。
  • ToF測距ユニット：ToF(Time of Flight)による距離の計測を行うセンサユニットです。
  • 反射型赤外線センサユニット：赤外線による障害物検出を行うセンサユニットです。
  • TMOS PIRユニット：赤外線による人感センサユニットです。

4. 今回のロボットのシステム構成

今回は上記センサの中で超音波測距ユニットを用いて、障害物を回避しながら移動するロボットを作っていきたいと思います。

今回のような障害物検知では、超音波測距ユニット以外の距離センサでも検出は可能です。超音波を用いたセンサは、ToFに比べると距離精度は落ちますが、環境や対象物の影響を受けにくく、安定して距離が取得できるという特徴があります。

超音波測距ユニットは以下のようなセンサです。

今回のシステムの全体構成は以下のようになります。

M5Stackに超音波測距ユニットを接続し、センサ情報を外部PCにWifiで送信します。送信されたセンサ情報を用いて、障害物を避けるような軌道修正を外部PC内で計算し、それをM5Stackに送信することで、障害物を避けながらの移動が可能になります。
ソフトとして必要となるのはM5Stack上でセンサ情報を受け取り、送信したり、モータを動かすプログラムと、外部PC上でセンサ情報に基づいて、軌道を修正し、それをM5Stackに指示するプログラムになります。
M5StackのプログラムはArduinoIDEという開発環境が用意されており、その中でC言語で開発を行っていきます。一方、外部PCで動かすプログラムはROS2を使ってC++で開発します。前回はROS1で開発を行いましたが、ROS2のアップデートが進んでおり、今回はROS2を使っていこうと思います。
以降で説明するプログラムはすべて以下のGithubのリポジトリに含まれています。ここでは一部を抜粋しながら説明していきますので、コード全体を確認したい場合はこちらのリンクからコードを確認してください。
https://github.com/nanoxeed/plus_ros2_driver

5. M5Stackの設定

まずはM5Stackにモータの制御とセンサを読み込むためのプログラムを書き込んでいきます。
モータ制御の部分はPlusを提供しているLifeTechRoboticsの元のコードを流用して使用します。
https://raw.githubusercontent.com/takemoto-ltr/plus_joypad/refs/heads/main/m5base/src/main.cpp
これに超音波測距ユニット用のコードを追加していきます。
まずセンサユニットを扱うためのモジュールをインクルードします。

#include <M5Stack.h>    // CORE2の場合は <M5Core2.h>
#include <Unit_Sonic.h>  // <-- このモジュールを追加
#include <WiFi.h>
#include <Preferences.h>
#include <AsyncUDP.h>
#include "nvs_flash.h"

次にセンサからのデータ取得とそれをUDPで送信するためのオブジェクトの初期化を行います。

AsyncUDP gUdp;                              	// UDP通信用
SONIC_I2C sensor;                           	// ソニックセンサー用

メインの処理となるloop関数の中で、取得したセンサデータをUDPで送信します。

void loop() {
  …
  float dist = sensor.getDistance();  // センサデータ取得

  // 途中の処理
  …

  // UDPでのセンサの値の送信
  uint8_t sensor_buf[16];
  sprintf((char *)sensor_buf, "%f\n", dist);
  size_t sensor_buf_len = strlen((char *)sensor_buf);
  gUdp.broadcastTo(sensor_buf, sensor_buf_len, UDP_PORT);
  …
  // 終了処理
}

コードができたら、Arduino IDEを使ってコンパイルとM5Stackへのプログラムへのロードを行います。
リンクからArduino IDEのダウンロードとインストールを予め行っておいてください。
インストールできたら、M5StackとPCをUSB接続し、Arduino IDEを立ち上げて、上で作成したプログラムファイルを読み込みます。

最初にM5Stack用の設定を行います。
サイドバーにある以下のマークのBoard Managerをクリックして開きます。

検索に「m5stack」と入力して、出てきたモジュールをインストールします。

次に今回のプログラムに必要なライブラリをインストールします。
サイドバーにある以下のマークのLibrary Managerをクリックします。

検索から「M5Stack」を検索し、M5Stackをインストールします。

もう一つ、「unit_sonic」と検索し超音波測距ユニットのためのライブラリをインストールします。

インストールが完了したら、プログラムをコンパイルし、ロードを行います。
USBのポートを確認し接続した状態で、メニューバーにあるUploadボタンを押します。

Uploadがうまくいけば、IDEのOutputウィンドウで以下のように表示されます。
後はM5Stackを立ち上げて、前回も行ったWifiまわりの設定を行い、IPアドレスをメモしておきます。
これでM5Stack側の設定は完了です。

6. 外部PCの設定

次にM5StackとWifi経由で接続する外部PC側の設定を行っていきます。
ROS2自体のインストールやセットアップはROS2のサイトから進めてください。ROS2のセットアップ後、今回使用するプログラムをgitを用いて取得します。

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/nanoxeed/plus_ros2_driver.git

次にビルドを行います。

cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash  # jazzyの部分はROSのバージョンに合わせてください
colcon build

エラーなくビルドが終了すれば外部PC側のセットアップ完了です。
ビルドが完了すると、plus_ros2_driverパッケージの中でM5Stackと通信を行うplus_m5baseというプログラムを使用できるようになります。このプログラムは2つのトピックをやり取りしており、一つは移動の指令となるJoyコマンドをサブスクライブし、M5Stackにモータコマンドとして送信します。もう一つは超音波測距ユニットの距離の値をM5Stackから取得し、それをパブリッシュすることで他のROSノードからもセンサ値を取得できるようになっています。

7. 走行実験

ここまでのセットアップしたプログラムを用いて走行実験を始めていきます。
M5Stackに超音波測距ユニットを取り付け、以下のように車体の前に設置されるようにセンサを固定します。

まずは簡単な障害物を検知して、停止するプログラムを作って動かしてみます。
plus_m5baseからセンサ値を取得し、センサとの距離がある一定以下になった場合に停止コマンドを送信します。
実際のプログラムは以下のようになります。

#!/usr/bin/python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from std_msgs.msg import Float32


class SensorTest(Node):
   def __init__(self):
       super().__init__("sensor_test")
       self.dist_threshold = 100
       self.pub = self.create_publisher(Joy, "joy", 10)
       self.sub = self.create_subscription(Float32, "dist", self.dist_callback, 10)
       msg = Joy()
       msg.axes = [0, 0.2, 0, 0, 0, 0.2]
       self.pub.publish(msg)

   def dist_callback(self, msg):
       cmd = Joy()
       if msg.data < self.dist_threshold:
           self.get_logger().info("Detected obstacle and stop!")
           cmd.axes = [0, 0, 0, 0, 0, 0]
           self.pub.publish(cmd)
       else:
           cmd.axes = [0, 0.2, 0, 0, 0, 0.2]
           self.pub.publish(cmd)


def main():
   rclpy.init()
   node = SensorTest()
   rclpy.spin(node)
   rclpy.shutdown()


if __name__ == "__main__":
   main()

こちらのプログラムを実行するために、以下を立ち上げておきます。

ros2 launch plus_ros2_driver ui_control.launch ip:=<M5StackのIPアドレス>

次に別のターミナルで上記のスクリプトを実行します。

ros2 run plus_ros2_driver sensor_test.py

スクリプトが実行されるとロボットが前に動き出します。
ここで、ロボットの眼の前に障害物が現れるとそこで、動きが止まります。
障害物が無くなると再度前進し始めます。

次に障害物で止まるのではなく、障害物を避けながらさらに前に進むプログラムを作成してみます。
スクリプトは以下のようになります。

#!/usr/bin/python3
import time

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from std_msgs.msg import Float32


class ObstacleAvoidance(Node):
   def __init__(self):
       super().__init__("obstacle_avoidance")
       self.dist_threshold = 130
       self.pub = self.create_publisher(Joy, "joy", 10)
       self.sub = self.create_subscription(Float32, "dist", self.dist_callback, 10)
       msg = Joy()
       msg.axes = [0, 0.2, 0, 0, 0, 0.2]
       self.pub.publish(msg)

   def avoid_obstacle(self):
       cmd = Joy()
       cmd.axes = [0, 0, 0, 0, 0, 0]
       self.pub.publish(cmd)

       value = 0.2
       # Left turn
       cmd = Joy()
       cmd.axes = [0, -value, 0, 0, 0, value]
       self.pub.publish(cmd)
       time.sleep(3.5)

       # Forward
       cmd = Joy()
       cmd.axes = [0, 0.2, 0, 0, 0, 0.2]
       self.pub.publish(cmd)
       time.sleep(5.0)

       # Right turn
       cmd = Joy()
       cmd.axes = [0, value, 0, 0, 0, -value]
       self.pub.publish(cmd)
       time.sleep(3.5)

       # Forward
       cmd = Joy()
       cmd.axes = [0, 0.2, 0, 0, 0, 0.2]
       self.pub.publish(cmd)

   def dist_callback(self, msg):
       cmd = Joy()
       if msg.data < self.dist_threshold:
           self.get_logger().info("Detected obstacle and stop!")
           self.avoid_obstacle()
       else:
           cmd.axes = [0, 0.2, 0, 0, 0, 0.2]
           self.pub.publish(cmd)


def main():
   rclpy.init()
   node = ObstacleAvoidance()
   rclpy.spin(node)
   rclpy.shutdown()


if __name__ == "__main__":
   main()

先程の停止のみのスクリプトと異なる点は、障害物を検出した際に、旋回と前進を組み合わせることで障害物を回避する動作をとっているところです。
これにより障害物を回避しながら前進し続けることができます。

8. まとめ

今回は移動走行ロボットPlusに搭載されているM5Stackに超音波測距ユニットを接続することで、移動中の前方の障害物との距離情報を取得し、それを避けながら移動するロボットの構築を行いました。
M5Stackは他にも赤外線やToFなどのセンサも取り付けることができ、工夫次第で移動ロボットに様々な状況を認識させることができます。さらにLiDARやカメラなどのよりリッチなセンサを使用すればSLAMやナビゲーションなどの高度な制御も可能になります。
ぜひ、M5Stackとセンサを組み合わせていろいろな自律移動ロボットの制作にチャレンジしてみてください。

関連記事

特集記事

動かして学ぶ簡単ROS入門

M5Stackでできること：移動ロボットに超音波センサをつけてみる
技術コラム

360度深度カメラのPALを使ってROSで自己位置推定してみる
動かして学ぶ簡単ROS入門

ROSとは？ロボット開発のためのオープンソースフレームワーク　動かして学ぶ簡単ROS入門①
動かして学ぶ簡単ROS入門

ROSの基本的な概念: ノード、トピック、サービス 動かして学ぶ簡単ROS入門②　
動かして学ぶ簡単ROS入門

ROSとWebカメラの接続と活用法：動かして学ぶ簡単ROS入門③
動かして学ぶ簡単ROS入門

ROSとRealsense/画像処理と3次元データ処理：動かして学ぶ簡単ROS入門④

コメント

この記事へのコメントはありません。

TOP