From 6ddafa3dd2f1f24845cb5aca1f0533690fd7fe12 Mon Sep 17 00:00:00 2001 From: emelianov Date: Wed, 10 Jun 2026 13:33:07 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9F=D0=A3=D0=A3=D0=94=20=D1=81=20=D0=BF?= =?UTF-8?q?=D0=BE=D1=81=D1=82=D0=BE=D1=8F=D0=BD=D0=BD=D0=BE=D0=B9=20=D0=BE?= =?UTF-8?q?=D1=80=D0=B8=D0=B5=D0=BD=D1=82=D0=B0=D1=86=D0=B8=D0=B5=D0=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../pcp/angularmotion/ConstOrientPUUD.kt | 161 +++++++++++++++--- .../nstart/pcp/angularmotion/InternalMath.kt | 31 +++- .../AngularMotionCalculatorSmokeTest.kt | 25 ++- 3 files changed, 190 insertions(+), 27 deletions(-) diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt index 7c9c4d2..1623262 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt +++ b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/ConstOrientPUUD.kt @@ -3,7 +3,9 @@ package space.nstart.pcp.angularmotion import ballistics.orbitalPoints.timeStepper.AbstractStepper import ballistics.types.BLHPoint import ballistics.types.EarthType +import ballistics.types.OrbitalPoint import ballistics.types.Orientation +import ballistics.utils.math.Quaternion3D import ballistics.utils.math.Vector3D /** @@ -33,39 +35,143 @@ class ConstOrientPUUD( } private fun calculateConst(id: SurveyId, tn: Double, duration: Double): List { - val result = mutableListOf() val step = calculationStep() + + // В режиме ConstOrient направление ЦЛВ в ОСК фиксируется на старте. + // Это задает постоянные крен и тангаж. Оставшаяся степень свободы — + // рысканье вокруг ЦЛВ. В отличие от геометрического варианта, где рысканье + // выбиралось по направлению трассы, здесь рысканье интегрируется из самого + // критерия Wz/D = 0. Такой подход совпадает по смыслу с AzimuthPUUD: + // сначала находится угловая скорость, при которой wd.z равен нулю, затем по + // этой скорости строится программа углового движения. + val fixedClvOrientation = orientOnPoint(tn, id.b, id.l, id.h) + val tang = fixedClvOrientation.tang + val kren = fixedClvOrientation.kren + var risk = buildConstSample(id, tn, step, fixedClvOrientation).orientation.risk + + val result = mutableListOf() var t = tn var elapsed = 0.0 var previous: AngularMotionPoint? = null - // В режиме ConstOrient ЦЛВ не должна сопровождать одну и ту же земную точку. - // На старте фиксируем направление ЦЛВ в ОСК, а далее на каждом шаге заново - // пересекаем эту фиксированную ЦЛВ с Землей. Важно, чтобы первая точка маршрута - // тоже была получена через такое пересечение: иначе первый шаг сшивает заданную - // исходную ЦЛМ с расчетной трассой постоянной ЦЛВ и дает искусственный скачок рысканья. - val fixedClvOrientation = orientOnPoint(tn, id.b, id.l, id.h) - - while (elapsed <= duration + 2.0 * step + EPS) { - val currentGround = pointOnFixedClv(t, fixedClvOrientation) - ?: BLHPoint(id.b, id.l, id.h) - val routeDirectionGsk = fixedClvGroundDirection(t, step, fixedClvOrientation, currentGround) - val orientation = orientOnPoint( - t, - currentGround.lat, - currentGround.long, - currentGround.h, - routeDirectionGsk, - ) - val point = buildPoint(t, orientation, previous) + while (elapsed <= duration + EPS) { + val orientation = Orientation(tang, kren, normalizeAngle(risk)) + val omega = omegaForWdzZero(t, orientation) + val point = buildConstPoint(t, orientation, omega, previous) result += point previous = point + if (elapsed + step <= duration + EPS) { + risk = integrateRiskRK4(t, risk, step, tang, kren) + } elapsed += step t += step } - return if (result.size > 2) result.dropLast(2) else result + return result + } + + /** + * В AzimuthPUUD условие wd.z = 0 зашито непосредственно в формулы угловой + * скорости визирной системы. Для ConstOrient оставшаяся степень свободы только + * одна — производная рысканья. Поэтому в каждой точке решаем линейное уравнение + * по скорости рысканья: + * + * wd.z(t, risk, riskDot) = 0. + * + * Функция wd линейна по omega, а omega линейна по riskDot, поэтому достаточно + * двух расчетов: при riskDot = 0 и при riskDot = 1 рад/с. + */ + private fun riskRateForWdzZero(t: Double, orientation: Orientation): Double { + val point = pointAt(t) + val omega0 = omegaForRiskRate(t, orientation, 0.0) + val wz0 = wd(point, orientation, omega0).z + val omega1 = omegaForRiskRate(t, orientation, 1.0) + val wz1 = wd(point, orientation, omega1).z + val denom = wz1 - wz0 + return if (!denom.isFinite() || kotlin.math.abs(denom) < WDZ_SOLVER_EPS) { + 0.0 + } else { + -wz0 / denom + } + } + + private fun omegaForWdzZero(t: Double, orientation: Orientation): Vector3D = + omegaForRiskRate(t, orientation, riskRateForWdzZero(t, orientation)) + + /** + * Численно строит мгновенную угловую скорость ПСК при заданной скорости рысканья. + * Малый внутренний шаг нужен только для дифференцирования кватерниона; он не + * задает дискрет выдачи ПУУД. + */ + private fun omegaForRiskRate(t: Double, orientation: Orientation, riskRate: Double): Vector3D { + val dt = OMEGA_DERIVATIVE_STEP + val q0 = quaternionFor(pointAt(t), orientation) + val nextOrientation = Orientation( + orientation.tang, + orientation.kren, + normalizeAngle(orientation.risk + riskRate * dt), + ) + val q1 = quaternionFor(pointAt(t + dt), nextOrientation) + return omegaFromTwoQuat(q0, q1, dt) + } + + private fun integrateRiskRK4(t: Double, risk: Double, step: Double, tang: Double, kren: Double): Double { + fun f(time: Double, value: Double): Double = + riskRateForWdzZero(time, Orientation(tang, kren, normalizeAngle(value))) + + val k1 = f(t, risk) + val k2 = f(t + step / 2.0, risk + k1 * step / 2.0) + val k3 = f(t + step / 2.0, risk + k2 * step / 2.0) + val k4 = f(t + step, risk + k3 * step) + return normalizeAngle(risk + step * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0) + } + + private fun buildConstPoint( + t: Double, + orientation: Orientation, + omega: Vector3D, + previous: AngularMotionPoint?, + ): AngularMotionPoint { + val orbital = pointAt(t) + val ground = pointOnEarth(orbital, orientation) + val q = quaternionFor(orbital, orientation) + val eps = previous?.let { if (kotlin.math.abs(t - it.t) > EPS) (omega - it.omega) / (t - it.t) else Vector3D() } + ?: Vector3D() + val wd = wd(orbital, orientation, omega) + return AngularMotionPoint( + t = t, + orbitalPoint = orbital, + orientation = Orientation(orientation.tang, orientation.kren, orientation.risk), + groundPoint = ground, + omega = omega, + eps = eps, + quaternion = q, + wd = wd, + sdi = sdiForWd(wd), + ) + } + + private fun buildConstSample( + id: SurveyId, + t: Double, + step: Double, + fixedClvOrientation: Orientation, + ): ConstSample { + val orbital = pointAt(t) + val currentGround = pointOnFixedClv(t, fixedClvOrientation) + ?: BLHPoint(id.b, id.l, id.h) + val routeDirectionGsk = fixedClvGroundDirection(t, step, fixedClvOrientation, currentGround) + val orientation = orientOnPoint( + t, + currentGround.lat, + currentGround.long, + currentGround.h, + routeDirectionGsk, + ) + val ground = pointOnEarth(orbital, orientation) ?: currentGround + val quaternion = quaternionFor(orbital, orientation) + return ConstSample(t, orbital, orientation, ground, quaternion) } private fun pointOnFixedClv(t: Double, fixedClvOrientation: Orientation): BLHPoint? = @@ -93,4 +199,17 @@ class ConstOrientPUUD( val second = astro.earth.blh2xyz(to.lat, to.long, to.h) return second - first } + + private data class ConstSample( + val t: Double, + val orbitalPoint: OrbitalPoint, + val orientation: Orientation, + val groundPoint: BLHPoint?, + val quaternion: Quaternion3D, + ) + + private companion object { + const val WDZ_SOLVER_EPS = 1.0e-14 + const val OMEGA_DERIVATIVE_STEP = 1.0e-2 + } } diff --git a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt index 9872143..34cc830 100644 --- a/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt +++ b/libs/angular-motion-lib/src/main/kotlin/space/nstart/pcp/angularmotion/InternalMath.kt @@ -158,10 +158,35 @@ internal fun quaternionFromEuler(tang: Double, kren: Double, risk: Double): Quat internal fun omegaFromTwoQuat(q1: Quaternion3D, q2: Quaternion3D, step: Double): Vector3D { if (abs(step) < EPS) return Vector3D() val q1n = q1.normalized() - val dq = (q2.normalized() - q1n).scaled(1.0 / step) - val l1 = Vector3D(q1n.q1, q1n.q2, q1n.q3) + val q2n = alignQuaternion(q1n, q2.normalized()) + val dq = (q2n - q1n).scaled(1.0 / step) + return omegaFromQuaternionDerivative(q1n, dq) +} + +internal fun omegaFromThreeQuat( + qPrevious: Quaternion3D, + qCurrent: Quaternion3D, + qNext: Quaternion3D, + step: Double, +): Vector3D { + if (abs(step) < EPS) return Vector3D() + val current = qCurrent.normalized() + val previous = alignQuaternion(current, qPrevious.normalized()) + val next = alignQuaternion(current, qNext.normalized()) + val dq = (next - previous).scaled(1.0 / (2.0 * step)) + return omegaFromQuaternionDerivative(current, dq) +} + +private fun alignQuaternion(reference: Quaternion3D, candidate: Quaternion3D): Quaternion3D { + val dot = reference.q0 * candidate.q0 + reference.q1 * candidate.q1 + + reference.q2 * candidate.q2 + reference.q3 * candidate.q3 + return if (dot < 0.0) candidate.scaled(-1.0) else candidate +} + +private fun omegaFromQuaternionDerivative(q: Quaternion3D, dq: Quaternion3D): Vector3D { + val l1 = Vector3D(q.q1, q.q2, q.q3) val l2 = Vector3D(dq.q1, dq.q2, dq.q3) - return (l1 * (-dq.q0) + l2 * q1n.q0 - (l1.rem(l2))) * 2.0 + return (l1 * (-dq.q0) + l2 * q.q0 - (l1.rem(l2))) * 2.0 } internal fun integrateQuaternionRK4( diff --git a/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt b/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt index 616f29c..e6f12d3 100644 --- a/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt +++ b/libs/angular-motion-lib/src/test/kotlin/space/nstart/pcp/angularmotion/AngularMotionCalculatorSmokeTest.kt @@ -138,7 +138,25 @@ class AngularMotionCalculatorSmokeTest { println(rc.startTime) for (v in rc.points) { - println("${toDateTime( v.t)} ${v.orientation.tang * 180 / PI} ${v.orientation.kren * 180 / PI } ${v.orientation.risk * 180 / PI}") + println( + String.format( + Locale.US, + "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f , %.3f", + toDateTime(v.t).format(outputTimeFormatter), + v.orientation.tang * 180 / PI, + v.orientation.kren * 180 / PI, + v.orientation.risk * 180 / PI, + v.groundPoint?.let { String.format(Locale.US, "%.3f", it.lat * 180 / PI) }, + v.groundPoint?.let { String.format(Locale.US, "%.3f", it.long * 180 / PI) }, + v.omega.x, + v.omega.y, + v.omega.z, + v.wd.x, + v.wd.y, + v.wd.z, + v.sdi + ) + ) } } @@ -200,7 +218,7 @@ class AngularMotionCalculatorSmokeTest { println( String.format( Locale.US, - "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f", + "%s %.3f %.3f %.3f , %s %s , %.7f %.7f %.7f , %.7f %.7f %.7f , %.3f", toDateTime(v.t).format(outputTimeFormatter), v.orientation.tang * 180 / PI, v.orientation.kren * 180 / PI, @@ -212,7 +230,8 @@ class AngularMotionCalculatorSmokeTest { v.omega.z, v.wd.x, v.wd.y, - v.wd.z + v.wd.z, + v.sdi ) ) }