First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

chore: save current workspace progress

Browse Source
This commit is contained in:
梁薄云
2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/FootprintCollisionChecker.cs
+160
View File
@@ -0,0 +1,160 @@
using System;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
/// <summary>
/// 对以车辆几何中心表示的扩大矩形执行连续碰撞检查。
/// 地图查询使用 m；任何地图外车辆部分、占据格相交或擦边均按碰撞处理。
/// </summary>
public sealed class FootprintCollisionChecker
{
/// <summary>创建连续车辆碰撞检查器。</summary>
public FootprintCollisionChecker()
{
}
/// <summary>
/// 判断单个车辆位姿是否无碰撞。
/// 参数：pose 为车辆几何中心的世界 m/rad 位姿；map 为不可变规划地图；vehicle 为车辆尺寸；
/// additionalMarginMeters 为临时额外安全余量，单位 m；bodyClearanceMeters 输出不含该临时余量的保守车体净空下界，单位 m。
/// 返回：扩大车辆矩形完整位于地图内且不与任何占据格相交或擦边时为 true；无效输入保守地返回 false。
/// </summary>
public bool IsPoseCollisionFree(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
double additionalMarginMeters, out double bodyClearanceMeters)
{
bodyClearanceMeters = 0d;
if (map == null || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d ||
!VehicleFootprint.TryCreate(pose, vehicle, 0d, out VehicleFootprint bodyFootprint) ||
!VehicleFootprint.TryCreate(pose, vehicle, additionalMarginMeters, out VehicleFootprint checkedFootprint))
return false;
if (!AreCornersInsideMap(checkedFootprint, map)) return false;
double centerDistanceMeters = map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y);
bodyClearanceMeters = GetBodyClearance(centerDistanceMeters, bodyFootprint.CircumscribedRadiusMeters);
if (centerDistanceMeters > checkedFootprint.CircumscribedRadiusMeters) return true;
return !IntersectsOccupiedCell(checkedFootprint, map);
}
/// <summary>
/// 判断两个位姿之间的平移和转向扫掠是否无碰撞。
/// 参数：from、to 为世界 m/rad 位姿；maximumCenterStepMeters 为允许的最大中心采样间距，单位 m；
/// minimumBodyClearanceMeters 输出沿途不含临时扫掠余量的保守车体净空下界，单位 m。
/// 返回：端点和每个分段扫掠均无碰撞时为 true；无效输入、地图外或任一中间碰撞时返回 false。
/// </summary>
public bool IsSweptMotionCollisionFree(Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
double maximumCenterStepMeters, out double minimumBodyClearanceMeters)
{
minimumBodyClearanceMeters = 0d;
if (map == null || from == null || to == null || !NumericGuard.IsFinite(maximumCenterStepMeters) || maximumCenterStepMeters <= 0d ||
!NumericGuard.IsFinite(from.X) || !NumericGuard.IsFinite(from.Y) || !NumericGuard.IsFinite(from.Heading) ||
!NumericGuard.IsFinite(to.X) || !NumericGuard.IsFinite(to.Y) || !NumericGuard.IsFinite(to.Heading) ||
!VehicleFootprint.TryCreate(from, vehicle, 0d, out VehicleFootprint bodyFootprint))
return false;
double allowedStepMeters = Math.Min(maximumCenterStepMeters, map.ResolutionMeters / 2d);
if (!NumericGuard.IsPositiveFinite(allowedStepMeters)) return false;
if (!IsPoseCollisionFree(from, map, vehicle, 0d, out double fromClearanceMeters)) return false;
minimumBodyClearanceMeters = fromClearanceMeters;
double deltaX = to.X - from.X;
double deltaY = to.Y - from.Y;
double centerDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
if (!NumericGuard.IsFinite(centerDistanceMeters)) return false;
double headingDeltaRadians = AngleMath.ShortestSignedDifference(from.Heading, to.Heading);
if (!NumericGuard.IsFinite(headingDeltaRadians)) return false;
double rawSegmentCount = Math.Ceiling(centerDistanceMeters / allowedStepMeters);
if (!NumericGuard.IsFinite(rawSegmentCount) || rawSegmentCount > int.MaxValue) return false;
int segmentCount = Math.Max(1, (int)rawSegmentCount);
Pose2D previousPose = from;
for (int segment = 1; segment <= segmentCount; segment++)
{
double endFraction = (double)segment / segmentCount;
double middleFraction = ((double)segment - 0.5d) / segmentCount;
var currentPose = new Pose2D(
from.X + deltaX * endFraction,
from.Y + deltaY * endFraction,
from.Heading + headingDeltaRadians * endFraction);
var middlePose = new Pose2D(
from.X + deltaX * middleFraction,
from.Y + deltaY * middleFraction,
from.Heading + headingDeltaRadians * middleFraction);
double segmentDeltaX = currentPose.X - previousPose.X;
double segmentDeltaY = currentPose.Y - previousPose.Y;
double segmentCenterDisplacementMeters = Math.Sqrt(segmentDeltaX * segmentDeltaX + segmentDeltaY * segmentDeltaY);
double segmentHeadingDeltaRadians = currentPose.Heading - previousPose.Heading;
double temporaryMarginMeters = 0.5d * (segmentCenterDisplacementMeters +
bodyFootprint.CircumscribedRadiusMeters * Math.Abs(segmentHeadingDeltaRadians));
if (!NumericGuard.IsFinite(temporaryMarginMeters) ||
!IsPoseCollisionFree(middlePose, map, vehicle, temporaryMarginMeters, out double middleClearanceMeters))
return false;
minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, middleClearanceMeters);
previousPose = currentPose;
}
if (!IsPoseCollisionFree(to, map, vehicle, 0d, out double toClearanceMeters)) return false;
minimumBodyClearanceMeters = Math.Min(minimumBodyClearanceMeters, toClearanceMeters);
return true;
}
private static bool AreCornersInsideMap(VehicleFootprint footprint, PlanningGridMap map)
{
for (int index = 0; index < 4; index++)
{
footprint.GetCorner(index, out double cornerX, out double cornerY);
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
}
return true;
}
private static double GetBodyClearance(double centerDistanceMeters, double bodyRadiusMeters)
{
if (double.IsPositiveInfinity(centerDistanceMeters)) return double.PositiveInfinity;
if (!NumericGuard.IsFinite(centerDistanceMeters) || !NumericGuard.IsFinite(bodyRadiusMeters)) return 0d;
return Math.Max(0d, centerDistanceMeters - bodyRadiusMeters);
}
private static bool IntersectsOccupiedCell(VehicleFootprint footprint, PlanningGridMap map)
{
GetCellRange(map, footprint.MinX, footprint.MaxX, footprint.MinY, footprint.MaxY,
out int firstRow, out int lastRow, out int firstCol, out int lastCol);
for (int row = firstRow; row <= lastRow; row++)
for (int col = firstCol; col <= lastCol; col++)
{
if (!map.IsOccupied(row, col)) continue;
GetCellBoundsMeters(map, row, col, out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY);
if (OrientedRectangleCellIntersection.Intersects(footprint, cellMinX, cellMaxX, cellMinY, cellMaxY)) return true;
}
return false;
}
private static void GetCellRange(PlanningGridMap map, double minX, double maxX, double minY, double maxY,
out int firstRow, out int lastRow, out int firstCol, out int lastCol)
{
double minimumMapX = map.Bounds.XMin / 1000d;
double minimumMapY = map.Bounds.YMin / 1000d;
firstCol = Clamp((int)Math.Floor((minX - minimumMapX) / map.ResolutionMeters) - 1, 0, map.Cols - 1);
lastCol = Clamp((int)Math.Floor((maxX - minimumMapX) / map.ResolutionMeters), 0, map.Cols - 1);
firstRow = Clamp((int)Math.Floor((minY - minimumMapY) / map.ResolutionMeters) - 1, 0, map.Rows - 1);
lastRow = Clamp((int)Math.Floor((maxY - minimumMapY) / map.ResolutionMeters), 0, map.Rows - 1);
}
private static void GetCellBoundsMeters(PlanningGridMap map, int row, int col,
out double cellMinX, out double cellMaxX, out double cellMinY, out double cellMaxY)
{
cellMinX = map.Bounds.XMin / 1000d + col * map.ResolutionMeters;
cellMinY = map.Bounds.YMin / 1000d + row * map.ResolutionMeters;
cellMaxX = Math.Min(map.Bounds.XMax / 1000d, cellMinX + map.ResolutionMeters);
cellMaxY = Math.Min(map.Bounds.YMax / 1000d, cellMinY + map.ResolutionMeters);
}
private static int Clamp(int value, int minimum, int maximum)
{
return value < minimum ? minimum : value > maximum ? maximum : value;
}
}
ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/OrientedRectangleCellIntersection.cs
+33
View File
@@ -0,0 +1,33 @@
using System;
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
/// <summary>旋转矩形与轴对齐栅格的分离轴相交判定。</summary>
internal static class OrientedRectangleCellIntersection
{
/// <summary>任一投影轴没有严格分离时返回 true；擦边按相交处理。</summary>
public static bool Intersects(VehicleFootprint rectangle, double cellMinX, double cellMaxX, double cellMinY, double cellMaxY)
{
if (rectangle == null || cellMaxX < cellMinX || cellMaxY < cellMinY) return false;
double cellCenterX = (cellMinX + cellMaxX) / 2d;
double cellCenterY = (cellMinY + cellMaxY) / 2d;
double cellHalfX = (cellMaxX - cellMinX) / 2d;
double cellHalfY = (cellMaxY - cellMinY) / 2d;
return !HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLongitudinalX, rectangle.AxisLongitudinalY) &&
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, rectangle.AxisLateralX, rectangle.AxisLateralY) &&
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 1d, 0d) &&
!HasStrictSeparation(rectangle, cellCenterX, cellCenterY, cellHalfX, cellHalfY, 0d, 1d);
}
private static bool HasStrictSeparation(VehicleFootprint rectangle, double cellCenterX, double cellCenterY, double cellHalfX, double cellHalfY,
double axisX, double axisY)
{
double rectangleCenter = rectangle.CenterX * axisX + rectangle.CenterY * axisY;
double cellCenter = cellCenterX * axisX + cellCenterY * axisY;
double rectangleRadius = rectangle.HalfLengthMeters * Math.Abs(rectangle.AxisLongitudinalX * axisX + rectangle.AxisLongitudinalY * axisY) +
rectangle.HalfWidthMeters * Math.Abs(rectangle.AxisLateralX * axisX + rectangle.AxisLateralY * axisY);
double cellRadius = cellHalfX * Math.Abs(axisX) + cellHalfY * Math.Abs(axisY);
return Math.Abs(rectangleCenter - cellCenter) > rectangleRadius + cellRadius;
}
}
ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/VehicleFootprint.cs
+90
View File
@@ -0,0 +1,90 @@
using System;
using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
/// <summary>以车辆几何中心为原点的扩大旋转矩形。</summary>
internal sealed class VehicleFootprint
{
private VehicleFootprint(Pose2D pose, double halfLengthMeters, double halfWidthMeters)
{
CenterX = pose.X;
CenterY = pose.Y;
HalfLengthMeters = halfLengthMeters;
HalfWidthMeters = halfWidthMeters;
AxisLongitudinalX = Math.Cos(pose.Heading);
AxisLongitudinalY = Math.Sin(pose.Heading);
AxisLateralX = -AxisLongitudinalY;
AxisLateralY = AxisLongitudinalX;
CircumscribedRadiusMeters = Math.Sqrt(halfLengthMeters * halfLengthMeters + halfWidthMeters * halfWidthMeters);
double minX = double.PositiveInfinity;
double maxX = double.NegativeInfinity;
double minY = double.PositiveInfinity;
double maxY = double.NegativeInfinity;
for (int index = 0; index < 4; index++)
{
GetCorner(index, out double x, out double y);
minX = Math.Min(minX, x);
maxX = Math.Max(maxX, x);
minY = Math.Min(minY, y);
maxY = Math.Max(maxY, y);
}
MinX = minX;
MaxX = maxX;
MinY = minY;
MaxY = maxY;
}
public double CenterX { get; }
public double CenterY { get; }
public double HalfLengthMeters { get; }
public double HalfWidthMeters { get; }
public double AxisLongitudinalX { get; }
public double AxisLongitudinalY { get; }
public double AxisLateralX { get; }
public double AxisLateralY { get; }
public double CircumscribedRadiusMeters { get; }
public double MinX { get; }
public double MaxX { get; }
public double MinY { get; }
public double MaxY { get; }
/// <summary>创建包含车辆安全余量和临时扫掠余量的矩形。</summary>
public static bool TryCreate(Pose2D pose, VehicleParameters vehicle, double additionalMarginMeters, out VehicleFootprint footprint)
{
footprint = null;
if (pose == null || vehicle == null || !NumericGuard.IsFinite(pose.X) || !NumericGuard.IsFinite(pose.Y) ||
!NumericGuard.IsFinite(pose.Heading) || !NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
!NumericGuard.IsPositiveFinite(vehicle.WidthMeters) || !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) ||
vehicle.SafetyMarginMeters < 0d || !NumericGuard.IsFinite(additionalMarginMeters) || additionalMarginMeters < 0d)
return false;
double totalMarginMeters = vehicle.SafetyMarginMeters + additionalMarginMeters;
if (!NumericGuard.IsFinite(totalMarginMeters)) return false;
double halfLengthMeters = vehicle.LengthMeters / 2d + totalMarginMeters;
double halfWidthMeters = vehicle.WidthMeters / 2d + totalMarginMeters;
if (!NumericGuard.IsPositiveFinite(halfLengthMeters) || !NumericGuard.IsPositiveFinite(halfWidthMeters)) return false;
footprint = new VehicleFootprint(pose, halfLengthMeters, halfWidthMeters);
return NumericGuard.IsFinite(footprint.CircumscribedRadiusMeters) && NumericGuard.IsFinite(footprint.MinX) &&
NumericGuard.IsFinite(footprint.MaxX) && NumericGuard.IsFinite(footprint.MinY) && NumericGuard.IsFinite(footprint.MaxY);
}
/// <summary>获取指定角点。索引按逆时针顺序为 0 到 3。</summary>
public void GetCorner(int index, out double x, out double y)
{
double longitudinalSign;
double lateralSign;
switch (index)
{
case 0: longitudinalSign = 1d; lateralSign = 1d; break;
case 1: longitudinalSign = -1d; lateralSign = 1d; break;
case 2: longitudinalSign = -1d; lateralSign = -1d; break;
case 3: longitudinalSign = 1d; lateralSign = -1d; break;
default: throw new ArgumentOutOfRangeException(nameof(index));
}
x = CenterX + longitudinalSign * HalfLengthMeters * AxisLongitudinalX + lateralSign * HalfWidthMeters * AxisLateralX;
y = CenterY + longitudinalSign * HalfLengthMeters * AxisLongitudinalY + lateralSign * HalfWidthMeters * AxisLateralY;
}
}
ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/VehicleKinematics.cs
+40
View File
@@ -0,0 +1,40 @@
using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
/// <summary>
/// 从车辆参数提取运动学限制的辅助方法。
/// 曲率单位为 1/m；当最大曲率与最小转弯半径同时给出时，始终选择更保守的较小曲率。
/// </summary>
public static class VehicleKinematics
{
/// <summary>
/// 尝试获取车辆允许的最大绝对曲率。
/// 参数：vehicle 为车辆参数；maximumCurvaturePerMeter 为输出的正有限曲率，单位 1/m。
/// 返回：至少提供一种正有限曲率限制时为 true；任一已提供限制无效或两种限制均未提供时为 false。
/// </summary>
public static bool TryGetMaximumCurvaturePerMeter(VehicleParameters vehicle, out double maximumCurvaturePerMeter)
{
maximumCurvaturePerMeter = 0d;
if (vehicle == null) return false;
bool hasMaximumCurvature = vehicle.MaximumCurvaturePerMeter.HasValue;
bool hasMinimumRadius = vehicle.MinimumTurningRadiusMeters.HasValue;
if (hasMaximumCurvature && !NumericGuard.IsPositiveFinite(vehicle.MaximumCurvaturePerMeter.Value)) return false;
if (hasMinimumRadius && !NumericGuard.IsPositiveFinite(vehicle.MinimumTurningRadiusMeters.Value)) return false;
if (!hasMaximumCurvature && !hasMinimumRadius) return false;
if (hasMaximumCurvature && hasMinimumRadius)
{
maximumCurvaturePerMeter = System.Math.Min(
vehicle.MaximumCurvaturePerMeter.Value,
1d / vehicle.MinimumTurningRadiusMeters.Value);
return true;
}
maximumCurvaturePerMeter = hasMaximumCurvature
? vehicle.MaximumCurvaturePerMeter.Value
: 1d / vehicle.MinimumTurningRadiusMeters.Value;
return NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter);
}
}