跳到主要内容

几何方式解算偏移

2025-04-11 to 2025-04-12

时隔三个月，现在学习了平面向量，我某天想到新的解算漂移的方法

回忆​

之前我要求的是一个三元二次方程组：

{(x1+ox)2+(y1+oy)2+(z1+oz)2=9.82(x2+ox)2+(y2+oy)2+(z2+oz)2=9.82(x3+ox)2+(y3+oy)2+(z3+oz)2=9.82\left\{\begin{matrix} (x_1+o_x)^2+(y_1+o_y)^2+(z_1+o_z)^2=9.8^2\\ (x_2+o_x)^2+(y_2+o_y)^2+(z_2+o_z)^2=9.8^2\\ (x_3+o_x)^2+(y_3+o_y)^2+(z_3+o_z)^2=9.8^2 \end{matrix}\right.

ox, oy, ozo_x,\ o_y,\ o_z 是要求的三个值。

然后，不难看出这三个方程与球的一般方程 (x−Ox)2+(y−Oy)2+(z−Oz)2=R2(x-O_x)^2+(y-O_y)^2+(z-O_z)^2=R^2 相似，只不过一个正负号的区别。所以，该方程组可以看作是三维空间中三个球的交点。

比如之前的这组数据：

(0.03,−0.31,8.88)(−9.49,−0.86,−0.92)(0.71,−10.09,−0.12)(0.03,-0.31,8.88)\\ (-9.49,-0.86,-0.92)\\ (0.71,-10.09,-0.12)\\

其方程组：

{(0.03+ox)2+(−0.31+oy)2+(8.88+oz)2=9.82(−9.49+ox)2+(−0.86+oy)2+(−0.92+oz)2=9.82(0.71+ox)2+(−10.09+oy)2+(−0.12+oz)2=9.82\left\{\begin{matrix} (0.03+o_x)^2+(-0.31+o_y)^2+(8.88+o_z)^2=9.8^2\\ (-9.49+o_x)^2+(-0.86+o_y)^2+(-0.92+o_z)^2=9.8^2\\ (0.71+o_x)^2+(-10.09+o_y)^2+(-0.12+o_z)^2=9.8^2 \end{matrix}\right.

绘制转为球方程，绘制图像：


求解​

注意

现在我学的还是入门立体几何，以下的表述可能有问题。

已知 三个球，球心为 O1, O2, O3O_1,\ O_2,\ O_3 ，半径为 RR，三点坐标：

O1(x1,y1,z1)O2(x2,y2,z2)O3(x3,y3,z3)O_1(x_1,y_1,z_1)\\ O_2(x_2,y_2,z_2)\\ O_3(x_3,y_3,z_3)

求 交点 PP、QQ。

思路​

先算出 ⊙O1⊙O_1 与 ⊙O2⊙O_2 的交平面 α\alpha 和 ⊙O1⊙O_1 与 ⊙O3⊙O_3 的交平面 β\beta 。

再计算 α\alpha 和 β\beta 的交线 ll。

最后 ll 与 ⊙O1⊙O_1 会有两个交点，分别为 PP、QQ。

符号表式为： 求：

P,Q∈⊙O1∩⊙O2∩⊙O3P,Q\in ⊙O_1\cap ⊙O_2\cap ⊙O_3

思路：

⊙O1∩⊙O2⊂α，⊙O1∩⊙O3⊂β,α∩β⊂l,P,Q∈l∩⊙O1.\begin{matrix} ⊙O_1\cap ⊙O_2\subset \alpha ，\\ ⊙O_1\cap ⊙O_3\subset \beta,\\ \alpha \cap \beta \subset l,\\ P,Q\in l\cap ⊙O_1. \end{matrix}

求两圆交平面​

通过法向量O1O2→\overrightarrow {O_1O_2}和相交圆的圆心 BB，计算平面 α\alpha。

沿 O1O2O_1O_2 的切面：


易得：

O1O2→=(x2−x1,y2−y1,z2−z1)B(x1+12(x2−x1),y1+12(y2−y1),z1+12(z2−z1))\overrightarrow {O_1O_2}=(x_2-x_1,y_2-y_1,z_2-z_1)\\ B(x_1+\frac{1}{2}(x_2-x_1),y_1+\frac{1}{2}(y_2-y_1),z_1+\frac{1}{2}(z_2-z_1))

又法向量与平面上任意向量垂直，所以平面 α\alpha 上任意一点 DD：

DB→⋅O1O2→=0[12(x2+x1)−Dx](x2−x1)+[12(y2+y1)−Dy](y2−y1)+[12(z2+z1)−Dz](z2−z1)=0(x1−x2)Dx+(y1−y2)Dy+(z1−z2)Dz=12(x12+y12+z12−x22−y22−z22)\overrightarrow {DB}\cdot \overrightarrow {O_1O_2}=0\\ [\frac{1}{2}(x_2+x_1)-D_x](x_2-x_1)+[\frac{1}{2}(y_2+y_1)-D_y](y_2-y_1)+[\frac{1}{2}(z_2+z_1)-D_z](z_2-z_1)=0\\ (x_1-x_2)D_x+(y_1-y_2)D_y+(z_1-z_2)D_z=\frac{1}{2}(x_1^2+y_1^2+z_1^2-x_2^2-y_2^2-z_2^2)

所以平面：

α:(x1−x2)x+(y1−y2)y+(z1−z2)z+12(x22+y22+z22−x12−y12−z12)=0\alpha: (x_1-x_2)x+(y_1-y_2)y+(z_1-z_2)z+\frac{1}{2}(x_2^2+y_2^2+z_2^2-x_1^2-y_1^2-z_1^2)=0

同理：

β:(x1−x3)x+(y1−y3)y+(z1−z3)z+12(x32+y32+z32−x12−y12−z12)=0\beta: (x_1-x_3)x+(y_1-y_3)y+(z_1-z_3)z+\frac{1}{2}(x_3^2+y_3^2+z_3^2-x_1^2-y_1^2-z_1^2)=0

动态演示（点击下方窗口的右上角第二个选项“Translucent surfaces”可打开半透明）：


证明推理无误。

求交面交线​

刚刚已求得：

α:(x1−x2)x+(y1−y2)y+(z1−z2)z+12(x22+y22+z22−x12−y12−z12)=0β:(x1−x3)x+(y1−y3)y+(z1−z3)z+12(x32+y32+z32−x12−y12−z12)=0\alpha: (x_1-x_2)x+(y_1-y_2)y+(z_1-z_2)z+\frac{1}{2}(x_2^2+y_2^2+z_2^2-x_1^2-y_1^2-z_1^2)=0\\ \beta: (x_1-x_3)x+(y_1-y_3)y+(z_1-z_3)z+\frac{1}{2}(x_3^2+y_3^2+z_3^2-x_1^2-y_1^2-z_1^2)=0

字母对应替换简化为：

ax+by+cz+m=0dx+ey+fz+n=0ax+by+cz+m=0\\ dx+ey+fz+n=0

然后一通演算：

l:y=nc−mfbf−ce+cd−afbf−cex=na−mdbd−ae+af−cdbd−aezl:y=\frac{nc-mf}{bf-ce}+\frac{cd-af}{bf-ce}x=\frac{na-md}{bd-ae}+\frac{af-cd}{bd-ae}z

动态演示（红色为 ll 方程，左侧小箭头可展开方程式）：


演算无误

求线与球交点​

化简

l:y=nc−mfbf−ce+cd−afbf−cex=na−mdbd−ae+af−cdbd−aezl:y=\frac{nc-mf}{bf-ce}+\frac{cd-af}{bf-ce}x=\frac{na-md}{bd-ae}+\frac{af-cd}{bd-ae}z

为

l:y=k1x+b1=k2z+b2l:y=k_1x+b_1=k_2z+b_2

与 ⊙O1⊙O_1 联立：

(1k12+1+1k22)y2−[2(b1+k1x1k12+y1+b2+k2z1k22)]y+[(b1+k1x1)2k12+y12+(b2+k2z1)2k22−R2]=0(\frac{1}{k_1^2} + 1 + \frac{1}{k_2^2})y^2-[2(\frac{b_1 + k_1 x_1}{k_1^2} + y_1 + \frac{b_2 + k_2 z_1}{k_2^2})]y+[\frac{(b_1 + k_1 x_1)^2}{k_1^2} + y_1^2 + \frac{(b_2 + k_2 z_1)^2}{k_2^2} - R^2]=0

所以这个二次方程的系数为：

{A=1k12+1+1k22 B=2(b1+k1x1k12+y1+b2+k2z1k22) C=(b1+k1x1)2k12+y12+(b2+k2z1)2k22−R2\left \{ \begin{matrix} A=\frac{1}{k_1^2} + 1 + \frac{1}{k_2^2}\\ \ \\ B=2(\frac{b_1 + k_1 x_1}{k_1^2} + y_1 + \frac{b_2 + k_2 z_1}{k_2^2})\\ \ \\ C=\frac{(b_1 + k_1 x_1)^2}{k_1^2} + y_1^2 + \frac{(b_2 + k_2 z_1)^2}{k_2^2} - R^2 \end{matrix}\right .

编写程序​

太累了，直接使用 AI：

#include <Arduino.h>
#include <math.h>

// 定义三维向量结构体
struct Vector3 {
    float x, y, z;
};

// 向量运算函数
Vector3 subtract(const Vector3& a, const Vector3& b) {
    return {a.x - b.x, a.y - b.y, a.z - b.z};
}

float dot(const Vector3& a, const Vector3& b) {
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

Vector3 cross(const Vector3& a, const Vector3& b) {
    return {
        a.y * b.z - a.z * b.y,
        a.z * b.x - a.x * b.z,
        a.x * b.y - a.y * b.x
    };
}

// 计算两点间距离
float distance(const Vector3& a, const Vector3& b) {
    Vector3 diff = subtract(a, b);
    return sqrtf(dot(diff, diff));
}

// 几何法求解偏移量，返回两个解
bool solve_offset(const Vector3& O1, const Vector3& O2, const Vector3& O3, float R, Vector3& offset1, Vector3& offset2) {
    // 计算平面 α (O1 和 O2)
    Vector3 n1 = subtract(O2, O1);
    float d1 = (dot(O2, O2) - dot(O1, O1)) / 2.0f;

    // 计算平面 β (O1 和 O3)
    Vector3 n2 = subtract(O3, O1);
    float d2 = (dot(O3, O3) - dot(O1, O1)) / 2.0f;

    // 计算交线方向向量
    Vector3 dir = cross(n1, n2);
    float dir_len = sqrtf(dot(dir, dir));
    if (dir_len < 1e-6f) return false; // 平面平行
    dir = {dir.x / dir_len, dir.y / dir_len, dir.z / dir_len};

    // 寻找交线上一点（设 z = 0）
    Vector3 point;
    float denom = n1.x * n2.y - n1.y * n2.x;
    if (fabsf(denom) > 1e-6f) {
        point.x = (n2.y * d1 - n1.y * d2) / denom;
        point.y = (n1.x * d2 - n2.x * d1) / denom;
        point.z = 0;
    } else {
        return false; // 无法求解
    }

    // 直线与球面交点
    Vector3 oc = subtract(point, O1);
    float a = dot(dir, dir); // 归一化后应为 1
    float b = 2.0f * dot(oc, dir);
    float c = dot(oc, oc) - R * R;
    float discriminant = b * b - 4.0f * a * c;

    if (discriminant < 0) return false; // 无实交点

    float sqrt_d = sqrtf(discriminant);
    float t1 = (-b - sqrt_d) / (2.0f * a);
    float t2 = (-b + sqrt_d) / (2.0f * a);

    // 计算交点坐标
    Vector3 p1 = {point.x + t1 * dir.x, point.y + t1 * dir.y, point.z + t1 * dir.z};
    Vector3 p2 = {point.x + t2 * dir.x, point.y + t2 * dir.y, point.z + t2 * dir.z};

    // 计算偏移量
    offset1 = subtract(p1, O1);
    offset2 = subtract(p2, O1);

    return true;
}

void setup() {
    Serial.begin(115200);
}

void loop() {
    // 三个测量点
    Vector3 O1 = {0.03f, -0.31f, 8.88f};
    Vector3 O2 = {-9.49f, -0.86f, -0.92f};
    Vector3 O3 = {0.71f, -10.09f, -0.12f};
    float R = 9.8f;

    // 第四个球心（从用户提供的数据中选择）
    Vector3 O4 = {0.70f, -4.33f, 8.05f}; // 选择 (0.70, -4.33, 8.05)

    Vector3 offset1, offset2;
    if (solve_offset(O1, O2, O3, R, offset1, offset2)) {
        // 计算交点 P 和 Q
        Vector3 P = {O1.x + offset1.x, O1.y + offset1.y, O1.z + offset1.z};
        Vector3 Q = {O1.x + offset2.x, O1.y + offset2.y, O1.z + offset2.z};

        // 计算 O4 到 P 和 Q 的距离
        float dist_P = distance(P, O4);
        float dist_Q = distance(Q, O4);

        // 打印交点 P 和相关信息
        Serial.println("交点 P: (" + String(P.x, 6) + ", " + String(P.y, 6) + ", " + String(P.z, 6) + ")");
        Serial.println("偏移量 offset1: (" + String(offset1.x, 6) + ", " + String(offset1.y, 6) + ", " + String(offset1.z, 6) + ")");
        Serial.println("到 O4 的距离: " + String(dist_P, 6));

        // 打印交点 Q 和相关信息
        Serial.println("交点 Q: (" + String(Q.x, 6) + ", " + String(Q.y, 6) + ", " + String(Q.z, 6) + ")");
        Serial.println("偏移量 offset2: (" + String(offset2.x, 6) + ", " + String(offset2.y, 6) + ", " + String(offset2.z, 6) + ")");
        Serial.println("到 O4 的距离: " + String(dist_Q, 6));

        // 选择最优解
        float error_P = fabsf(dist_P - R);
        float error_Q = fabsf(dist_Q - R);
        if (error_P < error_Q) {
            Serial.println("最优解: 交点 P (offset1)");
        } else {
            Serial.println("最优解: 交点 Q (offset2)");
        }
    } else {
        Serial.println("未找到有效解。");
    }
    delay(1000); // 每秒输出一次
}

上传单片机输出

交点 P: (0.295723, -0.331201, -0.916374)
偏移量 offset1: (0.265723, -0.021201, -9.796374)
到 O4 的距离: 9.825971
交点 Q: (-6.383689, -7.117166, 5.953041)
偏移量 offset2: (-6.413689, -6.807166, -2.926960)
到 O4 的距离: 7.895833
最优解: 交点 P (offset1)

(0.295723,−0.331201,−0.916374)(0.295723, -0.331201, -0.916374) 与之前算出来的 (−0.2957,0.3312,0.9163)(-0.2957,0.3312,0.9163) 一样，太棒了！

艰苦历程​

从昨天这个点干到现在，四点半睡觉，八点半起床，弄完程序弄数学。现在已经 22:56 了！

作业没写一点，Steam 也没打开，太他妈有意思了。