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

新增倒车以及项目结构优化

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-11 17:06:26 +08:00
parent a59499e638
commit 33a33af710
41 changed files with 817 additions and 2654 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Movements/ClampToTarget.cs
-91
View File
@@ -1,91 +0,0 @@
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;
}
}
}
}
MultiWheelC/Movements/CrabMotionFrameTracker.cs
-817
View File
@@ -1,817 +0,0 @@
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MyParking.Shared;
using System;
using System.Collections.Generic;
using System.Numerics;
namespace MultiWheelC
{
// C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
public sealed class CrabMotionFrameTracker : MovementDefinition
{
public enum ReferencePathKind
{
Straight = 0,
LeftArc = 1,
SCurve = 2
}
public enum ChassisCommandBackend
{
SendXYThSpeed = 0,
SendMotion = 1
}
public ReferencePathKind PathKind;
public ChassisCommandBackend CommandBackend =
ChassisCommandBackend.SendMotion;
public Vector2 StartPosition;
public double InitialBodyYawRadians;
public float LengthMillimeters = 4000f;
public float RadiusMillimeters = 2000f;
public float SCurveLateralOffsetMillimeters = 400f;
public double ArcSweepRadians = Math.PI / 2.0;
public float CruiseSpeed = 0.2f;
public float SlowDistanceMillimeters = 600f;
public float FinishDistanceMillimeters = 30f;
public float MinimumSpeed = 0.04f;
public double LateralGainPerSecond = 0.8;
public double MaximumLateralCorrection = 0.12;
public double HeadingGainPerSecond = 1.5;
public double MaximumAngularSpeedRadiansPerSecond =
AngleMath.DegreesToRadians(30.0);
public double MaximumVirtualSteeringRadians =
AngleMath.DegreesToRadians(30.0);
public float WheelAlignmentToleranceDegrees = 2f;
public float WheelAlignmentStableSeconds = 0.3f;
public float WheelAlignmentTimeoutSeconds = 10f;
public float TrackingTimeoutSeconds = 60f;
public Action<float, float, float> CommandObserver;
// 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
private double _lastSCurveProgress;
public override IEnumerable<bool> Get()
{
ValidateParameters();
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
adapter.ResetToBodyFrame();
var lastCommandTime = DateTime.Now;
try
{
// 模式切换阶段只转舵轮，驱动速度始终保持为零。
var alignmentStarted = DateTime.Now;
DateTime? stableSince = null;
while (true)
{
if (!adapter.PrepareParallelDirection(
MotionFrameYawInBodyRadians))
throw new InvalidOperationException(
"无法生成运动坐标系对应的舵轮准备姿态。");
var aligned =
adapter.AreParallelWheelsAligned(
MotionFrameYawInBodyRadians,
AngleMath.DegreesToRadians(
WheelAlignmentToleranceDegrees));
if (aligned)
{
if (stableSince == null)
stableSince = DateTime.Now;
if ((DateTime.Now - stableSince.Value)
.TotalSeconds >=
WheelAlignmentStableSeconds)
break;
}
else
{
stableSince = null;
}
if ((DateTime.Now - alignmentStarted)
.TotalSeconds >
WheelAlignmentTimeoutSeconds)
throw new TimeoutException(
"舵轮在限定时间内未稳定到达运动坐标系初始方向。");
yield return true;
}
if (CommandBackend ==
ChassisCommandBackend.SendMotion)
{
// 舵轮已按真实机械角度完成预对齐；
// 现在由Shared适配层激活SendMotion虚拟运动坐标系。
adapter.ActivateMotionFrame(
MotionFrameYawInBodyRadians);
}
var trackingStarted = DateTime.Now;
while (true)
{
if ((DateTime.Now - trackingStarted)
.TotalSeconds >
TrackingTimeoutSeconds)
throw new TimeoutException(
"蟹行轨迹在限定时间内未完成。");
var location =
DetourInterface.getCartLocation();
if (!IsFinite(location.x) ||
!IsFinite(location.y) ||
!IsFinite(location.th))
throw new InvalidOperationException(
"蟹行轨迹测试期间Detour位姿无效。");
var currentPosition = new Vector2(
(float)location.x,
(float)location.y);
var currentBodyYaw =
AngleMath.DegreesToRadians(location.th);
CalculateReference(
currentPosition,
out var tangentYaw,
out var referencePoint,
out var remainingMillimeters,
out var referenceCurvature);
if (remainingMillimeters <=
FinishDistanceMillimeters)
break;
var speed =
CalculateSpeed(remainingMillimeters);
var tangent = new Vector2(
(float)Math.Cos(tangentYaw),
(float)Math.Sin(tangentYaw));
var leftNormal = new Vector2(
-tangent.Y,
tangent.X);
var positionError =
currentPosition - referencePoint;
var lateralErrorMeters =
Vector2.Dot(
positionError,
leftNormal) / 1000.0;
var normalCorrection =
Limit(
-LateralGainPerSecond *
lateralErrorMeters,
MaximumLateralCorrection);
// 先在世界坐标中组合切向速度与横向纠偏速度。
var worldVx =
tangent.X * speed +
leftNormal.X * (float)normalCorrection;
var worldVy =
tangent.Y * speed +
leftNormal.Y * (float)normalCorrection;
// 将世界速度表达为当前蟹行运动坐标系速度。
var motionYaw =
currentBodyYaw +
MotionFrameYawInBodyRadians;
var motionCos = Math.Cos(motionYaw);
var motionSin = Math.Sin(motionYaw);
var vxInMotion =
motionCos * worldVx +
motionSin * worldVy;
var vyInMotion =
-motionSin * worldVx +
motionCos * worldVy;
var desiredBodyYaw =
tangentYaw -
MotionFrameYawInBodyRadians;
var headingError =
AngleMath.ShortestDifferenceRadians(
desiredBodyYaw,
currentBodyYaw);
var omega =
speed * referenceCurvature +
HeadingGainPerSecond * headingError;
omega = Limit(
omega,
MaximumAngularSpeedRadiansPerSecond);
var now = DateTime.Now;
var interval = now - lastCommandTime;
lastCommandTime = now;
bool commandAccepted;
Twist2D bodyTwist;
if (CommandBackend ==
ChassisCommandBackend.SendMotion)
{
// 运动坐标系相对车体系旋转+90°：
// 运动系正向速度会转换成车体系+Y速度。
bodyTwist =
FrameTransform2D
.TransformTwistAtSamePoint(
new Pose2D(
0.0,
0.0,
MotionFrameYawInBodyRadians),
new Twist2D(
vxInMotion,
vyInMotion,
omega));
// 将运动坐标系原点和前后几何控制点处的速度，
// 转换为SendMotion需要的前后轴方向。
var controlPointRadiusMeters =
Math.Max(
chassis.ControlPointRadius /
1000.0,
0.001);
var frontVelocityY =
vyInMotion +
omega *
controlPointRadiusMeters;
var rearVelocityY =
vyInMotion -
omega *
controlPointRadiusMeters;
var frontSteeringRadians =
Math.Atan2(
frontVelocityY,
vxInMotion);
var rearSteeringRadians =
Math.Atan2(
rearVelocityY,
vxInMotion);
// 蟹行测试绕过M层ManualControl并直接调用SendMotion，
// 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
if (IsCrabMotionFrame())
{
var geometryRatio =
adapter.HalfTrackWidthMeters /
adapter.HalfWheelBaseMeters;
frontSteeringRadians =
ConvertToCrabSteering(
frontSteeringRadians,
geometryRatio);
rearSteeringRadians =
ConvertToCrabSteering(
rearSteeringRadians,
geometryRatio);
}
var frontThetaDegrees =
(float)AngleMath.RadiansToDegrees(
frontSteeringRadians);
var rearThetaDegrees =
(float)AngleMath.RadiansToDegrees(
rearSteeringRadians);
var motionSpeed =
(float)Math.Sqrt(
vxInMotion * vxInMotion +
vyInMotion * vyInMotion);
commandAccepted =
chassis.SendMotion(
motionSpeed,
frontThetaDegrees,
rearThetaDegrees,
interval);
}
else if (CommandBackend ==
ChassisCommandBackend
.SendXYThSpeed)
{
// 安全XYTh后端根据舵角误差统一压低驱动轮速。
bodyTwist =
FrameTransform2D
.TransformTwistAtSamePoint(
new Pose2D(
0.0,
0.0,
MotionFrameYawInBodyRadians),
new Twist2D(
vxInMotion,
vyInMotion,
omega));
var command = new ChassisCommand(
PilotDefinition.Self.CarNum,
bodyTwist);
commandAccepted =
adapter.Send(
command,
interval);
}
else
{
throw new InvalidOperationException(
$"不支持的底盘命令后端：{CommandBackend}。");
}
if (!commandAccepted)
throw new InvalidOperationException(
"运动坐标系轨迹底盘解算失败：" +
chassis
.LastMotionDecomposeFailureReason);
CommandObserver?.Invoke(
(float)bodyTwist.VxMetersPerSecond,
(float)bodyTwist.VyMetersPerSecond,
(float)bodyTwist
.OmegaRadiansPerSecond);
yield return true;
}
}
finally
{
adapter.StopImmediately();
if (CommandBackend ==
ChassisCommandBackend.SendMotion)
{
// 测试退出后恢复真实车体坐标系，避免影响后续测试。
adapter.ResetToBodyFrame();
}
CommandObserver?.Invoke(0f, 0f, 0f);
}
yield return false;
}
// 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
private bool IsCrabMotionFrame()
{
return Math.Abs(
AngleMath.ShortestDifferenceRadians(
Math.PI / 2.0,
MotionFrameYawInBodyRadians)) <
1e-6;
}
// 按车体几何比例缩小蟹行转角。
// +90°运动坐标系已经完成方向映射，此处不能再次反号。
private double ConvertToCrabSteering(
double normalSteeringRadians,
double geometryRatio)
{
var crabSteeringRadians =
Math.Atan(
geometryRatio *
Math.Tan(
normalSteeringRadians));
return Limit(
crabSteeringRadians,
MaximumVirtualSteeringRadians);
}
// 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
private void CalculateReference(
Vector2 currentPosition,
out double tangentYaw,
out Vector2 referencePoint,
out float remainingMillimeters,
out double curvaturePerMeter)
{
var initialMotionYaw =
InitialBodyYawRadians +
MotionFrameYawInBodyRadians;
if (PathKind == ReferencePathKind.Straight)
{
var tangent = new Vector2(
(float)Math.Cos(initialMotionYaw),
(float)Math.Sin(initialMotionYaw));
var relative = currentPosition - StartPosition;
var progress =
Vector2.Dot(relative, tangent);
var clampedProgress =
Math.Max(
0f,
Math.Min(progress, LengthMillimeters));
tangentYaw = initialMotionYaw;
referencePoint =
StartPosition +
tangent * clampedProgress;
remainingMillimeters =
Math.Max(
0f,
LengthMillimeters - progress);
curvaturePerMeter = 0.0;
return;
}
if (PathKind == ReferencePathKind.SCurve)
{
CalculateSCurveReference(
currentPosition,
initialMotionYaw,
out tangentYaw,
out referencePoint,
out remainingMillimeters,
out curvaturePerMeter);
return;
}
var center = GetArcCenter();
var startRadialYaw =
initialMotionYaw - Math.PI / 2.0;
var radial = currentPosition - center;
var currentRadialYaw =
Math.Atan2(radial.Y, radial.X);
var progressRadians =
AngleMath.NormalizeRadians(
currentRadialYaw - startRadialYaw);
// 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
if (progressRadians < 0.0)
progressRadians = 0.0;
var clampedProgressRadians =
Math.Min(
progressRadians,
ArcSweepRadians);
var referenceRadialYaw =
startRadialYaw +
clampedProgressRadians;
referencePoint = center + new Vector2(
RadiusMillimeters *
(float)Math.Cos(referenceRadialYaw),
RadiusMillimeters *
(float)Math.Sin(referenceRadialYaw));
tangentYaw =
referenceRadialYaw + Math.PI / 2.0;
remainingMillimeters =
(float)Math.Max(
0.0,
(ArcSweepRadians - progressRadians) *
RadiusMillimeters);
curvaturePerMeter =
1000.0 / RadiusMillimeters;
}
// 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
private void CalculateSCurveReference(
Vector2 currentPosition,
double initialMotionYaw,
out double tangentYaw,
out Vector2 referencePoint,
out float remainingMillimeters,
out double curvaturePerMeter)
{
const int nearestPointSamples = 200;
var searchStart =
Math.Max(
0.0,
_lastSCurveProgress - 0.02);
var bestProgress = _lastSCurveProgress;
var bestDistanceSquared = double.MaxValue;
for (var i = 0;
i <= nearestPointSamples;
i++)
{
var progress =
searchStart +
(1.0 - searchStart) *
i / nearestPointSamples;
EvaluateSCurve(
progress,
out var localPoint,
out _,
out _);
var worldPoint =
LocalPathPointToWorld(
localPoint,
initialMotionYaw);
var distanceSquared =
Vector2.DistanceSquared(
currentPosition,
worldPoint);
if (distanceSquared <
bestDistanceSquared)
{
bestDistanceSquared =
distanceSquared;
bestProgress = progress;
}
}
// 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
_lastSCurveProgress =
Math.Max(
_lastSCurveProgress,
bestProgress);
EvaluateSCurve(
_lastSCurveProgress,
out var bestLocalPoint,
out var firstDerivative,
out var secondDerivative);
referencePoint =
LocalPathPointToWorld(
bestLocalPoint,
initialMotionYaw);
tangentYaw =
initialMotionYaw +
Math.Atan2(
firstDerivative.Y,
firstDerivative.X);
var derivativeMagnitude =
Math.Sqrt(
firstDerivative.X *
firstDerivative.X +
firstDerivative.Y *
firstDerivative.Y);
if (derivativeMagnitude < 1e-6)
{
curvaturePerMeter = 0.0;
}
else
{
// 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
curvaturePerMeter =
(firstDerivative.X *
secondDerivative.Y -
firstDerivative.Y *
secondDerivative.X) *
1000.0 /
Math.Pow(
derivativeMagnitude,
3.0);
}
remainingMillimeters =
ApproximateSCurveRemainingLength(
_lastSCurveProgress);
}
// 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
private void EvaluateSCurve(
double progress,
out Vector2 point,
out Vector2 firstDerivative,
out Vector2 secondDerivative)
{
progress =
Math.Max(
0.0,
Math.Min(progress, 1.0));
Vector2 p0;
Vector2 p1;
Vector2 p2;
Vector2 p3;
double t;
if (progress <= 0.25)
{
t = progress * 4.0;
p0 = new Vector2(0f, 0f);
p1 = new Vector2(
LengthMillimeters / 12f,
0f);
p2 = new Vector2(
LengthMillimeters / 6f,
SCurveLateralOffsetMillimeters);
p3 = new Vector2(
LengthMillimeters * 0.25f,
SCurveLateralOffsetMillimeters);
}
else if (progress <= 0.75)
{
t = (progress - 0.25) * 2.0;
p0 = new Vector2(
LengthMillimeters * 0.25f,
SCurveLateralOffsetMillimeters);
p1 = new Vector2(
LengthMillimeters / 3f,
SCurveLateralOffsetMillimeters);
p2 = new Vector2(
LengthMillimeters * 2f / 3f,
-SCurveLateralOffsetMillimeters);
p3 = new Vector2(
LengthMillimeters * 0.75f,
-SCurveLateralOffsetMillimeters);
}
else
{
t = (progress - 0.75) * 4.0;
p0 = new Vector2(
LengthMillimeters * 0.75f,
-SCurveLateralOffsetMillimeters);
p1 = new Vector2(
LengthMillimeters * 5f / 6f,
-SCurveLateralOffsetMillimeters);
p2 = new Vector2(
LengthMillimeters * 11f / 12f,
0f);
p3 = new Vector2(
LengthMillimeters,
0f);
}
var oneMinusT = 1.0 - t;
point =
p0 * (float)(
oneMinusT *
oneMinusT *
oneMinusT) +
p1 * (float)(
3.0 *
oneMinusT *
oneMinusT *
t) +
p2 * (float)(
3.0 *
oneMinusT *
t *
t) +
p3 * (float)(t * t * t);
firstDerivative =
(p1 - p0) *
(float)(
3.0 *
oneMinusT *
oneMinusT) +
(p2 - p1) *
(float)(
6.0 *
oneMinusT *
t) +
(p3 - p2) *
(float)(3.0 * t * t);
secondDerivative =
(p2 - 2f * p1 + p0) *
(float)(6.0 * oneMinusT) +
(p3 - 2f * p2 + p1) *
(float)(6.0 * t);
}
// 通过分段采样估算从当前S曲线进度到终点的实际弧长。
private float ApproximateSCurveRemainingLength(
double startProgress)
{
const int lengthSamples = 100;
EvaluateSCurve(
startProgress,
out var previousPoint,
out _,
out _);
var length = 0f;
for (var i = 1;
i <= lengthSamples;
i++)
{
var progress =
startProgress +
(1.0 - startProgress) *
i / lengthSamples;
EvaluateSCurve(
progress,
out var point,
out _,
out _);
length +=
Vector2.Distance(
previousPoint,
point);
previousPoint = point;
}
return length;
}
// 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
private Vector2 LocalPathPointToWorld(
Vector2 localPoint,
double initialMotionYaw)
{
var cos =
(float)Math.Cos(initialMotionYaw);
var sin =
(float)Math.Sin(initialMotionYaw);
return StartPosition + new Vector2(
localPoint.X * cos -
localPoint.Y * sin,
localPoint.X * sin +
localPoint.Y * cos);
}
// 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
public Vector2 GetArcCenter()
{
var initialMotionYaw =
InitialBodyYawRadians +
MotionFrameYawInBodyRadians;
return StartPosition + new Vector2(
-RadiusMillimeters *
(float)Math.Sin(initialMotionYaw),
RadiusMillimeters *
(float)Math.Cos(initialMotionYaw));
}
// 获取圆弧测试的理论终点。
public Vector2 GetArcDestination()
{
var initialMotionYaw =
InitialBodyYawRadians +
MotionFrameYawInBodyRadians;
var startRadialYaw =
initialMotionYaw - Math.PI / 2.0;
var endRadialYaw =
startRadialYaw + ArcSweepRadians;
var center = GetArcCenter();
return center + new Vector2(
RadiusMillimeters *
(float)Math.Cos(endRadialYaw),
RadiusMillimeters *
(float)Math.Sin(endRadialYaw));
}
// 根据剩余路径长度生成终点减速速度。
private float CalculateSpeed(
float remainingMillimeters)
{
if (remainingMillimeters >=
SlowDistanceMillimeters)
return CruiseSpeed;
var ratio =
remainingMillimeters /
Math.Max(
SlowDistanceMillimeters,
1f);
return Math.Max(
MinimumSpeed,
CruiseSpeed * ratio);
}
private void ValidateParameters()
{
if (CruiseSpeed <= 0f ||
!IsFinite(CruiseSpeed) ||
LengthMillimeters <= 0f ||
!IsFinite(LengthMillimeters) ||
RadiusMillimeters <= 0f ||
!IsFinite(RadiusMillimeters) ||
SCurveLateralOffsetMillimeters <= 0f ||
!IsFinite(
SCurveLateralOffsetMillimeters) ||
ArcSweepRadians <= 0.0 ||
!IsFinite(ArcSweepRadians) ||
SlowDistanceMillimeters <= 0f ||
!IsFinite(SlowDistanceMillimeters) ||
FinishDistanceMillimeters < 0f ||
!IsFinite(FinishDistanceMillimeters) ||
TrackingTimeoutSeconds <= 0f ||
!IsFinite(TrackingTimeoutSeconds) ||
MaximumVirtualSteeringRadians <= 0.0 ||
MaximumVirtualSteeringRadians >=
Math.PI / 2.0 ||
!IsFinite(
MaximumVirtualSteeringRadians))
throw new ArgumentOutOfRangeException(
"蟹行轨迹测试参数无效。");
}
private static double Limit(
double value,
double absoluteLimit)
{
return Math.Max(
-absoluteLimit,
Math.Min(value, absoluteLimit));
}
private static bool IsFinite(double value)
{
return
!double.IsNaN(value) &&
!double.IsInfinity(value);
}
}
}
MultiWheelC/Movements/DriverMovements.cs
-123
View File
@@ -1,123 +0,0 @@
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;
}
}
}
MultiWheelC/Movements/DstTracker.cs
-55
View File
@@ -1,55 +0,0 @@
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();
}
}
}
}
MultiWheelC/Movements/LineTracking.cs
-178
View File
@@ -1,178 +0,0 @@
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;
}
}
}
MultiWheelC/Movements/MotionPlanExecutor.cs
+49 -29
View File
@@ -2,7 +2,7 @@ using System;
using System.Collections.Generic;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using MDCSToolBox.Commons.Controllers;
using CommonUsage.Chassis;
using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
@@ -81,7 +81,7 @@ namespace MultiWheelC
public IReadOnlyList<MotionPlanSegment> Segments;
/// <summary>
/// 获取或设置所有动作段共享的车辆状态源；为空时使用Detour状态源。
/// 获取或设置所有动作段共享的车辆状态源；为空时组合Detour位姿与电机反馈速度。
/// </summary>
public IVehicleStateProvider StateProvider;
@@ -118,23 +118,26 @@ namespace MultiWheelC
/// </summary>
public override IEnumerable<bool> Get()
{
if (Segments == null || Segments.Count == 0)
var segments = ValidateAndSnapshotSegments();
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
throw new InvalidOperationException(
"组合运动计划至少需要包含一个动作段。");
"当前底盘不是MultiWheelChassis，无法执行组合运动计划。");
}
var stateProvider =
StateProvider ??
new DetourVehicleStateProvider();
ParkingVehicleStateProviderFactory.Create(
chassis);
for (var index = 0;
index < Segments.Count;
index < segments.Count;
index++)
{
var segment = Segments[index] ??
throw new InvalidOperationException(
$"组合运动计划第{index}段为空。");
var segment = segments[index];
SegmentStarted?.Invoke(index, segment);
@@ -186,7 +189,6 @@ namespace MultiWheelC
if (segment is RotateInPlaceMotionPlanSegment rotate)
{
var config = PilotDefinition.Conf;
var movement =
new MultiWheelRotateInPlace
{
@@ -194,25 +196,6 @@ namespace MultiWheelC
(float)AngleMath.RadiansToDegrees(
rotate.TargetYawRadians),
StateProvider = stateProvider,
PidparamsRead = () => new PIDParams
{
Kp = config.InPlaceRotateKp,
Ki = config.InPlaceRotateKi,
Kd = config.InPlaceRotateKd,
DeadZone =
config.InPlaceRotateArriveDeg,
SpeedAccPerSec =
config.InPlaceRotateAcc,
OutputUpperThreshold =
config.InPlaceRotateMaxSpeed,
MaxI = config.InPlaceRotateMaxI
},
MinimumAngularSpeedDegreesPerSecond =
config.InPlaceRotateMinimumSpeed,
WheelAlignmentToleranceDegrees =
config.InPlaceRotateWheelAlignDeg,
RotationTimeoutSeconds =
config.InPlaceRotateTimeoutSec,
CommandAngularSpeedObserver =
commandDegreesPerSecond =>
RotationCommandObserver?.Invoke(
@@ -242,5 +225,42 @@ namespace MultiWheelC
// 所有子动作均已完成后，才向外层DriveTask发送组合计划结束信号。
yield return false;
}
/// <summary>
/// 在车辆动作开始前验证全部动作段，并创建本次执行使用的稳定快照。
/// </summary>
private IReadOnlyList<MotionPlanSegment>
ValidateAndSnapshotSegments()
{
if (Segments == null || Segments.Count == 0)
{
throw new InvalidOperationException(
"组合运动计划至少需要包含一个动作段。");
}
var segments =
new MotionPlanSegment[Segments.Count];
for (var index = 0;
index < Segments.Count;
index++)
{
var segment = Segments[index] ??
throw new InvalidOperationException(
$"组合运动计划第{index}段为空。");
if (!(segment is TrackMotionPlanSegment) &&
!(segment is RotateInPlaceMotionPlanSegment))
{
throw new NotSupportedException(
"组合运动计划不支持动作段类型：" +
$"{segment.GetType().FullName}。");
}
segments[index] = segment;
}
return segments;
}
}
}
MultiWheelC/Movements/PrepareWheelsForward.cs
+47 -9
View File
@@ -9,16 +9,54 @@ using MyParking.Shared;
namespace MultiWheelC
{
// C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
/// <summary>
/// 停车并等待四个舵轮稳定回到车体前向0°。
/// </summary>
public class PrepareWheelsForward : MovementDefinition
{
public float ToleranceDegrees = 2f;
public float StableSeconds = 0.3f;
public float TimeoutSeconds = 10f;
/// <summary>
/// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。
/// </summary>
public float? ToleranceDegrees;
/// <summary>
/// 获取或设置本次动作的稳定确认时间覆盖值，单位为s；为空时读取车辆配置。
/// </summary>
public float? StableSeconds;
/// <summary>
/// 获取或设置本次动作的超时覆盖值，单位为s；为空时读取车辆配置，0表示关闭超时。
/// </summary>
public float? TimeoutSeconds;
public bool Completed { get; private set; }
/// <summary>
/// 读取一次有效配置并等待全部舵轮在容差内稳定保持车体前向0°。
/// </summary>
public override IEnumerable<bool> Get()
{
var config = PilotDefinition.Conf;
var toleranceDegrees =
ToleranceDegrees ??
config.ParkingWheelForwardToleranceDegrees;
var stableSeconds =
StableSeconds ??
config.ParkingWheelForwardStableSeconds;
var timeoutSeconds =
TimeoutSeconds ??
config.ParkingWheelForwardTimeoutSeconds;
NumericGuard.EnsureFiniteNonNegative(
toleranceDegrees,
nameof(ToleranceDegrees));
NumericGuard.EnsureFiniteNonNegative(
stableSeconds,
nameof(StableSeconds));
NumericGuard.EnsureFiniteNonNegative(
timeoutSeconds,
nameof(TimeoutSeconds));
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
@@ -32,7 +70,7 @@ namespace MultiWheelC
PilotDefinition.Self.CarNum);
adapter.ResetToBodyFrame();
var toleranceRadians =
AngleMath.DegreesToRadians(ToleranceDegrees);
AngleMath.DegreesToRadians(toleranceDegrees);
var startTime = DateTime.UtcNow;
DateTime? alignedSince = null;
@@ -59,7 +97,7 @@ namespace MultiWheelC
if ((DateTime.UtcNow -
alignedSince.Value).TotalSeconds >=
StableSeconds)
stableSeconds)
{
Completed = true;
yield break;
@@ -70,12 +108,12 @@ namespace MultiWheelC
alignedSince = null;
}
if (TimeoutSeconds > 0f &&
if (timeoutSeconds > 0f &&
(DateTime.UtcNow - startTime).TotalSeconds >
TimeoutSeconds)
timeoutSeconds)
{
throw new TimeoutException(
$"舵轮回正超过{TimeoutSeconds:F1}s，" +
$"舵轮回正超过{timeoutSeconds:F1}s，" +
"测试已经取消。");
}
MultiWheelC/Movements/RotateInPlace.cs
+135 -39
View File
@@ -11,6 +11,9 @@ using MultiWheelC.StateEstimation;
namespace MultiWheelC
{
/// <summary>
/// 将四个舵轮准备到自转姿态并按世界航向闭环旋转，正常完成后等待舵轮回正。
/// </summary>
public class MultiWheelRotateInPlace : MovementDefinition
{
/// <summary>
@@ -18,23 +21,26 @@ namespace MultiWheelC
/// </summary>
public float AngleTarget;
// 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
// 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
public Func<float> ThetaReader;
public IVehicleStateProvider StateProvider =
new DetourVehicleStateProvider();
public IVehicleStateProvider StateProvider;
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
public MultiWheelChassis Chassis =
PilotDefinition.Chassis as MultiWheelChassis;
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
/// <summary>
/// 获取或设置本次动作的PID参数读取覆盖；为空时读取车辆配置。
/// </summary>
public Func<PIDParams> PidparamsRead;
public PIDController thPid;
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
public Action<float> CommandAngularSpeedObserver;
// 自转前舵轮实际角度允许误差，单位deg。
public float WheelAlignmentToleranceDegrees = 2f;
// 自转前舵轮实际角度允许误差覆盖值，单位deg；为空时读取车辆配置。
public float? WheelAlignmentToleranceDegrees;
// 自转舵轮连续保持到位的时间，单位s。
public float WheelAlignmentStableSeconds = 0.3f;
@@ -42,20 +48,56 @@ namespace MultiWheelC
// 自转舵轮准备超时时间，单位s。
public float WheelAlignmentTimeoutSeconds = 10f;
// 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
public float MinimumAngularSpeedDegreesPerSecond = 1f;
// 航向尚未到位时允许下发的最小有效角速度覆盖值，单位deg/s；为空时读取车辆配置。
public float? MinimumAngularSpeedDegreesPerSecond;
// 舵轮到位后执行航向闭环允许的最长时间，单位s。
public float RotationTimeoutSeconds = 15f;
// 舵轮到位后执行航向闭环允许的最长时间覆盖值，单位s；为空时读取车辆配置。
public float? RotationTimeoutSeconds;
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
/// <summary>
/// 读取一次有效配置，闭环旋转到目标航向并在正常完成后等待舵轮稳定回正。
/// </summary>
public override IEnumerable<bool> Get()
{
if (Chassis == null)
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
ValidateParameters();
var config = PilotDefinition.Conf;
var pidParameters =
PidparamsRead == null
? new PIDParams
{
Kp = config.InPlaceRotateKp,
Ki = config.InPlaceRotateKi,
Kd = config.InPlaceRotateKd,
MaxI = config.InPlaceRotateMaxI,
DeadZone = config.InPlaceRotateArriveDeg,
SpeedAccPerSec = config.InPlaceRotateAcc,
OutputUpperThreshold =
config.InPlaceRotateMaxSpeed
}
: PidparamsRead();
var wheelAlignmentToleranceDegrees =
WheelAlignmentToleranceDegrees ??
config.InPlaceRotateWheelAlignDeg;
var minimumAngularSpeedDegreesPerSecond =
MinimumAngularSpeedDegreesPerSecond ??
config.InPlaceRotateMinimumSpeed;
var rotationTimeoutSeconds =
RotationTimeoutSeconds ??
config.InPlaceRotateTimeoutSec;
var stateProvider =
StateProvider ??
ParkingVehicleStateProviderFactory.Create(
Chassis,
config);
ValidateParameters(
pidParameters,
wheelAlignmentToleranceDegrees,
minimumAngularSpeedDegreesPerSecond,
rotationTimeoutSeconds);
var adapter = new MultiWheelChassisAdapter(
Chassis,
@@ -70,7 +112,7 @@ namespace MultiWheelC
{
if (!adapter.PrepareSpin(
alignmentToleranceDegrees:
WheelAlignmentToleranceDegrees))
wheelAlignmentToleranceDegrees))
throw new InvalidOperationException(
"无法生成原地自转舵轮目标：" +
adapter.LastFailureReason);
@@ -101,7 +143,7 @@ namespace MultiWheelC
var alignmentToleranceRadians =
AngleMath.DegreesToRadians(
WheelAlignmentToleranceDegrees);
wheelAlignmentToleranceDegrees);
if (!adapter.AdoptPreparedSpinForXYTh(
alignmentToleranceRadians))
{
@@ -112,14 +154,20 @@ namespace MultiWheelC
var targetAngle =
(float)AngleMath.NormalizeDegrees(AngleTarget);
var p = PidparamsRead();
var currentAngle = ReadCurrentAngleDegrees();
var currentAngle =
ReadCurrentAngleDegrees(stateProvider);
var cachedCurrentAngle = currentAngle;
thPid = new PIDController(
() => cachedCurrentAngle,
p.Kp);
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
p.OutputUpperThreshold, p.SpeedAccPerSec);
pidParameters.Kp);
thPid.ChangeParameters(
pidParameters.Kp,
pidParameters.Ki,
pidParameters.Kd,
pidParameters.MaxI,
pidParameters.DeadZone,
pidParameters.OutputUpperThreshold,
pidParameters.SpeedAccPerSec);
var lastCommandTime = DateTime.Now;
var rotationStarted = DateTime.Now;
@@ -127,13 +175,14 @@ namespace MultiWheelC
{
if ((DateTime.Now - rotationStarted)
.TotalSeconds >
RotationTimeoutSeconds)
rotationTimeoutSeconds)
{
throw new TimeoutException(
$"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
$"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。");
}
currentAngle = ReadCurrentAngleDegrees();
currentAngle =
ReadCurrentAngleDegrees(stateProvider);
cachedCurrentAngle = currentAngle;
var s = thPid.GetResponse(targetAngle, true);
var angleErrorDegrees =
@@ -145,7 +194,7 @@ namespace MultiWheelC
// PID进入到位死区后等待其0.3s稳定确认；等待期间
// 只清零驱动速度，不清除已经准备好的自转舵角状态。
if (Math.Abs(angleErrorDegrees) <=
p.DeadZone)
pidParameters.DeadZone)
{
CommandAngularSpeedObserver?.Invoke(0f);
adapter
@@ -162,10 +211,10 @@ namespace MultiWheelC
// 避免接近目标时反复出现微小命令但车辆实际不动。
if (Math.Abs(s) > 1e-6f &&
Math.Abs(s) <
MinimumAngularSpeedDegreesPerSecond)
minimumAngularSpeedDegreesPerSecond)
{
s = Math.Sign(angleErrorDegrees) *
MinimumAngularSpeedDegreesPerSecond;
minimumAngularSpeedDegreesPerSecond;
}
// PID加速限制在首周期可能暂时输出零；此时保留
@@ -204,7 +253,30 @@ namespace MultiWheelC
yield return true;
}
Console.WriteLine($"final rotate to {targetAngle}");
CommandAngularSpeedObserver?.Invoke(0f);
adapter.StopXYThDrivePreserveSteeringState();
// 航向正常到位后复用统一回正动作；异常或取消会直接进入finally停车。
var wheelPreparation =
new PrepareWheelsForward();
foreach (var keepRunning in wheelPreparation.Get())
{
if (!keepRunning)
{
break;
}
yield return true;
}
if (!wheelPreparation.Completed)
{
throw new InvalidOperationException(
"原地自转完成后舵轮未能稳定回到车头方向。");
}
Console.WriteLine(
$"final rotate to {targetAngle}, wheels forward");
}
finally
{
@@ -216,10 +288,14 @@ namespace MultiWheelC
/// <summary>
/// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
/// </summary>
private void ValidateParameters()
private void ValidateParameters(
PIDParams pidParameters,
float wheelAlignmentToleranceDegrees,
float minimumAngularSpeedDegreesPerSecond,
float rotationTimeoutSeconds)
{
EnsureFinitePositive(
WheelAlignmentToleranceDegrees,
wheelAlignmentToleranceDegrees,
nameof(WheelAlignmentToleranceDegrees),
allowZero: true);
EnsureFinitePositive(
@@ -230,13 +306,12 @@ namespace MultiWheelC
WheelAlignmentTimeoutSeconds,
nameof(WheelAlignmentTimeoutSeconds));
EnsureFinitePositive(
MinimumAngularSpeedDegreesPerSecond,
minimumAngularSpeedDegreesPerSecond,
nameof(MinimumAngularSpeedDegreesPerSecond));
EnsureFinitePositive(
RotationTimeoutSeconds,
rotationTimeoutSeconds,
nameof(RotationTimeoutSeconds));
var pidParameters = PidparamsRead();
if (pidParameters == null)
{
throw new InvalidOperationException(
@@ -256,7 +331,7 @@ namespace MultiWheelC
pidParameters.Kp,
"PidparamsRead.Kp");
if (MinimumAngularSpeedDegreesPerSecond >
if (minimumAngularSpeedDegreesPerSecond >
pidParameters.OutputUpperThreshold)
{
throw new InvalidOperationException(
@@ -267,7 +342,8 @@ namespace MultiWheelC
/// <summary>
/// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
/// </summary>
private float ReadCurrentAngleDegrees()
private float ReadCurrentAngleDegrees(
IVehicleStateProvider stateProvider)
{
if (ThetaReader != null)
{
@@ -283,20 +359,40 @@ namespace MultiWheelC
angleDegrees);
}
if (StateProvider == null ||
!StateProvider.TryGetState(out var state))
if (stateProvider == null ||
!stateProvider.TryGetState(out var state))
{
throw new InvalidOperationException(
"无法从Detour状态源读取有效车辆航向。" +
(StateProvider is DetourVehicleStateProvider provider
? provider.LastFailureReason
: ""));
GetStateProviderFailureReason(
stateProvider));
}
return (float)AngleMath.RadiansToDegrees(
state.PoseInWorld.YawRadians);
}
/// <summary>
/// 获取已知停车状态源最近一次失败原因，未知实现返回空字符串。
/// </summary>
private static string GetStateProviderFailureReason(
IVehicleStateProvider stateProvider)
{
if (stateProvider is
WheelFeedbackVehicleStateProvider wheelProvider)
{
return wheelProvider.LastFailureReason;
}
if (stateProvider is
DetourVehicleStateProvider detourProvider)
{
return detourProvider.LastFailureReason;
}
return string.Empty;
}
/// <summary>
/// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
/// </summary>
MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+174 -77
View File
@@ -4,6 +4,7 @@ using System.Diagnostics;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.Control.Execution;
using MultiWheelC.Control.Lateral;
@@ -26,120 +27,120 @@ namespace MultiWheelC
public Trajectory2D Trajectory;
/// <summary>
/// 获取或设置本次动作使用的车辆状态源；为空时自动创建Detour状态源。
/// 获取或设置本次动作使用的车辆状态源；为空时组合Detour位姿与电机反馈速度。
/// </summary>
public IVehicleStateProvider StateProvider;
/// <summary>
/// 获取或设置横向控制器创建委托；参数为车辆控制点半径（m），为空时使用配置化Stanley控制器。
/// </summary>
public Func<double, ILateralController>
LateralControllerFactory;
/// <summary>
/// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
/// </summary>
public Action<ParkingGeometricController> CycleObserver;
/// <summary>
/// Stanley横向误差增益，单位为1/s。
/// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
/// </summary>
public double StanleyCrossTrackGainPerSecond = 0.4;
public double? StanleyCrossTrackGainPerSecond;
/// <summary>
/// Stanley航向误差增益。
/// 获取或设置本次动作的Stanley航向误差增益覆盖值；为空时读取车辆配置。
/// </summary>
public double StanleyHeadingErrorGain = 1.0;
public double? StanleyHeadingErrorGain;
/// <summary>
/// Stanley低速分母保护速度，单位为m/s。
/// 获取或设置本次动作的Stanley低速分母保护速度覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double StanleyMinimumSpeedMetersPerSecond = 0.15;
public double? StanleyMinimumSpeedMetersPerSecond;
/// <summary>
/// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
/// 获取或设置本次动作是否使用实际纵向速度的覆盖值；为空时读取车辆配置。
/// </summary>
public bool StanleyUsesActualSpeed = true;
public bool? StanleyUsesActualSpeed;
/// <summary>
/// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
/// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
public double MaximumCrossTrackCorrectionRadians =
AngleMath.DegreesToRadians(10.0);
public double? MaximumCrossTrackCorrectionRadians;
/// <summary>
/// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
/// 获取或设置本次动作的Stanley航向修正上限覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
public double MaximumHeadingCorrectionRadians =
AngleMath.DegreesToRadians(10.0);
public double? MaximumHeadingCorrectionRadians;
/// <summary>
/// 纵向速度外环比例增益。
/// 获取或设置本次动作的纵向速度比例增益覆盖值；为空时读取车辆配置。
/// </summary>
public double LongitudinalKp = 0.5;
public double? LongitudinalKp;
/// <summary>
/// 纵向速度外环积分增益，单位为1/s。
/// 获取或设置本次动作的纵向速度积分增益覆盖值，单位为1/s；为空时读取车辆配置。
/// </summary>
public double LongitudinalKiPerSecond;
public double? LongitudinalKiPerSecond;
/// <summary>
/// 纵向速度外环微分增益，单位为s。
/// 获取或设置本次动作的纵向速度微分增益覆盖值，单位为s；为空时读取车辆配置。
/// </summary>
public double LongitudinalKdSeconds;
public double? LongitudinalKdSeconds;
/// <summary>
/// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
/// 获取或设置本次动作的纵向积分修正上限覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
public double? MaximumIntegralCorrectionMetersPerSecond;
/// <summary>
/// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
/// 获取或设置本次动作的纵向速度误差死区覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
0.025;
public double? LongitudinalSpeedErrorDeadbandMetersPerSecond;
/// <summary>
/// 底盘纵向命令速度绝对值上限，单位为m/s。
/// 获取或设置本次动作的底盘纵向命令速度上限覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double MaximumCommandSpeedMetersPerSecond = 0.50;
public double? MaximumCommandSpeedMetersPerSecond;
/// <summary>
/// 前后GCP允许的最大转角绝对值，单位为rad。
/// 获取或设置本次动作的GCP转角上限覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
public double MaximumGcpAngleRadians =
AngleMath.DegreesToRadians(45.0);
public double? MaximumGcpAngleRadians;
/// <summary>
/// 前后GCP目标转角最大变化率，单位为rad/s。
/// 获取或设置本次动作的GCP转角变化率上限覆盖值，单位为rad/s；为空时读取车辆配置。
/// </summary>
public double MaximumGcpAngleRateRadiansPerSecond =
AngleMath.DegreesToRadians(15.0);
public double? MaximumGcpAngleRateRadiansPerSecond;
/// <summary>
/// 终点位置和剩余弧长的完成容差，单位为m。
/// 获取或设置本次动作的终点距离容差覆盖值，单位为m；为空时读取车辆配置。
/// </summary>
public double FinishDistanceMeters = 0.03;
public double? FinishDistanceMeters;
/// <summary>
/// 终点停稳判定允许的实际线速度，单位为m/s。
/// 获取或设置本次动作的终点速度容差覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double FinishSpeedMetersPerSecond = 0.02;
public double? FinishSpeedMetersPerSecond;
/// <summary>
/// 终点航向完成容差，单位为rad。
/// 获取或设置本次动作的终点航向容差覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
public double FinishHeadingToleranceRadians =
AngleMath.DegreesToRadians(3.0);
public double? FinishHeadingToleranceRadians;
/// <summary>
/// 终点减速阶段提前读取参考速度的距离，单位为m。
/// 获取或设置本次动作的终点制动预瞄距离覆盖值，单位为m；为空时读取车辆配置。
/// </summary>
public double TerminalBrakingPreviewMeters = 0.02;
public double? TerminalBrakingPreviewMeters;
/// <summary>
/// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
/// 获取或设置本次动作的最大轨迹偏离距离覆盖值，单位为m；为空时读取车辆配置。
/// </summary>
public double MaximumDistanceToTrajectoryMeters = 0.30;
public double? MaximumDistanceToTrajectoryMeters;
/// <summary>
/// 单次轨迹动作允许的最长执行时间，单位为s。
/// 获取或设置本次动作的执行超时覆盖值，单位为s；为空时读取车辆配置。
/// </summary>
public double ExecutionTimeoutSeconds = 120.0;
public double? ExecutionTimeoutSeconds;
/// <summary>
/// 获取本次动作创建的控制器，尚未开始时为空。
@@ -147,11 +148,78 @@ namespace MultiWheelC
public ParkingGeometricController Controller { get; private set; }
/// <summary>
/// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
/// 等待舵轮稳定回正后创建控制器并持续执行，直到轨迹完成、失败或动作被取消。
/// </summary>
public override IEnumerable<bool> Get()
{
ValidateParameters();
var config = PilotDefinition.Conf;
var stanleyCrossTrackGainPerSecond =
StanleyCrossTrackGainPerSecond ??
config.ParkingStanleyCrossTrackGain;
var stanleyHeadingErrorGain =
StanleyHeadingErrorGain ??
config.ParkingStanleyHeadingGain;
var stanleyMinimumSpeedMetersPerSecond =
StanleyMinimumSpeedMetersPerSecond ??
config.ParkingStanleyMinimumSpeed;
var stanleyUsesActualSpeed =
StanleyUsesActualSpeed ??
config.ParkingStanleyUseActualSpeed;
var maximumCrossTrackCorrectionRadians =
MaximumCrossTrackCorrectionRadians ??
AngleMath.DegreesToRadians(
config.ParkingMaximumCrossTrackCorrectionDegrees);
var maximumHeadingCorrectionRadians =
MaximumHeadingCorrectionRadians ??
AngleMath.DegreesToRadians(
config.ParkingMaximumHeadingCorrectionDegrees);
var longitudinalKp =
LongitudinalKp ??
config.ParkingLongitudinalKp;
var longitudinalKiPerSecond =
LongitudinalKiPerSecond ??
config.ParkingLongitudinalKi;
var longitudinalKdSeconds =
LongitudinalKdSeconds ??
config.ParkingLongitudinalKd;
var maximumIntegralCorrectionMetersPerSecond =
MaximumIntegralCorrectionMetersPerSecond ??
config.ParkingMaximumIntegralCorrection;
var maximumCommandSpeedMetersPerSecond =
MaximumCommandSpeedMetersPerSecond ??
config.ParkingMaximumCommandSpeed;
var longitudinalSpeedErrorDeadbandMetersPerSecond =
LongitudinalSpeedErrorDeadbandMetersPerSecond ??
config.ParkingLongitudinalSpeedErrorDeadband;
var maximumGcpAngleRadians =
MaximumGcpAngleRadians ??
AngleMath.DegreesToRadians(
config.ParkingMaximumGcpAngleDegrees);
var maximumGcpAngleRateRadiansPerSecond =
MaximumGcpAngleRateRadiansPerSecond ??
AngleMath.DegreesToRadians(
config.ParkingMaximumGcpAngleRateDegreesPerSecond);
var finishDistanceMeters =
FinishDistanceMeters ??
config.ParkingFinishDistance;
var finishSpeedMetersPerSecond =
FinishSpeedMetersPerSecond ??
config.ParkingFinishSpeed;
var finishHeadingToleranceRadians =
FinishHeadingToleranceRadians ??
AngleMath.DegreesToRadians(
config.ParkingFinishHeadingToleranceDegrees);
var terminalBrakingPreviewMeters =
TerminalBrakingPreviewMeters ??
config.ParkingTerminalBrakingPreview;
var maximumDistanceToTrajectoryMeters =
MaximumDistanceToTrajectoryMeters ??
config.ParkingMaximumDistanceToTrajectory;
var executionTimeoutSeconds =
ExecutionTimeoutSeconds ??
config.ParkingExecutionTimeoutSeconds;
ValidateParameters(executionTimeoutSeconds);
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
@@ -161,6 +229,24 @@ namespace MultiWheelC
"当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
}
var wheelPreparation =
new PrepareWheelsForward();
foreach (var keepRunning in wheelPreparation.Get())
{
if (!keepRunning)
{
break;
}
yield return true;
}
if (!wheelPreparation.Completed)
{
throw new InvalidOperationException(
"轨迹跟踪开始前舵轮未能稳定回到车头方向。");
}
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
@@ -170,34 +256,44 @@ namespace MultiWheelC
var stateProvider =
StateProvider ??
new DetourVehicleStateProvider();
ParkingVehicleStateProviderFactory.Create(
chassis,
config);
var controlPointRadiusMeters =
chassis.ControlPointRadius / 1000.0;
var lateralController =
new StanleyLateralController(
controlPointRadiusMeters,
StanleyCrossTrackGainPerSecond,
StanleyHeadingErrorGain,
StanleyMinimumSpeedMetersPerSecond,
StanleyUsesActualSpeed,
MaximumCrossTrackCorrectionRadians,
MaximumHeadingCorrectionRadians);
LateralControllerFactory == null
? new StanleyLateralController(
controlPointRadiusMeters,
stanleyCrossTrackGainPerSecond,
stanleyHeadingErrorGain,
stanleyMinimumSpeedMetersPerSecond,
stanleyUsesActualSpeed,
maximumCrossTrackCorrectionRadians,
maximumHeadingCorrectionRadians)
: LateralControllerFactory(
controlPointRadiusMeters);
if (lateralController == null)
{
throw new InvalidOperationException(
"横向控制器创建委托不能返回空值。");
}
var longitudinalController =
new PidLongitudinalController(
LongitudinalKp,
LongitudinalKiPerSecond,
LongitudinalKdSeconds,
MaximumIntegralCorrectionMetersPerSecond,
MaximumCommandSpeedMetersPerSecond,
LongitudinalSpeedErrorDeadbandMetersPerSecond);
longitudinalKp,
longitudinalKiPerSecond,
longitudinalKdSeconds,
maximumIntegralCorrectionMetersPerSecond,
maximumCommandSpeedMetersPerSecond,
longitudinalSpeedErrorDeadbandMetersPerSecond);
var gcpAllocator =
new GcpCommandAllocator(
MaximumGcpAngleRadians);
maximumGcpAngleRadians);
var commandExecutor =
new GcpCommandExecutor(
adapter,
MaximumGcpAngleRateRadiansPerSecond);
maximumGcpAngleRateRadiansPerSecond);
Controller = new ParkingGeometricController(
stateProvider,
@@ -205,11 +301,11 @@ namespace MultiWheelC
longitudinalController,
gcpAllocator,
commandExecutor,
FinishDistanceMeters,
FinishSpeedMetersPerSecond,
FinishHeadingToleranceRadians,
MaximumDistanceToTrajectoryMeters,
TerminalBrakingPreviewMeters);
finishDistanceMeters,
finishSpeedMetersPerSecond,
finishHeadingToleranceRadians,
maximumDistanceToTrajectoryMeters,
terminalBrakingPreviewMeters);
var clock = Stopwatch.StartNew();
var previousCycleSeconds =
@@ -221,10 +317,10 @@ namespace MultiWheelC
while (true)
{
if (clock.Elapsed.TotalSeconds >
ExecutionTimeoutSeconds)
executionTimeoutSeconds)
{
throw new TimeoutException(
$"新版轨迹跟踪超过{ExecutionTimeoutSeconds:F1}s仍未完成。");
$"新版轨迹跟踪超过{executionTimeoutSeconds:F1}s仍未完成。");
}
var currentCycleSeconds =
@@ -291,7 +387,8 @@ namespace MultiWheelC
/// <summary>
/// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
/// </summary>
private void ValidateParameters()
private void ValidateParameters(
double executionTimeoutSeconds)
{
if (Trajectory == null)
{
@@ -299,9 +396,9 @@ namespace MultiWheelC
"新版轨迹跟踪动作没有设置Trajectory。");
}
if (double.IsNaN(ExecutionTimeoutSeconds) ||
double.IsInfinity(ExecutionTimeoutSeconds) ||
ExecutionTimeoutSeconds <= 0.0)
if (double.IsNaN(executionTimeoutSeconds) ||
double.IsInfinity(executionTimeoutSeconds) ||
executionTimeoutSeconds <= 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(ExecutionTimeoutSeconds),