Trilateration三邊測量定位算法

轉載自Jiaxing / 2014年2月22日

基本原理

Trilateration（三邊測量）是一種常用的定位算法：

1. 已知三點位置 (x₁, y₁), (x₂, y₂), (x_3, y₃)
2. 已知未知點 (x₀, y₀) 到三點距離 d₁, d₂, d₃

以 d₁, d₂, d₃ 為半徑作三個圓，根據畢達哥拉斯定理，得出交點即未知點的位置計算公式：

( x1 - x0 )2 + ( y1 - y0 )2 = d12 ( x2 - x0 )2 + ( y2 - y0 )2 = d22 ( x3 - x0 )2 + ( y3 - y0 )2 = d32

解法推導

設未知點位置為 (x, y)， 令其中的第一個球形 P₁ 的球心坐標為 (0, 0)，P₂ 處於相同縱坐標，球心坐標為 (d, 0)，P₃ 球心坐標為 (i, j)，三個球形半徑分別為 r₁, r₂, r₃，z為三球形相交點與水平面高度。則有：

r12 = x2 + y2 + z2 r22 = (x - d)2 + y2 + z2 r32 = (x - i)2 + (y - j)2 + z2

當 z = 0 時， 即為三個圓在水平面上相交為一點，首先解出 x:

x = (r12 - r22 + d2) / 2d

將公式二變形，將公式一的 z² 代入公式二，再代入公式三得到 y 的計算公式：

y = (r12 - r32 - x2 + (x - i)2 + j2) / 2j

D3.js實現

定義三個圓的坐標及半徑，計算出交點的坐標 (obj_x, obj_y).

var x_0 = 150, y_0 = 150; var x_1 = x_0, y_1 = y_0, d = 150, x_2 = x_0 + d, x_3 = 225, y_3 = 315, r = 100; var i = x_3 - x_0, j = y_3 - y_0; var x = (Math.pow(r, 2) - Math.pow(r, 2) + Math.pow(d, 2)) / (2 * d) + x_0; var obj_x = x + x_0; var y = (Math.pow(r, 2) - Math.pow(r, 2) - Math.pow(x, 2) + Math.pow((x - i), 2) + Math.pow(j, 2)) / (2 * j); var obj_y = y + y_0;

繪出圓形及交點：

svg.append("circle").attr("cx", x_1) .attr("cy", y_1).attr("r", r) .style("fill", "blue").style("opacity", 0.3); svg.append("circle").attr("cx", x_2) .attr("cy", y_0).attr("r", r) .style("fill", "red").style("opacity", 0.4); svg.append("circle").attr("cx", x_3) .attr("cy", y_3).style("opacity", 0.5) .attr("r", r).style("fill", "yellow"); svg.append("circle").attr("cx", obj_x) .attr("cy", obj_y).attr("r", 3) .style("fill", "red");

但在實際定位中，給定的距離由於測量的誤差，並不能真正讓三個圓交於一點，需要在相交區域繪制出矩形並計算矩形的中心位置，除三邊測量計算公式之外，也許還可以采用K-Means或Overlap聚類算法，后兩者在地圖標注上使用比較廣，有興趣的可以進一步對定位算法進行研究。

參考

A robust localization algorithm in wireless sensor networks
Calculating the intersection area of 3+ circles
Clustering Map Symbols