完善蟹行虚拟阿克曼与SendMotion运动坐标系，并添加轮速诊断日志
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -20,7 +20,16 @@ namespace MultiWheelC
|
||||
SCurve = 2
|
||||
}
|
||||
|
||||
public enum ChassisCommandBackend
|
||||
{
|
||||
SendXYThSpeed = 0,
|
||||
SendMotion = 1,
|
||||
VirtualAckermann = 2
|
||||
}
|
||||
|
||||
public ReferencePathKind PathKind;
|
||||
public ChassisCommandBackend CommandBackend =
|
||||
ChassisCommandBackend.SendMotion;
|
||||
public Vector2 StartPosition;
|
||||
public double InitialBodyYawRadians;
|
||||
public float LengthMillimeters = 4000f;
|
||||
@@ -36,6 +45,8 @@ namespace MultiWheelC
|
||||
public double HeadingGainPerSecond = 1.5;
|
||||
public double MaximumAngularSpeedRadiansPerSecond =
|
||||
30.0 * Math.PI / 180.0;
|
||||
public double MaximumVirtualSteeringRadians =
|
||||
30.0 * Math.PI / 180.0;
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
public float WheelAlignmentStableSeconds = 0.3f;
|
||||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||||
@@ -104,6 +115,15 @@ namespace MultiWheelC
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 舵轮已按真实机械角度完成预对齐；
|
||||
// 现在由Shared适配层激活SendMotion虚拟运动坐标系。
|
||||
adapter.ActivateMotionFrame(
|
||||
MotionFrameYawInBodyRadians);
|
||||
}
|
||||
|
||||
var trackingStarted = DateTime.Now;
|
||||
while (true)
|
||||
{
|
||||
@@ -179,8 +199,20 @@ namespace MultiWheelC
|
||||
-motionSin * worldVx +
|
||||
motionCos * worldVy;
|
||||
|
||||
// 虚拟阿克曼不能直接执行运动坐标系横向速度，
|
||||
// 因此将横向纠偏量转换为期望航向修正。
|
||||
var courseCorrection =
|
||||
CommandBackend ==
|
||||
ChassisCommandBackend.VirtualAckermann
|
||||
? Math.Atan2(
|
||||
normalCorrection,
|
||||
Math.Max(
|
||||
speed,
|
||||
MinimumSpeed))
|
||||
: 0.0;
|
||||
var desiredBodyYaw =
|
||||
tangentYaw -
|
||||
tangentYaw +
|
||||
courseCorrection -
|
||||
MotionFrameYawInBodyRadians;
|
||||
var headingError =
|
||||
FrameTransform2D
|
||||
@@ -194,31 +226,166 @@ namespace MultiWheelC
|
||||
omega,
|
||||
MaximumAngularSpeedRadiansPerSecond);
|
||||
|
||||
// 运动坐标系相对车体系旋转+90°：
|
||||
// 运动系正向速度会转换成车体系+Y速度。
|
||||
var bodyTwist =
|
||||
FrameTransform2D
|
||||
.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionFrameYawInBodyRadians),
|
||||
new Twist2D(
|
||||
vxInMotion,
|
||||
vyInMotion,
|
||||
omega));
|
||||
|
||||
var now = DateTime.Now;
|
||||
var interval = now - lastCommandTime;
|
||||
lastCommandTime = now;
|
||||
var command = new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
bodyTwist);
|
||||
|
||||
if (!adapter.Send(command, interval))
|
||||
bool commandAccepted;
|
||||
Twist2D bodyTwist;
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 运动坐标系相对车体系旋转+90°：
|
||||
// 运动系正向速度会转换成车体系+Y速度。
|
||||
bodyTwist =
|
||||
FrameTransform2D
|
||||
.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionFrameYawInBodyRadians),
|
||||
new Twist2D(
|
||||
vxInMotion,
|
||||
vyInMotion,
|
||||
omega));
|
||||
|
||||
// 将运动坐标系原点和前后几何控制点处的速度，
|
||||
// 转换为SendMotion需要的前后轴方向。
|
||||
var controlPointRadiusMeters =
|
||||
Math.Max(
|
||||
chassis.ControlPointRadius /
|
||||
1000.0,
|
||||
0.001);
|
||||
var frontVelocityY =
|
||||
vyInMotion +
|
||||
omega *
|
||||
controlPointRadiusMeters;
|
||||
var rearVelocityY =
|
||||
vyInMotion -
|
||||
omega *
|
||||
controlPointRadiusMeters;
|
||||
var frontSteeringRadians =
|
||||
Math.Atan2(
|
||||
frontVelocityY,
|
||||
vxInMotion);
|
||||
var rearSteeringRadians =
|
||||
Math.Atan2(
|
||||
rearVelocityY,
|
||||
vxInMotion);
|
||||
|
||||
// 蟹行测试绕过M层ManualControl并直接调用SendMotion，
|
||||
// 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
|
||||
if (IsCrabMotionFrame())
|
||||
{
|
||||
var geometryRatio =
|
||||
adapter.HalfTrackWidthMeters /
|
||||
adapter.HalfWheelBaseMeters;
|
||||
|
||||
frontSteeringRadians =
|
||||
ConvertToCrabSteering(
|
||||
frontSteeringRadians,
|
||||
geometryRatio);
|
||||
rearSteeringRadians =
|
||||
ConvertToCrabSteering(
|
||||
rearSteeringRadians,
|
||||
geometryRatio);
|
||||
}
|
||||
|
||||
var frontThetaDegrees =
|
||||
(float)(
|
||||
frontSteeringRadians *
|
||||
180.0 / Math.PI);
|
||||
var rearThetaDegrees =
|
||||
(float)(
|
||||
rearSteeringRadians *
|
||||
180.0 / Math.PI);
|
||||
var motionSpeed =
|
||||
(float)Math.Sqrt(
|
||||
vxInMotion * vxInMotion +
|
||||
vyInMotion * vyInMotion);
|
||||
|
||||
commandAccepted =
|
||||
chassis.SendMotion(
|
||||
motionSpeed,
|
||||
frontThetaDegrees,
|
||||
rearThetaDegrees,
|
||||
interval);
|
||||
}
|
||||
else if (CommandBackend ==
|
||||
ChassisCommandBackend
|
||||
.VirtualAckermann)
|
||||
{
|
||||
// 虚拟阿克曼以运动坐标系X轴为前向，
|
||||
// 通过曲率控制转弯，不直接下发横向纠偏速度。
|
||||
var virtualHalfWheelBaseMeters =
|
||||
adapter.HalfTrackWidthMeters;
|
||||
var steeringRadians =
|
||||
Math.Atan2(
|
||||
omega *
|
||||
virtualHalfWheelBaseMeters,
|
||||
speed);
|
||||
steeringRadians = Limit(
|
||||
steeringRadians,
|
||||
MaximumVirtualSteeringRadians);
|
||||
|
||||
// 转向限幅后重新计算实际可下发角速度，
|
||||
// 保证记录值与底盘最终收到的命令一致。
|
||||
var acceptedOmega =
|
||||
speed *
|
||||
Math.Tan(steeringRadians) /
|
||||
virtualHalfWheelBaseMeters;
|
||||
bodyTwist = new Twist2D(
|
||||
speed *
|
||||
Math.Cos(
|
||||
MotionFrameYawInBodyRadians),
|
||||
speed *
|
||||
Math.Sin(
|
||||
MotionFrameYawInBodyRadians),
|
||||
acceptedOmega);
|
||||
|
||||
commandAccepted =
|
||||
adapter.SendVirtualAckermann(
|
||||
MotionFrameYawInBodyRadians,
|
||||
speed,
|
||||
steeringRadians,
|
||||
virtualHalfWheelBaseMeters,
|
||||
interval);
|
||||
}
|
||||
else if (CommandBackend ==
|
||||
ChassisCommandBackend
|
||||
.SendXYThSpeed)
|
||||
{
|
||||
// 安全XYTh后端根据舵角误差统一压低驱动轮速。
|
||||
bodyTwist =
|
||||
FrameTransform2D
|
||||
.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionFrameYawInBodyRadians),
|
||||
new Twist2D(
|
||||
vxInMotion,
|
||||
vyInMotion,
|
||||
omega));
|
||||
var command = new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
bodyTwist);
|
||||
commandAccepted =
|
||||
adapter.Send(
|
||||
command,
|
||||
interval);
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"蟹行轨迹底盘解算失败：" +
|
||||
adapter.LastFailureReason);
|
||||
$"不支持的底盘命令后端：{CommandBackend}。");
|
||||
}
|
||||
|
||||
if (!commandAccepted)
|
||||
throw new InvalidOperationException(
|
||||
"运动坐标系轨迹底盘解算失败：" +
|
||||
chassis
|
||||
.LastMotionDecomposeFailureReason);
|
||||
|
||||
CommandObserver?.Invoke(
|
||||
(float)bodyTwist.VxMetersPerSecond,
|
||||
@@ -232,12 +399,46 @@ namespace MultiWheelC
|
||||
finally
|
||||
{
|
||||
adapter.StopImmediately();
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 测试退出后恢复真实车体坐标系，避免影响后续测试。
|
||||
adapter.ResetToBodyFrame();
|
||||
}
|
||||
CommandObserver?.Invoke(0f, 0f, 0f);
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
// 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
|
||||
private bool IsCrabMotionFrame()
|
||||
{
|
||||
return Math.Abs(
|
||||
FrameTransform2D
|
||||
.ShortestAngleDifference(
|
||||
Math.PI / 2.0,
|
||||
MotionFrameYawInBodyRadians)) <
|
||||
1e-6;
|
||||
}
|
||||
|
||||
// 按车体几何比例缩小蟹行转角。
|
||||
// +90°运动坐标系已经完成方向映射，此处不能再次反号。
|
||||
private double ConvertToCrabSteering(
|
||||
double normalSteeringRadians,
|
||||
double geometryRatio)
|
||||
{
|
||||
var crabSteeringRadians =
|
||||
Math.Atan(
|
||||
geometryRatio *
|
||||
Math.Tan(
|
||||
normalSteeringRadians));
|
||||
|
||||
return Limit(
|
||||
crabSteeringRadians,
|
||||
MaximumVirtualSteeringRadians);
|
||||
}
|
||||
|
||||
// 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
|
||||
private void CalculateReference(
|
||||
Vector2 currentPosition,
|
||||
@@ -642,10 +843,15 @@ namespace MultiWheelC
|
||||
!IsFinite(SlowDistanceMillimeters) ||
|
||||
FinishDistanceMillimeters < 0f ||
|
||||
!IsFinite(FinishDistanceMillimeters) ||
|
||||
TrackingTimeoutSeconds <= 0f ||
|
||||
!IsFinite(TrackingTimeoutSeconds))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
"蟹行轨迹测试参数无效。");
|
||||
TrackingTimeoutSeconds <= 0f ||
|
||||
!IsFinite(TrackingTimeoutSeconds) ||
|
||||
MaximumVirtualSteeringRadians <= 0.0 ||
|
||||
MaximumVirtualSteeringRadians >=
|
||||
Math.PI / 2.0 ||
|
||||
!IsFinite(
|
||||
MaximumVirtualSteeringRadians))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
"蟹行轨迹测试参数无效。");
|
||||
}
|
||||
|
||||
private static double Limit(
|
||||
|
||||
Reference in New Issue
Block a user