First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Update fleet crab walk control

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-01 20:34:22 +08:00
parent 1c46d85587
commit 63ef3c8bb8
5 changed files with 298 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/MovementTests.cs
+109 -70
View File
@@ -364,39 +364,32 @@ public class FleetRotateInPlaceTest : MovementTest
}
// ===== 车队联动-自动蟹行动作 =====
// 以当前车队中心为起点，构造与车队朝向夹角 x、长度 y 的直线路径；执行侧复用
// TickMultiVehicle 的脚本手动等价输入(mode=1)，也就是 FleetRemote 手动蟹行同一条下发链路。
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
//
// 手动蟹行已验证丝滑，自动动作只额外做两件事：
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度和横向偏差；
// 2) 用小幅、带斜率限制的方向修正写 MultiVehicleScriptVx/Vy，避免几何控制器 bias/dTh 阶跃造成抖动。
//
// 与 FleetRemote 手动蟹行(mode==1)对照：TickMultiVehicle 仍负责合成 frontTh==rearTh 的蟹行舵角并广播给从车。
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
//
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
public class FleetCrabWalk : MovementDefinition
{
/// <summary>与当前车队朝向的夹角(deg，逆时针为正)。</summary>
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
public float CrabAngleDeg = 45f;
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
public float BodyToPathAngleDeg = 45f;
/// <summary>路径长度(mm)。</summary>
public float CrabLengthMm = 2000f;
/// <summary>行驶速度(m/s)。</summary>
public float CrabSpeed = 0.2f;
/// <summary>兼容旧配置；当前脚本手动等价实现不再直接使用几何控制器 gcp 上限。</summary>
/// <summary>前后 GCP 舵角修正上限(deg)。</summary>
public float GcpThetaThreshold = 95f;
/// <summary>横向误差转向增益，沿用 Stanley 形式：atan(gain * lateral / speed)。</summary>
public float CorrectionGain = 1f;
/// <summary>自动纠偏最大改向角(deg)。越小越接近手动蟹行，越大收敛越快。</summary>
public float CorrectionAngleThreshold = 8f;
/// <summary>脚本 Vx/Vy 命令斜率限制(m/s^2)，避免纠偏量变化造成舵角阶跃。</summary>
public float CommandAccel = 0.4f;
private bool _stopping;
private void Cleanup()
@@ -420,11 +413,20 @@ public class FleetCrabWalk : MovementDefinition
Cleanup();
}
private static float Slew(float current, float target, float maxStep)
private static float Clamp(float value, float min, float max)
{
var diff = target - current;
if (Math.Abs(diff) <= maxStep) return target;
return current + Math.Sign(diff) * maxStep;
if (value < min) return min;
if (value > max) return max;
return value;
}
private static float ClampAbs(float value, float limit)
{
var absLimit = Math.Abs(limit);
if (absLimit <= 0) return value;
if (value > absLimit) return absLimit;
if (value < -absLimit) return -absLimit;
return value;
}
public override IEnumerable<bool> Get()
@@ -437,7 +439,8 @@ public class FleetCrabWalk : MovementDefinition
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
$"manualLike=true corrGain={CorrectionGain:0.00} corrMax={CorrectionAngleThreshold:0.0} accel={CommandAccel:0.00}",
$"autoFields=true pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
$"gcpLimit={GcpThetaThreshold:0.0} biasFac={conf.BiasFac:0.00} dthFac={conf.DthLinearFac:0.00}",
"FleetCrabDbg");
if (conf.MultiVehicleMasterEndpoint != "/")
@@ -458,25 +461,34 @@ public class FleetCrabWalk : MovementDefinition
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
var dst = CommonMath.Transform2D(new Vector2(x0, y0), phi, new Vector2(CrabLengthMm, 0));
var phiRad = phi / 180.0 * Math.PI;
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
DLog.Log(
$"START center=({x0:0},{y0:0},{theta:0.0}) crabAngle={CrabAngleDeg:0.0} phi={phi:0.0} " +
$"START center=({x0:0},{y0:0},{theta:0.0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
$"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
$"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}",
"FleetCrabDbg");
// 手动蟹行已验证丝滑：这里复用 TickMultiVehicle 的脚本手动等价输入(mode=1)，
// 只在本动作内根据车队中心相对直线的横向误差缓慢调整 Vx/Vy 方向。
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleScriptMode = 1;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;
self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoIdealX = x0;
self.MultiVehicleAutoIdealY = y0;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
self.PrimeMasterAutoFromSlam();
DLog.Log("WARMUP 已启用脚本蟹行(mode=1)，等待编队成员就位…", "FleetCrabDbg");
DLog.Log("WARMUP auto fields enabled, waiting for fleet members...", "FleetCrabDbg");
var warmEnd = DateTime.Now.AddSeconds(2.0);
var warmIter = 0;
@@ -484,8 +496,17 @@ public class FleetCrabWalk : MovementDefinition
while (!_stopping && DateTime.Now < warmEnd)
{
warmIter++;
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleScriptMode = 1;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoIdealX = x0;
self.MultiVehicleAutoIdealY = y0;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
self.PrimeMasterAutoFromSlam();
var snap = self.GetFleetCenterSnapshot();
int cnt;
@@ -493,7 +514,7 @@ public class FleetCrabWalk : MovementDefinition
if (warmIter % 5 == 0)
DLog.Log(
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
$"scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
$"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum}",
"FleetCrabDbg");
if (cnt >= conf.MultiVehicleFleetNum)
{
@@ -506,27 +527,30 @@ public class FleetCrabWalk : MovementDefinition
yield return true;
}
if (!warmReady)
DLog.Log("WARMUP 超时：编队仍未就位，继续进入脚本蟹行（若不动请查看 FleetDiagClumsy ready/cnt）",
DLog.Log("WARMUP timeout: fleet members are not ready; continue with auto fields and safety interlock.",
"FleetCrabDbg");
Hedingben.ToastText($"车队蟹行 夹角{CrabAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
Hedingben.ToastText($"车队蟹行 路径{CrabAngleDeg:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{CrabLengthMm:0}mm", "FleetCrab");
var iter = 0;
var cmdVx = 0f;
var cmdVy = 0f;
var lastTime = DateTime.Now;
var lastLog = DateTime.MinValue;
var finishDistance = Math.Max(20f, conf.FinishDistance);
var slowDistance = Math.Max(finishDistance + 1f, conf.SlowDistance);
var baseSpeed = Math.Abs(CrabSpeed);
var finishSpeed = Math.Min(baseSpeed, Math.Abs(conf.FinishSpeed));
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
var stopReason = "done";
while (!_stopping)
{
iter++;
var now = DateTime.Now;
var dt = (float)Math.Min(0.2, Math.Max(0.001, (now - lastTime).TotalSeconds));
lastTime = now;
self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth);
if (!self.TryGetFleetCenterFromSlam(out var cx, out var cy, out var cth))
{
stopReason = "fleet center invalid";
DLog.Log("ABORT: TryGetFleetCenterFromSlam returned false during auto crab.", "FleetCrabDbg");
break;
}
var delta = new Vector2(cx - x0, cy - y0);
var along = Vector2.Dot(delta, pathDir);
var lateral = Vector2.Dot(delta, pathLeft);
@@ -534,29 +558,37 @@ public class FleetCrabWalk : MovementDefinition
if (remain <= finishDistance)
break;
var speed = Math.Abs(CrabSpeed);
if (remain < conf.SlowDistance && conf.SlowDistance > 1)
var speed = baseSpeed;
if (remain < slowDistance)
{
var ratio = (float)Math.Pow(Math.Max(0, remain) / conf.SlowDistance, conf.SlowingPow);
speed = ratio * (speed - Math.Abs(conf.FinishSpeed)) + Math.Abs(conf.FinishSpeed);
var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow);
speed = ratio * (baseSpeed - finishSpeed) + finishSpeed;
}
var correction = (float)(-Math.Atan(CorrectionGain * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
correction = Math.Sign(correction) * Math.Min(Math.Abs(correction), Math.Abs(CorrectionAngleThreshold));
var desiredWorldAngle = CommonMath.RoundTh(phi + correction);
var localAngle = (float)CommonMath.ThDiff(desiredWorldAngle, cth);
var localRad = localAngle / 180.0 * Math.PI;
var targetVx = speed * (float)Math.Cos(localRad);
var targetVy = speed * (float)Math.Sin(localRad);
var maxStep = Math.Max(0.05f, CommandAccel) * dt;
cmdVx = Slew(cmdVx, targetVx, maxStep);
cmdVy = Slew(cmdVy, targetVy, maxStep);
var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
var dthItem = ClampAbs(conf.DthLinearFac * headingErr, conf.DthLinearThreshold);
var biasItem = (float)(-Math.Atan(conf.BiasFac * lateral / 1000f / Math.Max(speed, 0.3f)) / Math.PI * 180.0);
biasItem = ClampAbs(biasItem, conf.BiasThreshold);
var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit);
var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit);
var idealAlong = Clamp(along, 0f, CrabLengthMm);
var ideal = new Vector2(x0, y0) + pathDir * idealAlong;
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleScriptMode = 1;
self.MultiVehicleScriptVx = cmdVx;
self.MultiVehicleScriptVy = cmdVy;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;
self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = speed;
self.MultiVehicleAutoFrontTh = frontTh;
self.MultiVehicleAutoRearTh = rearTh;
self.MultiVehicleAutoIdealX = ideal.X;
self.MultiVehicleAutoIdealY = ideal.Y;
self.MultiVehicleAutoIdealTh = targetBodyTh;
self.MultiVehicleAutoHasIdeal = true;
self.MultiVehicleAutoCmdTime = DateTime.Now;
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
{
@@ -566,8 +598,11 @@ public class FleetCrabWalk : MovementDefinition
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
DLog.Log(
$"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
$"along={along:0} lateral={lateral:0} remain={remain:0} corr={correction:0.0} " +
$"localAngle={localAngle:0.0} cmd=({cmdVx:0.000},{cmdVy:0.000}) cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
$"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
$"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
$"auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
$"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
$"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
"FleetCrabDbg");
}
yield return true;
@@ -576,14 +611,20 @@ public class FleetCrabWalk : MovementDefinition
if (_stopping)
stopReason = "stop";
self.MultiVehicleScriptVx = 0;
self.MultiVehicleScriptVy = 0;
self.MultiVehicleScriptVth = 0;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoCmdTime = DateTime.Now;
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
while (!_stopping && DateTime.Now < settleEnd)
{
self.MultiVehicleScriptEnabled = true;
self.MultiVehicleScriptMode = 1;
self.MultiVehicleScriptEnabled = false;
self.MultiVehicleScriptMode = 0;
self.MultiVehicleAutoEnabled = true;
self.MultiVehicleAutoVx = 0;
self.MultiVehicleAutoFrontTh = 0;
self.MultiVehicleAutoRearTh = 0;
self.MultiVehicleAutoCmdTime = DateTime.Now;
yield return true;
}
@@ -604,12 +645,10 @@ public class FleetCrabWalkTest : MovementTest
_proc = new FleetCrabWalk
{
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
CorrectionGain = PilotDefinition.Conf.FleetCrabCorrectionGain,
CorrectionAngleThreshold = PilotDefinition.Conf.FleetCrabCorrectionAngleDeg,
CommandAccel = PilotDefinition.Conf.FleetCrabCommandAccel
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
};
_task = new DriveTask(_proc.Get());
_task.Wait();