UWBモジュールとESP32を使って高精度な3D測位をする


###はじめに
最近、**iPhone11**などスマートフォンに**UWB**の無線機能が搭載された！とのこと
今後、**タグ**や**決済**、**クルマのキー**など、生活の様々な場所で利用が広がるらしいです
https://xtech.nikkei.com/atcl/nxt/column/18/01267/00062/
※つい最近AirTagが発表されましたね！追記(2021/4/28)
https://xtech.nikkei.com/atcl/nxt/column/18/00141/042600123/?n_cid=nbpnxt_mled_dm

それを受けて今回はインパルス方式のUWBモジュール（DWS1000 Arduino Shield）を購入して試しに**高精度な3D測位**を検討してみます！
DW1000のスペックを見ると周波数3.5GHｚ~6.5GHｚ、帯域1GHｚとかです。すすすごい！
![DW1000_Channels.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/87274/3e4753d3-9342-2c4e-e92d-71b4f10d2d4c.png)
#インパルス方式
因みにUWBにも色々通信方式があるのですが今回のヤツはPULSE-POSITION-MODULATIONみたいです。パルスの位置で1/0を送るようです。インパルスなので超広帯域なのですね。こんな面白い方式だとは知りませんでした。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/87274/b6c7a992-ed19-2a2d-6103-cdee9ecf3d55.png" width=60%>

#レシピ
##DWS1000 Arduino Shield
UWB通信モジュールにArduinoシールドの変換基板が付いたものです（立派ですね）
**DCDCコンバータが搭載**されているので、USBなどの**5V電源から**UWBモジュールに**3.3Vを供給**できます。
電源が不安定でモジュールが動かない！なんて事例もあるようなので、これは結構有難いです。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/87274/800c8ea0-f5cd-46de-e5ff-d73891036507.png" width=50%>
#####特徴
シングルチップ、IEEE802.15.4-2011 UWB準拠、超広帯域技術に基づくワイヤレストランシーバー
リアルタイムロケーションシステム（RTLS）で、最大5m / sで移動しているときでも、屋内で10cmの精度でオブジェクトの位置を特定できます。
ワイヤレスセンサーネットワーク（WSN）で、最大6.8Mb / sの高データレート通信を可能にします。
コヒーレント受信機技術のおかげで最大290mの優れた通信範囲。
短いパケット期間は、半径20mで最大11,000の高いタグ密度をサポートします
マルチパスフェージングに対する耐性が高く高フェージング環境での信頼性の高い通信を可能にします。
消費電力が少ないため、モードによってはバッテリーからの長時間動作が可能
（スペック見るだけでなんか**ワクワク**しますね！）
##ESP-WROOM-32
ESP32に変換基板が付いたものです。今回はWiFi/Bluetoothとかは使いませんが、将来何かの役に立つかも・・
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/87274/d68de11a-8a1e-5d22-62d0-81a6cc3b0e91.jpeg" width=70%>

#DWS1000 Arduino ShieldとESP-WROOM-32を接続したもの
これを2台作って片方を**TAG**、もう片方を**Anchor**としてプログラムを書き込みます
（さらに**TAG**を2台追加すれば３D測位が出来ます）

![IMG_5650.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/87274/ffa32b97-7db3-84ab-51b2-caa32f6696e0.jpeg)


#Arduinoライブラリ
https://github.com/thotro/arduino-dw1000/tree/master/
今回の３D測位ではANCHORとTAGの距離が分かれば良いので
ライブラリの中のDEMOサンプルDW1000Ranging_ANCHORとDW1000Ranging_TAGを使えばよいです

#色々調べてみるとESP32でモジュールを動作させるには以下の変更が必要のようです

ライブラリ（arduino-dw1000-masterフォルダ）で以下の3つのファイルをESP32対応に編集します
DW1000Ranging_ANCHOR.ino
DW1000Ranging_TAG.ino
DW1000.cpp
#####ボードのピン設定をESP32用に変更します
```c

// connection pins
const uint8_t PIN_SCK = 18;
const uint8_t PIN_MOSI = 23;
const uint8_t PIN_MISO = 19;
const uint8_t PIN_SS = 2;
const uint8_t PIN_RST = 15;
const uint8_t PIN_IRQ = 17;
```
#####DW1000.cppファイル（176行目）をESP32用に変更します
```c
#ifndef ESP8266　
//↓変更
#ifndef ESP32
```

#構想としては下図のような３D測位の実験をする予定です
UWBモジュール＋ESP32でANCHOR（アンカー）３台とTAG（タグ）1台を作って3点測位する
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/87274/9d80e818-b9c5-0024-176a-f8749a5872b9.png" width=80%>

#TAGの位置座標を求める計算
GPSの位置測位と似た感じで
以下の計算でいけると思います（某大学1年生に頼んで解いてもらいました）

TAGの座標：$(x,y,z)  > 0$
ANCHOR0の座標：$(0,0,0)$
ANCHOR1の座標：$(x_{1},0,0)$
ANCHOR2の座標：$(x_{2},y_{2},z_{2})$

ANCHOR0からTAGまでの距離：$R_{0}$
ANCHOR1からTAGまでの距離：$R_{1}$
ANCHOR2からTAGまでの距離：$R_{2}$

$x = \frac{(x_{1}^2 + R_{0}^2 - R_{1}^2)}{2*x_{1}}$
$c = ( \frac{x_{2}^2 + y_{2}^2 + z_{2}^2 + R_{0}^2 - R_{2}^2}{2} - x_{2}*x)/z_{2}$  

$p = -y_{2}/z_{2}$
$q = c^2 + x^2 - R_{0}^2$
$y = -p + \sqrt{(p^2 * c^2 - (1+p^2)*q)}$
$z = \sqrt{(R_{0}^2 - x^2 - y^2)}$

#まとめ
購入したUWBモジュールで３D測位をやってみようと考え
モジュールとESP32を接続するときの変更点などを調べてみました
さらに3D測位の計算方法について簡単な検討をしてみました
すみませんが今回はここまでです(m´・ω・｀)m ｺﾞﾒﾝ…

![UWB_3D測位.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/87274/96b7eb7f-6b73-f335-92a4-5f9c7932251b.gif)
夢の３D測位！（Processingでシミュレーション）