この記事の概要

Ubuntu + ROS + arduino + RealSenseで，特定の色の物体を追従するビジュアルサーボ制御プログラムを実装・公開（プログラムはGitHubで公開してます）
初めてUbuntuを使ってロボット関連のプログラムを書く初心者向けの教材の様なものになると嬉しい

この記事執筆のモチベーション

学部3年生の研修という名目の授業で，「何か制御を体験できるものを作成せよ」という指示を受けました．私は「Linux以外でのプログラミングの仕方に詳しくない」し，学生は「Linuxなんて知らないし」という状況なわけで，どうしたものか，と思っておりました．そこで，意を決して初心者向けに勉強できる何かを作成することとしました．そしてせっかくなので公開しておこうと思い記事を書きました．

研究をやる以上，どうしてもLinuxを使いたいと感じるシーンは多いと思います．しかし，機械系の学生はLinuxに触れる機会なんてほとんどなわけで，Linuxに慣れるだけでもかなりの時間を要してしまいます．そしてLinuxを使えるようになった後は，ROSとかの勉強も始まるわけで，勉強しているだけであっという間に時間が過ぎてしまいます．今回公開しているプログラムは，ROSを使って何かプログラムをする，という教材的なものとしても利用できれば良いなと思っています（GitHub）．

LinuxとROSのチュートリアル（超最低限）

大したものではありませんが，Linuxを使って最低限プログラムを書くために必要なチュートリアル的な説明書もいれています．またtest_pkgというパッケージも入れてあり，ここにはC++でHello worldを出力するプログラムや，ROSのpublishとsubscribeを理解するためのプログラムも入れています．

インストール

以下のコマンドを実行し，プログラムをダウロード，インストールしてください．なお，LinuxやROS（gitも）のインストールは事前にできているものとしますので，他の記事を参考にそれぞれインストールしてください．

$ git clone https://github.com/NaokiAkai/ros_intro.git
$ cd ros_intro
$ ./make.sh

このプログラムではOctoMapとrealsense-rosを用いています．もしコンパイルが通らない場合は，ココを参考にそれぞれのパッケージをインストールしてください．

概要

実装したシステムは以下の通りです．

センサとしてRealSenseを使用（realsense-rosを利用してセンサの値を読み取り）
RealSenseの出力する色付き点群から特定の色を抽出
抽出された色付き点群をクラスタリング
最も近いクラスタの重心に対してカメラが正対する様にサーボモータを制御
サーボモータをarduinoで制御するプログラムを実装（なんちゃってで速度制御できる様に工夫）

なお，システムの外観図は以下の様になっています．

arduinoにサーボモータ（ES08MA-2）が付いていて，そのサーボモータにRealSenseが搭載されています（サーボモータはもっと大きいものの方が安定して良いです）．今回は赤いペンを追従する研修を行いました．なお，サーボモータとRealSenseは養生テープで固定してあります...

RealSenseをPCに接続しても，そのままだと認識されません．まずココを参考に，/etc/udev/rules.d/99-realsense-libusb.rulesを作成し編集してください．そうすると，/dev/video#（#は0とか1の値）が認識される様になります．

arduinoをPCに接続すると，/dev/ttyACM#（#は0とか1の値）として認識されます．しかしこの状態だと，一般ユーザが触ることができない，つまり権限のない状態となっています．そこで以下のコマンドを実行し，権限を変更します（0の値は環境に合わせて要変更）．

$ sudo chmod 666 /dev/ttyACM0

今回実装したプログラムは，このlaunchスクリプトを実行すると一気に実行できます．こちらはros_introの下のlaunchというディレクトリに入っていますので，そちらに入っていただき，以下のコマンドを実行してください．

$ roslaunch colored_object_tracker.launch

またros_introの下に，rvizというディレクトリがあります．そちらに入っていただき以下のコマンドを実行すると，rvizでの可視化結果が確認できます．

$ rviz -d realsense_examples.rviz

以下では，それぞれの実装したプログラムの簡単な内容について簡単に解説していきます．

センサの値の読み取り

realsense-rosを用いてセンサの値を読み込みます．インストールすれば，以下のコマンドでセンサの値が読み込める様になります．

$ roslaunch realsense2_camera rs_rgbd.launch

色付き点群から特定の色を抽出

RGBの色情報を持つ点群をHSV表色系に変換し，色抽出を行います．launchスクリプト内の以下の値を変更すると，抽出する色を変更することができます（デフォルトだと赤色を抽出します）．

    <arg name="target_hue_angle" default="0.0" />
    <arg name="delta_hue_threshold" default="30.0" />
    <arg name="saturation_threshold" default="0.3" />
    <arg name="value_threshold" default="0.3" />

target_hue_angleとdelta_hue_thresholdの単位はでdegreeで，saturation_thresholdとvalue_thresholdは0から1の値です．値の詳細に関しては，HSV表色系を調べてみてください．

色付き点群をクラスタリング

抽出された色付き点群をクラスタリングします（クラスタリングにはPCLのライブラリを用いています）．launchスクリプト内の以下の変数を変更すると，クラスタリングの結果が変わります．

    <arg name="clustering_vgf_reso_x" default="0.01" />
    <arg name="clustering_vgf_reso_y" default="0.01" />
    <arg name="clustering_vgf_reso_z" default="0.01" />
    <arg name="min_cluster_size" default="10" />
    <arg name="cluster_tolerance" default="0.03" />

clustering_vgf_reso_#（#はx，y，zのどれか）の単位はmです．点群を間引く空間の解像度になります（値が大きいほど点群を間引く量が多くなります）．点群を間引かないと，計算時間が大きくなりすぎます．min_cluster_sizeは，クラスタとして認識する最小の点の数になります（1にすると，すべての点がクラスタになるので大変です）．cluster_toleranceの単位もmです．点が離れているか，くっついているか，を認識する距離のパラメータです．

クラスタの重心に対してカメラが正対する様にサーボ制御

最もセンサから近いクラスタの重心を選択し，そのクラスタの重心と，センサが成す角が0となる様に，つまり対象とするクラスタ重心に対してカメラが正対する様に，PID制御を用いてサーボモータを制御します．なお，位置制御だと制御している感覚が味わえないので，（なんちゃって）速度制御ができる様にしています．制御に関するパラメータは，launchスクリプト内の以下を編集することで変更することができます．

    <arg name="servo_controller_p_gain" default="1.0" />
    <arg name="servo_controller_i_gain" default="0.0" />
    <arg name="servo_controller_d_gain" default="0.01" />
    <arg name="servo_controller_max_ang_vel" default="40.0" />

servo_controller_#_gain（#はp，i，dのどれか）は，PID制御のゲインになります．ゲインを変化させることで，制御の様子が変わることが確認できます．servo_controller_max_ang_velの単位はdegree/secondになります．この値が大きくなるほど，サーボモータが早く動く様になります．

一応今回のプログラム作成の目的は，制御の実習のためですのでで，今回のPID制御を数式で書くと，

\omega(t) = K_{p} e(t) + K_{d} \frac{de(t)}{dt} + K_{i} \int e(t)dt

という感じて角速度$\omega(t)$を求めていることになります．なお$e(t)$が，クラスタ重心とセンサが成す角度（偏差）になります．これを0にすると，センサがクラスタ重心に対して正対することになります．

arduinoによるサーボモータの制御（なんちゃって速度制御）

通常arduinoでサーボモータを制御する場合，角度制御，つまり角度をサーボモータの角度を指定することしかできません．しかし角度制御だけだと，今回の例ではPID制御のありがたみが感じられません．そこで以下のプログラムを実装し，なんちゃってで速度制御をできる様にします．単にPCから角速度を受け取り，その速度に基づいて，サーボモータの角度を積算していくものとしているだけです．

void loop() {
  static float deg = initialDeg;
  static int angVel = 0;

  if (Serial.available() > 0) {
    delay(10);
    angVel = serialRead();
    if (angVel < -180) {
      angVel = -180;
    } else if (angVel > 180) {
      angVel = 180;
    }
  }

  deg += (float)angVel * deltaTime;
  if (deg < minDeg) {
    deg = minDeg;
  } else if (deg > maxDeg) {
    deg = maxDeg;
  }
  servo.write((int)deg);

  delay(delayTime);
}

まとめ

Ubuntu + ROS + arduino + RealSenseを使って，PID制御を用いたビジュアルサーボを学べるプログラムを作成しました．また，公開しているパッケージには，非常に簡単ですが，Linuxの使い方とROSを使ってプログラムするサンプルが入っています．日本語でのコメントも沢山いれています．Linuxを使ってROSでプログラムするということを初めてやる方にとって，少しでも役に立つと幸いです．

Ubuntu + ROS + arduino + RealSenseでビジュアルサーボ制御（初心者向け）