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

完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-08-07 13:00:56 +08:00
co-authored by Cursor
parent f8881bc243
commit 14ca1150e4
12 changed files with 971 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+1 -1
View File
@@ -28,7 +28,7 @@ namespace MultiWheelC.Control.Execution
1e-6;
private const double StartupRegionMeters = 0.02;
private const double StartupPreviewDistanceMeters = 0.05;
private const double MaximumStartupSpeedMetersPerSecond = 0.05;
private const double MaximumStartupSpeedMetersPerSecond = 0.08;
private readonly IVehicleStateProvider _stateProvider;
private readonly ILateralController _lateralController;
MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs
+43 -1
View File
@@ -23,11 +23,15 @@ namespace MultiWheelC.Control.Longitudinal
double integralGainPerSecond,
double derivativeGainSeconds,
double maximumIntegralCorrectionMetersPerSecond,
double maximumCommandSpeedMetersPerSecond)
double maximumCommandSpeedMetersPerSecond,
double speedErrorDeadbandMetersPerSecond = 0.025)
{
EnsureFinitePositive(
maximumCommandSpeedMetersPerSecond,
nameof(maximumCommandSpeedMetersPerSecond));
EnsureFiniteNonNegative(
speedErrorDeadbandMetersPerSecond,
nameof(speedErrorDeadbandMetersPerSecond));
_feedbackPid = new PidController(
proportionalGain,
@@ -37,6 +41,8 @@ namespace MultiWheelC.Control.Longitudinal
derivativeOnMeasurement: true);
MaximumCommandSpeedMetersPerSecond =
maximumCommandSpeedMetersPerSecond;
SpeedErrorDeadbandMetersPerSecond =
speedErrorDeadbandMetersPerSecond;
}
/// <summary>
@@ -49,6 +55,11 @@ namespace MultiWheelC.Control.Longitudinal
/// </summary>
public double MaximumCommandSpeedMetersPerSecond { get; }
/// <summary>
/// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
/// </summary>
public double SpeedErrorDeadbandMetersPerSecond { get; }
/// <summary>
/// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
/// </summary>
@@ -98,6 +109,20 @@ namespace MultiWheelC.Control.Longitudinal
referenceSpeedMetersPerSecond);
}
var speedErrorMetersPerSecond =
referenceSpeedMetersPerSecond -
context.ActualLongitudinalSpeedMetersPerSecond;
// Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
// 同时清除PID历史，避免噪声持续积累后产生突发修正。
if (Math.Abs(speedErrorMetersPerSecond) <=
SpeedErrorDeadbandMetersPerSecond)
{
Reset();
return LimitReferenceSpeed(
referenceSpeedMetersPerSecond);
}
GetCorrectionOutputRange(
referenceSpeedMetersPerSecond,
out var minimumCorrectionMetersPerSecond,
@@ -178,5 +203,22 @@ namespace MultiWheelC.Control.Longitudinal
"纵向控制器最大命令速度必须是正有限值。");
}
}
/// <summary>
/// 检查速度误差死区是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value) ||
value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"纵向控制器速度误差死区必须是非负有限值。");
}
}
}
}
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+3 -3
View File
@@ -330,17 +330,17 @@ namespace MultiWheelC
/// <summary>
/// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
/// </summary>
public double CurvatureTransitionLengthMeters = 0.60;
public double CurvatureTransitionLengthMeters = 0.70;
/// <summary>
/// 获取或设置两段直线的最大参考速度，单位为m/s。
/// </summary>
public double StraightMaximumSpeedMetersPerSecond = 0.30;
public double StraightMaximumSpeedMetersPerSecond = 0.40;
/// <summary>
/// 获取或设置半圆段的最大参考速度，单位为m/s。
/// </summary>
public double SemicircleMaximumSpeedMetersPerSecond = 0.25;
public double SemicircleMaximumSpeedMetersPerSecond = 0.30;
/// <summary>
/// 获取或设置参考速度加速度，单位为m/s²。
MultiWheelC/Experiments/RotationTests.cs
+91 -27
View File
@@ -1,6 +1,7 @@
using System;
using System.Globalization;
using System.Numerics;
using System.Threading;
using ClumsyCore;
@@ -17,7 +18,6 @@ namespace MultiWheelC
public abstract class InPlaceRotateTestBase : MovementTest
{
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
public int TrialNumber = 1; // 重复实验编号。
private DriveTask _task;
@@ -37,11 +37,18 @@ namespace MultiWheelC
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
public override void Test()
{
var config = PilotDefinition.Conf;
if (float.IsNaN(RelativeAngleDegrees) ||
float.IsInfinity(RelativeAngleDegrees) ||
float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
MaxAngularSpeedDegreesPerSecond <= 0f)
float.IsNaN(config.InPlaceRotateMaxSpeed) ||
float.IsInfinity(config.InPlaceRotateMaxSpeed) ||
config.InPlaceRotateMaxSpeed <= 0f ||
float.IsNaN(config.InPlaceRotateMinimumSpeed) ||
float.IsInfinity(config.InPlaceRotateMinimumSpeed) ||
config.InPlaceRotateMinimumSpeed <= 0f ||
config.InPlaceRotateMinimumSpeed >
config.InPlaceRotateMaxSpeed)
{
Console.WriteLine("原地旋转测试参数无效。");
return;
@@ -66,8 +73,25 @@ namespace MultiWheelC
(float)AngleMath.NormalizeDegrees(
location.th + RelativeAngleDegrees);
Console.WriteLine(
"原地自转实际参数：" +
$"Kp={config.InPlaceRotateKp:F3}，" +
$"Ki={config.InPlaceRotateKi:F3}，" +
$"Kd={config.InPlaceRotateKd:F3}，" +
$"到位误差={config.InPlaceRotateArriveDeg:F2}°，" +
$"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" +
$"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" +
$"角加速度={config.InPlaceRotateAcc:F2}°/s²，" +
$"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
$"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
$"起点航向={location.th:F2}°，" +
$"目标航向={targetWorldAngle:F2}°。");
Console.WriteLine(
"原地自转CSV保存目录：" +
TrackingExperimentRecorder.DefaultOutputDirectory);
_recorder = new TrackingExperimentRecorder(
controllerName: "InPlaceRotatePID",
controllerName: "InPlaceRotateFilteredPID",
trajectoryName: _trajectoryName,
trialNumber: TrialNumber,
referenceStart: rotationCenter,
@@ -75,7 +99,7 @@ namespace MultiWheelC
referenceSpeed: 0f,
referenceAngularSpeed:
(float)AngleMath.DegreesToRadians(
MaxAngularSpeedDegreesPerSecond));
config.InPlaceRotateMaxSpeed));
_recorder.Start();
try
@@ -88,21 +112,26 @@ namespace MultiWheelC
PidparamsRead = () => new PIDParams
{
Kp =
PilotDefinition.Conf.InPlaceRotateKp,
config.InPlaceRotateKp,
Ki =
PilotDefinition.Conf.InPlaceRotateKi,
config.InPlaceRotateKi,
Kd =
PilotDefinition.Conf.InPlaceRotateKd,
config.InPlaceRotateKd,
DeadZone =
PilotDefinition.Conf
.InPlaceRotateArriveDeg,
config.InPlaceRotateArriveDeg,
SpeedAccPerSec =
PilotDefinition.Conf.InPlaceRotateAcc,
config.InPlaceRotateAcc,
OutputUpperThreshold =
MaxAngularSpeedDegreesPerSecond,
config.InPlaceRotateMaxSpeed,
MaxI =
PilotDefinition.Conf.InPlaceRotateMaxI
config.InPlaceRotateMaxI
},
MinimumAngularSpeedDegreesPerSecond =
config.InPlaceRotateMinimumSpeed,
WheelAlignmentToleranceDegrees =
config.InPlaceRotateWheelAlignDeg,
RotationTimeoutSeconds =
config.InPlaceRotateTimeoutSec,
CommandAngularSpeedObserver =
commandAngularSpeed =>
_recorder?.UpdateCommand(
@@ -136,23 +165,58 @@ namespace MultiWheelC
}
[MovementTest(name = "SendXYThSpeed：原地自转90°")]
public sealed class TestRotate90 :
[MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
public sealed class TestRotateAngle :
InPlaceRotateTestBase
{
public TestRotate90()
: base(90f, "Rotate90")
public TestRotateAngle()
: base(0f, "RotateCustomAngle")
{
}
/// <summary>
/// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。
/// </summary>
public override void Test()
{
var input = UI.GetInput(
"输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
if ((!float.TryParse(
input,
NumberStyles.Float,
CultureInfo.CurrentCulture,
out var relativeAngleDegrees) &&
!float.TryParse(
input,
NumberStyles.Float,
CultureInfo.InvariantCulture,
out relativeAngleDegrees)) ||
float.IsNaN(relativeAngleDegrees) ||
float.IsInfinity(relativeAngleDegrees))
{
Console.WriteLine("旋转角度输入无效，测试已经取消。");
return;
}
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
{
Console.WriteLine("旋转角度不能为0，测试已经取消。");
return;
}
// 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
if (Math.Abs(relativeAngleDegrees) >= 180f)
{
Console.WriteLine(
"输入角度必须满足-180° < angle < 180°；" +
"当前最短角控制不支持指定精确±180°的旋转方向。");
return;
}
RelativeAngleDegrees = relativeAngleDegrees;
base.Test();
}
}
[MovementTest(name = "SendXYThSpeed：原地自转180°")]
public sealed class TestRotate180 :
InPlaceRotateTestBase
{
public TestRotate180()
: base(180f, "Rotate180")
{
}
}
}
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+21 -3
View File
@@ -145,6 +145,14 @@ namespace MultiWheelC
// 保存成功后的CSV绝对路径；尚未保存时为空。
public string SavedFilePath { get; private set; }
/// <summary>
/// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。
/// </summary>
public static string DefaultOutputDirectory =>
Path.Combine(
AppContext.BaseDirectory,
"TrackingExperiments");
// 启动后台采样线程。
public void Start()
{
@@ -242,6 +250,17 @@ namespace MultiWheelC
}
}
/// <summary>
/// 清除上一轨迹段参考量，避免停车或原地自转期间沿用已经结束的投影结果。
/// </summary>
public void ClearControlReference()
{
lock (_stateSyncRoot)
{
_hasControlReference = false;
}
}
// 停止采样并将本次实验保存为CSV；重复调用只保存一次。
public void StopAndSave()
{
@@ -444,9 +463,8 @@ namespace MultiWheelC
new List<TrackingSample>(_samples);
}
var outputDirectory = Path.Combine(
AppContext.BaseDirectory,
"TrackingExperiments");
var outputDirectory =
DefaultOutputDirectory;
Directory.CreateDirectory(outputDirectory);
MultiWheelC/Movements/RotateInPlace.cs
+191 -5
View File
@@ -7,6 +7,7 @@ using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Commons.Controllers;
using MyParking.Shared;
using MultiWheelC.StateEstimation;
namespace MultiWheelC
{
@@ -17,7 +18,11 @@ namespace MultiWheelC
/// </summary>
public float AngleTarget;
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
// 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
public Func<float> ThetaReader;
public IVehicleStateProvider StateProvider =
new DetourVehicleStateProvider();
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
@@ -37,6 +42,12 @@ namespace MultiWheelC
// 自转舵轮准备超时时间，单位s。
public float WheelAlignmentTimeoutSeconds = 10f;
// 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
public float MinimumAngularSpeedDegreesPerSecond = 1f;
// 舵轮到位后执行航向闭环允许的最长时间，单位s。
public float RotationTimeoutSeconds = 15f;
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
public override IEnumerable<bool> Get()
{
@@ -44,6 +55,8 @@ namespace MultiWheelC
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
ValidateParameters();
var adapter = new MultiWheelChassisAdapter(
Chassis,
PilotDefinition.Self.CarNum);
@@ -55,7 +68,9 @@ namespace MultiWheelC
DateTime? alignedSince = null;
while (true)
{
if (!adapter.PrepareSpin())
if (!adapter.PrepareSpin(
alignmentToleranceDegrees:
WheelAlignmentToleranceDegrees))
throw new InvalidOperationException(
"无法生成原地自转舵轮目标：" +
adapter.LastFailureReason);
@@ -84,18 +99,86 @@ namespace MultiWheelC
yield return true;
}
var alignmentToleranceRadians =
AngleMath.DegreesToRadians(
WheelAlignmentToleranceDegrees);
if (!adapter.AdoptPreparedSpinForXYTh(
alignmentToleranceRadians))
{
throw new InvalidOperationException(
"无法将已到位的自转舵角交接给XYTh：" +
adapter.LastFailureReason);
}
var targetAngle =
(float)AngleMath.NormalizeDegrees(AngleTarget);
var p = PidparamsRead();
thPid = new PIDController(ThetaReader, p.Kp);
var currentAngle = ReadCurrentAngleDegrees();
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);
var lastCommandTime = DateTime.Now;
var rotationStarted = DateTime.Now;
while (true)
{
if ((DateTime.Now - rotationStarted)
.TotalSeconds >
RotationTimeoutSeconds)
{
throw new TimeoutException(
$"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
}
currentAngle = ReadCurrentAngleDegrees();
cachedCurrentAngle = currentAngle;
var s = thPid.GetResponse(targetAngle, true);
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
var angleErrorDegrees =
(float)AngleMath
.ShortestDifferenceDegrees(
targetAngle,
currentAngle);
// PID进入到位死区后等待其0.3s稳定确认；等待期间
// 只清零驱动速度，不清除已经准备好的自转舵角状态。
if (Math.Abs(angleErrorDegrees) <=
p.DeadZone)
{
CommandAngularSpeedObserver?.Invoke(0f);
adapter
.StopXYThDrivePreserveSteeringState();
if (thPid.IsArrived())
break;
yield return true;
continue;
}
// PID输出低于底盘有效轮速范围时提高到最小可执行值，
// 避免接近目标时反复出现微小命令但车辆实际不动。
if (Math.Abs(s) > 1e-6f &&
Math.Abs(s) <
MinimumAngularSpeedDegreesPerSecond)
{
s = Math.Sign(angleErrorDegrees) *
MinimumAngularSpeedDegreesPerSecond;
}
// PID加速限制在首周期可能暂时输出零；此时保留
// 已交接的自转状态，等待下一周期产生有效角速度。
if (Math.Abs(s) <= 1e-6f)
{
CommandAngularSpeedObserver?.Invoke(0f);
adapter
.StopXYThDrivePreserveSteeringState();
yield return true;
continue;
}
CommandAngularSpeedObserver?.Invoke(s);
var now = DateTime.Now;
var interval = now - lastCommandTime;
@@ -118,7 +201,6 @@ namespace MultiWheelC
"安全XYTh原地旋转底盘解算失败：" +
adapter.LastFailureReason);
}
if (thPid.IsArrived()) break;
yield return true;
}
@@ -130,5 +212,109 @@ namespace MultiWheelC
adapter.StopImmediately();
}
}
/// <summary>
/// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
/// </summary>
private void ValidateParameters()
{
EnsureFinitePositive(
WheelAlignmentToleranceDegrees,
nameof(WheelAlignmentToleranceDegrees),
allowZero: true);
EnsureFinitePositive(
WheelAlignmentStableSeconds,
nameof(WheelAlignmentStableSeconds),
allowZero: true);
EnsureFinitePositive(
WheelAlignmentTimeoutSeconds,
nameof(WheelAlignmentTimeoutSeconds));
EnsureFinitePositive(
MinimumAngularSpeedDegreesPerSecond,
nameof(MinimumAngularSpeedDegreesPerSecond));
EnsureFinitePositive(
RotationTimeoutSeconds,
nameof(RotationTimeoutSeconds));
var pidParameters = PidparamsRead();
if (pidParameters == null)
{
throw new InvalidOperationException(
"原地自转PID参数读取结果为空。");
}
EnsureFinitePositive(
pidParameters.DeadZone,
"PidparamsRead.DeadZone");
EnsureFinitePositive(
pidParameters.OutputUpperThreshold,
"PidparamsRead.OutputUpperThreshold");
EnsureFinitePositive(
pidParameters.SpeedAccPerSec,
"PidparamsRead.SpeedAccPerSec");
EnsureFinitePositive(
pidParameters.Kp,
"PidparamsRead.Kp");
if (MinimumAngularSpeedDegreesPerSecond >
pidParameters.OutputUpperThreshold)
{
throw new InvalidOperationException(
"原地自转最小有效角速度不能大于最大角速度。");
}
}
/// <summary>
/// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
/// </summary>
private float ReadCurrentAngleDegrees()
{
if (ThetaReader != null)
{
var angleDegrees = ThetaReader();
if (float.IsNaN(angleDegrees) ||
float.IsInfinity(angleDegrees))
{
throw new InvalidOperationException(
"自定义航向读取结果不是有效角度。");
}
return (float)AngleMath.NormalizeDegrees(
angleDegrees);
}
if (StateProvider == null ||
!StateProvider.TryGetState(out var state))
{
throw new InvalidOperationException(
"无法从Detour状态源读取有效车辆航向。" +
(StateProvider is DetourVehicleStateProvider provider
? provider.LastFailureReason
: ""));
}
return (float)AngleMath.RadiansToDegrees(
state.PoseInWorld.YawRadians);
}
/// <summary>
/// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
/// </summary>
private static void EnsureFinitePositive(
float value,
string parameterName,
bool allowZero = false)
{
if (float.IsNaN(value) ||
float.IsInfinity(value) ||
(allowZero
? value < 0f
: value <= 0f))
{
throw new ArgumentOutOfRangeException(
parameterName,
"原地自转参数必须是有效的正数。");
}
}
}
}
MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+9 -2
View File
@@ -75,6 +75,12 @@ namespace MultiWheelC
/// </summary>
public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
/// <summary>
/// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
/// </summary>
public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
0.025;
/// <summary>
/// 底盘纵向命令速度绝对值上限，单位为m/s。
/// </summary>
@@ -90,7 +96,7 @@ namespace MultiWheelC
/// 前后GCP目标转角最大变化率，单位为rad/s。
/// </summary>
public double MaximumGcpAngleRateRadiansPerSecond =
AngleMath.DegreesToRadians(10.0);
AngleMath.DegreesToRadians(15.0);
/// <summary>
/// 终点位置和剩余弧长的完成容差，单位为m。
@@ -164,7 +170,8 @@ namespace MultiWheelC
LongitudinalKiPerSecond,
LongitudinalKdSeconds,
MaximumIntegralCorrectionMetersPerSecond,
MaximumCommandSpeedMetersPerSecond);
MaximumCommandSpeedMetersPerSecond,
LongitudinalSpeedErrorDeadbandMetersPerSecond);
var gcpAllocator =
new AckermannGcpAllocator(
controlPointRadiusMeters,
MultiWheelC/PilotConfig.cs
+9 -8
View File
@@ -26,7 +26,7 @@ public class PilotConfig : MultiWheelPilotConfig
public float InPlaceRotateSpeed = 30f;
[FieldMember(desc = "原地旋转：到位角度精度(deg)")]
public float InPlaceRotateArriveDeg = 1f;
public float InPlaceRotateArriveDeg = 1.5f;
[FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
public float InPlaceRotateWheelAlignDeg = 2f;
@@ -38,24 +38,25 @@ public class PilotConfig : MultiWheelPilotConfig
#region 单车-临时
[FieldMember(desc = "原地旋转Kp")]
public float InPlaceRotateKp = 0.2f;
// public float InPlaceRotateKp = 0.2f;
public float InPlaceRotateKp = 1.1f;
[FieldMember(desc = "原地旋转Ki")]
public float InPlaceRotateKi = 0.01f;
// public float InPlaceRotateKi = 0.01f;
public float InPlaceRotateKi = 0f;
[FieldMember(desc = "原地旋转Kd")]
public float InPlaceRotateKd = 0f;
[FieldMember(desc = "原地旋转积分限幅")]
public float InPlaceRotateMaxI = 0.01f;
public float InPlaceRotateMaxI = 0f;
[FieldMember(desc = "原地旋转最小有效角速度(deg/s)")]
public float InPlaceRotateMinimumSpeed = 1f;
[FieldMember(desc = "原地旋转最大角速度(deg/s)")]
public float InPlaceRotateMaxSpeed = 30f;
public float InPlaceRotateMaxSpeed = 47.5f;
[FieldMember(desc = "原地旋转角加速度(deg/s²)")]
public float InPlaceRotateAcc = 30f;
public float InPlaceRotateAcc = 60f;
[FieldMember(desc = "原地旋转超时(s)")]
public float InPlaceRotateTimeoutSec = 15f;
MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs
+2 -2
View File
@@ -188,11 +188,11 @@ namespace MultiWheelC.StateEstimation
poseInWorld,
elapsedSeconds))
{
state = AcceptPoseAfterVelocityRebase(
state = AcceptPoseAfterReset(
poseInWorld,
timestampSeconds);
LastFailureReason =
"Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。";
"Detour位姿偏离速度预测，已重新建立速度估计基准。";
return true;
}