カテゴリー: ロボット製作

ネタ見せで一番失敗しているのは、テレパシーの問題。テレパシーは、ロボットどうしのコミュニケーションのプロトコルである。ロボットどうしのテレパシーが機能しないとネタが成立しない。最近は、テレパシーをチェックするダイアローグをいれて、サリーに「テレパシーチェック」と語りかける。これで、サリーがHALにテレパシーを送り、レスポンスが返ってきたら、サリーが「大丈夫です」とこたえるというシステムにしていた。

しかし、ロボットに話しかけるというのがまわりに迷惑をかける。そこで、スクリプトは無関係にスマホのコントローラー場で確認できるようにした。

常に心配なことの一番は、電源問題である。ロボットHALには12V2000mAhのニッケル水素バッテリーが4つ並列に乗っている。大きすぎるかもしれないとは思うが、モーターは結構電気を食うので、しかたがない。ただ、充電をどうするかが悩ましい。ニッケル水素電池は、リチウムイオン電池より安全だとはいわれている。継ぎ足し充電ができないとも言われる。過充電は避けなければならない。

充電用には、24Vのアダプタ経由で15Vに落として、電池に持っていっている。この電圧を13Vより下げたら充電効果がなくなった。逆流防止のためにダイオード（60V）をはさんだ。過充電を避けるために、スイッチ経由で電池の両端の電圧を測れるようにした。スイッチを入れると表示する。使っているニッケル水素バッテリーは、充電すると電圧が13V付近で安定するので、充電完了のタイミングをとらえるのが難しい。

なんどもやっているうちに、電池の特性がわかるのではないかと思っている。

ロボット用のcomputerであるraspberrypi3にインストールしたopencvには、人の顔や瞳を自動で認識する識別器がすでに付属している。それは大変便利で、raspberrypiに接続したカメラで、すぐに識別が可能になる。

もし、サリーの顔が識別できれば、ロボットはサリーに向けて自動で近づいたり離れたりできるようになる。これはとても便利だ。しかし、opencv付属の人の顔の識別器では、サリーの顔を識別できないことがわかった。だから、サリーの顔を識別する識別器を自作した。

参考にさせていただいたサイトは、こちらである。

今後、サリー以外の物体認識にも使いたいのでメモしておく。

（１）positive画像とnegative画像：検索画像のダウンロード
フリーのツールはたくさんあるので、なんでもいいと思うが、私は、こちらのを使わせていただいた。どのサイトか、使えないサイトもあったが、特に大きな問題はなかった。このダウンロードソフトを使って、ワードをrobot naoで検索して得られた画像、positive画像約650枚ほどを使った。ただし、gifは使えない、pngは使えるのかもしれないが、削除したので100枚ほど少なくなった。jpgは、jpegとしてもJPGと拡張子が違っていてもOKのようだ。
naoの顔が入っていないnegative画像も、このソフトで、確か「人形 部屋」とかいう検索をかけてダウンロードして、500ほどで打ち切った。

（２）顔画像の位置の指定
positive画像のそれぞれについて、naoの顔がある位置を指定するのがとても面倒だった。600枚ほどの位置指定に3,4時間かかったような気がする。TrainingAssistantというフリーのソフトを使わせていただいた。超便利だった。指示通りにアプリを組み込む。そして、先ほどダウンロードした画像を、上記サイトに指示されているフォルダーに放り込む。
% python views.py
で、自前のウェッブサーバーを立ち上げて、ブラウザで
http://localhost:5000
を表示させると、画像が順番に出てくるので、naoの顔の部分の座標をマウスをスライドさせて指定する。それらは、info.datファイルに次のような感じで自動的に保存される。
static/img/Nao_robot.jpg 1 434 2 316 271
static/img/29437446525_059b230e13_z.jpg 2 324 93 86 67 219 148 38 30
・・・・・・
最初が画像のパスで、次がその画像の中の顔の数、そして座標とサイズである。

（３）positive.datとnegative.datの作成
画像リストデータファイルを作成する。positive.datは、新しい場所に置き換えた場合も元々の場合も、そのフォルダーの絶対パスを冒頭のパスに全て書き換える。私の場合は、次のステップのvectorファイルの作成が、macではうまくいかず、raspberrypi に全部移したので、そちらのパスに書き換えた。negative.datは、単にその画像のある絶対パスのリストで良い。

（４）positiveベクトルの作成
Macのopencvでは、opencv_createsamplesがうまく機能しなかった。原因は不明。で、raspberrypiのopencvで作成することにした。linuxmintでもよかったかと思う。どのosのopencvでもいいのだ。
Raspberrypiのopencvは、cameraを使用する関係で、少し手の込んだインストールが必要で、。次のサイトに基づいている。
https://qiita.com/NaotakaSaito/items/f1f1548c8b760629cd26
あとは、positive.datが置いてあるフォルダでopencv_createsampleコマンドを実行すればいい。私の場合は次のようにした。
opencv_createsamples -info positive.dat -vec positive.vec -num 574 -w 40 -h 40
-numは、positive画像数である。-w 40 -h 40は、ベクトルサイズらしく、よくわからない。ただ、認識対象の映像の中で最小認識可能な画像サイズと考えればいいようだ。私の場合、画像サイズを、3320X240にしているので、その中で、顔が最小限どのくらいのサイズで映った時に認識して欲しいかという基準でやったが、もっと大きくてもいいかもしれない。（その後、60X60でやってみた。この60X60の方がnaoの顔を安定して識別しつずける可能性があるように思えた）

（５）学習器の作成
私の場合のコマンドは次のようになった。
opencv_traincascade -data /home/pi/Project/ObjectRecognition/data/model -vec /home/pi/Project/ObjectRecognition/src/positive.vec -bg /home/pi/Project/ObjectRecognition/src/negative.dat -numPos 450 -numNeg 400 -featureType HOG -maxFalseAlarmRate 0.1 -w 40 -h 40 -minHitRate 0.97 -numStages 17
絶対パスで指定しなくても良いかと思う。
途中までやったのは生かされる。

最終の出力は以下のようである。
---------------------------------------------
Training parameters are pre-loaded from the parameter file in data folder!
Please empty this folder if you want to use a NEW set of training parameters.
--------------------------------------------
PARAMETERS:
cascadeDirName: /home/pi/Project/ObjectRecognition/data/model
vecFileName: /home/pi/Project/ObjectRecognition/src/positive.vec
bgFileName: /home/pi/Project/ObjectRecognition/src/negative.dat
numPos: 450
numNeg: 400
numStages: 17
precalcValBufSize[Mb] : 1024
precalcIdxBufSize[Mb] : 1024
acceptanceRatioBreakValue : -1
stageType: BOOST
featureType: HOG
sampleWidth: 40
sampleHeight: 40
boostType: GAB
minHitRate: 0.97
maxFalseAlarmRate: 0.1
weightTrimRate: 0.95
maxDepth: 1
maxWeakCount: 100
Number of unique features given windowSize [40,40] : 100

Stages 0-4 are loaded

===== TRAINING 5-stage =====

Training until now has taken 0 days 0 hours 7 minutes 23 seconds.

===== TRAINING 6-stage =====
<BEGIN
POS count : consumed 450 : 522

ロボットの自己位置認識は、地図を作成しながらその地図の中の自己位置を捉えて行く方法がメジャーのような気がする。しかし、舞台上でロボットの自己位置認識をさせる場合は、舞台のマップは大概の場合すでに与えられている。そこで、舞台地図が与えられた場合、距離センサーの測定値から自己位置を推定する手続きを考える。地図を作成しながらより、もっと単純になる。（以下の図は、GrabitDesignerといアプリで描いた、今までは、illustratorを使っていたが、長期的には、無料ソフトがいいと思って鞍替えしている、よく似ているので使いやすい）

舞台上の左下を原点として、上側にy軸、横方向にx軸をとる。舞台奥行きはDepth、横幅はWidthで表されているとしよう。位置の点は黒丸で表される。また、ロボットの向き角度は、下手（左）から上手（右）に向かう方向をゼロとして、反時計回りの角度で表されるとしよう。

今、ロボットがθ方向に、距離dをセンサーによって取得したとしよう。ただし、その自己位置(x, y)はわかっていず、距離の対象になった点(x0, y0)もわかっていないとする。

ただし、重要なのは、この時点で、舞台が限られていることから、(x, y)が舞台の中にあるとしたらその場所は、自由ではありえない。対象となる点は、舞台の客側を除いた、(0, 0)→(o, Depth)→(Width, Depth)→(0, Width)をつなぐ線上のいずれかの点である。この舞台枠のラインをSとしよう。

今、測定誤差を前後左右の辺の長さが2εの正方形で表されるとしよう。(x0, y0)をS上に与えた時にd,θから規定される(x, y)が舞台の中に収まっている可能性があるかどうかを計算することができる。

上記不等式は、(x0, y0)から、ロボットの位置(x, y)の可能性の範囲を示している。この時、(x0, y0)を舞台枠Sに沿って動かせば、d, θを測定した場合のロボット位置の範囲全体が示せる。この領域をEとする。

今、ロボットが角度を変えたり、位置を変えたりして（あるいは、違う方向を向いたセンサー情報でも良い）、情報が追加されれば、その都度、この領域が与えられる。第i回目の測定で計算された領域をとするとその共通（積）集合、の中に、ロボットがいる可能性が高いということになる。

測定は、角度だけ変えるのが基本である。位置まで変えると、ややこしくなってしまうし、また、舞台上に机などがあると、このように単純にはいかないが、その場合も測定できるように拡張することは、それほど難しくないと考えている。

RobotShopで16,000円以上もする距離センサーLidar Lite v3を購入した。

I2Sで動かそうとしたのだが、ネット上のプログラムでは動かなかった。理由はわからない。そこで、pi4jライブラリを使ってjavaで動かすプログラムを作成したら、すんなり動いてくれた。

ここに公開しておく。（こちらのpythonプログラムを参考にさせていただいた）

import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CDevice;
import com.pi4j.io.i2c.I2CFactory;
import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;
 
/**
 *
 * @author washida
 */
public class LidarLiteV3 {
    public static final int LIDAR_LITE_ADDR = 0x62; // address pin not connected (FLOATING)
    public static final byte ACQ_COMMAND = (byte) 0x00;
    public static final byte STATUS = (byte) 0x01;
    public static final byte FULL_DELAY_HIGH = (byte) 0x0f;
    public static final byte FULL_DELAY_LOW = (byte) 0x10;
 
    /**
     * @param args
     * @throws com.pi4j.io.i2c.I2CFactory.UnsupportedBusNumberException
     * @throws java.io.IOException
     */
    public static void main(String[] args) throws I2CFactory.UnsupportedBusNumberException, IOException  {
        I2CBus i2c = I2CFactory.getInstance(I2CBus.BUS_1);
        I2CDevice device = i2c.getDevice(LIDAR_LITE_ADDR);
        while (true) {
            int response = device.read(0x04);
            device.write(ACQ_COMMAND, (byte) response);
            try {
                Thread.sleep(500);
            } catch (InterruptedException ex) {
                Logger.getLogger(LidarLiteV3.class.getName()).log(Level.SEVERE, null, ex);
            }
            int value = device.read(STATUS);
            while ((value & 0x01) == 1) {
                value = device.read(STATUS);
            }
            int high = device.read(FULL_DELAY_HIGH);
            int low = device.read(FULL_DELAY_LOW);
            int val = (high << 8) + low;
            int dist = val;
            try {
                Thread.sleep(1000);
            } catch (InterruptedException ex) {
                Logger.getLogger(LidarLiteV3.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }
}

あるテレビ番組のオーディションが夜にあって、それに向けて、またその場で浮かび上がった課題を記しておく。
（１）メカナムホイール で移動する際、振動が発生する理由は、モーターを取り付ける金具が、振動するためだと思う。そこで、モーターをさらに固定するアルミ板を当てる必要がある。
（２）音声デバイスのカード番号、デバイス番号が適当に変わるので、プログラムで読み取って出力時に自動的に指定するようにすべきだ。
（３）エンコーダーによる回転の把握を、より細かくして、その粗さからくる四つのモータの停止位置の微妙な差をより小さくすべきだ。

すけるくんの最初のバージョンを抜本的に組み直した。春休みのおかげで、ほぼ引きこもりで作業してきた。主な変更点は次の通り。
（１）衝撃にも壊れにくくする
（２）前後左右に超音波距離センサーを動かすことができる（SPIでデータ取得）
（３）エンコーダーで4個のモーターの回転数を独立にカウントできる（SPIでデータ取得）
（４）新基盤の上に制御ボードを配置、部品を圧縮配置
（５）6電池を廃止し、12V（X4）のみにした、外部電源は24Vのまま
（６）非接触型DCDCコンバータを並列にして、5V電源の容量を増大させた
以上に伴い、その他、細かい仕様の変更を行った。音声出力は、12.5X12.5のまま。音声認識マイクもそのまま。

その動画をyoutubeにアップしました。
https://youtu.be/u_VU2cOFWVY
デバイスアドレスを０、１に区別することで、一つのSPIチャンネルで別々に制御できることが確かめられた。

４つの距離センサは、Cで制御し、4つのモータのエンコーダーからの回転数取得はjavaで制御しなければならない事情を先に書いた。そこで、２つのMCP23S17を使って、それぞれを距離センサとエンコーダ処理用にする戦略をとることにした。
RaspberryPi の一つのSPI０チャンネルに、上記の二つのMCP23S17をバスでぶら下げることにする。デバイスの識別は、A0,A1,A2端子を１、０で区別すればいい（８個区別出来る）。基本的に、０番と１番のデバイスにする。
距離センサはCプログラムで制御するのだが、ロボットの方は、javaで制御システムが書かれているので、センサのCプログラムは、ライブラリ化（libdistance.so）して、jna経由で、javaから直接そのライブラリを使うようにすればいい。これはすでにやったことがあるので、簡単にできると思ったが、結局、昨日の夜から今日の夜まで、かかった（途中、大学に行って学生の修論指導はしたが）！
第一に手こずった問題は、libdistance.soを作成して、javaのjnaを使ったプログラムを書いて、lib distance.soをLD_LIBRARY_PATHの環境変数で与えられる場所において、動かしてみたのだが、どうやっても、
「java.lang.UnsatisfiedLinkError: /tmp/jna-3577/jna760224136558438898.tmp: /tmp/jna-3577/jna760224136558438898.tmp: 共有オブジェクトファイルを開けません: そのようなファイルやディレクトリはありません」
というエラーが出る。色々やって、かなり長時間格闘して、結論的には、最新のjna.jar (4.5.1)とraspberrypiの何らかの相性が悪いという判断をした。同じ状況で、linux（ubuntu16.04）では、ちゃんと動くのだから。
これは間違っているかもしれない。が、私の能力の範囲での結論はそうなのだ。だから、去年ちゃんとraspberrypi上で動いたjna.jarを持ってきて、それでセットしたらちゃんと動いた。あ〜あ。
もう一つ、こちらは大したことはないのだが、wiringPiのライブラリもちゃんとくっつけて、libdistance.soを作成しないと、wiringPiの関数が見つからないと怒られる！
プログラムをいかに掲載する。まず、Cライブラリのプログラム distance.cは以下のようになる。

/*
距離センサーライブラリ
MCP23S17をデバイス０番でセットしておく
使い方：
getdistance('センサー番号：整数 0-3')
*/
#include <stdio.h>
#include <wiringPi.h>
#include <mcp23s17.h>
#include <sys/time.h>
#include <unistd.h>
// 64より大きな数字、適当 wiringpiの要請
#define    BASE       123
int trigPin, echoPin;
int pulseIn(int pin, int level, int timeout){
  struct timeval tn, t0, t1;
  long micros;
  gettimeofday(&amp;t0, NULL);
  micros = 0;
  while (digitalRead(pin) != level){
      gettimeofday(&amp;tn, NULL);
      if (tn.tv_sec &gt; t0.tv_sec) micros = 1000000L; else micros = 0;
      micros += (tn.tv_usec - t0.tv_usec);
      if (micros &gt; timeout) return 0;
    }
  gettimeofday(&amp;t1, NULL);
  while (digitalRead(pin) == level){
      gettimeofday(&amp;tn, NULL);
      if (tn.tv_sec &gt; t0.tv_sec) micros = 1000000L; else micros = 0;
      micros = micros + (tn.tv_usec - t0.tv_usec);
      if (micros &gt; timeout) return 0;
    }
  if (tn.tv_sec &gt; t1.tv_sec) micros = 1000000L; else micros = 0;
  micros = micros + (tn.tv_usec - t1.tv_usec);
  return micros;
}
// セットアップ関数の呼び出しは、最初の１回だけである
//　何度も呼び出してはならない
void setup(int pin){ // pin -&gt; 0-3
	trigPin = BASE+pin+8; // B0-B3
	echoPin = BASE+pin;   // A0-A3
    wiringPiSetup () ;
    mcp23s17Setup (BASE, 0, 0) ;
    pinMode (trigPin, OUTPUT) ;
    pinMode         (echoPin, INPUT) ;
    pullUpDnControl (echoPin, PUD_UP) ;
}
int getdistance(){
    long duration, distance;
	//　HIGHが10μs継続するパルスを出す
    digitalWrite(trigPin, LOW);  // Added this line
    delayMicroseconds(2); // Added this line
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10); // Added this line
	digitalWrite(trigPin, LOW);
	//　その結果出力された超音波を受け取る
	//　その時間差から距離を測る
    duration = pulseIn(echoPin, HIGH, 1000000);
    //printf("duration=%d ",duration);
    distance = (duration/2) / 29.1;
    //printf("距離 = %d cm\n",distance);
    //sleep(1);
	return (int)distance;
}

これをコンパイルするためには次のようにする。
gcc -fPIC -shared -o libdistance.so distance.c -lwiringPi
最後に、wiringPiのライブラリをくっつけているところ大事。というのも、これをつけなくても、コンパイル自身は難なく通ってしまうから、実行時に叱られる。
export LD_LIBRARY_PATH=../XXX :$LD_LIBRARY_PATH
sudo ldconfig
とかする。../XXXはlibdistance.soをおく場所である。あるいは、
/etc/ld.so.conf.d
の設定ファイルに、そのパスを書いておき、ldconfigを実行するとかする。

import com.sun.jna.Library;
import com.sun.jna.Native;
import java.util.logging.Level;
import java.util.logging.Logger;
// ここの部分がlibdistance.soと接続するおまじない
interface DistanceLib extends Library {
    DistanceLib INSTANCE = (DistanceLib) Native.loadLibrary("distance", DistanceLib.class);
    // 使うべき関数を宣言する
    int getdistance() ;
    void setup(int pin);
}
/**
 *
 * @author washida
 */
public class Distance {
    DistanceLib dlib = DistanceLib.INSTANCE;
    void show(){
        dlib.setup(0);
        for(int i=0;i<100;i++){
            int dis = dlib.getdistance();
            System.out.println("No."+ i +": 距離 = "+dis+"cm");
            try {
                Thread.sleep(1000);
            } catch (InterruptedException ex) {
                Logger.getLogger(Distance.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }
    /**
     * @param args the command line arguments
     */
    public static void main(String[] args) {
        // TODO code application logic here
        Distance dis = new Distance();
        dis.show();
    }
}

こちらは特に問題はないだろう。

ロボットに四個の距離センサーをつけたい。四方の距離をいつでも測りたいというわけだ。US-015は、音波を発信してその跳ね返り時間で距離を測る。音を出すトリガとエコーを受け取る２つのgpioを消費する。八個のgpioを直接ラズパイのポートから取るというのは勿体なさすぎる。そこで、spiモジュールを使って、ラズパイのgpioを拡張してそこから取りたい。これが目標だ。
モーターの回転数をMCP23S17経由で取るというのは成功した。しかも、JAVAのpi4jを使って。これは先に書いた。
距離センサーは、同じようにpi4jでやろうとしたのだが、うまくいかない。超音波の発信と受信は、微妙な時間のタイミングで行われるので、どうもpi4jのライブラリとの相性が悪いと思った。
そこで止むを得ず、CのWiringPiライブラリで動かすことにした。まあ、動いた。
https://youtu.be/1zhjbX62gA8
ただ、問題はある。モータ制御との整合性だ。モーターは、JAVAで、距離センサーは、Cというのは、何しろ一つのMCP23S17なので、見るからにうまくいきそうにない。
両方Cで書くしかないが、モータの方はピンの値の変化をpi4jの割り込みで取っているので、それを割り込みを使わないでやれるようなCのプログラムを書かなければならない。それが面倒だ
距離センサのプログラムを以下に掲載しておく。
http://nopnop2002.webcrow.jp/HC-SR04/HC-SR04-1.html
と、wiringPiのMCP23S17サンプルを参考にさせていただいた
わかりやすく言うと、その二つを融合しただけである

#include <stdio.h>
#include <wiringPi.h>
#include <mcp23s17.h>
#include <sys/time.h>
#include <unistd.h>
#define BASE    123
// トリガピンとエコーピンは、MCP23S17のピン番号である
// 8ピンは、B0
// 7ピンは、A7 である
#define    trigPin    8
#define    echoPin    7
int pulseIn(int pin, int level, int timeout)
{
  struct timeval tn, t0, t1;
  long micros;
  gettimeofday(&t0, NULL);
  micros = 0;
  while (digitalRead(pin) != level)
    {
      gettimeofday(&tn, NULL);
      if (tn.tv_sec > t0.tv_sec) micros = 1000000L; else micros = 0;
      micros += (tn.tv_usec - t0.tv_usec);
      if (micros > timeout) return 0;
    }
  gettimeofday(&t1, NULL);
  while (digitalRead(pin) == level)
    {
      gettimeofday(&tn, NULL);
      if (tn.tv_sec > t0.tv_sec) micros = 1000000L; else micros = 0;
      micros = micros + (tn.tv_usec - t0.tv_usec);
      if (micros > timeout) return 0;
    }
  if (tn.tv_sec > t1.tv_sec) micros = 1000000L; else micros = 0;
  micros = micros + (tn.tv_usec - t1.tv_usec);
  return micros;
}
int main (void)
{
    int i, bit ;
    int j,k,l,m;
    long duration, distance;
    wiringPiSetup () ;
    mcp23s17Setup (BASE, 0, 0) ;
    printf ("距離センサーテスト with Raspberry Pi - MCP23S17\n") ;
    pinMode (BASE + trigPin, OUTPUT) ;
    pinMode         (BASE + echoPin, INPUT) ;
    pullUpDnControl (BASE + echoPin, PUD_UP) ;
    for(i=0;i<100;i++){
      digitalWrite(BASE + trigPin, LOW);  // Added this line
      delayMicroseconds(2); // Added this line
      digitalWrite(BASE + trigPin, HIGH);
      delayMicroseconds(10); // Added this line
      digitalWrite(BASE + trigPin, LOW);
      duration = pulseIn(BASE+echoPin, HIGH, 1000000);
      //printf("duration=%d ",duration);
      distance = (duration/2) / 29.1;
      printf("距離 = %d cm\n",distance);
      sleep(1);
    }
    return 0 ;
}