668 lines
24 KiB
C#
668 lines
24 KiB
C#
using ClumsyCore;
|
|
using ClumsyCore.DTools;
|
|
using ClumsyCore.Interfaces;
|
|
using ClumsyCore.Pilot;
|
|
using CommonUsage.Chassis;
|
|
using MyParking.Shared;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Numerics;
|
|
|
|
namespace MultiWheelC
|
|
{
|
|
// C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
|
|
public sealed class CrabMotionFrameTracker : MovementDefinition
|
|
{
|
|
public enum ReferencePathKind
|
|
{
|
|
Straight = 0,
|
|
LeftArc = 1,
|
|
SCurve = 2
|
|
}
|
|
|
|
public ReferencePathKind PathKind;
|
|
public Vector2 StartPosition;
|
|
public double InitialBodyYawRadians;
|
|
public float LengthMillimeters = 4000f;
|
|
public float RadiusMillimeters = 2000f;
|
|
public float SCurveLateralOffsetMillimeters = 400f;
|
|
public double ArcSweepRadians = Math.PI / 2.0;
|
|
public float CruiseSpeed = 0.2f;
|
|
public float SlowDistanceMillimeters = 600f;
|
|
public float FinishDistanceMillimeters = 30f;
|
|
public float MinimumSpeed = 0.04f;
|
|
public double LateralGainPerSecond = 0.8;
|
|
public double MaximumLateralCorrection = 0.12;
|
|
public double HeadingGainPerSecond = 1.5;
|
|
public double MaximumAngularSpeedRadiansPerSecond =
|
|
30.0 * Math.PI / 180.0;
|
|
public float WheelAlignmentToleranceDegrees = 2f;
|
|
public float WheelAlignmentStableSeconds = 0.3f;
|
|
public float WheelAlignmentTimeoutSeconds = 10f;
|
|
public float TrackingTimeoutSeconds = 60f;
|
|
public Action<float, float, float> CommandObserver;
|
|
|
|
// 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
|
|
public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
|
|
private double _lastSCurveProgress;
|
|
|
|
public override IEnumerable<bool> Get()
|
|
{
|
|
ValidateParameters();
|
|
|
|
var chassis =
|
|
PilotDefinition.Chassis as MultiWheelChassis;
|
|
if (chassis == null)
|
|
throw new InvalidOperationException(
|
|
"当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");
|
|
|
|
var adapter = new MultiWheelChassisAdapter(
|
|
chassis,
|
|
PilotDefinition.Self.CarNum);
|
|
|
|
var lastCommandTime = DateTime.Now;
|
|
|
|
try
|
|
{
|
|
// 模式切换阶段只转舵轮，驱动速度始终保持为零。
|
|
var alignmentStarted = DateTime.Now;
|
|
DateTime? stableSince = null;
|
|
while (true)
|
|
{
|
|
if (!adapter.PrepareParallelDirection(
|
|
MotionFrameYawInBodyRadians))
|
|
throw new InvalidOperationException(
|
|
"无法生成运动坐标系对应的舵轮准备姿态。");
|
|
|
|
var aligned =
|
|
adapter.AreParallelWheelsAligned(
|
|
MotionFrameYawInBodyRadians,
|
|
WheelAlignmentToleranceDegrees *
|
|
Math.PI / 180.0);
|
|
|
|
if (aligned)
|
|
{
|
|
if (stableSince == null)
|
|
stableSince = DateTime.Now;
|
|
|
|
if ((DateTime.Now - stableSince.Value)
|
|
.TotalSeconds >=
|
|
WheelAlignmentStableSeconds)
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
stableSince = null;
|
|
}
|
|
|
|
if ((DateTime.Now - alignmentStarted)
|
|
.TotalSeconds >
|
|
WheelAlignmentTimeoutSeconds)
|
|
throw new TimeoutException(
|
|
"舵轮在限定时间内未稳定到达运动坐标系初始方向。");
|
|
|
|
yield return true;
|
|
}
|
|
|
|
var trackingStarted = DateTime.Now;
|
|
while (true)
|
|
{
|
|
if ((DateTime.Now - trackingStarted)
|
|
.TotalSeconds >
|
|
TrackingTimeoutSeconds)
|
|
throw new TimeoutException(
|
|
"蟹行轨迹在限定时间内未完成。");
|
|
|
|
var location =
|
|
DetourInterface.getCartLocation();
|
|
if (!IsFinite(location.x) ||
|
|
!IsFinite(location.y) ||
|
|
!IsFinite(location.th))
|
|
throw new InvalidOperationException(
|
|
"蟹行轨迹测试期间Detour位姿无效。");
|
|
|
|
var currentPosition = new Vector2(
|
|
(float)location.x,
|
|
(float)location.y);
|
|
var currentBodyYaw =
|
|
location.th * Math.PI / 180.0;
|
|
|
|
CalculateReference(
|
|
currentPosition,
|
|
out var tangentYaw,
|
|
out var referencePoint,
|
|
out var remainingMillimeters,
|
|
out var referenceCurvature);
|
|
|
|
if (remainingMillimeters <=
|
|
FinishDistanceMillimeters)
|
|
break;
|
|
|
|
var speed =
|
|
CalculateSpeed(remainingMillimeters);
|
|
var tangent = new Vector2(
|
|
(float)Math.Cos(tangentYaw),
|
|
(float)Math.Sin(tangentYaw));
|
|
var leftNormal = new Vector2(
|
|
-tangent.Y,
|
|
tangent.X);
|
|
var positionError =
|
|
currentPosition - referencePoint;
|
|
var lateralErrorMeters =
|
|
Vector2.Dot(
|
|
positionError,
|
|
leftNormal) / 1000.0;
|
|
var normalCorrection =
|
|
Limit(
|
|
-LateralGainPerSecond *
|
|
lateralErrorMeters,
|
|
MaximumLateralCorrection);
|
|
|
|
// 先在世界坐标中组合切向速度与横向纠偏速度。
|
|
var worldVx =
|
|
tangent.X * speed +
|
|
leftNormal.X * (float)normalCorrection;
|
|
var worldVy =
|
|
tangent.Y * speed +
|
|
leftNormal.Y * (float)normalCorrection;
|
|
|
|
// 将世界速度表达为当前蟹行运动坐标系速度。
|
|
var motionYaw =
|
|
currentBodyYaw +
|
|
MotionFrameYawInBodyRadians;
|
|
var motionCos = Math.Cos(motionYaw);
|
|
var motionSin = Math.Sin(motionYaw);
|
|
var vxInMotion =
|
|
motionCos * worldVx +
|
|
motionSin * worldVy;
|
|
var vyInMotion =
|
|
-motionSin * worldVx +
|
|
motionCos * worldVy;
|
|
|
|
var desiredBodyYaw =
|
|
tangentYaw -
|
|
MotionFrameYawInBodyRadians;
|
|
var headingError =
|
|
FrameTransform2D
|
|
.ShortestAngleDifference(
|
|
desiredBodyYaw,
|
|
currentBodyYaw);
|
|
var omega =
|
|
speed * referenceCurvature +
|
|
HeadingGainPerSecond * headingError;
|
|
omega = Limit(
|
|
omega,
|
|
MaximumAngularSpeedRadiansPerSecond);
|
|
|
|
// 运动坐标系相对车体系旋转+90°：
|
|
// 运动系正向速度会转换成车体系+Y速度。
|
|
var bodyTwist =
|
|
FrameTransform2D
|
|
.TransformTwistAtSamePoint(
|
|
new Pose2D(
|
|
0.0,
|
|
0.0,
|
|
MotionFrameYawInBodyRadians),
|
|
new Twist2D(
|
|
vxInMotion,
|
|
vyInMotion,
|
|
omega));
|
|
|
|
var now = DateTime.Now;
|
|
var interval = now - lastCommandTime;
|
|
lastCommandTime = now;
|
|
var command = new ChassisCommand(
|
|
PilotDefinition.Self.CarNum,
|
|
bodyTwist);
|
|
|
|
if (!adapter.Send(command, interval))
|
|
throw new InvalidOperationException(
|
|
"蟹行轨迹底盘解算失败：" +
|
|
adapter.LastFailureReason);
|
|
|
|
CommandObserver?.Invoke(
|
|
(float)bodyTwist.VxMetersPerSecond,
|
|
(float)bodyTwist.VyMetersPerSecond,
|
|
(float)bodyTwist
|
|
.OmegaRadiansPerSecond);
|
|
|
|
yield return true;
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
adapter.StopImmediately();
|
|
CommandObserver?.Invoke(0f, 0f, 0f);
|
|
}
|
|
|
|
yield return false;
|
|
}
|
|
|
|
// 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
|
|
private void CalculateReference(
|
|
Vector2 currentPosition,
|
|
out double tangentYaw,
|
|
out Vector2 referencePoint,
|
|
out float remainingMillimeters,
|
|
out double curvaturePerMeter)
|
|
{
|
|
var initialMotionYaw =
|
|
InitialBodyYawRadians +
|
|
MotionFrameYawInBodyRadians;
|
|
|
|
if (PathKind == ReferencePathKind.Straight)
|
|
{
|
|
var tangent = new Vector2(
|
|
(float)Math.Cos(initialMotionYaw),
|
|
(float)Math.Sin(initialMotionYaw));
|
|
var relative = currentPosition - StartPosition;
|
|
var progress =
|
|
Vector2.Dot(relative, tangent);
|
|
var clampedProgress =
|
|
Math.Max(
|
|
0f,
|
|
Math.Min(progress, LengthMillimeters));
|
|
|
|
tangentYaw = initialMotionYaw;
|
|
referencePoint =
|
|
StartPosition +
|
|
tangent * clampedProgress;
|
|
remainingMillimeters =
|
|
Math.Max(
|
|
0f,
|
|
LengthMillimeters - progress);
|
|
curvaturePerMeter = 0.0;
|
|
return;
|
|
}
|
|
|
|
if (PathKind == ReferencePathKind.SCurve)
|
|
{
|
|
CalculateSCurveReference(
|
|
currentPosition,
|
|
initialMotionYaw,
|
|
out tangentYaw,
|
|
out referencePoint,
|
|
out remainingMillimeters,
|
|
out curvaturePerMeter);
|
|
return;
|
|
}
|
|
|
|
var center = GetArcCenter();
|
|
var startRadialYaw =
|
|
initialMotionYaw - Math.PI / 2.0;
|
|
var radial = currentPosition - center;
|
|
var currentRadialYaw =
|
|
Math.Atan2(radial.Y, radial.X);
|
|
var progressRadians =
|
|
FrameTransform2D.NormalizeAngle(
|
|
currentRadialYaw - startRadialYaw);
|
|
|
|
// 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
|
|
if (progressRadians < 0.0)
|
|
progressRadians = 0.0;
|
|
|
|
var clampedProgressRadians =
|
|
Math.Min(
|
|
progressRadians,
|
|
ArcSweepRadians);
|
|
var referenceRadialYaw =
|
|
startRadialYaw +
|
|
clampedProgressRadians;
|
|
referencePoint = center + new Vector2(
|
|
RadiusMillimeters *
|
|
(float)Math.Cos(referenceRadialYaw),
|
|
RadiusMillimeters *
|
|
(float)Math.Sin(referenceRadialYaw));
|
|
tangentYaw =
|
|
referenceRadialYaw + Math.PI / 2.0;
|
|
remainingMillimeters =
|
|
(float)Math.Max(
|
|
0.0,
|
|
(ArcSweepRadians - progressRadians) *
|
|
RadiusMillimeters);
|
|
curvaturePerMeter =
|
|
1000.0 / RadiusMillimeters;
|
|
}
|
|
|
|
// 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
|
|
private void CalculateSCurveReference(
|
|
Vector2 currentPosition,
|
|
double initialMotionYaw,
|
|
out double tangentYaw,
|
|
out Vector2 referencePoint,
|
|
out float remainingMillimeters,
|
|
out double curvaturePerMeter)
|
|
{
|
|
const int nearestPointSamples = 200;
|
|
var searchStart =
|
|
Math.Max(
|
|
0.0,
|
|
_lastSCurveProgress - 0.02);
|
|
var bestProgress = _lastSCurveProgress;
|
|
var bestDistanceSquared = double.MaxValue;
|
|
|
|
for (var i = 0;
|
|
i <= nearestPointSamples;
|
|
i++)
|
|
{
|
|
var progress =
|
|
searchStart +
|
|
(1.0 - searchStart) *
|
|
i / nearestPointSamples;
|
|
EvaluateSCurve(
|
|
progress,
|
|
out var localPoint,
|
|
out _,
|
|
out _);
|
|
var worldPoint =
|
|
LocalPathPointToWorld(
|
|
localPoint,
|
|
initialMotionYaw);
|
|
var distanceSquared =
|
|
Vector2.DistanceSquared(
|
|
currentPosition,
|
|
worldPoint);
|
|
|
|
if (distanceSquared <
|
|
bestDistanceSquared)
|
|
{
|
|
bestDistanceSquared =
|
|
distanceSquared;
|
|
bestProgress = progress;
|
|
}
|
|
}
|
|
|
|
// 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
|
|
_lastSCurveProgress =
|
|
Math.Max(
|
|
_lastSCurveProgress,
|
|
bestProgress);
|
|
EvaluateSCurve(
|
|
_lastSCurveProgress,
|
|
out var bestLocalPoint,
|
|
out var firstDerivative,
|
|
out var secondDerivative);
|
|
referencePoint =
|
|
LocalPathPointToWorld(
|
|
bestLocalPoint,
|
|
initialMotionYaw);
|
|
tangentYaw =
|
|
initialMotionYaw +
|
|
Math.Atan2(
|
|
firstDerivative.Y,
|
|
firstDerivative.X);
|
|
|
|
var derivativeMagnitude =
|
|
Math.Sqrt(
|
|
firstDerivative.X *
|
|
firstDerivative.X +
|
|
firstDerivative.Y *
|
|
firstDerivative.Y);
|
|
if (derivativeMagnitude < 1e-6)
|
|
{
|
|
curvaturePerMeter = 0.0;
|
|
}
|
|
else
|
|
{
|
|
// 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
|
|
curvaturePerMeter =
|
|
(firstDerivative.X *
|
|
secondDerivative.Y -
|
|
firstDerivative.Y *
|
|
secondDerivative.X) *
|
|
1000.0 /
|
|
Math.Pow(
|
|
derivativeMagnitude,
|
|
3.0);
|
|
}
|
|
|
|
remainingMillimeters =
|
|
ApproximateSCurveRemainingLength(
|
|
_lastSCurveProgress);
|
|
}
|
|
|
|
// 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
|
|
private void EvaluateSCurve(
|
|
double progress,
|
|
out Vector2 point,
|
|
out Vector2 firstDerivative,
|
|
out Vector2 secondDerivative)
|
|
{
|
|
progress =
|
|
Math.Max(
|
|
0.0,
|
|
Math.Min(progress, 1.0));
|
|
|
|
Vector2 p0;
|
|
Vector2 p1;
|
|
Vector2 p2;
|
|
Vector2 p3;
|
|
double t;
|
|
|
|
if (progress <= 0.25)
|
|
{
|
|
t = progress * 4.0;
|
|
p0 = new Vector2(0f, 0f);
|
|
p1 = new Vector2(
|
|
LengthMillimeters / 12f,
|
|
0f);
|
|
p2 = new Vector2(
|
|
LengthMillimeters / 6f,
|
|
SCurveLateralOffsetMillimeters);
|
|
p3 = new Vector2(
|
|
LengthMillimeters * 0.25f,
|
|
SCurveLateralOffsetMillimeters);
|
|
}
|
|
else if (progress <= 0.75)
|
|
{
|
|
t = (progress - 0.25) * 2.0;
|
|
p0 = new Vector2(
|
|
LengthMillimeters * 0.25f,
|
|
SCurveLateralOffsetMillimeters);
|
|
p1 = new Vector2(
|
|
LengthMillimeters / 3f,
|
|
SCurveLateralOffsetMillimeters);
|
|
p2 = new Vector2(
|
|
LengthMillimeters * 2f / 3f,
|
|
-SCurveLateralOffsetMillimeters);
|
|
p3 = new Vector2(
|
|
LengthMillimeters * 0.75f,
|
|
-SCurveLateralOffsetMillimeters);
|
|
}
|
|
else
|
|
{
|
|
t = (progress - 0.75) * 4.0;
|
|
p0 = new Vector2(
|
|
LengthMillimeters * 0.75f,
|
|
-SCurveLateralOffsetMillimeters);
|
|
p1 = new Vector2(
|
|
LengthMillimeters * 5f / 6f,
|
|
-SCurveLateralOffsetMillimeters);
|
|
p2 = new Vector2(
|
|
LengthMillimeters * 11f / 12f,
|
|
0f);
|
|
p3 = new Vector2(
|
|
LengthMillimeters,
|
|
0f);
|
|
}
|
|
|
|
var oneMinusT = 1.0 - t;
|
|
point =
|
|
p0 * (float)(
|
|
oneMinusT *
|
|
oneMinusT *
|
|
oneMinusT) +
|
|
p1 * (float)(
|
|
3.0 *
|
|
oneMinusT *
|
|
oneMinusT *
|
|
t) +
|
|
p2 * (float)(
|
|
3.0 *
|
|
oneMinusT *
|
|
t *
|
|
t) +
|
|
p3 * (float)(t * t * t);
|
|
firstDerivative =
|
|
(p1 - p0) *
|
|
(float)(
|
|
3.0 *
|
|
oneMinusT *
|
|
oneMinusT) +
|
|
(p2 - p1) *
|
|
(float)(
|
|
6.0 *
|
|
oneMinusT *
|
|
t) +
|
|
(p3 - p2) *
|
|
(float)(3.0 * t * t);
|
|
secondDerivative =
|
|
(p2 - 2f * p1 + p0) *
|
|
(float)(6.0 * oneMinusT) +
|
|
(p3 - 2f * p2 + p1) *
|
|
(float)(6.0 * t);
|
|
}
|
|
|
|
// 通过分段采样估算从当前S曲线进度到终点的实际弧长。
|
|
private float ApproximateSCurveRemainingLength(
|
|
double startProgress)
|
|
{
|
|
const int lengthSamples = 100;
|
|
EvaluateSCurve(
|
|
startProgress,
|
|
out var previousPoint,
|
|
out _,
|
|
out _);
|
|
var length = 0f;
|
|
|
|
for (var i = 1;
|
|
i <= lengthSamples;
|
|
i++)
|
|
{
|
|
var progress =
|
|
startProgress +
|
|
(1.0 - startProgress) *
|
|
i / lengthSamples;
|
|
EvaluateSCurve(
|
|
progress,
|
|
out var point,
|
|
out _,
|
|
out _);
|
|
length +=
|
|
Vector2.Distance(
|
|
previousPoint,
|
|
point);
|
|
previousPoint = point;
|
|
}
|
|
|
|
return length;
|
|
}
|
|
|
|
// 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
|
|
private Vector2 LocalPathPointToWorld(
|
|
Vector2 localPoint,
|
|
double initialMotionYaw)
|
|
{
|
|
var cos =
|
|
(float)Math.Cos(initialMotionYaw);
|
|
var sin =
|
|
(float)Math.Sin(initialMotionYaw);
|
|
|
|
return StartPosition + new Vector2(
|
|
localPoint.X * cos -
|
|
localPoint.Y * sin,
|
|
localPoint.X * sin +
|
|
localPoint.Y * cos);
|
|
}
|
|
|
|
// 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
|
|
public Vector2 GetArcCenter()
|
|
{
|
|
var initialMotionYaw =
|
|
InitialBodyYawRadians +
|
|
MotionFrameYawInBodyRadians;
|
|
return StartPosition + new Vector2(
|
|
-RadiusMillimeters *
|
|
(float)Math.Sin(initialMotionYaw),
|
|
RadiusMillimeters *
|
|
(float)Math.Cos(initialMotionYaw));
|
|
}
|
|
|
|
// 获取圆弧测试的理论终点。
|
|
public Vector2 GetArcDestination()
|
|
{
|
|
var initialMotionYaw =
|
|
InitialBodyYawRadians +
|
|
MotionFrameYawInBodyRadians;
|
|
var startRadialYaw =
|
|
initialMotionYaw - Math.PI / 2.0;
|
|
var endRadialYaw =
|
|
startRadialYaw + ArcSweepRadians;
|
|
var center = GetArcCenter();
|
|
|
|
return center + new Vector2(
|
|
RadiusMillimeters *
|
|
(float)Math.Cos(endRadialYaw),
|
|
RadiusMillimeters *
|
|
(float)Math.Sin(endRadialYaw));
|
|
}
|
|
|
|
// 根据剩余路径长度生成终点减速速度。
|
|
private float CalculateSpeed(
|
|
float remainingMillimeters)
|
|
{
|
|
if (remainingMillimeters >=
|
|
SlowDistanceMillimeters)
|
|
return CruiseSpeed;
|
|
|
|
var ratio =
|
|
remainingMillimeters /
|
|
Math.Max(
|
|
SlowDistanceMillimeters,
|
|
1f);
|
|
return Math.Max(
|
|
MinimumSpeed,
|
|
CruiseSpeed * ratio);
|
|
}
|
|
|
|
private void ValidateParameters()
|
|
{
|
|
if (CruiseSpeed <= 0f ||
|
|
!IsFinite(CruiseSpeed) ||
|
|
LengthMillimeters <= 0f ||
|
|
!IsFinite(LengthMillimeters) ||
|
|
RadiusMillimeters <= 0f ||
|
|
!IsFinite(RadiusMillimeters) ||
|
|
SCurveLateralOffsetMillimeters <= 0f ||
|
|
!IsFinite(
|
|
SCurveLateralOffsetMillimeters) ||
|
|
ArcSweepRadians <= 0.0 ||
|
|
!IsFinite(ArcSweepRadians) ||
|
|
SlowDistanceMillimeters <= 0f ||
|
|
!IsFinite(SlowDistanceMillimeters) ||
|
|
FinishDistanceMillimeters < 0f ||
|
|
!IsFinite(FinishDistanceMillimeters) ||
|
|
TrackingTimeoutSeconds <= 0f ||
|
|
!IsFinite(TrackingTimeoutSeconds))
|
|
throw new ArgumentOutOfRangeException(
|
|
"蟹行轨迹测试参数无效。");
|
|
}
|
|
|
|
private static double Limit(
|
|
double value,
|
|
double absoluteLimit)
|
|
{
|
|
return Math.Max(
|
|
-absoluteLimit,
|
|
Math.Min(value, absoluteLimit));
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return
|
|
!double.IsNaN(value) &&
|
|
!double.IsInfinity(value);
|
|
}
|
|
}
|
|
}
|