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-21 17:33:29 +08:00
parent fea2265e2d
commit 0ab409cd2a
33 changed files with 1949 additions and 989 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Movements/RotateInPlace.cs
+144 -13
View File
@@ -12,15 +12,30 @@ using MultiWheelC.StateEstimation;
namespace MultiWheelC
{
/// <summary>
/// 将四个舵轮准备到自转姿态并按世界航向闭环旋转，正常完成后等待舵轮回正。
/// 指定原地自转使用Detour绝对航向或轮组相对角度反馈。
/// </summary>
public enum InPlaceRotationFeedbackMode
{
DetourAbsoluteHeading,
RelativeWheelOdometry
}
/// <summary>
/// 将四个舵轮准备到自转姿态，按所选反馈旋转并在完成后等待舵轮回正。
/// </summary>
public class MultiWheelRotateInPlace : MovementDefinition
{
/// <summary>
/// 旋转目标角度
/// Detour模式表示世界目标航向，轮组模式表示有符号相对旋转角度，单位deg。
/// </summary>
public float AngleTarget;
/// <summary>
/// 获取或设置原地自转反馈模式；默认保持现有Detour绝对航向闭环。
/// </summary>
public InPlaceRotationFeedbackMode FeedbackMode =
InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
// 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
public Func<float> ThetaReader;
@@ -152,10 +167,32 @@ namespace MultiWheelC
adapter.LastFailureReason);
}
var useRelativeWheelOdometry =
FeedbackMode ==
InPlaceRotationFeedbackMode
.RelativeWheelOdometry;
var wheelStateProvider =
useRelativeWheelOdometry
? stateProvider as
WheelFeedbackVehicleStateProvider
: null;
if (useRelativeWheelOdometry &&
wheelStateProvider == null)
{
throw new InvalidOperationException(
"轮组相对角度自转需要" +
"WheelFeedbackVehicleStateProvider。");
}
var targetAngle =
(float)AngleMath.NormalizeDegrees(AngleTarget);
useRelativeWheelOdometry
? AngleTarget
: (float)AngleMath.NormalizeDegrees(
AngleTarget);
var currentAngle =
ReadCurrentAngleDegrees(stateProvider);
useRelativeWheelOdometry
? 0f
: ReadCurrentAngleDegrees(stateProvider);
var cachedCurrentAngle = currentAngle;
thPid = new PIDController(
() => cachedCurrentAngle,
@@ -170,6 +207,10 @@ namespace MultiWheelC
pidParameters.SpeedAccPerSec);
var lastCommandTime = DateTime.Now;
var rotationStarted = DateTime.Now;
var accumulatedWheelAngleRadians = 0.0;
var previousWheelOmegaRadiansPerSecond = 0.0;
var previousWheelTimestampSeconds = 0.0;
var hasPreviousWheelSample = false;
while (true)
{
@@ -181,15 +222,74 @@ namespace MultiWheelC
$"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。");
}
currentAngle =
ReadCurrentAngleDegrees(stateProvider);
if (useRelativeWheelOdometry)
{
if (!wheelStateProvider.TryGetWheelTwist(
out var wheelTwist,
out var wheelTimestampSeconds))
{
CommandAngularSpeedObserver?.Invoke(0f);
adapter
.StopXYThDrivePreserveSteeringState();
if (hasPreviousWheelSample)
{
throw new InvalidOperationException(
"原地自转期间轮组角速度不可用：" +
wheelStateProvider.LastFailureReason);
}
yield return true;
continue;
}
if (hasPreviousWheelSample)
{
var wheelDeltaTimeSeconds =
wheelTimestampSeconds -
previousWheelTimestampSeconds;
if (!NumericGuard.IsFinite(
wheelDeltaTimeSeconds) ||
wheelDeltaTimeSeconds <= 0.0)
{
throw new InvalidOperationException(
"轮组角速度采样时间没有单调递增。");
}
accumulatedWheelAngleRadians +=
0.5 *
(previousWheelOmegaRadiansPerSecond +
wheelTwist
.OmegaRadiansPerSecond) *
wheelDeltaTimeSeconds;
}
previousWheelOmegaRadiansPerSecond =
wheelTwist.OmegaRadiansPerSecond;
previousWheelTimestampSeconds =
wheelTimestampSeconds;
hasPreviousWheelSample = true;
currentAngle =
(float)AngleMath.RadiansToDegrees(
accumulatedWheelAngleRadians);
}
else
{
currentAngle =
ReadCurrentAngleDegrees(stateProvider);
}
cachedCurrentAngle = currentAngle;
var s = thPid.GetResponse(targetAngle, true);
var s = thPid.GetResponse(
targetAngle,
!useRelativeWheelOdometry);
var angleErrorDegrees =
(float)AngleMath
.ShortestDifferenceDegrees(
targetAngle,
currentAngle);
useRelativeWheelOdometry
? targetAngle - currentAngle
: (float)AngleMath
.ShortestDifferenceDegrees(
targetAngle,
currentAngle);
// PID进入到位死区后等待其0.3s稳定确认；等待期间
// 只清零驱动速度，不清除已经准备好的自转舵角状态。
@@ -255,7 +355,8 @@ namespace MultiWheelC
CommandAngularSpeedObserver?.Invoke(0f);
adapter.StopXYThDrivePreserveSteeringState();
if (IsLocalizationRecoveryPending(
if (!useRelativeWheelOdometry &&
IsLocalizationRecoveryPending(
stateProvider))
{
BeginPostRotationPositionRecovery(
@@ -309,7 +410,10 @@ namespace MultiWheelC
}
Console.WriteLine(
$"final rotate to {targetAngle}, wheels forward");
useRelativeWheelOdometry
? "final relative wheel rotate to " +
$"{currentAngle:F2}deg, wheels forward"
: $"final rotate to {targetAngle}, wheels forward");
}
finally
{
@@ -345,6 +449,33 @@ namespace MultiWheelC
rotationTimeoutSeconds,
nameof(RotationTimeoutSeconds));
if (float.IsNaN(AngleTarget) ||
float.IsInfinity(AngleTarget))
{
throw new ArgumentOutOfRangeException(
nameof(AngleTarget),
"原地自转目标角度必须是有限值。");
}
if (!Enum.IsDefined(
typeof(InPlaceRotationFeedbackMode),
FeedbackMode))
{
throw new ArgumentOutOfRangeException(
nameof(FeedbackMode),
"原地自转反馈模式无效。");
}
if (FeedbackMode ==
InPlaceRotationFeedbackMode
.RelativeWheelOdometry &&
Math.Abs(AngleTarget) >= 180f)
{
throw new ArgumentOutOfRangeException(
nameof(AngleTarget),
"轮组相对自转角度必须满足-180° < angle < 180°。");
}
if (pidParameters == null)
{
throw new InvalidOperationException(