增加车队轨迹控制核心与轮组自转模式
This commit is contained in:
@@ -96,67 +96,19 @@ namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
try
|
||||
{
|
||||
var actualCarSpeed =
|
||||
_chassis.GetCarSpeed(true);
|
||||
var rawBodyVxMetersPerSecond =
|
||||
(double)actualCarSpeed.Vx;
|
||||
var rawBodyVyMetersPerSecond =
|
||||
(double)actualCarSpeed.Vy;
|
||||
// CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
|
||||
// 状态估计内部统一转换为rad/s。
|
||||
var rawBodyOmegaRadiansPerSecond =
|
||||
AngleMath.DegreesToRadians(
|
||||
actualCarSpeed.Vw);
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyVxMetersPerSecond,
|
||||
"电机反馈车体纵向速度");
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyVyMetersPerSecond,
|
||||
"电机反馈车体横向速度");
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyOmegaRadiansPerSecond,
|
||||
"电机反馈车体角速度");
|
||||
|
||||
var wheelSpeedTimestampSeconds =
|
||||
_wheelSpeedClock.Elapsed.TotalSeconds;
|
||||
|
||||
UpdateBodyVelocityFilters(
|
||||
rawBodyVxMetersPerSecond,
|
||||
rawBodyVyMetersPerSecond,
|
||||
rawBodyOmegaRadiansPerSecond,
|
||||
wheelSpeedTimestampSeconds,
|
||||
out var filteredBodyVxMetersPerSecond,
|
||||
out var filteredBodyVyMetersPerSecond,
|
||||
out var filteredBodyOmegaRadiansPerSecond,
|
||||
ReadFilteredWheelTwist(
|
||||
out var filteredWheelTwist,
|
||||
out _,
|
||||
out var hasValidWheelSpeedEstimate);
|
||||
|
||||
_latestRawWheelBodyVxMetersPerSecond =
|
||||
rawBodyVxMetersPerSecond;
|
||||
_latestFilteredWheelBodyVxMetersPerSecond =
|
||||
filteredBodyVxMetersPerSecond;
|
||||
_latestRawWheelBodyVyMetersPerSecond =
|
||||
rawBodyVyMetersPerSecond;
|
||||
_latestFilteredWheelBodyVyMetersPerSecond =
|
||||
filteredBodyVyMetersPerSecond;
|
||||
_latestRawWheelBodyOmegaRadiansPerSecond =
|
||||
rawBodyOmegaRadiansPerSecond;
|
||||
_latestFilteredWheelBodyOmegaRadiansPerSecond =
|
||||
filteredBodyOmegaRadiansPerSecond;
|
||||
_latestWheelSampleTimestampSeconds =
|
||||
wheelSpeedTimestampSeconds;
|
||||
_latestWheelVelocityValid =
|
||||
hasValidWheelSpeedEstimate;
|
||||
_latestWheelFeedbackReadSucceeded = true;
|
||||
|
||||
// Detour位姿跳变确认期间需要用轮速维持短时运动预测。
|
||||
if (_poseProvider is DetourVehicleStateProvider
|
||||
detourStateProvider)
|
||||
{
|
||||
detourStateProvider.UpdateWheelVelocityEstimate(
|
||||
filteredBodyVxMetersPerSecond,
|
||||
filteredBodyVyMetersPerSecond,
|
||||
filteredBodyOmegaRadiansPerSecond,
|
||||
filteredWheelTwist.VxMetersPerSecond,
|
||||
filteredWheelTwist.VyMetersPerSecond,
|
||||
filteredWheelTwist.OmegaRadiansPerSecond,
|
||||
hasValidWheelSpeedEstimate);
|
||||
}
|
||||
|
||||
@@ -175,13 +127,18 @@ namespace MultiWheelC.StateEstimation
|
||||
poseState.HasValidVelocityEstimate;
|
||||
_hasVelocityDiagnostics = true;
|
||||
|
||||
// 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
|
||||
// 避免轮速差和舵角误差放大Omega噪声。
|
||||
// 轮组反馈有效后统一使用滤波后的平面速度；初始化期间
|
||||
// 暂时保留Detour角速度作为回退值。
|
||||
var omegaRadiansPerSecond =
|
||||
hasValidWheelSpeedEstimate
|
||||
? filteredWheelTwist
|
||||
.OmegaRadiansPerSecond
|
||||
: poseState.TwistInBody
|
||||
.OmegaRadiansPerSecond;
|
||||
var twistInBody = new Twist2D(
|
||||
filteredBodyVxMetersPerSecond,
|
||||
filteredBodyVyMetersPerSecond,
|
||||
poseState.TwistInBody
|
||||
.OmegaRadiansPerSecond);
|
||||
filteredWheelTwist.VxMetersPerSecond,
|
||||
filteredWheelTwist.VyMetersPerSecond,
|
||||
omegaRadiansPerSecond);
|
||||
|
||||
var twistInWorld =
|
||||
FrameTransform2D
|
||||
@@ -210,6 +167,47 @@ namespace MultiWheelC.StateEstimation
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取并滤波轮组反馈速度，不访问Detour；首帧仅建立滤波时间基准并返回false。
|
||||
/// </summary>
|
||||
public bool TryGetWheelTwist(
|
||||
out Twist2D twistInBody,
|
||||
out double sampleTimestampSeconds)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
try
|
||||
{
|
||||
ReadFilteredWheelTwist(
|
||||
out var filteredWheelTwist,
|
||||
out sampleTimestampSeconds,
|
||||
out var hasValidWheelSpeedEstimate);
|
||||
|
||||
if (!hasValidWheelSpeedEstimate)
|
||||
{
|
||||
twistInBody = Twist2D.Zero;
|
||||
LastFailureReason =
|
||||
"轮组速度估计正在建立采样时间基准。";
|
||||
return false;
|
||||
}
|
||||
|
||||
twistInBody = filteredWheelTwist;
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_latestWheelFeedbackReadSucceeded = false;
|
||||
twistInBody = Twist2D.Zero;
|
||||
sampleTimestampSeconds = 0.0;
|
||||
LastFailureReason =
|
||||
"舵轮电机反馈车体速度解算失败：" +
|
||||
exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。
|
||||
/// </summary>
|
||||
@@ -524,6 +522,73 @@ namespace MultiWheelC.StateEstimation
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从底盘读取一次轮组速度，统一转换为SI单位并更新共用低通滤波状态。
|
||||
/// </summary>
|
||||
private void ReadFilteredWheelTwist(
|
||||
out Twist2D filteredTwistInBody,
|
||||
out double sampleTimestampSeconds,
|
||||
out bool hasValidWheelSpeedEstimate)
|
||||
{
|
||||
var actualCarSpeed =
|
||||
_chassis.GetCarSpeed(true);
|
||||
var rawBodyVxMetersPerSecond =
|
||||
(double)actualCarSpeed.Vx;
|
||||
var rawBodyVyMetersPerSecond =
|
||||
(double)actualCarSpeed.Vy;
|
||||
// CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
|
||||
// 状态估计内部统一转换为rad/s。
|
||||
var rawBodyOmegaRadiansPerSecond =
|
||||
AngleMath.DegreesToRadians(
|
||||
actualCarSpeed.Vw);
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyVxMetersPerSecond,
|
||||
"电机反馈车体纵向速度");
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyVyMetersPerSecond,
|
||||
"电机反馈车体横向速度");
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyOmegaRadiansPerSecond,
|
||||
"电机反馈车体角速度");
|
||||
|
||||
sampleTimestampSeconds =
|
||||
_wheelSpeedClock.Elapsed.TotalSeconds;
|
||||
|
||||
UpdateBodyVelocityFilters(
|
||||
rawBodyVxMetersPerSecond,
|
||||
rawBodyVyMetersPerSecond,
|
||||
rawBodyOmegaRadiansPerSecond,
|
||||
sampleTimestampSeconds,
|
||||
out var filteredBodyVxMetersPerSecond,
|
||||
out var filteredBodyVyMetersPerSecond,
|
||||
out var filteredBodyOmegaRadiansPerSecond,
|
||||
out hasValidWheelSpeedEstimate);
|
||||
|
||||
filteredTwistInBody = new Twist2D(
|
||||
filteredBodyVxMetersPerSecond,
|
||||
filteredBodyVyMetersPerSecond,
|
||||
filteredBodyOmegaRadiansPerSecond);
|
||||
|
||||
_latestRawWheelBodyVxMetersPerSecond =
|
||||
rawBodyVxMetersPerSecond;
|
||||
_latestFilteredWheelBodyVxMetersPerSecond =
|
||||
filteredBodyVxMetersPerSecond;
|
||||
_latestRawWheelBodyVyMetersPerSecond =
|
||||
rawBodyVyMetersPerSecond;
|
||||
_latestFilteredWheelBodyVyMetersPerSecond =
|
||||
filteredBodyVyMetersPerSecond;
|
||||
_latestRawWheelBodyOmegaRadiansPerSecond =
|
||||
rawBodyOmegaRadiansPerSecond;
|
||||
_latestFilteredWheelBodyOmegaRadiansPerSecond =
|
||||
filteredBodyOmegaRadiansPerSecond;
|
||||
_latestWheelSampleTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
_latestWheelVelocityValid =
|
||||
hasValidWheelSpeedEstimate;
|
||||
_latestWheelFeedbackReadSucceeded = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。
|
||||
/// </summary>
|
||||
|
||||
Reference in New Issue
Block a user