563 lines
20 KiB
C#
563 lines
20 KiB
C#
using System;
|
||
using System.Diagnostics;
|
||
using System.Globalization;
|
||
using System.Numerics;
|
||
using System.Threading;
|
||
using ClumsyCore;
|
||
using ClumsyCore.Pilot;
|
||
using CommonUsage.Chassis;
|
||
using FundamentalLib;
|
||
using MultiWheelC.StateEstimation;
|
||
using MyParking.Shared;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
/// <summary>
|
||
/// 提供纵向开环辨识测试共用的舵轮准备、速度下发、采样和安全停车流程。
|
||
/// </summary>
|
||
public abstract class LongitudinalIdentificationTestBase
|
||
: MovementTest
|
||
{
|
||
private const double MaximumTargetSpeedMetersPerSecond =
|
||
0.70;
|
||
private const double MinimumTargetSpeedMetersPerSecond =
|
||
0.02;
|
||
private const float MillimetersPerMeter = 1000f;
|
||
|
||
private DriveTask _preparationTask;
|
||
private MultiWheelChassis _chassis;
|
||
private MultiWheelChassisAdapter _adapter;
|
||
private WheelFeedbackVehicleStateProvider _stateProvider;
|
||
private TrackingExperimentRecorder _recorder;
|
||
private int _testRunning;
|
||
private int _stopRequested;
|
||
|
||
/// <summary>
|
||
/// 获取或设置本次重复实验编号。
|
||
/// </summary>
|
||
public int TrialNumber = 1;
|
||
|
||
/// <summary>
|
||
/// 获取或设置速度阶跃前的静止记录时间，单位为s。
|
||
/// </summary>
|
||
public double BaselineSeconds = 1.0;
|
||
|
||
/// <summary>
|
||
/// 获取或设置目标速度保持时间，单位为s。
|
||
/// </summary>
|
||
public double CommandHoldSeconds = 5.0;
|
||
|
||
/// <summary>
|
||
/// 获取或设置停车后的继续记录时间，单位为s。
|
||
/// </summary>
|
||
public double PostStopSeconds = 2.0;
|
||
|
||
/// <summary>
|
||
/// 获取或设置速度命令循环周期，单位为ms。
|
||
/// </summary>
|
||
public int ControlIntervalMilliseconds = 50;
|
||
|
||
/// <summary>
|
||
/// 派生测试选择是否使用底盘DeAccPerSecond完成正常减速。
|
||
/// </summary>
|
||
protected abstract bool UseConfiguredDeceleration { get; }
|
||
|
||
/// <summary>
|
||
/// 获取用于CSV文件名区分停车方式的标识。
|
||
/// </summary>
|
||
protected abstract string StopModeName { get; }
|
||
|
||
/// <summary>
|
||
/// 读取目标速度，完成舵轮回正后执行纵向开环命令并保存CSV。
|
||
/// </summary>
|
||
public override void Test()
|
||
{
|
||
if (Interlocked.CompareExchange(
|
||
ref _testRunning,
|
||
1,
|
||
0) != 0)
|
||
{
|
||
Console.WriteLine(
|
||
"纵向辨识测试已经在运行，请先停止当前测试。");
|
||
return;
|
||
}
|
||
|
||
Interlocked.Exchange(ref _stopRequested, 0);
|
||
|
||
try
|
||
{
|
||
ValidateSettings();
|
||
if (!TryReadTargetSpeed(
|
||
out var targetSpeedMetersPerSecond))
|
||
{
|
||
return;
|
||
}
|
||
|
||
_chassis =
|
||
PilotDefinition.Chassis as MultiWheelChassis;
|
||
if (_chassis == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"当前底盘不是MultiWheelChassis，无法执行纵向辨识。");
|
||
}
|
||
|
||
ValidateChassisAndTarget(
|
||
targetSpeedMetersPerSecond);
|
||
|
||
Console.WriteLine(
|
||
"纵向辨识开始前请确认M层轮速诊断记录已经开启。");
|
||
Hedingben.ToastText(
|
||
"请确认M层轮速诊断记录已开启。",
|
||
"LongitudinalIdentification");
|
||
|
||
if (!MovementTestPreparation.AlignWheelsForward(
|
||
ref _preparationTask))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"四个舵轮未能稳定回正，纵向辨识已经取消。");
|
||
}
|
||
|
||
ThrowIfStopRequested();
|
||
|
||
_adapter = new MultiWheelChassisAdapter(
|
||
_chassis,
|
||
PilotDefinition.Self.CarNum);
|
||
_adapter.ResetToBodyFrame();
|
||
_adapter.StopImmediately();
|
||
|
||
_stateProvider =
|
||
ParkingVehicleStateProviderFactory.Create(
|
||
_chassis);
|
||
if (!_stateProvider.TryGetState(
|
||
out var initialState))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法读取纵向辨识起点位姿：" +
|
||
_stateProvider.LastFailureReason);
|
||
}
|
||
|
||
_recorder = CreateRecorder(
|
||
targetSpeedMetersPerSecond,
|
||
initialState.PoseInWorld);
|
||
_recorder.Start();
|
||
|
||
Console.WriteLine(
|
||
$"纵向辨识参数：目标速度={targetSpeedMetersPerSecond:F3}m/s，" +
|
||
$"AccPerSecond={_chassis.AccPerSecond:F3}m/s²，" +
|
||
$"DeAccPerSecond={_chassis.DeAccPerSecond:F3}m/s²，" +
|
||
$"保持={CommandHoldSeconds:F1}s，停车方式={StopModeName}。");
|
||
|
||
RunStationaryPhase(
|
||
BaselineSeconds,
|
||
"静止基线");
|
||
RunTargetSpeedPhase(
|
||
targetSpeedMetersPerSecond);
|
||
|
||
if (UseConfiguredDeceleration)
|
||
{
|
||
RunConfiguredDecelerationPhase(
|
||
targetSpeedMetersPerSecond);
|
||
}
|
||
else
|
||
{
|
||
_adapter.StopImmediately();
|
||
RunStationaryPhase(
|
||
PostStopSeconds,
|
||
"立即停车后记录");
|
||
}
|
||
|
||
Console.WriteLine(
|
||
"纵向辨识测试完成。请同时保存对应的M层轮速诊断CSV。");
|
||
Hedingben.ToastText(
|
||
"纵向辨识完成，请停止并保存M层轮速诊断记录。",
|
||
"LongitudinalIdentification");
|
||
}
|
||
catch (OperationCanceledException)
|
||
{
|
||
Console.WriteLine("纵向辨识已由用户停止。");
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
Console.WriteLine(
|
||
"纵向辨识失败：" +
|
||
exception.Message);
|
||
Hedingben.ToastText(
|
||
"纵向辨识失败：" +
|
||
exception.Message,
|
||
"LongitudinalIdentification");
|
||
}
|
||
finally
|
||
{
|
||
_adapter?.StopImmediately();
|
||
_chassis?.PredefinedDriveStop();
|
||
_recorder?.UpdateBodyCommand(0f, 0f, 0f);
|
||
_recorder?.StopAndSave();
|
||
|
||
_preparationTask?.Stop();
|
||
_preparationTask = null;
|
||
_recorder = null;
|
||
_stateProvider = null;
|
||
_adapter = null;
|
||
_chassis = null;
|
||
Interlocked.Exchange(ref _testRunning, 0);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 请求停止当前辨识并立即清零底盘驱动速度。
|
||
/// </summary>
|
||
public override void TestStop()
|
||
{
|
||
Interlocked.Exchange(ref _stopRequested, 1);
|
||
_preparationTask?.Stop();
|
||
_adapter?.StopImmediately();
|
||
_chassis?.PredefinedDriveStop();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在固定周期内保持零命令并记录静止数据。
|
||
/// </summary>
|
||
private void RunStationaryPhase(
|
||
double durationSeconds,
|
||
string phaseName)
|
||
{
|
||
_recorder.UpdateBodyCommand(0f, 0f, 0f);
|
||
RunPeriodicPhase(
|
||
durationSeconds,
|
||
phaseName,
|
||
_ => UpdateWheelDiagnostics());
|
||
}
|
||
|
||
/// <summary>
|
||
/// 通过标准车体速度适配器持续下发直线目标速度。
|
||
/// </summary>
|
||
private void RunTargetSpeedPhase(
|
||
double targetSpeedMetersPerSecond)
|
||
{
|
||
_recorder.UpdateBodyCommand(
|
||
(float)targetSpeedMetersPerSecond,
|
||
0f,
|
||
0f);
|
||
|
||
RunPeriodicPhase(
|
||
CommandHoldSeconds,
|
||
"目标速度保持",
|
||
interval =>
|
||
{
|
||
var accepted = _adapter.SendBodyTwist(
|
||
new Twist2D(
|
||
targetSpeedMetersPerSecond,
|
||
0.0,
|
||
0.0),
|
||
interval);
|
||
if (!accepted)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"底盘拒绝纵向速度命令：" +
|
||
_adapter.LastFailureReason);
|
||
}
|
||
|
||
UpdateWheelDiagnostics();
|
||
});
|
||
}
|
||
|
||
/// <summary>
|
||
/// 重复发送零速SendMotion，使底盘内部DeAccPerSecond斜坡真实参与减速。
|
||
/// </summary>
|
||
private void RunConfiguredDecelerationPhase(
|
||
double targetSpeedMetersPerSecond)
|
||
{
|
||
_recorder.UpdateBodyCommand(0f, 0f, 0f);
|
||
|
||
var rampDurationSeconds =
|
||
Math.Abs(targetSpeedMetersPerSecond) /
|
||
_chassis.DeAccPerSecond;
|
||
var totalDurationSeconds =
|
||
rampDurationSeconds +
|
||
PostStopSeconds;
|
||
|
||
RunPeriodicPhase(
|
||
totalDurationSeconds,
|
||
"配置化正常减速",
|
||
interval =>
|
||
{
|
||
// SendBodyTwist(Zero)会立即停车；辨识DeAccPerSecond时必须
|
||
// 直接保持SendMotion零目标，让底盘内部发送速度逐周期下降。
|
||
var accepted = _chassis.SendMotion(
|
||
0f,
|
||
0f,
|
||
0f,
|
||
interval);
|
||
if (!accepted)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"底盘拒绝正常减速命令：" +
|
||
_chassis.LastMotionDecomposeFailureReason);
|
||
}
|
||
|
||
UpdateWheelDiagnostics();
|
||
});
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按实际循环间隔运行一个阶段，并响应测试界面的停止请求。
|
||
/// </summary>
|
||
private void RunPeriodicPhase(
|
||
double durationSeconds,
|
||
string phaseName,
|
||
Action<TimeSpan> cycleAction)
|
||
{
|
||
Console.WriteLine(
|
||
$"纵向辨识阶段：{phaseName}，预计{durationSeconds:F2}s。");
|
||
|
||
var periodSeconds =
|
||
ControlIntervalMilliseconds / 1000.0;
|
||
var phaseClock = Stopwatch.StartNew();
|
||
var previousCycleSeconds = -periodSeconds;
|
||
|
||
while (phaseClock.Elapsed.TotalSeconds <
|
||
durationSeconds)
|
||
{
|
||
ThrowIfStopRequested();
|
||
|
||
var cycleStartSeconds =
|
||
phaseClock.Elapsed.TotalSeconds;
|
||
var deltaTimeSeconds =
|
||
cycleStartSeconds -
|
||
previousCycleSeconds;
|
||
previousCycleSeconds = cycleStartSeconds;
|
||
|
||
cycleAction(
|
||
TimeSpan.FromSeconds(
|
||
deltaTimeSeconds));
|
||
|
||
var elapsedMilliseconds =
|
||
(phaseClock.Elapsed.TotalSeconds -
|
||
cycleStartSeconds) * 1000.0;
|
||
var remainingMilliseconds =
|
||
ControlIntervalMilliseconds -
|
||
elapsedMilliseconds;
|
||
if (remainingMilliseconds > 1.0)
|
||
{
|
||
Thread.Sleep(
|
||
(int)Math.Floor(
|
||
remainingMilliseconds));
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 刷新轮组原始与滤波速度，供后台CSV记录器读取最新诊断值。
|
||
/// </summary>
|
||
private void UpdateWheelDiagnostics()
|
||
{
|
||
_stateProvider.TryGetWheelTwist(
|
||
out _,
|
||
out _);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建复用现有字段格式的纵向辨识CSV记录器。
|
||
/// </summary>
|
||
private TrackingExperimentRecorder CreateRecorder(
|
||
double targetSpeedMetersPerSecond,
|
||
Pose2D initialPoseInWorld)
|
||
{
|
||
var expectedTravelMeters =
|
||
targetSpeedMetersPerSecond *
|
||
CommandHoldSeconds;
|
||
var referenceStart = new Vector2(
|
||
(float)(
|
||
initialPoseInWorld.XMeters *
|
||
MillimetersPerMeter),
|
||
(float)(
|
||
initialPoseInWorld.YMeters *
|
||
MillimetersPerMeter));
|
||
var referenceEnd = new Vector2(
|
||
referenceStart.X +
|
||
(float)(
|
||
Math.Cos(initialPoseInWorld.YawRadians) *
|
||
expectedTravelMeters *
|
||
MillimetersPerMeter),
|
||
referenceStart.Y +
|
||
(float)(
|
||
Math.Sin(initialPoseInWorld.YawRadians) *
|
||
expectedTravelMeters *
|
||
MillimetersPerMeter));
|
||
var speedMillimetersPerSecond =
|
||
targetSpeedMetersPerSecond *
|
||
MillimetersPerMeter;
|
||
var trajectoryName =
|
||
"LongitudinalStep_" +
|
||
speedMillimetersPerSecond.ToString(
|
||
"+0;-0;0",
|
||
CultureInfo.InvariantCulture) +
|
||
"mmps_" +
|
||
StopModeName;
|
||
|
||
return new TrackingExperimentRecorder(
|
||
controllerName:
|
||
"OpenLoopLongitudinalIdentification",
|
||
trajectoryName: trajectoryName,
|
||
trialNumber: TrialNumber,
|
||
referenceStart: referenceStart,
|
||
referenceEnd: referenceEnd,
|
||
referenceSpeed:
|
||
(float)targetSpeedMetersPerSecond,
|
||
sampleIntervalMs:
|
||
ControlIntervalMilliseconds,
|
||
referenceMotionFrameYawDegrees: 0f,
|
||
referenceAccelerationMetersPerSecondSquared:
|
||
_chassis.AccPerSecond,
|
||
referenceDecelerationMetersPerSecondSquared:
|
||
_chassis.DeAccPerSecond,
|
||
diagnosticChassis: _chassis,
|
||
diagnosticStateProvider: _stateProvider);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从测试界面读取带方向的纵向目标速度，正值前进、负值倒车。
|
||
/// </summary>
|
||
private static bool TryReadTargetSpeed(
|
||
out double targetSpeedMetersPerSecond)
|
||
{
|
||
targetSpeedMetersPerSecond = 0.0;
|
||
var input = UI.GetInput(
|
||
"输入纵向目标速度（m/s，正数前进、负数倒车，" +
|
||
$"范围-{MaximumTargetSpeedMetersPerSecond:F1}～" +
|
||
$"{MaximumTargetSpeedMetersPerSecond:F1}且不能为0）：");
|
||
var parsed = double.TryParse(
|
||
input,
|
||
NumberStyles.Float,
|
||
CultureInfo.CurrentCulture,
|
||
out targetSpeedMetersPerSecond) ||
|
||
double.TryParse(
|
||
input,
|
||
NumberStyles.Float,
|
||
CultureInfo.InvariantCulture,
|
||
out targetSpeedMetersPerSecond);
|
||
|
||
if (!parsed ||
|
||
double.IsNaN(targetSpeedMetersPerSecond) ||
|
||
double.IsInfinity(targetSpeedMetersPerSecond) ||
|
||
Math.Abs(targetSpeedMetersPerSecond) <
|
||
MinimumTargetSpeedMetersPerSecond ||
|
||
Math.Abs(targetSpeedMetersPerSecond) >
|
||
MaximumTargetSpeedMetersPerSecond)
|
||
{
|
||
Console.WriteLine(
|
||
"目标速度必须是绝对值位于" +
|
||
$"{MinimumTargetSpeedMetersPerSecond:F2}～" +
|
||
$"{MaximumTargetSpeedMetersPerSecond:F2}m/s之间的有限数值。");
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证实验时间、循环周期和编号配置。
|
||
/// </summary>
|
||
private void ValidateSettings()
|
||
{
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
BaselineSeconds,
|
||
nameof(BaselineSeconds));
|
||
NumericGuard.EnsureFinitePositive(
|
||
CommandHoldSeconds,
|
||
nameof(CommandHoldSeconds));
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
PostStopSeconds,
|
||
nameof(PostStopSeconds));
|
||
|
||
if (CommandHoldSeconds > 30.0 ||
|
||
BaselineSeconds > 10.0 ||
|
||
PostStopSeconds > 10.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(CommandHoldSeconds),
|
||
"辨识阶段时间超出测试允许范围。");
|
||
}
|
||
|
||
if (ControlIntervalMilliseconds < 20 ||
|
||
ControlIntervalMilliseconds > 200)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(ControlIntervalMilliseconds),
|
||
"纵向辨识命令周期必须在20～200ms之间。");
|
||
}
|
||
|
||
if (TrialNumber <= 0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(TrialNumber),
|
||
"测试编号必须大于零。");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证底盘运行时加载的速度、加速度和减速度配置。
|
||
/// </summary>
|
||
private void ValidateChassisAndTarget(
|
||
double targetSpeedMetersPerSecond)
|
||
{
|
||
NumericGuard.EnsureFinitePositive(
|
||
_chassis.MaxSpeed,
|
||
nameof(_chassis.MaxSpeed));
|
||
NumericGuard.EnsureFinitePositive(
|
||
_chassis.AccPerSecond,
|
||
nameof(_chassis.AccPerSecond));
|
||
NumericGuard.EnsureFinitePositive(
|
||
_chassis.DeAccPerSecond,
|
||
nameof(_chassis.DeAccPerSecond));
|
||
|
||
if (Math.Abs(targetSpeedMetersPerSecond) >
|
||
_chassis.MaxSpeed)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(targetSpeedMetersPerSecond),
|
||
"目标速度超过底盘当前MaxSpeed配置。");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在收到停止请求时中断当前阶段并转入finally安全停车。
|
||
/// </summary>
|
||
private void ThrowIfStopRequested()
|
||
{
|
||
if (Volatile.Read(ref _stopRequested) != 0)
|
||
{
|
||
throw new OperationCanceledException();
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 测量速度阶跃、加速与稳态，并在保持结束后立即清零驱动速度。
|
||
/// </summary>
|
||
[MovementTest(name = "纵向辨识：速度阶跃与立即停车")]
|
||
public sealed class LongitudinalStepImmediateStopTest
|
||
: LongitudinalIdentificationTestBase
|
||
{
|
||
protected override bool UseConfiguredDeceleration =>
|
||
false;
|
||
|
||
protected override string StopModeName =>
|
||
"ImmediateStop";
|
||
}
|
||
|
||
/// <summary>
|
||
/// 测量速度阶跃、稳态以及由底盘DeAccPerSecond形成的正常减速过程。
|
||
/// </summary>
|
||
[MovementTest(name = "纵向辨识：速度阶跃与正常减速")]
|
||
public sealed class LongitudinalStepConfiguredDecelerationTest
|
||
: LongitudinalIdentificationTestBase
|
||
{
|
||
protected override bool UseConfiguredDeceleration =>
|
||
true;
|
||
|
||
protected override string StopModeName =>
|
||
"ConfiguredDeceleration";
|
||
}
|
||
}
|