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/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>