web-dev-qa-db-ja.com

三辺測量とポイント（x、y、z）の特定

すべてが既知の座標を持つ3つ以上のノードから参照距離が離れている空間のどこかにある未知のノードの座標を見つけたいのですが。

この問題は、ここで説明するTrilateration Trilateration とまったく同じです。

しかし、「予備計算と最終計算」（ウィキペディアのサイトを参照）の部分がわかりません。 P1、P2、およびP3を見つけることができる場所がわからないので、それらの方程式に入れることができますか？

ありがとう

algorithmlocalizationgeometrytriangulationtrilateration
22
CB4

三辺測量は、3つの球が交差する領域の中心を見つけるプロセスです。 3つの球のそれぞれの中心点と半径がわかっている必要があります。

3つの例の中心点P1 [-1,1]、P2 [1,1]、およびP3 [-1、-1]を考えてみましょう。最初の要件は、P1 'が原点にあることです。したがって、オフセットベクトルV [1、-1]を3つすべてに追加して、ポイントを適宜調整します。

P1' = P1 + V = [0, 0]
P2' = P2 + V = [2, 0]
P3' = P3 + V = [0,-2]

注：調整されたポイントは、 '（プライム）アノテーションで示されます。

P2 'もx軸上になければなりません。この場合はすでに実行されているため、調整は必要ありません。

各球の半径を2と仮定します。

これで3つの方程式（与えられた）と3つの未知数（交点の中心点のX、Y、Z）ができました。

P4'xを解く：

x = (r1^2 - r2^2 + d^2) / 2d  //(d,0) are coords of P2'
x = (2^2 - 2^2 + 2^2) / 2*2
x = 1

P4'yを解く：

y = (r1^2 - r3^2 + i^2 + j^2) / 2j - (i/j)x //(i,j) are coords of P3'
y = (2^2 - 2^2 + 0 + -2^2) / 2*-2 - 0
y = -1

2Dの問題ではzを無視します。

P4 '= [1、-1]

次に、オフセットベクトルVを減算して、元の座標空間に変換します。

P4 = P4 '-V = [0,0]

解点P4は、予想どおり原点にあります。

記事の後半では、P1が原点にない、またはP2がx軸上になく、これらの制約に適合するようなポイントのセットを表す方法について説明しています。私はそれを翻訳と考えるのが好きですが、どちらの方法でも同じ解決策が得られます。

編集：P2 'をx軸に回転

P1を原点に変換した後、P2 'がX軸上にない場合、ビューで回転を実行する必要があります。

最初に、例として使用するいくつかの新しいベクトルを作成しましょう：P1 = [2,3] P2 = [3,4] P3 = [5,2]

まず、P1をOriginに変換する必要があります。いつものように、オフセットベクトルVは-P1です。この場合、V = [-2、-3]

P1' = P1 + V = [2,3] + [-2,-3] = [0, 0]
P2' = P2 + V = [3,4] + [-2,-3] = [1, 1]
P3' = P3 + V = [5,2] + [-2,-3] = [3,-1]

回転角度を決定するには、P2 'と[1,0]（x軸）の間の角度を見つける必要があります。

dot product equalityを使用できます。

A dot B = ||A|| ||B|| cos(theta)

Bが[1,0]の場合、これは簡略化できます。ドットBは常にAのXコンポーネントであり、|| B ||です。 （Bの大きさ）は常に1の乗算であるため、無視できます。

Ax = || A ||ができました。 cos（theta）。これを最終的な方程式に再配置できます。

theta = acos(Ax / ||A||)

または私たちの場合：

theta = acos(P2'x / ||P2'||)

|| A ||を使用してP2 'の大きさを計算します= sqrt（Ax + Ay + Az）

||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)

差し込むことでシータを解決できます

theta = acos(1 / sqrt(2)) = 45 degrees

回転行列 を使用してシーンを-45度回転します。 P2'yは正であり、回転行列は反時計回りに回転するため、負の回転を使用してP2をx軸に揃えます（P2'yが負の場合は、シータを無効にしないでください）。

R(theta) = [cos(theta) -sin(theta)]
           [sin(theta)  cos(theta)]

  R(-45) = [cos(-45) -sin(-45)]
           [sin(-45)  cos(-45)]

ダブルプライム表記 ''を使用して、平行移動と回転の両方が行われたベクトルを示します。

P1'' = [0,0] (no need to calculate this one)

P2'' = [1 cos(-45) - 1 sin(-45)] = [sqrt(2)] = [1.414]
       [1 sin(-45) + 1 cos(-45)] = [0]       = [0]

P3'' = [3 cos(-45) - (-1) sin(-45)] = [sqrt(2)]    = [ 1.414]
       [3 sin(-45) + (-1) cos(-45)] = [-2*sqrt(2)] = [-2.828]

これで、P1 ''、P2 ''、およびP3 ''を使用してP4 ''を解くことができます。 P4 ''に逆回転を適用してP4 'を取得し、次に逆変換を適用して中心点であるP4を取得します。

回転を元に戻すには、P4 ''にR（-theta）、この場合はR（45）を乗算します。変換を元に戻すには、オフセットベクトルVを減算します。これは、P1を追加するのと同じです（最初にVとして-P1を使用した場合）。

28
Dan Bechard

これは、3Dプリンターのファームウェアで使用するアルゴリズムです。座標系の回転は回避されますが、最適ではない場合があります。

三辺測量問題には2つの解決策があります。 2番目のものを取得するには、2次方程式の解で「-sqrtf」を「+ sqrtf」に置き換えます。

明らかに、十分なプロセッサ能力とメモリがある場合は、floatの代わりにdoubleを使用できます。

// Primary parameters
float anchorA[3], anchorB[3], anchorC[3];               // XYZ coordinates of the anchors

// Derived parameters
float Da2, Db2, Dc2;
float Xab, Xbc, Xca;
float Yab, Ybc, Yca;
float Zab, Zbc, Zca;
float P, Q, R, P2, U, A;

...

inline float fsquare(float f) { return f * f; }

...

// Precompute the derived parameters - they don't change unless the anchor positions change.
Da2 = fsquare(anchorA[0]) + fsquare(anchorA[1]) + fsquare(anchorA[2]);
Db2 = fsquare(anchorB[0]) + fsquare(anchorB[1]) + fsquare(anchorB[2]);
Dc2 = fsquare(anchorC[0]) + fsquare(anchorC[1]) + fsquare(anchorC[2]);
Xab = anchorA[0] - anchorB[0];
Xbc = anchorB[0] - anchorC[0];
Xca = anchorC[0] - anchorA[0];
Yab = anchorA[1] - anchorB[1];
Ybc = anchorB[1] - anchorC[1];
Yca = anchorC[1] - anchorA[1];
Zab = anchorB[2] - anchorC[2];
Zbc = anchorB[2] - anchorC[2];
Zca = anchorC[2] - anchorA[2];
P = (  anchorB[0] * Yca
     - anchorA[0] * anchorC[1]
     + anchorA[1] * anchorC[0]
     - anchorB[1] * Xca
    ) * 2;
P2 = fsquare(P);
Q = (  anchorB[1] * Zca
     - anchorA[1] * anchorC[2]
     + anchorA[2] * anchorC[1]
     - anchorB[2] * Yca
    ) * 2;  

R = - (  anchorB[0] * Zca
       + anchorA[0] * anchorC[2]
       + anchorA[2] * anchorC[0]
       - anchorB[2] * Xca
      ) * 2;
U = (anchorA[2] * P2) + (anchorA[0] * Q * P) + (anchorA[1] * R * P);
A = (P2 + fsquare(Q) + fsquare(R)) * 2;

...

// Calculate Cartesian coordinates given the distances to the anchors (La, Lb and Lc)
// First calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P.
// P, Q and R depend only on the anchor positions, so they are pre-computed
const float S = - Yab * (fsquare(Lc) - Dc2)
                - Yca * (fsquare(Lb) - Db2)
                - Ybc * (fsquare(La) - Da2);
const float T = - Xab * (fsquare(Lc) - Dc2)
                + Xca * (fsquare(Lb) - Db2)
                + Xbc * (fsquare(La) - Da2);

// Calculate quadratic equation coefficients
const float halfB = (S * Q) - (R * T) - U;
const float C = fsquare(S) + fsquare(T) + (anchorA[1] * T - anchorA[0] * S) * P * 2 + (Da2 - fsquare(La)) * P2;

// Solve the quadratic equation for z
float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A;

// Substitute back for X and Y
float x = (Q * z + S)/P;
float y = (R * z + T)/P;
0
dc42

これがOpenSCADスクリプトで表示されたWikipediaの計算です。これは問題を視覚的に理解するのに役立ち、結果が正しいことを簡単に確認する方法を提供すると思います。 スクリプトからの出力例

// Trilateration example
// from Wikipedia
// 
// pA, pB and pC are the centres of the spheres
// If necessary the spheres must be translated
// and rotated so that:
// -- all z values are 0
// -- pA is at the Origin
pA = [0,0,0];
// -- pB is on the x axis
pB = [10,0,0];
pC = [9,7,0];

// rA , rB and rC are the radii of the spheres
rA = 9;
rB = 5;
rC = 7;


if ( pA != [0,0,0]){
   echo ("ERROR: pA must be at the Origin");
   assert(false);
}

if ( (pB[2] !=0 ) || pC[2] !=0){
   echo("ERROR: all sphere centers must be in z = 0 plane");
   assert(false);
}

if (pB[1] != 0){
   echo("pB centre must be on the x axis");
   assert(false);
}

// show the spheres
module spheres(){
   translate (pA){
      sphere(r= rA, $fn = rA * 10);
   }

   translate(pB){
      sphere(r = rB, $fn = rB * 10);
   }

   translate(pC){
      sphere (r = rC, $fn = rC * 10);
   }
}

function unit_vector( v) = v / norm(v);

ex = unit_vector(pB - pA) ;
echo(ex = ex);

i = ex * ( pC - pA);
echo (i = i);

ey = unit_vector(pC - pA - i * ex);
echo (ey = ey);

d = norm(pB - pA);
echo (d = d);

j =  ey * ( pC - pA);
echo (j = j);

x = (pow(rA,2) - pow(rB,2) + pow(d,2)) / (2 * d);
echo( x = x);

// size of the cube to subtract to show 
// the intersection of the spheres
cube_size = [10,10,10];

if ( ((d - rA) >= rB) || ( rB >= ( d + rA)) ){
   echo ("Error Y not solvable");
}else{
   y = (( pow(rA,2) - pow(rC,2) + pow(i,2) + pow(j,2)) / (2 * j))
      - ( i / j) * x;
   echo(y = y);
   zpow2 = pow(rA,2) - pow(x,2) - pow(y,2);
   if ( zpow2 < 0){
      echo ("z not solvable");
   }else{
      z = sqrt(zpow2);
      echo (z = z);
      // subtract a cube with one of its corners 
      // at the point where the sphers intersect
      difference(){
         spheres();
         translate ([x,y - cube_size[1],z]){
           cube(cube_size);
         }
      }
      translate ([x,y - cube_size[1],z]){
           %cube(cube_size);
      }
  }
}
0
QuatCoder