こんにちは．修論に時速320km/hの速さで追われているume-boshiです．
このブログで進捗報告をしていた自作VIVE trackerについて，5月末から全く触れてきませんでしたがその件についてです．
ちなみに，Seeeduino UG Advent Calendar 16日目用の記事です．2度も期日を伸ばしてしまったこと，申し訳なく思っています． 本記事の当初の目標はこの試作品の完成だったので，キリのいいところまで開発したかったのですが，完成のメドが立たないので現状までの実装内容の紹介となります．
まあ，そもそもseeeduino XIAOをマイコンに使っているだけで，対して特性を活かしているわけでもないので，あまり期待せずにご覧ください．
今回の進捗
Base Stationの座標を推定するために，trackerがもつ4つのフォトダイオードでの受光を行い，trackerまでのベクトルを求める段階で詰まっています．
そもそもなぜBase Stationの座標が必要かというと，既知の位置・姿勢がわかっている2つのBase Stationがあり，そこから発される赤外線信号をVIVE trackerが読み取ることでtracker自身の座標を推定することができるからです．この際，Base Stationの配置は人が適当に行うため，そのたびに手作業で独自の座標空間を定めて位置姿勢を求めることは現実的ではありません．そのため，trackerがもつ複数のフォトダイオードから，Base Stationの座標を逆算して推定してあげる必要があります．
このBase Stationの位置姿勢の推定のためには，4つ以上の位置関係がわかっているフォトダイオード（以降，センサ）が必要であり，そのセンサが受光するわずかな時間差をもとにして算出していきます．
実装内容
フォトダイオードを追加
まず，何をするにもセンサが存在しなければ意味がありません．
そこで，3つのセンサを追加しきれいに配置ました．センサ用の小型基板には，φ2.1mmの穴をあけていたため，ねじ止めが可能です．いい感じの剛体がなかったので，ユニバーサル基板をちょっとだけ加工して取り付けました．
後に，センサ同士の位置関係を基地の値として取り扱うため，センサはきれいに配置するに限ります．
ということで今回は，縦40 mm，横 54 mm，対角 67 mmほどの距離に配置されることになりました．
センサごとにBase Stationからの赤外線信号を受信
赤外線信号を複数のセンサから受信する際のプログラムは，ただの力業で実現しています．要するに4つのセンサがあれば，4つの割り込み用関数を用意して，グローバル変数に保持された値を更新していく形です．Base Stationの信号は120Hzで受信され，センサ同士の距離が近い場合はほぼ同時に割り込み処理が生じます．正直，割り込み処理の仕様は知らないのですが，割り込みで呼び出される関数で処理する内容はできるだけ軽いほうが良いと思われます．
そこで，複数のセンサで統一できるデータは同じものを使用し，必要な値のみを割り込み関数で取得します．そこで，信号処理のメインとなるセンサを1つ用意します．今回ではphoto1．それ以外はサブセンサとして捉えます．
以下が，割り込み関数における2つのセンサの抜粋です．
void photo1interrupt() { photo1.signalStatus += 1; long now = micros(); if (digitalRead(photo1.PIN) == 1) { //HIGHになったタイミング /* ------------ ① ------------ */ photo1.highStart = now; photo1.lowMicroSec = (now - photo1.lowStart); } else { //LOWになったタイミング photo1.lowStart = now; photo1.highMicroSec = (now - photo1.highStart); if ((photo1.highMicroSec > 50 && photo1.highMicroSec <= 135) && photo1.lowMicroSec > 1500) { photo1.signalStatus = 1; photo1.signalStart = photo1.highStart; // 色々と初期化処理を記述する！ LH[0] = false; LH[1] = false; } /* ------------ ② ------------ */ if (photo1.signalStatus > 5) { // 受信を受け付けない threeTiming = false; photo1.signalStatus = 10; LH[0] = false; LH[1] = false; return; } // micro secではなく、tickで考える ticks = (int)(photo1.highMicroSec * 48.148); if (photo1.signalStatus < 6) { if (ticks > 2500 && ticks <= 6500) { //はじめの2パルス // 最初のパルスがどれかを判断するために、 // パルスの長さと前回からのパルスの経過時間を用いる /* ------------ ③ ------------ */ //Serial.printf("Ticks:%d ", ticks); pulse = (int)((ticks - 2501) / 500.0); //0~7の値のはず axis = pulse & 0b00000001; dataBit = (pulse)&0b00000010; skip = (pulse)&0b00000100; // Serial.printf("Pulse = %d ********** Axis = %d, bit = %d, skip = %d \n", pulse, axis, dataBit, skip); // Lighthouseからのデータを溜めていく OOTX形式 /*省略*/ // Lighthouse A,Bのどちらかのうちskipしない側の、LighthouseをLH配列で覚えておく // 1つ目のHIGHを1, 2つ目を3, 2つ目を5の状態 if (skip == 0) { LH[(photo1.signalStatus - 1) / 2] = true; photo1.nextAxis = axis; if (photo1.signalStatus == 3) threeTiming = true; } } /* ------------ ④ ------------ */ else if (photo1.signalStatus == 4 || photo1.signalStatus == 5) { //xy軸方向のレーザであるはず if (((LH[0] & LH[1]) != true) && ((LH[0] | LH[1]) != false)) { //片方だけが更新であるとき int t = now - photo1.signalStart; if (t > 1222 && t < 6777) { updateBaseStationSum(&photo1, 0, t); threeTiming = false; } } } } else photo1.signalStatus = 10; } } void photo2interrupt() { long now = micros(); if (digitalRead(photo2.PIN) == 1) { //HIGHになったタイミング photo2.highStart = now; photo2.lowMicroSec = (now - photo2.lowStart); } else { //LOWになったタイミング photo2.lowStart = now; photo2.highMicroSec = (now - photo2.highStart); // micro secではなく、tickで考える /* ------------ ⑤ ------------ */ if ((photo1.signalStatus >= 3) && (photo1.signalStatus < 6)) { //xy軸方向のレーザであるはず if (((LH[0] & LH[1]) != true) && ((LH[0] | LH[1]) != false)) { //片方だけが更新であるとき int t = now - photo1.signalStart; if (t > 1222 && t < 6777) { updateBaseStationSum(&photo2, 1, t); } } } } }
以前の記事でも取り上げた個所が混ざっている気はしますが，ちょっと説明をしておこうと思います．
①Base Stationからの信号について，HIGHとLOWになった両者の時点でデータを取得しているのですが，HIGHになったタイミングでは，パルスの時間を判断できないため時間を覚えること以外は何もすることがありません．これは複数センサを用いている場合でも同じで必須です．
②Base Stationから受信する信号が，時々正常に読み取れないことがあります．その場合，1フェーズの開始を読み取れず，本来より多くのパルスを受信できてしまいます．したがって，HIGHとLOWの割り込み回数が，合計で5以上になったもの（3パルス以上読み取っている）については，エラーとして除外しています．
③同期信号からaxis, data, skipの情報を取得することは，1つのセンサからのみで十分です．ここが一番短縮できる部分であり，他のセンサにおける割り込み処理を軽くするポイントになります．
④センサ側は，受光量が少ない場合（？），HIGHとLOWのどちらかしか反応しないことがあります．そのためsweep信号は特に読み取りづらく，厳密にすべて正常に受信できた時のみに処理を行うのでは，かなりのロスが生じます．したがって，できるだけ緩くパルスを見張るために，3パルス目の立ち上がり（4）と立ち下がり（5）のどちらでも受光したことを認めます． // もしかしたら2重処理の回避処理書いてない？w
⑤サブセンサでは，メインセンサとは座標が異なるため，受光するタイミングも異なります．受光が発生しうるのは，メインセンサの信号取得フェーズにける3以上6未満（3パルス目の前後を許容）の場合のみであり，その条件に当てはまったらsweep信号の受光成功とみなします．
はじめは結構厳密に処理をしようとしていたのですが，ノイズや環境行の問題によってデータロスをすることが非常に多かったため，緩めの処理をしたら結構うまくいくようになりました．センサの選定とかをすると，もっと厳密なままで処理を進められるのかもしれません．
200点平均をとり，Base Stationから各センサへの方向ベクトルを求める
次に，受信時間のノイズをできるだけ排除するための，200点平均を導出します．200点平均する対象は，Base Stationから見たセンサ位置のx, y角度です．4つのセンサがある場合，4 * 2軸 = 8個の角度を保持することになり，それぞれ別々に集計していきます．
void updateBaseStationAngle(PhotoSensor *p, int index, int t){ float rad = 0; //LighthouseA (Base Station) if (LH[0]) { p->axisTime[0][int(photo1.nextAxis)] = t; // radianで求めている /* ----------- ① -----------*/ rad = 240 * (int(p->axisTime[0][int(photo1.nextAxis)]) - 4000) * 3.141592 * 0.000001; /* ----------- ② -----------*/ if (calib[0] < 8) { if (p->getCount[0][int(photo1.nextAxis)] < CALIBRATIONMMAX) { positionAverageA[index][int(photo1.nextAxis)] += rad; p->getCount[0][int(photo1.nextAxis)]++; } } // 現在の角度を保存 p->bsAngle[0][int(photo1.nextAxis)] = rad; } /*いろいろ省略*/ // CALIBRATIONMMAX = 200 点平均の導出 if (p->getCount[0][int(photo1.nextAxis)] == CALIBRATIONMMAX){ positionAverageA[index][int(photo1.nextAxis)] = positionAverageA[index][int(photo1.nextAxis)] / CALIBRATIONMMAX; p->getCount[0][int(photo1.nextAxis)]++; //複数回出力を防ぐために1つダミーを足す calib[0] += 1; //printRays(index, int(photo1.nextAxis), positionAverageA[index][0], positionAverageA[index][1],0); } /*いろいろ省略*/ /* ----------- ③ -----------*/ // 2つの角度から方向ベクトルを算出 void calcRayFrom2Angles(double src[3], double phi, double theta) { src[0] = -cos(theta) * sin(phi); src[1] = sin(theta) * cos(phi); src[2] = -cos(theta) * cos(phi); } /*いろいろ省略*/ double v0[3] = {}, v1[3] = {}, v2[3] = {}, v3[3] = {}; calcRayFrom2Angles(v0, positionAverageA[0][0], positionAverageA[0][1]); calcRayFrom2Angles(v1, positionAverageA[1][0], positionAverageA[1][1]); calcRayFrom2Angles(v2, positionAverageA[2][0], positionAverageA[2][1]); calcRayFrom2Angles(v3, positionAverageA[3][0], positionAverageA[3][1]);
①信号受光タイミングから，Base Stationから見たセンサへの角度を求める式です．前回のRay可視化に関する記事では，計算式を間違っていました．120Hzでsweep信号は回転していることから，120 * 2 pi (rad/s)の速度で回転しています．そして，データ取得の単位時間はusなので，0.000001を乗算することで角度radが求められます．
②calibというグローバル配列には，記録すべき8つの角度について，どこまで取得し終わったかを保存しています．変数名が変でごめん;)
③Base Stationから見たセンサがある角度 theta, phi がわかると，センサまでの方向ベクトルが導出できます．2つの平面が交わる直線が求めたい方向ベクトルであり，これは外積によって求められます．再掲載ですが，↓ のイメージです．
（私はここで，方向ベクトルのx, y, zで求めるべきものを，theta, phi だけの値だけで進めてしまい，その後の処理で全くうまくいかない失態をかなりの時間を浪費してしまいました．どんまぴよ．）
Base Stationからセンサまでの距離k0, k1, k2, k3を代数方程式で求める
ここまでが下準備で，ここからようやくBase Stationの座標を推定し始められます．とはいっても，まずはBase Stationと各センサまでの距離k (mm)を求めなければなりません．そのために代数方程式を解く必要があります．
過去の再掲載となりますが，下記の連立方程式を満たすような距離k0, k1, k2, k3を探索します．
既存の計算方法を考えるのが面倒くさかったので，自力で適当に実装したのですがそれが多分いろいろ問題になっている気がします．というのも，私が実装した手法は，10 x 10 x 10 x 10 の区切りで適当な範囲を逐次探索し，その探索範囲を徐々に最小二乗誤差を狭めていくような手法を取りました．この手法は，局所解に陥らない場合に有効ですが，残念ながら今回対象としたセンサへの距離の組み合わせ問題は，局所解が存在して効果的でないみたいです．他にニュートン法などで実装すべきなことはわかっていますが，とりあえず若干正常に動作することもある現状の計算方法を紹介しておこうと思います．
if (calib[0] == 8 && (k0 + k1 + k2 + k3 == 0)) { // 多分cmでkを定義できている float minRange = 0, maxRange = 1000; float kRangeMin[4] = {minRange, minRange, minRange, minRange}; float kRangeMax[4] = {maxRange, maxRange, maxRange, maxRange}; float kNear[4] = {}; // 計算中の二乗和が最少になったときのk値を保持 double diffSumMin = 1000000000; // 計算中の最少二乗和 double diffSum = 0; int searchInterval = 10; int searchCount = 0; /*定義回りの省略*/ /*デバッグ用表示省略*/ // 広範囲から徐々に狭めてn回かけて逐次推定 for (searchCount = 0; searchCount < 5; searchCount++) { float searchLength = (kRangeMax[0] - kRangeMin[0]) / searchInterval; /* ----------- ① -----------*/ for (k0 = kRangeMin[0]; k0 < kRangeMax[0]; k0 += searchLength) { for (k1 = kRangeMin[1]; k1 < kRangeMax[1]; k1 += (kRangeMax[1] - kRangeMin[1]) / searchInterval) { for (k2 = kRangeMin[2]; k2 < kRangeMax[2]; k2 += (kRangeMax[2] - kRangeMin[2]) / searchInterval) { for (k3 = kRangeMin[3]; k3 < kRangeMax[3]; k3 += (kRangeMax[3] - kRangeMin[3]) / searchInterval) { double d01 = k0 * k0 + k1 * k1 - 2 * k0 * k1 * v01 - r01 * r01; double d02 = k0 * k0 + k2 * k2 - 2 * k0 * k2 * v02 - r02 * r02; double d03 = k0 * k0 + k3 * k3 - 2 * k0 * k3 * v03 - r03 * r03; double d12 = k1 * k1 + k2 * k2 - 2 * k1 * k2 * v12 - r12 * r12; double d13 = k1 * k1 + k3 * k3 - 2 * k1 * k3 * v13 - r13 * r13; double d23 = k2 * k2 + k3 * k3 - 2 * k2 * k3 * v23 - r23 * r23; /* ----------- ② -----------*/ diffSum = d01 * d01 + d02 * d02 + d03 * d03 + d12 * d12 + d13 * d13 + d23 * d23; if (diffSum < diffSumMin) { // 最小二乗誤差の更新 diffSumMin = diffSum; kNear[0] = k0; kNear[1] = k1; kNear[2] = k2; kNear[3] = k3; Serial.printf("%.2f, %.2f, %.2f, %.2f, ", k0, k1, k2, k3); Serial.printf("k0**2:%.2f, k1**2:%.2f, k0**2+k1**2:%.2f, ", k0 * k0, k1 * k1, k0 * k0 + k1 * k1); Serial.printf("-2k0k1v01:%.2f, r01**2:%.2f, diffSumMin: %.2f", -2 * k0 * k1 * v01, r01 * r01, diffSumMin); Serial.println(""); } } } } } // rangeの更新処理 for (int i = 0; i < 4; i++) { /* ----------- ③ -----------*/ if (minRange != kNear[i]) kRangeMin[i] = kNear[i] - searchLength; if (maxRange != kNear[i]) kRangeMax[i] = kNear[i] + searchLength; } Serial.printf("searchCount: %d, diffSumMin: %f, searchLength: %f ", searchCount, diffSumMin, searchLength); Serial.printf("K= %.2f, %.2f, %.2f, %.2f", kNear[0], kNear[1], kNear[2], kNear[3]); Serial.println(""); } Serial.println(""); Serial.printf("searchCount: %d, diffSumMin: %f, K= %.2f, %.2f, %.2f, %.2f", searchCount, diffSumMin, kNear[0], kNear[1], kNear[2], kNear[3]); Serial.println(""); float p[4][3] = { {kNear[0] * v0[0], kNear[0] * v0[1], kNear[0] * v0[2]}, {kNear[1] * v1[0], kNear[1] * v1[1], kNear[1] * v1[2]}, {kNear[2] * v2[0], kNear[2] * v2[1], kNear[2] * v2[2]}, {kNear[3] * v3[0], kNear[3] * v3[1], kNear[3] * v3[2]}}; }
①ここでは，徐々に範囲を狭めていく逐次探索を実装しています．直感的に処理量がかなりかかることが予想できると思いますが，その通りです．分割数searchIntervalは15ぐらいが限界かなと思います．10くらいで区切り，範囲を狭めながら4回繰り返すとかだと，(104) * 4 = 4000 回程度のループ回数なので，意外にも10秒もかからず処理できちゃいます．seeeduinoが優秀なのだろうか．
②適当なknの値を当てはめた結果は，二乗誤差を見ることでよい値であるかを見極めます．最小二乗誤差を少しずつ更新していき，そのたびにknの目星をつけていき，調べる範囲を狭める参考にしています．
③調べる範囲を狭める際，初期設定した範囲外にならない処理を設けています．別に無くても困らんけどな．
現状の実行結果（文字だけ）
実装した点について実行してみた結果ですが，それっぽい値を導出できているように見えます．下記がその出力です（図で可視化できなくてすみません）．
start calibration!!! ----------Base StationのSweepを受信した角度 theta, phi---------- A02, -0.012, 0.123 A12, 0.016, 0.151 A22, 0.033, 0.165 A32, -0.035, 0.182 ----------Base Stationからみたセンサへの方向ベクトル---------- A03, 0.012, 0.123 A13, -0.015, 0.150 A23, -0.032, 0.164 A33, 0.035, 0.181 ----------方向ベクトル同士の角度①---------- v01: 0.999256, deg = 2.209931 v02: 0.998150, deg = 3.485393 v03: 0.997968, deg = 3.652790 v12: 0.999738, deg = 1.312491 v13: 0.998205, deg = 3.433950 v23: 0.997562, deg = 4.002042 ----------代数方程式の計算（ただの逐次探索）②---------- searchCount: 0, diffSumMin: 16750572.217250, searchLength: 100.000000 K= 900.00, 900.00, 900.00, 900.00 searchCount: 1, diffSumMin: 5474216.397448, searchLength: 20.000000 K= 840.00, 860.00, 800.00, 820.00 searchCount: 2, diffSumMin: 3444616.012254, searchLength: 4.000000 K= 828.00, 844.00, 792.00, 800.00 searchCount: 3, diffSumMin: 3331278.016753, searchLength: 0.800000 K= 825.60, 840.80, 788.00, 796.00 searchCount: 4, diffSumMin: 3313374.906096, searchLength: 0.159998 K= 825.12, 840.00, 787.20, 795.20 searchCount: 5, diffSumMin: 3313374.906096, K= 825.12, 840.00, 787.20, 795.20 ----------base stationからセンサまでの距離を考慮したベクトル---------- p0x: 9.731887, p0y: 101.528069, p0z: -818.791077 p1x: -12.947811, p1y: 125.983315, p1z: -830.395508 p2x: -25.369558, p2y: 129.214645, p2z: -776.096619 p3x: 27.727877, p3y: 144.187103, p3z: -781.510132 ----------センサ間の距離について，rnmが実際の距離で，pnmが推定値を示す③---------- r01:40.000000, r02:67.000000, r03:54.000000, r12:54.000000, r13:67.000000, r23: p01 len: 35.314171, err: -4.685829 p02 len: 61.818077, err: -5.181923 p03 len: 59.443401, err: 5.443401 p12 len: 55.795258, err: 1.795258 p13 len: 66.148849, err: -0.851151 p23 len: 55.433006, err: 15.433006
この出力について「それっぽい値」を出力しているものの，怪しいところが実はたくさんあります．
まずそもそも①方向ベクトル同士の角度が最大で4° もあり，「これは誠の値か？」と疑いたくなりますね．
次に②距離knの単位は mm なのですが，Base Stationからセンサまでの距離が，大体 80 cm となっていることがわかります．実際に距離を測ると，100 cm以上は離れているため，異常な値をはじき出しているわけです．
最後に，③センサ同士の推定距離pnmについて，その誤差が最大で15 mmあるようです．これはうまく最大5 mm未満で計算できることもあるのですが，自身のアルゴリズムの悪さを示しているわけですね．ちなみに，10 mm以上の誤差が存在する場合，「うまく200点平均の値が取れていないよ」と海外のエンジニアが書いていたので，どこか間違いが存在することは確実そうです．
終わりに
「実装してそれっぽい値が出たけれど結局狂っています」というくそ記事でした．色々と問題はありそうですが，とりあえず現状の計算式の状態でBase Stationの座標・姿勢を今後推定してみて絶望していきたいなと思います．
それでは，Part2をすぐに書けることを願って頑張っていきます～
