机上の空論主義者

-♰- 有言不実行の自信をブログ名で戒めろ -♰-

2つのBase Stationからセンサの座標を推定する計算方法【19/31記事目】

こんにちは。

昨日紹介した、Base Stationの赤外線信号を受信したうえで、どう計算すればセンサ座標を推定できるかについて説明していきたいと思います。

ume-boshi.hatenablog.jp


Rayの方向ベクトルについて

前回の記事で、sweepの赤外線を受光するタイミングを見ることで、xy軸方向のベクトルがわかるとお話ししました。sweepは、1フェーズの開始から 1222 ~ 6777(us)の間に受光できる赤外線平面です。また、sweepの回転速度は120π(rad/s)となっています。中心となるタイミングの4000(us)との時間差を基に、x, y方向それぞれへの角度を取得することが可能です。

f:id:ume-boshi:20210519224126p:plain
Sweepを受光した角度を取得する方法

ここで取得できた角度は、ある平面上にあるベクトルであり、このベクトルを基にx,y軸それぞれのsweepに関して法線ベクトルを求めます。そしてできた2つの法線ベクトルに直行するベクトルが、2平面が交差する直線を示します。そしてこれが求めたい方向ベクトルです。法線ベクトル同士を外積するだけで求まります。

f:id:ume-boshi:20210519224746p:plain
2つの法線ベクトルの外積が、求めたい方向ベクトルである。


2つの方向ベクトルの交点と距離について

1つのBase Stationからは、トラッキング対象までの方向ベクトルしか分からず、距離が分かりません。そのため、まだ物体の位置は計算できません。2つのBase Stationからの対象までの方向ベクトルについて、交点を求めることでようやく座標を推定できます。

ここで注意しなければならないのは、2つの方向ベクトルは厳密には交差しないことです!そのため、最も近づくと思われる距離をs, tと媒介変数としておいて、連立方程式を求める必要があります。

f:id:ume-boshi:20210519225456p:plain
2つの方向ベクトルの交点を求め、センサの座標を推定する。


おわりに

昔から線形代数が苦手で、この手の計算は先人がいなければ難しいものですね。。。
この計算式を見ると、Base Stationは絶対に2つ無いとならなさそうに見えます。ただ、これはフォトダイオードのセンサ1つで計算するための式であり、VIVE trackerのように大量のフォトダイオードを持っている場合は、1つのBase Stationでも賄えるのかもしれません。センサ同士の絶対距離が分かっていますからね。

「Base Stationの回転行列をどう求めるんだ!」という人は、今度そのうち書く記事の方を参照していただければと思います。

参考文献

https://hivetracker.github.io/files/AH18-HiveTracker.pdf

github.com