bonus
/
wrj_drone
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

方位角计算优化

This commit is contained in:
fl 2025-03-28 17:49:11 +08:00
parent b4bfa58c1f
commit c75a78b0da
5 changed files with 118 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
app/src/main/AndroidManifest.xml
View File

@ -14,6 +14,8 @@
<uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
<uses-permission android:name="android.permission.INTERNET" />
<uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
<uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE"/>
<uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE"/>
<uses-permission android:name="android.permission.BIND_JOB_SERVICE"
tools:ignore="ProtectedPermissions" />

4
app/src/main/java/com/bonus/uav/service/RadarRunnable.java
View File

@ -198,8 +198,8 @@ public class RadarRunnable implements Runnable {
*/
List<RadarPointBean> 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());
// 输出结果

60
app/src/main/java/com/bonus/uav/utils/RTKUtils.java
View File

@ -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);
}
/**
* 计算两个三维点之间的方向向量并计算方位角和仰角
*/

53
app/src/main/java/com/bonus/uav/utils/SerialConnectUtils.java
View File

@ -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;

33
app/src/main/java/com/bonus/uav/utils/sag/RadarMathUtil.java
View File

@ -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();
}
}