机上の空論主義者

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

BaseStationの位置を4つのPhotoDiodeからキャリブレーションする方法【23/31記事目】

こんばんは。

先日のBaseStationからセンサ座標を推定する方法に引き続き、今回はBaseStationの位置・姿勢を推定する方法についてです。

ume-boshi.hatenablog.jp

ume-boshi.hatenablog.jp


前提条件

まず、BaseStationは原点にあると仮定して計算を進める。このBaseStationの座標については、センサを原点として考えたときの座標を後に算出可能である。

BaseStationからの赤外線を受光するPhotoDiodeは4個以上あり、それらの位置と距離関係は既知のものとする。例えば、正方形の拡張頂点に4つのPhotoDiodeを配置するといった感じ。そして、それぞれのセンサがある位置をp0 ~ p3と定義する。

f:id:ume-boshi:20210523213407p:plain
前提条件



この際、BaseStationの赤外線信号を読み取ることで、方向ベクトルv0 ~ v3までが分かるが、距離k0 ~ k3は未知数であり、一番初めに求めたい要素である。


計算手法

前提条件より、センサの位置がすでに分かっているので、それをr01, r02, ... , r23として6つの定数が判明している。このr01などは、それぞれ |p0-p1| のようにベクトルの差によって表される。これをもとに、r012 を展開して行くと、r01などをk0 ~ k1で表せるようになる。

f:id:ume-boshi:20210523213412p:plain
センサ間の距離(既知)を用いてk0 ~ k3の関係を暴く準備


r01 ~ r23まで6つの定数について k0 ~ k3を用いて表現することができたので、6つの方程式からk0 ~ k3を求めていく。この際、r01 ~ r23までの値と、v0 ~ v3までの値は定数として既知の情報である。
未知数の数に比べて式の数が多く、なおかつ実測値には誤差が含まれているため、解が一意に収束することはない。そのため、k0 ~ k3に適当な値を代入していき、方程式の誤差が最小になるものを探す必要がある。4重ループで0~1000くらいの値を突っ込んでもいいし、より効率的な手法を知っているならそれでもいい。Pythonのsympyライブラリにおけるnsolveという関数を用いると、考えなくても解くことができる。

※この計算では極力誤差を減らすため、v0 ~ v3の値は200点平均を取っておくことが推奨されている。

f:id:ume-boshi:20210523213418p:plain
変数に値を適当に代入していくことで、k0 ~ k3までの最適解を求める


k0 ~ k3が求まると、ようやくBaseStationから見たPの座標が計算できる。下画像のように、k02 ~ k32から求める方法では、④の計算方法と同じような感じで、P=(x, y, z)とおいて方程式を解いていく。

f:id:ume-boshi:20210523213424p:plain
最後にPのベクトルを求めて終了


そのほかに考えられる手法として、p0を原点とした座標系を考えて、P=-p0の位置にあると捉えることができる。また、p0 ~ p3までの重心を原点ととらえる場合は、 p'0=(-d/2, d/2, 0), ... というようにベクトルを表現しなおして、⑤の方程式を求める方法である。多分、-(p0 + p1)/2 がPの位置ベクトルになると思う。

f:id:ume-boshi:20210523221345p:plain
適当な値を代入する方法?