人間になりたい類人猿

人間以前の技術屋ブログ

三点測位をやってみた~TriThingPulse~

随分と放置してしまったけど、そろそろ何か書かないとと思ったので、特に有用性は無い事でも書いてみようと……

三点測位って?

GPS等で位置検知に使われる三点測位という手法がある。
センサからの電波強度を持って距離を測定し、緯度経度に直すらしい。
センサからの電波強度を求める式はわりと直ぐ見つかる↓(参考URL:
iBeaconのRSSIでiPhoneの二次元座標をとれたらいいな - tsujimotterのノートブック
)
             RSSI(r) = A - 10B\log_{10}(r)
※RSSI(r)……測定地点での電波強度
 A……センサから1m付近での電波強度
 B……電波の減衰を表す定数(一般には2らしい)
 r……センサから観測地点までの距離


でも意外と直交座標系に直すようなツールやプログラムが特に無い。
ちょっと作ってみた。(先に言うと柔軟性はあまり考えていなかった(;´ω`))
というわけで

プロジェクト「TriThingPulse」開始

※特に意味は無い


どうすれば出来る?

まずは上の電波強度を求める式を変換して、距離を求める式にする。
ここで久々の対数→指数で戸惑う。そもそもここで間違ってたら終わり。(;・ω・)
             r = 10^\left(\frac{A-RSSI(r)}{10B}\right)
これを使うと↓みたいな形で三点からの距離が求まる。
f:id:wannabehuman:20160727232720p:plain

だから、距離を半径とした円の方程式を作って交点を求めれば良い。
つまり↓を計算して、(x,y)を求めれば良いはず。
             (x-p_1)^2 + (y-q_1)^2 = r^2_1
             (x-p_2)^2 + (y-q_2)^2 = r^2_2
             (x-p_3)^2 + (y-q_3)^2 = r^2_3
p_n,q_nはそれぞれ円の中心のx座標/y座標つまりセンサの位置。

この式に定数を設定する。決めなければならない定数は
① 三点の座標(どこかを原点にしないといけない→センサのどれかを原点にするのが多分楽?)
② 三点それぞれの1m付近で観測される電波強度
③ 電波の減衰率
④ 観測点での電波強度

コードに落としこむ

○ざっくり設計
 <画面(windowsフォームアプリ)>
  ① 定数の固定値のB、各センサの座標、A値、RSSI(r)値をそれぞれ入力出来る
  ② 計算結果が出力される(ついでに①の情報とかも出しとく?)
 <計算>
  ① 計算については事前計算はC#、交点の計算はpythonに任せる。(理由:めんどくさいから)
  ② 事前計算とは各センサからの距離の計算。
 <フレームワークとか>
  ① C#(.Net4.0)
  ② python3.5.1
  ③ sympy-1.0.win32.exe(交点計算用:http://www.sympy.org/en/index.html)

○作ってみた
 C#側(事前計算部分とpythonを実行。表示。)

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Net;
using System.Diagnostics;

namespace TriThingPulse
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        private void button1_Click(object sender, EventArgs e)
        {
            //3つのWi-Fiのアクセスポイント(AP)からの距離を三点測位の計算式より算出
            //各APの座標を与える事で、端末の位置を直交座標系で算出する。
            //3つの波という事でTriThingPulse。他意はない。

            //まぁ長く使うもんでもないし……動けばよかろ。

            //AとBとRSSI(r)からAPからの距離を計算する
            double distApA = 0.0;
            double distApB = 0.0;
            double distApC = 0.0;
            try {
                distApA = Math.Pow(10.0, (double.Parse(AApA.Text) - double.Parse(RrApA.Text)) / (10.0 * double.Parse(B.Text)));
                distApB = Math.Pow(10.0, (double.Parse(AApB.Text) - double.Parse(RrApB.Text)) / (10.0 * double.Parse(B.Text)));
                distApC = Math.Pow(10.0, (double.Parse(AApC.Text) - double.Parse(RrApC.Text)) / (10.0 * double.Parse(B.Text)));
            }
            catch (Exception ex) {
                resultBox.Text = ex.Message + "\r\n";
            }

            //デバック時(1.0, 0.0)が交点になる
            #if DEBUG
                XApA.Text = Convert.ToString(0);
                YApA.Text = Convert.ToString(0);
                XApB.Text = Convert.ToString(-3);
                YApB.Text = Convert.ToString(0);
                XApC.Text = Convert.ToString(2);
                YApC.Text = Convert.ToString(0);
                distApA = 1.0;
                distApB = 2.0;
                distApC = 3.0;
            #endif

            //結果のboxに表示
            resultBox.Text = "各APからの距離は\r\n";
            resultBox.Text += "AP-A:" + distApA + "\r\n";
            resultBox.Text += "AP-B:" + distApB + "\r\n";
            resultBox.Text += "AP-C:" + distApC + "\r\n";


            //フォームの入力内容と共に、pythonのスクリプトを実行。
            //引数はそれぞれのAPのx,y,それぞれの距離(xA yA rA xB yB rB xC yC rC)
            Process p = new Process();
            p.StartInfo.FileName = Environment.GetEnvironmentVariable("ComSpec");
            p.StartInfo.UseShellExecute = false;
            p.StartInfo.RedirectStandardOutput = true;
            p.StartInfo.RedirectStandardInput = false;
            p.StartInfo.CreateNoWindow = true;
            p.StartInfo.Arguments = @"/c python [f:id:wannabehuman:20160728001516p:plain]script/CalcGridPosition.py " + string.Join(" ", XApA.Text, YApA.Text, distApA, XApB.Text, YApB.Text, distApB, XApC.Text, YApC.Text, distApC);

            resultBox.Text += "execute cmd: " + p.StartInfo.Arguments.ToString() + "\r\n";

            //起動
            p.Start();

            //出力を読み取る
            string results = p.StandardOutput.ReadToEnd();
            p.WaitForExit();
            p.Close();

            //結果を取得
            resultBox.Text += "result: " + results;
        }
    }
}

 python

import sys
from sympy import *

param = sys.argv
try:
 p1 = float(param[1])
 q1 = float(param[2])
 r1 = float(param[3])
 p2 = float(param[4])
 q2 = float(param[5])
 r2 = float(param[6])
 p3 = float(param[7])
 q3 = float(param[8])
 r3 = float(param[9])

 x,y,p,q,r = symbols('x y p q r')

 print(str(p1) + " " + str(q1) + " " + str(r1))
 print(str(p2) + " " + str(q2) + " " + str(r2))
 print(str(p3) + " " + str(q3) + " " + str(r3))

 circle = (x -(-1 * p))**2 + (y-(-1 * q))**2 - r**2

 c1 = circle.subs({p:p1, q:q1, r:r1})
 c2 = circle.subs({p:p2, q:q2, r:r2})
 c3 = circle.subs({p:p3, q:q3, r:r3})

 print(solve({c1,c2,c3},{x,y}))
except Exception as e:
 print(e.message)

※コードの綺麗さはあまり考えてない
で、こんな感じのが出来上がった。※デバッグモードで実行してる(いい感じの電波強度とかワカラン)
f:id:wannabehuman:20160728001516p:plain

作ってみた結果

実際にラズパイでwi-fiセンサ作って電波強度測って使ってみたが、
三点じゃまず交点が一点に定まらない……
調べてみたら、GPSだと最低でも6基使ってるらしい。
6つもラズパイ持ってない!!(´ . .̫ . `)

今後

まぁこのツールは一瞬使う為だけにやっつけで作っただけだが、
python側のコードは設計や計算を追加させれば普通に使えそうなので、隙をみて改修する。
(jsonとかのファイル読み込みに変えればOKかなぁ)

以上!!
明日も何か書こうヽ(゚∀゚)ノ パッ☆