【无标题】

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

【UWB】三边测量法的原理与实现代码c#

前言

随着uwb无线定位技术的发展，PDOA和TOA定位逐渐应用于矿道及园区等场景定位中。三边定位算法则是uwb算法中常用的算法

一、三边定位原理

用户使用三个基站来分别测量出标签距离三个基站的距离，分别为r1,r2,r3。根据测距结果，我们分别用基站为圆心，距离位半径进行画圆，三个圆的交点处即为标签的位置

通过上述公式，可以很容易的计算出标签的位置。但是基于实际场景中仍然会存在定位误差。
例如：
1.测距过程中，物体遮挡导致距离偏差。
2.标签处于基站的包络范围以外。
以上误差就要求我们基站部署时尽量能保证标签和基站的视距范围显示。
误差处理：
可以使用多个基站进行测量，使用质心算法来纠正偏差。
使用卡尔曼滤波的方式保证运动标签稳定变化不会出现激变的情况。
使用pdoa算法可以根据当前标签所在的基站范围角度来进行误差纠偏

二、使用步骤

1.定好坐标系

根据当前基站的位置，选择基站坐标系，定好XY轴。同时标定好基站的坐标信息。

2.测量标签对应每个基站的距离信息

固定好标签的位置，测量出各个基站的距离信息。

3.计算代码

 public static double[] GetLocation(Point anchor1, Point anchor2, Point anchor3, int r1, int r2, int r3)
        {
            double[,] A = new double[2, 2];
            double[,] B = new double[2, 2];
            double[] C = new double[2];
            double[] pointXY = new double[2];

            A[0, 0] = 2 * (anchor1.X - anchor2.X); A[0, 1] = 2 * (anchor1.Y - anchor2.Y);
            A[1, 0] = 2 * (anchor1.X - anchor3.X); A[1, 1] = 2 * (anchor1.Y - anchor3.Y);
            
            double det = 0;
            det = A[0, 0] * A[1, 1] - A[1, 0] * A[0, 1];
            if (det != 0)
            {
                B[0, 0] = A[1, 1] / det;
                B[0, 1] = -A[0, 1] / det;


                B[1, 0] = -A[1, 0] / det;
                B[1, 1] = A[0, 0] / det;

                C[0] = r2 * r2 - r1 * r1 - Math.Pow(anchor2.X, 2) + Math.Pow(anchor1.X, 2) - Math.Pow(anchor2.Y, 2) + Math.Pow(anchor1.Y, 2);
                C[1] = r3 * r3 - r1 * r1 - Math.Pow(anchor3.X, 2) + Math.Pow(anchor1.X, 2) - Math.Pow(anchor3.Y, 2) + Math.Pow(anchor1.Y, 2);

                pointXY[0] = B[0, 0] * C[0] + B[0, 1] * C[1];
                pointXY[1] = B[1, 0] * C[0] + B[1, 1] * C[1];

                //Point_xy[0] = Math.Round(B[0, 0] * C[0] + B[0, 1] * C[1], 2)/100;
                //Point_xy[1] = Math.Round(B[1, 0] * C[0] + B[1, 1] * C[1], 2)/100;

                return pointXY;
            }
            else
            {
                pointXY[0] = 0;
                pointXY[1] = 0;

                return pointXY;
            }

总结

通过以上算法即可得出标签位置，提示：使用该算法 基站之间需要间隔一米以上否则则定位不出来。