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
MedullaAdapter/DiverCartDefinition.cs
+134 -31
View File
@@ -70,6 +70,9 @@ namespace MedullaAdapter
[AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
[AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")]
public float ManualSteeringAlignmentSigmaDegrees = 8.0f;
[AsInitParam(desc = "轮速诊断日志相对目录")]
public string WheelSpeedDiagnosticDirectory =
@"logs\wheel-speed";
[AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000;
[AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
[AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
@@ -91,6 +94,10 @@ namespace MedullaAdapter
[IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR;
[IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
[IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
[IOObjectMonitor(desc = "轮速诊断记录已启用")]
public bool WheelSpeedDiagnosticEnabled;
[IOObjectMonitor(desc = "轮速诊断记录状态")]
public string WheelSpeedDiagnosticStatus = "未启动";
[IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
[IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
[IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
@@ -116,6 +123,22 @@ namespace MedullaAdapter
{
DisableFromM = true;
}
// M层诊断：请求开始保存CAN轮速事件和底盘周期快照。
[IOObjectUtility]
public void StartWheelSpeedDiagnostic()
{
WheelSpeedDiagnosticEnabled = true;
WheelSpeedDiagnosticStatus = "等待创建日志文件";
}
// M层诊断：请求停止轮速记录并刷新CSV文件。
[IOObjectUtility]
public void StopWheelSpeedDiagnostic()
{
WheelSpeedDiagnosticEnabled = false;
WheelSpeedDiagnosticStatus = "等待停止并刷新日志";
}
#endregion
public override void CommunicationInit()
@@ -234,10 +257,8 @@ namespace MedullaAdapter
Math.Sign(x);
var steeringDegrees =
-normalizedSteering * MaxManualTheta;
var omega = CalculateManualOmega(
speed,
steeringDegrees,
adapter.HalfWheelBaseMeters);
var frontTh = steeringDegrees;
var rearTh = -steeringDegrees;
ManualMode = (int)mode;
switch (mode)
@@ -245,27 +266,96 @@ namespace MedullaAdapter
case ManualControlMode.Normal:
// 普通模式统一使用车体速度命令：
// X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
SendBodyCommand(
vx: speed,
vy: 0.0,
omegaRadiansPerSecond: omega,
interval);
// SendBodyCommand(
// vx: speed,
// vy: 0.0,
// omegaRadiansPerSecond: omega,
// interval);
Chassis.SendMotion(
speed,
frontTh,
rearTh,
interval);
break;
case ManualControlMode.Crab:
SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval);
// 舵轮机械范围为[-120°,120°]。
// 蟹行后虚拟轴距由原车宽度决定，比正常模式轴距短。
// 按几何比例缩小转角，使相同摇杆输入获得接近一致的曲率。
var normalSteeringRadians =
steeringDegrees *
Math.PI / 180.0;
var geometryRatio =
adapter.HalfTrackWidthMeters /
adapter.HalfWheelBaseMeters;
// +90°运动坐标系已经把虚拟左侧映射为车体后方，
// 此处保持普通模式的转向符号，避免再次取反导致左右颠倒。
var crabSteeringRadians =
Math.Atan(
geometryRatio *
Math.Tan(
normalSteeringRadians));
// 蟹行转角最终限制为±30°，为±120°机械舵角保留余量。
var maximumCrabSteeringRadians =
30.0 * Math.PI / 180.0;
crabSteeringRadians = Math.Max(
-maximumCrabSteeringRadians,
Math.Min(
maximumCrabSteeringRadians,
crabSteeringRadians));
// 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
// 复用与普通模式相同的SendMotion前后控制点解算。
if (!adapter.SendVirtualAckermannMotion(
motionDirectionRadians:
Math.PI / 2.0,
speedMetersPerSecond:
speed,
steeringRadians:
crabSteeringRadians,
interval))
{
adapter.StopImmediately();
Console.WriteLine(
"蟹行SendMotion命令分解失败，车辆已经停车：" +
adapter.LastFailureReason);
}
break;
case ManualControlMode.Spin:
// 自转时speed表示最外侧舵轮的目标切向速度。
// 根据v=omega*r换算角速度，不能把m/s与deg/s直接相乘。
var spinOmega =
// 摇杆处于中位时只清零驱动速度，保持已经准备好的
// 自转舵角；下次推动摇杆时仍会重新检查实际舵角。
if (Math.Abs(speed) < 1e-6f)
{
adapter
.StopXYThDrivePreserveSteeringState();
break;
}
// 自转时speed表示最外侧舵轮中心的目标切向速度，
// 根据v=omega*r换算为SendXYThSpeed需要的角速度。
var spinOmegaRadiansPerSecond =
speed /
adapter.MaximumWheelRadiusMeters;
SendBodyCommand(
vx: 0.0,
vy: 0.0,
omegaRadiansPerSecond: spinOmega,
interval);
// 普通安全版SendXYThSpeed只下发角速度，
// 四轮实际舵角未到位时不会开放驱动速度。
if (!adapter.Send(
new ChassisCommand(
CarNum,
new Twist2D(
0.0,
0.0,
spinOmegaRadiansPerSecond)),
interval))
{
adapter.StopImmediately();
Console.WriteLine(
"SendXYThSpeed原地自转命令分解失败，车辆已经停车：" +
adapter.LastFailureReason);
}
break;
default:
ManualMode = -1;
@@ -294,6 +384,10 @@ namespace MedullaAdapter
if (_pendingManualMode != mode)
{
adapter.StopImmediately();
// 所有模式的准备角度均按真实机械舵角表达；
// 先退出上一模式的虚拟运动坐标系，再执行预对齐。
adapter.ResetToBodyFrame();
_activeManualMode = null;
var preparationAccepted = mode switch
{
@@ -354,24 +448,33 @@ namespace MedullaAdapter
if (!aligned)
return false;
if (mode == ManualControlMode.Spin)
{
// 四轮实际舵角确认到位后只交接一次，保留PrepareSpin
// 选定的机械舵角和轮速方向，避免首条XYTh命令重新选角。
if (!adapter.AdoptPreparedSpinForXYTh(
toleranceRadians))
{
return false;
}
}
// 蟹行轮子在真实车体系中到达机械+90°后，
// 再将车体左侧激活为SendMotion的虚拟X正方向。
else if (mode == ManualControlMode.Crab)
{
adapter.ActivateMotionFrame(
Math.PI / 2.0);
}
else
{
adapter.ResetToBodyFrame();
}
_activeManualMode = mode;
_pendingManualMode = null;
return true;
}
private double CalculateManualOmega(
float speed,
float steeringDegrees,
double halfWheelBaseMeters)
{
var steeringRadians =
steeringDegrees * Math.PI / 180.0;
return speed *
Math.Tan(steeringRadians) /
Math.Max(halfWheelBaseMeters, 0.01);
}
internal void SendBodyCommand(double vx, double vy, double omegaRadiansPerSecond, TimeSpan? interval = null)
{
var adapter = GetChassisAdapter();