C# 3D三边测量法：K1/K2含义与常规坐标转换问询

Context #

You're implementing 3D positioning using three base stations (p1, p2, p3) and their corresponding distance radii (r1, r2, r3), with these example cases:

Example 1 #

P1=(2,2,0), P2=(3,3,0), P3=(1,4,0)，r1=1、r2=1、r3=1.4142，预期结果P=(2,3,0)

Example 2 #

P1=(2,1,0), P2=(4,3,0), P3=(4,4,1)，r1=2、r2=2、r3=2.449，预期结果P=(2,3,0)

Here's your C# implementation:

private void button1_Click(object sender, EventArgs e) { 
    double x1, x2, x3, y1, y2, y3, z1, z2, z3; 
    double.TryParse(textBox1.Text, out x1); 
    double.TryParse(textBox2.Text, out x2); 
    double.TryParse(textBox3.Text, out x3); 
    double.TryParse(textBox4.Text, out y1); 
    double.TryParse(textBox5.Text, out y2); 
    double.TryParse(textBox6.Text, out y3); 
    double.TryParse(textBox7.Text, out z1); 
    double.TryParse(textBox8.Text, out z2); 
    double.TryParse(textBox9.Text, out z3); 
    Vector3D p1 = new Vector3D(x1, y1, z1); 
    Vector3D p2 = new Vector3D(x2, y2, z2); 
    Vector3D p3 = new Vector3D(x3, y3, z3); 
    Vector3D p_1 = new Vector3D(); 
    Vector3D p_2 = new Vector3D(); 
    Vector3D p_3 = new Vector3D(); 
    Vector3D xn = new Vector3D(); 
    Vector3D yn = new Vector3D(); 
    Vector3D zn = new Vector3D(); 
    double r1_pow, r2_pow, r3_pow; 
    double.TryParse(textBox10.Text, out r1_pow); 
    double.TryParse(textBox11.Text, out r2_pow); 
    double.TryParse(textBox12.Text, out r3_pow); 
    //r1_pow = p1.Length; // r2_pow = p2.Length; //r3_pow = p3.Length; 
    double X, Y, Z1, Z2, d, i, j; 
    p_1 = p2 - p1; 
    p_2 = p3 - p1; 
    xn = Vector3D.Divide(p_1, p_1.Length); 
    i = Vector3D.DotProduct(xn, p_2); 
    d = p_1.Length; 
    yn = Vector3D.Divide((p_2 - (i * xn)), (p_2 - (i * xn)).Length); 
    j = Vector3D.DotProduct(yn, p_2); 
    zn = Vector3D.CrossProduct(xn, yn); 
    X = ((r1_pow * r1_pow) - (r2_pow * r2_pow) + (d * d)) / (2 * d); 
    Y = (((r1_pow * r1_pow) - (r3_pow * r3_pow) + (i * i) + (j * j)) / (2 * j)) - ((i / j) * X); 
    double X2 = X * X; 
    double Y2 = Y * Y; 
    Z1 = Math.Sqrt((r1_pow * r1_pow) - Math.Round(X2, 1) - Math.Round(Y2, 1)); 
    Z2 = -Math.Sqrt((r1_pow * r1_pow) - Math.Round(X2, 1) - Math.Round(Y2, 1)); 
    double a, B; 
    Vector3D k2 = new Vector3D(); 
    Vector3D k1 = new Vector3D(); 
    k1 = p1 + ((X * xn) + (Y * yn) + (Z1 * zn)); // k1.Normalize(); 
    k2 = p1 + (X * xn) + (Y * yn) - (Z2 * zn); 
    MessageBox.Show(k1.ToString()); 
} 

Answers to Your Questions #

What do K1 and K2 mean? #

In 3D trilateration, three spheres (each centered at a base station with radius equal to the distance to the target) can intersect at at most two points—these are exactly K1 and K2. They are the two mathematical solutions that satisfy all three sphere equations.

Breaking down your formula:

• You first built a local orthogonal coordinate system using Xn, Yn, and Zn:
  • Xn points from P1 to P2 (normalized)
  • Yn lies in the plane of P1/P2/P3, perpendicular to Xn (normalized)
  • Zn is perpendicular to that plane (calculated via cross product)
• X * Xn + Y * Yn + Z * Zn represents the target point's coordinates in this local system. Adding P1's global coordinates converts this to a global position.
• K1 uses the positive square root Z1, giving one possible intersection point (above the P1/P2/P3 plane)
• K2 uses the negative square root Z2, giving the other possible intersection point (below the P1/P2/P3 plane)

How to get the target point P from K1 and K2? #

Mathematically there are two solutions, but only one will match your real-world scenario. You can select the correct P using context-specific rules:

• For 2D-like scenarios (e.g., all points have Z=0, like Example 1): One of the solutions will have a Z value close to 0 (in Example 1, both K1 and K2 will have Z=0 since the spheres intersect on the 2D plane).
• For true 3D scenarios:
  • Use a known constraint (e.g., the target is near ground level, so pick the solution with Z closest to 0)
  • Add a fourth base station to validate which solution matches the fourth radius
  • Use motion continuity (if tracking a moving target, pick the solution closest to the previous frame's position)

For Example 2, the correct P=(2,3,0) will be the solution with Z=0—you can compare K1 and K2's Z values and select the one that matches this expected constraint.

Quick note: Your code uses Math.Round(X2,1) and Math.Round(Y2,1) when calculating Z1/Z2. This could introduce unnecessary precision errors—consider removing the rounding unless you're explicitly handling floating-point noise.

内容的提问来源于stack exchange，提问作者user9783769