９割くらい終わる

# はじめに こんにちわ、リナちゃん@chrmlinux03 です。 普段は毎日圧着PINを打ったりArduino用ライブラリを作ったり、近所の食堂のヘルプに呼ばれてお食事を作ってます。 今回は Spresense を使って**uLiDAR**を自作し自律移動するロボット(クローラ型)を作ってみたいと思います。 あくまでも **uLiDAR** を作るのが目的ですからモジュール化し **足回り** は付け替えOKな感じで。 # 準備 ## uLiDARとは 最近のロボットには LiDAR と呼ばれるレーザによる測距センサーをぐるぐる回す機械が搭載されてます。  これは自分自身と壁までの距離つかって自分の今居る場所を推定させるためのデバイスであり、屋内ではこれはありかも知れませんが屋外ではちょっと...という感じです。それと**回転系**のデバイスは....。 そこでVL53L5CXというTOFセンサを64個(8x8)格子状に搭載したなんとも贅沢なセンサを使って **自前**でLiDARならぬ uLiDARを作ってみようと思います。 探査法は右手法アルゴリズムにて行いますので簡単な安価な測距センサも右手側に装備する必要があります。 >引用：ウイキペディア::迷路 >  ## VL53L5CXとは 簡単に言うと TOFセンサ(Time Of Flight方式の光学式測距)が 8x8 の格子状に並べられていて 前方 400cm (4m) におけるFOV が45°という**広範囲**のデータが取得できるセンサになってます 発売は随分前なんですが、何故かあまり使っている方々がおりません。(便利なのに) >引用:VL53L5CXユーザーズマニュアル > > > ## 自律移動とは 外部からのイベントによらず 自分で考えて移動する事ができる。これが自律移動。目的？それはあとで考える事にしよう。初期位置が設定出来ればそこにQi系の充電台を置いて自動充電も可能かも... ## クローラとは 屋内屋外問わず オムニホイールやメカナムホイールまたは車輪ですと どうしても落ちているものに引っかかってしまう。またサーボモータだと小さい型だとパワーが足りない。ここはDCモータでクローラ(キャタピラ)駆動させることにする。 ## DCモータとは 2本のIO(DegitalWrite + AnalogWrite) を使えば1個のモータはPWM駆動できます。 他の方々のライブラリは2本のIO(AnalogWrite x 2)を使っているようなのですが DCモータは所詮 **差動**で動くわけですから、ちょっと工夫をしてみました。 @[twitter](https://twitter.com/chrmlinux03/status/1559526096030531590?s=20&t=VKLJcG37k7H4nGBP9xmPyg) DCモータとはマブチモータ等の+(プラス)-(マイナス)で直接駆動し、反転はそれを逆にすることで実現可能。ただしDCモータは電流値が高く、マイコンから直接動かすことはできない。動かすためにはモータドライバなりの回路が必要。 またDCモータはON/OFF や OFF/ON の動作時に激しい電流の変化があるため回路やデバイスに対して**スムーズ**な駆動が必要なために、**台形制御**と呼ばれる駆動方法が必要です。そこで**移動平均**によるDCモータ駆動も可能と考え今回のDCモータ駆動は下記の自前ライブラリを使用する事にしました。  >自作台形制御ライブラリ >https://www.arduinolibraries.info/libraries/trape-zoid ## モータドライバとは 1モータに付き2本の配線を使いその差動によりモータを駆動させる回路。たとえば 1 0 だったら正回転(CW) 0 1 だったら逆回転(CCW) 0 0 が停止 1 1がブレーキをいう構成で動かす事が出来ます。 過去の経験を活かし2PIN(DegitalOut + AnalogOut)のみで1個のモータを駆動し、モータの優劣(劣化/個体差)によるFactも加味してあります >自作DCモータ駆動ライブラリ >https://github.com/chrmlinux/tinyDC ## SLAMとは Simultaneous Localization and Mapping の頭文字をとってSLAM(スラム)と呼ばれる手法。Simultaneous=同時に　Localization=位置推定　and=と　Mapping=地図作成　の意味となります。 Simultaneousは"いろいろなセンサ"を同時につかいという意味であり 今回使用する IMU(Inertial Measurement Unit)や接触センサ等が含まれます。自律移動させるためにはこれは必須。 > 過去に**SLAMを自作しましょう**とかいう大胆な発言を某社でしていたのですが >有名大学の先生さまが「いやそれ出来れば一生食って行けます」とか言われて居たのに....まさか本気で自作するとは。時代はSLAM自作可能な域まで迫ってきましたね ## IMUとは IMU には色々なタイプがありますが 6軸(加速度センサ+ジャイロセンサ) 9軸(加速度センサ+ジャイロセンサ+地磁気センサ)等が有名な所。今回9軸を使用して開発を進めていたのですがモータが駆動されると"地磁気が乱れる"という当たり前な現象に悩まされ泣く泣く6軸の使用となりました。 今回は　MPU6050 という6軸タイプのセンサを使いましたが、wifiModem で使用するモジュールに M5AtomMatrix を使えば MPU6885 用の配線は不要です。  # ハード製作 ## ハード構成 Spresense本基板 Spresense拡張基板(Arduino形状) AddonBoard01基板 バッテリ基板(3.7V->5V充放電基板) バッテリ(LiION3.7V 400mAH) DCモータ5Vx2 クローラセット2輪 各種センサ DAISOのカバー的なもの ## AddOnBord01基板 配線が色々と大変ですので基板化しました 製作と言ってもコネクタをつけまくって電源を張りまくってって感じです。  ## Arduinoバニラシールド 遠い記憶に Arduio用自作基板があったのを思い出し倉庫から引っ張り出してきましたぴったりです。   ## MotorDriver(MX1919) GPIO 4本を使い2個のモータを制御させる事が出来ます。差動によりモータを駆動させるたとえば 1 0 だったら正回転(CW) 0 1 だったら逆回転(CCW) 0 0 が停止 1 1がブレーキをいう手順で動かす事が出来ます。 ## WiFiModule(M5AtomLite) 今後の拡張のため WiFiはどうしても必要により苦肉の策、M5AtomLiteはI2CSlaveとして駆動させるので今回はWiFiModemとして使用されます。ただしあくまでも今回の主役は Spresense であって M5Atom は I2C Slave として動作します。 ROSもWiFiであれば受け入れてくれるかもしれない....  >自作 I2C MASTER/SLAVE ライブラリ >https://github.com/chrmlinux/esp32MasterSlave ## TOF64センサー VL53L5CX(8x8x400cmマルチゾーン対応ToF測距センサ)  ## HC-SR04(300cm 超音波距離センサ) 右手法アルゴリズムにより**洞窟**等の道順探索にはかならず右手を**壁**に手を触れて洞窟を探索する事になります。 今回のTOF64センサは前方を見て壁の有無を45°のFOV で検出し、SR-04 は右手法アルゴリズムの**右手**の役目をします。  ## カバー セリアに売っていた**ダミーカメラ**の筐体を使うことにした。 色々入って110円(税込)は奇跡である。  往年の**ハカイダー**もしくは**禁断の惑星**を彷彿させる姿ではあるが中身が見えるとカッコいい感は否めない。  ## 電源 今まで使っていたリチウムイオン充放電基板が 3.7V -> 5V が別基板となっているので収まりが悪い。 そこで 新たに バッテリ(3.7V) -> 充放電 -> 出力(5V) という素敵な基板を入手。 但し マイクロUSB基板が自前での取り付けなのでちょっと面倒。 だけどコンパクトに勝るものは無い  > 電源充電器ボード モジュール 2A 5V ## ハーネス ダミーカメラの前面に TOF64 対して 右手側に SR04、後ろ側に マイクロUSB 充電端子を配置。 この マイクロUSB端子に Qi 充電パネルを取り付け下部に配置すると Qi 系の充電が出来そう。 充電時間?それは後の話である。  # ソフト製作

## コアの割り当てと共通領域 今回のメインはあくまでも Spresense ですので MainCoreにて共通領域の確保とセンサからのデータ取得 を行い SubCore1 では受け取ったデータを使って 24時間 365日 **自己位置推定**/**移動距離**/**回転方向(Yaw角)** の算出を行います。 ## ファイルの配置 各Coreごとのinoファイルは共通の config.h / uLiDAR.hpp を参照させる必要があるためにフォルダ構造を以下のように構成しました config.h / uLiDAR.hpp は Sub1配下に存在するために Main をコンパイルする場合には Sub1 配下を必ず**書込**して最新にする必要があります ```c++:folder uLiDAR +Main | Main.ino + Sub1 | Sub1.ino | config.h | uLiDAR.hpp ``` ## 各Coreのメモリ割当と共通領域 Spresense 独特の表現方法で MainCore/SubCore で使用するメモリの割り当てを行うことが出来る またSharedMemory のポインタを使用する事でより多いデータを各core間でデータの共有を行う事が出来る(**重要**) 今回は以下の割り当てで行う事とした MainCore : 512 KByte  SubCore1: 256 KByte  SharedMem : 768 KByte(**余ったメモリをすべて使い切る**) ```c++:Sub1/config.h 一部抜粋 //======================================== // Spresense Muluti Core Module //======================================== #include <MP.h> #include <MPMutex.h> MPMutex mtx(MP_MUTEX_ID0); #define ERROR_LEDID (3) #define MAINCORE (0) int8_t msgid = 10; int subcore = 1; //======================================== // uLiDAR config //======================================== #define TOF64_HEIGHT (8) #define TOF64_WIDTH (8) #define TOF64_MAXCNT (TOF64_HEIGHT * TOF64_WIDTH) #define AREA_MAXHEIGHT (896) #define AREA_MAXWIDTH (896) #define AREA_DATABIT ( 8) #define AREA_MAXCNT (AREA_MAXHEIGHT * AREA_MAXWIDTH) #define BLYNK_MAXCNT ( 8) #define WIRE_FREQ (1000 * 1000) //======================================== // Shard Memory //======================================== struct AXS_T { float x; float y; float z; }; struct ULIDAR_T { //------------------------------------------- // System Data //------------------------------------------- uint32_t sz; int func; int resp; int stat; //------------------------------------------- // Sensor Data //------------------------------------------- uint16_t tof64Ary[TOF64_MAXCNT]; uint16_t side; AXS_T g; AXS_T a; AXS_T m; //------------------------------------------- // Map Data //------------------------------------------- uint8_t areaAry[AREA_MAXCNT]; //------------------------------------------- // Blynk Data //------------------------------------------- uint8_t Bdt[BLYNK_MAXCNT]; //------------------------------------------- // Last Tag //------------------------------------------- int32_t cnt; }; static ULIDAR_T *adrs; //======================================== // mutex //======================================== void mtxLock(void) { int rtn; do {rtn = mtx.Trylock();} while (rtn); } void mtxUnLock(void) { mtx.Unlock(); } //======================================== // ledary //======================================== #define LEDS_MAXCNT (4) static uint8_t ledary[LEDS_MAXCNT] = { LED0, LED1, LED2, LED3 }; //======================================== // ledOnOff //======================================== void ledOnOff(int ledid) { static int stat = 0; if (stat) ledOn (ledary[ledid]); else ledOff(ledary[ledid]); stat = !stat; } //======================================== // ledOnOffNoDelay //======================================== void ledOnOffNoDelay(int ledid, uint32_t delaytm) { static uint32_t tm = millis(); if ((millis() - tm) > delaytm) { tm = millis(); ledOnOff(ledid); } } //======================================== // ledOnOffNoWhite //======================================== void ledOnOffWhile(int ledid, uint32_t delaytm) { while(1) { ledOnOffNoDelay(ledid, 1000); } } ``` config.h で定義された構造体 ULIDAR_T の大きさを構造体メンバー adrs->sz に格納し そのサイズで作業を簡潔にさせる事とした ```c++:Main.ino 一部抜粋 #include "Sub1/config.h" #include "Sub1/uLiDAR.hpp" #ifdef SUBCORE #error "Core selection is wrong!!" #endif void *dmy; void setup(void) { setupLiDAR(MAINCORE); uint32_t sz = sizeof(ULIDAR_T); adrs = (ULIDAR_T *)MP.AllocSharedMemory(sz); if (!adrs) ledOnOffWhile(ERROR_LEDID, 100); memset(adrs, 0x0, sz); adrs->sz = sz; MPLog("SharedMemory size=%d adrsess=%08x\n", adrs->sz, adrs); MP.RecvTimeout(MP_RECV_BLOCKING); MP.begin(subcore); } void loop(void) { updateLiDAR(MAINCORE); MP.Send(msgid, adrs, subcore); MP.Recv(&msgid, &dmy, subcore); MPLog("recv cnt=%d\n", adrs->cnt); } ``` ```c++:Sub1.ino 一部抜粋 #include "config.h" #include "uLiDAR.hpp" #if (SUBCORE != 1) #error "Core selection is wrong!!" #endif void setup(void) { setupLiDAR(SUBCORE); MP.RecvTimeout(MP_RECV_POLLING); MP.begin(); } void loop(void) { if (MP.Recv(&msgid, &adrs) > 0) { updateLiDAR(SUBCORE); MP.Send(msgid, adrs); } else { 　update(); } } ``` ## IMUから移動距離/回転速度を算出 uLiDAR は走行時(運用時)キャリブレーションを行う必要があります。 これは 6軸センサから移動距離/回転方向を算出するのに必要な処理であり特に 回転角Yaw の算出/クローラと床面の摩擦係数を取得するのに必須な作業となります。 今回はこれを**オートキャリブレーション**機能を搭載することにより簡単にそれを行う事にしました。 ### オートキャリブレーション #### 前後移動により移動速度から移動距離を算出する  Wall に対して第3象限位置に置かれた車体を**一定速度**で前後移動を行い DYの値を取得し車体の移動速度を算出する。これにより一定時間あたりの速度が算出出来るため一定時間あたりの移動距離を算出することが出来る。これを数回行う事により床面とクローラの適正値を算出する事が出来る。 #### 回転により移動速度から移動距離を算出する  Wallに対してDY/DXを同じ距離を初期値として回転方向CWで**一定速度**で回転させTOF64センサが同じ距離を算出した時点で 1/4 回転(90°)とする。6軸センサのyaw各の補完として使用する事が出来る。これを数回行う事により床面とクローラの適正値を算出する事が出来る。 ## 移動距離/回転方向に関して 壁に対して TOF64 は以下のような信号を排出する。 単一のTOFと違い中心点から fov45°で扇状に出力されるデータなので排出されたデータに円弧処理を多用する事になる。 @[twitter](https://twitter.com/chrmlinux03/status/1570972350027538434?s=20&t=VKLJcG37k7H4nGBP9xmPyg) ## 取得されたTOF64データの補完 取得されたデータはあくまでも 8x8 のデータであるのでソフトウエアで 15 x 15 の倍分解能を持つデータに補完することとします。

 >自作補完ライブラリ >https://github.com/chrmlinux/ArrayExt ## yaw/Roll/Pitch に関して 

一般の6軸センサは Pitch/Roll は良い感じなのですが Yawがなかなかひどい状況です。 キャリブレーションにより**ある程度**の補完をする予定です。

# 5 実機動作

TOF64 のFOVは45 °ですのでかなりな**壁**を用意しないと中々上手く **壁**を捉える事が出来ませんが なんとなく**壁**に対する自分の位置/角度は検出出来ていると思われます。 > 過去に 週に4日会議とプレゼン用データ**可視化**の作業を行っていたのですが、**可視化**はあくまでもデモ用でありデモだけの為に**可視化**ソフトを製作するのはいつもどうもなぁと言う感じです # 6 最後に 中々ハードな1か月でしたがなんとなく完了の予感です これを期にTOF64とSpresense を使ったロボットが沢山出てくれば良いなという感じです。 ご清聴ありがとうございました。