Algorithm 三边测量和定位点（x、y、z）
我想找到一个未知节点的坐标，它位于空间的某个地方，它的参考距离是3个或更多的节点，它们都有已知的坐标 这个问题和这里描述的三边测量完全一样 然而，我不理解“初步和最终计算”部分（参考维基百科网站）。我找不到P1，P2和P3的位置，这样我就可以把它们放在方程里了Algorithm 三边测量和定位点（x、y、z）,algorithm,localization,geometry,triangulation,trilateration,Algorithm,Localization,Geometry,Triangulation,Trilateration,我想找到一个未知节点的坐标，它位于空间的某个地方，它的参考距离是3个或更多的节点，它们都有已知的坐标 这个问题和这里描述的三边测量完全一样 然而，我不理解“初步和最终计算”部分（参考维基百科网站）。我找不到P1，P2和P3的位置，这样我就可以把它们放在方程里了 感谢三边测量是找到三个球体相交区域中心的过程。三个球体的中心点和半径必须已知 让我们考虑你的三个例子中心点P1[-1,1]，P2[1,1]，和P3[-1，-1 ]。第一个要求是P1'位于原点，因此让我们通过向所有三个点添加偏移向量V[1，
感谢三边测量是找到三个球体相交区域中心的过程。三个球体的中心点和半径必须已知
让我们考虑你的三个例子中心点P1[-1,1]，P2[1,1]，和P3[-1，-1 ]。第一个要求是P1'位于原点，因此让我们通过向所有三个点添加偏移向量V[1，-1]来相应地调整点：
P1' = P1 + V = [0, 0]
P2' = P2 + V = [2, 0]
P3' = P3 + V = [0,-2]
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
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
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]
A dot B = ||A|| ||B|| cos(theta)
theta = acos(Ax / ||A||)
theta = acos(P2'x / ||P2'||)
theta = acos(1 / sqrt(2)) = 45 degrees
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]
要撤消旋转，请将P4''乘以R（-θ），在本例中为R（45）。要撤消转换，请减去偏移向量V，这与添加P1相同（假设您最初使用-P1作为V）。这是我在3D打印机固件中使用的算法。它避免了旋转坐标系，但可能不是最好的 三边测量问题有两种解决方案。为了得到第二个，在二次方程解中将“-sqrtf”替换为“+sqrtf” 显然，如果您有足够的处理器能力和内存，您可以使用double而不是float
// 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;
以下是维基百科的计算，以OpenSCAD脚本的形式呈现，我认为这有助于以直观的方式理解问题，并提供了一种检查结果是否正确的简单方法
//三边测量示例
//来自维基百科
//
//pA、pB和pC是球体的中心
//如有必要，球体必须平移
//并旋转，以便：
//--所有z值均为0
//--爸爸在原点
pA=[0,0,0]；
//--pB位于x轴上
pB=[10,0,0]；
pC=[9,7,0]；
//rA、rB和rC是球体的半径
rA=9；
rB=5；
rC=7；
if（pA！=[0,0,0]）{
回波（“错误：pA必须在原点”）；
断言（假）；
}
如果（（pB[2]！=0）| pC[2]！=0）{
回波（“错误：所有球体中心必须在z=0平面内”）；
断言（假）；
}
如果（pB[1]！=0）{
回波（“pB中心必须在x轴上”）；
断言（假）；
}
//显示球体
模块（）{
翻译（pA）{
球体（r=rA，$fn=rA*10）；
}
翻译（pB）{
球面（r=rB，$fn=rB*10）；
}
翻译（电脑）{
球体（r=rC，$fn=rC*10）；
}
}
函数单位_向量（v）=v/范数（v）；
ex=单位向量（pB-pA）；
回波（ex=ex）；
i=ex*（pC-pA）；
回声（i=i）；
ey=单位向量（pC-pA-i*ex）；
回声（ey=ey）；
d=标准值（pB-pA）；
回声（d=d）；
j=ey*（pC-pA）；
回波（j=j）；
x=（pow（rA，2）-pow（rB，2）+pow（d，2））/（2*d）；
回波（x=x）；
//要减去以显示的多维数据集的大小
//球体的交点
立方体大小=[10,10,10]；
如果（（（d-rA）>=rB）|（rB>=（d+rA）））{
echo（“错误Y不可解决”）；
}否则{
y=（（pow（rA，2）-pow（rC，2）+pow（i，2）+pow（j，2））/（2*j））
-（i/j）*x；
回波（y=y）；
zpow2=pow（rA，2）-pow（x，2）-pow（y，2）；
if（zpow2<0）{
回声（“z不可解”）；
}否则{
z=sqrt（zpow2）；
回波（z=z）；
//用一个角减去一个立方体
//在球体相交的点处
差异（）{
球体（）；
平移（[x，y-立方体大小[1]，z]）{
立方体（立方体大小）；
}
}
平移（[x，y-立方体大小[1]，z]）{
%立方体（立方体大小）；
}
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]
// 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;
// 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);
}
}
}