增加GCP运动学并完善车体平面速度反馈分析
This commit is contained in:
@@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user