From c75a78b0dae2c937f5f53561e95b9ed7673fd32b Mon Sep 17 00:00:00 2001 From: fl <3098731433@qq.com> Date: Fri, 28 Mar 2025 17:49:11 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B9=E4=BD=8D=E8=A7=92=E8=AE=A1=E7=AE=97?= =?UTF-8?q?=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/main/AndroidManifest.xml | 2 + .../com/bonus/uav/service/RadarRunnable.java | 4 +- .../java/com/bonus/uav/utils/RTKUtils.java | 60 ++++++++++++++++--- .../bonus/uav/utils/SerialConnectUtils.java | 53 ++++++++++++---- .../bonus/uav/utils/sag/RadarMathUtil.java | 33 ++++++---- 5 files changed, 118 insertions(+), 34 deletions(-) diff --git a/app/src/main/AndroidManifest.xml b/app/src/main/AndroidManifest.xml index ffe79bd..f395248 100644 --- a/app/src/main/AndroidManifest.xml +++ b/app/src/main/AndroidManifest.xml @@ -14,6 +14,8 @@ + + diff --git a/app/src/main/java/com/bonus/uav/service/RadarRunnable.java b/app/src/main/java/com/bonus/uav/service/RadarRunnable.java index 71877e8..33789f7 100644 --- a/app/src/main/java/com/bonus/uav/service/RadarRunnable.java +++ b/app/src/main/java/com/bonus/uav/service/RadarRunnable.java @@ -198,8 +198,8 @@ public class RadarRunnable implements Runnable { */ List filteredList = scanDistanceData.stream() - .filter(it -> it.getDistance() != 0.0 && it.getDistance() > 0.05 && it.getDistance() < 4.0 - && ((it.getAngle() > 14.8 && it.getAngle() < 90.3))) + .filter(it -> it.getDistance() != 0.0 && it.getDistance() > 1.0 && it.getDistance() < 6 + && ((it.getAngle() > 14.8 && it.getAngle() < 80))) .sorted(Comparator.comparing(RadarPointBean::getAngle)) .collect(Collectors.toList()); // 输出结果 diff --git a/app/src/main/java/com/bonus/uav/utils/RTKUtils.java b/app/src/main/java/com/bonus/uav/utils/RTKUtils.java index 9c368a2..d39aeb2 100644 --- a/app/src/main/java/com/bonus/uav/utils/RTKUtils.java +++ b/app/src/main/java/com/bonus/uav/utils/RTKUtils.java @@ -81,9 +81,9 @@ public class RTKUtils { //测点经度 //bearing 不转换的方位角 //天线1的经纬度 - double lon2 = lon1 + 0.389 * Math.sin(((bearing-10)+0)/180*Math.PI)/Rq/Math.cos(lat1/180*Math.PI)*180/Math.PI; + double lon2 = lon1 + 0.409 * Math.sin(((bearing-14.3)+0)/180*Math.PI)/Rq/Math.cos(lat1/180*Math.PI)*180/Math.PI; //测点纬度 - double lat2 = lat1 + (0.389*Math.cos(((bearing-10)+0)/180*Math.PI)/Rq)*180/Math.PI; + double lat2 = lat1 + (0.409 * Math.cos(((bearing-14.3)+0)/180*Math.PI)/Rq)*180/Math.PI; double h2 = alt1; System.out.printf("雷达点经纬度：lat：%.8f; lon：%.8f; 高度：%.2f; 方位角：%.2f\n", lat2, lon2, h2,bearing); return new double[]{lat2,lon2,h2}; @@ -93,7 +93,7 @@ public class RTKUtils { * 根据两个点的经纬度，计算两点之间某点的经纬度 */ public static double[] calculatePoint(double lat1, double lon1, double alt1, - double lat2, double lon2, double alt2,float bearing) { + double lat2, double lon2, double alt2,double bearing) { double[] ecef1 = convertToECEF(lat1, lon1, alt1); double[] ecef2 = convertToECEF(lat2, lon2, alt2); @@ -126,7 +126,7 @@ public class RTKUtils { } // 计算方位角 - public static float calculateBearing(double lat1, double lon1, double lat2, double lon2) { + public static double calculateBearing(double lat1, double lon1, double lat2, double lon2) { Location antenna1 = new Location("antenna1"); antenna1.setLatitude(lat1); antenna1.setLongitude(lon1); @@ -135,13 +135,57 @@ public class RTKUtils { antenna2.setLatitude(lat2); antenna2.setLongitude(lon2); - float du = antenna1.bearingTo(antenna2)+369; - -// return du-276; -// return du-290; + double du = antenna1.bearingTo(antenna2)+369; + // 规范化处理，使结果在0到360度之间 +// double du = ((antenna1.bearingTo(antenna2) + 369) % 360); return du; } + // 计算方位角 + public static double calculateBearing2(double lat1, double lon1, double lat2, double lon2) { + double d1 = lon1/180*Math.PI; + double d2 = lat1/180*Math.PI; + double d3 = lon2/180*Math.PI; + double d4 = lat2/180*Math.PI; + double y = Math.sin(-d1+d3) * Math.cos(d4); + double x = Math.cos(d2)*Math.sin(d4) - Math.sin(d2)*Math.cos(d4)*Math.cos(-d1+d3); + double du = Math.atan2(y,x) * 180/Math.PI; + return normalizeAngle(du); + } + + /** + * 调整航向角在0-360度之间 + * @param x + * @return + */ + public static double normalizeAngle(double x) { + x = x % 360; // 先对360取模 + if (x < 0) { + x += 360; // 如果结果为负，则加360度调整 + } + return x; + } + + public static double calculateBearing3(double lat1, double lon1, double lat2, double lon2) { + // 将纬度和经度从度数转换为弧度 + double φ1 = Math.toRadians(lat1); + double φ2 = Math.toRadians(lat2); + double Δλ = Math.toRadians(lon2 - lon1); + + // 计算Δφ和Δλ的差值 + double y = Math.sin(Δλ) * Math.cos(φ2); + double x = Math.cos(φ1) * Math.sin(φ2) - Math.sin(φ1) * Math.cos(φ2) * Math.cos(Δλ); + + // 计算初始方位角（以弧度为单位） + double bearing = Math.atan2(y, x); + + // 转换为度数 + bearing = Math.toDegrees(bearing); + + // 规范化处理，使结果在0到360度之间 + return (double) ((bearing + 360) % 360); + } + /** * 计算两个三维点之间的方向向量并计算方位角和仰角 */ diff --git a/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java b/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java index 1d7ae94..ed17832 100644 --- a/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java +++ b/app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java @@ -177,6 +177,35 @@ public class SerialConnectUtils { }); } + public static void main(String[] args) { + double lonC=117.16341925; + double latC=31.78579748; + double altC=0.0; + double lon=117.16341193; + double lat=31.78579944; + double alt=0.0; + // 计算方位角 + double bearing = RTKUtils.calculateBearing2(latC, lonC, lat, lon); + System.out.println("BBB雷达方位角: " + bearing + "°"); +// double[] midPoint = RTKUtils.calculatePoint(lat, lon, alt, latC, lonC, altC, bearing); + double[] midPoint = RTKUtils.calculateMidPoint2(latC, lonC, altC, bearing); + System.out.println("雷达点纬度: " + midPoint[0]); + System.out.println("雷达点经度: " + midPoint[1]); + double midPointHight = midPoint[2]; + System.out.println("雷达点高度: " + midPoint[2]); + //所以传感器结果 + SensorDataBean sensorDataBean = new SensorDataBean(); + sensorDataBean.setLon(midPoint[1]); + sensorDataBean.setLat(midPoint[0]); + sensorDataBean.setH(midPointHight); + sensorDataBean.setAzimuthAngle(bearing); + System.out.printf("RTK：lat：%.8f; lon：%.8f; 高度：%.2f\n", sensorDataBean.getLat(), sensorDataBean.getLon(), sensorDataBean.getH()); + RadarPointBean radarPointBean = new RadarPointBean(); + radarPointBean.setDistance(1.53); + radarPointBean.setAngle(52.85); + Coordinate coordinate = RadarMathUtil.transferPoint2(radarPointBean, sensorDataBean); + } + /** * 传感器返回值回调 * @@ -277,21 +306,25 @@ public class SerialConnectUtils { } if (latC != 0.0 && lonC != 0.0 && lat != 0.0 && lon != 0.0){ //主天线、从天线都有数据 先计算中点坐标 - // 计算中心点 -// double[] midPoint = RTKUtils.calculateMidPoint(lat, lon, alt, latC, lonC, altC); -// System.out.println("中心点纬度: " + midPoint[0]); -// System.out.println("中心点经度: " + midPoint[1]); -// System.out.println("中心点高度: " + midPoint[2]); // 计算方位角 - float bearing = RTKUtils.calculateBearing(lat, lon, latC, lonC); + double bearing = RTKUtils.calculateBearing2(lat, lon, latC, lonC); System.out.println("BBB雷达方位角: " + bearing + "°"); logger.log("ABCD雷达方位角：", bearing+ "°"); - double[] midPoint = RTKUtils.calculatePoint(lat, lon, alt, latC, lonC, altC, bearing); - RTKUtils.calculateMidPoint2(lat, lon, alt,bearing); + // 算法一，计算中心点在偏移高差计算雷达点。 +// double[] midPoint = RTKUtils.calculateMidPoint(lat, lon, alt, latC, lonC, altC); +// System.out.println("中心点纬度: " + midPoint[0]); +// System.out.println("中心点经度: " + midPoint[1]); +// double midPointHight = midPoint[2] - 0.3; +// System.out.println("中心点高度: " + midPointHight); + + // 算法二，以天线一与雷达方位计算雷达点。 +// double[] midPoint = RTKUtils.calculatePoint(lat, lon, alt, latC, lonC, altC, bearing); + double[] midPoint = RTKUtils.calculateMidPoint2(lat, lon, alt,bearing); System.out.println("雷达点纬度: " + midPoint[0]); System.out.println("雷达点经度: " + midPoint[1]); + double midPointHight = midPoint[2]; System.out.println("雷达点高度: " + midPoint[2]); logger.log("ABCD雷达点经纬度：", "纬度：" + midPoint[0] + ";经度:" + midPoint[1] + ";海拔:" + midPoint[2]); @@ -301,7 +334,7 @@ public class SerialConnectUtils { sensorDataBean.setLon(midPoint[1]); sensorDataBean.setLat(midPoint[0]); - sensorDataBean.setH(midPoint[2]); + sensorDataBean.setH(midPointHight); sensorDataBean.setPositioningMode(qual); sensorDataBean.setAzimuthAngle(bearing); } @@ -515,8 +548,6 @@ public class SerialConnectUtils { assert serialHelper != null; Log.e(TAG, "2.测量数据返回：" + sb.toString()); serialHelper.sendHex(sb.toString()); - - // else { // SerialHelper serialHelper = serialHelperMap.get("数传"); // assert serialHelper != null; diff --git a/app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java b/app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java index 9c1e043..9d32c1f 100644 --- a/app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java +++ b/app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java @@ -5,6 +5,9 @@ import com.bonus.uav.entity.RadarPointBean; import com.bonus.uav.entity.SensorDataBean; import com.bonus.uav.utils.math.MathUtils; +import java.math.BigDecimal; +import java.math.RoundingMode; + public class RadarMathUtil { /** @@ -22,8 +25,8 @@ public class RadarMathUtil { /** * 激光雷达与测点连线与正北方向偏角 顺时针为正 */ -// private static final double radarAngle1 = -90 ; - private static final double radarAngle1 = 0 ; + private static final double radarAngle1 = 90 ; +// private static final double radarAngle1 = 0 ; /** * 3）、测点经纬度计算(pdf) @@ -86,12 +89,12 @@ public class RadarMathUtil { public static Coordinate transferPoint2(RadarPointBean radarPointBean, SensorDataBean sensorDataBean) { double radarAngle = radarPointBean.getAngle(); double dis = radarPointBean.getDistance(); - + dis = dis * Math.sin(Math.toRadians(radarAngle)); double lonGps = sensorDataBean.getLon(); double latGps = sensorDataBean.getLat(); double hGps = sensorDataBean.getH(); // double azimuthAngle = sensorDataBean.getAzimuthAngle() - 15.0; - double azimuthAngle = sensorDataBean.getAzimuthAngle()+90; + double azimuthAngle = sensorDataBean.getAzimuthAngle(); // //1.RTK天线到激光雷达点转换 // //激光雷达经度 // double lonRadar = lonGps + RTKDistance * Math.sin((azimuthAngle+RTKAngle)/180*Math.PI)/Rq/Math.cos(latGps/180*Math.PI)*180/Math.PI; @@ -100,8 +103,6 @@ public class RadarMathUtil { // //激光雷达高程 // double hRadar = hGps; // System.out.printf("雷达点：lat：%.8f; lon：%.8f; 高度：%.2f; 方位角：%.2f\n", latRadar, lonRadar, hRadar,azimuthAngle); - - double lonRadar=lonGps; double latRadar=latGps; double hRadar = hGps; @@ -112,13 +113,13 @@ public class RadarMathUtil { //测点纬度 double lat2 = latRadar + (dis*Math.cos((azimuthAngle+radarAngle1)/180*Math.PI)/Rq)*180/Math.PI; //测点高程 可能为减 - //2025-01-16,激光雷达扫描角度偏差90度，修改角度 - if(radarAngle>90){ - radarAngle = radarAngle - 90; - }else if(radarAngle<90){ - radarAngle = 90 - radarAngle; - } - double h2 = hRadar + dis * MathUtils.sin(radarAngle - sensorDataBean.getRollAngle()); + //2025-01-16,激光雷达扫描角度偏差90度，修改角度,2025-03-27换成cos了不需要换算补角了 +// if(radarAngle>90){ +// radarAngle = radarAngle - 90; +// }else if(radarAngle<90){ +// radarAngle = 90 - radarAngle; +// } + double h2 = hRadar + dis * MathUtils.cos(radarAngle - sensorDataBean.getRollAngle()); System.out.printf("ABC中心雷达点：lat：%.8f; lon：%.8f; 高度：%.2f; 方位角：%.2f\n", latRadar, lonRadar, hRadar,azimuthAngle); System.out.printf("ABC测量点：lat：%.8f; lon：%.8f; 高度：%.2f\n", lat2, lon2, h2); return new Coordinate(lon2,lat2,h2); @@ -206,5 +207,11 @@ public class RadarMathUtil { return Math.pow(l,2)* Math.abs(k)/4; } + public static double keepDouble(double number,int num) { + BigDecimal bd = new BigDecimal(Double.toString(number)); + bd = bd.setScale(num, RoundingMode.HALF_UP); + return bd.doubleValue(); + } + }