水下定位算法笔记

• 由于项目需求，需要进行对水下设备的定位（几千米深），为此又研究了一下水下定位方式，水下定位与陆地平面定位有少许差别但原理相通。

一、概述

• 水下定位系统包括长基线定位系统（LBL）、短基线定位系统（SBL）和超短基线定位系统（USBL）。
  

1、LBL(长基线)水声定位

• LBL(长基线)水声定位采用的同样是测距定位技术，通过时间测量得到目标与每
  个水声换能器之间的距离，通过计算确定目标的相对位置。
• 而与短基线水声定位不同的是 LBL(长基线)水声定位的水上收发装置只有一个水声换能器，而水声换能器组是安装固定在海底，位于近表层，基线长度通常为数百米到数公里。水上收发装置通过固定在海底位于近底层的水声换能器组对水下收发装置即水声应答器进行定位。
• LBL(长基线)水声定位系统示意图如下图：
  
• LBL(长基线)水声定位系统具有精度高的优点，但是系统较为复杂，成本也较高。

2、SBL(短基线)水声定位

• SBL(短基线)水声定位采用的是测距定位技术，通过时间测量得到目标与每个水声换能器之间的距离，通过计算确定目标的相对位置。SBL(短基线)水声定位水面收发装置采用的是水声换能器组，安装于船体底部，位于海面近表层，基线长度，即水声换能器之间的距离通常为数十米。SBL(短基线)水声定位的水下收发装置是一个水声应答器，一般位于中下层至近底层。
• SBL(短基线)水声定位系统示意图如下图：
  
• SBL(短基线)水声定位系统具有系统操作优势不明显、性价比偏低的特点。

3、USBL(超短基线)水声定位

• USBL(超短基线)水声定位采用的是相控测量技术，通过时间测量确定目标距离，并通过相位测量确定目标的水平方位角度以及垂直掠射角度，结合目标距离以及目标方位即可得到目标的相对位置。
• 水声定位的水面收发装置是水声换能器组，定位基线的长度是指水声换能器之间的间距。
  USBL(超短基线)水声定位采用的是水声换能基阵，即有多个接收探头的水声换能器，安装于船体底部，相当于位于海面近表层，而探头之间的距离通常为几厘米到几十厘米。USBL(超短基线)水声定位的水下收发装置是一个水声换能器，即水声应答器，一般位于深海，即中下层至近底层。USBL(超短基线)水声定位系统示意图如下图：
  
• USBL(超短基线)水声定位系统具有系统简单、成本低等优点，同时也具有定位
  精度相对较低的缺点。

4、项目情况

• 根据项目现有的条件（只具备一个换能器、测距功能），一项都不符合，定位系统的原理非常好， 奈何现有条件不足。
• 不足的原因：
  1、长基线定位系统（LBL）：需要的水下布置众多换能器组。
  2、短基线（SBL）和超短基线（USBL）定位系统：这两个系统都需要换能器组（≥3个），而目前的项目只有一个，同时安装布置角度非常重要，因为算法是与角度与测距时间差密切相关的。
• 没办法，项目需求就得想办法做啊，便采用了很简单的三点定位算法，空间中是3个球形相交确定一点，平面就是3个圆相交的点（由于实施过程中肯定会有误差，因此可能不是一个点，而是一个区域）。
  

二、实施

• 按照3个圆定位的方式，只需要移动3个点（同时获取经纬度），并且在每个点上准确的测出距设备的距离，以这个距离为半径画圆就能够得出交汇区域，当然是不局限于3个点（点越多越精确）。
• 1、使用的是C#开发的Winform，画图采用GDI（因为在远海是没有网络的，地图后期在加，所以采用背景图）；
• 2、经纬度获取是采用GPS+北斗双模模块（1s获取一次），注意获取到的经纬度（WGS84）还需要转换为“度.度”的格式，转换的目的是为了换算经纬度与实际距离，就因此此前没有转换导致换算距离不精确；
• 3、绘制标识刻度，可以根据实际情况变换，其中的难点就是计算一个像素点的代表的实际距离，这个需要同时根据标识的变换而改变；
• 4、每次开始定位后，以当前位置为圆心画圆，当前位置的经纬度就赋值给中心点坐标了，后续的移动，就能够实时的与中心坐标做差，算出经纬差，再转换成实际相差距离，实际相差距离换算为与中心点相差像素点，这样就能做到在图上实时的显示移动的位置；
• 5、再移动两次，同时测两次距离，画两个圆得到交汇处就是实际设备的位置，要知道这个位置的坐标，只需要把鼠标移动到这个交汇区域就能得到坐标值。
• 6、下图是实际开发的效果，在平地上大体模拟测试了一下，交汇处就是实际设备位置，还是比较准确的。放在海洋中，因为布放的设备深度（几千米）是已知的，所以只需要根据勾股定理 “测距^2 = 半径^2 + 深度^2”，换算出测量半径，也是能够画出圆的。
  

三、定位常用公式

1、计算两个经纬度之间的距离

        //地球半径，单位米
        private const double EARTH_RADIUS = 6378137;
        /// <summary>
        /// 计算两点位置的距离，返回两点的距离，单位 米
        /// 该公式为GOOGLE提供，误差小于0.2米
        /// </summary>
        /// <param name="lat1">第一点纬度</param>
        /// <param name="lng1">第一点经度</param>
        /// <param name="lat2">第二点纬度</param>
        /// <param name="lng2">第二点经度</param>
        /// <returns></returns>
        public static double GetDistance(double lat1, double lng1, double lat2, double lng2)
        {
            double radLat1 = Rad(lat1);
            double radLng1 = Rad(lng1);
            double radLat2 = Rad(lat2);
            double radLng2 = Rad(lng2);
            double a = radLat1 - radLat2;
            double b = radLng1 - radLng2;
            double result = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(a / 2), 2) + Math.Cos(radLat1) * Math.Cos(radLat2) * Math.Pow(Math.Sin(b / 2), 2))) * EARTH_RADIUS;
            return result;
        }

        /// <summary>
        /// 经纬度转化成弧度
        /// </summary>
        /// <param name="d"></param>
        /// <returns></returns>
        private static double Rad(double d)
        {
            return (double)d * Math.PI / 180d;
        }

2、经度差换算为距离差

double lngDifferDistance = lngdiffer * 111000 * Math.Cos(lat1);//在除赤道外的其他纬线上，经度差1度对应的实际距离是111*cos纬度,米。

3、纬度差换算为距离差

double latDifferDistance = latdiffer * 111000;    //在经线上纬度差1度对应的实际距离是111千米

4、经纬度格式转换（度.分–>度.度）

//模块输出的是“度.分”格式   经度 11324.420920 纬度：2303.006850，需用下面函数转换成“度.度”格式（00.000000,000.000000）
        double DegreeConvert(double sDegree)
        {
            double dDegree;
            if (sDegree == 0) return 0;
            int integer = (int)sDegree;
            double decimal1 = sDegree - (int)sDegree;
            double min = integer % 100;
            int hour = (int)(integer / 100);
            dDegree = (double)hour + (double)(min / 60) + (double)(decimal1 / 60);
            return dDegree;
        }