完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -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²。
|
||||
|
||||
@@ -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")
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user