优化差速转舵并增加纵向辨识测试
This commit is contained in:
@@ -0,0 +1,562 @@
|
||||
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";
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user