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

完善蟹行虚拟阿克曼与SendMotion运动坐标系，并添加轮速诊断日志

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-07-30 18:10:22 +08:00
co-authored by Cursor
parent 7e05ff098e
commit c1fe73caec
92 changed files with 1794 additions and 776 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Shared/MultiWheelChassisAdapter.cs
+270 -6
View File
@@ -30,6 +30,12 @@ namespace MyParking.Shared
/// </summary>
public double HalfWheelBaseMeters { get; }
/// <summary>
/// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
/// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
/// </summary>
public double HalfTrackWidthMeters { get; }
/// <summary>
/// Width of the steering-alignment speed gate, in degrees.
/// </summary>
@@ -58,19 +64,52 @@ namespace MyParking.Shared
/// </summary>
private void EnsureBodyFrameIsActive()
{
EnsureMotionFrameIsActive(0.0);
}
/// <summary>
/// 检查旧底盘当前是否处于指定的运动坐标系。
/// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
/// </summary>
private void EnsureMotionFrameIsActive(
double motionDirectionRadians)
{
ValidateFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
var expectedBiasDegrees =
(float)(
-FrameTransform2D.NormalizeAngle(
motionDirectionRadians) *
RadiansToDegrees);
var bias = _chassis.GetOriginBias();
var angleErrorDegrees =
NormalizeDegrees(
bias.Z - expectedBiasDegrees);
if (Math.Abs(bias.X) <= BiasTolerance &&
Math.Abs(bias.Y) <= BiasTolerance &&
Math.Abs(bias.Z) <= BiasTolerance)
Math.Abs(angleErrorDegrees) <=
BiasTolerance)
{
return;
}
throw new InvalidOperationException(
"MultiWheelChassis的坐标偏置在适配器创建后被修改。" +
$"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°。" +
"请不要再调用DirectionAngle或SetOriginBias控制蟹行。");
"MultiWheelChassis当前运动坐标系与命令不一致。" +
$"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°，" +
$"期望Th={expectedBiasDegrees}°。");
}
/// <summary>
/// 将角度归一化到[-180°,180°]附近。
/// </summary>
private static float NormalizeDegrees(float degrees)
{
return (float)(
degrees -
Math.Round(degrees / 360.0) * 360.0);
}
/// <summary>
/// 检查底盘命令是否包含无效数值。
@@ -118,6 +157,7 @@ namespace MyParking.Shared
/// </summary>
public string LastFailureReason =>
_chassis.LastMotionDecomposeFailureReason;
#endregion
/// <summary>
@@ -125,10 +165,45 @@ namespace MyParking.Shared
/// </summary>
public void ResetToBodyFrame()
{
ActivateMotionFrame(0.0);
}
/// <summary>
/// 激活指定运动方向对应的SendMotion坐标系。
/// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
/// </summary>
public void ActivateMotionFrame(
double motionDirectionRadians)
{
ValidateFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
var biasDegrees =
(float)(
-FrameTransform2D.NormalizeAngle(
motionDirectionRadians) *
RadiansToDegrees);
var currentBias =
_chassis.GetOriginBias();
if (Math.Abs(currentBias.X) <=
BiasTolerance &&
Math.Abs(currentBias.Y) <=
BiasTolerance &&
Math.Abs(
NormalizeDegrees(
currentBias.Z -
biasDegrees)) <=
BiasTolerance)
{
return;
}
_chassis.SetOriginBias(
x: 0.0f,
y: 0.0f,
th: 0.0f);
th: biasDegrees);
}
public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
{
@@ -154,6 +229,7 @@ namespace MyParking.Shared
// 保证SendXYThSpeed直接使用真实车体坐标系。
var maximumWheelRadiusMillimeters = 0.0;
var maximumLongitudinalOffsetMillimeters = 0.0;
var maximumLateralOffsetMillimeters = 0.0;
foreach (var wheel in wheels)
{
maximumWheelRadiusMillimeters = Math.Max(
@@ -163,15 +239,22 @@ namespace MyParking.Shared
maximumLongitudinalOffsetMillimeters = Math.Max(
maximumLongitudinalOffsetMillimeters,
Math.Abs(wheel.PhysicalPosition.X));
maximumLateralOffsetMillimeters = Math.Max(
maximumLateralOffsetMillimeters,
Math.Abs(wheel.PhysicalPosition.Y));
}
MaximumWheelRadiusMeters =
maximumWheelRadiusMillimeters / 1000.0;
HalfWheelBaseMeters =
maximumLongitudinalOffsetMillimeters / 1000.0;
HalfTrackWidthMeters =
maximumLateralOffsetMillimeters / 1000.0;
if (MaximumWheelRadiusMeters <= 0.0 ||
HalfWheelBaseMeters <= 0.0)
HalfWheelBaseMeters <= 0.0 ||
HalfTrackWidthMeters <= 0.0)
{
throw new InvalidOperationException(
"Wheel positions cannot produce valid chassis dimensions.");
@@ -220,6 +303,145 @@ namespace MyParking.Shared
}
return success;
}
/// <summary>
/// 发送单车原地自转命令。
/// 适配层使用rad/s，底层SendRotateMotion使用deg/s。
/// </summary>
public bool SendRotateMotion(
double omegaRadiansPerSecond,
TimeSpan? interval = null)
{
ValidateFinite(
omegaRadiansPerSecond,
nameof(omegaRadiansPerSecond));
EnsureBodyFrameIsActive();
var success = _chassis.SendRotateMotion(
(float)(
omegaRadiansPerSecond *
RadiansToDegrees),
interval);
if (!success)
{
_chassis.PredefinedDriveStop();
}
return success;
}
/// <summary>
/// 将指定运动方向上的虚拟阿克曼命令转换为车体二维速度。
/// 运动方向0表示车头，正90度表示车体左侧；
/// 转向角为正时向该虚拟运动方向的左侧转弯。
/// </summary>
public bool SendVirtualAckermann(
double motionDirectionRadians,
double speedMetersPerSecond,
double steeringRadians,
double virtualHalfWheelBaseMeters,
TimeSpan? interval = null)
{
ValidateFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
ValidateFinite(
speedMetersPerSecond,
nameof(speedMetersPerSecond));
ValidateFinite(
steeringRadians,
nameof(steeringRadians));
ValidateFinite(
virtualHalfWheelBaseMeters,
nameof(virtualHalfWheelBaseMeters));
if (virtualHalfWheelBaseMeters <= 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(virtualHalfWheelBaseMeters),
"虚拟阿克曼半轴距必须是正数。");
}
var steeringCosine =
Math.Cos(steeringRadians);
if (Math.Abs(steeringCosine) < 1e-6)
{
throw new ArgumentOutOfRangeException(
nameof(steeringRadians),
"虚拟阿克曼转向角不能达到正负90度。");
}
var vxMetersPerSecond =
speedMetersPerSecond *
Math.Cos(motionDirectionRadians);
var vyMetersPerSecond =
speedMetersPerSecond *
Math.Sin(motionDirectionRadians);
var omegaRadiansPerSecond =
speedMetersPerSecond *
Math.Tan(steeringRadians) /
virtualHalfWheelBaseMeters;
return Send(
new ChassisCommand(
VehicleId,
new Twist2D(
vxMetersPerSecond,
vyMetersPerSecond,
omegaRadiansPerSecond)),
interval);
}
/// <summary>
/// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
/// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
/// </summary>
public bool SendVirtualAckermannMotion(
double motionDirectionRadians,
double speedMetersPerSecond,
double steeringRadians,
TimeSpan? interval = null)
{
ValidateFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
ValidateFinite(
speedMetersPerSecond,
nameof(speedMetersPerSecond));
ValidateFinite(
steeringRadians,
nameof(steeringRadians));
EnsureMotionFrameIsActive(
motionDirectionRadians);
if (Math.Abs(steeringRadians) >=
Math.PI / 2.0)
{
throw new ArgumentOutOfRangeException(
nameof(steeringRadians),
"虚拟阿克曼转向角必须位于正负90度以内。");
}
var steeringDegrees =
(float)(
steeringRadians *
RadiansToDegrees);
var success =
_chassis.SendMotion(
(float)speedMetersPerSecond,
steeringDegrees,
-steeringDegrees,
interval);
if (!success)
{
_chassis.PredefinedDriveStop();
}
return success;
}
/// <summary>
/// 按底盘减速度配置平滑停车，需要在控制周期中持续调用。
/// </summary>
@@ -235,6 +457,14 @@ namespace MyParking.Shared
_chassis.PredefinedDriveStop();
}
/// <summary>
/// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。
/// </summary>
public void StopXYThDrivePreserveSteeringState()
{
_chassis.StopXYThDrivePreserveSteeringState();
}
/// <summary>
/// 停车并将所有舵轮转到指定的车体角度。
/// 只调整舵轮角度，不产生车辆线速度。
@@ -339,6 +569,40 @@ namespace MyParking.Shared
}
return success;
}
/// <summary>
/// 将已到位的自转舵角和轮速方向一次性交接给XYTh，
/// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。
/// </summary>
public bool AdoptPreparedSpinForXYTh(
double toleranceRadians =
2.0 * Math.PI / 180.0)
{
if (double.IsNaN(toleranceRadians) ||
double.IsInfinity(toleranceRadians) ||
toleranceRadians < 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(toleranceRadians),
"自转状态交接容差必须是非负有限值。");
}
EnsureBodyFrameIsActive();
var success =
_chassis
.AdoptPreparedRotateWheelsForXYTh(
(float)(
toleranceRadians *
RadiansToDegrees));
if (!success)
{
_chassis.PredefinedDriveStop();
}
return success;
}
/// <summary>
/// 所有舵轮是否已对齐到原地自转方向。
/// </summary>