拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1 @@
|
||||
// 所有横向控制器的统一接口
|
||||
@@ -0,0 +1 @@
|
||||
// 统一纵向控制接口
|
||||
@@ -0,0 +1 @@
|
||||
// 横向控制器的输出
|
||||
@@ -0,0 +1 @@
|
||||
// 保存一次控制周期需要的完整输入
|
||||
@@ -0,0 +1 @@
|
||||
// 车体中心命令曲率转换成前后GCP方向
|
||||
@@ -0,0 +1,9 @@
|
||||
// 表示发送给底盘前的中间命令
|
||||
// public readonly struct GcpMotionCommand
|
||||
// {
|
||||
// public double SpeedMetersPerSecond { get; }
|
||||
|
||||
// public double FrontAngleRadians { get; }
|
||||
|
||||
// public double RearAngleRadians { get; }
|
||||
// }
|
||||
@@ -0,0 +1 @@
|
||||
// 负责把纯数学命令转换成现有底盘调用
|
||||
@@ -0,0 +1 @@
|
||||
// 负责组织一个控制周期
|
||||
@@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
using System;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public abstract class ClampMovementTestBase : MovementTest
|
||||
{
|
||||
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
|
||||
|
||||
private DriveTask _task;
|
||||
protected abstract bool Close { get; }
|
||||
|
||||
// 根据派生测试类型驱动左右夹臂同步夹紧或打开。
|
||||
public override void Test()
|
||||
{
|
||||
var leftTarget = Close
|
||||
? PilotDefinition.Self.LeftArmUpperPos
|
||||
: PilotDefinition.Self.LeftArmLowerPos;
|
||||
var rightTarget = Close
|
||||
? PilotDefinition.Self.RightArmUpperPos
|
||||
: PilotDefinition.Self.RightArmLowerPos;
|
||||
|
||||
if (float.IsNaN(leftTarget) ||
|
||||
float.IsInfinity(leftTarget) ||
|
||||
float.IsNaN(rightTarget) ||
|
||||
float.IsInfinity(rightTarget))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"夹臂目标位置无效，取消夹臂运动测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
// 防止重复点击时上一项夹臂任务仍在运行。
|
||||
TestStop();
|
||||
Console.WriteLine(
|
||||
$"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
|
||||
$"左目标={leftTarget}，右目标={rightTarget}");
|
||||
|
||||
var task = new DriveTask(
|
||||
new ClampToTarget
|
||||
{
|
||||
LeftClampTarget = leftTarget,
|
||||
RightClampTarget = rightTarget,
|
||||
TimeoutSeconds = TimeoutSeconds
|
||||
}.Get());
|
||||
_task = task;
|
||||
|
||||
try
|
||||
{
|
||||
task.Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
if (ReferenceEquals(_task, task))
|
||||
_task = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止夹臂任务并立即清零左右夹臂下发速度。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_task = null;
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂关闭测试")]
|
||||
public sealed class TestClampCloseMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => false;
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂启动测试")]
|
||||
public sealed class TestClampOpenMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => true;
|
||||
}
|
||||
}
|
||||
@@ -1,108 +1,20 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MyParking.Shared;
|
||||
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
internal static class MovementTestPreparation
|
||||
{
|
||||
// 在测试正式开始前，将四个舵轮稳定回正到车体前向。
|
||||
public static bool AlignWheelsForward(
|
||||
ref DriveTask activeTask)
|
||||
{
|
||||
var preparation = new PrepareWheelsForward();
|
||||
var task = new DriveTask(preparation.Get());
|
||||
activeTask = task;
|
||||
|
||||
try
|
||||
{
|
||||
task.Wait();
|
||||
return preparation.Completed;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"测试前舵轮回正失败：{ex.Message}");
|
||||
return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task.Stop();
|
||||
if (ReferenceEquals(activeTask, task))
|
||||
activeTask = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
|
||||
public static bool AreWheelsForward(
|
||||
float toleranceDegrees = 2f)
|
||||
{
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
var toleranceRadians =
|
||||
AngleMath.DegreesToRadians(toleranceDegrees);
|
||||
|
||||
if (adapter.AreParallelWheelsAligned(
|
||||
0.0,
|
||||
toleranceRadians))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
"四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
|
||||
return false;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"检查舵轮方向失败：{ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "准备：四个舵轮与车头方向一致")]
|
||||
public class AlignWheelsForwardTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
// 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
|
||||
public override void Test()
|
||||
{
|
||||
MovementTestPreparation.AlignWheelsForward(
|
||||
ref _task);
|
||||
}
|
||||
|
||||
// 停止正在执行的舵轮回正任务并清零底盘运动命令。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_task = null;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendMotion：连续前进4m")]
|
||||
public class TestForward4m : MovementTest
|
||||
{
|
||||
@@ -179,148 +91,6 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
public abstract class InPlaceRotateTestBase : MovementTest
|
||||
{
|
||||
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
|
||||
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
|
||||
public int TrialNumber = 1; // 重复实验编号。
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
private readonly string _trajectoryName;
|
||||
|
||||
protected InPlaceRotateTestBase(
|
||||
float relativeAngleDegrees,
|
||||
string trajectoryName)
|
||||
{
|
||||
RelativeAngleDegrees =
|
||||
relativeAngleDegrees;
|
||||
_trajectoryName =
|
||||
trajectoryName;
|
||||
}
|
||||
|
||||
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
|
||||
public override void Test()
|
||||
{
|
||||
if (float.IsNaN(RelativeAngleDegrees) ||
|
||||
float.IsInfinity(RelativeAngleDegrees) ||
|
||||
float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
|
||||
float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
|
||||
MaxAngularSpeedDegreesPerSecond <= 0f)
|
||||
{
|
||||
Console.WriteLine("原地旋转测试参数无效。");
|
||||
return;
|
||||
}
|
||||
|
||||
var location = DetourInterface.getCartLocation();
|
||||
if (double.IsNaN(location.x) ||
|
||||
double.IsInfinity(location.x) ||
|
||||
double.IsNaN(location.y) ||
|
||||
double.IsInfinity(location.y) ||
|
||||
double.IsNaN(location.th) ||
|
||||
double.IsInfinity(location.th))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"Detour当前位姿无效，取消原地旋转测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var rotationCenter =
|
||||
new Vector2((float)location.x, (float)location.y);
|
||||
var targetWorldAngle =
|
||||
(float)AngleMath.NormalizeDegrees(
|
||||
location.th + RelativeAngleDegrees);
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "InPlaceRotatePID",
|
||||
trajectoryName: _trajectoryName,
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
referenceEnd: rotationCenter,
|
||||
referenceSpeed: 0f,
|
||||
referenceAngularSpeed:
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
MaxAngularSpeedDegreesPerSecond));
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(
|
||||
new MultiWheelRotateInPlace
|
||||
{
|
||||
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
|
||||
AngleTarget = targetWorldAngle,
|
||||
PidparamsRead = () => new PIDParams
|
||||
{
|
||||
Kp =
|
||||
PilotDefinition.Conf.InPlaceRotateKp,
|
||||
Ki =
|
||||
PilotDefinition.Conf.InPlaceRotateKi,
|
||||
Kd =
|
||||
PilotDefinition.Conf.InPlaceRotateKd,
|
||||
DeadZone =
|
||||
PilotDefinition.Conf
|
||||
.InPlaceRotateArriveDeg,
|
||||
SpeedAccPerSec =
|
||||
PilotDefinition.Conf.InPlaceRotateAcc,
|
||||
OutputUpperThreshold =
|
||||
MaxAngularSpeedDegreesPerSecond,
|
||||
MaxI =
|
||||
PilotDefinition.Conf.InPlaceRotateMaxI
|
||||
},
|
||||
CommandAngularSpeedObserver =
|
||||
commandAngularSpeed =>
|
||||
_recorder?.UpdateCommand(
|
||||
0f,
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
commandAngularSpeed))
|
||||
}.Get());
|
||||
|
||||
_task.Wait();
|
||||
|
||||
// 保留少量停止后的样本，用于观察角速度是否回到零。
|
||||
Thread.Sleep(300);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止原地旋转并保存当前已经采集的实验数据。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：原地自转90°")]
|
||||
public sealed class TestRotate90 :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestRotate90()
|
||||
: base(90f, "Rotate90")
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：原地自转180°")]
|
||||
public sealed class TestRotate180 :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestRotate180()
|
||||
: base(180f, "Rotate180")
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
|
||||
public class TestArcMovement : MovementTest
|
||||
{
|
||||
@@ -449,7 +219,6 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
#region 蟹行运动测试
|
||||
[MovementTest(name = "SendMotion：蟹行直线4m")]
|
||||
public class TestCrabForward4m : MovementTest
|
||||
{
|
||||
@@ -863,87 +632,4 @@ namespace MultiWheelC
|
||||
origin.Y + localX * sin + localY * cos);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 夹臂运动测试
|
||||
public abstract class ClampMovementTestBase : MovementTest
|
||||
{
|
||||
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
|
||||
|
||||
private DriveTask _task;
|
||||
protected abstract bool Close { get; }
|
||||
|
||||
// 根据派生测试类型驱动左右夹臂同步夹紧或打开。
|
||||
public override void Test()
|
||||
{
|
||||
var leftTarget = Close
|
||||
? PilotDefinition.Self.LeftArmUpperPos
|
||||
: PilotDefinition.Self.LeftArmLowerPos;
|
||||
var rightTarget = Close
|
||||
? PilotDefinition.Self.RightArmUpperPos
|
||||
: PilotDefinition.Self.RightArmLowerPos;
|
||||
|
||||
if (float.IsNaN(leftTarget) ||
|
||||
float.IsInfinity(leftTarget) ||
|
||||
float.IsNaN(rightTarget) ||
|
||||
float.IsInfinity(rightTarget))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"夹臂目标位置无效，取消夹臂运动测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
// 防止重复点击时上一项夹臂任务仍在运行。
|
||||
TestStop();
|
||||
Console.WriteLine(
|
||||
$"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
|
||||
$"左目标={leftTarget}，右目标={rightTarget}");
|
||||
|
||||
var task = new DriveTask(
|
||||
new ClampToTarget
|
||||
{
|
||||
LeftClampTarget = leftTarget,
|
||||
RightClampTarget = rightTarget,
|
||||
TimeoutSeconds = TimeoutSeconds
|
||||
}.Get());
|
||||
_task = task;
|
||||
|
||||
try
|
||||
{
|
||||
task.Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
if (ReferenceEquals(_task, task))
|
||||
_task = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止夹臂任务并立即清零左右夹臂下发速度。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_task = null;
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂关闭测试")]
|
||||
public sealed class TestClampCloseMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => false;
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂启动测试")]
|
||||
public sealed class TestClampOpenMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => true;
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
|
||||
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public abstract class InPlaceRotateTestBase : MovementTest
|
||||
{
|
||||
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
|
||||
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
|
||||
public int TrialNumber = 1; // 重复实验编号。
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
private readonly string _trajectoryName;
|
||||
|
||||
protected InPlaceRotateTestBase(
|
||||
float relativeAngleDegrees,
|
||||
string trajectoryName)
|
||||
{
|
||||
RelativeAngleDegrees =
|
||||
relativeAngleDegrees;
|
||||
_trajectoryName =
|
||||
trajectoryName;
|
||||
}
|
||||
|
||||
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
|
||||
public override void Test()
|
||||
{
|
||||
if (float.IsNaN(RelativeAngleDegrees) ||
|
||||
float.IsInfinity(RelativeAngleDegrees) ||
|
||||
float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
|
||||
float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
|
||||
MaxAngularSpeedDegreesPerSecond <= 0f)
|
||||
{
|
||||
Console.WriteLine("原地旋转测试参数无效。");
|
||||
return;
|
||||
}
|
||||
|
||||
var location = DetourInterface.getCartLocation();
|
||||
if (double.IsNaN(location.x) ||
|
||||
double.IsInfinity(location.x) ||
|
||||
double.IsNaN(location.y) ||
|
||||
double.IsInfinity(location.y) ||
|
||||
double.IsNaN(location.th) ||
|
||||
double.IsInfinity(location.th))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"Detour当前位姿无效，取消原地旋转测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var rotationCenter =
|
||||
new Vector2((float)location.x, (float)location.y);
|
||||
var targetWorldAngle =
|
||||
(float)AngleMath.NormalizeDegrees(
|
||||
location.th + RelativeAngleDegrees);
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "InPlaceRotatePID",
|
||||
trajectoryName: _trajectoryName,
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
referenceEnd: rotationCenter,
|
||||
referenceSpeed: 0f,
|
||||
referenceAngularSpeed:
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
MaxAngularSpeedDegreesPerSecond));
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(
|
||||
new MultiWheelRotateInPlace
|
||||
{
|
||||
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
|
||||
AngleTarget = targetWorldAngle,
|
||||
PidparamsRead = () => new PIDParams
|
||||
{
|
||||
Kp =
|
||||
PilotDefinition.Conf.InPlaceRotateKp,
|
||||
Ki =
|
||||
PilotDefinition.Conf.InPlaceRotateKi,
|
||||
Kd =
|
||||
PilotDefinition.Conf.InPlaceRotateKd,
|
||||
DeadZone =
|
||||
PilotDefinition.Conf
|
||||
.InPlaceRotateArriveDeg,
|
||||
SpeedAccPerSec =
|
||||
PilotDefinition.Conf.InPlaceRotateAcc,
|
||||
OutputUpperThreshold =
|
||||
MaxAngularSpeedDegreesPerSecond,
|
||||
MaxI =
|
||||
PilotDefinition.Conf.InPlaceRotateMaxI
|
||||
},
|
||||
CommandAngularSpeedObserver =
|
||||
commandAngularSpeed =>
|
||||
_recorder?.UpdateCommand(
|
||||
0f,
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
commandAngularSpeed))
|
||||
}.Get());
|
||||
|
||||
_task.Wait();
|
||||
|
||||
// 保留少量停止后的样本，用于观察角速度是否回到零。
|
||||
Thread.Sleep(300);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止原地旋转并保存当前已经采集的实验数据。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：原地自转90°")]
|
||||
public sealed class TestRotate90 :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestRotate90()
|
||||
: base(90f, "Rotate90")
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：原地自转180°")]
|
||||
public sealed class TestRotate180 :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestRotate180()
|
||||
: base(180f, "Rotate180")
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,103 @@
|
||||
|
||||
|
||||
using System;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
internal static class MovementTestPreparation
|
||||
{
|
||||
// 在测试正式开始前，将四个舵轮稳定回正到车体前向。
|
||||
public static bool AlignWheelsForward(
|
||||
ref DriveTask activeTask)
|
||||
{
|
||||
var preparation = new PrepareWheelsForward();
|
||||
var task = new DriveTask(preparation.Get());
|
||||
activeTask = task;
|
||||
|
||||
try
|
||||
{
|
||||
task.Wait();
|
||||
return preparation.Completed;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"测试前舵轮回正失败：{ex.Message}");
|
||||
return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task.Stop();
|
||||
if (ReferenceEquals(activeTask, task))
|
||||
activeTask = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
|
||||
public static bool AreWheelsForward(
|
||||
float toleranceDegrees = 2f)
|
||||
{
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
var toleranceRadians =
|
||||
AngleMath.DegreesToRadians(toleranceDegrees);
|
||||
|
||||
if (adapter.AreParallelWheelsAligned(
|
||||
0.0,
|
||||
toleranceRadians))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
"四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
|
||||
return false;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"检查舵轮方向失败：{ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "准备：四个舵轮与车头方向一致")]
|
||||
public class AlignWheelsForwardTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
// 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
|
||||
public override void Test()
|
||||
{
|
||||
MovementTestPreparation.AlignWheelsForward(
|
||||
ref _task);
|
||||
}
|
||||
|
||||
// 停止正在执行的舵轮回正任务并清零底盘运动命令。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_task = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,629 +0,0 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using FundamentalLib;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
|
||||
public class PrepareWheelsForward : MovementDefinition
|
||||
{
|
||||
public float ToleranceDegrees = 2f;
|
||||
public float StableSeconds = 0.3f;
|
||||
public float TimeoutSeconds = 10f;
|
||||
public bool Completed { get; private set; }
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
|
||||
}
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
var toleranceRadians =
|
||||
AngleMath.DegreesToRadians(ToleranceDegrees);
|
||||
var startTime = DateTime.UtcNow;
|
||||
DateTime? alignedSince = null;
|
||||
|
||||
Completed = false;
|
||||
if (!adapter.PrepareParallelDirection(0.0))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"无法将所有舵轮下发到车体前向0°。");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var aligned =
|
||||
adapter.AreParallelWheelsAligned(
|
||||
0.0,
|
||||
toleranceRadians);
|
||||
|
||||
if (aligned)
|
||||
{
|
||||
if (!alignedSince.HasValue)
|
||||
alignedSince = DateTime.UtcNow;
|
||||
|
||||
if ((DateTime.UtcNow -
|
||||
alignedSince.Value).TotalSeconds >=
|
||||
StableSeconds)
|
||||
{
|
||||
Completed = true;
|
||||
yield break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
alignedSince = null;
|
||||
}
|
||||
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
throw new TimeoutException(
|
||||
$"舵轮回正超过{TimeoutSeconds:F1}s，" +
|
||||
"测试已经取消。");
|
||||
}
|
||||
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 只清零驱动速度，保留已经下发的0°舵角。
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#region 功能项
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Second <= 0)
|
||||
{
|
||||
yield return false;
|
||||
yield break;
|
||||
}
|
||||
|
||||
var endTime = DateTime.UtcNow.AddSeconds(Second);
|
||||
while (DateTime.UtcNow < endTime)
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
public class DriverAble : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
// C层单车硬件：请求全部驱动轮复位并恢复使能。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
|
||||
// 至少保留一个调度周期，确保M层能收到复位请求。
|
||||
yield return true;
|
||||
|
||||
while (!PilotDefinition.Self.WheelAbleState &&
|
||||
(DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
// C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var startTime = DateTime.UtcNow;
|
||||
var success = false;
|
||||
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
// 至少保持一个C层调度周期，确保M层能收到下使能请求。
|
||||
yield return true;
|
||||
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
while (!success &&
|
||||
(DateTime.UtcNow - startTime).TotalMilliseconds <
|
||||
timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
|
||||
success =
|
||||
!PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
if (!success)
|
||||
{
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 无论正常完成、超时、异常还是任务被停止，都撤销请求。
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
}
|
||||
if (success)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能完成，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能超时，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}，" +
|
||||
$"等待{timeoutMs}ms");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region 直线运动
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
// 本次轨迹的巡航速度上限，单位m/s。
|
||||
public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
DriveTask task = null;
|
||||
try
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController
|
||||
{
|
||||
BaseSpeed = MaxSpeed
|
||||
}.Get();
|
||||
// 要求路径末端速度下降到零。
|
||||
tracker.FinishSpeed = 0f;
|
||||
var linePath = new LineTrack(Src, Dst)
|
||||
{
|
||||
CarDirectionBias = CarDirectionBias,
|
||||
Speed = MaxSpeed
|
||||
};
|
||||
tracker.AddTrack(linePath);
|
||||
task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
yield return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
//直线行走基于轮里程
|
||||
// C层单车底盘：按照车轮里程行驶指定的相对距离。
|
||||
public class LineTracking : MovementDefinition
|
||||
{
|
||||
// 相对动作启动位置的行驶距离，单位mm。
|
||||
// 正数表示前进，负数表示后退。
|
||||
public float TargetDistance;
|
||||
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||||
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||||
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||||
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||||
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction;
|
||||
// 接近目标后是否保留速度，交给下一个动作接管。
|
||||
public bool EnableHandover;
|
||||
// 进入动作衔接的剩余距离，单位mm。
|
||||
public float HandoverDistance = 80f;
|
||||
// HandoverSpeed小于0时，使用MaxSpeed的此比例。
|
||||
public float HandoverSpeedRatio = 0.5f;
|
||||
// 大于等于0时，直接作为衔接速度，单位m/s。
|
||||
public float HandoverSpeed = -1f;
|
||||
public float MinHandoverSpeed = 0.05f;
|
||||
private PIDController _pid;
|
||||
// 读取当前单车直线行驶里程，单位mm。
|
||||
private static float ReadPosition()
|
||||
{
|
||||
return
|
||||
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
|
||||
}
|
||||
|
||||
// 根据动作启动位置和目标距离执行直线里程闭环。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(TargetDistance),
|
||||
"目标行驶距离必须是有限值。");
|
||||
}
|
||||
|
||||
if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(MaxSpeed),
|
||||
"最大速度必须是大于零的有限值。");
|
||||
}
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
// 每次启动动作时重新读取起始编码器位置。
|
||||
var startPosition = ReadPosition();
|
||||
// PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
|
||||
var targetPosition = startPosition + TargetDistance;
|
||||
_pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
|
||||
{
|
||||
SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
|
||||
};
|
||||
var handoverRequested = false;
|
||||
var keepHandoverSpeed = false;
|
||||
DLog.Log(
|
||||
$"直线里程动作：" +
|
||||
$"起点={startPosition:F1}mm，" +
|
||||
$"距离={TargetDistance:F1}mm，" +
|
||||
$"目标={targetPosition:F1}mm",
|
||||
"straight_line");
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var currentPosition = ReadPosition();
|
||||
var remainingDistance = targetPosition - currentPosition;
|
||||
// 接近目标后，保留一定速度交给后续动作。
|
||||
if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
|
||||
{
|
||||
var direction = Math.Sign(remainingDistance);
|
||||
if (direction == 0)
|
||||
{
|
||||
direction = Math.Sign(TargetDistance);
|
||||
}
|
||||
var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
|
||||
var maximumSpeed = Math.Abs(MaxSpeed);
|
||||
var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
|
||||
var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
|
||||
var handoverSpeed = limitedSpeed * direction;
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
|
||||
handoverRequested = true;
|
||||
// 保持一个调度周期，让速度命令实际生效。
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = _pid.GetResponse(targetPosition);
|
||||
chassis.SendXYThSpeed(speed, 0f, 0f);
|
||||
if (_pid.IsArrived())
|
||||
{
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (SrcId != -1 &&
|
||||
LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
// 只有正常完成动作衔接时才允许保留非零速度。
|
||||
keepHandoverSpeed = handoverRequested;
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 普通完成、人工停止或异常退出时都必须停车。
|
||||
if (!keepHandoverSpeed)
|
||||
{
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
}
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
//直线行走基于detour
|
||||
public class LineTracking_based_detour : MovementDefinition
|
||||
{
|
||||
public float LineDistance = 1000f;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
public Painter painter = UI.GetPainter("Line", false);
|
||||
// C层单车轨迹：执行早期版本的两点直线跟踪动作。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var curpose = DetourInterface.getCartLocation();
|
||||
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||
var headingRadians =
|
||||
AngleMath.DegreesToRadians(curpose.th);
|
||||
var dst = new Vector2(
|
||||
(float)(curpose.x +
|
||||
LineDistance * Math.Cos(headingRadians)),
|
||||
(float)(curpose.y +
|
||||
LineDistance * Math.Sin(headingRadians)));
|
||||
// var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||||
// (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||||
Console.WriteLine($"src:{src.X} {src.Y}");
|
||||
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||||
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||||
tracker.AddTrack(linePath);
|
||||
var _dt = new DriveTask(tracker.Track());
|
||||
_dt.Wait();
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region 旋转运动
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
/// 旋转目标角度
|
||||
/// </summary>
|
||||
public float AngleTarget;
|
||||
|
||||
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||||
|
||||
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
|
||||
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
|
||||
public Action<float> CommandAngularSpeedObserver;
|
||||
|
||||
// 自转前舵轮实际角度允许误差，单位deg。
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
|
||||
// 自转舵轮连续保持到位的时间，单位s。
|
||||
public float WheelAlignmentStableSeconds = 0.3f;
|
||||
|
||||
// 自转舵轮准备超时时间，单位s。
|
||||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||||
|
||||
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Chassis == null)
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
Chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
|
||||
try
|
||||
{
|
||||
var alignmentStarted = DateTime.Now;
|
||||
DateTime? alignedSince = null;
|
||||
while (true)
|
||||
{
|
||||
if (!adapter.PrepareSpin())
|
||||
throw new InvalidOperationException(
|
||||
"无法生成原地自转舵轮目标：" +
|
||||
adapter.LastFailureReason);
|
||||
|
||||
if (adapter.AreSpinWheelsAligned)
|
||||
{
|
||||
if (alignedSince == null)
|
||||
alignedSince = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - alignedSince.Value)
|
||||
.TotalSeconds >=
|
||||
WheelAlignmentStableSeconds)
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
alignedSince = null;
|
||||
}
|
||||
|
||||
if ((DateTime.Now - alignmentStarted)
|
||||
.TotalSeconds >
|
||||
WheelAlignmentTimeoutSeconds)
|
||||
throw new TimeoutException(
|
||||
"原地自转舵轮在限定时间内未稳定到位。");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
var targetAngle =
|
||||
(float)AngleMath.NormalizeDegrees(AngleTarget);
|
||||
var p = PidparamsRead();
|
||||
thPid = new PIDController(ThetaReader, p.Kp);
|
||||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
|
||||
p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||
var lastCommandTime = DateTime.Now;
|
||||
|
||||
while (true)
|
||||
{
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
CommandAngularSpeedObserver?.Invoke(s);
|
||||
var now = DateTime.Now;
|
||||
var interval = now - lastCommandTime;
|
||||
lastCommandTime = now;
|
||||
|
||||
// PID输出s为deg/s，Shared命令统一使用rad/s。
|
||||
// adapter.Send最终调用普通安全版SendXYThSpeed。
|
||||
var omegaRadiansPerSecond =
|
||||
(float)AngleMath.DegreesToRadians(s);
|
||||
if (!adapter.Send(
|
||||
new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
omegaRadiansPerSecond)),
|
||||
interval))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"安全XYTh原地旋转底盘解算失败：" +
|
||||
adapter.LastFailureReason);
|
||||
}
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
CommandAngularSpeedObserver?.Invoke(0f);
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region 夹臂运动
|
||||
public class ClampToTarget : MovementDefinition
|
||||
{
|
||||
public float LeftClampTarget;
|
||||
public float RightClampTarget;
|
||||
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
|
||||
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
|
||||
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
|
||||
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
|
||||
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
|
||||
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
|
||||
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||||
public float TimeoutSeconds = 30f;
|
||||
private PIDController leftpid, rightpid;
|
||||
|
||||
// C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
try
|
||||
{
|
||||
leftpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosLeftArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
rightpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosRightArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
var startTime = DateTime.UtcNow;
|
||||
while (true)
|
||||
{
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"夹臂运动超时({TimeoutSeconds:F1}s)，" +
|
||||
"停止左右夹臂。");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var leftspeed =
|
||||
leftpid.GetResponse(LeftClampTarget);
|
||||
var rightspeed =
|
||||
rightpid.GetResponse(RightClampTarget);
|
||||
Console.WriteLine(
|
||||
$"left arm speed:{leftspeed} " +
|
||||
$"right arm speed:{rightspeed}");
|
||||
|
||||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||
|
||||
var leftArrived = leftpid.IsArrived();
|
||||
var rightArrived = rightpid.IsArrived();
|
||||
if (leftArrived)
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
if (rightArrived)
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
|
||||
if (leftArrived && rightArrived)
|
||||
break;
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"left clamp to target:{LeftClampTarget} " +
|
||||
$"right clamp to target:{RightClampTarget}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Pilot;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class ClampToTarget : MovementDefinition
|
||||
{
|
||||
public float LeftClampTarget;
|
||||
public float RightClampTarget;
|
||||
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
|
||||
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
|
||||
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
|
||||
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
|
||||
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
|
||||
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
|
||||
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||||
public float TimeoutSeconds = 30f;
|
||||
private PIDController leftpid, rightpid;
|
||||
|
||||
// C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
try
|
||||
{
|
||||
leftpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosLeftArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
rightpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosRightArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
var startTime = DateTime.UtcNow;
|
||||
while (true)
|
||||
{
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"夹臂运动超时({TimeoutSeconds:F1}s)，" +
|
||||
"停止左右夹臂。");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var leftspeed =
|
||||
leftpid.GetResponse(LeftClampTarget);
|
||||
var rightspeed =
|
||||
rightpid.GetResponse(RightClampTarget);
|
||||
Console.WriteLine(
|
||||
$"left arm speed:{leftspeed} " +
|
||||
$"right arm speed:{rightspeed}");
|
||||
|
||||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||
|
||||
var leftArrived = leftpid.IsArrived();
|
||||
var rightArrived = rightpid.IsArrived();
|
||||
if (leftArrived)
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
if (rightArrived)
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
|
||||
if (leftArrived && rightArrived)
|
||||
break;
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"left clamp to target:{LeftClampTarget} " +
|
||||
$"right clamp to target:{RightClampTarget}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
+2
-2
@@ -794,8 +794,8 @@ namespace MultiWheelC
|
||||
Math.PI / 2.0 ||
|
||||
!IsFinite(
|
||||
MaximumVirtualSteeringRadians))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
"蟹行轨迹测试参数无效。");
|
||||
throw new ArgumentOutOfRangeException(
|
||||
"蟹行轨迹测试参数无效。");
|
||||
}
|
||||
|
||||
private static double Limit(
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using ClumsyCore.Pilot;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Second <= 0)
|
||||
{
|
||||
yield return false;
|
||||
yield break;
|
||||
}
|
||||
|
||||
var endTime = DateTime.UtcNow.AddSeconds(Second);
|
||||
while (DateTime.UtcNow < endTime)
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverAble : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
// C层单车硬件：请求全部驱动轮复位并恢复使能。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
|
||||
// 至少保留一个调度周期，确保M层能收到复位请求。
|
||||
yield return true;
|
||||
|
||||
while (!PilotDefinition.Self.WheelAbleState &&
|
||||
(DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
// C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var startTime = DateTime.UtcNow;
|
||||
var success = false;
|
||||
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
// 至少保持一个C层调度周期，确保M层能收到下使能请求。
|
||||
yield return true;
|
||||
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
while (!success &&
|
||||
(DateTime.UtcNow - startTime).TotalMilliseconds <
|
||||
timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
|
||||
success =
|
||||
!PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
if (!success)
|
||||
{
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 无论正常完成、超时、异常还是任务被停止，都撤销请求。
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
}
|
||||
if (success)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能完成，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能超时，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}，" +
|
||||
$"等待{timeoutMs}ms");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
// 本次轨迹的巡航速度上限，单位m/s。
|
||||
public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
DriveTask task = null;
|
||||
try
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController
|
||||
{
|
||||
BaseSpeed = MaxSpeed
|
||||
}.Get();
|
||||
// 要求路径末端速度下降到零。
|
||||
tracker.FinishSpeed = 0f;
|
||||
var linePath = new LineTrack(Src, Dst)
|
||||
{
|
||||
CarDirectionBias = CarDirectionBias,
|
||||
Speed = MaxSpeed
|
||||
};
|
||||
tracker.AddTrack(linePath);
|
||||
task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
yield return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,178 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层单车底盘：按照车轮里程行驶指定的相对距离。
|
||||
public class LineTracking : MovementDefinition
|
||||
{
|
||||
// 相对动作启动位置的行驶距离，单位mm。
|
||||
// 正数表示前进，负数表示后退。
|
||||
public float TargetDistance;
|
||||
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||||
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||||
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||||
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||||
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction;
|
||||
// 接近目标后是否保留速度，交给下一个动作接管。
|
||||
public bool EnableHandover;
|
||||
// 进入动作衔接的剩余距离，单位mm。
|
||||
public float HandoverDistance = 80f;
|
||||
// HandoverSpeed小于0时，使用MaxSpeed的此比例。
|
||||
public float HandoverSpeedRatio = 0.5f;
|
||||
// 大于等于0时，直接作为衔接速度，单位m/s。
|
||||
public float HandoverSpeed = -1f;
|
||||
public float MinHandoverSpeed = 0.05f;
|
||||
private PIDController _pid;
|
||||
// 读取当前单车直线行驶里程，单位mm。
|
||||
private static float ReadPosition()
|
||||
{
|
||||
return
|
||||
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
|
||||
}
|
||||
|
||||
// 根据动作启动位置和目标距离执行直线里程闭环。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(TargetDistance),
|
||||
"目标行驶距离必须是有限值。");
|
||||
}
|
||||
|
||||
if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(MaxSpeed),
|
||||
"最大速度必须是大于零的有限值。");
|
||||
}
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
// 每次启动动作时重新读取起始编码器位置。
|
||||
var startPosition = ReadPosition();
|
||||
// PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
|
||||
var targetPosition = startPosition + TargetDistance;
|
||||
_pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
|
||||
{
|
||||
SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
|
||||
};
|
||||
var handoverRequested = false;
|
||||
var keepHandoverSpeed = false;
|
||||
DLog.Log(
|
||||
$"直线里程动作：" +
|
||||
$"起点={startPosition:F1}mm，" +
|
||||
$"距离={TargetDistance:F1}mm，" +
|
||||
$"目标={targetPosition:F1}mm",
|
||||
"straight_line");
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var currentPosition = ReadPosition();
|
||||
var remainingDistance = targetPosition - currentPosition;
|
||||
// 接近目标后，保留一定速度交给后续动作。
|
||||
if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
|
||||
{
|
||||
var direction = Math.Sign(remainingDistance);
|
||||
if (direction == 0)
|
||||
{
|
||||
direction = Math.Sign(TargetDistance);
|
||||
}
|
||||
var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
|
||||
var maximumSpeed = Math.Abs(MaxSpeed);
|
||||
var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
|
||||
var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
|
||||
var handoverSpeed = limitedSpeed * direction;
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
|
||||
handoverRequested = true;
|
||||
// 保持一个调度周期，让速度命令实际生效。
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = _pid.GetResponse(targetPosition);
|
||||
chassis.SendXYThSpeed(speed, 0f, 0f);
|
||||
if (_pid.IsArrived())
|
||||
{
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (SrcId != -1 &&
|
||||
LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
// 只有正常完成动作衔接时才允许保留非零速度。
|
||||
keepHandoverSpeed = handoverRequested;
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 普通完成、人工停止或异常退出时都必须停车。
|
||||
if (!keepHandoverSpeed)
|
||||
{
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
}
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
//直线行走基于detour
|
||||
public class LineTracking_based_detour : MovementDefinition
|
||||
{
|
||||
public float LineDistance = 1000f;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
public Painter painter = UI.GetPainter("Line", false);
|
||||
// C层单车轨迹：执行早期版本的两点直线跟踪动作。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var curpose = DetourInterface.getCartLocation();
|
||||
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||
var headingRadians =
|
||||
AngleMath.DegreesToRadians(curpose.th);
|
||||
var dst = new Vector2(
|
||||
(float)(curpose.x +
|
||||
LineDistance * Math.Cos(headingRadians)),
|
||||
(float)(curpose.y +
|
||||
LineDistance * Math.Sin(headingRadians)));
|
||||
// var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||||
// (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||||
Console.WriteLine($"src:{src.X} {src.Y}");
|
||||
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||||
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||||
tracker.AddTrack(linePath);
|
||||
var _dt = new DriveTask(tracker.Track());
|
||||
_dt.Wait();
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
|
||||
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
|
||||
public class PrepareWheelsForward : MovementDefinition
|
||||
{
|
||||
public float ToleranceDegrees = 2f;
|
||||
public float StableSeconds = 0.3f;
|
||||
public float TimeoutSeconds = 10f;
|
||||
public bool Completed { get; private set; }
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
|
||||
}
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
var toleranceRadians =
|
||||
AngleMath.DegreesToRadians(ToleranceDegrees);
|
||||
var startTime = DateTime.UtcNow;
|
||||
DateTime? alignedSince = null;
|
||||
|
||||
Completed = false;
|
||||
if (!adapter.PrepareParallelDirection(0.0))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"无法将所有舵轮下发到车体前向0°。");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var aligned =
|
||||
adapter.AreParallelWheelsAligned(
|
||||
0.0,
|
||||
toleranceRadians);
|
||||
|
||||
if (aligned)
|
||||
{
|
||||
if (!alignedSince.HasValue)
|
||||
alignedSince = DateTime.UtcNow;
|
||||
|
||||
if ((DateTime.UtcNow -
|
||||
alignedSince.Value).TotalSeconds >=
|
||||
StableSeconds)
|
||||
{
|
||||
Completed = true;
|
||||
yield break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
alignedSince = null;
|
||||
}
|
||||
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
throw new TimeoutException(
|
||||
$"舵轮回正超过{TimeoutSeconds:F1}s，" +
|
||||
"测试已经取消。");
|
||||
}
|
||||
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 只清零驱动速度，保留已经下发的0°舵角。
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
/// 旋转目标角度
|
||||
/// </summary>
|
||||
public float AngleTarget;
|
||||
|
||||
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||||
|
||||
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
|
||||
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
|
||||
public Action<float> CommandAngularSpeedObserver;
|
||||
|
||||
// 自转前舵轮实际角度允许误差，单位deg。
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
|
||||
// 自转舵轮连续保持到位的时间，单位s。
|
||||
public float WheelAlignmentStableSeconds = 0.3f;
|
||||
|
||||
// 自转舵轮准备超时时间，单位s。
|
||||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||||
|
||||
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Chassis == null)
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
Chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
|
||||
try
|
||||
{
|
||||
var alignmentStarted = DateTime.Now;
|
||||
DateTime? alignedSince = null;
|
||||
while (true)
|
||||
{
|
||||
if (!adapter.PrepareSpin())
|
||||
throw new InvalidOperationException(
|
||||
"无法生成原地自转舵轮目标：" +
|
||||
adapter.LastFailureReason);
|
||||
|
||||
if (adapter.AreSpinWheelsAligned)
|
||||
{
|
||||
if (alignedSince == null)
|
||||
alignedSince = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - alignedSince.Value)
|
||||
.TotalSeconds >=
|
||||
WheelAlignmentStableSeconds)
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
alignedSince = null;
|
||||
}
|
||||
|
||||
if ((DateTime.Now - alignmentStarted)
|
||||
.TotalSeconds >
|
||||
WheelAlignmentTimeoutSeconds)
|
||||
throw new TimeoutException(
|
||||
"原地自转舵轮在限定时间内未稳定到位。");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
var targetAngle =
|
||||
(float)AngleMath.NormalizeDegrees(AngleTarget);
|
||||
var p = PidparamsRead();
|
||||
thPid = new PIDController(ThetaReader, p.Kp);
|
||||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
|
||||
p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||
var lastCommandTime = DateTime.Now;
|
||||
|
||||
while (true)
|
||||
{
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
CommandAngularSpeedObserver?.Invoke(s);
|
||||
var now = DateTime.Now;
|
||||
var interval = now - lastCommandTime;
|
||||
lastCommandTime = now;
|
||||
|
||||
// PID输出s为deg/s，Shared命令统一使用rad/s。
|
||||
// adapter.Send最终调用普通安全版SendXYThSpeed。
|
||||
var omegaRadiansPerSecond =
|
||||
(float)AngleMath.DegreesToRadians(s);
|
||||
if (!adapter.Send(
|
||||
new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
omegaRadiansPerSecond)),
|
||||
interval))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"安全XYTh原地旋转底盘解算失败：" +
|
||||
adapter.LastFailureReason);
|
||||
}
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
CommandAngularSpeedObserver?.Invoke(0f);
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,509 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using ClumsyCore.Interfaces;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 读取Detour位姿，忽略重复或明显异常的观测，并估算车辆二维速度。
|
||||
/// </summary>
|
||||
public sealed class DetourVehicleStateProvider
|
||||
: IVehicleStateProvider
|
||||
{
|
||||
public const double DefaultMaximumLinearSpeedMetersPerSecond =
|
||||
1.20;
|
||||
public const double DefaultMaximumAngularSpeedRadiansPerSecond =
|
||||
Math.PI / 4.0;
|
||||
public const double DefaultPositionJumpMarginMeters =
|
||||
0.03;
|
||||
public const double DefaultHeadingJumpMarginRadians =
|
||||
5.0 * Math.PI / 180.0;
|
||||
public const double DefaultVelocityPositionResidualMeters =
|
||||
0.04;
|
||||
public const double DefaultVelocityHeadingResidualRadians =
|
||||
5.0 * Math.PI / 180.0;
|
||||
public const double DefaultStationaryConfirmationSeconds =
|
||||
0.35;
|
||||
|
||||
private const double MillimetersPerMeter = 1000.0;
|
||||
private const double PositionEqualityToleranceMeters = 1e-9;
|
||||
private const double HeadingEqualityToleranceRadians = 1e-8;
|
||||
|
||||
private readonly object _syncRoot = new object();
|
||||
private readonly Stopwatch _clock = Stopwatch.StartNew();
|
||||
private readonly VelocityEstimator2D _velocityEstimator;
|
||||
private readonly double _maximumLinearSpeedMetersPerSecond;
|
||||
private readonly double _maximumAngularSpeedRadiansPerSecond;
|
||||
private readonly double _positionJumpMarginMeters;
|
||||
private readonly double _headingJumpMarginRadians;
|
||||
private readonly double _velocityPositionResidualMeters;
|
||||
private readonly double _velocityHeadingResidualRadians;
|
||||
private readonly double _stationaryConfirmationSeconds;
|
||||
|
||||
private bool _hasAcceptedPose;
|
||||
private Pose2D _acceptedPoseInWorld;
|
||||
private double _acceptedTimestampSeconds;
|
||||
private VehicleState _latestState;
|
||||
private bool _stationaryHoldActive;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用停车机器人默认物理边界和速度滤波参数的Detour状态源。
|
||||
/// </summary>
|
||||
public DetourVehicleStateProvider()
|
||||
: this(
|
||||
new VelocityEstimator2D(),
|
||||
DefaultMaximumLinearSpeedMetersPerSecond,
|
||||
DefaultMaximumAngularSpeedRadiansPerSecond,
|
||||
DefaultPositionJumpMarginMeters,
|
||||
DefaultHeadingJumpMarginRadians,
|
||||
DefaultVelocityPositionResidualMeters,
|
||||
DefaultVelocityHeadingResidualRadians,
|
||||
DefaultStationaryConfirmationSeconds)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定物理边界、静止确认时间和速度估计器的Detour状态源。
|
||||
/// </summary>
|
||||
public DetourVehicleStateProvider(
|
||||
VelocityEstimator2D velocityEstimator,
|
||||
double maximumLinearSpeedMetersPerSecond,
|
||||
double maximumAngularSpeedRadiansPerSecond,
|
||||
double positionJumpMarginMeters,
|
||||
double headingJumpMarginRadians,
|
||||
double velocityPositionResidualMeters,
|
||||
double velocityHeadingResidualRadians,
|
||||
double stationaryConfirmationSeconds)
|
||||
{
|
||||
_velocityEstimator = velocityEstimator ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(velocityEstimator));
|
||||
|
||||
EnsureFinitePositive(
|
||||
maximumLinearSpeedMetersPerSecond,
|
||||
nameof(maximumLinearSpeedMetersPerSecond));
|
||||
EnsureFinitePositive(
|
||||
maximumAngularSpeedRadiansPerSecond,
|
||||
nameof(maximumAngularSpeedRadiansPerSecond));
|
||||
EnsureFiniteNonNegative(
|
||||
positionJumpMarginMeters,
|
||||
nameof(positionJumpMarginMeters));
|
||||
EnsureFiniteNonNegative(
|
||||
headingJumpMarginRadians,
|
||||
nameof(headingJumpMarginRadians));
|
||||
EnsureFinitePositive(
|
||||
velocityPositionResidualMeters,
|
||||
nameof(velocityPositionResidualMeters));
|
||||
EnsureFinitePositive(
|
||||
velocityHeadingResidualRadians,
|
||||
nameof(velocityHeadingResidualRadians));
|
||||
EnsureFinitePositive(
|
||||
stationaryConfirmationSeconds,
|
||||
nameof(stationaryConfirmationSeconds));
|
||||
|
||||
_maximumLinearSpeedMetersPerSecond =
|
||||
maximumLinearSpeedMetersPerSecond;
|
||||
_maximumAngularSpeedRadiansPerSecond =
|
||||
maximumAngularSpeedRadiansPerSecond;
|
||||
_positionJumpMarginMeters =
|
||||
positionJumpMarginMeters;
|
||||
_headingJumpMarginRadians =
|
||||
headingJumpMarginRadians;
|
||||
_velocityPositionResidualMeters =
|
||||
velocityPositionResidualMeters;
|
||||
_velocityHeadingResidualRadians =
|
||||
velocityHeadingResidualRadians;
|
||||
_stationaryConfirmationSeconds =
|
||||
stationaryConfirmationSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次读取失败或异常观测被忽略的原因，正常时为空字符串。
|
||||
/// </summary>
|
||||
public string LastFailureReason { get; private set; } = "";
|
||||
|
||||
/// <summary>
|
||||
/// 尝试读取Detour；重复帧保留最近状态，明显异常帧只忽略本次观测。
|
||||
/// </summary>
|
||||
public bool TryGetState(out VehicleState state)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
try
|
||||
{
|
||||
var poseInWorld =
|
||||
ReadDetourPoseInWorld();
|
||||
var timestampSeconds =
|
||||
_clock.Elapsed.TotalSeconds;
|
||||
|
||||
if (!_hasAcceptedPose)
|
||||
{
|
||||
state = AcceptPoseAfterReset(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
LastFailureReason = "";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (ArePosesEquivalent(
|
||||
poseInWorld,
|
||||
_acceptedPoseInWorld))
|
||||
{
|
||||
state = HandleRepeatedPose(
|
||||
timestampSeconds);
|
||||
LastFailureReason = "";
|
||||
return true;
|
||||
}
|
||||
|
||||
// 静止保持后出现新定位时重新建立差分基准，
|
||||
// 避免用很长的静止时间稀释第一次运动速度。
|
||||
if (_stationaryHoldActive)
|
||||
{
|
||||
state = AcceptPoseAfterReset(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
LastFailureReason = "";
|
||||
return true;
|
||||
}
|
||||
|
||||
var elapsedSeconds =
|
||||
timestampSeconds -
|
||||
_acceptedTimestampSeconds;
|
||||
|
||||
if (!IsMotionPlausible(
|
||||
_acceptedPoseInWorld,
|
||||
poseInWorld,
|
||||
elapsedSeconds))
|
||||
{
|
||||
// 单帧异常不进入差分器，也不中断调用方；下一次
|
||||
// 正常观测仍相对最近有效位姿和真实时间差计算。
|
||||
state = _latestState;
|
||||
LastFailureReason =
|
||||
"Detour位姿变化超过车辆绝对运动边界，本次观测已忽略。";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (IsVelocityInnovationAbnormal(
|
||||
poseInWorld,
|
||||
elapsedSeconds))
|
||||
{
|
||||
state = AcceptPoseAfterVelocityRebase(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
LastFailureReason =
|
||||
"Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。";
|
||||
return true;
|
||||
}
|
||||
|
||||
state = AcceptContinuousPose(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
LastFailureReason = "";
|
||||
return true;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour车辆状态读取失败：" +
|
||||
exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除Detour位姿历史和速度估计状态。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
_velocityEstimator.Reset();
|
||||
_hasAcceptedPose = false;
|
||||
_acceptedPoseInWorld = Pose2D.Identity;
|
||||
_acceptedTimestampSeconds = 0.0;
|
||||
_latestState = default;
|
||||
_stationaryHoldActive = false;
|
||||
LastFailureReason = "";
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取Detour毫米和角度数据并转换为世界坐标SI位姿。
|
||||
/// </summary>
|
||||
private static Pose2D ReadDetourPoseInWorld()
|
||||
{
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
|
||||
EnsureFinite(location.x, "DetourX");
|
||||
EnsureFinite(location.y, "DetourY");
|
||||
EnsureFinite(location.th, "DetourTheta");
|
||||
|
||||
return new Pose2D(
|
||||
location.x / MillimetersPerMeter,
|
||||
location.y / MillimetersPerMeter,
|
||||
AngleMath.NormalizeRadians(
|
||||
AngleMath.DegreesToRadians(
|
||||
location.th)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 接受连续有效定位并更新速度估计和差分基准。
|
||||
/// </summary>
|
||||
private VehicleState AcceptContinuousPose(
|
||||
Pose2D poseInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
_latestState =
|
||||
_velocityEstimator.Update(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
_acceptedPoseInWorld = poseInWorld;
|
||||
_acceptedTimestampSeconds =
|
||||
timestampSeconds;
|
||||
_stationaryHoldActive = false;
|
||||
return _latestState;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。
|
||||
/// </summary>
|
||||
private VehicleState AcceptPoseAfterVelocityRebase(
|
||||
Pose2D poseInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
_latestState =
|
||||
_velocityEstimator
|
||||
.RebasePreservingVelocity(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
_acceptedPoseInWorld = poseInWorld;
|
||||
_acceptedTimestampSeconds =
|
||||
timestampSeconds;
|
||||
_stationaryHoldActive = false;
|
||||
return _latestState;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
|
||||
/// </summary>
|
||||
private VehicleState AcceptPoseAfterReset(
|
||||
Pose2D poseInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
_latestState =
|
||||
_velocityEstimator.Reset(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
_acceptedPoseInWorld = poseInWorld;
|
||||
_acceptedTimestampSeconds =
|
||||
timestampSeconds;
|
||||
_hasAcceptedPose = true;
|
||||
_stationaryHoldActive = false;
|
||||
return _latestState;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对重复Detour观测保留最近状态，并在持续不变后将速度归零。
|
||||
/// </summary>
|
||||
private VehicleState HandleRepeatedPose(
|
||||
double timestampSeconds)
|
||||
{
|
||||
var unchangedSeconds =
|
||||
timestampSeconds -
|
||||
_acceptedTimestampSeconds;
|
||||
|
||||
if (!_stationaryHoldActive &&
|
||||
unchangedSeconds >=
|
||||
_stationaryConfirmationSeconds)
|
||||
{
|
||||
_latestState =
|
||||
new VehicleState(
|
||||
timestampSeconds,
|
||||
_acceptedPoseInWorld,
|
||||
Twist2D.Zero,
|
||||
true);
|
||||
_stationaryHoldActive = true;
|
||||
}
|
||||
|
||||
return _latestState;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两次有效Detour观测之间的变化是否超过车辆绝对运动能力。
|
||||
/// </summary>
|
||||
private bool IsMotionPlausible(
|
||||
Pose2D startPoseInWorld,
|
||||
Pose2D endPoseInWorld,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
if (!IsFinite(deltaTimeSeconds) ||
|
||||
deltaTimeSeconds <= 0.0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var deltaX =
|
||||
endPoseInWorld.XMeters -
|
||||
startPoseInWorld.XMeters;
|
||||
var deltaY =
|
||||
endPoseInWorld.YMeters -
|
||||
startPoseInWorld.YMeters;
|
||||
var displacementMeters =
|
||||
Math.Sqrt(
|
||||
deltaX * deltaX +
|
||||
deltaY * deltaY);
|
||||
var headingChangeRadians =
|
||||
Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
endPoseInWorld.YawRadians,
|
||||
startPoseInWorld.YawRadians));
|
||||
|
||||
var maximumDisplacementMeters =
|
||||
_maximumLinearSpeedMetersPerSecond *
|
||||
deltaTimeSeconds +
|
||||
_positionJumpMarginMeters;
|
||||
var maximumHeadingChangeRadians =
|
||||
_maximumAngularSpeedRadiansPerSecond *
|
||||
deltaTimeSeconds +
|
||||
_headingJumpMarginRadians;
|
||||
|
||||
return displacementMeters <=
|
||||
maximumDisplacementMeters &&
|
||||
headingChangeRadians <=
|
||||
maximumHeadingChangeRadians;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断新位姿是否明显偏离上一滤波速度给出的恒速预测。
|
||||
/// </summary>
|
||||
private bool IsVelocityInnovationAbnormal(
|
||||
Pose2D poseInWorld,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
if (!_latestState.HasValidVelocityEstimate)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var predictedX =
|
||||
_acceptedPoseInWorld.XMeters +
|
||||
_latestState.TwistInWorld
|
||||
.VxMetersPerSecond *
|
||||
deltaTimeSeconds;
|
||||
var predictedY =
|
||||
_acceptedPoseInWorld.YMeters +
|
||||
_latestState.TwistInWorld
|
||||
.VyMetersPerSecond *
|
||||
deltaTimeSeconds;
|
||||
var predictedYaw =
|
||||
AngleMath.NormalizeRadians(
|
||||
_acceptedPoseInWorld.YawRadians +
|
||||
_latestState.TwistInWorld
|
||||
.OmegaRadiansPerSecond *
|
||||
deltaTimeSeconds);
|
||||
|
||||
var positionResidualX =
|
||||
poseInWorld.XMeters - predictedX;
|
||||
var positionResidualY =
|
||||
poseInWorld.YMeters - predictedY;
|
||||
var positionResidualMeters =
|
||||
Math.Sqrt(
|
||||
positionResidualX * positionResidualX +
|
||||
positionResidualY * positionResidualY);
|
||||
var headingResidualRadians =
|
||||
Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
poseInWorld.YawRadians,
|
||||
predictedYaw));
|
||||
|
||||
return positionResidualMeters >
|
||||
_velocityPositionResidualMeters ||
|
||||
headingResidualRadians >
|
||||
_velocityHeadingResidualRadians;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两次读取是否为Detour保持输出的同一数值帧。
|
||||
/// </summary>
|
||||
private static bool ArePosesEquivalent(
|
||||
Pose2D firstPose,
|
||||
Pose2D secondPose)
|
||||
{
|
||||
return Math.Abs(
|
||||
firstPose.XMeters -
|
||||
secondPose.XMeters) <=
|
||||
PositionEqualityToleranceMeters &&
|
||||
Math.Abs(
|
||||
firstPose.YMeters -
|
||||
secondPose.YMeters) <=
|
||||
PositionEqualityToleranceMeters &&
|
||||
Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
firstPose.YawRadians,
|
||||
secondPose.YawRadians)) <=
|
||||
HeadingEqualityToleranceRadians;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为正有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePositive(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数必须是正有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数和Detour位姿必须是有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于状态估计。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 使用真实采样时间间隔对单个连续量执行在线一阶低通滤波。
|
||||
/// </summary>
|
||||
public sealed class FirstOrderLowPassFilter
|
||||
{
|
||||
private readonly double _timeConstantSeconds;
|
||||
private bool _isInitialized;
|
||||
private double _value;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定时间常数的一阶低通滤波器。
|
||||
/// </summary>
|
||||
public FirstOrderLowPassFilter(
|
||||
double timeConstantSeconds)
|
||||
{
|
||||
EnsureFinitePositive(
|
||||
timeConstantSeconds,
|
||||
nameof(timeConstantSeconds));
|
||||
|
||||
_timeConstantSeconds =
|
||||
timeConstantSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
|
||||
/// </summary>
|
||||
public double TimeConstantSeconds =>
|
||||
_timeConstantSeconds;
|
||||
|
||||
/// <summary>
|
||||
/// 获取滤波器是否已经接收过有效初值。
|
||||
/// </summary>
|
||||
public bool IsInitialized =>
|
||||
_isInitialized;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前滤波输出；尚未初始化时读取会抛出异常。
|
||||
/// </summary>
|
||||
public double Value
|
||||
{
|
||||
get
|
||||
{
|
||||
if (!_isInitialized)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"一阶低通滤波器尚未初始化。");
|
||||
}
|
||||
|
||||
return _value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用当前输入和真实采样间隔更新滤波结果。
|
||||
/// </summary>
|
||||
public double Update(
|
||||
double input,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
EnsureFinite(
|
||||
input,
|
||||
nameof(input));
|
||||
EnsureFinitePositive(
|
||||
deltaTimeSeconds,
|
||||
nameof(deltaTimeSeconds));
|
||||
|
||||
if (!_isInitialized)
|
||||
{
|
||||
_value = input;
|
||||
_isInitialized = true;
|
||||
return _value;
|
||||
}
|
||||
|
||||
var alpha =
|
||||
deltaTimeSeconds /
|
||||
(_timeConstantSeconds +
|
||||
deltaTimeSeconds);
|
||||
|
||||
_value += alpha * (input - _value);
|
||||
return _value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除历史输出，使下一次有效输入直接成为新的初值。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
_value = 0.0;
|
||||
_isInitialized = false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将滤波器立即重置到指定的有限初值。
|
||||
/// </summary>
|
||||
public void Reset(double initialValue)
|
||||
{
|
||||
EnsureFinite(
|
||||
initialValue,
|
||||
nameof(initialValue));
|
||||
|
||||
_value = initialValue;
|
||||
_isInitialized = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为正有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePositive(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"滤波时间常数和采样间隔必须是正有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"滤波输入必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 为轨迹控制器提供与具体定位来源无关的统一车辆状态读取接口。
|
||||
/// </summary>
|
||||
public interface IVehicleStateProvider
|
||||
{
|
||||
/// <summary>
|
||||
/// 尝试读取当前有效车辆状态；定位不可用或过期时返回false。
|
||||
/// </summary>
|
||||
bool TryGetState(out VehicleState state);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,138 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。
|
||||
/// </summary>
|
||||
public readonly struct VehicleState
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。
|
||||
/// </summary>
|
||||
public VehicleState(
|
||||
double sampleTimestampSeconds,
|
||||
Pose2D poseInWorld,
|
||||
Twist2D twistInWorld,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteTwist(
|
||||
twistInWorld,
|
||||
nameof(twistInWorld));
|
||||
|
||||
SampleTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
HasValidVelocityEstimate =
|
||||
hasValidVelocityEstimate;
|
||||
|
||||
// 第一帧或定位重置后的速度不可用于闭环控制，
|
||||
// 此时显式置零，避免调用方误用残留速度。
|
||||
TwistInWorld = hasValidVelocityEstimate
|
||||
? twistInWorld
|
||||
: Twist2D.Zero;
|
||||
|
||||
var worldPoseInBody =
|
||||
FrameTransform2D.Inverse(
|
||||
PoseInWorld);
|
||||
TwistInBody =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
worldPoseInBody,
|
||||
TwistInWorld);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态源单调时钟中的采样时刻，单位为s。
|
||||
/// </summary>
|
||||
public double SampleTimestampSeconds { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
|
||||
/// </summary>
|
||||
public Pose2D PoseInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。
|
||||
/// </summary>
|
||||
public Twist2D TwistInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。
|
||||
/// </summary>
|
||||
public Twist2D TwistInBody { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
|
||||
/// </summary>
|
||||
public bool HasValidVelocityEstimate { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 检查位姿是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(pose.XMeters) ||
|
||||
!IsFinite(pose.YMeters) ||
|
||||
!IsFinite(pose.YawRadians))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"车辆位姿必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查速度是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteTwist(
|
||||
Twist2D twist,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(twist.VxMetersPerSecond) ||
|
||||
!IsFinite(twist.VyMetersPerSecond) ||
|
||||
!IsFinite(twist.OmegaRadiansPerSecond))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"车辆速度必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"采样时刻必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于车辆状态计算。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,275 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。
|
||||
/// </summary>
|
||||
public sealed class VelocityEstimator2D
|
||||
{
|
||||
public const double DefaultLinearFilterTimeConstantSeconds =
|
||||
0.15;
|
||||
public const double DefaultAngularFilterTimeConstantSeconds =
|
||||
0.20;
|
||||
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_worldVelocityXFilter;
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_worldVelocityYFilter;
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_angularVelocityFilter;
|
||||
|
||||
private bool _hasPreviousSample;
|
||||
private Pose2D _previousPoseInWorld;
|
||||
private double _previousTimestampSeconds;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。
|
||||
/// </summary>
|
||||
public VelocityEstimator2D()
|
||||
: this(
|
||||
DefaultLinearFilterTimeConstantSeconds,
|
||||
DefaultAngularFilterTimeConstantSeconds)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定线速度和角速度滤波时间常数的估计器。
|
||||
/// </summary>
|
||||
public VelocityEstimator2D(
|
||||
double linearFilterTimeConstantSeconds,
|
||||
double angularFilterTimeConstantSeconds)
|
||||
{
|
||||
_worldVelocityXFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
linearFilterTimeConstantSeconds);
|
||||
_worldVelocityYFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
linearFilterTimeConstantSeconds);
|
||||
_angularVelocityFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
angularFilterTimeConstantSeconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否已经保存了可用于下一次差分的位姿基准。
|
||||
/// </summary>
|
||||
public bool HasPreviousSample =>
|
||||
_hasPreviousSample;
|
||||
|
||||
/// <summary>
|
||||
/// 使用一个新的有效定位样本更新并返回车辆状态。
|
||||
/// </summary>
|
||||
public VehicleState Update(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
var normalizedPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
|
||||
if (!_hasPreviousSample)
|
||||
{
|
||||
return Reset(
|
||||
normalizedPoseInWorld,
|
||||
sampleTimestampSeconds);
|
||||
}
|
||||
|
||||
var deltaTimeSeconds =
|
||||
sampleTimestampSeconds -
|
||||
_previousTimestampSeconds;
|
||||
|
||||
if (deltaTimeSeconds <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(sampleTimestampSeconds),
|
||||
"新定位样本的单调时间戳必须严格大于上一帧。");
|
||||
}
|
||||
|
||||
var rawVelocityXInWorld =
|
||||
(normalizedPoseInWorld.XMeters -
|
||||
_previousPoseInWorld.XMeters) /
|
||||
deltaTimeSeconds;
|
||||
var rawVelocityYInWorld =
|
||||
(normalizedPoseInWorld.YMeters -
|
||||
_previousPoseInWorld.YMeters) /
|
||||
deltaTimeSeconds;
|
||||
var rawAngularVelocity =
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
normalizedPoseInWorld.YawRadians,
|
||||
_previousPoseInWorld.YawRadians) /
|
||||
deltaTimeSeconds;
|
||||
|
||||
var filteredTwistInWorld =
|
||||
new Twist2D(
|
||||
_worldVelocityXFilter.Update(
|
||||
rawVelocityXInWorld,
|
||||
deltaTimeSeconds),
|
||||
_worldVelocityYFilter.Update(
|
||||
rawVelocityYInWorld,
|
||||
deltaTimeSeconds),
|
||||
_angularVelocityFilter.Update(
|
||||
rawAngularVelocity,
|
||||
deltaTimeSeconds));
|
||||
|
||||
_previousPoseInWorld =
|
||||
normalizedPoseInWorld;
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
normalizedPoseInWorld,
|
||||
filteredTwistInWorld,
|
||||
true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
|
||||
/// </summary>
|
||||
public VehicleState RebasePreservingVelocity(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
var normalizedPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
|
||||
_previousPoseInWorld =
|
||||
normalizedPoseInWorld;
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
_hasPreviousSample = true;
|
||||
|
||||
var hasValidVelocityEstimate =
|
||||
_worldVelocityXFilter.IsInitialized &&
|
||||
_worldVelocityYFilter.IsInitialized &&
|
||||
_angularVelocityFilter.IsInitialized;
|
||||
|
||||
var retainedTwistInWorld =
|
||||
hasValidVelocityEstimate
|
||||
? new Twist2D(
|
||||
_worldVelocityXFilter.Value,
|
||||
_worldVelocityYFilter.Value,
|
||||
_angularVelocityFilter.Value)
|
||||
: Twist2D.Zero;
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
normalizedPoseInWorld,
|
||||
retainedTwistInWorld,
|
||||
hasValidVelocityEstimate);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
|
||||
/// </summary>
|
||||
public VehicleState Reset(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
_previousPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
_hasPreviousSample = true;
|
||||
|
||||
_worldVelocityXFilter.Reset();
|
||||
_worldVelocityYFilter.Reset();
|
||||
_angularVelocityFilter.Reset();
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
_previousPoseInWorld,
|
||||
Twist2D.Zero,
|
||||
false);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
_hasPreviousSample = false;
|
||||
_previousPoseInWorld = Pose2D.Identity;
|
||||
_previousTimestampSeconds = 0.0;
|
||||
|
||||
_worldVelocityXFilter.Reset();
|
||||
_worldVelocityYFilter.Reset();
|
||||
_angularVelocityFilter.Reset();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查位姿是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(pose.XMeters) ||
|
||||
!IsFinite(pose.YMeters) ||
|
||||
!IsFinite(pose.YawRadians))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"速度估计使用的车辆位姿必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"速度估计使用的采样时刻必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于速度估计。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
// 兼容现有 MDCS 的 AbstractTrack
|
||||
@@ -0,0 +1,172 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Linq;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存一条经过基本合法性检查的只读二维参考轨迹。
|
||||
/// </summary>
|
||||
public sealed class Trajectory2D
|
||||
{
|
||||
private const double StartArcLengthToleranceMeters = 1e-9;
|
||||
private const double MinimumSegmentLengthMeters = 1e-6;
|
||||
|
||||
private readonly TrajectoryPoint[] _points;
|
||||
private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;
|
||||
|
||||
/// <summary>
|
||||
/// 复制并验证按累计弧长升序排列的参考轨迹点。
|
||||
/// </summary>
|
||||
public Trajectory2D(
|
||||
IEnumerable<TrajectoryPoint> points)
|
||||
{
|
||||
if (points == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
_points = points.ToArray();
|
||||
|
||||
if (_points.Length < 2)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"二维轨迹至少需要两个轨迹点。",
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
if (Math.Abs(_points[0].ArcLengthMeters) >
|
||||
StartArcLengthToleranceMeters)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"二维轨迹起点的累计弧长必须为0m。",
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
for (var index = 1;
|
||||
index < _points.Length;
|
||||
index++)
|
||||
{
|
||||
ValidateSegment(
|
||||
_points[index - 1],
|
||||
_points[index],
|
||||
index,
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
_readOnlyPoints =
|
||||
Array.AsReadOnly(_points);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹点数量。
|
||||
/// </summary>
|
||||
public int Count => _points.Length;
|
||||
|
||||
/// <summary>
|
||||
/// 获取指定索引处的轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint this[int index] =>
|
||||
_points[index];
|
||||
|
||||
/// <summary>
|
||||
/// 获取不可修改的有序轨迹点集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<TrajectoryPoint> Points =>
|
||||
_readOnlyPoints;
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹起点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint StartPoint =>
|
||||
_points[0];
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹终点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint EndPoint =>
|
||||
_points[_points.Length - 1];
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹总弧长，单位为m。
|
||||
/// </summary>
|
||||
public double TotalLengthMeters =>
|
||||
EndPoint.ArcLengthMeters;
|
||||
|
||||
/// <summary>
|
||||
/// 根据当前累计弧长计算到轨迹终点的剩余距离。
|
||||
/// </summary>
|
||||
public double GetRemainingDistanceMeters(
|
||||
double arcLengthMeters)
|
||||
{
|
||||
EnsureFinite(
|
||||
arcLengthMeters,
|
||||
nameof(arcLengthMeters));
|
||||
|
||||
if (arcLengthMeters <= 0.0)
|
||||
return TotalLengthMeters;
|
||||
|
||||
if (arcLengthMeters >= TotalLengthMeters)
|
||||
return 0.0;
|
||||
|
||||
return TotalLengthMeters - arcLengthMeters;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查相邻轨迹点是否构成有效的非零长度有序线段。
|
||||
/// </summary>
|
||||
private static void ValidateSegment(
|
||||
TrajectoryPoint previous,
|
||||
TrajectoryPoint current,
|
||||
int currentIndex,
|
||||
string parameterName)
|
||||
{
|
||||
if (current.ArcLengthMeters <=
|
||||
previous.ArcLengthMeters)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。",
|
||||
parameterName);
|
||||
}
|
||||
|
||||
var deltaX =
|
||||
current.PoseInWorld.XMeters -
|
||||
previous.PoseInWorld.XMeters;
|
||||
var deltaY =
|
||||
current.PoseInWorld.YMeters -
|
||||
previous.PoseInWorld.YMeters;
|
||||
var segmentLengthSquared =
|
||||
deltaX * deltaX +
|
||||
deltaY * deltaY;
|
||||
var minimumLengthSquared =
|
||||
MinimumSegmentLengthMeters *
|
||||
MinimumSegmentLengthMeters;
|
||||
|
||||
if (segmentLengthSquared <
|
||||
minimumLengthSquared)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
|
||||
parameterName);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹弧长必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
|
||||
/// </summary>
|
||||
public readonly struct TrajectoryPoint
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建包含中心位姿、曲率和速度信息的参考轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint(
|
||||
double arcLengthMeters,
|
||||
Pose2D poseInWorld,
|
||||
double curvaturePerMeter,
|
||||
double referenceSpeedMetersPerSecond)
|
||||
{
|
||||
EnsureFiniteNonNegative(
|
||||
arcLengthMeters,
|
||||
nameof(arcLengthMeters));
|
||||
EnsureFinite(
|
||||
poseInWorld.XMeters,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
poseInWorld.YMeters,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
poseInWorld.YawRadians,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
curvaturePerMeter,
|
||||
nameof(curvaturePerMeter));
|
||||
EnsureFinite(
|
||||
referenceSpeedMetersPerSecond,
|
||||
nameof(referenceSpeedMetersPerSecond));
|
||||
ArcLengthMeters = arcLengthMeters;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
CurvaturePerMeter = curvaturePerMeter;
|
||||
ReferenceSpeedMetersPerSecond =
|
||||
referenceSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取从轨迹起点累计到当前点的弧长，单位为m。
|
||||
/// </summary>
|
||||
public double ArcLengthMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心参考坐标系在世界坐标系中的位姿。
|
||||
/// </summary>
|
||||
public Pose2D PoseInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
|
||||
/// </summary>
|
||||
public double CurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。
|
||||
/// </summary>
|
||||
public double ReferenceSpeedMetersPerSecond { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹点参数必须是有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹累计弧长不能为负数。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存车体中心投影到二维参考轨迹后得到的只读结果。
|
||||
/// </summary>
|
||||
public readonly struct TrajectoryProjection
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
|
||||
/// </summary>
|
||||
public TrajectoryProjection(
|
||||
int segmentStartIndex,
|
||||
TrajectoryPoint referencePoint,
|
||||
double lateralErrorMeters,
|
||||
double headingErrorRadians,
|
||||
double distanceToTrajectoryMeters,
|
||||
double remainingDistanceMeters)
|
||||
{
|
||||
if (segmentStartIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(segmentStartIndex),
|
||||
"投影线段起点索引不能为负数。");
|
||||
}
|
||||
|
||||
EnsureFinite(
|
||||
lateralErrorMeters,
|
||||
nameof(lateralErrorMeters));
|
||||
EnsureFinite(
|
||||
headingErrorRadians,
|
||||
nameof(headingErrorRadians));
|
||||
EnsureFiniteNonNegative(
|
||||
distanceToTrajectoryMeters,
|
||||
nameof(distanceToTrajectoryMeters));
|
||||
EnsureFiniteNonNegative(
|
||||
remainingDistanceMeters,
|
||||
nameof(remainingDistanceMeters));
|
||||
|
||||
SegmentStartIndex = segmentStartIndex;
|
||||
ReferencePoint = referencePoint;
|
||||
LateralErrorMeters = lateralErrorMeters;
|
||||
HeadingErrorRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
headingErrorRadians);
|
||||
DistanceToTrajectoryMeters =
|
||||
distanceToTrajectoryMeters;
|
||||
RemainingDistanceMeters =
|
||||
remainingDistanceMeters;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
|
||||
/// </summary>
|
||||
public int SegmentStartIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置插值得到的车体中心参考轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint ReferencePoint { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
|
||||
/// </summary>
|
||||
public double LateralErrorMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
|
||||
/// </summary>
|
||||
public double HeadingErrorRadians { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心到投影点的欧氏距离，单位为m。
|
||||
/// </summary>
|
||||
public double DistanceToTrajectoryMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
|
||||
/// </summary>
|
||||
public double RemainingDistanceMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置从轨迹起点累计的弧长，单位为m。
|
||||
/// </summary>
|
||||
public double ArcLengthMeters =>
|
||||
ReferencePoint.ArcLengthMeters;
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹投影参数必须是有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹投影距离不能为负数。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,219 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
|
||||
/// </summary>
|
||||
public static class TrajectoryProjector
|
||||
{
|
||||
/// <summary>
|
||||
/// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
|
||||
/// </summary>
|
||||
public static TrajectoryProjection Project(
|
||||
Trajectory2D trajectory,
|
||||
Pose2D vehiclePoseInWorld)
|
||||
{
|
||||
if (trajectory == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(trajectory));
|
||||
}
|
||||
|
||||
EnsureFinitePose(
|
||||
vehiclePoseInWorld,
|
||||
nameof(vehiclePoseInWorld));
|
||||
|
||||
var bestSegmentStartIndex = 0;
|
||||
var bestInterpolationRatio = 0.0;
|
||||
var bestProjectedX = 0.0;
|
||||
var bestProjectedY = 0.0;
|
||||
var bestDistanceSquared =
|
||||
double.PositiveInfinity;
|
||||
|
||||
for (var segmentStartIndex = 0;
|
||||
segmentStartIndex < trajectory.Count - 1;
|
||||
segmentStartIndex++)
|
||||
{
|
||||
var segmentStart =
|
||||
trajectory[segmentStartIndex];
|
||||
var segmentEnd =
|
||||
trajectory[segmentStartIndex + 1];
|
||||
|
||||
var segmentX =
|
||||
segmentEnd.PoseInWorld.XMeters -
|
||||
segmentStart.PoseInWorld.XMeters;
|
||||
var segmentY =
|
||||
segmentEnd.PoseInWorld.YMeters -
|
||||
segmentStart.PoseInWorld.YMeters;
|
||||
var segmentLengthSquared =
|
||||
segmentX * segmentX +
|
||||
segmentY * segmentY;
|
||||
|
||||
var vehicleFromSegmentStartX =
|
||||
vehiclePoseInWorld.XMeters -
|
||||
segmentStart.PoseInWorld.XMeters;
|
||||
var vehicleFromSegmentStartY =
|
||||
vehiclePoseInWorld.YMeters -
|
||||
segmentStart.PoseInWorld.YMeters;
|
||||
|
||||
var interpolationRatio =
|
||||
InterpolationMath.Clamp01(
|
||||
(vehicleFromSegmentStartX * segmentX +
|
||||
vehicleFromSegmentStartY * segmentY) /
|
||||
segmentLengthSquared);
|
||||
|
||||
var projectedX =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.PoseInWorld.XMeters,
|
||||
segmentEnd.PoseInWorld.XMeters,
|
||||
interpolationRatio);
|
||||
var projectedY =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.PoseInWorld.YMeters,
|
||||
segmentEnd.PoseInWorld.YMeters,
|
||||
interpolationRatio);
|
||||
|
||||
var projectionErrorX =
|
||||
projectedX -
|
||||
vehiclePoseInWorld.XMeters;
|
||||
var projectionErrorY =
|
||||
projectedY -
|
||||
vehiclePoseInWorld.YMeters;
|
||||
var distanceSquared =
|
||||
projectionErrorX * projectionErrorX +
|
||||
projectionErrorY * projectionErrorY;
|
||||
|
||||
if (distanceSquared >= bestDistanceSquared)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
bestSegmentStartIndex =
|
||||
segmentStartIndex;
|
||||
bestInterpolationRatio =
|
||||
interpolationRatio;
|
||||
bestProjectedX = projectedX;
|
||||
bestProjectedY = projectedY;
|
||||
bestDistanceSquared = distanceSquared;
|
||||
}
|
||||
|
||||
return BuildProjection(
|
||||
trajectory,
|
||||
vehiclePoseInWorld,
|
||||
bestSegmentStartIndex,
|
||||
bestInterpolationRatio,
|
||||
bestProjectedX,
|
||||
bestProjectedY,
|
||||
bestDistanceSquared);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据最近线段和插值比例生成控制器使用的完整投影结果。
|
||||
/// </summary>
|
||||
private static TrajectoryProjection BuildProjection(
|
||||
Trajectory2D trajectory,
|
||||
Pose2D vehiclePoseInWorld,
|
||||
int segmentStartIndex,
|
||||
double interpolationRatio,
|
||||
double projectedX,
|
||||
double projectedY,
|
||||
double distanceSquared)
|
||||
{
|
||||
var segmentStart =
|
||||
trajectory[segmentStartIndex];
|
||||
var segmentEnd =
|
||||
trajectory[segmentStartIndex + 1];
|
||||
|
||||
var referenceYawRadians =
|
||||
AngleMath.LerpRadians(
|
||||
segmentStart.PoseInWorld.YawRadians,
|
||||
segmentEnd.PoseInWorld.YawRadians,
|
||||
interpolationRatio);
|
||||
var referenceArcLengthMeters =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.ArcLengthMeters,
|
||||
segmentEnd.ArcLengthMeters,
|
||||
interpolationRatio);
|
||||
var referenceCurvaturePerMeter =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.CurvaturePerMeter,
|
||||
segmentEnd.CurvaturePerMeter,
|
||||
interpolationRatio);
|
||||
var referenceSpeedMetersPerSecond =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.ReferenceSpeedMetersPerSecond,
|
||||
segmentEnd.ReferenceSpeedMetersPerSecond,
|
||||
interpolationRatio);
|
||||
|
||||
var referencePoint =
|
||||
new TrajectoryPoint(
|
||||
referenceArcLengthMeters,
|
||||
new Pose2D(
|
||||
projectedX,
|
||||
projectedY,
|
||||
referenceYawRadians),
|
||||
referenceCurvaturePerMeter,
|
||||
referenceSpeedMetersPerSecond);
|
||||
|
||||
var segmentX =
|
||||
segmentEnd.PoseInWorld.XMeters -
|
||||
segmentStart.PoseInWorld.XMeters;
|
||||
var segmentY =
|
||||
segmentEnd.PoseInWorld.YMeters -
|
||||
segmentStart.PoseInWorld.YMeters;
|
||||
var segmentLength =
|
||||
Math.Sqrt(
|
||||
segmentX * segmentX +
|
||||
segmentY * segmentY);
|
||||
|
||||
// 以轨迹线段的前进方向判断左右：
|
||||
// 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
|
||||
var vehicleToProjectionX =
|
||||
projectedX -
|
||||
vehiclePoseInWorld.XMeters;
|
||||
var vehicleToProjectionY =
|
||||
projectedY -
|
||||
vehiclePoseInWorld.YMeters;
|
||||
var lateralErrorMeters =
|
||||
(segmentX * vehicleToProjectionY -
|
||||
segmentY * vehicleToProjectionX) /
|
||||
segmentLength;
|
||||
|
||||
var headingErrorRadians =
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
referenceYawRadians,
|
||||
vehiclePoseInWorld.YawRadians);
|
||||
|
||||
return new TrajectoryProjection(
|
||||
segmentStartIndex,
|
||||
referencePoint,
|
||||
lateralErrorMeters,
|
||||
headingErrorRadians,
|
||||
Math.Sqrt(distanceSquared),
|
||||
trajectory.GetRemainingDistanceMeters(
|
||||
referenceArcLengthMeters));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查用于投影的实际车体中心位姿是否包含有限数值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(pose.XMeters) ||
|
||||
double.IsInfinity(pose.XMeters) ||
|
||||
double.IsNaN(pose.YMeters) ||
|
||||
double.IsInfinity(pose.YMeters) ||
|
||||
double.IsNaN(pose.YawRadians) ||
|
||||
double.IsInfinity(pose.YawRadians))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"用于轨迹投影的车体位姿必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -81,6 +81,29 @@ namespace MyParking.Shared
|
||||
return NormalizeDegrees(targetDegrees - currentDegrees);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。
|
||||
/// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio，
|
||||
/// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。
|
||||
/// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。
|
||||
/// </summary>
|
||||
public static double LerpRadians(
|
||||
double startRadians,
|
||||
double endRadians,
|
||||
double ratio)
|
||||
{
|
||||
EnsureFinite(startRadians, nameof(startRadians));
|
||||
EnsureFinite(endRadians, nameof(endRadians));
|
||||
EnsureFinite(ratio, nameof(ratio));
|
||||
|
||||
var shortestDifference = ShortestDifferenceRadians(
|
||||
endRadians,
|
||||
startRadians);
|
||||
|
||||
return NormalizeRadians(
|
||||
startRadians + ratio * shortestDifference);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度从度转换为弧度，不进行归一化。
|
||||
/// </summary>
|
||||
|
||||
@@ -0,0 +1,61 @@
|
||||
|
||||
using System;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供与具体业务和坐标系无关的基础插值功能。
|
||||
/// </summary>
|
||||
public static class InterpolationMath
|
||||
{
|
||||
/// <summary>
|
||||
/// 将插值比例限制到[0, 1]闭区间。
|
||||
/// </summary>
|
||||
public static double Clamp01(double value)
|
||||
{
|
||||
EnsureFinite(value, nameof(value));
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
if (value >= 1.0)
|
||||
{
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对两个标量执行线性插值。
|
||||
/// ratio为0时返回start，ratio为1时返回end；本方法不限制ratio，
|
||||
/// 因此也支持区间外的线性外插。
|
||||
/// </summary>
|
||||
public static double Lerp(
|
||||
double start,
|
||||
double end,
|
||||
double ratio)
|
||||
{
|
||||
EnsureFinite(start, nameof(start));
|
||||
EnsureFinite(end, nameof(end));
|
||||
EnsureFinite(ratio, nameof(ratio));
|
||||
|
||||
return start + ratio * (end - start);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证输入是可用于插值计算的有限数值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"插值参数必须是有限数值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,760 @@
|
||||
"""对比原始Detour差分与C#在线车辆状态估计结果。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from collections import Counter
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
|
||||
|
||||
REQUIRED_COLUMNS = {
|
||||
"ElapsedSeconds",
|
||||
"DetourX",
|
||||
"DetourY",
|
||||
"DetourTheta",
|
||||
}
|
||||
|
||||
|
||||
def wrap_radians(angle: float | np.ndarray) -> float | np.ndarray:
|
||||
"""将弧度归一化到[-π, π)区间。"""
|
||||
return (angle + np.pi) % (2.0 * np.pi) - np.pi
|
||||
|
||||
|
||||
def angle_difference(target: float, current: float) -> float:
|
||||
"""计算从当前角到目标角的最短有符号弧度差。"""
|
||||
return float(wrap_radians(target - current))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Pose:
|
||||
"""保存世界坐标系中的二维位姿，单位为m和rad。"""
|
||||
|
||||
x: float
|
||||
y: float
|
||||
yaw: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class State:
|
||||
"""保存脚本复现得到的世界位姿和世界速度。"""
|
||||
|
||||
timestamp: float
|
||||
pose: Pose
|
||||
vx: float
|
||||
vy: float
|
||||
omega: float
|
||||
velocity_valid: bool
|
||||
|
||||
|
||||
class LowPassFilter:
|
||||
"""复现FirstOrderLowPassFilter的一阶低通计算。"""
|
||||
|
||||
def __init__(self, time_constant_seconds: float) -> None:
|
||||
if not np.isfinite(time_constant_seconds) or time_constant_seconds <= 0:
|
||||
raise ValueError("滤波时间常数必须是正有限值。")
|
||||
self.time_constant_seconds = float(time_constant_seconds)
|
||||
self.initialized = False
|
||||
self.value = 0.0
|
||||
|
||||
def update(self, value: float, delta_time_seconds: float) -> float:
|
||||
"""按照真实采样间隔更新滤波输出。"""
|
||||
if not self.initialized:
|
||||
self.value = float(value)
|
||||
self.initialized = True
|
||||
return self.value
|
||||
alpha = delta_time_seconds / (
|
||||
self.time_constant_seconds + delta_time_seconds
|
||||
)
|
||||
self.value += alpha * (float(value) - self.value)
|
||||
return self.value
|
||||
|
||||
def reset(self) -> None:
|
||||
"""清除滤波历史。"""
|
||||
self.initialized = False
|
||||
self.value = 0.0
|
||||
|
||||
|
||||
class VelocityEstimator:
|
||||
"""复现VelocityEstimator2D的世界速度差分与低通处理。"""
|
||||
|
||||
def __init__(self, linear_tau: float, angular_tau: float) -> None:
|
||||
self.vx_filter = LowPassFilter(linear_tau)
|
||||
self.vy_filter = LowPassFilter(linear_tau)
|
||||
self.omega_filter = LowPassFilter(angular_tau)
|
||||
self.previous_pose: Pose | None = None
|
||||
self.previous_timestamp = 0.0
|
||||
|
||||
def reset(self, pose: Pose | None = None, timestamp: float = 0.0) -> State | None:
|
||||
"""清除历史，并可使用当前位姿建立新的零速差分基准。"""
|
||||
self.vx_filter.reset()
|
||||
self.vy_filter.reset()
|
||||
self.omega_filter.reset()
|
||||
self.previous_pose = pose
|
||||
self.previous_timestamp = float(timestamp)
|
||||
if pose is None:
|
||||
return None
|
||||
return State(timestamp, pose, 0.0, 0.0, 0.0, False)
|
||||
|
||||
def update(self, pose: Pose, timestamp: float) -> State:
|
||||
"""使用一个新的有效位姿更新速度估计。"""
|
||||
if self.previous_pose is None:
|
||||
state = self.reset(pose, timestamp)
|
||||
assert state is not None
|
||||
return state
|
||||
delta_time = timestamp - self.previous_timestamp
|
||||
if delta_time <= 0.0:
|
||||
raise ValueError("新样本时间戳必须严格递增。")
|
||||
raw_vx = (pose.x - self.previous_pose.x) / delta_time
|
||||
raw_vy = (pose.y - self.previous_pose.y) / delta_time
|
||||
raw_omega = angle_difference(
|
||||
pose.yaw,
|
||||
self.previous_pose.yaw,
|
||||
) / delta_time
|
||||
state = State(
|
||||
timestamp,
|
||||
pose,
|
||||
self.vx_filter.update(raw_vx, delta_time),
|
||||
self.vy_filter.update(raw_vy, delta_time),
|
||||
self.omega_filter.update(raw_omega, delta_time),
|
||||
True,
|
||||
)
|
||||
self.previous_pose = pose
|
||||
self.previous_timestamp = timestamp
|
||||
return state
|
||||
|
||||
def rebase_preserving_velocity(
|
||||
self,
|
||||
pose: Pose,
|
||||
timestamp: float,
|
||||
) -> State:
|
||||
"""更新差分基准但保留三个低通滤波器的当前输出。"""
|
||||
self.previous_pose = pose
|
||||
self.previous_timestamp = timestamp
|
||||
velocity_valid = (
|
||||
self.vx_filter.initialized
|
||||
and self.vy_filter.initialized
|
||||
and self.omega_filter.initialized
|
||||
)
|
||||
return State(
|
||||
timestamp,
|
||||
pose,
|
||||
self.vx_filter.value if velocity_valid else 0.0,
|
||||
self.vy_filter.value if velocity_valid else 0.0,
|
||||
self.omega_filter.value if velocity_valid else 0.0,
|
||||
velocity_valid,
|
||||
)
|
||||
|
||||
|
||||
class DetourProviderSimulator:
|
||||
"""按当前简化版DetourVehicleStateProvider处理离线CSV样本。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
linear_tau: float = 0.15,
|
||||
angular_tau: float = 0.20,
|
||||
maximum_linear_speed: float = 1.20,
|
||||
maximum_angular_speed: float = np.pi / 4.0,
|
||||
position_jump_margin: float = 0.03,
|
||||
heading_jump_margin: float = np.deg2rad(5.0),
|
||||
stationary_seconds: float = 0.35,
|
||||
) -> None:
|
||||
self.estimator = VelocityEstimator(linear_tau, angular_tau)
|
||||
self.maximum_linear_speed = maximum_linear_speed
|
||||
self.maximum_angular_speed = maximum_angular_speed
|
||||
self.position_jump_margin = position_jump_margin
|
||||
self.heading_jump_margin = heading_jump_margin
|
||||
self.stationary_seconds = stationary_seconds
|
||||
|
||||
self.accepted_pose: Pose | None = None
|
||||
self.accepted_timestamp = 0.0
|
||||
self.latest_state: State | None = None
|
||||
self.stationary_hold = False
|
||||
|
||||
@staticmethod
|
||||
def poses_equal(first: Pose, second: Pose) -> bool:
|
||||
"""判断两次读取是否为Detour保持输出的同一数值帧。"""
|
||||
return (
|
||||
abs(first.x - second.x) <= 1e-9
|
||||
and abs(first.y - second.y) <= 1e-9
|
||||
and abs(angle_difference(first.yaw, second.yaw)) <= 1e-8
|
||||
)
|
||||
|
||||
def motion_plausible(self, start: Pose, end: Pose, delta_time: float) -> bool:
|
||||
"""按照车辆绝对运动能力判断两帧是否连续。"""
|
||||
if not np.isfinite(delta_time) or delta_time <= 0.0:
|
||||
return False
|
||||
displacement = np.hypot(end.x - start.x, end.y - start.y)
|
||||
heading_change = abs(angle_difference(end.yaw, start.yaw))
|
||||
return (
|
||||
displacement
|
||||
<= self.maximum_linear_speed * delta_time
|
||||
+ self.position_jump_margin
|
||||
and heading_change
|
||||
<= self.maximum_angular_speed * delta_time
|
||||
+ self.heading_jump_margin
|
||||
)
|
||||
|
||||
def accept_after_reset(self, pose: Pose, timestamp: float) -> State:
|
||||
"""接受首帧或确认后的重定位并清除速度历史。"""
|
||||
state = self.estimator.reset(pose, timestamp)
|
||||
assert state is not None
|
||||
self.latest_state = state
|
||||
self.accepted_pose = pose
|
||||
self.accepted_timestamp = timestamp
|
||||
self.stationary_hold = False
|
||||
return state
|
||||
|
||||
def accept_continuous(self, pose: Pose, timestamp: float) -> State:
|
||||
"""接受连续正常位姿并更新速度估计。"""
|
||||
state = self.estimator.update(pose, timestamp)
|
||||
self.latest_state = state
|
||||
self.accepted_pose = pose
|
||||
self.accepted_timestamp = timestamp
|
||||
self.stationary_hold = False
|
||||
return state
|
||||
|
||||
def handle_repeated(self, timestamp: float) -> tuple[State, str]:
|
||||
"""保留重复帧，并在长期不变后将估计速度归零。"""
|
||||
assert self.accepted_pose is not None
|
||||
assert self.latest_state is not None
|
||||
unchanged = timestamp - self.accepted_timestamp
|
||||
if not self.stationary_hold and unchanged >= self.stationary_seconds:
|
||||
self.latest_state = State(
|
||||
timestamp,
|
||||
self.accepted_pose,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
True,
|
||||
)
|
||||
self.stationary_hold = True
|
||||
return self.latest_state, "stationary_zero"
|
||||
return self.latest_state, "duplicate"
|
||||
|
||||
def process(
|
||||
self,
|
||||
pose: Pose,
|
||||
timestamp: float,
|
||||
velocity_innovation_abnormal: bool = False,
|
||||
) -> tuple[State | None, str]:
|
||||
"""处理一帧CSV中的Detour读取结果。"""
|
||||
if self.accepted_pose is None:
|
||||
return self.accept_after_reset(pose, timestamp), "initialized"
|
||||
if self.poses_equal(pose, self.accepted_pose):
|
||||
return self.handle_repeated(timestamp)
|
||||
if self.stationary_hold:
|
||||
return self.accept_after_reset(pose, timestamp), "restart_after_stationary"
|
||||
|
||||
elapsed = timestamp - self.accepted_timestamp
|
||||
if not self.motion_plausible(self.accepted_pose, pose, elapsed):
|
||||
assert self.latest_state is not None
|
||||
return self.latest_state, "physical_anomaly"
|
||||
if velocity_innovation_abnormal:
|
||||
self.latest_state = self.estimator.rebase_preserving_velocity(
|
||||
pose,
|
||||
timestamp,
|
||||
)
|
||||
self.accepted_pose = pose
|
||||
self.accepted_timestamp = timestamp
|
||||
self.stationary_hold = False
|
||||
return self.latest_state, "velocity_rebase"
|
||||
return self.accept_continuous(pose, timestamp), "accepted"
|
||||
|
||||
|
||||
def segmented_unwrap_degrees(values_radians: np.ndarray) -> np.ndarray:
|
||||
"""分别展开由NaN分隔的有效航向区间。"""
|
||||
result = np.full(values_radians.shape, np.nan, dtype=float)
|
||||
finite = np.isfinite(values_radians)
|
||||
indices = np.flatnonzero(finite)
|
||||
if not indices.size:
|
||||
return result
|
||||
starts = np.r_[0, np.flatnonzero(np.diff(indices) > 1) + 1]
|
||||
ends = np.r_[starts[1:], indices.size]
|
||||
for start, end in zip(starts, ends):
|
||||
segment_indices = indices[start:end]
|
||||
result[segment_indices] = np.rad2deg(
|
||||
np.unwrap(values_radians[segment_indices])
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def calculate_naive_derivatives(
|
||||
time: np.ndarray,
|
||||
x: np.ndarray,
|
||||
y: np.ndarray,
|
||||
yaw: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""直接逐记录帧差分，保留重复帧造成的零值和更新尖峰。"""
|
||||
speed = np.full(time.shape, np.nan, dtype=float)
|
||||
omega = np.full(time.shape, np.nan, dtype=float)
|
||||
delta_time = np.diff(time)
|
||||
valid = np.isfinite(delta_time) & (delta_time > 0.0)
|
||||
delta_x = np.diff(x)
|
||||
delta_y = np.diff(y)
|
||||
delta_yaw = wrap_radians(np.diff(yaw))
|
||||
speed_values = np.full(delta_time.shape, np.nan, dtype=float)
|
||||
omega_values = np.full(delta_time.shape, np.nan, dtype=float)
|
||||
speed_values[valid] = (
|
||||
np.hypot(delta_x[valid], delta_y[valid])
|
||||
/ delta_time[valid]
|
||||
)
|
||||
omega_values[valid] = delta_yaw[valid] / delta_time[valid]
|
||||
speed[1:] = speed_values
|
||||
omega[1:] = omega_values
|
||||
return speed, omega
|
||||
|
||||
|
||||
def configure_matplotlib() -> None:
|
||||
"""配置常见中文字体和负号显示。"""
|
||||
plt.rcParams["font.sans-serif"] = [
|
||||
"Microsoft YaHei",
|
||||
"SimHei",
|
||||
"Arial Unicode MS",
|
||||
"DejaVu Sans",
|
||||
]
|
||||
plt.rcParams["axes.unicode_minus"] = False
|
||||
|
||||
|
||||
def load_csv(csv_path: Path) -> pd.DataFrame:
|
||||
"""读取并校验状态估计对比所需的CSV字段。"""
|
||||
frame = pd.read_csv(csv_path)
|
||||
missing = REQUIRED_COLUMNS.difference(frame.columns)
|
||||
if missing:
|
||||
raise ValueError(
|
||||
f"{csv_path.name}缺少字段：{', '.join(sorted(missing))}"
|
||||
)
|
||||
for column in REQUIRED_COLUMNS:
|
||||
frame[column] = pd.to_numeric(frame[column], errors="coerce")
|
||||
frame = (
|
||||
frame.dropna(subset=list(REQUIRED_COLUMNS))
|
||||
.sort_values("ElapsedSeconds")
|
||||
.drop_duplicates("ElapsedSeconds", keep="last")
|
||||
.reset_index(drop=True)
|
||||
)
|
||||
if len(frame) < 3:
|
||||
raise ValueError(f"{csv_path.name}有效数据不足3行。")
|
||||
frame["ElapsedSeconds"] -= frame["ElapsedSeconds"].iloc[0]
|
||||
return frame
|
||||
|
||||
|
||||
def detect_visual_anomalies(
|
||||
time: np.ndarray,
|
||||
x: np.ndarray,
|
||||
y: np.ndarray,
|
||||
yaw: np.ndarray,
|
||||
linear_tau: float,
|
||||
angular_tau: float,
|
||||
position_residual_meters: float,
|
||||
heading_residual_radians: float,
|
||||
stationary_seconds: float,
|
||||
) -> np.ndarray:
|
||||
"""用恒速预测残差标注可疑跳变，不修改任何状态估计数据。"""
|
||||
anomalies = np.zeros(time.shape, dtype=bool)
|
||||
estimator = VelocityEstimator(linear_tau, angular_tau)
|
||||
previous_pose: Pose | None = None
|
||||
previous_update_time = 0.0
|
||||
latest_state: State | None = None
|
||||
stationary = False
|
||||
|
||||
for index, timestamp in enumerate(time):
|
||||
pose = Pose(
|
||||
float(x[index]),
|
||||
float(y[index]),
|
||||
float(wrap_radians(yaw[index])),
|
||||
)
|
||||
|
||||
if previous_pose is None:
|
||||
latest_state = estimator.reset(pose, float(timestamp))
|
||||
previous_pose = pose
|
||||
previous_update_time = float(timestamp)
|
||||
continue
|
||||
|
||||
if DetourProviderSimulator.poses_equal(pose, previous_pose):
|
||||
if (
|
||||
not stationary
|
||||
and timestamp - previous_update_time >= stationary_seconds
|
||||
):
|
||||
stationary = True
|
||||
continue
|
||||
|
||||
# 静止后的第一个新定位只重新建立差分基准，避免把起步误标为跳变。
|
||||
if stationary:
|
||||
latest_state = estimator.reset(pose, float(timestamp))
|
||||
previous_pose = pose
|
||||
previous_update_time = float(timestamp)
|
||||
stationary = False
|
||||
continue
|
||||
|
||||
delta_time = float(timestamp) - previous_update_time
|
||||
if (
|
||||
latest_state is not None
|
||||
and latest_state.velocity_valid
|
||||
and delta_time > 0.0
|
||||
):
|
||||
predicted_x = previous_pose.x + latest_state.vx * delta_time
|
||||
predicted_y = previous_pose.y + latest_state.vy * delta_time
|
||||
predicted_yaw = float(
|
||||
wrap_radians(
|
||||
previous_pose.yaw + latest_state.omega * delta_time
|
||||
)
|
||||
)
|
||||
position_residual = np.hypot(
|
||||
pose.x - predicted_x,
|
||||
pose.y - predicted_y,
|
||||
)
|
||||
heading_residual = abs(
|
||||
angle_difference(pose.yaw, predicted_yaw)
|
||||
)
|
||||
|
||||
if (
|
||||
position_residual > position_residual_meters
|
||||
or heading_residual > heading_residual_radians
|
||||
):
|
||||
anomalies[index] = True
|
||||
# 标注后从当前观测重新开始，避免一个跳变引发连续误标。
|
||||
latest_state = estimator.rebase_preserving_velocity(
|
||||
pose,
|
||||
float(timestamp),
|
||||
)
|
||||
previous_pose = pose
|
||||
previous_update_time = float(timestamp)
|
||||
continue
|
||||
|
||||
latest_state = estimator.update(pose, float(timestamp))
|
||||
previous_pose = pose
|
||||
previous_update_time = float(timestamp)
|
||||
|
||||
return anomalies
|
||||
|
||||
|
||||
def simulate(
|
||||
frame: pd.DataFrame,
|
||||
args: argparse.Namespace,
|
||||
) -> tuple[pd.DataFrame, Counter]:
|
||||
"""使用当前C#参数处理整份Detour记录。"""
|
||||
time = frame["ElapsedSeconds"].to_numpy(float)
|
||||
raw_x = frame["DetourX"].to_numpy(float) / 1000.0
|
||||
raw_y = frame["DetourY"].to_numpy(float) / 1000.0
|
||||
raw_yaw = np.deg2rad(frame["DetourTheta"].to_numpy(float))
|
||||
raw_speed, raw_omega = calculate_naive_derivatives(
|
||||
time,
|
||||
raw_x,
|
||||
raw_y,
|
||||
raw_yaw,
|
||||
)
|
||||
visual_anomalies = detect_visual_anomalies(
|
||||
time,
|
||||
raw_x,
|
||||
raw_y,
|
||||
raw_yaw,
|
||||
args.linear_tau,
|
||||
args.angular_tau,
|
||||
args.annotation_position_residual_mm / 1000.0,
|
||||
np.deg2rad(args.annotation_heading_residual_deg),
|
||||
args.stationary_seconds,
|
||||
)
|
||||
|
||||
simulator = DetourProviderSimulator(
|
||||
linear_tau=args.linear_tau,
|
||||
angular_tau=args.angular_tau,
|
||||
maximum_linear_speed=args.maximum_linear_speed,
|
||||
maximum_angular_speed=np.deg2rad(args.maximum_angular_speed_deg),
|
||||
position_jump_margin=args.position_jump_margin_mm / 1000.0,
|
||||
heading_jump_margin=np.deg2rad(args.heading_jump_margin_deg),
|
||||
stationary_seconds=args.stationary_seconds,
|
||||
)
|
||||
|
||||
processed_x = np.full(time.shape, np.nan)
|
||||
processed_y = np.full(time.shape, np.nan)
|
||||
processed_yaw = np.full(time.shape, np.nan)
|
||||
processed_speed = np.full(time.shape, np.nan)
|
||||
processed_omega = np.full(time.shape, np.nan)
|
||||
events: list[str] = []
|
||||
|
||||
for index, timestamp in enumerate(time):
|
||||
pose = Pose(
|
||||
raw_x[index],
|
||||
raw_y[index],
|
||||
float(wrap_radians(raw_yaw[index])),
|
||||
)
|
||||
state, event = simulator.process(
|
||||
pose,
|
||||
float(timestamp),
|
||||
bool(visual_anomalies[index]),
|
||||
)
|
||||
events.append(event)
|
||||
if state is None:
|
||||
continue
|
||||
processed_x[index] = state.pose.x
|
||||
processed_y[index] = state.pose.y
|
||||
processed_yaw[index] = state.pose.yaw
|
||||
if state.velocity_valid:
|
||||
processed_speed[index] = np.hypot(state.vx, state.vy)
|
||||
processed_omega[index] = state.omega
|
||||
|
||||
result = pd.DataFrame(
|
||||
{
|
||||
"TimeSeconds": time,
|
||||
"RawX": raw_x,
|
||||
"RawY": raw_y,
|
||||
"RawYawRadians": raw_yaw,
|
||||
"RawSpeed": raw_speed,
|
||||
"RawOmegaRadiansPerSecond": raw_omega,
|
||||
"ProcessedX": processed_x,
|
||||
"ProcessedY": processed_y,
|
||||
"ProcessedYawRadians": processed_yaw,
|
||||
"ProcessedSpeed": processed_speed,
|
||||
"ProcessedOmegaRadiansPerSecond": processed_omega,
|
||||
"VisualAnomaly": visual_anomalies,
|
||||
"Event": events,
|
||||
}
|
||||
)
|
||||
counts = Counter(events)
|
||||
counts["visual_anomaly"] = int(visual_anomalies.sum())
|
||||
return result, counts
|
||||
|
||||
|
||||
def plot_comparison(
|
||||
csv_path: Path,
|
||||
frame: pd.DataFrame,
|
||||
result: pd.DataFrame,
|
||||
event_counts: Counter,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成位置、航向、线速度和角速度处理前后对比图。"""
|
||||
time = result["TimeSeconds"].to_numpy(float)
|
||||
anomalous = result["VisualAnomaly"].to_numpy(bool)
|
||||
raw_yaw_degrees = segmented_unwrap_degrees(
|
||||
result["RawYawRadians"].to_numpy(float)
|
||||
)
|
||||
processed_yaw_degrees = segmented_unwrap_degrees(
|
||||
result["ProcessedYawRadians"].to_numpy(float)
|
||||
)
|
||||
|
||||
fig, axes = plt.subplots(
|
||||
5,
|
||||
1,
|
||||
figsize=(13.0, 16.0),
|
||||
sharex=True,
|
||||
)
|
||||
|
||||
series = [
|
||||
("RawX", "ProcessedX", "世界坐标X / m"),
|
||||
("RawY", "ProcessedY", "世界坐标Y / m"),
|
||||
]
|
||||
for axis, (raw_name, processed_name, ylabel) in zip(axes[:2], series):
|
||||
axis.plot(time, result[raw_name], color="0.65", linewidth=1.0, label="原始Detour")
|
||||
axis.plot(time, result[processed_name], color="tab:blue", linewidth=1.5, label="在线处理后")
|
||||
axis.scatter(
|
||||
time[anomalous],
|
||||
result.loc[anomalous, raw_name],
|
||||
color="tab:red",
|
||||
marker="x",
|
||||
s=26,
|
||||
label="异常位置",
|
||||
zorder=3,
|
||||
)
|
||||
axis.set_ylabel(ylabel)
|
||||
axis.grid(True, alpha=0.3)
|
||||
axis.legend(loc="best")
|
||||
|
||||
axes[2].plot(time, raw_yaw_degrees, color="0.65", linewidth=1.0, label="原始Detour")
|
||||
axes[2].plot(time, processed_yaw_degrees, color="tab:blue", linewidth=1.5, label="在线处理后")
|
||||
axes[2].scatter(
|
||||
time[anomalous],
|
||||
raw_yaw_degrees[anomalous],
|
||||
color="tab:red",
|
||||
marker="x",
|
||||
s=26,
|
||||
label="异常位置",
|
||||
zorder=3,
|
||||
)
|
||||
axes[2].set_ylabel("展开航向角 / deg")
|
||||
axes[2].grid(True, alpha=0.3)
|
||||
axes[2].legend(loc="best")
|
||||
|
||||
axes[3].plot(time, result["RawSpeed"], color="0.65", linewidth=1.0, label="逐记录帧直接差分")
|
||||
axes[3].plot(time, result["ProcessedSpeed"], color="tab:green", linewidth=1.5, label="去重、跳变保护和低通后")
|
||||
if "CommandSpeed" in frame.columns:
|
||||
command_speed = pd.to_numeric(
|
||||
frame["CommandSpeed"], errors="coerce"
|
||||
).to_numpy(float)
|
||||
axes[3].plot(time, command_speed, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令线速度")
|
||||
axes[3].set_ylabel("合线速度 / (m/s)")
|
||||
axes[3].grid(True, alpha=0.3)
|
||||
axes[3].legend(loc="best")
|
||||
|
||||
axes[4].plot(
|
||||
time,
|
||||
np.rad2deg(result["RawOmegaRadiansPerSecond"]),
|
||||
color="0.65",
|
||||
linewidth=1.0,
|
||||
label="逐记录帧最短角差",
|
||||
)
|
||||
axes[4].plot(
|
||||
time,
|
||||
np.rad2deg(result["ProcessedOmegaRadiansPerSecond"]),
|
||||
color="tab:purple",
|
||||
linewidth=1.5,
|
||||
label="去重、跳变保护和低通后",
|
||||
)
|
||||
if "CommandAngularSpeedRadPerSecond" in frame.columns:
|
||||
command_omega = np.rad2deg(
|
||||
pd.to_numeric(
|
||||
frame["CommandAngularSpeedRadPerSecond"],
|
||||
errors="coerce",
|
||||
).to_numpy(float)
|
||||
)
|
||||
axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度")
|
||||
elif "CommandAngularSpeed" in frame.columns:
|
||||
# 旧版CSV只有CommandAngularSpeed列，该列历史单位是deg/s；
|
||||
# 新版CSV另增RadPerSecond列，不能把旧列再次按rad/s换算。
|
||||
command_omega = pd.to_numeric(
|
||||
frame["CommandAngularSpeed"],
|
||||
errors="coerce",
|
||||
).to_numpy(float)
|
||||
axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度")
|
||||
axes[4].set_ylabel("角速度 / (deg/s)")
|
||||
axes[4].set_xlabel("时间 / s")
|
||||
axes[4].grid(True, alpha=0.3)
|
||||
axes[4].legend(loc="best")
|
||||
|
||||
controller = (
|
||||
str(frame["ControllerName"].iloc[0])
|
||||
if "ControllerName" in frame.columns
|
||||
else "UnknownController"
|
||||
)
|
||||
trajectory = (
|
||||
str(frame["TrajectoryName"].iloc[0])
|
||||
if "TrajectoryName" in frame.columns
|
||||
else csv_path.stem
|
||||
)
|
||||
anomaly_count = int(anomalous.sum())
|
||||
fig.suptitle(
|
||||
"Detour状态估计处理前后对比\n"
|
||||
f"{controller} - {trajectory}，"
|
||||
f"标注异常位置{anomaly_count}帧",
|
||||
fontsize=14,
|
||||
)
|
||||
fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.965))
|
||||
|
||||
if output_directory:
|
||||
destination_directory = Path(output_directory)
|
||||
else:
|
||||
destination_directory = csv_path.parent / "state_estimation_plots"
|
||||
destination_directory.mkdir(parents=True, exist_ok=True)
|
||||
destination = destination_directory / (
|
||||
csv_path.stem + "_state_estimation_comparison.png"
|
||||
)
|
||||
fig.savefig(destination, dpi=220, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
return destination
|
||||
|
||||
|
||||
def discover_csv_files(arguments: list[str]) -> list[Path]:
|
||||
"""解析文件或目录；目录会被递归展开为全部轨迹CSV。"""
|
||||
input_paths = (
|
||||
[Path(argument).resolve() for argument in arguments]
|
||||
if arguments
|
||||
else [Path(__file__).resolve().parent]
|
||||
)
|
||||
csv_files: set[Path] = set()
|
||||
|
||||
for input_path in input_paths:
|
||||
if input_path.is_file():
|
||||
if input_path.suffix.lower() == ".csv":
|
||||
csv_files.add(input_path)
|
||||
continue
|
||||
|
||||
if input_path.is_dir():
|
||||
csv_files.update(
|
||||
path.resolve()
|
||||
for path in input_path.rglob("*.csv")
|
||||
if not any(
|
||||
part.startswith("state_estimation_plots")
|
||||
for part in path.parts
|
||||
)
|
||||
)
|
||||
continue
|
||||
|
||||
raise FileNotFoundError(
|
||||
f"输入文件或目录不存在：{input_path}"
|
||||
)
|
||||
|
||||
return sorted(csv_files)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
"""解析命令行并批量生成Detour状态估计对比图。"""
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="比较原始Detour与当前C#在线状态估计算法。"
|
||||
)
|
||||
parser.add_argument(
|
||||
"files",
|
||||
nargs="*",
|
||||
help="一个或多个轨迹实验CSV或包含CSV的目录",
|
||||
)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
parser.add_argument("--linear-tau", type=float, default=0.15)
|
||||
parser.add_argument("--angular-tau", type=float, default=0.20)
|
||||
parser.add_argument("--maximum-linear-speed", type=float, default=1.20)
|
||||
parser.add_argument("--maximum-angular-speed-deg", type=float, default=45.0)
|
||||
parser.add_argument("--position-jump-margin-mm", type=float, default=30.0)
|
||||
parser.add_argument("--heading-jump-margin-deg", type=float, default=5.0)
|
||||
parser.add_argument(
|
||||
"--annotation-position-residual-mm",
|
||||
type=float,
|
||||
default=40.0,
|
||||
help="只用于图中红色异常位置标注的预测位置残差阈值",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--annotation-heading-residual-deg",
|
||||
type=float,
|
||||
default=5.0,
|
||||
help="只用于图中红色异常位置标注的预测航向残差阈值",
|
||||
)
|
||||
parser.add_argument("--stationary-seconds", type=float, default=0.35)
|
||||
args = parser.parse_args()
|
||||
|
||||
processed_count = 0
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
try:
|
||||
frame = load_csv(csv_path)
|
||||
result, counts = simulate(frame, args)
|
||||
destination = plot_comparison(
|
||||
csv_path,
|
||||
frame,
|
||||
result,
|
||||
counts,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(
|
||||
f"{csv_path.name}: "
|
||||
f"重复帧={counts['duplicate']}，"
|
||||
f"异常位置={counts['visual_anomaly']}"
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
processed_count += 1
|
||||
except Exception as exception:
|
||||
print(f"跳过{csv_path.name}：{exception}")
|
||||
|
||||
if processed_count == 0:
|
||||
raise SystemExit("没有找到包含有效Detour字段的轨迹实验CSV。")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,365 @@
|
||||
需要处理，而且对横向误差、航向误差和速度估计都会有明显影响。检测本身不难，困难的是区分：
|
||||
|
||||
```text
|
||||
单帧错误定位
|
||||
持续性的定位重定位/地图修正
|
||||
车辆真实的快速运动
|
||||
```
|
||||
|
||||
对你这种低速停车机器人，可以先采用一套偏安全、容易验证的处理。
|
||||
|
||||
## 定位跳变会造成什么影响
|
||||
|
||||
假设Detour在50ms内突然跳了10cm：
|
||||
|
||||
\[
|
||||
v=\frac{0.1}{0.05}=2m/s
|
||||
\]
|
||||
|
||||
实际车辆可能只有 `0.3m/s`，但差分速度会产生 `2m/s` 的尖峰。
|
||||
|
||||
对控制器还有三个直接影响：
|
||||
|
||||
- 横向位置跳10cm，横向误差可能瞬间变化10cm。
|
||||
- 航向跳5°，航向误差会瞬间变化5°。
|
||||
- 全局轨迹投影可能跳到另一条临近或相交的轨迹线段。
|
||||
|
||||
Stanley在低速时尤其敏感：
|
||||
|
||||
\[
|
||||
\delta =
|
||||
e_\theta+
|
||||
\arctan\left(\frac{k e_y}{v+\varepsilon}\right)
|
||||
\]
|
||||
|
||||
低速时分母较小，横向误差突然变大，会产生很大的转向指令。因此不能完全不处理。
|
||||
|
||||
## 不建议直接低通掉定位跳变
|
||||
|
||||
不要简单地对Detour位置做强低通：
|
||||
|
||||
```text
|
||||
错误位置跳变
|
||||
→ 低通缓慢跟过去
|
||||
```
|
||||
|
||||
这样虽然曲线看起来平滑，但控制器会在一段时间内使用滞后、虚构的位置，可能更加危险。
|
||||
|
||||
更好的做法是：
|
||||
|
||||
```text
|
||||
检测跳变
|
||||
→ 暂时不把该帧用于速度差分和控制
|
||||
→ 观察后续定位
|
||||
→ 判断是单帧异常还是持续重定位
|
||||
```
|
||||
|
||||
## 第一层：运动学合理性检查
|
||||
|
||||
将当前Detour位姿和上一次接受的位姿比较。
|
||||
|
||||
位置变化：
|
||||
|
||||
\[
|
||||
\Delta p=\sqrt{\Delta x^2+\Delta y^2}
|
||||
\]
|
||||
|
||||
航向变化：
|
||||
|
||||
\[
|
||||
\Delta\theta=
|
||||
|\operatorname{ShortestAngleDifference}|
|
||||
\]
|
||||
|
||||
允许的最大变化量可以按照车辆物理能力计算：
|
||||
|
||||
```csharp
|
||||
var maximumAllowedDistance =
|
||||
maximumLinearSpeedMetersPerSecond *
|
||||
deltaTimeSeconds +
|
||||
positionJumpMarginMeters;
|
||||
|
||||
var maximumAllowedHeadingChange =
|
||||
maximumAngularSpeedRadiansPerSecond *
|
||||
deltaTimeSeconds +
|
||||
headingJumpMarginRadians;
|
||||
```
|
||||
|
||||
然后判断：
|
||||
|
||||
```csharp
|
||||
var positionJump =
|
||||
displacementMeters >
|
||||
maximumAllowedDistance;
|
||||
|
||||
var headingJump =
|
||||
headingChangeRadians >
|
||||
maximumAllowedHeadingChange;
|
||||
```
|
||||
|
||||
你当前小车最高约 `1.2m/s`，假设Detour更新周期为50ms：
|
||||
|
||||
```text
|
||||
物理最大位移约为：
|
||||
1.2 × 0.05 = 0.06m
|
||||
```
|
||||
|
||||
初期可以额外留出约 `0.03～0.05m` 的定位余量。不过这些阈值最终应根据实际Detour数据确定，不建议永久写死。
|
||||
|
||||
## 第二层：不要立即接受异常帧
|
||||
|
||||
检测到一个异常帧时，不要立刻改变车辆状态：
|
||||
|
||||
```text
|
||||
上一正常位置 A
|
||||
异常位置 B
|
||||
下一帧又回到 A 附近
|
||||
```
|
||||
|
||||
这种情况说明B很可能是单帧异常，应直接丢弃。
|
||||
|
||||
如果后续连续若干帧都稳定在B附近：
|
||||
|
||||
```text
|
||||
A → B → B附近 → B附近
|
||||
```
|
||||
|
||||
这更可能是Detour发生了持续性的重定位。
|
||||
|
||||
可以使用：
|
||||
|
||||
```text
|
||||
连续2～3个新定位帧相互一致
|
||||
```
|
||||
|
||||
作为重新接受定位的条件。
|
||||
|
||||
## 第三层：重定位后必须重置速度估计
|
||||
|
||||
如果确认新的定位是持续有效的，不能用：
|
||||
|
||||
```text
|
||||
新位置B - 旧位置A
|
||||
```
|
||||
|
||||
计算速度，因为A到B是定位修正，不是车辆真实运动。
|
||||
|
||||
正确处理是：
|
||||
|
||||
```csharp
|
||||
_velocityEstimator.Reset(
|
||||
newPose,
|
||||
currentTimestamp);
|
||||
```
|
||||
|
||||
这会：
|
||||
|
||||
- 将新位姿作为新的差分起点。
|
||||
- 清除之前的速度历史。
|
||||
- 重置三个低通滤波器。
|
||||
- 将速度暂时标记为无效或零。
|
||||
- 等下一次正常Detour更新后重新开始估计。
|
||||
|
||||
## 第四层：控制器应该如何响应
|
||||
|
||||
对于实车轨迹跟踪，建议状态分为：
|
||||
|
||||
```text
|
||||
Valid 正常定位，可以控制
|
||||
Suspected 检测到疑似跳变
|
||||
Reacquiring 正在确认新的定位
|
||||
Stale 定位长时间没有更新
|
||||
```
|
||||
|
||||
你的第一版不一定需要单独增加复杂枚举，但控制行为至少应该满足：
|
||||
|
||||
```text
|
||||
正常：
|
||||
继续轨迹跟踪
|
||||
|
||||
疑似单帧跳变：
|
||||
不使用异常帧更新状态
|
||||
短时间保持上一状态
|
||||
|
||||
连续异常或定位超时：
|
||||
停车，不继续使用旧状态运动
|
||||
|
||||
确认重定位：
|
||||
接受新位姿
|
||||
重置速度估计
|
||||
重新执行轨迹投影
|
||||
确认稳定后恢复控制
|
||||
```
|
||||
|
||||
停车机器人速度低、场地有限，定位连续异常时停车比盲目继续跟踪更合适。
|
||||
|
||||
## 还需要限制轨迹投影进度
|
||||
|
||||
即使Detour跳变检测做了，轨迹投影也最好增加进度保护。
|
||||
|
||||
当前投影器在整条轨迹上找最近点，如果轨迹自交，车辆可能从：
|
||||
|
||||
```text
|
||||
当前第20段
|
||||
```
|
||||
|
||||
突然投影到：
|
||||
|
||||
```text
|
||||
第80段
|
||||
```
|
||||
|
||||
后续可以增加一个 `TrajectoryProgressTracker`：
|
||||
|
||||
```text
|
||||
正常时只在上次线段索引附近搜索
|
||||
例如 [previousIndex - 5, previousIndex + 20]
|
||||
|
||||
定位重定位后：
|
||||
重新执行一次全局搜索
|
||||
```
|
||||
|
||||
同时限制单周期允许的弧长进度变化：
|
||||
|
||||
\[
|
||||
|\Delta s|
|
||||
\leq
|
||||
v_{\max}\Delta t+\text{margin}
|
||||
\]
|
||||
|
||||
这样可以避免定位轻微波动导致轨迹进度突然跳段。
|
||||
|
||||
## 在五个文件中的分工
|
||||
|
||||
```text
|
||||
DetourVehicleStateProvider
|
||||
├─ 检查数值有效性
|
||||
├─ 检查定位是否过期
|
||||
├─ 检测重复帧
|
||||
├─ 检测位置/航向跳变
|
||||
├─ 暂存疑似新定位
|
||||
└─ 确认重定位后触发Reset
|
||||
|
||||
VelocityEstimator2D
|
||||
├─ 正常位姿差分
|
||||
├─ 真实时间差计算
|
||||
├─ 速度滤波
|
||||
└─ 提供Reset()
|
||||
|
||||
FirstOrderLowPassFilter
|
||||
└─ 提供Update()和Reset()
|
||||
|
||||
VehicleState
|
||||
├─ 保存位姿和速度
|
||||
└─ 表示速度估计是否有效
|
||||
|
||||
IVehicleStateProvider
|
||||
└─ 向控制器返回有效状态或读取失败
|
||||
```
|
||||
|
||||
因此定位跳变的主要逻辑放在 `DetourVehicleStateProvider`，而不是低通滤波器。
|
||||
|
||||
第一版建议先实现：
|
||||
|
||||
1. 非有限值检查。
|
||||
2. 重复帧识别。
|
||||
3. 基于最大速度和最大角速度的跳变检测。
|
||||
4. 单帧异常拒绝。
|
||||
5. 连续2～3帧确认重定位。
|
||||
6. 重定位后重置速度估计。
|
||||
7. 长时间定位无效时通知控制器停车。
|
||||
|
||||
这套方案足以保护当前Stanley和后续纵向控制，不需要现在就修改SLAM源码。
|
||||
|
||||
|
||||
已经给 [DetourVehicleStateProvider.cs](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:233) 增加了基于上一有效速度的预测残差检测。
|
||||
|
||||
现在采用两级判断：
|
||||
|
||||
```text
|
||||
第一级：绝对物理运动边界
|
||||
第二级：上一状态恒速预测残差
|
||||
```
|
||||
|
||||
预测公式为：
|
||||
|
||||
\[
|
||||
x_{pred}=x_{last}+v_x\Delta t
|
||||
\]
|
||||
|
||||
\[
|
||||
y_{pred}=y_{last}+v_y\Delta t
|
||||
\]
|
||||
|
||||
\[
|
||||
\theta_{pred}=
|
||||
\theta_{last}+\omega\Delta t
|
||||
\]
|
||||
|
||||
实测残差为：
|
||||
|
||||
\[
|
||||
e_p=
|
||||
\sqrt{
|
||||
(x_{measured}-x_{pred})^2+
|
||||
(y_{measured}-y_{pred})^2
|
||||
}
|
||||
\]
|
||||
|
||||
以及：
|
||||
|
||||
\[
|
||||
e_\theta=
|
||||
\operatorname{ShortestDifference}
|
||||
(\theta_{measured},\theta_{pred})
|
||||
\]
|
||||
|
||||
对应实现在：
|
||||
|
||||
- [IsPredictionResidualPlausible()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:536)
|
||||
- [PredictPoseInWorld()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:573)
|
||||
|
||||
默认阈值为：
|
||||
|
||||
```csharp
|
||||
位置预测残差：0.04m
|
||||
航向预测残差：5°
|
||||
```
|
||||
|
||||
没有采用最初考虑的30mm，是因为用现有数据离线统计后发现：
|
||||
|
||||
| 位置残差阈值 | 触发次数 | 占有效更新比例 |
|
||||
|---:|---:|---:|
|
||||
| 30mm | 55次 | 1.24% |
|
||||
| 35mm | 28次 | 0.63% |
|
||||
| 40mm | 13次 | 0.29% |
|
||||
| 45mm | 7次 | 0.16% |
|
||||
| 50mm | 4次 | 0.09% |
|
||||
|
||||
30mm容易把正常Detour波动也判成异常；40mm仍能识别现有数据中约49mm和66mm的明显跳变，同时误触发风险更低。
|
||||
|
||||
当前判定逻辑：
|
||||
|
||||
```csharp
|
||||
if (!isWithinPhysicalBoundary ||
|
||||
!isWithinPredictionResidual)
|
||||
{
|
||||
// 进入疑似重定位确认状态。
|
||||
}
|
||||
```
|
||||
|
||||
另外同步修正了跳变恢复逻辑：
|
||||
|
||||
- 发生预测残差跳变时，保存当时的预测位姿。
|
||||
- 后续定位必须回到预测位姿附近，才能认为是单帧异常后的正常恢复。
|
||||
- 不再使用较宽松的绝对物理范围立即放行。
|
||||
- 持续远端定位仍需满足3次观测和0.25秒，才按重定位接管。
|
||||
- 接管后重置速度估计，避免把坐标修正计算成车辆速度。
|
||||
|
||||
第一帧或重定位后的速度还没有建立时，不启用预测残差检测，只使用物理极限检查，避免没有速度基准时误判。
|
||||
|
||||
完整构建结果：
|
||||
|
||||
- CommonUsage：0警告、0错误
|
||||
- MedullaAdapter：0警告、0错误
|
||||
- MultiWheelC：0警告、0错误
|
||||
+26
-21
@@ -1,23 +1,28 @@
|
||||
还可以把底盘的失败原因暴露出来：
|
||||
/// <summary>
|
||||
/// 获取最近一次底盘运动分解失败原因。
|
||||
/// </summary>
|
||||
public string LastFailureReason =>
|
||||
_chassis.LastMotionDecomposeFailureReason;
|
||||
这样调用方可以打印：
|
||||
if (!adapter.Send(command))
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"底盘命令执行失败：{adapter.LastFailureReason}");
|
||||
}
|
||||
|
||||
|
||||
需要注意，Detour 差分速度会有噪声，建议在 Python 中：
|
||||
按固定频率重新采样。
|
||||
对位置做轻微滤波或使用 Savitzky–Golay 求导。
|
||||
再计算速度，避免直接逐点差分产生尖峰。
|
||||
Stanley 和 LQR 必须使用相同的滤波和采样参数。
|
||||
|
||||
|
||||
编译命令：
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1
|
||||
|
||||
|
||||
|
||||
1. TrajectoryPoint.cs 已完成
|
||||
2. Trajectory2D.cs 下一步
|
||||
3. TrajectoryProjection.cs 定义一次投影结果
|
||||
4. TrajectoryProjector.cs 实现连续线段投影
|
||||
5. TrajectoryBuilder.cs 原始离散点转标准轨迹
|
||||
|
||||
1. VehicleState.cs
|
||||
↓
|
||||
2. FirstOrderLowPassFilter.cs
|
||||
↓
|
||||
3. VelocityEstimator2D.cs
|
||||
↓
|
||||
4. IVehicleStateProvider.cs
|
||||
↓
|
||||
5. DetourVehicleStateProvider.cs
|
||||
|
||||
|
||||
|
||||
private const double LinearVelocityFilterTimeConstantSeconds =
|
||||
0.10;
|
||||
|
||||
private const double AngularVelocityFilterTimeConstantSeconds =
|
||||
0.10;
|
||||
Reference in New Issue
Block a user