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

增加GCP运动学并完善车体平面速度反馈分析

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-14 17:45:17 +08:00
parent 9fe8901c4c
commit d8de901a80
21 changed files with 445 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Control/Allocation/GcpKinematics.cs
+125
View File
@@ -0,0 +1,125 @@
using System;
using MyParking.Shared;
namespace MultiWheelC.Control.Allocation
{
/// <summary>
/// 在对称前后GCP方向命令与车体中心刚体速度之间执行纯几何转换。
/// </summary>
public static class GcpKinematics
{
private const double ParallelDirectionTolerance = 1e-9;
private const double StopSpeedDeadbandMetersPerSecond = 1e-6;
/// <summary>
/// 将有符号中心速度和前后GCP方向转换为真实车体坐标系中的Twist2D。
/// </summary>
public static Twist2D ToBodyTwist(
GcpMotionCommand command,
double controlPointRadiusMeters)
{
NumericGuard.EnsureFinitePositive(
controlPointRadiusMeters,
nameof(controlPointRadiusMeters));
if (Math.Abs(command.SpeedMetersPerSecond) <=
StopSpeedDeadbandMetersPerSecond)
{
return Twist2D.Zero;
}
var frontAngleRadians =
command.FrontAngleRadians;
var rearAngleRadians =
command.RearAngleRadians;
var directionDeterminant =
Math.Sin(
rearAngleRadians -
frontAngleRadians);
if (Math.Abs(directionDeterminant) <=
ParallelDirectionTolerance)
{
var averageDirectionRadians =
Math.Atan2(
Math.Sin(frontAngleRadians) +
Math.Sin(rearAngleRadians),
Math.Cos(frontAngleRadians) +
Math.Cos(rearAngleRadians));
return new Twist2D(
command.SpeedMetersPerSecond *
Math.Cos(averageDirectionRadians),
command.SpeedMetersPerSecond *
Math.Sin(averageDirectionRadians),
0.0);
}
var frontCosine =
Math.Cos(frontAngleRadians);
var frontSine =
Math.Sin(frontAngleRadians);
var rearCosine =
Math.Cos(rearAngleRadians);
var rearSine =
Math.Sin(rearAngleRadians);
// 两个GCP速度方向的法线交点就是瞬时旋转中心，坐标位于真实车体系。
var rotationCenterXMeters =
controlPointRadiusMeters *
(frontCosine * rearSine +
frontSine * rearCosine) /
directionDeterminant;
var rotationCenterYMeters =
-2.0 *
controlPointRadiusMeters *
frontCosine *
rearCosine /
directionDeterminant;
var centerRadiusMeters =
Math.Sqrt(
rotationCenterXMeters *
rotationCenterXMeters +
rotationCenterYMeters *
rotationCenterYMeters);
if (!NumericGuard.IsFinite(centerRadiusMeters) ||
centerRadiusMeters <= 0.0)
{
throw new InvalidOperationException(
"前后GCP方向不能生成有效的车体中心旋转半径。");
}
// 用有符号速度决定绕ICR的实际转向，避免倒车时把同一组GCP轴向解释成反向运动。
var requestedDirectionSign =
Math.Sign(command.SpeedMetersPerSecond);
var positiveAngularFrontVelocityXMetersPerSecond =
rotationCenterYMeters;
var positiveAngularFrontVelocityYMetersPerSecond =
controlPointRadiusMeters -
rotationCenterXMeters;
var frontDirectionAlignment =
positiveAngularFrontVelocityXMetersPerSecond *
requestedDirectionSign *
frontCosine +
positiveAngularFrontVelocityYMetersPerSecond *
requestedDirectionSign *
frontSine;
var omegaSign =
frontDirectionAlignment >= 0.0
? 1.0
: -1.0;
var omegaRadiansPerSecond =
omegaSign *
Math.Abs(command.SpeedMetersPerSecond) /
centerRadiusMeters;
return new Twist2D(
omegaRadiansPerSecond *
rotationCenterYMeters,
-omegaRadiansPerSecond *
rotationCenterXMeters,
omegaRadiansPerSecond);
}
}
}
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+4
View File
@@ -307,6 +307,8 @@ namespace MultiWheelC
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
out var rawWheelBodyVy,
out var filteredWheelBodyVy,
out var wheelVelocityValid))
{
_recorder?.UpdateVelocityDiagnostics(
@@ -314,6 +316,8 @@ namespace MultiWheelC
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
rawWheelBodyVy,
filteredWheelBodyVy,
wheelVelocityValid);
}
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+11
View File
@@ -224,6 +224,9 @@ namespace MultiWheelC
referenceSpeed:
(float)CruiseSpeedMetersPerSecond,
sampleIntervalMs: 50,
referenceMotionFrameYawDegrees:
(float)AngleMath.RadiansToDegrees(
MotionDirectionInBodyRadians),
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
@@ -432,6 +435,8 @@ namespace MultiWheelC
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
out var rawWheelBodyVy,
out var filteredWheelBodyVy,
out var wheelVelocityValid))
{
return;
@@ -442,6 +447,8 @@ namespace MultiWheelC
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
rawWheelBodyVy,
filteredWheelBodyVy,
wheelVelocityValid);
}
@@ -855,6 +862,8 @@ namespace MultiWheelC
out var detourVelocityValid,
out var rawWheelBodyVx,
out var filteredWheelBodyVx,
out var rawWheelBodyVy,
out var filteredWheelBodyVy,
out var wheelVelocityValid))
{
return;
@@ -865,6 +874,8 @@ namespace MultiWheelC
detourVelocityValid,
rawWheelBodyVx,
filteredWheelBodyVx,
rawWheelBodyVy,
filteredWheelBodyVy,
wheelVelocityValid);
}
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+30 -2
View File
@@ -53,12 +53,14 @@ namespace MultiWheelC
public double CurvaturePreviewDistanceMeters;
public double FeedforwardCurvaturePerMeter;
// 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
// 并列保存Detour速度与轮速解算速度，避免StateBodyVx/Vy的数据来源产生歧义。
public bool HasVelocityDiagnostics;
public double DetourEstimatedBodyVxMetersPerSecond;
public bool DetourVelocityEstimateValid;
public double WheelFeedbackRawBodyVxMetersPerSecond;
public double WheelFeedbackFilteredBodyVxMetersPerSecond;
public double WheelFeedbackRawBodyVyMetersPerSecond;
public double WheelFeedbackFilteredBodyVyMetersPerSecond;
public bool WheelFeedbackVelocityEstimateValid;
// 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
@@ -176,6 +178,8 @@ namespace MultiWheelC
private bool _detourVelocityEstimateValid;
private double _wheelFeedbackRawBodyVxMetersPerSecond;
private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
private double _wheelFeedbackRawBodyVyMetersPerSecond;
private double _wheelFeedbackFilteredBodyVyMetersPerSecond;
private bool _wheelFeedbackVelocityEstimateValid;
private bool _hasGcpCommand;
private double _requestedFrontGcpAngleRadians;
@@ -340,13 +344,15 @@ namespace MultiWheelC
}
/// <summary>
/// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
/// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波平面速度。
/// </summary>
public void UpdateVelocityDiagnostics(
double detourEstimatedBodyVxMetersPerSecond,
bool detourVelocityEstimateValid,
double wheelFeedbackRawBodyVxMetersPerSecond,
double wheelFeedbackFilteredBodyVxMetersPerSecond,
double wheelFeedbackRawBodyVyMetersPerSecond,
double wheelFeedbackFilteredBodyVyMetersPerSecond,
bool wheelFeedbackVelocityEstimateValid)
{
lock (_stateSyncRoot)
@@ -359,6 +365,10 @@ namespace MultiWheelC
wheelFeedbackRawBodyVxMetersPerSecond;
_wheelFeedbackFilteredBodyVxMetersPerSecond =
wheelFeedbackFilteredBodyVxMetersPerSecond;
_wheelFeedbackRawBodyVyMetersPerSecond =
wheelFeedbackRawBodyVyMetersPerSecond;
_wheelFeedbackFilteredBodyVyMetersPerSecond =
wheelFeedbackFilteredBodyVyMetersPerSecond;
_wheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid;
_hasVelocityDiagnostics = true;
@@ -543,6 +553,8 @@ namespace MultiWheelC
bool detourVelocityEstimateValid;
double wheelFeedbackRawBodyVxMetersPerSecond;
double wheelFeedbackFilteredBodyVxMetersPerSecond;
double wheelFeedbackRawBodyVyMetersPerSecond;
double wheelFeedbackFilteredBodyVyMetersPerSecond;
bool wheelFeedbackVelocityEstimateValid;
bool hasGcpCommand;
double requestedFrontGcpAngleRadians;
@@ -618,6 +630,10 @@ namespace MultiWheelC
_wheelFeedbackRawBodyVxMetersPerSecond;
wheelFeedbackFilteredBodyVxMetersPerSecond =
_wheelFeedbackFilteredBodyVxMetersPerSecond;
wheelFeedbackRawBodyVyMetersPerSecond =
_wheelFeedbackRawBodyVyMetersPerSecond;
wheelFeedbackFilteredBodyVyMetersPerSecond =
_wheelFeedbackFilteredBodyVyMetersPerSecond;
wheelFeedbackVelocityEstimateValid =
_wheelFeedbackVelocityEstimateValid;
hasGcpCommand = _hasGcpCommand;
@@ -681,6 +697,10 @@ namespace MultiWheelC
wheelFeedbackRawBodyVxMetersPerSecond,
WheelFeedbackFilteredBodyVxMetersPerSecond =
wheelFeedbackFilteredBodyVxMetersPerSecond,
WheelFeedbackRawBodyVyMetersPerSecond =
wheelFeedbackRawBodyVyMetersPerSecond,
WheelFeedbackFilteredBodyVyMetersPerSecond =
wheelFeedbackFilteredBodyVyMetersPerSecond,
WheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid,
HasGcpCommand = hasGcpCommand,
@@ -905,6 +925,8 @@ namespace MultiWheelC
"DetourVelocityEstimateValid," +
"WheelFeedbackRawBodyVxMetersPerSecond," +
"WheelFeedbackFilteredBodyVxMetersPerSecond," +
"WheelFeedbackRawBodyVyMetersPerSecond," +
"WheelFeedbackFilteredBodyVyMetersPerSecond," +
"WheelFeedbackVelocityEstimateValid," +
"HasSteeringDiagnostics," +
"TargetSteerLeftFrontDegrees," +
@@ -1034,6 +1056,12 @@ namespace MultiWheelC
FormatOptional(
sample.HasVelocityDiagnostics,
sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
FormatOptional(
sample.HasVelocityDiagnostics,
sample.WheelFeedbackRawBodyVyMetersPerSecond),
FormatOptional(
sample.HasVelocityDiagnostics,
sample.WheelFeedbackFilteredBodyVyMetersPerSecond),
sample.HasVelocityDiagnostics
? sample.WheelFeedbackVelocityEstimateValid
? "1"
MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
+1 -1
View File
@@ -5,7 +5,7 @@ using MyParking.Shared;
namespace MultiWheelC.StateEstimation
{
/// <summary>
/// 根据车载配置创建Detour位姿过滤与电机反馈纵向速度组合的停车状态源。
/// 根据车载配置创建Detour位姿过滤与电机反馈平面速度组合的停车状态源。
/// </summary>
public static class ParkingVehicleStateProviderFactory
{
MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs
+73 -28
View File
@@ -6,7 +6,7 @@ using System.Diagnostics;
namespace MultiWheelC.StateEstimation
{
/// <summary>
/// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体纵向速度替换Detour差分纵向速度。
/// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体平面速度替换Detour差分线速度。
/// </summary>
public sealed class WheelFeedbackVehicleStateProvider
: IVehicleStateProvider
@@ -19,6 +19,7 @@ namespace MultiWheelC.StateEstimation
private readonly IVehicleStateProvider _poseProvider;
private readonly MultiWheelChassis _chassis;
private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
private readonly FirstOrderLowPassFilter _lateralSpeedFilter;
private bool _hasPreviousTimestamp;
private double _previousTimestampSeconds;
@@ -27,10 +28,12 @@ namespace MultiWheelC.StateEstimation
private bool _latestDetourVelocityValid;
private double _latestRawWheelBodyVxMetersPerSecond;
private double _latestFilteredWheelBodyVxMetersPerSecond;
private double _latestRawWheelBodyVyMetersPerSecond;
private double _latestFilteredWheelBodyVyMetersPerSecond;
private bool _latestWheelVelocityValid;
/// <summary>
/// 创建使用默认0.10s低通时间常数的电机反馈纵向速度状态源。
/// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
/// </summary>
public WheelFeedbackVehicleStateProvider(
IVehicleStateProvider poseProvider,
@@ -43,7 +46,7 @@ namespace MultiWheelC.StateEstimation
}
/// <summary>
/// 创建使用指定低通时间常数的电机反馈纵向速度状态源。
/// 创建使用指定低通时间常数的电机反馈平面速度状态源。
/// </summary>
public WheelFeedbackVehicleStateProvider(
IVehicleStateProvider poseProvider,
@@ -59,6 +62,9 @@ namespace MultiWheelC.StateEstimation
_longitudinalSpeedFilter =
new FirstOrderLowPassFilter(
velocityFilterTimeConstantSeconds);
_lateralSpeedFilter =
new FirstOrderLowPassFilter(
velocityFilterTimeConstantSeconds);
}
/// <summary>
@@ -87,40 +93,50 @@ namespace MultiWheelC.StateEstimation
{
var actualCarSpeed =
_chassis.GetCarSpeed(true);
var rawLongitudinalSpeedMetersPerSecond =
var rawBodyVxMetersPerSecond =
(double)actualCarSpeed.Vx;
var rawBodyVyMetersPerSecond =
(double)actualCarSpeed.Vy;
NumericGuard.EnsureFinite(
rawLongitudinalSpeedMetersPerSecond,
rawBodyVxMetersPerSecond,
"电机反馈车体纵向速度");
NumericGuard.EnsureFinite(
rawBodyVyMetersPerSecond,
"电机反馈车体横向速度");
var wheelSpeedTimestampSeconds =
_wheelSpeedClock.Elapsed.TotalSeconds;
var filteredLongitudinalSpeedMetersPerSecond =
UpdateLongitudinalSpeedFilter(
rawLongitudinalSpeedMetersPerSecond,
wheelSpeedTimestampSeconds,
out var hasValidWheelSpeedEstimate);
UpdateBodyVelocityFilters(
rawBodyVxMetersPerSecond,
rawBodyVyMetersPerSecond,
wheelSpeedTimestampSeconds,
out var filteredBodyVxMetersPerSecond,
out var filteredBodyVyMetersPerSecond,
out var hasValidWheelSpeedEstimate);
_latestDetourBodyVxMetersPerSecond =
poseState.TwistInBody.VxMetersPerSecond;
_latestDetourVelocityValid =
poseState.HasValidVelocityEstimate;
_latestRawWheelBodyVxMetersPerSecond =
rawLongitudinalSpeedMetersPerSecond;
rawBodyVxMetersPerSecond;
_latestFilteredWheelBodyVxMetersPerSecond =
filteredLongitudinalSpeedMetersPerSecond;
filteredBodyVxMetersPerSecond;
_latestRawWheelBodyVyMetersPerSecond =
rawBodyVyMetersPerSecond;
_latestFilteredWheelBodyVyMetersPerSecond =
filteredBodyVyMetersPerSecond;
_latestWheelVelocityValid =
hasValidWheelSpeedEstimate;
_hasVelocityDiagnostics = true;
// 第一阶段只替换控制器使用的车体纵向速度；横向速度和角速度
// 继续使用Detour估计，避免轮速差和舵角误差放大Vy与Omega噪声。
// 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
// 避免轮速差和舵角误差放大Omega噪声。
var twistInBody = new Twist2D(
filteredLongitudinalSpeedMetersPerSecond,
poseState.TwistInBody
.VyMetersPerSecond,
filteredBodyVxMetersPerSecond,
filteredBodyVyMetersPerSecond,
poseState.TwistInBody
.OmegaRadiansPerSecond);
@@ -151,13 +167,15 @@ namespace MultiWheelC.StateEstimation
}
/// <summary>
/// 读取最近一帧Detour纵向速度和轮速解算纵向速度，供实验记录使用。
/// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
/// </summary>
public bool TryGetLatestVelocityDiagnostics(
out double detourBodyVxMetersPerSecond,
out bool detourVelocityValid,
out double rawWheelBodyVxMetersPerSecond,
out double filteredWheelBodyVxMetersPerSecond,
out double rawWheelBodyVyMetersPerSecond,
out double filteredWheelBodyVyMetersPerSecond,
out bool wheelVelocityValid)
{
lock (_syncRoot)
@@ -170,6 +188,10 @@ namespace MultiWheelC.StateEstimation
_latestRawWheelBodyVxMetersPerSecond;
filteredWheelBodyVxMetersPerSecond =
_latestFilteredWheelBodyVxMetersPerSecond;
rawWheelBodyVyMetersPerSecond =
_latestRawWheelBodyVyMetersPerSecond;
filteredWheelBodyVyMetersPerSecond =
_latestFilteredWheelBodyVyMetersPerSecond;
wheelVelocityValid =
_latestWheelVelocityValid;
return _hasVelocityDiagnostics;
@@ -184,6 +206,7 @@ namespace MultiWheelC.StateEstimation
lock (_syncRoot)
{
_longitudinalSpeedFilter.Reset();
_lateralSpeedFilter.Reset();
_wheelSpeedClock.Restart();
_hasPreviousTimestamp = false;
_previousTimestampSeconds = 0.0;
@@ -192,17 +215,22 @@ namespace MultiWheelC.StateEstimation
_latestDetourVelocityValid = false;
_latestRawWheelBodyVxMetersPerSecond = 0.0;
_latestFilteredWheelBodyVxMetersPerSecond = 0.0;
_latestRawWheelBodyVyMetersPerSecond = 0.0;
_latestFilteredWheelBodyVyMetersPerSecond = 0.0;
_latestWheelVelocityValid = false;
LastFailureReason = string.Empty;
}
}
/// <summary>
/// 使用真实状态时间间隔更新纵向速度低通滤波，并在首帧建立基准。
/// 使用同一个真实采样间隔更新车体Vx和Vy低通滤波，并在首帧建立共同时间基准。
/// </summary>
private double UpdateLongitudinalSpeedFilter(
double rawLongitudinalSpeedMetersPerSecond,
private void UpdateBodyVelocityFilters(
double rawBodyVxMetersPerSecond,
double rawBodyVyMetersPerSecond,
double timestampSeconds,
out double filteredBodyVxMetersPerSecond,
out double filteredBodyVyMetersPerSecond,
out bool hasValidWheelSpeedEstimate)
{
NumericGuard.EnsureFiniteNonNegative(
@@ -212,11 +240,17 @@ namespace MultiWheelC.StateEstimation
if (!_hasPreviousTimestamp)
{
_longitudinalSpeedFilter.Reset(
rawLongitudinalSpeedMetersPerSecond);
rawBodyVxMetersPerSecond);
_lateralSpeedFilter.Reset(
rawBodyVyMetersPerSecond);
_previousTimestampSeconds = timestampSeconds;
_hasPreviousTimestamp = true;
hasValidWheelSpeedEstimate = false;
return rawLongitudinalSpeedMetersPerSecond;
filteredBodyVxMetersPerSecond =
rawBodyVxMetersPerSecond;
filteredBodyVyMetersPerSecond =
rawBodyVyMetersPerSecond;
return;
}
var deltaTimeSeconds =
@@ -227,15 +261,26 @@ namespace MultiWheelC.StateEstimation
if (deltaTimeSeconds <= 0.0)
{
_longitudinalSpeedFilter.Reset(
rawLongitudinalSpeedMetersPerSecond);
rawBodyVxMetersPerSecond);
_lateralSpeedFilter.Reset(
rawBodyVyMetersPerSecond);
hasValidWheelSpeedEstimate = false;
return rawLongitudinalSpeedMetersPerSecond;
filteredBodyVxMetersPerSecond =
rawBodyVxMetersPerSecond;
filteredBodyVyMetersPerSecond =
rawBodyVyMetersPerSecond;
return;
}
hasValidWheelSpeedEstimate = true;
return _longitudinalSpeedFilter.Update(
rawLongitudinalSpeedMetersPerSecond,
deltaTimeSeconds);
filteredBodyVxMetersPerSecond =
_longitudinalSpeedFilter.Update(
rawBodyVxMetersPerSecond,
deltaTimeSeconds);
filteredBodyVyMetersPerSecond =
_lateralSpeedFilter.Update(
rawBodyVyMetersPerSecond,
deltaTimeSeconds);
}
}
MultiWheelC/build/Clumsy/CommonUsage.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.pdb
BIN
View File
Binary file not shown.