三边测量定位算法C语言实现

三边定位算法

三边定位算法简介

三个位置已知点（锚节点，圆心）以及其到另外一个未知点（待定位点）的距离即半径（不准确），求位置节点坐标的过程；

实际上（例如用rssi算距离）三个点到未知点的距离半径很可能不交与一点；怎么办呢？

参考文章 https://www.jianshu.com/p/94475aacc95a

这里引用下方法的逻辑：

两圆不相交时，按比例取中点（和）。当两圆相交时，就是拆分成几个三角形，通过一系列三级函数计算出未知的两个交点。最后将三点连成三角形，此三角形的重心（即点M）就是最终定位点，步骤如下：

通过勾股定律用a、b长度计算出线段AB长度（即点A到点B距离），使用 ra + rb 与AB对比即可得知两圆的对应情况，一共有三种情况：两圆相离ra + rb < AB、两圆相切ra + rb == AB、两圆相交ra + rb > AB。
两圆相离：按照两圆半径的比例在线段AZ上求点，即；因为“两圆相切ra + rb == AB”在实际程序中出现的几率太小，所以直接使用“两圆相离”相同的求法。
两圆相交：求出相交点C的坐标 {Cx, Cy}，可通过得出Q1，通过得出Q2，最后计算出点C的坐标：

同理可求出点D的坐标。得到C、D两交点后取距离圆心Z点近的交点作为最后三个参考点中的一点。
将最后求得的三个参考点连接成一个三角形，该三角形的重心即为最后的定位点M：

作者：Hank_Zhong
链接：https://www.jianshu.com/p/94475aacc95a
来源：简书

发现的问题

但是这篇文章对应的如下代码好像不太对，，
具体就是Eo=0.5*EF那里（第11行），这应该是一种工程近似，在两圆相交的很少的情况之下可以这样近似，可实际情况复杂多变，两个圆相交很多的情况很常见；
总要判断相对位置正负号很麻烦；

-(CGPoint)sidePointCalculationWith:(double)x1 :(double)y1 :(double)r1 
                                  :(double)x2 :(double)y2 :(double)r2 
                                  :(double)x3 :(double)y3{
    //勾股定理  sqrt(X)是X开根号  pow（X,n）是X的n次方
    //取beacon1圆心A 与 beacon2圆心B的距离
    double AB = sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));
    double rAB = (r1 + r2);
    if (rAB > AB && (r1 < AB && r2 < AB)) {
        //两圆有相交点,两圆相交点为C、D。两圆与AB的相交点为E、F。o是EF的中点。
        double EF = rAB - AB;
        double Eo = EF * 0.5;
        double AE = r1 - EF;
        double Ao = AE + Eo;
        double AQ1 = acos((x2 - x1) / AB);
        double AQ2 = acos(Ao / r1);
        
        double BF = r2 - EF;
        double Bo = BF + Eo;
//        double BQ1 = acos(fabs(x1 - x2) / AB);
        double BQ2 = acos(Bo / r2);
        
        //原点{0,0}在左上角的情况下
        double Cx = x1 + (r1 * cos(AQ1 + AQ2));
        double Cy = 0.0;
        double Dx = x2 - (r2 * cos(AQ1 + BQ2));
        double Dy = 0.0;
        if (x1 < x2) {
            Dx = x2 - (r2 * cos(AQ1 + BQ2));
            if (y1 < y2) {
                Cy = y1 + (r1 * sin(AQ1 + AQ2));
                Dy = y2 - (r2 * sin(AQ1 + BQ2));
            }else{
                Cy = y1 - (r1 * sin(AQ1 + AQ2));
                Dy = y2 + (r2 * sin(AQ1 + BQ2));
            }
        }else{
            Cy = y1 + (r1 * sin(AQ1 + AQ2));
            if (y1 < y2) {
                Dy = y2 - (r2 * sin(AQ1 + BQ2));
            }else{
                Dy = y2 + (r2 * sin(AQ1 + BQ2));
            }
        }
        
        double Cc = sqrt(pow(Cx - x3, 2) + pow(Cy - y3, 2));
        double Dc = sqrt(pow(Dx - x3, 2) + pow(Dy - y3, 2));

        return Cc < Dc ? CGPointMake(Cx, Cy) : CGPointMake(Dx, Dy);
    }else{
        //两圆无相交点
        return [self midpointCalculationWith:x1 :y1 :r1
                                            :x2 :y2 :r2];
    }
}

重新按照这个逻辑写

思路

仍然是从三个点之中取出两个点，求这两个点之间的参考点，两两之间求，求三次得到三个参考点再取重心；原代码中的两圆相离或相切部分没有问题，不再赘述，主要关注两圆相交求参考点的问题；
其中有两个子问题：

1：求两圆的交点

参考这篇：求解两圆相交的交点坐标

利用其中的方法二，可以避免分母为零的情况；由两圆的交点可以得到两圆之间交点所连的线段的公式（两点式）；

2：求线段上一点到线段外一点距离最小

这个可以把线段转换为一元参数式求解，得到的二次方程可以通过一元二次公式求解析解的；

代码

这个函数需要被调用三次的，每次求的是两个已知点之间的参考点；

//C语言函数 输入三个已知点的坐标以及两个圆的半径
struct ref_p Find_Position(double x1,double x2,double x3,double y1,double y2,double y3,double r1,double r2)
{//sqrt(X)是X开根号  pow（X,n）是X的n次方
    struct ref_p tmp;
    double AB = sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));//两个圆心之间的距离
//     r1=r1>AB?AB:r1;
//     r2=r2>AB?AB:r2;
    double rAB = (r1 + r2);
//     if (rAB > AB && (r1 < AB && r2 < AB)) // 如果一个圆半径大于两个圆心的距离则认为相离，这使得更大半径圆的信任程度降低
      if (rAB > AB && (r1 < (AB+r2) && r2 < (AB+r1))) //如果两圆相交
    {        
        double a=(pow(r1,2)-pow(r2,2)+pow(AB,2))/(2*AB); //求两个圆的交点坐标
        double x0=x1+(x2-x1)*a/AB;
        double y0=y1+(y2-y1)*a/AB;
        double hdr=sqrt(pow(r1,2)-pow(a,2))/AB; //表示h和d的比值rate
        double Cx=x0-hdr*(y2-y1);// 两圆交点坐标
        double Cy=y0+hdr*(x2-x1);
        double Dx=x0+hdr*(y2-y1);
        double Dy=y0-hdr*(x2-x1);

        double t=-((Dx-Cx)*(Cx-x3)+(Dy-Cy)*(Cy-y3))/(pow(Dx-Cx,2)+pow(Dy-Cy,2));//根据一元二次方程解析解的求解公式列出
        t=t<=0?0:(t>=1?1:t); //为了保证所求点在线段上，t=0或1时为线段的两个端点，t超过了0,1之间的话代表在线段之外，就让其变成端点
        printf("t=%f\n",t);
        tmp.ref_x = Cx+t*(Dx-Cx); //通过参数方程的t求参考点
        tmp.ref_y = Cy+t*(Dy-Cy);
        return tmp;
    }
    else
    {
        //两圆相切或相离
    double a = y1 - y2;//竖边
    double b = x1 - x2;//横边
    double rr = r1 + r2;
    double s = r1 / rr;
    
    double x = fabs(x1 - (b * s)) ;
    double y = fabs(y1 - (a * s)) ;
    tmp.ref_x = x;
    tmp.ref_y = y;
    return tmp;
       
    }
}

这样不用担心分母为零的情况，以及判断两圆的相对位置决定正负号什么的，还可以求出比较准确的解析解，代码短一些；

ps：第一次写，写的不好也没啥营养，，，欢迎大家批评指正~