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/TireFollowing.cs
+24 -8
View File
@@ -122,6 +122,7 @@ namespace MultiWheelC
private Painter _painter;
private List<float> _remainDistanceList = 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)
{
@@ -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 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;
continue;
@@ -375,9 +376,22 @@ namespace MultiWheelC
{
lastDetectX = ((target.Src + target.Dst) / 2f).X;
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);
_remainDistanceList.Add(rd);
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
@@ -395,8 +409,8 @@ namespace MultiWheelC
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
}
WalkBlindCarPathDstX = path.CarPath.Dst.X;
WalkBlindCarPathDstY = path.CarPath.Dst.Y;
WalkBlindCarPathDstX = trackDst.X;
WalkBlindCarPathDstY = trackDst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " +
@@ -410,6 +424,7 @@ namespace MultiWheelC
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear();
_targetYList.Clear();
lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
continue;
@@ -420,8 +435,8 @@ namespace MultiWheelC
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
{
WalkBlindStage2 = true;
WalkBlindCarPathDstX = path.CarPath.Dst.X;
WalkBlindCarPathDstY = path.CarPath.Dst.Y;
WalkBlindCarPathDstX = trackDst.X;
WalkBlindCarPathDstY = trackDst.Y;
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
$"{WalkBlindCarPathDstY:F2} " +
@@ -444,11 +459,12 @@ namespace MultiWheelC
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
_remainDistanceList.Clear();
_targetYList.Clear();
continue;
}
}
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;
controller.UpdateTracks(new List<AbstractTrack> { track });
NoTarget = false;