feat: 添加 ahrs 和 excavator-bucket-position 模块
This commit is contained in:
11
libs/excavator-bucket-position/build.gradle.kts
Normal file
11
libs/excavator-bucket-position/build.gradle.kts
Normal file
@@ -0,0 +1,11 @@
|
||||
plugins {
|
||||
kotlin("jvm")
|
||||
}
|
||||
|
||||
repositories {
|
||||
mavenCentral()
|
||||
}
|
||||
|
||||
dependencies {
|
||||
implementation(project(":libs:ahrs"))
|
||||
}
|
||||
@@ -0,0 +1,263 @@
|
||||
package com.icegps.excavator.model
|
||||
|
||||
import kotlin.math.*
|
||||
|
||||
/**
|
||||
* BLH -> ENU
|
||||
*
|
||||
* @author lm
|
||||
* @date 2025/3/12
|
||||
* @link https://gist.github.com/komasaru/6ce0634475923ddac597f868288c54e9
|
||||
*/
|
||||
class BlhToEnu {
|
||||
|
||||
companion object {
|
||||
private const val PI_180 = Math.PI / 180.0
|
||||
|
||||
// WGS84 坐标参数
|
||||
private const val A = 6378137.0 // a（地球椭球体长半径（赤道面平均半径））
|
||||
private const val ONE_F = 298.257223563 // 1 / f（地球椭球体扁平率 = (a - b) / a）
|
||||
private val B = A * (1.0 - 1.0 / ONE_F) // b（地球椭球体短半径）
|
||||
private val E2 = (1.0 / ONE_F) * (2 - (1.0 / ONE_F))
|
||||
|
||||
// e^2 = 2 * f - f * f = (a^2 - b^2) / a^2
|
||||
private val ED2 = E2 * A * A / (B * B) // e'^2 = (a^2 - b^2) / b^2
|
||||
}
|
||||
|
||||
private var originLat: Double = 0.0
|
||||
private var originLon: Double = 0.0
|
||||
private var originHeight: Double = 0.0
|
||||
private var isOriginSet: Boolean = false
|
||||
|
||||
fun resetEnuBenchmarkPoint() {
|
||||
isOriginSet = false
|
||||
}
|
||||
|
||||
fun wgs84ToEnu(lat: Double, lon: Double, height: Double = 0.0): DoubleArray {
|
||||
if (!isOriginSet) {
|
||||
originLat = lat
|
||||
originLon = lon
|
||||
originHeight = height
|
||||
isOriginSet = true
|
||||
return doubleArrayOf(0.0, 0.0, 0.0)
|
||||
}
|
||||
|
||||
val enu = blh2enu(originLat, originLon, originHeight, lat, lon, height)
|
||||
var az = atan2(enu[0], enu[1]) * 180.0 / Math.PI
|
||||
if (az < 0.0) {
|
||||
az += 360.0
|
||||
}
|
||||
val el = atan2(
|
||||
enu[2],
|
||||
sqrt(enu[0] * enu[0] + enu[1] * enu[1])
|
||||
) * 180.0 / Math.PI
|
||||
val dst = sqrt(enu.sumOf { it * it })
|
||||
|
||||
// println("--->")
|
||||
// println(
|
||||
// """
|
||||
// ENU: E = ${enu[0].format(3)}m
|
||||
// N = ${enu[1].format(3)}m
|
||||
// U = ${enu[2].format(3)}m
|
||||
// 方位角 = ${az.format(3)}°
|
||||
// 仰角 = ${el.format(3)}°
|
||||
// 距离 = ${dst.format(3)}m
|
||||
// """.trimIndent()
|
||||
// )
|
||||
|
||||
return enu
|
||||
}
|
||||
|
||||
fun enuToWgs84(e: Double, n: Double, u: Double): DoubleArray {
|
||||
if (!isOriginSet) {
|
||||
return doubleArrayOf(0.0, 0.0, 0.0)
|
||||
}
|
||||
|
||||
val blh = enu2blh(originLat, originLon, originHeight, e, n, u)
|
||||
|
||||
// println("--->")
|
||||
// println(
|
||||
// """
|
||||
// BLH: Beta = ${blh[0].format(8)}°
|
||||
// Lambda = ${blh[1].format(8)}°
|
||||
// Height = ${blh[2].format(3)}m
|
||||
// """.trimIndent()
|
||||
// )
|
||||
|
||||
return blh
|
||||
}
|
||||
|
||||
private fun Double.format(digits: Int) = "%.${digits}f".format(this)
|
||||
|
||||
/**
|
||||
* BLH -> ENU 转换(East, North, Up)
|
||||
*
|
||||
* @param bO 原点 Beta(纬度)
|
||||
* @param lO 原点 Lambda(经度)
|
||||
* @param hO 原点 Height(高度)
|
||||
* @param b 目标点 Beta(纬度)
|
||||
* @param l 目标点 Lambda(经度)
|
||||
* @param h 目标点 Height(高度)
|
||||
* @return ENU 坐标 [e, n, u]
|
||||
*/
|
||||
private fun blh2enu(bO: Double, lO: Double, hO: Double, b: Double, l: Double, h: Double): DoubleArray {
|
||||
val (xO, yO, zO) = blh2ecef(bO, lO, hO)
|
||||
val (x, y, z) = blh2ecef(b, l, h)
|
||||
val mat0 = matZ(90.0)
|
||||
val mat1 = matY(90.0 - bO)
|
||||
val mat2 = matZ(lO)
|
||||
val mat = mulMat(mulMat(mat0, mat1), mat2)
|
||||
return rotate(mat, doubleArrayOf(x - xO, y - yO, z - zO))
|
||||
}
|
||||
|
||||
/**
|
||||
* BLH -> ECEF 转换
|
||||
*
|
||||
* @param lat 纬度
|
||||
* @param lon 经度
|
||||
* @param ht 高度
|
||||
* @return ECEF 坐标 [x, y, z]
|
||||
*/
|
||||
private fun blh2ecef(lat: Double, lon: Double, ht: Double): DoubleArray {
|
||||
val n = { x: Double -> A / sqrt(1.0 - E2 * sin(x * PI_180).pow(2)) }
|
||||
val x = (n(lat) + ht) * cos(lat * PI_180) * cos(lon * PI_180)
|
||||
val y = (n(lat) + ht) * cos(lat * PI_180) * sin(lon * PI_180)
|
||||
val z = (n(lat) * (1.0 - E2) + ht) * sin(lat * PI_180)
|
||||
return doubleArrayOf(x, y, z)
|
||||
}
|
||||
|
||||
/**
|
||||
* ENU -> BLH 转换
|
||||
*
|
||||
* @param e East 坐标
|
||||
* @param n North 坐标
|
||||
* @param u Up 坐标
|
||||
* @return WGS84 坐标 [纬度, 经度, 高度]
|
||||
*/
|
||||
private fun enu2blh(bO: Double, lO: Double, hO: Double, e: Double, n: Double, u: Double): DoubleArray {
|
||||
val mat0 = matZ(-lO)
|
||||
val mat1 = matY(-(90.0 - bO))
|
||||
val mat2 = matZ(-90.0)
|
||||
val mat = mulMat(mulMat(mat0, mat1), mat2)
|
||||
|
||||
val enu = doubleArrayOf(e, n, u)
|
||||
val xyz = rotate(mat, enu)
|
||||
|
||||
val (xO, yO, zO) = blh2ecef(bO, lO, hO)
|
||||
val x = xyz[0] + xO
|
||||
val y = xyz[1] + yO
|
||||
val z = xyz[2] + zO
|
||||
|
||||
return ecef2blh(x, y, z)
|
||||
}
|
||||
|
||||
/**
|
||||
* ECEF -> BLH 转换
|
||||
* @param x ECEF X 坐标
|
||||
* @param y ECEF Y 坐标
|
||||
* @param z ECEF Z 坐标
|
||||
* @return WGS84 坐标 [纬度, 经度, 高度]
|
||||
*/
|
||||
private fun ecef2blh(x: Double, y: Double, z: Double): DoubleArray {
|
||||
val p = sqrt(x * x + y * y)
|
||||
val theta = atan2(z * A, p * B)
|
||||
val sinTheta = sin(theta)
|
||||
val cosTheta = cos(theta)
|
||||
|
||||
val lat = atan2(
|
||||
z + ED2 * B * sinTheta.pow(3),
|
||||
p - E2 * A * cosTheta.pow(3)
|
||||
)
|
||||
val lon = atan2(y, x)
|
||||
|
||||
val sinLat = sin(lat)
|
||||
val n = A / sqrt(1.0 - E2 * sinLat * sinLat)
|
||||
val h = p / cos(lat) - n
|
||||
|
||||
return doubleArrayOf(
|
||||
lat * 180.0 / Math.PI,
|
||||
lon * 180.0 / Math.PI,
|
||||
h
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 以 x 轴为轴的旋转矩阵
|
||||
*
|
||||
* @param ang 旋转角度(°)
|
||||
* @return 旋转矩阵(3x3)
|
||||
*/
|
||||
private fun matX(ang: Double): Array<DoubleArray> {
|
||||
val a = ang * PI_180
|
||||
val c = cos(a)
|
||||
val s = sin(a)
|
||||
return arrayOf(
|
||||
doubleArrayOf(1.0, 0.0, 0.0),
|
||||
doubleArrayOf(0.0, c, s),
|
||||
doubleArrayOf(0.0, -s, c)
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 以 y 轴为轴的旋转矩阵
|
||||
*
|
||||
* @param ang 旋转角度(°)
|
||||
* @return 旋转矩阵(3x3)
|
||||
*/
|
||||
private fun matY(ang: Double): Array<DoubleArray> {
|
||||
val a = ang * PI_180
|
||||
val c = cos(a)
|
||||
val s = sin(a)
|
||||
return arrayOf(
|
||||
doubleArrayOf(c, 0.0, -s),
|
||||
doubleArrayOf(0.0, 1.0, 0.0),
|
||||
doubleArrayOf(s, 0.0, c)
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 以 z 轴为轴的旋转矩阵
|
||||
*
|
||||
* @param ang 旋转角度(°)
|
||||
* @return 旋转矩阵(3x3)
|
||||
*/
|
||||
private fun matZ(ang: Double): Array<DoubleArray> {
|
||||
val a = ang * PI_180
|
||||
val c = cos(a)
|
||||
val s = sin(a)
|
||||
return arrayOf(
|
||||
doubleArrayOf(c, s, 0.0),
|
||||
doubleArrayOf(-s, c, 0.0),
|
||||
doubleArrayOf(0.0, 0.0, 1.0)
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 两个矩阵(3x3)的乘积
|
||||
*
|
||||
* @param matA 3x3 矩阵
|
||||
* @param matB 3x3 矩阵
|
||||
* @return 3x3 矩阵
|
||||
*/
|
||||
private fun mulMat(matA: Array<DoubleArray>, matB: Array<DoubleArray>): Array<DoubleArray> {
|
||||
return Array(3) { k ->
|
||||
DoubleArray(3) { j ->
|
||||
(0..2).sumOf { i -> matA[k][i] * matB[i][j] }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 点的旋转
|
||||
*
|
||||
* @param mat 3x3 旋转矩阵
|
||||
* @param pt 旋转前坐标 [x, y, z]
|
||||
* @return 旋转后坐标 [x, y, z]
|
||||
*/
|
||||
private fun rotate(mat: Array<DoubleArray>, pt: DoubleArray): DoubleArray {
|
||||
return DoubleArray(3) { j ->
|
||||
(0..2).sumOf { i -> mat[j][i] * pt[i] }
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
package com.icegps.excavator.model
|
||||
|
||||
/**
|
||||
* @author xjh
|
||||
* @date 2025/2/25
|
||||
*/
|
||||
|
||||
// 添加一个全局变量来存储缓存的LCE值
|
||||
var cachedLCE: Double = 0.0
|
||||
|
||||
/**
|
||||
* 挖掘机参数配置类
|
||||
*/
|
||||
data class ExcavatorConfig(
|
||||
|
||||
/**
|
||||
* RTK 相关距离参数
|
||||
*/
|
||||
val rtkConfig: RTKConfiguration = RTKConfiguration(
|
||||
mainRtkToBoomAxis = 1.0, // 主 RTK 相位中心到支架底部的垂直距离
|
||||
subRtkToBoomAxis = 1.0, // 副 RTK 相位中心到支架底部的垂直距离
|
||||
mainRtkFootToJoint1 = 1.0, // 主 RTK 在大臂上的垂足到关节 1 的距离 （不必须）
|
||||
subRtkFootToJoint2 = 1.0 // 副 RTK 在大臂上的垂足到关节 2 的距离
|
||||
),
|
||||
|
||||
/**
|
||||
* 挖掘机尺寸信息
|
||||
*/
|
||||
val excavatorDims: ExcavatorDimensions = ExcavatorDimensions(
|
||||
boom = 1.000, // 大臂长度（不必须）
|
||||
stick = 1.0, // 小臂长度
|
||||
bucket = 1.0, // 挖斗长度（不必要）
|
||||
mainBoomWidth = 1.0, // 主天线处大臂宽度
|
||||
subBoomWidth = 1.0, // 副天线处大臂宽度
|
||||
mainAntennaBracketWidth = 1.0, // 主天线支架宽度
|
||||
subAntennaBracketWidth = 1.0, // 副天线支架宽度
|
||||
mainAntennaBottomToBoomAxis = 1.0, // 主天线底面到大臂轴心的垂直距离
|
||||
subAntennaBottomToBoomAxis = 1.0 // 副天线底面到大臂轴心的垂直距离
|
||||
),
|
||||
|
||||
/**
|
||||
* 连杆长度参数
|
||||
*/
|
||||
val linkageLengths: LinkageLengths = LinkageLengths(
|
||||
LAB = 1.0, // 惰连杆长度(AB)
|
||||
LBC = 1.0, // 斗连杆长度(BC)
|
||||
LCD = 1.0, // 前后轴套长度(CD)
|
||||
LAD = 1.0, // 伪轴至斗齿长度(AD)
|
||||
LAE = 1.0, // 伪轴至斗齿距离(AE)
|
||||
LCE = cachedLCE, // (CE)
|
||||
LDE = 1.0, // 斗轴至斗齿距离(DE)
|
||||
W = 1.0 // 铲斗宽度(W)
|
||||
),
|
||||
|
||||
// 安装模式
|
||||
val antennaInstallationMode: AntennaInstallationMode = AntennaInstallationMode.LEFT,
|
||||
val sensorInstallationMode: SensorInstallationMode = SensorInstallationMode.CUSTOM,
|
||||
|
||||
// 传感器偏移量
|
||||
val stickOffsetEulerAngles: EulerAngles = EulerAngles(0.0, 0.0, 0.0),
|
||||
val idleConnectingRodOffsetEulerAngles: EulerAngles = EulerAngles(0.0, 0.0, 0.0)
|
||||
)
|
||||
@@ -0,0 +1,603 @@
|
||||
package com.icegps.excavator.model
|
||||
|
||||
import java.util.Locale
|
||||
import kotlin.math.PI
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.acos
|
||||
import kotlin.math.atan2
|
||||
import kotlin.math.cos
|
||||
import kotlin.math.min
|
||||
import kotlin.math.pow
|
||||
import kotlin.math.sin
|
||||
import kotlin.math.sqrt
|
||||
|
||||
/**
|
||||
* 3D 向量类，用于表示位置和方向
|
||||
*/
|
||||
data class Vector3(val x: Double, val y: Double, val z: Double) {
|
||||
operator fun plus(other: Vector3) = Vector3(x + other.x, y + other.y, z + other.z)
|
||||
operator fun minus(other: Vector3) = Vector3(x - other.x, y - other.y, z - other.z)
|
||||
operator fun times(scalar: Double) = Vector3(x * scalar, y * scalar, z * scalar)
|
||||
operator fun div(scalar: Double) = Vector3(x / scalar, y / scalar, z / scalar)
|
||||
operator fun unaryMinus() = Vector3(-x, -y, -z)
|
||||
|
||||
fun norm(): Double = sqrt(x * x + y * y + z * z)
|
||||
fun normalize(): Vector3 = this / norm()
|
||||
fun dot(other: Vector3): Double = x * other.x + y * other.y + z * other.z
|
||||
fun cross(other: Vector3): Vector3 = Vector3(
|
||||
y * other.z - z * other.y,
|
||||
z * other.x - x * other.z,
|
||||
x * other.y - y * other.x
|
||||
)
|
||||
|
||||
fun distanceTo(other: Vector3): Double = (this - other).magnitude()
|
||||
fun magnitude(): Double = sqrt(x * x + y * y + z * z)
|
||||
|
||||
companion object {
|
||||
val ZERO = Vector3(0.0, 0.0, 0.0)
|
||||
val UP = Vector3(0.0, 0.0, 1.0)
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 天线安装方式
|
||||
*/
|
||||
enum class AntennaInstallationMode {
|
||||
LEFT, // 左侧安装
|
||||
RIGHT // 右侧安装
|
||||
}
|
||||
|
||||
/**
|
||||
* 传感器安装方式
|
||||
*/
|
||||
enum class SensorInstallationMode {
|
||||
CUSTOM, // 自定义安装方式
|
||||
VERTICAL, // 垂直安装（正装）：传感器的 Z 轴与机体 Z 轴平行
|
||||
HORIZONTAL // 水平安装（侧装）：传感器的 Y 轴与机体 Z 轴平行
|
||||
}
|
||||
|
||||
/**
|
||||
* E 点计算方式
|
||||
*/
|
||||
enum class ECalculationMethod {
|
||||
USE_AE, // 使用 AE 方式计算
|
||||
USE_CE, // 使用 CE 方式计算
|
||||
}
|
||||
|
||||
/**
|
||||
* RTK 相关距离参数
|
||||
*/
|
||||
data class RTKConfiguration(
|
||||
val mainRtkToBoomAxis: Double, // 主 RTK 相位中心到支架底部的垂直距离
|
||||
val subRtkToBoomAxis: Double, // 副 RTK 相位中心到支架底部的垂直距离
|
||||
val mainRtkFootToJoint1: Double, // 主 RTK 在大臂上的垂足到关节 1 的距离
|
||||
val subRtkFootToJoint2: Double // 副 RTK 在大臂上的垂足到关节 2 的距离
|
||||
)
|
||||
|
||||
/**
|
||||
* 挖掘机尺寸信息
|
||||
*/
|
||||
data class ExcavatorDimensions(
|
||||
val boom: Double, // 大臂长度
|
||||
val stick: Double, // 小臂长度
|
||||
val bucket: Double, // 铲斗长度
|
||||
val mainBoomWidth: Double, // 主天线处大臂宽度
|
||||
val subBoomWidth: Double, // 副天线处大臂宽度
|
||||
val mainAntennaBracketWidth: Double, // 主天线支架宽度
|
||||
val subAntennaBracketWidth: Double, // 副天线支架宽度
|
||||
val mainAntennaBottomToBoomAxis: Double, // 主天线底面到大小臂轴心的垂直距离
|
||||
val subAntennaBottomToBoomAxis: Double // 副天线底面到大小臂轴心的垂直距离
|
||||
)
|
||||
|
||||
/**
|
||||
* 连杆长度参数
|
||||
*/
|
||||
data class LinkageLengths(
|
||||
val LAD: Double, // A 点到 D 点的距离
|
||||
val LAB: Double, // A 点到 B 点的距离
|
||||
val LBC: Double, // B 点到 C 点的距离
|
||||
val LCD: Double, // C 点到 D 点的距离
|
||||
val LAE: Double, // A 点到 E 点的距离
|
||||
val LCE: Double, // C 点到 E 点的距离
|
||||
val LDE: Double, // D 点到 E 点的距离
|
||||
val W: Double // E1 到 E2 的距离（铲斗宽度）
|
||||
)
|
||||
|
||||
/**
|
||||
* 欧拉角
|
||||
*/
|
||||
data class EulerAngles(
|
||||
val yaw: Double, // 偏航角
|
||||
val pitch: Double, // 俯仰角
|
||||
val roll: Double // 横滚角
|
||||
) {
|
||||
fun toList(): List<Double> = listOf(yaw, pitch, roll)
|
||||
}
|
||||
|
||||
/**
|
||||
* 挖掘机关键点
|
||||
*/
|
||||
data class ExcavatorKeyPoints(
|
||||
val joint1: Vector3, // 关节1（大臂起点位置）
|
||||
val joint2: Vector3, // 关节2（大臂末端/小臂起点位置）
|
||||
val joint3: Vector3, // 关节3（小臂末端/铲斗连接点位置）
|
||||
val A: Vector3, // A点
|
||||
val B: Vector3, // B点
|
||||
val C: Vector3, // C点
|
||||
val D: Vector3, // D点
|
||||
val E: Vector3, // 齿尖中心点
|
||||
val E1: Vector3, // 齿尖左侧点
|
||||
val E2: Vector3 // 齿尖右侧点
|
||||
)
|
||||
|
||||
/**
|
||||
* 挖掘机位置计算器
|
||||
*
|
||||
* @author lm
|
||||
* @date 2025/3/5
|
||||
*/
|
||||
class ExcavatorPositionCalculator {
|
||||
|
||||
/**
|
||||
* 应用欧拉角偏移量
|
||||
*/
|
||||
private fun applyOffsets(angles: EulerAngles, offset: EulerAngles): EulerAngles {
|
||||
return EulerAngles(
|
||||
yaw = angles.yaw - offset.yaw,
|
||||
pitch = angles.pitch - offset.pitch,
|
||||
roll = angles.roll - offset.roll
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算挖掘机关键点位置
|
||||
*
|
||||
* 机体坐标系：
|
||||
* - X 轴：指向机器前方
|
||||
* - Y 轴：指向机器右侧
|
||||
* - Z 轴：指向机器上方
|
||||
*
|
||||
* @param mainRtkPos 主 RTK 相位中心的 ENU 坐标
|
||||
* @param subRtkPos 副 RTK 相位中心的 ENU 坐标
|
||||
* @param rtkConfig RTK 相关配置参数
|
||||
* @param excavatorDims 挖掘机尺寸参数
|
||||
* @param linkageLengths 连杆长度参数
|
||||
* @param stickAngleSensor 小臂传感器的欧拉角，单位为弧度
|
||||
* @param idleConnectingRodAngleSensor 惰连杆传感器的欧拉角，单位为弧度
|
||||
* @param antennaInstallationMode 天线安装模式
|
||||
* @param sensorInstallationMode 传感器安装模式
|
||||
* @param eCalculationMethod E 点计算方式
|
||||
* @param stickOffsetEulerAngles 小臂传感器的欧拉角偏移量，单位为弧度
|
||||
* @param idleConnectingRodOffsetEulerAngles 惰连杆传感器的欧拉角偏移量，单位为弧度
|
||||
* @return ExcavatorKeyPoints 包含所有关键点坐标的结果对象
|
||||
*/
|
||||
fun calculateExcavatorKeyPoints(
|
||||
mainRtkPos: Vector3,
|
||||
subRtkPos: Vector3,
|
||||
rtkConfig: RTKConfiguration,
|
||||
excavatorDims: ExcavatorDimensions,
|
||||
linkageLengths: LinkageLengths,
|
||||
stickAngleSensor: EulerAngles,
|
||||
idleConnectingRodAngleSensor: EulerAngles,
|
||||
antennaInstallationMode: AntennaInstallationMode = AntennaInstallationMode.LEFT,
|
||||
sensorInstallationMode: SensorInstallationMode = SensorInstallationMode.VERTICAL,
|
||||
eCalculationMethod: ECalculationMethod = ECalculationMethod.USE_CE,
|
||||
stickOffsetEulerAngles: EulerAngles = EulerAngles(0.0, 0.0, 0.0),
|
||||
idleConnectingRodOffsetEulerAngles: EulerAngles = EulerAngles(0.0, 0.0, 0.0)
|
||||
): ExcavatorKeyPoints {
|
||||
// 应用偏移量
|
||||
val stickAngleSensorCorrected = applyOffsets(stickAngleSensor, stickOffsetEulerAngles)
|
||||
val idleConnectingRodAngleSensorCorrected = applyOffsets(idleConnectingRodAngleSensor, idleConnectingRodOffsetEulerAngles)
|
||||
|
||||
// 根据传感器安装模式选择正确的角度
|
||||
val stickAngle = when (sensorInstallationMode) {
|
||||
SensorInstallationMode.CUSTOM -> stickAngleSensorCorrected.roll
|
||||
SensorInstallationMode.VERTICAL -> stickAngleSensorCorrected.pitch
|
||||
SensorInstallationMode.HORIZONTAL -> stickAngleSensorCorrected.yaw
|
||||
}
|
||||
val idleConnectingRodAngle = when (sensorInstallationMode) {
|
||||
SensorInstallationMode.CUSTOM -> idleConnectingRodAngleSensorCorrected.roll
|
||||
SensorInstallationMode.VERTICAL -> idleConnectingRodAngleSensorCorrected.pitch
|
||||
SensorInstallationMode.HORIZONTAL -> idleConnectingRodAngleSensorCorrected.yaw
|
||||
}
|
||||
|
||||
// 计算天线方向向量
|
||||
val antennaVector = subRtkPos - mainRtkPos
|
||||
val antennaDirection = antennaVector.normalize()
|
||||
|
||||
// 计算垂直于天线方向的向量（水平方向）
|
||||
val antennaPerpendicular = antennaDirection.cross(Vector3.UP).normalize()
|
||||
|
||||
// 计算垂直于天线方向且指向"上方"的向量（垂直方向）
|
||||
val antennaUp = antennaPerpendicular.cross(antennaDirection).normalize()
|
||||
|
||||
// 根据天线安装模式确定偏移方向
|
||||
val offsetDirection = when (antennaInstallationMode) {
|
||||
AntennaInstallationMode.LEFT -> -antennaPerpendicular
|
||||
AntennaInstallationMode.RIGHT -> antennaPerpendicular
|
||||
}
|
||||
|
||||
// 计算天线到大臂中心线的偏移
|
||||
val mainOffset = (excavatorDims.mainBoomWidth + excavatorDims.mainAntennaBracketWidth) / 2
|
||||
val subOffset = (excavatorDims.subBoomWidth + excavatorDims.subAntennaBracketWidth) / 2
|
||||
|
||||
// 计算 RTK 到大臂中心线的垂足
|
||||
val mainRtkFoot = mainRtkPos - antennaUp * (rtkConfig.mainRtkToBoomAxis + excavatorDims.mainAntennaBottomToBoomAxis) - offsetDirection * mainOffset
|
||||
val subRtkFoot = subRtkPos - antennaUp * (rtkConfig.subRtkToBoomAxis + excavatorDims.subAntennaBottomToBoomAxis) - offsetDirection * subOffset
|
||||
|
||||
// 计算大臂实际方向向量
|
||||
val boomVector = subRtkFoot - mainRtkFoot
|
||||
val boomDirection = boomVector.normalize()
|
||||
|
||||
// 计算连接点1的位置（大臂起点）
|
||||
val joint1Pos = mainRtkFoot + boomDirection * rtkConfig.mainRtkFootToJoint1
|
||||
|
||||
// 计算连接点2的位置（大臂末端/小臂起点）
|
||||
val joint2Pos = subRtkFoot - boomDirection * rtkConfig.subRtkFootToJoint2
|
||||
|
||||
// 计算 yaw 角（大臂在水平面上的方向）
|
||||
val boomYaw = atan2(boomDirection.y, boomDirection.x)
|
||||
|
||||
// 计算小臂的方向
|
||||
val stickDirection = Vector3(
|
||||
cos(boomYaw) * cos(stickAngle),
|
||||
sin(boomYaw) * cos(stickAngle),
|
||||
sin(stickAngle)
|
||||
).normalize()
|
||||
|
||||
// 确保小臂方向与大臂方向在同一平面
|
||||
val boomNormal = boomDirection.cross(Vector3.UP)
|
||||
val stickDirectionAdjusted = (stickDirection - boomNormal * stickDirection.dot(boomNormal)).normalize()
|
||||
|
||||
// 计算连接点3的位置（小臂末端 / 铲斗连接点）
|
||||
val joint3Pos = joint2Pos + stickDirectionAdjusted * excavatorDims.stick
|
||||
|
||||
// 设置 D 点
|
||||
val D = joint3Pos
|
||||
|
||||
// 计算 A 点坐标
|
||||
val A = D - stickDirectionAdjusted * linkageLengths.LAD
|
||||
|
||||
// 计算 B 点坐标
|
||||
val idleConnectingRodDirection = Vector3(
|
||||
cos(boomYaw) * cos(idleConnectingRodAngle),
|
||||
sin(boomYaw) * cos(idleConnectingRodAngle),
|
||||
sin(idleConnectingRodAngle)
|
||||
)
|
||||
val B = A + idleConnectingRodDirection * linkageLengths.LAB
|
||||
|
||||
val planeNormal = boomDirection.cross(Vector3.UP).normalize()
|
||||
|
||||
// 使用球面交点方法计算 C 点坐标
|
||||
val (candidateC1, candidateC2) = findIntersection3d(B, linkageLengths.LBC, D, linkageLengths.LCD, planeNormal)
|
||||
.let { (c1, c2) -> Pair(c1 ?: D, c2 ?: D) }
|
||||
|
||||
// 选择正确的 C 点
|
||||
val C = selectCorrectCPoint(A, B, D, candidateC1, candidateC2)
|
||||
|
||||
// 计算 E 点坐标
|
||||
val (candidateE1, candidateE2) = when (eCalculationMethod) {
|
||||
ECalculationMethod.USE_AE ->
|
||||
findIntersection3d(A, linkageLengths.LAE, D, linkageLengths.LDE, planeNormal)
|
||||
.let { (e1, e2) -> Pair(e1 ?: D, e2 ?: D) }
|
||||
|
||||
ECalculationMethod.USE_CE ->
|
||||
findIntersection3d(C, linkageLengths.LCE, D, linkageLengths.LDE, planeNormal)
|
||||
.let { (e1, e2) -> Pair(e1 ?: D, e2 ?: D) }
|
||||
}
|
||||
|
||||
// 选择正确的 E 点（位于 AB 的对面）
|
||||
val ABMidpoint = (A + B) * 0.5
|
||||
val ABToD = D - ABMidpoint
|
||||
val E = if ((candidateE1 - ABMidpoint).dot(ABToD) > (candidateE2 - ABMidpoint).dot(ABToD)) candidateE1 else candidateE2
|
||||
|
||||
// 计算 E1 和 E2 点坐标
|
||||
val boomDirectionHorizontal = Vector3(
|
||||
boomDirection.x,
|
||||
boomDirection.y,
|
||||
0.0
|
||||
).normalize()
|
||||
|
||||
// 计算挖掘机朝向的右侧向量
|
||||
val rightVector = boomDirectionHorizontal.cross(Vector3.UP).normalize()
|
||||
|
||||
// 计算铲斗方向向量
|
||||
val bucketDirection = (E - D).normalize()
|
||||
|
||||
// 计算铲斗左右方向向量
|
||||
var bucketLeftRight = bucketDirection.cross(stickDirectionAdjusted).normalize()
|
||||
|
||||
// 确保 bucketLeftRight 与 rightVector 方向一致
|
||||
if (bucketLeftRight.dot(rightVector) < 0) {
|
||||
bucketLeftRight = -bucketLeftRight
|
||||
}
|
||||
|
||||
// 计算 E1（左铲尖）和 E2（右铲尖）
|
||||
val E1 = E - bucketLeftRight * (linkageLengths.W / 2)
|
||||
val E2 = E + bucketLeftRight * (linkageLengths.W / 2)
|
||||
|
||||
return ExcavatorKeyPoints(
|
||||
joint1 = joint1Pos,
|
||||
joint2 = joint2Pos,
|
||||
joint3 = joint3Pos,
|
||||
A = A,
|
||||
B = B,
|
||||
C = C,
|
||||
D = D,
|
||||
E = E,
|
||||
E1 = E1,
|
||||
E2 = E2
|
||||
)
|
||||
}
|
||||
|
||||
/**
|
||||
* 限制角度在 -55 到 55 度之间
|
||||
*/
|
||||
private fun limitAngle(angle: Double): Double {
|
||||
return angle.coerceIn(-55.0, 55.0)
|
||||
}
|
||||
|
||||
/**
|
||||
* 选择正确的 C 点，基于多个几何和运动学条件
|
||||
*
|
||||
* @param A 已知的四连杆点坐标
|
||||
* @param B 已知的四连杆点坐标
|
||||
* @param D 已知的四连杆点坐标
|
||||
* @param C1 候选 C 点坐标
|
||||
* @param C2 候选 C 点坐标
|
||||
* @return 选中的 C 点坐标
|
||||
*/
|
||||
private fun selectCorrectCPoint(A: Vector3, B: Vector3, D: Vector3, C1: Vector3, C2: Vector3): Vector3 {
|
||||
fun scoreCPoint(C: Vector3): Double {
|
||||
/**
|
||||
* 对 C 点进行综合评分
|
||||
*/
|
||||
if (!isCoplanar(A, B, C, D) || !isValidQuadrilateral(A, B, C, D)) {
|
||||
return Double.NEGATIVE_INFINITY
|
||||
}
|
||||
|
||||
val angleScore = 1.0 / (1.0 + abs(calculateAngleSum(A, B, C, D) - Math.PI))
|
||||
val transmissionScore = 1.0 / (1.0 + calculateTransmissionAngle(A, B, C, D))
|
||||
val singularScore = if (checkSingularPosition(A, B, C, D)) 0.0 else 1.0
|
||||
|
||||
return angleScore + transmissionScore + singularScore
|
||||
}
|
||||
|
||||
val C1Score = scoreCPoint(C1)
|
||||
val C2Score = scoreCPoint(C2)
|
||||
|
||||
return if (C1Score > C2Score) C1 else C2
|
||||
}
|
||||
|
||||
/**
|
||||
* 检查四点是否共面
|
||||
*/
|
||||
private fun isCoplanar(A: Vector3, B: Vector3, C: Vector3, D: Vector3, tolerance: Double = 1e-6): Boolean {
|
||||
return abs((D - A).dot((B - A).cross(C - A))) < tolerance
|
||||
}
|
||||
|
||||
/**
|
||||
* 检查是否为有效的四边形
|
||||
*/
|
||||
private fun isValidQuadrilateral(A: Vector3, B: Vector3, C: Vector3, D: Vector3): Boolean {
|
||||
val points = listOf(A, B, C, D)
|
||||
if (points.any { p1 -> points.any { p2 -> p1 != p2 && p1.distanceTo(p2) < 1e-6 } }) {
|
||||
return false // 存在重合点
|
||||
}
|
||||
|
||||
return !(lineIntersection(A, B, C, D) || lineIntersection(A, D, B, C))
|
||||
}
|
||||
|
||||
/**
|
||||
* 检查两条线段是否相交
|
||||
*/
|
||||
private fun lineIntersection(p1: Vector3, p2: Vector3, p3: Vector3, p4: Vector3): Boolean {
|
||||
val v1 = p2 - p1
|
||||
val v2 = p4 - p3
|
||||
val normal = v1.cross(v2)
|
||||
if (normal.magnitude() < 1e-6) {
|
||||
return false // 平行或共线
|
||||
}
|
||||
|
||||
val t = (p3 - p1).dot(v2.cross(normal)) / v1.dot(v2.cross(normal))
|
||||
val u = (p1 - p3).dot(v1.cross(normal)) / v2.dot(v1.cross(normal))
|
||||
|
||||
return t in 0.0..1.0 && u in 0.0..1.0
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算 ∠BAD 和 ∠BCD 的和
|
||||
*/
|
||||
private fun calculateAngleSum(A: Vector3, B: Vector3, C: Vector3, D: Vector3): Double {
|
||||
val BA = A - B
|
||||
val BC = C - B
|
||||
val DA = A - D
|
||||
val DC = C - D
|
||||
val angleBAD = acos(BA.dot(DA) / (BA.magnitude() * DA.magnitude()))
|
||||
val angleBCD = acos(BC.dot(DC) / (BC.magnitude() * DC.magnitude()))
|
||||
return angleBAD + angleBCD
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算传动角，返回与 90 度的偏差
|
||||
*/
|
||||
private fun calculateTransmissionAngle(A: Vector3, B: Vector3, C: Vector3, D: Vector3): Double {
|
||||
val BA = A - B
|
||||
val BC = C - B
|
||||
val DC = C - D
|
||||
val DA = A - D
|
||||
val angle1 = acos(BA.dot(BC) / (BA.magnitude() * BC.magnitude()))
|
||||
val angle2 = acos(DC.dot(DA) / (DC.magnitude() * DA.magnitude()))
|
||||
return min(abs(Math.PI / 2 - angle1), abs(Math.PI / 2 - angle2))
|
||||
}
|
||||
|
||||
/**
|
||||
* 检查是否处于奇异位置
|
||||
*/
|
||||
private fun checkSingularPosition(A: Vector3, B: Vector3, C: Vector3, D: Vector3, tolerance: Double = 1e-6): Boolean {
|
||||
val BA = A - B
|
||||
val BC = C - B
|
||||
val DC = C - D
|
||||
val DA = A - D
|
||||
return (abs(BA.dot(BC) / (BA.magnitude() * BC.magnitude())) > 1 - tolerance ||
|
||||
abs(DC.dot(DA) / (DC.magnitude() * DA.magnitude())) > 1 - tolerance)
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算 3D 空间中两个球面在给定平面上的交点
|
||||
*
|
||||
* @param center1 第一个球的中心点坐标
|
||||
* @param radius1 第一个球的半径
|
||||
* @param center2 第二个球的中心点坐标
|
||||
* @param radius2 第二个球的半径
|
||||
* @param planeNormal 平面的法向量
|
||||
* @return 两个交点的坐标，如果没有交点则返回 null, null
|
||||
*/
|
||||
private fun findIntersection3d(center1: Vector3, radius1: Double, center2: Vector3, radius2: Double, planeNormal: Vector3): Pair<Vector3?, Vector3?> {
|
||||
// 计算两个球心之间的向量
|
||||
val centerVector = center2 - center1
|
||||
|
||||
// 计算中心向量在平面法线上的投影
|
||||
val projection = planeNormal * centerVector.dot(planeNormal)
|
||||
|
||||
// 计算中心向量在平面内的分量
|
||||
val centerVectorInPlane = centerVector - projection
|
||||
|
||||
// 计算平面内两圆心距离
|
||||
val d = centerVectorInPlane.magnitude()
|
||||
|
||||
// 检查是否有交点，如果两圆心距离大于两半径之和，或小于两半径之差，则没有交点
|
||||
if (d > radius1 + radius2 || d < abs(radius1 - radius2)) {
|
||||
return Pair(null, null)
|
||||
}
|
||||
|
||||
// 计算交点到 center1 的距离
|
||||
val a = (radius1.pow(2) - radius2.pow(2) + d.pow(2)) / (2 * d)
|
||||
|
||||
// 计算交点到连接线的垂直距离
|
||||
val h = sqrt(radius1.pow(2) - a.pow(2))
|
||||
|
||||
// 计算交点在连接线上的位置
|
||||
val intersectionBase = center1 + centerVectorInPlane * (a / d)
|
||||
|
||||
// 计算垂直于连接线和平面法线的向量
|
||||
val perpendicular = centerVectorInPlane.cross(planeNormal).normalize()
|
||||
|
||||
// 计算两个交点
|
||||
val intersection1 = intersectionBase + perpendicular * h
|
||||
val intersection2 = intersectionBase - perpendicular * h
|
||||
|
||||
return Pair(intersection1, intersection2)
|
||||
}
|
||||
|
||||
/**
|
||||
* 将角度转换为弧度
|
||||
*/
|
||||
fun degreesToRadians(degrees: Double): Double = degrees * PI / 180.0
|
||||
|
||||
/**
|
||||
* 将弧度转换为角度
|
||||
*/
|
||||
fun radiansToDegrees(radians: Double): Double = radians * 180.0 / PI
|
||||
|
||||
/**
|
||||
* 将 ±180 度系统转换为 0-360 度系统
|
||||
*/
|
||||
fun convert180To360(angle: Double): Double {
|
||||
return when {
|
||||
angle in -90.0..90.0 -> 90.0 - angle
|
||||
angle > 90.0 -> 450.0 - angle
|
||||
else -> 90.0 - angle // angle < -90
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 0-360 度系统转换为 ±180 度系统
|
||||
*/
|
||||
fun convert360To180(angle: Double): Double {
|
||||
return when {
|
||||
angle in 0.0..180.0 -> 90.0 - angle
|
||||
else -> 450.0 - angle // 180 < angle < 360
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 将欧拉角格式化为字符串，同时显示弧度和度数
|
||||
*/
|
||||
fun formatEulerAngles(eulerAngles: EulerAngles): String {
|
||||
return eulerAngles.toList().joinToString(", ") { angle ->
|
||||
String.format(Locale.CHINA, "%.4f rad (%.2f°)", angle, radiansToDegrees(angle))
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算两点之间的俯仰角
|
||||
*
|
||||
* 范围为 ±180 度：
|
||||
* - 0度：水平向前
|
||||
* - +90度：垂直向上
|
||||
* - -90度：垂直向下
|
||||
* - ±180度：水平向后
|
||||
*/
|
||||
fun calculatePitch(point1: Vector3, point2: Vector3): Double {
|
||||
// 计算两点之间的差值
|
||||
val dx = point2.x - point1.x
|
||||
val dy = point2.y - point1.y
|
||||
val dz = point2.z - point1.z
|
||||
|
||||
// 计算水平距离
|
||||
val horizontalDistance = sqrt(dx * dx + dy * dy)
|
||||
|
||||
// 使用 atan2 计算俯仰角
|
||||
var pitch = atan2(dz, horizontalDistance)
|
||||
|
||||
// 调整角度范围到 ±180 度
|
||||
if (dx < 0) {
|
||||
pitch = if (pitch > 0) {
|
||||
PI - pitch
|
||||
} else {
|
||||
-PI - pitch
|
||||
}
|
||||
}
|
||||
|
||||
return pitch
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算两点之间的偏航角
|
||||
*/
|
||||
fun calculateYaw(point1: Vector3, point2: Vector3): Double {
|
||||
// 计算水平平面上的差值
|
||||
val dx = point2.x - point1.x
|
||||
val dy = point2.y - point1.y
|
||||
|
||||
// 使用 atan2 计算偏航角
|
||||
val yaw = atan2(dy, dx)
|
||||
|
||||
return yaw
|
||||
}
|
||||
|
||||
/**
|
||||
* 计算两点之间的横滚角
|
||||
*/
|
||||
fun calculateRoll(point1: Vector3, point2: Vector3, upVector: Vector3): Double {
|
||||
// 计算从 point1 到 point2 的方向向量并归一化
|
||||
val direction = (point2 - point1).normalize()
|
||||
|
||||
// 计算右向量（叉乘得到垂直于 direction 和 up_vector 的向量）并归一化
|
||||
val right = direction.cross(upVector).normalize()
|
||||
|
||||
// 计算新的上向量（叉乘得到垂直于 direction 和 right 的向量）
|
||||
val newUp = right.cross(direction)
|
||||
|
||||
// 计算 roll 角（新的上向量与原始上向量之间的角度）
|
||||
val cosRoll = newUp.dot(upVector)
|
||||
var roll = acos(cosRoll.coerceIn(-1.0, 1.0))
|
||||
|
||||
// 确定 roll 的符号
|
||||
if (right.dot(upVector) < 0) roll = -roll
|
||||
|
||||
return roll
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,306 @@
|
||||
package com.icegps.excavator.model
|
||||
|
||||
import com.icegps.ahrs.AHRS401AParsedCanData
|
||||
import java.text.SimpleDateFormat
|
||||
import java.util.*
|
||||
|
||||
/**
|
||||
* @author xjh
|
||||
* @date 2025/2/25
|
||||
*/
|
||||
class ExcavatorPositionManager(
|
||||
private val excavatorPositionCalculator: ExcavatorPositionCalculator,
|
||||
private var config: ExcavatorConfig,
|
||||
private var isCalibrationMode: Boolean = false
|
||||
) {
|
||||
companion object {
|
||||
private const val TAG = "ExcavatorPositionManager"
|
||||
}
|
||||
|
||||
// 回调接口定义
|
||||
interface OnPositionCalculatedListener {
|
||||
fun onPositionCalculated(logMessage: String)
|
||||
}
|
||||
|
||||
// 回调列表
|
||||
private val listeners = mutableListOf<OnPositionCalculatedListener>()
|
||||
|
||||
// 添加回调
|
||||
fun addOnPositionCalculatedListener(listener: OnPositionCalculatedListener) {
|
||||
if (!listeners.contains(listener)) {
|
||||
listeners.add(listener)
|
||||
}
|
||||
}
|
||||
|
||||
// 移除回调
|
||||
fun removeOnPositionCalculatedListener(listener: OnPositionCalculatedListener) {
|
||||
listeners.remove(listener)
|
||||
}
|
||||
|
||||
private var mainRTKData: RTKData? = null
|
||||
private var subRTKData: RTKData? = null
|
||||
private var attitudeData: RTKData? = null
|
||||
var ahrsData: AHRS401AParsedCanData? = null
|
||||
var subAhrsData: AHRS401AParsedCanData? = null
|
||||
private var geoHelper: GeoHelper = GeoHelper.getSharedInstance()
|
||||
private var keyPoints: ExcavatorKeyPoints? = null
|
||||
|
||||
|
||||
/**
|
||||
* 更新配置
|
||||
*/
|
||||
fun updateConfig(newConfig: ExcavatorConfig) {
|
||||
this.config = newConfig
|
||||
}
|
||||
|
||||
/**
|
||||
* 设置校准模式
|
||||
*/
|
||||
fun setCalibrationMode(calibrationMode: Boolean) {
|
||||
this.isCalibrationMode = calibrationMode
|
||||
println("校准模式设置为: $calibrationMode")
|
||||
}
|
||||
|
||||
/**
|
||||
* 更新主天线RTK数据（位置）
|
||||
*/
|
||||
private fun updateMainRTKData(data: RTKData) {
|
||||
if (data.isMainAntenna()) {
|
||||
mainRTKData = data
|
||||
println("更新主RTK数据: $data")
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 更新副天线RTK数据（位置）
|
||||
*/
|
||||
private fun updateSubRTKData(data: RTKData) {
|
||||
if (data.isSubAntenna()) {
|
||||
subRTKData = data
|
||||
println("更新副RTK数据: $data")
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 更新RTK数据（自动判断是主天线、副天线还是姿态数据）
|
||||
*/
|
||||
fun updateRtkData(data: RTKData) {
|
||||
if (data.isMainAntenna()) {
|
||||
println("主天线 RTK定位质量:${data.fixQuality} ")
|
||||
updateMainRTKData(data)
|
||||
} else if (data.isSubAntenna()) {
|
||||
println("副天线 RTK定位质量:${data.fixQuality} ")
|
||||
updateSubRTKData(data)
|
||||
} else if (data.isAttitudeData()) {
|
||||
updateAttitudeData(data)
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 更新姿态数据
|
||||
*/
|
||||
private fun updateAttitudeData(data: RTKData) {
|
||||
if (data.isAttitudeData()) {
|
||||
if (mainRTKData?.hasPositionData() == true && subRTKData?.hasPositionData() == true) {
|
||||
attitudeData = data
|
||||
println("更新姿态数据: $data")
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 更新倾角传感器数据
|
||||
*/
|
||||
fun updateAhrsData(data: AHRS401AParsedCanData) {
|
||||
ahrsData = data
|
||||
// println("更新AHRS传感器数据: Pitch=${data.pitch}, Roll=${data.roll}, Yaw=${data.yaw}")
|
||||
}
|
||||
|
||||
fun updateSubAhrsData(data: AHRS401AParsedCanData) {
|
||||
subAhrsData = data
|
||||
// println("更新subAHRS传感器数据: Pitch=${data.pitch}, Roll=${data.roll}, Yaw=${data.yaw}")
|
||||
}
|
||||
|
||||
fun calculateBucketPoints(): ExcavatorKeyPoints? {
|
||||
|
||||
if (mainRTKData == null || !mainRTKData!!.hasPositionData() || !mainRTKData!!.isRtkFixed()) {
|
||||
println("主天线数据无效或未固定")
|
||||
return null
|
||||
}
|
||||
|
||||
if (subRTKData == null || !subRTKData!!.hasPositionData() || !subRTKData!!.isRtkFixed()) {
|
||||
println("副天线数据无效或未固定")
|
||||
return null
|
||||
}
|
||||
|
||||
// 转换主RTK WGS84坐标到ENU坐标
|
||||
val mainRtkEnu = geoHelper.wgs84ToENU(
|
||||
mainRTKData!!.longitude,
|
||||
mainRTKData!!.latitude,
|
||||
mainRTKData!!.altitude
|
||||
)
|
||||
val mainRtkPos = Vector3(mainRtkEnu.x, mainRtkEnu.y, mainRtkEnu.z)
|
||||
|
||||
// 转换副RTK WGS84坐标到ENU坐标
|
||||
val subRtkEnu = geoHelper.wgs84ToENU(
|
||||
subRTKData!!.longitude,
|
||||
subRTKData!!.latitude,
|
||||
subRTKData!!.altitude
|
||||
)
|
||||
val subRtkPos = Vector3(subRtkEnu.x, subRtkEnu.y, subRtkEnu.z)
|
||||
|
||||
// 小臂传感器的欧拉角
|
||||
val stickSensor = EulerAngles(
|
||||
yaw = Math.toRadians(ahrsData!!.yaw.toDouble()),
|
||||
pitch = Math.toRadians(ahrsData!!.pitch.toDouble()),
|
||||
roll = Math.toRadians(ahrsData!!.roll.toDouble())
|
||||
)
|
||||
|
||||
// 惰连杆传感器的欧拉角
|
||||
val idleConnectingRodAngleSensor = EulerAngles(
|
||||
yaw = Math.toRadians(subAhrsData!!.yaw.toDouble()),
|
||||
pitch = Math.toRadians(subAhrsData!!.pitch.toDouble()),
|
||||
roll = Math.toRadians(subAhrsData!!.roll.toDouble())
|
||||
)
|
||||
|
||||
// 选择E点计算方法，校准模式下使用AE方法，校准后使用CE方法
|
||||
val eCalculationMethod = if (isCalibrationMode) {
|
||||
ECalculationMethod.USE_AE
|
||||
} else {
|
||||
ECalculationMethod.USE_CE
|
||||
}
|
||||
|
||||
println(
|
||||
"传参:\n" +
|
||||
"mainRtkPos: $mainRtkPos\n" +
|
||||
"subRtkPos: $subRtkPos\n" +
|
||||
"rtkConfig: ${config.rtkConfig}\n" +
|
||||
"excavatorDims: ${config.excavatorDims}\n" +
|
||||
"linkageLengths: ${config.linkageLengths}\n" +
|
||||
"stickAngleSensor: $stickSensor\n" +
|
||||
"idleConnectingRodAngleSensor: $idleConnectingRodAngleSensor\n" +
|
||||
"antennaInstallationMode: ${config.antennaInstallationMode}\n" +
|
||||
"sensorInstallationMode: ${config.sensorInstallationMode}\n" +
|
||||
"eCalculationMethod: $eCalculationMethod\n" +
|
||||
"isCalibrationMode: $isCalibrationMode"
|
||||
)
|
||||
|
||||
// 计算铲斗位置
|
||||
keyPoints = excavatorPositionCalculator.calculateExcavatorKeyPoints(
|
||||
mainRtkPos = mainRtkPos,
|
||||
subRtkPos = subRtkPos,
|
||||
rtkConfig = config.rtkConfig,
|
||||
excavatorDims = config.excavatorDims,
|
||||
linkageLengths = config.linkageLengths,
|
||||
stickAngleSensor = stickSensor,
|
||||
idleConnectingRodAngleSensor = idleConnectingRodAngleSensor,
|
||||
antennaInstallationMode = config.antennaInstallationMode,
|
||||
sensorInstallationMode = config.sensorInstallationMode,
|
||||
stickOffsetEulerAngles = config.stickOffsetEulerAngles,
|
||||
eCalculationMethod = eCalculationMethod,
|
||||
idleConnectingRodOffsetEulerAngles = config.idleConnectingRodOffsetEulerAngles
|
||||
)
|
||||
notifyListeners(true, keyPoints, stickSensor, idleConnectingRodAngleSensor)
|
||||
// println("铲斗位置计算成功——经纬度: ${getKeyPointsWGS84Positions()}")
|
||||
return keyPoints
|
||||
}
|
||||
|
||||
fun getKeyPointsWGS84Positions(): Map<String, Triple<Double, Double, Double>>? {
|
||||
return keyPoints?.let { keyPoints ->
|
||||
mapOf(
|
||||
"joint1" to convertToWGS84(keyPoints.joint1),
|
||||
"joint2" to convertToWGS84(keyPoints.joint2),
|
||||
"joint3" to convertToWGS84(keyPoints.joint3),
|
||||
"A" to convertToWGS84(keyPoints.A),
|
||||
"B" to convertToWGS84(keyPoints.B),
|
||||
"C" to convertToWGS84(keyPoints.C),
|
||||
"D" to convertToWGS84(keyPoints.D),
|
||||
"E" to convertToWGS84(keyPoints.E),
|
||||
"E1" to convertToWGS84(keyPoints.E1),
|
||||
"E2" to convertToWGS84(keyPoints.E2)
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
private fun convertToWGS84(vector: Vector3): Triple<Double, Double, Double> {
|
||||
val enu = GeoHelper.Enu(vector.x, vector.y, vector.z)
|
||||
val wgs84 = geoHelper.enuToWGS84(enu)
|
||||
return Triple(wgs84[0], wgs84[1], wgs84[2]) // 经度, 纬度, 高度
|
||||
}
|
||||
|
||||
private fun notifyListeners(
|
||||
success: Boolean,
|
||||
keyPoints: ExcavatorKeyPoints?,
|
||||
stickSensor: EulerAngles?,
|
||||
idleConnectingRodAngleSensor: EulerAngles?
|
||||
) {
|
||||
val timestamp = Date()
|
||||
val dateFormat = SimpleDateFormat("HH:mm:ss.SSS", Locale.CHINA)
|
||||
val timeString = dateFormat.format(timestamp)
|
||||
|
||||
// 构建日志消息
|
||||
val logMessage = buildString {
|
||||
append("===== $timeString =====\n")
|
||||
|
||||
// 添加RTK数据信息
|
||||
append("RTK数据状态:\n")
|
||||
mainRTKData?.let {
|
||||
append("主天线: 经度=${String.format("%.8f", it.longitude)}, ")
|
||||
append("纬度=${String.format("%.8f", it.latitude)}, ")
|
||||
append("高度=${String.format("%.3f", it.altitude)}m, ")
|
||||
append("质量=${it.fixQuality}\n")
|
||||
} ?: append("主天线: 无数据\n")
|
||||
|
||||
subRTKData?.let {
|
||||
append("副天线: 经度=${String.format("%.8f", it.longitude)}, ")
|
||||
append("纬度=${String.format("%.8f", it.latitude)}, ")
|
||||
append("高度=${String.format("%.3f", it.altitude)}m, ")
|
||||
append("质量=${it.fixQuality}\n")
|
||||
} ?: append("副天线: 无数据\n")
|
||||
|
||||
// 添加传感器数据
|
||||
stickSensor?.let {
|
||||
append("传感器0: Roll=${String.format("%.2f", Math.toDegrees(it.roll))}°, ")
|
||||
append("Pitch=${String.format("%.2f", Math.toDegrees(it.pitch))}°, ")
|
||||
append("Yaw=${String.format("%.2f", Math.toDegrees(it.yaw))}°\n")
|
||||
} ?: append("传感器0: 无数据\n")
|
||||
|
||||
idleConnectingRodAngleSensor?.let {
|
||||
append("传感器1: Roll=${String.format("%.2f", Math.toDegrees(it.roll))}°, ")
|
||||
append("Pitch=${String.format("%.2f", Math.toDegrees(it.pitch))}°, ")
|
||||
append("Yaw=${String.format("%.2f", Math.toDegrees(it.yaw))}°\n")
|
||||
} ?: append("传感器1: 无数据\n")
|
||||
|
||||
// 添加计算方法和CE长度信息
|
||||
val eCalculationMethod = if (isCalibrationMode) {
|
||||
ECalculationMethod.USE_AE
|
||||
} else {
|
||||
ECalculationMethod.USE_CE
|
||||
}
|
||||
append("【计算方法】: $eCalculationMethod\n")
|
||||
append("【连杆长度】: ${config.linkageLengths}\n\n")
|
||||
|
||||
// 添加计算结果
|
||||
if (success && keyPoints != null) {
|
||||
append("铲斗位置计算成功:\n")
|
||||
val wgs84Positions = getKeyPointsWGS84Positions()
|
||||
wgs84Positions?.forEach { (name, position) ->
|
||||
val (lon, lat, alt) = position
|
||||
append("$name: 经度=${String.format("%.8f", lon)}, ")
|
||||
append("纬度=${String.format("%.8f", lat)}, ")
|
||||
append("高度=${String.format("%.3f", alt)}m\n")
|
||||
}
|
||||
} else {
|
||||
append("铲斗位置计算失败\n")
|
||||
}
|
||||
|
||||
append("\n")
|
||||
}
|
||||
|
||||
// 通知所有监听器
|
||||
val listenersCopy = ArrayList(listeners) // 创建副本避免并发修改
|
||||
listenersCopy.forEach { listener ->
|
||||
listener.onPositionCalculated(logMessage)
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,243 @@
|
||||
package com.icegps.excavator.model
|
||||
|
||||
import kotlin.math.*
|
||||
|
||||
/**
|
||||
* WGS84、EPSG3857、ENU 的坐标转换工具类
|
||||
*
|
||||
* @author lm
|
||||
* @date 2024/8/2
|
||||
*/
|
||||
class GeoHelper private constructor() {
|
||||
|
||||
companion object {
|
||||
private var sharedInstance: GeoHelper? = null
|
||||
|
||||
fun getSharedInstance(): GeoHelper {
|
||||
return sharedInstance ?: GeoHelper().also { sharedInstance = it }
|
||||
}
|
||||
|
||||
fun createInstance(): GeoHelper = GeoHelper()
|
||||
}
|
||||
|
||||
// WGS-84 ellipsoid parameters
|
||||
private val RADIUS = 6378137.0 // Major radius
|
||||
private val RADIUS_B = 6356752.314245 // Minor radius
|
||||
private val E = (RADIUS * RADIUS - RADIUS_B * RADIUS_B) / (RADIUS * RADIUS) // Eccentricity
|
||||
private val HALF_SIZE = Math.PI * RADIUS // Half circumference of Earth
|
||||
private val DEG2RAD = Math.PI / 180 // Degrees to radians conversion factor
|
||||
private val RAD2DEG = 180 / Math.PI // Radians to degrees conversion factor
|
||||
private val RE_WGS84 = 6378137.0 // Earth's equatorial radius in WGS84
|
||||
private val FE_WGS84 = 1.0 / 298.257223563 // Flattening of the WGS84 ellipsoid
|
||||
|
||||
private var isFirstPoint = true
|
||||
private val bPos = DoubleArray(3)
|
||||
private var bECEF = DoubleArray(3)
|
||||
private val rPos = DoubleArray(3)
|
||||
private var rECEF = DoubleArray(3)
|
||||
private val vECEF = DoubleArray(3)
|
||||
|
||||
private var useBlhToEnu: Boolean = true
|
||||
private var blhToEnu: BlhToEnu = BlhToEnu()
|
||||
|
||||
/**
|
||||
* 重置 ENU 基准点
|
||||
* 调用此方法后，下一次 wgs84ToENU 调用将设置新的基准点
|
||||
*/
|
||||
fun resetEnuBenchmarkPoint() {
|
||||
if (useBlhToEnu) {
|
||||
blhToEnu.resetEnuBenchmarkPoint()
|
||||
return
|
||||
}
|
||||
|
||||
isFirstPoint = true
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 WGS84 坐标转换为 ENU (East-North-Up) 坐标
|
||||
* 如果是第一个点，它将被设置为 ENU 坐标系的基准点
|
||||
*
|
||||
* @param lon 经度（度）
|
||||
* @param lat 纬度（度）
|
||||
* @param alt 高度（米）
|
||||
* @return 包含 ENU 坐标的 Enu 对象
|
||||
*/
|
||||
fun wgs84ToENU(lon: Double, lat: Double, alt: Double): Enu {
|
||||
if (useBlhToEnu) {
|
||||
val enu = blhToEnu.wgs84ToEnu(lon = lon, lat = lat, height = alt)
|
||||
return Enu(enu[0], enu[1], enu[2])
|
||||
}
|
||||
|
||||
if (isFirstPoint) setEnuBenchmark(lon, lat, alt)
|
||||
rPos[0] = lat * DEG2RAD
|
||||
rPos[1] = lon * DEG2RAD
|
||||
rPos[2] = alt
|
||||
|
||||
rECEF = pos2ecef(rPos)
|
||||
vECEF[0] = rECEF[0] - bECEF[0]
|
||||
vECEF[1] = rECEF[1] - bECEF[1]
|
||||
vECEF[2] = rECEF[2] - bECEF[2]
|
||||
val enuDoubleArray = ecef2enu(bPos, vECEF)
|
||||
return Enu(enuDoubleArray[0], enuDoubleArray[1], enuDoubleArray[2])
|
||||
}
|
||||
|
||||
/**
|
||||
* 设置 ENU 坐标系的基准点
|
||||
*
|
||||
* @param lon 基准点经度（度）
|
||||
* @param lat 基准点纬度（度）
|
||||
* @param alt 基准点高度（米）
|
||||
*/
|
||||
private fun setEnuBenchmark(lon: Double, lat: Double, alt: Double) {
|
||||
bPos[0] = lat * DEG2RAD
|
||||
bPos[1] = lon * DEG2RAD
|
||||
bPos[2] = alt
|
||||
bECEF = pos2ecef(bPos)
|
||||
isFirstPoint = false
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 ENU 坐标转换为 WGS84 坐标
|
||||
*
|
||||
* @param enu 包含 ENU 坐标的 Enu 对象
|
||||
* @return 包含 WGS84 坐标 {经度, 纬度, 高度} 的 DoubleArray
|
||||
*/
|
||||
fun enuToWGS84(enu: Enu): DoubleArray {
|
||||
if (useBlhToEnu) {
|
||||
val wgs84 = blhToEnu.enuToWgs84(e = enu.x, n = enu.y, u = enu.z)
|
||||
return doubleArrayOf(wgs84[1], wgs84[0], wgs84[2])
|
||||
}
|
||||
|
||||
val enuArray = doubleArrayOf(enu.x, enu.y, enu.z)
|
||||
val enuToEcefMatrix = xyz2enu(bPos)
|
||||
val ecefArray = matmul(charArrayOf('T', 'N'), 3, 1, 3, 1.0, enuToEcefMatrix, enuArray, 0.0)
|
||||
vECEF[0] = bECEF[0] + ecefArray[0]
|
||||
vECEF[1] = bECEF[1] + ecefArray[1]
|
||||
vECEF[2] = bECEF[2] + ecefArray[2]
|
||||
return ecef2pos(vECEF)
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 WGS84 坐标转换为 EPSG3857 坐标
|
||||
*
|
||||
* @param lon 经度（度）
|
||||
* @param lat 纬度（度）
|
||||
* @return 包含 EPSG3857 坐标的 EPSG3857 对象
|
||||
*/
|
||||
fun wgs84ToEPSG3857(lon: Double, lat: Double): EPSG3857 {
|
||||
val x = lon * HALF_SIZE / 180
|
||||
var y = RADIUS * ln(tan(Math.PI * (lat + 90) / 360))
|
||||
y = y.coerceIn(-HALF_SIZE, HALF_SIZE)
|
||||
return EPSG3857(x, y)
|
||||
}
|
||||
|
||||
/**
|
||||
* 将 EPSG3857 坐标转换为 WGS84 坐标
|
||||
*
|
||||
* @param epsg3857 包含 EPSG3857 坐标的 EPSG3857 对象
|
||||
* @return 包含 WGS84 坐标 {经度, 纬度} 的 DoubleArray
|
||||
*/
|
||||
fun epsg3857ToWGS84(epsg3857: EPSG3857): DoubleArray {
|
||||
val lon = (epsg3857.x / HALF_SIZE) * 180.0
|
||||
val lat = (2 * atan(exp(epsg3857.y / RADIUS)) - Math.PI / 2) * RAD2DEG
|
||||
return doubleArrayOf(lon, lat)
|
||||
}
|
||||
|
||||
private fun pos2ecef(pos: DoubleArray): DoubleArray {
|
||||
val (lat, lon, alt) = pos
|
||||
val sinp = sin(lat)
|
||||
val cosp = cos(lat)
|
||||
val sin_l = sin(lon)
|
||||
val cos_l = cos(lon)
|
||||
val e2 = FE_WGS84 * (2.0 - FE_WGS84)
|
||||
val v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp)
|
||||
|
||||
return doubleArrayOf(
|
||||
(v + alt) * cosp * cos_l,
|
||||
(v + alt) * cosp * sin_l,
|
||||
(v * (1.0 - e2) + alt) * sinp
|
||||
)
|
||||
}
|
||||
|
||||
private fun ecef2enu(pos: DoubleArray, r: DoubleArray): DoubleArray {
|
||||
val E = xyz2enu(pos)
|
||||
return matmul(charArrayOf('N', 'N'), 3, 1, 3, 1.0, E, r, 0.0)
|
||||
}
|
||||
|
||||
private fun matmul(
|
||||
tr: CharArray,
|
||||
n: Int,
|
||||
k: Int,
|
||||
m: Int,
|
||||
alpha: Double,
|
||||
A: DoubleArray,
|
||||
B: DoubleArray,
|
||||
beta: Double
|
||||
): DoubleArray {
|
||||
val f = when {
|
||||
tr[0] == 'N' && tr[1] == 'N' -> 1
|
||||
tr[0] == 'N' && tr[1] == 'T' -> 2
|
||||
tr[0] == 'T' && tr[1] == 'N' -> 3
|
||||
else -> 4
|
||||
}
|
||||
val C = DoubleArray(n * k)
|
||||
for (i in 0 until n) {
|
||||
for (j in 0 until k) {
|
||||
var d = 0.0
|
||||
when (f) {
|
||||
1 -> for (x in 0 until m) d += A[i + x * n] * B[x + j * m]
|
||||
2 -> for (x in 0 until m) d += A[i + x * n] * B[j + x * k]
|
||||
3 -> for (x in 0 until m) d += A[x + i * m] * B[x + j * m]
|
||||
4 -> for (x in 0 until m) d += A[x + i * m] * B[j + x * k]
|
||||
}
|
||||
C[i + j * n] = alpha * d + beta * C[i + j * n]
|
||||
}
|
||||
}
|
||||
return C
|
||||
}
|
||||
|
||||
private fun xyz2enu(pos: DoubleArray): DoubleArray {
|
||||
val (lat, lon) = pos
|
||||
val sinp = sin(lat)
|
||||
val cosp = cos(lat)
|
||||
val sin_l = sin(lon)
|
||||
val cos_l = cos(lon)
|
||||
|
||||
return doubleArrayOf(
|
||||
-sin_l, cos_l, 0.0,
|
||||
-sinp * cos_l, -sinp * sin_l, cosp,
|
||||
cosp * cos_l, cosp * sin_l, sinp
|
||||
)
|
||||
}
|
||||
|
||||
private fun ecef2pos(ecef: DoubleArray): DoubleArray {
|
||||
val (x, y, z) = ecef
|
||||
val a = RE_WGS84
|
||||
val b = a * (1 - FE_WGS84)
|
||||
val e2 = (a * a - b * b) / (a * a)
|
||||
val e2p = (a * a - b * b) / (b * b)
|
||||
val r2 = x * x + y * y
|
||||
val r = sqrt(r2)
|
||||
val E2 = a * a - b * b
|
||||
val F = 54 * b * b * z * z
|
||||
val G = r2 + (1 - e2) * z * z - e2 * E2
|
||||
val c = (e2 * e2 * F * r2) / (G * G * G)
|
||||
val s = cbrt(1 + c + sqrt(c * c + 2 * c))
|
||||
val P = F / (3 * (s + 1 / s + 1) * (s + 1 / s + 1) * G * G)
|
||||
val Q = sqrt(1 + 2 * e2 * e2 * P)
|
||||
val r0 =
|
||||
-(P * e2 * r) / (1 + Q) + sqrt(0.5 * a * a * (1 + 1.0 / Q) - P * (1 - e2) * z * z / (Q * (1 + Q)) - 0.5 * P * r2)
|
||||
val U = sqrt((r - e2 * r0) * (r - e2 * r0) + z * z)
|
||||
val V = sqrt((r - e2 * r0) * (r - e2 * r0) + (1 - e2) * z * z)
|
||||
val Z0 = b * b * z / (a * V)
|
||||
|
||||
val lon = atan2(y, x) * RAD2DEG
|
||||
val lat = atan((z + e2p * Z0) / r) * RAD2DEG
|
||||
val alt = U * (1 - b * b / (a * V))
|
||||
return doubleArrayOf(lon, lat, alt)
|
||||
}
|
||||
|
||||
data class EPSG3857(var x: Double = 0.0, var y: Double = 0.0)
|
||||
|
||||
data class Enu(var x: Double = 0.0, var y: Double = 0.0, var z: Double = 0.0)
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
package com.icegps.excavator.model
|
||||
|
||||
/**
|
||||
* @author xjh
|
||||
* @date 2025/2/25
|
||||
*/
|
||||
|
||||
/**
|
||||
* 数据类，存储RTK位置信息和姿态信息（包括航向角、俯仰角、滚转角）
|
||||
*/
|
||||
data class RTKData(
|
||||
val latitude: Double = 0.0, // 纬度，单位：度
|
||||
val longitude: Double = 0.0, // 经度，单位：度
|
||||
val altitude: Double = 0.0, // 高度，单位：米
|
||||
val fixQuality: Int = 0, // 定位质量：0=未定，1=单点，2=伪距，4=固定，5=浮动
|
||||
val satellites: Int = 0, // 使用的卫星数量
|
||||
val hdop: Double = 0.0, // 水平精度因子
|
||||
val time: String = "", // 时间戳（用于所有类型）
|
||||
val messageType: String = "GNGGA", // 消息类型：GNGGA、GNGGAH或GNHPR
|
||||
val yaw: Double = 0.0, // 航向角，单位：度
|
||||
val pitch: Double = 0.0, // 俯仰角，单位：度
|
||||
val roll: Double = 0.0 // 滚转角，单位：度
|
||||
) {
|
||||
/**
|
||||
* 检查RTK是否固定
|
||||
*/
|
||||
fun isRtkFixed(): Boolean = fixQuality == 4
|
||||
|
||||
/**
|
||||
* 检查是否为主天线数据
|
||||
*/
|
||||
fun isMainAntenna(): Boolean = messageType == "GNGGA"
|
||||
|
||||
/**
|
||||
* 检查是否为副天线数据
|
||||
*/
|
||||
fun isSubAntenna(): Boolean = messageType == "GNGGAH"
|
||||
|
||||
/**
|
||||
* 检查是否为姿态数据（如航向、俯仰、滚转）
|
||||
*/
|
||||
fun isAttitudeData(): Boolean = messageType == "GNHPR"
|
||||
|
||||
/**
|
||||
* 是否为有效的位置数据（主或副天线）
|
||||
*/
|
||||
fun hasPositionData(): Boolean = latitude != 0.0 && longitude != 0.0 && altitude != 0.0
|
||||
|
||||
override fun toString(): String {
|
||||
return if (isAttitudeData()) {
|
||||
"RTKData($messageType: heading=$yaw, pitch=$pitch, roll=$roll, time=$time)"
|
||||
} else {
|
||||
"RTKData($messageType: lat=$latitude, lon=$longitude, alt=$altitude, fix=$fixQuality, sats=$satellites, hdop=$hdop, time=$time)"
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user