First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

控制器测试横向跟踪，增加记录

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-10 10:10:21 +08:00
parent 88f651e0c2
commit 2f9e4285d3
22 changed files with 769 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+2 -2
View File
@@ -47,11 +47,11 @@ namespace MultiWheelC.Control.Execution
ILongitudinalController longitudinalController,
GcpCommandAllocator gcpAllocator,
GcpCommandExecutor commandExecutor,
double finishDistanceMeters = 0.03,
double finishDistanceMeters = 0.04,
double finishSpeedMetersPerSecond = 0.02,
double finishHeadingToleranceRadians =
3.0 * Math.PI / 180.0,
double maximumDistanceToTrajectoryMeters = 0.50)
double maximumDistanceToTrajectoryMeters = 0.30)
{
_stateProvider = stateProvider ??
throw new ArgumentNullException(
MultiWheelC/Control/Lateral/StanleyLateralController.cs
+1 -1
View File
@@ -73,7 +73,7 @@ namespace MultiWheelC.Control.Lateral
public double MinimumSpeedMetersPerSecond { get; }
/// <summary>
/// 获取是否优先使用Detour估算的实际纵向速度计算横向修正。
/// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。
/// </summary>
public bool UseActualSpeedForGain { get; }
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+52 -9
View File
@@ -16,7 +16,7 @@ namespace MultiWheelC
/// <summary>
/// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
/// </summary>
[MovementTest(name = "新版控制器：曲线-停车自转-直线组合测试")]
[MovementTest(name = "新版控制器：直线-圆弧-折线组合测试")]
public sealed class CompositeStopTurnGoTest : MovementTest
{
private const float MillimetersPerMeter = 1000f;
@@ -52,6 +52,13 @@ namespace MultiWheelC
return;
}
if (!TrajectoryExperimentInput
.TryReadLateralOffsetMeters(
out var lateralOffsetMeters))
{
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
@@ -65,19 +72,30 @@ namespace MultiWheelC
return;
}
var stateProvider = new DetourVehicleStateProvider();
if (!stateProvider.TryGetState(out var initialState))
var detourStateProvider =
new DetourVehicleStateProvider();
if (!detourStateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取组合运动起点位姿：" +
stateProvider.LastFailureReason);
detourStateProvider.LastFailureReason);
return;
}
var stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
var planStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
initialState.PoseInWorld,
lateralOffsetMeters);
var firstTrajectory =
TestTrajectoryFactory
.CreateStraightSmoothLeftTurnStraight(
initialState.PoseInWorld,
planStartPose,
StraightLengthMeters,
TurnRadiusMeters,
AngleMath.DegreesToRadians(
@@ -133,7 +151,9 @@ namespace MultiWheelC
_recorder = new TrackingExperimentRecorder(
controllerName: "NewStanleyPidComposite",
trajectoryName:
"SmoothTurnStopRotateStraight",
TrajectoryExperimentInput.BuildTrajectoryName(
"SmoothTurnStopRotateStraight",
lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: ToMillimeterVector(
firstTrajectory.StartPoint.PoseInWorld),
@@ -145,7 +165,8 @@ namespace MultiWheelC
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared);
(float)DecelerationMetersPerSecondSquared,
diagnosticChassis: chassis);
_recorder.Start();
var controlPointRadiusMeters =
@@ -157,6 +178,7 @@ namespace MultiWheelC
StateProvider = stateProvider,
ConfigureTrackingMovement = tracking =>
{
tracking.StanleyUsesActualSpeed = true;
tracking.MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond;
tracking
@@ -166,6 +188,7 @@ namespace MultiWheelC
SegmentStarted = (index, segment) =>
{
_recorder?.ClearControlReference();
_recorder?.ClearGcpCommand();
_recorder?.UpdateCommand(0f, 0f);
Console.WriteLine(
$"组合运动开始第{index + 1}段：" +
@@ -174,7 +197,8 @@ namespace MultiWheelC
TrackingCycleObserver = (index, controller) =>
RecordTrackingCycle(
controller,
controlPointRadiusMeters),
controlPointRadiusMeters,
stateProvider),
RotationCommandObserver = (index, omega) =>
_recorder?.UpdateCommand(
0f,
@@ -276,7 +300,8 @@ namespace MultiWheelC
/// </summary>
private void RecordTrackingCycle(
ParkingGeometricController controller,
double controlPointRadiusMeters)
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastVehicleState.HasValue)
{
@@ -284,6 +309,21 @@ namespace MultiWheelC
controller.LastVehicleState.Value);
}
if (stateProvider.TryGetLatestVelocityDiagnostics(
out var detourBodyVx,
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
out var wheelVelocityValid))
{
_recorder?.UpdateVelocityDiagnostics(
detourBodyVx,
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
wheelVelocityValid);
}
if (!controller.LastCommand.HasValue)
{
return;
@@ -305,6 +345,9 @@ namespace MultiWheelC
}
var command = controller.LastCommand.Value;
_recorder?.UpdateGcpCommand(
command.FrontAngleRadians,
command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+210 -19
View File
@@ -1,5 +1,6 @@
using System;
using System.Drawing;
using System.Globalization;
using System.Numerics;
using System.Threading;
using ClumsyCore;
@@ -13,6 +14,82 @@ using MyParking.Shared;
namespace MultiWheelC
{
/// <summary>
/// 统一读取轨迹实验的有符号横向偏移，并将车体局部偏移转换到世界坐标系。
/// </summary>
internal static class TrajectoryExperimentInput
{
private const double MaximumOffsetCentimeters = 30.0;
/// <summary>
/// 从Clumsy输入框读取车体左正右负的横向偏移，单位转换为m。
/// </summary>
public static bool TryReadLateralOffsetMeters(
out double lateralOffsetMeters)
{
lateralOffsetMeters = 0.0;
var input = UI.GetInput(
"输入轨迹横向偏移（cm，左正右负，范围-30～30）：");
var parsed = double.TryParse(
input,
NumberStyles.Float,
CultureInfo.CurrentCulture,
out var offsetCentimeters) ||
double.TryParse(
input,
NumberStyles.Float,
CultureInfo.InvariantCulture,
out offsetCentimeters);
if (!parsed ||
double.IsNaN(offsetCentimeters) ||
double.IsInfinity(offsetCentimeters) ||
Math.Abs(offsetCentimeters) >
MaximumOffsetCentimeters)
{
Console.WriteLine(
"轨迹横向偏移必须是-30～30cm之间的有限数值，测试未启动。");
return false;
}
lateralOffsetMeters =
offsetCentimeters / 100.0;
return true;
}
/// <summary>
/// 沿初始车体左方向平移参考轨迹起点，同时保持世界坐标航向不变。
/// </summary>
public static Pose2D OffsetPoseLaterally(
Pose2D poseInWorld,
double lateralOffsetMeters)
{
var yawRadians = poseInWorld.YawRadians;
return new Pose2D(
poseInWorld.XMeters -
Math.Sin(yawRadians) *
lateralOffsetMeters,
poseInWorld.YMeters +
Math.Cos(yawRadians) *
lateralOffsetMeters,
yawRadians);
}
/// <summary>
/// 生成带毫米偏移标识的实验轨迹名称。
/// </summary>
public static string BuildTrajectoryName(
string baseName,
double lateralOffsetMeters)
{
return baseName +
"_Offset" +
(lateralOffsetMeters * 1000.0)
.ToString("+0;-0;0", CultureInfo.InvariantCulture) +
"mm";
}
}
/// <summary>
/// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
/// </summary>
@@ -27,7 +104,7 @@ namespace MultiWheelC
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
private DetourVehicleStateProvider _stateProvider;
private IVehicleStateProvider _stateProvider;
/// <summary>
/// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -37,7 +114,7 @@ namespace MultiWheelC
/// <summary>
/// 获取或设置4m直线的巡航参考速度，单位为m/s。
/// </summary>
public double CruiseSpeedMetersPerSecond = 0.30;
public double CruiseSpeedMetersPerSecond = 0.40;
/// <summary>
/// 获取或设置参考速度加速度，单位为m/s²。
@@ -66,6 +143,13 @@ namespace MultiWheelC
return;
}
if (!TrajectoryExperimentInput
.TryReadLateralOffsetMeters(
out var lateralOffsetMeters))
{
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
@@ -80,21 +164,30 @@ namespace MultiWheelC
return;
}
_stateProvider =
var detourStateProvider =
new DetourVehicleStateProvider();
if (!_stateProvider.TryGetState(
if (!detourStateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
_stateProvider.LastFailureReason);
detourStateProvider.LastFailureReason);
_stateProvider = null;
return;
}
_stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
initialState.PoseInWorld,
lateralOffsetMeters);
var trajectory =
TestTrajectoryFactory.CreateStraight4Meters(
initialState.PoseInWorld,
trajectoryStartPose,
CruiseSpeedMetersPerSecond,
AccelerationMetersPerSecondSquared,
DecelerationMetersPerSecondSquared,
@@ -109,7 +202,10 @@ namespace MultiWheelC
var recorder =
new TrackingExperimentRecorder(
controllerName: "NewStanleyPid",
trajectoryName: "ProfiledStraight4m",
trajectoryName:
TrajectoryExperimentInput.BuildTrajectoryName(
"ProfiledStraight4m",
lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
referenceEnd: referenceEnd,
@@ -119,7 +215,8 @@ namespace MultiWheelC
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared);
(float)DecelerationMetersPerSecondSquared,
diagnosticChassis: chassis);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -130,12 +227,15 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond = 0.50,
CycleObserver = controller =>
RecordControlCycle(
recorder,
controller,
controlPointRadiusMeters)
controlPointRadiusMeters,
_stateProvider as
WheelFeedbackVehicleStateProvider)
};
recorder.Start();
@@ -239,7 +339,8 @@ namespace MultiWheelC
private static void RecordControlCycle(
TrackingExperimentRecorder recorder,
ParkingGeometricController controller,
double controlPointRadiusMeters)
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastVehicleState.HasValue)
{
@@ -247,6 +348,10 @@ namespace MultiWheelC
controller.LastVehicleState.Value);
}
UpdateVelocityDiagnostics(
recorder,
stateProvider);
if (!controller.LastCommand.HasValue)
{
return;
@@ -267,6 +372,9 @@ namespace MultiWheelC
}
var command = controller.LastCommand.Value;
recorder.UpdateGcpCommand(
command.FrontAngleRadians,
command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
@@ -279,6 +387,32 @@ namespace MultiWheelC
(float)angularSpeedRadiansPerSecond);
}
/// <summary>
/// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
/// </summary>
private static void UpdateVelocityDiagnostics(
TrackingExperimentRecorder recorder,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (stateProvider == null ||
!stateProvider.TryGetLatestVelocityDiagnostics(
out var detourBodyVx,
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
out var wheelVelocityValid))
{
return;
}
recorder.UpdateVelocityDiagnostics(
detourBodyVx,
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
wheelVelocityValid);
}
/// <summary>
/// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
/// </summary>
@@ -293,6 +427,7 @@ namespace MultiWheelC
poseInWorld.YMeters *
MillimetersPerMeter));
}
}
/// <summary>
@@ -310,7 +445,7 @@ namespace MultiWheelC
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
private DetourVehicleStateProvider _stateProvider;
private IVehicleStateProvider _stateProvider;
/// <summary>
/// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -369,6 +504,13 @@ namespace MultiWheelC
return;
}
if (!TrajectoryExperimentInput
.TryReadLateralOffsetMeters(
out var lateralOffsetMeters))
{
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
@@ -383,22 +525,31 @@ namespace MultiWheelC
return;
}
_stateProvider =
var detourStateProvider =
new DetourVehicleStateProvider();
if (!_stateProvider.TryGetState(
if (!detourStateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
_stateProvider.LastFailureReason);
detourStateProvider.LastFailureReason);
_stateProvider = null;
return;
}
_stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
initialState.PoseInWorld,
lateralOffsetMeters);
var trajectory =
TestTrajectoryFactory
.CreateStraightLeftSemicircleStraight(
initialState.PoseInWorld,
trajectoryStartPose,
StraightLengthMeters,
TurnRadiusMeters,
CurvatureTransitionLengthMeters,
@@ -418,7 +569,9 @@ namespace MultiWheelC
new TrackingExperimentRecorder(
controllerName: "NewStanleyPid",
trajectoryName:
"ProfiledStraightSmoothLeftTurnStraight",
TrajectoryExperimentInput.BuildTrajectoryName(
"ProfiledStraightSmoothLeftTurnStraight",
lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
referenceEnd: referenceEnd,
@@ -428,7 +581,8 @@ namespace MultiWheelC
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared);
(float)DecelerationMetersPerSecondSquared,
diagnosticChassis: chassis);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -439,13 +593,16 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond,
CycleObserver = controller =>
RecordControlCycle(
recorder,
controller,
controlPointRadiusMeters)
controlPointRadiusMeters,
_stateProvider as
WheelFeedbackVehicleStateProvider)
};
recorder.Start();
@@ -549,7 +706,8 @@ namespace MultiWheelC
private static void RecordControlCycle(
TrackingExperimentRecorder recorder,
ParkingGeometricController controller,
double controlPointRadiusMeters)
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastVehicleState.HasValue)
{
@@ -557,6 +715,10 @@ namespace MultiWheelC
controller.LastVehicleState.Value);
}
UpdateVelocityDiagnostics(
recorder,
stateProvider);
if (!controller.LastCommand.HasValue)
{
return;
@@ -577,6 +739,9 @@ namespace MultiWheelC
}
var command = controller.LastCommand.Value;
recorder.UpdateGcpCommand(
command.FrontAngleRadians,
command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
@@ -589,6 +754,32 @@ namespace MultiWheelC
(float)angularSpeedRadiansPerSecond);
}
/// <summary>
/// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
/// </summary>
private static void UpdateVelocityDiagnostics(
TrackingExperimentRecorder recorder,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (stateProvider == null ||
!stateProvider.TryGetLatestVelocityDiagnostics(
out var detourBodyVx,
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
out var wheelVelocityValid))
{
return;
}
recorder.UpdateVelocityDiagnostics(
detourBodyVx,
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
wheelVelocityValid);
}
/// <summary>
/// 将Shared世界坐标系米制位姿转换为Clumsy绘图和记录器使用的毫米坐标。
/// </summary>
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+298 -4
View File
@@ -7,6 +7,7 @@ using System.IO;
using System.Numerics;
using System.Text;
using System.Threading;
using CommonUsage.Chassis;
using MyParking.Shared;
using MultiWheelC.StateEstimation;
@@ -47,6 +48,28 @@ namespace MultiWheelC
public double ControlHeadingErrorRadians;
public double ControlDistanceToTrajectoryMeters;
public double ControlRemainingDistanceMeters;
// 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
public bool HasVelocityDiagnostics;
public double DetourEstimatedBodyVxMetersPerSecond;
public bool DetourVelocityEstimateValid;
public double WheelFeedbackRawBodyVxMetersPerSecond;
public double WheelFeedbackFilteredBodyVxMetersPerSecond;
public bool WheelFeedbackVelocityEstimateValid;
// 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
public bool HasSteeringDiagnostics;
public double TargetSteerLeftFrontDegrees;
public double TargetSteerLeftRearDegrees;
public double TargetSteerRightFrontDegrees;
public double TargetSteerRightRearDegrees;
public double ActualSteerLeftFrontDegrees;
public double ActualSteerLeftRearDegrees;
public double ActualSteerRightFrontDegrees;
public double ActualSteerRightRearDegrees;
public bool HasGcpCommand;
public double CommandFrontGcpAngleRadians;
public double CommandRearGcpAngleRadians;
}
// C层实验工具：统一采集并保存轨迹跟踪实验数据。
@@ -63,6 +86,7 @@ namespace MultiWheelC
private readonly float _referenceAccelerationMetersPerSecondSquared;
private readonly float _referenceDecelerationMetersPerSecondSquared;
private readonly int _sampleIntervalMs;
private readonly MultiWheelChassis _diagnosticChassis;
private readonly List<TrackingSample> _samples =
new List<TrackingSample>();
@@ -97,6 +121,15 @@ namespace MultiWheelC
private double _controlHeadingErrorRadians;
private double _controlDistanceToTrajectoryMeters;
private double _controlRemainingDistanceMeters;
private bool _hasVelocityDiagnostics;
private double _detourEstimatedBodyVxMetersPerSecond;
private bool _detourVelocityEstimateValid;
private double _wheelFeedbackRawBodyVxMetersPerSecond;
private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
private bool _wheelFeedbackVelocityEstimateValid;
private bool _hasGcpCommand;
private double _commandFrontGcpAngleRadians;
private double _commandRearGcpAngleRadians;
public TrackingExperimentRecorder(
string controllerName,
@@ -109,7 +142,8 @@ namespace MultiWheelC
int sampleIntervalMs = 50,
float referenceMotionFrameYawDegrees = 0f,
float referenceAccelerationMetersPerSecondSquared = 0f,
float referenceDecelerationMetersPerSecondSquared = 0f)
float referenceDecelerationMetersPerSecondSquared = 0f,
MultiWheelChassis diagnosticChassis = null)
{
if (string.IsNullOrWhiteSpace(controllerName))
throw new ArgumentException(
@@ -140,6 +174,7 @@ namespace MultiWheelC
_referenceDecelerationMetersPerSecondSquared =
referenceDecelerationMetersPerSecondSquared;
_sampleIntervalMs = sampleIntervalMs;
_diagnosticChassis = diagnosticChassis;
}
// 保存成功后的CSV绝对路径；尚未保存时为空。
@@ -221,6 +256,60 @@ namespace MultiWheelC
}
}
/// <summary>
/// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
/// </summary>
public void UpdateVelocityDiagnostics(
double detourEstimatedBodyVxMetersPerSecond,
bool detourVelocityEstimateValid,
double wheelFeedbackRawBodyVxMetersPerSecond,
double wheelFeedbackFilteredBodyVxMetersPerSecond,
bool wheelFeedbackVelocityEstimateValid)
{
lock (_stateSyncRoot)
{
_detourEstimatedBodyVxMetersPerSecond =
detourEstimatedBodyVxMetersPerSecond;
_detourVelocityEstimateValid =
detourVelocityEstimateValid;
_wheelFeedbackRawBodyVxMetersPerSecond =
wheelFeedbackRawBodyVxMetersPerSecond;
_wheelFeedbackFilteredBodyVxMetersPerSecond =
wheelFeedbackFilteredBodyVxMetersPerSecond;
_wheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid;
_hasVelocityDiagnostics = true;
}
}
/// <summary>
/// 保存经过GCP角速度限制后实际交给底盘的前后虚拟控制点转角。
/// </summary>
public void UpdateGcpCommand(
double frontGcpAngleRadians,
double rearGcpAngleRadians)
{
lock (_stateSyncRoot)
{
_commandFrontGcpAngleRadians =
frontGcpAngleRadians;
_commandRearGcpAngleRadians =
rearGcpAngleRadians;
_hasGcpCommand = true;
}
}
/// <summary>
/// 清除上一轨迹段的GCP命令，避免停车或原地自转阶段沿用旧角度。
/// </summary>
public void ClearGcpCommand()
{
lock (_stateSyncRoot)
{
_hasGcpCommand = false;
}
}
/// <summary>
/// 保存新版控制器本周期实际使用的轨迹投影、误差和参考速度。
/// </summary>
@@ -331,6 +420,15 @@ namespace MultiWheelC
double controlHeadingErrorRadians;
double controlDistanceToTrajectoryMeters;
double controlRemainingDistanceMeters;
bool hasVelocityDiagnostics;
double detourEstimatedBodyVxMetersPerSecond;
bool detourVelocityEstimateValid;
double wheelFeedbackRawBodyVxMetersPerSecond;
double wheelFeedbackFilteredBodyVxMetersPerSecond;
bool wheelFeedbackVelocityEstimateValid;
bool hasGcpCommand;
double commandFrontGcpAngleRadians;
double commandRearGcpAngleRadians;
lock (_commandSyncRoot)
{
@@ -382,6 +480,23 @@ namespace MultiWheelC
_controlDistanceToTrajectoryMeters;
controlRemainingDistanceMeters =
_controlRemainingDistanceMeters;
hasVelocityDiagnostics =
_hasVelocityDiagnostics;
detourEstimatedBodyVxMetersPerSecond =
_detourEstimatedBodyVxMetersPerSecond;
detourVelocityEstimateValid =
_detourVelocityEstimateValid;
wheelFeedbackRawBodyVxMetersPerSecond =
_wheelFeedbackRawBodyVxMetersPerSecond;
wheelFeedbackFilteredBodyVxMetersPerSecond =
_wheelFeedbackFilteredBodyVxMetersPerSecond;
wheelFeedbackVelocityEstimateValid =
_wheelFeedbackVelocityEstimateValid;
hasGcpCommand = _hasGcpCommand;
commandFrontGcpAngleRadians =
_commandFrontGcpAngleRadians;
commandRearGcpAngleRadians =
_commandRearGcpAngleRadians;
}
var sample = new TrackingSample
@@ -411,9 +526,28 @@ namespace MultiWheelC
ControlDistanceToTrajectoryMeters =
controlDistanceToTrajectoryMeters,
ControlRemainingDistanceMeters =
controlRemainingDistanceMeters
controlRemainingDistanceMeters,
HasVelocityDiagnostics =
hasVelocityDiagnostics,
DetourEstimatedBodyVxMetersPerSecond =
detourEstimatedBodyVxMetersPerSecond,
DetourVelocityEstimateValid =
detourVelocityEstimateValid,
WheelFeedbackRawBodyVxMetersPerSecond =
wheelFeedbackRawBodyVxMetersPerSecond,
WheelFeedbackFilteredBodyVxMetersPerSecond =
wheelFeedbackFilteredBodyVxMetersPerSecond,
WheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid,
HasGcpCommand = hasGcpCommand,
CommandFrontGcpAngleRadians =
commandFrontGcpAngleRadians,
CommandRearGcpAngleRadians =
commandRearGcpAngleRadians
};
CaptureSteeringDiagnostics(sample);
if (processedState.HasValue)
{
var state = processedState.Value;
@@ -452,6 +586,90 @@ namespace MultiWheelC
}
}
/// <summary>
/// 按舵轮物理安装位置记录四轮目标角和实际反馈角。
/// </summary>
private void CaptureSteeringDiagnostics(
TrackingSample sample)
{
if (_diagnosticChassis == null)
{
return;
}
#pragma warning disable CS0612, CS0618
var wheels = _diagnosticChassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618
var leftFront = FindWheel(
wheels,
requireFront: true,
requireLeft: true);
var leftRear = FindWheel(
wheels,
requireFront: false,
requireLeft: true);
var rightFront = FindWheel(
wheels,
requireFront: true,
requireLeft: false);
var rightRear = FindWheel(
wheels,
requireFront: false,
requireLeft: false);
if (leftFront == null ||
leftRear == null ||
rightFront == null ||
rightRear == null)
{
return;
}
sample.HasSteeringDiagnostics = true;
sample.TargetSteerLeftFrontDegrees =
leftFront.GetSendAngle();
sample.TargetSteerLeftRearDegrees =
leftRear.GetSendAngle();
sample.TargetSteerRightFrontDegrees =
rightFront.GetSendAngle();
sample.TargetSteerRightRearDegrees =
rightRear.GetSendAngle();
sample.ActualSteerLeftFrontDegrees =
leftFront.ReadAngle();
sample.ActualSteerLeftRearDegrees =
leftRear.ReadAngle();
sample.ActualSteerRightFrontDegrees =
rightFront.ReadAngle();
sample.ActualSteerRightRearDegrees =
rightRear.ReadAngle();
}
/// <summary>
/// 根据车体X向前、Y向左的物理坐标查找指定象限中的舵轮。
/// </summary>
private static SteerWheel FindWheel(
IReadOnlyList<SteerWheel> wheels,
bool requireFront,
bool requireLeft)
{
foreach (var wheel in wheels)
{
var isFront =
wheel.PhysicalPosition.X >= 0f;
var isLeft =
wheel.PhysicalPosition.Y >= 0f;
if (isFront == requireFront &&
isLeft == requireLeft)
{
return wheel;
}
}
return null;
}
// 将内存中的采样数据写入CSV。
private void SaveCsv()
{
@@ -523,7 +741,25 @@ namespace MultiWheelC
"ControlLateralErrorMeters," +
"ControlHeadingErrorRadians," +
"ControlDistanceToTrajectoryMeters," +
"ControlRemainingDistanceMeters");
"ControlRemainingDistanceMeters," +
"HasVelocityDiagnostics," +
"DetourEstimatedBodyVxMetersPerSecond," +
"DetourVelocityEstimateValid," +
"WheelFeedbackRawBodyVxMetersPerSecond," +
"WheelFeedbackFilteredBodyVxMetersPerSecond," +
"WheelFeedbackVelocityEstimateValid," +
"HasSteeringDiagnostics," +
"TargetSteerLeftFrontDegrees," +
"TargetSteerLeftRearDegrees," +
"TargetSteerRightFrontDegrees," +
"TargetSteerRightRearDegrees," +
"ActualSteerLeftFrontDegrees," +
"ActualSteerLeftRearDegrees," +
"ActualSteerRightFrontDegrees," +
"ActualSteerRightRearDegrees," +
"HasGcpCommand," +
"CommandFrontGcpAngleRadians," +
"CommandRearGcpAngleRadians");
foreach (var sample in snapshot)
{
@@ -610,7 +846,65 @@ namespace MultiWheelC
sample.ControlDistanceToTrajectoryMeters),
FormatOptional(
sample.HasControlReference,
sample.ControlRemainingDistanceMeters)));
sample.ControlRemainingDistanceMeters),
sample.HasVelocityDiagnostics
? "1"
: "0",
FormatOptional(
sample.HasVelocityDiagnostics,
sample.DetourEstimatedBodyVxMetersPerSecond),
sample.HasVelocityDiagnostics
? sample.DetourVelocityEstimateValid
? "1"
: "0"
: string.Empty,
FormatOptional(
sample.HasVelocityDiagnostics,
sample.WheelFeedbackRawBodyVxMetersPerSecond),
FormatOptional(
sample.HasVelocityDiagnostics,
sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
sample.HasVelocityDiagnostics
? sample.WheelFeedbackVelocityEstimateValid
? "1"
: "0"
: string.Empty,
sample.HasSteeringDiagnostics
? "1"
: "0",
FormatOptional(
sample.HasSteeringDiagnostics,
sample.TargetSteerLeftFrontDegrees),
FormatOptional(
sample.HasSteeringDiagnostics,
sample.TargetSteerLeftRearDegrees),
FormatOptional(
sample.HasSteeringDiagnostics,
sample.TargetSteerRightFrontDegrees),
FormatOptional(
sample.HasSteeringDiagnostics,
sample.TargetSteerRightRearDegrees),
FormatOptional(
sample.HasSteeringDiagnostics,
sample.ActualSteerLeftFrontDegrees),
FormatOptional(
sample.HasSteeringDiagnostics,
sample.ActualSteerLeftRearDegrees),
FormatOptional(
sample.HasSteeringDiagnostics,
sample.ActualSteerRightFrontDegrees),
FormatOptional(
sample.HasSteeringDiagnostics,
sample.ActualSteerRightRearDegrees),
sample.HasGcpCommand
? "1"
: "0",
FormatOptional(
sample.HasGcpCommand,
sample.CommandFrontGcpAngleRadians),
FormatOptional(
sample.HasGcpCommand,
sample.CommandRearGcpAngleRadians)));
}
}
}
MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+2 -2
View File
@@ -51,7 +51,7 @@ namespace MultiWheelC
public double StanleyMinimumSpeedMetersPerSecond = 0.15;
/// <summary>
/// 获取或设置Stanley是否优先使用Detour估算的实际速度。
/// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
/// </summary>
public bool StanleyUsesActualSpeed = true;
@@ -129,7 +129,7 @@ namespace MultiWheelC
/// <summary>
/// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
/// </summary>
public double MaximumDistanceToTrajectoryMeters = 0.50;
public double MaximumDistanceToTrajectoryMeters = 0.30;
/// <summary>
/// 单次轨迹动作允许的最长执行时间，单位为s。
MultiWheelC/PilotConfig.cs
+1 -1
View File
@@ -38,7 +38,7 @@ public class PilotConfig : MultiWheelPilotConfig
#region 单车-临时
[FieldMember(desc = "原地旋转Kp")]
public float InPlaceRotateKp = 1.1f;
public float InPlaceRotateKp = 1.05f;
[FieldMember(desc = "原地旋转Ki")]
public float InPlaceRotateKi = 0f;
MultiWheelC/build/Clumsy/CommonUsage.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.pdb
BIN
View File
Binary file not shown.