こんばんは。
先日のBaseStationからセンサ座標を推定する方法に引き続き、今回はBaseStationの位置・姿勢を推定する方法についてです。
前提条件
まず、BaseStationは原点にあると仮定して計算を進める。このBaseStationの座標については、センサを原点として考えたときの座標を後に算出可能である。
BaseStationからの赤外線を受光するPhotoDiodeは4個以上あり、それらの位置と距離関係は既知のものとする。例えば、正方形の拡張頂点に4つのPhotoDiodeを配置するといった感じ。そして、それぞれのセンサがある位置をp0 ~ p3と定義する。
この際、BaseStationの赤外線信号を読み取ることで、方向ベクトルv0 ~ v3までが分かるが、距離k0 ~ k3は未知数であり、一番初めに求めたい要素である。
計算手法
前提条件より、センサの位置がすでに分かっているので、それをr01, r02, ... , r23として6つの定数が判明している。このr01などは、それぞれ |p0-p1| のようにベクトルの差によって表される。これをもとに、r012 を展開して行くと、r01などをk0 ~ k1で表せるようになる。
r01 ~ r23まで6つの定数について k0 ~ k3を用いて表現することができたので、6つの方程式からk0 ~ k3を求めていく。この際、r01 ~ r23までの値と、v0 ~ v3までの値は定数として既知の情報である。
未知数の数に比べて式の数が多く、なおかつ実測値には誤差が含まれているため、解が一意に収束することはない。そのため、k0 ~ k3に適当な値を代入していき、方程式の誤差が最小になるものを探す必要がある。4重ループで0~1000くらいの値を突っ込んでもいいし、より効率的な手法を知っているならそれでもいい。Pythonのsympyライブラリにおけるnsolveという関数を用いると、考えなくても解くことができる。
※この計算では極力誤差を減らすため、v0 ~ v3の値は200点平均を取っておくことが推奨されている。
k0 ~ k3が求まると、ようやくBaseStationから見たPの座標が計算できる。下画像のように、k02 ~ k32から求める方法では、④の計算方法と同じような感じで、P=(x, y, z)とおいて方程式を解いていく。
そのほかに考えられる手法として、p0を原点とした座標系を考えて、P=-p0の位置にあると捉えることができる。また、p0 ~ p3までの重心を原点ととらえる場合は、 p'0=(-d/2, d/2, 0), ... というようにベクトルを表現しなおして、⑤の方程式を求める方法である。多分、-(p0 + p1)/2 がPの位置ベクトルになると思う。
