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

完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-08-07 13:00:56 +08:00
co-authored by Cursor
parent f8881bc243
commit 14ca1150e4
12 changed files with 971 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+1 -1
View File
@@ -28,7 +28,7 @@ namespace MultiWheelC.Control.Execution
1e-6;
private const double StartupRegionMeters = 0.02;
private const double StartupPreviewDistanceMeters = 0.05;
private const double MaximumStartupSpeedMetersPerSecond = 0.05;
private const double MaximumStartupSpeedMetersPerSecond = 0.08;
private readonly IVehicleStateProvider _stateProvider;
private readonly ILateralController _lateralController;
MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs
+43 -1
View File
@@ -23,11 +23,15 @@ namespace MultiWheelC.Control.Longitudinal
double integralGainPerSecond,
double derivativeGainSeconds,
double maximumIntegralCorrectionMetersPerSecond,
double maximumCommandSpeedMetersPerSecond)
double maximumCommandSpeedMetersPerSecond,
double speedErrorDeadbandMetersPerSecond = 0.025)
{
EnsureFinitePositive(
maximumCommandSpeedMetersPerSecond,
nameof(maximumCommandSpeedMetersPerSecond));
EnsureFiniteNonNegative(
speedErrorDeadbandMetersPerSecond,
nameof(speedErrorDeadbandMetersPerSecond));
_feedbackPid = new PidController(
proportionalGain,
@@ -37,6 +41,8 @@ namespace MultiWheelC.Control.Longitudinal
derivativeOnMeasurement: true);
MaximumCommandSpeedMetersPerSecond =
maximumCommandSpeedMetersPerSecond;
SpeedErrorDeadbandMetersPerSecond =
speedErrorDeadbandMetersPerSecond;
}
/// <summary>
@@ -49,6 +55,11 @@ namespace MultiWheelC.Control.Longitudinal
/// </summary>
public double MaximumCommandSpeedMetersPerSecond { get; }
/// <summary>
/// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
/// </summary>
public double SpeedErrorDeadbandMetersPerSecond { get; }
/// <summary>
/// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
/// </summary>
@@ -98,6 +109,20 @@ namespace MultiWheelC.Control.Longitudinal
referenceSpeedMetersPerSecond);
}
var speedErrorMetersPerSecond =
referenceSpeedMetersPerSecond -
context.ActualLongitudinalSpeedMetersPerSecond;
// Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
// 同时清除PID历史，避免噪声持续积累后产生突发修正。
if (Math.Abs(speedErrorMetersPerSecond) <=
SpeedErrorDeadbandMetersPerSecond)
{
Reset();
return LimitReferenceSpeed(
referenceSpeedMetersPerSecond);
}
GetCorrectionOutputRange(
referenceSpeedMetersPerSecond,
out var minimumCorrectionMetersPerSecond,
@@ -178,5 +203,22 @@ namespace MultiWheelC.Control.Longitudinal
"纵向控制器最大命令速度必须是正有限值。");
}
}
/// <summary>
/// 检查速度误差死区是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value) ||
value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"纵向控制器速度误差死区必须是非负有限值。");
}
}
}
}
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+3 -3
View File
@@ -330,17 +330,17 @@ namespace MultiWheelC
/// <summary>
/// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
/// </summary>
public double CurvatureTransitionLengthMeters = 0.60;
public double CurvatureTransitionLengthMeters = 0.70;
/// <summary>
/// 获取或设置两段直线的最大参考速度，单位为m/s。
/// </summary>
public double StraightMaximumSpeedMetersPerSecond = 0.30;
public double StraightMaximumSpeedMetersPerSecond = 0.40;
/// <summary>
/// 获取或设置半圆段的最大参考速度，单位为m/s。
/// </summary>
public double SemicircleMaximumSpeedMetersPerSecond = 0.25;
public double SemicircleMaximumSpeedMetersPerSecond = 0.30;
/// <summary>
/// 获取或设置参考速度加速度，单位为m/s²。
MultiWheelC/Experiments/RotationTests.cs
+91 -27
View File
@@ -1,6 +1,7 @@
using System;
using System.Globalization;
using System.Numerics;
using System.Threading;
using ClumsyCore;
@@ -17,7 +18,6 @@ namespace MultiWheelC
public abstract class InPlaceRotateTestBase : MovementTest
{
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
public int TrialNumber = 1; // 重复实验编号。
private DriveTask _task;
@@ -37,11 +37,18 @@ namespace MultiWheelC
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
public override void Test()
{
var config = PilotDefinition.Conf;
if (float.IsNaN(RelativeAngleDegrees) ||
float.IsInfinity(RelativeAngleDegrees) ||
float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
MaxAngularSpeedDegreesPerSecond <= 0f)
float.IsNaN(config.InPlaceRotateMaxSpeed) ||
float.IsInfinity(config.InPlaceRotateMaxSpeed) ||
config.InPlaceRotateMaxSpeed <= 0f ||
float.IsNaN(config.InPlaceRotateMinimumSpeed) ||
float.IsInfinity(config.InPlaceRotateMinimumSpeed) ||
config.InPlaceRotateMinimumSpeed <= 0f ||
config.InPlaceRotateMinimumSpeed >
config.InPlaceRotateMaxSpeed)
{
Console.WriteLine("原地旋转测试参数无效。");
return;
@@ -66,8 +73,25 @@ namespace MultiWheelC
(float)AngleMath.NormalizeDegrees(
location.th + RelativeAngleDegrees);
Console.WriteLine(
"原地自转实际参数：" +
$"Kp={config.InPlaceRotateKp:F3}，" +
$"Ki={config.InPlaceRotateKi:F3}，" +
$"Kd={config.InPlaceRotateKd:F3}，" +
$"到位误差={config.InPlaceRotateArriveDeg:F2}°，" +
$"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" +
$"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" +
$"角加速度={config.InPlaceRotateAcc:F2}°/s²，" +
$"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
$"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
$"起点航向={location.th:F2}°，" +
$"目标航向={targetWorldAngle:F2}°。");
Console.WriteLine(
"原地自转CSV保存目录：" +
TrackingExperimentRecorder.DefaultOutputDirectory);
_recorder = new TrackingExperimentRecorder(
controllerName: "InPlaceRotatePID",
controllerName: "InPlaceRotateFilteredPID",
trajectoryName: _trajectoryName,
trialNumber: TrialNumber,
referenceStart: rotationCenter,
@@ -75,7 +99,7 @@ namespace MultiWheelC
referenceSpeed: 0f,
referenceAngularSpeed:
(float)AngleMath.DegreesToRadians(
MaxAngularSpeedDegreesPerSecond));
config.InPlaceRotateMaxSpeed));
_recorder.Start();
try
@@ -88,21 +112,26 @@ namespace MultiWheelC
PidparamsRead = () => new PIDParams
{
Kp =
PilotDefinition.Conf.InPlaceRotateKp,
config.InPlaceRotateKp,
Ki =
PilotDefinition.Conf.InPlaceRotateKi,
config.InPlaceRotateKi,
Kd =
PilotDefinition.Conf.InPlaceRotateKd,
config.InPlaceRotateKd,
DeadZone =
PilotDefinition.Conf
.InPlaceRotateArriveDeg,
config.InPlaceRotateArriveDeg,
SpeedAccPerSec =
PilotDefinition.Conf.InPlaceRotateAcc,
config.InPlaceRotateAcc,
OutputUpperThreshold =
MaxAngularSpeedDegreesPerSecond,
config.InPlaceRotateMaxSpeed,
MaxI =
PilotDefinition.Conf.InPlaceRotateMaxI
config.InPlaceRotateMaxI
},
MinimumAngularSpeedDegreesPerSecond =
config.InPlaceRotateMinimumSpeed,
WheelAlignmentToleranceDegrees =
config.InPlaceRotateWheelAlignDeg,
RotationTimeoutSeconds =
config.InPlaceRotateTimeoutSec,
CommandAngularSpeedObserver =
commandAngularSpeed =>
_recorder?.UpdateCommand(
@@ -136,23 +165,58 @@ namespace MultiWheelC
}
[MovementTest(name = "SendXYThSpeed：原地自转90°")]
public sealed class TestRotate90 :
[MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
public sealed class TestRotateAngle :
InPlaceRotateTestBase
{
public TestRotate90()
: base(90f, "Rotate90")
public TestRotateAngle()
: base(0f, "RotateCustomAngle")
{
}
/// <summary>
/// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。
/// </summary>
public override void Test()
{
var input = UI.GetInput(
"输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
if ((!float.TryParse(
input,
NumberStyles.Float,
CultureInfo.CurrentCulture,
out var relativeAngleDegrees) &&
!float.TryParse(
input,
NumberStyles.Float,
CultureInfo.InvariantCulture,
out relativeAngleDegrees)) ||
float.IsNaN(relativeAngleDegrees) ||
float.IsInfinity(relativeAngleDegrees))
{
Console.WriteLine("旋转角度输入无效，测试已经取消。");
return;
}
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
{
Console.WriteLine("旋转角度不能为0，测试已经取消。");
return;
}
// 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
if (Math.Abs(relativeAngleDegrees) >= 180f)
{
Console.WriteLine(
"输入角度必须满足-180° < angle < 180°；" +
"当前最短角控制不支持指定精确±180°的旋转方向。");
return;
}
RelativeAngleDegrees = relativeAngleDegrees;
base.Test();
}
}
[MovementTest(name = "SendXYThSpeed：原地自转180°")]
public sealed class TestRotate180 :
InPlaceRotateTestBase
{
public TestRotate180()
: base(180f, "Rotate180")
{
}
}
}
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+21 -3
View File
@@ -145,6 +145,14 @@ namespace MultiWheelC
// 保存成功后的CSV绝对路径；尚未保存时为空。
public string SavedFilePath { get; private set; }
/// <summary>
/// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。
/// </summary>
public static string DefaultOutputDirectory =>
Path.Combine(
AppContext.BaseDirectory,
"TrackingExperiments");
// 启动后台采样线程。
public void Start()
{
@@ -242,6 +250,17 @@ namespace MultiWheelC
}
}
/// <summary>
/// 清除上一轨迹段参考量，避免停车或原地自转期间沿用已经结束的投影结果。
/// </summary>
public void ClearControlReference()
{
lock (_stateSyncRoot)
{
_hasControlReference = false;
}
}
// 停止采样并将本次实验保存为CSV；重复调用只保存一次。
public void StopAndSave()
{
@@ -444,9 +463,8 @@ namespace MultiWheelC
new List<TrackingSample>(_samples);
}
var outputDirectory = Path.Combine(
AppContext.BaseDirectory,
"TrackingExperiments");
var outputDirectory =
DefaultOutputDirectory;
Directory.CreateDirectory(outputDirectory);
MultiWheelC/Movements/RotateInPlace.cs
+191 -5
View File
@@ -7,6 +7,7 @@ using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Commons.Controllers;
using MyParking.Shared;
using MultiWheelC.StateEstimation;
namespace MultiWheelC
{
@@ -17,7 +18,11 @@ namespace MultiWheelC
/// </summary>
public float AngleTarget;
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
// 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
public Func<float> ThetaReader;
public IVehicleStateProvider StateProvider =
new DetourVehicleStateProvider();
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
@@ -37,6 +42,12 @@ namespace MultiWheelC
// 自转舵轮准备超时时间，单位s。
public float WheelAlignmentTimeoutSeconds = 10f;
// 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
public float MinimumAngularSpeedDegreesPerSecond = 1f;
// 舵轮到位后执行航向闭环允许的最长时间，单位s。
public float RotationTimeoutSeconds = 15f;
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
public override IEnumerable<bool> Get()
{
@@ -44,6 +55,8 @@ namespace MultiWheelC
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
ValidateParameters();
var adapter = new MultiWheelChassisAdapter(
Chassis,
PilotDefinition.Self.CarNum);
@@ -55,7 +68,9 @@ namespace MultiWheelC
DateTime? alignedSince = null;
while (true)
{
if (!adapter.PrepareSpin())
if (!adapter.PrepareSpin(
alignmentToleranceDegrees:
WheelAlignmentToleranceDegrees))
throw new InvalidOperationException(
"无法生成原地自转舵轮目标：" +
adapter.LastFailureReason);
@@ -84,18 +99,86 @@ namespace MultiWheelC
yield return true;
}
var alignmentToleranceRadians =
AngleMath.DegreesToRadians(
WheelAlignmentToleranceDegrees);
if (!adapter.AdoptPreparedSpinForXYTh(
alignmentToleranceRadians))
{
throw new InvalidOperationException(
"无法将已到位的自转舵角交接给XYTh：" +
adapter.LastFailureReason);
}
var targetAngle =
(float)AngleMath.NormalizeDegrees(AngleTarget);
var p = PidparamsRead();
thPid = new PIDController(ThetaReader, p.Kp);
var currentAngle = ReadCurrentAngleDegrees();
var cachedCurrentAngle = currentAngle;
thPid = new PIDController(
() => cachedCurrentAngle,
p.Kp);
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
p.OutputUpperThreshold, p.SpeedAccPerSec);
var lastCommandTime = DateTime.Now;
var rotationStarted = DateTime.Now;
while (true)
{
if ((DateTime.Now - rotationStarted)
.TotalSeconds >
RotationTimeoutSeconds)
{
throw new TimeoutException(
$"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
}
currentAngle = ReadCurrentAngleDegrees();
cachedCurrentAngle = currentAngle;
var s = thPid.GetResponse(targetAngle, true);
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
var angleErrorDegrees =
(float)AngleMath
.ShortestDifferenceDegrees(
targetAngle,
currentAngle);
// PID进入到位死区后等待其0.3s稳定确认；等待期间
// 只清零驱动速度，不清除已经准备好的自转舵角状态。
if (Math.Abs(angleErrorDegrees) <=
p.DeadZone)
{
CommandAngularSpeedObserver?.Invoke(0f);
adapter
.StopXYThDrivePreserveSteeringState();
if (thPid.IsArrived())
break;
yield return true;
continue;
}
// PID输出低于底盘有效轮速范围时提高到最小可执行值，
// 避免接近目标时反复出现微小命令但车辆实际不动。
if (Math.Abs(s) > 1e-6f &&
Math.Abs(s) <
MinimumAngularSpeedDegreesPerSecond)
{
s = Math.Sign(angleErrorDegrees) *
MinimumAngularSpeedDegreesPerSecond;
}
// PID加速限制在首周期可能暂时输出零；此时保留
// 已交接的自转状态，等待下一周期产生有效角速度。
if (Math.Abs(s) <= 1e-6f)
{
CommandAngularSpeedObserver?.Invoke(0f);
adapter
.StopXYThDrivePreserveSteeringState();
yield return true;
continue;
}
CommandAngularSpeedObserver?.Invoke(s);
var now = DateTime.Now;
var interval = now - lastCommandTime;
@@ -118,7 +201,6 @@ namespace MultiWheelC
"安全XYTh原地旋转底盘解算失败：" +
adapter.LastFailureReason);
}
if (thPid.IsArrived()) break;
yield return true;
}
@@ -130,5 +212,109 @@ namespace MultiWheelC
adapter.StopImmediately();
}
}
/// <summary>
/// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
/// </summary>
private void ValidateParameters()
{
EnsureFinitePositive(
WheelAlignmentToleranceDegrees,
nameof(WheelAlignmentToleranceDegrees),
allowZero: true);
EnsureFinitePositive(
WheelAlignmentStableSeconds,
nameof(WheelAlignmentStableSeconds),
allowZero: true);
EnsureFinitePositive(
WheelAlignmentTimeoutSeconds,
nameof(WheelAlignmentTimeoutSeconds));
EnsureFinitePositive(
MinimumAngularSpeedDegreesPerSecond,
nameof(MinimumAngularSpeedDegreesPerSecond));
EnsureFinitePositive(
RotationTimeoutSeconds,
nameof(RotationTimeoutSeconds));
var pidParameters = PidparamsRead();
if (pidParameters == null)
{
throw new InvalidOperationException(
"原地自转PID参数读取结果为空。");
}
EnsureFinitePositive(
pidParameters.DeadZone,
"PidparamsRead.DeadZone");
EnsureFinitePositive(
pidParameters.OutputUpperThreshold,
"PidparamsRead.OutputUpperThreshold");
EnsureFinitePositive(
pidParameters.SpeedAccPerSec,
"PidparamsRead.SpeedAccPerSec");
EnsureFinitePositive(
pidParameters.Kp,
"PidparamsRead.Kp");
if (MinimumAngularSpeedDegreesPerSecond >
pidParameters.OutputUpperThreshold)
{
throw new InvalidOperationException(
"原地自转最小有效角速度不能大于最大角速度。");
}
}
/// <summary>
/// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
/// </summary>
private float ReadCurrentAngleDegrees()
{
if (ThetaReader != null)
{
var angleDegrees = ThetaReader();
if (float.IsNaN(angleDegrees) ||
float.IsInfinity(angleDegrees))
{
throw new InvalidOperationException(
"自定义航向读取结果不是有效角度。");
}
return (float)AngleMath.NormalizeDegrees(
angleDegrees);
}
if (StateProvider == null ||
!StateProvider.TryGetState(out var state))
{
throw new InvalidOperationException(
"无法从Detour状态源读取有效车辆航向。" +
(StateProvider is DetourVehicleStateProvider provider
? provider.LastFailureReason
: ""));
}
return (float)AngleMath.RadiansToDegrees(
state.PoseInWorld.YawRadians);
}
/// <summary>
/// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
/// </summary>
private static void EnsureFinitePositive(
float value,
string parameterName,
bool allowZero = false)
{
if (float.IsNaN(value) ||
float.IsInfinity(value) ||
(allowZero
? value < 0f
: value <= 0f))
{
throw new ArgumentOutOfRangeException(
parameterName,
"原地自转参数必须是有效的正数。");
}
}
}
}
MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+9 -2
View File
@@ -75,6 +75,12 @@ namespace MultiWheelC
/// </summary>
public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
/// <summary>
/// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
/// </summary>
public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
0.025;
/// <summary>
/// 底盘纵向命令速度绝对值上限，单位为m/s。
/// </summary>
@@ -90,7 +96,7 @@ namespace MultiWheelC
/// 前后GCP目标转角最大变化率，单位为rad/s。
/// </summary>
public double MaximumGcpAngleRateRadiansPerSecond =
AngleMath.DegreesToRadians(10.0);
AngleMath.DegreesToRadians(15.0);
/// <summary>
/// 终点位置和剩余弧长的完成容差，单位为m。
@@ -164,7 +170,8 @@ namespace MultiWheelC
LongitudinalKiPerSecond,
LongitudinalKdSeconds,
MaximumIntegralCorrectionMetersPerSecond,
MaximumCommandSpeedMetersPerSecond);
MaximumCommandSpeedMetersPerSecond,
LongitudinalSpeedErrorDeadbandMetersPerSecond);
var gcpAllocator =
new AckermannGcpAllocator(
controlPointRadiusMeters,
MultiWheelC/PilotConfig.cs
+9 -8
View File
@@ -26,7 +26,7 @@ public class PilotConfig : MultiWheelPilotConfig
public float InPlaceRotateSpeed = 30f;
[FieldMember(desc = "原地旋转：到位角度精度(deg)")]
public float InPlaceRotateArriveDeg = 1f;
public float InPlaceRotateArriveDeg = 1.5f;
[FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
public float InPlaceRotateWheelAlignDeg = 2f;
@@ -38,24 +38,25 @@ public class PilotConfig : MultiWheelPilotConfig
#region 单车-临时
[FieldMember(desc = "原地旋转Kp")]
public float InPlaceRotateKp = 0.2f;
// public float InPlaceRotateKp = 0.2f;
public float InPlaceRotateKp = 1.1f;
[FieldMember(desc = "原地旋转Ki")]
public float InPlaceRotateKi = 0.01f;
// public float InPlaceRotateKi = 0.01f;
public float InPlaceRotateKi = 0f;
[FieldMember(desc = "原地旋转Kd")]
public float InPlaceRotateKd = 0f;
[FieldMember(desc = "原地旋转积分限幅")]
public float InPlaceRotateMaxI = 0.01f;
public float InPlaceRotateMaxI = 0f;
[FieldMember(desc = "原地旋转最小有效角速度(deg/s)")]
public float InPlaceRotateMinimumSpeed = 1f;
[FieldMember(desc = "原地旋转最大角速度(deg/s)")]
public float InPlaceRotateMaxSpeed = 30f;
public float InPlaceRotateMaxSpeed = 47.5f;
[FieldMember(desc = "原地旋转角加速度(deg/s²)")]
public float InPlaceRotateAcc = 30f;
public float InPlaceRotateAcc = 60f;
[FieldMember(desc = "原地旋转超时(s)")]
public float InPlaceRotateTimeoutSec = 15f;
MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs
+2 -2
View File
@@ -188,11 +188,11 @@ namespace MultiWheelC.StateEstimation
poseInWorld,
elapsedSeconds))
{
state = AcceptPoseAfterVelocityRebase(
state = AcceptPoseAfterReset(
poseInWorld,
timestampSeconds);
LastFailureReason =
"Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。";
"Detour位姿偏离速度预测，已重新建立速度估计基准。";
return true;
}
Shared/Chassis/MultiWheelChassisAdapter.cs
+15 -2
View File
@@ -504,13 +504,26 @@ namespace MyParking.Shared
/// 返回是否成功生成舵轮目标。
/// </summary>
public bool PrepareSpin(
TimeSpan? interval = null)
TimeSpan? interval = null,
double alignmentToleranceDegrees = 2.0)
{
ValidateFinite(
alignmentToleranceDegrees,
nameof(alignmentToleranceDegrees));
if (alignmentToleranceDegrees < 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(alignmentToleranceDegrees),
"自转舵轮到位容差必须是非负有限值。");
}
EnsureBodyFrameIsActive();
var success =
_chassis.PrepareRotateWheels(
alignmentToleranceDegrees: 2.0f);
alignmentToleranceDegrees:
(float)alignmentToleranceDegrees);
if (!success)
{
data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py
+52 -16
View File
@@ -208,10 +208,13 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
has_control_reference = (
numeric_column(frame, "HasControlReference", 0.0) > 0.5
)
lateral_error = np.where(
has_control_reference & np.isfinite(recorded_lateral_error),
recorded_lateral_error,
derived_lateral_error,
recorded_lateral_valid = (
has_control_reference & np.isfinite(recorded_lateral_error)
)
lateral_error = (
np.where(recorded_lateral_valid, recorded_lateral_error, np.nan)
if np.any(recorded_lateral_valid)
else derived_lateral_error
)
state_yaw = numeric_column(frame, "StateYawRadians")
@@ -230,10 +233,28 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
frame,
"ControlHeadingErrorRadians",
)
heading_error = np.where(
has_control_reference & np.isfinite(recorded_heading_error),
recorded_heading_error,
derived_heading_error,
recorded_heading_valid = (
has_control_reference & np.isfinite(recorded_heading_error)
)
heading_error = (
np.where(recorded_heading_valid, recorded_heading_error, np.nan)
if np.any(recorded_heading_valid)
else derived_heading_error
)
# 投影定义满足：参考点 = 车体位置 + 横向误差 × 参考航向左法向。
# 因此无需假设轨迹类型，即可从有效控制周期还原车辆实际使用的参考轨迹。
projected_reference_yaw = actual_yaw + heading_error
reference_x = (
actual_x - lateral_error * np.sin(projected_reference_yaw)
)
reference_y = (
actual_y + lateral_error * np.cos(projected_reference_yaw)
)
valid_reference_position = (
has_control_reference
& np.isfinite(reference_x)
& np.isfinite(reference_y)
)
cruise_speed = first_finite(
@@ -270,6 +291,8 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
numeric_column(frame, "ControlReferenceSpeedMetersPerSecond"),
ideal_speed,
)
if np.any(has_control_reference):
reference_speed[~has_control_reference] = np.nan
actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond")
velocity_valid = (
numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
@@ -283,6 +306,9 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
"actual_x": actual_x,
"actual_y": actual_y,
"valid_position": valid_position,
"reference_x": reference_x,
"reference_y": reference_y,
"valid_reference_position": valid_reference_position,
"start": start,
"end": end,
"length": length_meters,
@@ -336,13 +362,23 @@ def plot_experiment(
valid_position = data["valid_position"]
fig, axis = plt.subplots(figsize=(9.0, 6.5))
axis.plot(
[data["start"][0], data["end"][0]],
[data["start"][1], data["end"][1]],
"--",
linewidth=2.0,
label="期望4m直线轨迹",
)
valid_reference_position = data["valid_reference_position"]
if np.count_nonzero(valid_reference_position) >= 2:
axis.plot(
data["reference_x"][valid_reference_position],
data["reference_y"][valid_reference_position],
"--",
linewidth=2.0,
label="控制器实际使用的参考轨迹",
)
else:
axis.plot(
[data["start"][0], data["end"][0]],
[data["start"][1], data["end"][1]],
"--",
linewidth=2.0,
label="参考起终点连线",
)
axis.plot(
data["actual_x"][valid_position],
data["actual_y"][valid_position],
@@ -451,7 +487,7 @@ def discover_csv_files(arguments: list[str]) -> list[Path]:
def main() -> None:
"""解析命令行并批量处理新版控制器实验CSV。"""
parser = argparse.ArgumentParser(
description="绘制新版控制器4m直线实验的四类对比图。"
description="绘制新版控制器轨迹实验的四类对比图。"
)
parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
parser.add_argument(
docs/clumsy参考.json
+534
View File
@@ -0,0 +1,534 @@
{
"layout": {
"chassis": {
"width": 1100.0,
"length": 1550.0,
"contour": [
-775.0,
550.0,
775.0,
550.0,
775.0,
-550.0,
-775.0,
-550.0
]
},
"components": [
{
"type": "wheel",
"options": {
"platform": 0,
"scale": 1.0,
"radius": 200.0,
"group": null,
"id": 1,
"name": "w1",
"x": 0.0,
"y": 300.0,
"yaw": 0.0,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "wheel",
"options": {
"platform": 6,
"scale": 1.0,
"radius": 200.0,
"group": null,
"id": 2,
"name": "w2",
"x": 0.0,
"y": -300.0,
"yaw": 0.0,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "lidarssc",
"options": {
"usingLidars": "frontlidar",
"stopDist": 100.0,
"directionX": 1.0,
"directionY": 0.0,
"thresDot": 9999,
"contour": [
0.0,
0.0
],
"group": [
"0",
"stop"
],
"id": 411683697,
"name": "autoStop",
"x": 0.0,
"y": 0.0,
"yaw": 0.0,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "lidarssc",
"options": {
"usingLidars": "frontlidar",
"stopDist": 100.0,
"directionX": 1.0,
"directionY": 0.0,
"thresDot": 999,
"contour": [
0.0,
0.0
],
"group": [
"0",
"slow"
],
"id": 726862610,
"name": "autoSlow",
"x": 0.0,
"y": 0.0,
"yaw": 0.0,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "lidar2d",
"options": {
"isCircle": true,
"ignoreDist": 10.0,
"maxDist": 200000.0,
"useFilter": "",
"filterChassis": true,
"afterImageFilterOutN": 7,
"afterImageFilterOutDeg": 2.0,
"reflexThres": 0.4,
"reflexFilterWndSz": 30,
"reflexDistWnd": 50.0,
"reflexChunkThres": 2.5,
"BindLidar2dName": "",
"BindRelativeX": -4.9166203,
"BindRelativeY": 938.9871,
"BindRelativeTh": 1.300003,
"group": null,
"id": 1444795304,
"name": "rightlidar",
"x": -749.0,
"y": -475.0,
"yaw": 180.8,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "lidar2d",
"options": {
"isCircle": true,
"ignoreDist": 10.0,
"maxDist": 200000.0,
"useFilter": "",
"filterChassis": true,
"afterImageFilterOutN": 7,
"afterImageFilterOutDeg": 2.0,
"reflexThres": 0.4,
"reflexFilterWndSz": 30,
"reflexDistWnd": 50.0,
"reflexChunkThres": 2.5,
"BindLidar2dName": "",
"BindRelativeX": 0.0,
"BindRelativeY": 0.0,
"BindRelativeTh": 0.0,
"group": null,
"id": 1983955111,
"name": "leftlidar",
"x": -734.0,
"y": 475.0,
"yaw": 179.8,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "lidar3d",
"options": {
"ignoreDist": 5.0,
"maxDist": 200000.0,
"reduce": false,
"voxelSize": 70.0,
"pcklen": 82560,
"angleSgn": -1,
"endAngle": 0.0,
"RotationMatrix": [
1.0,
0.0,
0.0,
0.0,
1.0,
0.0,
-0.0,
0.0,
1.0
],
"group": null,
"id": 1000069841,
"name": "frontlidar3d",
"x": 752.5,
"y": 0.0,
"yaw": 0.0,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "plannar3dlidarzrange",
"options": {
"zmin": -65.0,
"zmax": 7.0,
"useAbsolute": true,
"samples": 1024,
"lidar3dName": "frontlidar3d",
"isCircle": true,
"ignoreDist": 10.0,
"maxDist": 200000.0,
"useFilter": "",
"filterChassis": true,
"afterImageFilterOutN": 7,
"afterImageFilterOutDeg": 2.0,
"reflexThres": 0.4,
"reflexFilterWndSz": 30,
"reflexDistWnd": 50.0,
"reflexChunkThres": 2.5,
"BindLidar2dName": "",
"BindRelativeX": 0.0,
"BindRelativeY": 0.0,
"BindRelativeTh": 0.0,
"group": null,
"id": 1954892242,
"name": "frontlidar",
"x": 752.5,
"y": 0.0,
"yaw": 0.0,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "lidarssc",
"options": {
"usingLidars": "frontlidar",
"stopDist": 100.0,
"directionX": 1.0,
"directionY": 0.0,
"thresDot": 10,
"contour": [
600.0,
-650.0,
600.0,
650.0,
1200.0,
650.0,
1200.0,
-650.0
],
"group": [
"1",
"stop"
],
"id": 1519627219,
"name": "fstop1",
"x": 230.0,
"y": 0.0,
"yaw": 0.0,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
},
{
"type": "lidarssc",
"options": {
"usingLidars": "frontlidar",
"stopDist": 100.0,
"directionX": 1.0,
"directionY": 0.0,
"thresDot": 10,
"contour": [
1200.0,
-650.0,
1200.0,
650.0,
2600.0,
650.0,
2600.0,
-650.0
],
"group": [
"1",
"slow"
],
"id": 771141297,
"name": "fslow1",
"x": 230.0,
"y": 0.0,
"yaw": 0.0,
"z": 0.0,
"pitch": 0.0,
"roll": 0.0
}
}
]
},
"DriveTaskInterval": 30,
"DriveTaskTimeout": 9999.0,
"basicSpeed": 0.7,
"msConf": {
"SyncThAccPerSec": 5.0,
"TestCarSyncDistance": 2893.0,
"TestCarSyncTh": 0.0,
"ManualCarSyncVxFac": 0.3,
"ManualCarSyncVyFac": 1.0,
"ManualCarSyncVthFac": 30.0,
"MultiVehicleCrabSteerLimitDeg": 120.0,
"DeltaDetectCenter": 742.5,
"MultiVehicleSyncUseDetour": true,
"MultiVehicleManualUseDetourCorrection": true,
"MultiVehicleFleetNum": 2,
"MultiVehicleSyncInterval": 25,
"MultiVehicleMasterEndpoint": "/",
"SimpleIp": "192.168.1.101",
"MultiVehicleSelfEndpoint": "",
"MultiVehicleUseDetect": false,
"MultiVehicleControlRadius": 0.0,
"MultiVehicleAutoCmdTimeoutMs": 9999,
"MultiVehicleMemberTtlMs": 0,
"MultiVehicleAutoUseIdealCenter": true,
"MultiVehicleAutoRequireFleetCenter": true,
"MultiVehiclePosBiasXFac": 0.005,
"MultiVehiclePosBiasYFac": 0.15,
"MultiVehiclePosBiasThFac": 0.1,
"MultiVehiclePosBiasXThreshold": 0.05,
"MultiVehiclePosBiasYThreshold": 10.0,
"MultiVehiclePosBiasThThreshold": 5.0,
"MultiVehicleDetectBiasXFac": 0.0,
"MultiVehicleDetectBiasYFac": 0.0,
"MultiVehicleDetectBiasThFac": 0.0,
"MultiVehicleDetectBiasXThreshold": 0.0,
"MultiVehicleDetectBiasYThreshold": 0.0,
"MultiVehicleDetectBiasThThreshold": 0.0,
"MultiVehicleRotateCompXyFac": 0.003,
"MultiVehicleRotateCompXyIFac": 0.01,
"MultiVehicleRotateCompXyMax": 3.0,
"MultiVehicleRotateCompThFac": 0.1,
"MultiVehicleRotateCompThIFac": 0.01,
"MultiVehicleRotateCompThMax": 3.0,
"MultiVehicleRotateActiveOmega": 0.5,
"MultiVehicleRotateCompTangentFrac": 0.1,
"SingleCarSyncPrecisionXy": 10.0,
"SingleCarSyncPrecisionTh": 0.1,
"PlaygroundWebApiUrl": "http://localhost:18090",
"MultiVehicleRotatePoseWebApiDiagEnabled": false,
"PlaygroundRobotName": "agv_multi_1",
"PlaygroundNeighborRobotName": "agv_multi_2",
"WebApiTranslateMm": 100.0,
"WebApiRotateDeg": 5.0,
"InPlaceRotateTargetWorldDeg": 90.0,
"InPlaceRotateSpeed": 30.0,
"InPlaceRotateArriveDeg": 1.0,
"InPlaceRotateWheelAlignDeg": 2.0,
"InPlaceRotateActiveWheelAlignDeg": 10.0,
"FleetRotateOmega": 6.0,
"FleetRotateTargetDeltaDeg": 90.0,
"FleetRotateArriveDeg": 1.5,
"FleetRotateSlowDeg": 10.0,
"FleetRotateMinOmega": 0.5,
"FleetRotateAccel": 1.0,
"FleetRotateSettleSec": 0.5,
"FleetRotateUseDetourHeading": true,
"FleetCrabAngleDeg": 90.0,
"FleetCrabBodyWorldHeadingDeg": 0.0,
"FleetCrabLengthMm": 2000.0,
"FleetCrabSpeed": 0.35,
"FleetCrabAccel": 0.1,
"FleetCrabStartAccel": 0.02,
"FleetCrabSlowDistance": 800.0,
"FleetCrabFinishDistance": 10.0,
"FleetCrabFinishSpeed": 0.0,
"FleetCrabSlowingPow": 0.7,
"FleetCrabGcpThetaThreshold": 120.0,
"FleetCrabDthLinearFac": 3.3,
"FleetCrabDthLinearThreshold": 10.0,
"FleetCrabStartSyncTimeoutSec": 9999.0,
"FleetCrabStartWheelAlignDeg": 2.0,
"TwoLegLidarName": "leftlidar,rearlidar",
"TwoLegGuessX": -2000.0,
"TwoLegWidth": 450.0,
"TwoLegWidthErr": 50.0,
"TwoLegBlobDist": 100.0,
"TwoLegBlobSize": 200.0,
"TwoLegBlobPtCount": 10,
"TwoLegPadding": 5,
"TwoLegPillarFindingScope": 20,
"TwoLegSgnDir": 1,
"TwoLegCenterChangeX": 0.0,
"TwoLegOutputBiasX": -15.0,
"TwoLegOutputBiasY": 0.0,
"TwoLegFilterLength": 500.0,
"TwoLegFilterWidth": 800.0,
"TireFilterLength": 1000.0,
"TireFilterWidth": 1900.0,
"TireTwoLegWidth": 1470.0,
"TireTwoLegWidthErr": 200.0,
"TireTwoLegBlobPtCount": 10,
"TireFrontTwoLegBlobDist": 100.0,
"TireFrontTwoLegBlobSize": 200.0,
"TireFrontPadding": 10,
"TireFrontTwoLegPillarFindingScope": 10,
"TireFrontTwoLegSgnDir": 1,
"TireFrontTwoLegCenterChangeX": 0.0,
"TireBackTwoLegBlobDist": 100.0,
"TireBackTwoLegBlobSize": 200.0,
"TireBackPadding": 5,
"TireBackTwoLegPillarFindingScope": 20,
"TireBackTwoLegSgnDir": 1,
"TireBackTwoLegCenterChangeX": 0.0,
"ClampControlKp": 0.0015,
"ClampControlKi": 0.0,
"ClampControlKd": 0.0,
"ClampControlMaxI": 0.02,
"ClampControlSpeedAcc": 2.0,
"ClampControlThresh": 0.2,
"ClampControlDeadZone": 30.0,
"MaxClampSpeed": 12.0,
"LineTrackDistance": 1000.0,
"LineTrackMaxSpeed": 0.3,
"LineTrackKp": 0.001,
"LineTrackKi": 0.0,
"LineTrackKd": 0.0,
"LineTrackDeadZone": 10.0,
"TireFollowingWalkBlindSwitchingDistance": 1300.0,
"TireFollowingStage1GuessX": 2200.0,
"TireFollowingStage2GuessX": 2600.0,
"TireFollowingWalkBlindFinishDistance": 10.0,
"TireFollowingSlowDistance": 750.0,
"TireFollowingMaxSpeed": 0.3,
"TireFollowingFrontLidarPathTransformationX": 160.0,
"TireFollowingFrontLidarPathTransformationY": 3.0,
"TireFollowingFrontLidarWalkBlindTh": 0.0,
"TireFollowingBackLidarPathTransformationX": 155.0,
"TireFollowingBackLidarPathTransformationY": 0.0,
"TireFollowingBackLidarWalkBlindTh": 0.0,
"TireFollowingLeaveCarBackLidarPathTransformationX": 1700.0,
"TireFollowingLeaveCarWalkBlindSwitchingDistance": 2400.0,
"TireFollowingTireNum": 2,
"TireFollowingCloseDistance": 1400.0,
"TireFollowingAngleIgnoreThr": 1.0,
"TireFollowingYAverageFrameCount": 6,
"DstTrackerMaxSpeed": 0.3,
"GcpThetaThreshold": 70.0,
"DthLinearFac": 0.75,
"DthLinearThreshold": 25.0,
"BiasFac": 0.5,
"BiasThreshold": 15.0,
"BiasControlGainFac": 1.0,
"BiasSlowSigma": 55.0,
"LineMagKp": 0.1,
"LineMagKi": 0.0,
"LineMagKd": 0.1,
"MagMaxI": 10.0,
"MagDeadZone": 1.0,
"LineMagThresh": 35.0,
"CurveMagKp": 0.4,
"CurveMagKi": 0.0,
"CurveMagKd": 0.1,
"CurveMagThresh": 65.0,
"MotionDebugPrint": true,
"DebugCurvature": false,
"SlowDistance": 1000.0,
"SlowingPow": 0.8,
"FinishDistance": 5.0,
"FinishSpeed": 0.02,
"FirstThAccuracy": 2.0,
"ThContinuousThreshold": 10.0,
"FirstRotateSpeedFac": 1.0,
"FirstRotateMaxSpeed": 30.0,
"FirstRotateAcc": 20.0,
"FirstRotateDeAcc": 30.0,
"SpeedAccPerSecond": 0.1,
"SpeedDeAccPerSecond": 1.0,
"NotContinuousAngle": 3.0,
"PowerSteeringLookAhead": 100.0,
"SpeedLookAhead": 1500.0,
"SpeedLookAheadCurveDiff": 1000.0,
"SpeedLookBackCurveDiff": 200.0,
"SpeedLimitCurveDiffMin": 0.2,
"SpeedLimitCurveMin": 0.2,
"MaxRotateSpeedCurveLimit": 30.0,
"MaxRotateAccCurveLimit": 30.0,
"BaisAlarmValue": 1500.0,
"DthAlarmValue": 150.0,
"UseAutoAvoidance": false,
"ObstacleStopDistance": 1000.0,
"ObstacleSlowDistance": 2500.0,
"CoefficientOfExpansion": 1.0,
"TargetSpeed": 0.5,
"EmptyCartLength": 1550.0,
"EmptyCartWidth": 1100.0,
"RotateStopFac": 1.3,
"RotateSlowFac": 1.8,
"SlowPow": 1.2,
"ShieldAutoObstacle": false,
"LidarName": "frontlidar",
"ObstacleRecoveryTime": 500,
"UseCameraAvoidance": false,
"UseManualContorolAvoidance": true,
"ManualAutoAvoidcaneSlowDistance": 500.0,
"ManualAutoAvoidcaneStopDistance": 300.0,
"ShieldAutoAvoidance": false,
"UpCamPoseX": 0.0,
"UpCamPoseY": 0.0,
"UpCamPoseTh": 0.0,
"DownCamPoseX": 0.0,
"DownCamPoseY": 0.0,
"DownCamPoseTh": 0.0,
"OutMapEnable": true,
"GroundLossThreshold": 5.0,
"LaserLossThreshold": 15.0,
"RiskSlowdownThreshold": 15.0,
"UseSimpleDetector": false,
"LoseConnectionTime": 5000,
"GroundCameraDisconnectAlarmTime": 200,
"FrontLidarName": "null",
"RearLidarName": "null",
"UseSkidDetector": true,
"SkidTimeThreshold": 3000,
"SkidFacThreshold": 3.0,
"MissionWarningTime": 3000,
"UseGyrosDetector": false,
"GyrosErrorTime": 3000,
"GyrosErrorFac": 10,
"ObstacleStopDec": 0.1
},
"script": "MultiWheelC.dll",
"guru": {
"MaxLogFiles": 20,
"interpreter": "javascript",
"throwSAIError": true
},
"locationTimeout": 100,
"IOCheckIntegrity": true,
"detourHost": "127.0.0.1",
"detourPort": 4321
}