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

完善Detour状态估计与轨迹跟踪验证

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-19 17:38:17 +08:00
parent d8de901a80
commit 0d5539e595
55 changed files with 4473 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Shared/Chassis/MultiWheelChassisAdapter.cs
+46 -24
View File
@@ -1,33 +1,39 @@
// 将统一命令转换为原 Chassis API 调用
// Shared层底盘边界：对外使用SI单位，对内适配旧版MultiWheelChassis的混合单位接口。
using System;
using CommonUsage.Chassis;
namespace MyParking.Shared
{
/// <summary>
/// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
/// 车体坐标系固定为X向前、Y向左、逆时针为正。
/// 将真实车体系刚体速度转换为旧版MultiWheelChassis命令，车体系约定为X向前、Y向左、逆时针为正。
/// </summary>
public sealed class MultiWheelChassisAdapter
{
#region 辅助内容
// 旧底盘原点偏置使用float角度值，此容差用于判断坐标系是否已经切换到位。
private const float BiasTolerance = 0.001f;
// 小于该值的线速度或角速度视为零，避免在静止附近进入方向不确定的运动学分支。
private const double MotionDeadband = 1e-6;
private readonly MultiWheelChassis _chassis;
// β：当前运动系X轴相对真实车体X轴的逆时针夹角，单位为rad。
private double _activeMotionDirectionRadians;
/// <summary>
/// 当前适配器对应的车辆编号。
/// </summary>
public int VehicleId { get; }
/// <summary>
/// Maximum distance from the body origin to a wheel center, in metres.
/// 车体原点到最远舵轮中心的距离，单位为m，用于描述底盘整体外接半径。
/// </summary>
public double MaximumWheelRadiusMeters { get; }
/// <summary>
/// Maximum longitudinal wheel offset from the body origin, in metres.
/// For a symmetric four-wheel-steering chassis this is half the wheelbase.
/// 车体原点到最前或最后舵轮中心的最大纵向距离，单位为m；对称四舵轮底盘中通常为轴距的一半。
/// </summary>
public double HalfWheelBaseMeters { get; }
@@ -50,7 +56,7 @@ namespace MyParking.Shared
_activeMotionDirectionRadians;
/// <summary>
/// Width of the steering-alignment speed gate, in degrees.
/// 舵角误差高斯降速门控的宽度，单位为deg；数值越小，舵轮未对齐时驱动降速越明显。
/// </summary>
public double SteeringAlignmentSigmaDegrees
{
@@ -78,9 +84,9 @@ namespace MyParking.Shared
}
/// <summary>
/// 检查旧底盘当前是否处于指定的运动坐标系。
/// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
/// 检查旧底盘是否处于指定β运动坐标系，防止准备状态与当前命令使用的坐标系不一致。
/// </summary>
/// <param name="motionDirectionRadians">运动系X轴在真实车体系中的方向，单位为rad。</param>
private void EnsureMotionFrameIsActive(
double motionDirectionRadians)
{
@@ -130,6 +136,7 @@ namespace MyParking.Shared
twist.OmegaRadiansPerSecond),
nameof(twist.OmegaRadiansPerSecond));
}
/// <summary>
/// 检查数值是否为有限值且可安全转换为float。
/// </summary>
@@ -180,10 +187,9 @@ namespace MyParking.Shared
}
/// <summary>
/// 激活指定运动方向对应的SendMotion坐标系。
/// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
/// 调用方必须先停车，并确认舵轮已经按该方向完成预对齐。
/// 激活指定β对应的SendMotion运动坐标系；调用前必须停车并完成该方向的舵轮预对齐。
/// </summary>
/// <param name="motionDirectionRadians">运动系X轴在真实车体系中的方向，单位为rad；0为车头，π/2为车体左侧。</param>
public void ActivateMotionFrame(
double motionDirectionRadians)
{
@@ -195,6 +201,7 @@ namespace MyParking.Shared
AngleMath.NormalizeRadians(
motionDirectionRadians);
// 旧底盘以“真实车体系相对运动系”的角度保存偏置，因此符号与β相反。
var biasDegrees =
ConvertRadiansToSingleDegrees(
-normalizedDirectionRadians,
@@ -217,6 +224,8 @@ namespace MyParking.Shared
return;
}
// SetOriginBias会把每个真实轮位重新表达在运动系中，并同步设置舵角零方向；
// 车辆本体没有发生虚拟旋转，后续SendMotion仍使用这些真实轮位完成四轮解算。
_chassis.SetOriginBias(
x: 0.0f,
y: 0.0f,
@@ -235,6 +244,12 @@ namespace MyParking.Shared
AngleMath.NormalizeRadians(
motionDirectionRadians);
}
/// <summary>
/// 创建旧版底盘的SI单位适配器，并从真实轮位提取车辆几何尺寸。
/// </summary>
/// <param name="chassis">已经完成舵轮初始化的旧版多舵轮底盘。</param>
/// <param name="vehicleId">正整数车辆编号，仅标识该适配器所属车辆。</param>
public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
{
_chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
@@ -255,8 +270,7 @@ namespace MyParking.Shared
"MultiWheelChassis尚未完成舵轮初始化，" +
"不能创建底盘适配器。");
}
// 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
// 保证SendXYThSpeed直接使用真实车体坐标系。
// 几何尺寸必须取PhysicalPosition，避免受旧底盘当前原点偏置和运动坐标系影响。
var maximumWheelRadiusMillimeters = 0.0;
var maximumLongitudinalOffsetMillimeters = 0.0;
var maximumLateralOffsetMillimeters = 0.0;
@@ -301,9 +315,11 @@ namespace MyParking.Shared
}
/// <summary>
/// 将车体坐标系刚体速度统一转换为滚动SendMotion、原地自转或停车命令。
/// 非零平移命令使用调用方在运动段开始前已经准备并激活的β运动坐标系。
/// 将真实车体系刚体速度分派为滚动SendMotion、真实车体系纯自转或立即停车命令。
/// </summary>
/// <param name="bodyTwist">真实车体系速度，线速度单位为m/s，角速度单位为rad/s。</param>
/// <param name="interval">与上一条底盘命令的实际时间间隔，用于旧底盘速度和舵角变化率处理。</param>
/// <returns>旧底盘是否成功接受并完成运动分解。</returns>
public bool SendBodyTwist(
Twist2D bodyTwist,
TimeSpan? interval = null)
@@ -339,7 +355,7 @@ namespace MyParking.Shared
}
/// <summary>
/// 将车体刚体速度转换到已准备的运动坐标系，并生成该坐标系中的前后GCP方向。
/// 将真实车体系刚体速度转换到已激活的β运动系，并生成该运动系中的前后GCP命令。
/// </summary>
private bool SendRollingTwistInActiveMotionFrame(
Twist2D bodyTwist,
@@ -354,6 +370,8 @@ namespace MyParking.Shared
0.0,
0.0,
-_activeMotionDirectionRadians);
// 同一点的速度只需旋转表达坐标系；刚体角速度在二维旋转变换下保持不变。
var motionTwist =
FrameTransform2D.TransformTwistAtSamePoint(
bodyPoseInMotionFrame,
@@ -375,9 +393,13 @@ namespace MyParking.Shared
var travelDirection =
Math.Sign(
motionVxMetersPerSecond);
// SendMotion用速度符号表达前进/倒车，而GCP角度始终相对当前行驶方向计算。
var signedCenterSpeedMetersPerSecond =
travelDirection *
linearSpeedMetersPerSecond;
// 刚体速度关系v(point)=v(center)+ω×r；前后GCP位于运动系X轴的±ControlPointRadius处。
var frontVelocityYMetersPerSecond =
motionVyMetersPerSecond +
bodyTwist.OmegaRadiansPerSecond *
@@ -432,6 +454,7 @@ namespace MyParking.Shared
return success;
}
/// <summary>
/// 在当前已激活的运动坐标系中将有符号速度和前后GCP角度发送给旧版SendMotion。
/// </summary>
@@ -499,9 +522,9 @@ namespace MyParking.Shared
}
/// <summary>
/// 停车并将所有舵轮转到指定的车体角度。
/// 只调整舵轮角度，不产生车辆线速度。
/// 停车并将所有舵轮预对齐到真实车体系中的同一机械方向，不产生车辆线速度。
/// </summary>
/// <param name="directionRadians">舵轮相对真实车体X轴的目标方向，单位为rad。</param>
public bool PrepareParallelDirection(
double directionRadians)
{
@@ -544,7 +567,7 @@ namespace MyParking.Shared
}
/// <summary>
/// 检查所有舵轮是否已经对准给定方向。
/// 检查所有舵轮是否已在给定容差内对准真实车体系中的同一机械方向。
/// </summary>
public bool AreParallelWheelsAligned(
double directionRadians,
@@ -572,6 +595,7 @@ namespace MyParking.Shared
foreach (var wheel in wheels)
{
// 机械舵角受限于非环形区间，此处必须比较直接角差，不能使用圆周最短角差。
var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
if (Math.Abs(angleErrorDegrees) >
@@ -641,12 +665,10 @@ namespace MyParking.Shared
return success;
}
/// <summary>
/// 所有舵轮是否已对齐到原地自转方向。
/// 所有舵轮是否已对齐到最近一次原地自转准备所确定的目标方向。
/// </summary>
public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
}
}