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

update 轮胎跟踪目标点Y做N帧平均

Browse Source
This commit is contained in:
shuai.li
2026-06-30 14:21:35 +08:00
parent 32c380ace2
commit 85e365a22d
2 changed files with 27 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/PilotConfig.cs
+3
View File
@@ -184,6 +184,9 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f; [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f;
[FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1; [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1;
[FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400;
[FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f; [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
[FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
#endregion #endregion
} }
MultiWheel/MultiWheelC/TireFollowing.cs
+24 -8
View File
@@ -122,6 +122,7 @@ namespace MultiWheelC
private Painter _painter; private Painter _painter;
private List<float> _remainDistanceList = new List<float>(); private List<float> _remainDistanceList = new List<float>();
private List<float> _remainAngleList = new List<float>(); private List<float> _remainAngleList = new List<float>();
private List<float> _targetYList = new List<float>();
private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY) private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
{ {
@@ -350,7 +351,7 @@ namespace MultiWheelC
var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2f, (target.Src.Y + target.Dst.Y) / 2f); var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2f, (target.Src.Y + target.Dst.Y) / 2f);
var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2)); var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
//距离较近以后角度容易跳变 //距离较近以后角度容易跳变
if (dis2target < 1400 && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr) if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
{ {
yield return true; yield return true;
continue; continue;
@@ -375,9 +376,22 @@ namespace MultiWheelC
{ {
lastDetectX = ((target.Src + target.Dst) / 2f).X; lastDetectX = ((target.Src + target.Dst) / 2f).X;
lastDetectY = ((target.Src + target.Dst) / 2f).Y; lastDetectY = ((target.Src + target.Dst) / 2f).Y;
_painter.DrawLine(Color.GreenYellow, path.CarPath.Src, path.CarPath.Dst, endArrow: true);
var rd = (float)LessMath.PerpendicularPosition(0, 0, path.CarPath.Dst.X, path.CarPath.Dst.Y, var currentY = path.CarPath.Dst.Y;
if (Math.Abs(targetAngle) < PilotDefinition.Conf.TireFollowingAngleIgnoreThr &&
dis2target < PilotDefinition.Conf.TireFollowingCloseDistance)
{
_targetYList.Add(currentY);
while (_targetYList.Count > PilotDefinition.Conf.TireFollowingYAverageFrameCount) _targetYList.RemoveAt(0);
}
var trackDstY = _targetYList.Count > 0 ? _targetYList.Average() : currentY;
Hedingben.ToastText($"target Y:{_targetYList.Count} {trackDstY}", "target Y");
var trackDst = new Vector2(path.CarPath.Dst.X, trackDstY);
_painter.DrawLine(Color.GreenYellow, path.CarPath.Src, trackDst, endArrow: true);
var rd = (float)LessMath.PerpendicularPosition(0, 0, trackDst.X, trackDst.Y,
path.CarPath.Src.X, path.CarPath.Src.Y); path.CarPath.Src.X, path.CarPath.Src.Y);
_remainDistanceList.Add(rd); _remainDistanceList.Add(rd);
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0); while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
@@ -395,8 +409,8 @@ namespace MultiWheelC
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId); detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing"); DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
} }
WalkBlindCarPathDstX = path.CarPath.Dst.X; WalkBlindCarPathDstX = trackDst.X;
WalkBlindCarPathDstY = path.CarPath.Dst.Y; WalkBlindCarPathDstY = trackDst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh; WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " + DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " + $"{WalkBlindCarPathDstY:F2} " +
@@ -410,6 +424,7 @@ namespace MultiWheelC
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos; _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos; _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear(); _remainDistanceList.Clear();
_targetYList.Clear();
lastDetectX = detectors[detectorIndex + 1].StartGuessingX; lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
lastDetectY = detectors[detectorIndex + 1].StartGuessingY; lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
continue; continue;
@@ -420,8 +435,8 @@ namespace MultiWheelC
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd)) if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
{ {
WalkBlindStage2 = true; WalkBlindStage2 = true;
WalkBlindCarPathDstX = path.CarPath.Dst.X; WalkBlindCarPathDstX = trackDst.X;
WalkBlindCarPathDstY = path.CarPath.Dst.Y; WalkBlindCarPathDstY = trackDst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh; WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " + DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " + $"{WalkBlindCarPathDstY:F2} " +
@@ -444,11 +459,12 @@ namespace MultiWheelC
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos; _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos; _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear(); _remainDistanceList.Clear();
_targetYList.Clear();
continue; continue;
} }
} }
DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing"); DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
var track = new LineTrack(path.CarPath.Src, path.CarPath.Dst); var track = new LineTrack(path.CarPath.Src, trackDst);
track.CarDirectionBias = CarDirection; track.CarDirectionBias = CarDirection;
controller.UpdateTracks(new List<AbstractTrack> { track }); controller.UpdateTracks(new List<AbstractTrack> { track });
NoTarget = false; NoTarget = false;