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

测试后可正常执行

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-14 16:19:15 +08:00
parent a13e345f83
commit 9fe8901c4c
25 changed files with 381 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Control/Abstractions/PathTrackingContext.cs
+19 -4
View File
@@ -1,6 +1,7 @@
using System;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
using MyParking.Shared;
namespace MultiWheelC.Control.Abstractions
{
@@ -17,7 +18,8 @@ namespace MultiWheelC.Control.Abstractions
TrajectoryProjection projection,
double controlReferenceSpeedMetersPerSecond,
double feedforwardCurvaturePerMeter,
double deltaTimeSeconds)
double deltaTimeSeconds,
double motionDirectionInBodyRadians = 0.0)
{
EnsureFinite(
controlReferenceSpeedMetersPerSecond,
@@ -28,6 +30,9 @@ namespace MultiWheelC.Control.Abstractions
EnsureFinitePositive(
deltaTimeSeconds,
nameof(deltaTimeSeconds));
EnsureFinite(
motionDirectionInBodyRadians,
nameof(motionDirectionInBodyRadians));
VehicleState = vehicleState;
Projection = projection;
@@ -36,6 +41,9 @@ namespace MultiWheelC.Control.Abstractions
FeedforwardCurvaturePerMeter =
feedforwardCurvaturePerMeter;
DeltaTimeSeconds = deltaTimeSeconds;
MotionDirectionInBodyRadians =
AngleMath.NormalizeRadians(
motionDirectionInBodyRadians);
}
/// <summary>
@@ -59,11 +67,18 @@ namespace MultiWheelC.Control.Abstractions
public double ControlReferenceSpeedMetersPerSecond { get; }
/// <summary>
/// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
/// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。
/// </summary>
public double ActualLongitudinalSpeedMetersPerSecond =>
VehicleState.TwistInBody
.VxMetersPerSecond;
Math.Cos(MotionDirectionInBodyRadians) *
VehicleState.TwistInBody.VxMetersPerSecond +
Math.Sin(MotionDirectionInBodyRadians) *
VehicleState.TwistInBody.VyMetersPerSecond;
/// <summary>
/// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
/// </summary>
public double MotionDirectionInBodyRadians { get; }
/// <summary>
/// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
MultiWheelC/Control/Execution/GcpCommandExecutor.cs
+22 -5
View File
@@ -13,6 +13,7 @@ namespace MultiWheelC.Control.Execution
1e-6;
private readonly MultiWheelChassisAdapter _chassisAdapter;
private readonly double _motionDirectionInBodyRadians;
private double _lastFrontAngleRadians;
private double _lastRearAngleRadians;
@@ -22,7 +23,8 @@ namespace MultiWheelC.Control.Execution
public GcpCommandExecutor(
MultiWheelChassisAdapter chassisAdapter,
double maximumGcpAngleRateRadiansPerSecond =
10.0 * Math.PI / 180.0)
10.0 * Math.PI / 180.0,
double motionDirectionInBodyRadians = 0.0)
{
_chassisAdapter = chassisAdapter ??
throw new ArgumentNullException(
@@ -30,9 +32,15 @@ namespace MultiWheelC.Control.Execution
EnsureFinitePositive(
maximumGcpAngleRateRadiansPerSecond,
nameof(maximumGcpAngleRateRadiansPerSecond));
NumericGuard.EnsureFinite(
motionDirectionInBodyRadians,
nameof(motionDirectionInBodyRadians));
MaximumGcpAngleRateRadiansPerSecond =
maximumGcpAngleRateRadiansPerSecond;
_motionDirectionInBodyRadians =
AngleMath.NormalizeRadians(
motionDirectionInBodyRadians);
}
/// <summary>
@@ -103,10 +111,19 @@ namespace MultiWheelC.Control.Execution
_lastRearAngleRadians);
LastSentCommand = limitedCommand;
var success = _chassisAdapter.SendGcpMotion(
limitedCommand.SpeedMetersPerSecond,
limitedCommand.FrontAngleRadians,
limitedCommand.RearAngleRadians,
var motionFrameTwist =
GcpKinematics.ToBodyTwist(
limitedCommand,
_chassisAdapter.ControlPointRadiusMeters);
var bodyTwist =
FrameTransform2D.TransformTwistAtSamePoint(
new Pose2D(
0.0,
0.0,
_motionDirectionInBodyRadians),
motionFrameTwist);
var success = _chassisAdapter.SendBodyTwist(
bodyTwist,
TimeSpan.FromSeconds(deltaTimeSeconds));
LastFailureReason = success
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+19 -8
View File
@@ -86,6 +86,7 @@ namespace MultiWheelC.Control.Execution
private readonly ILongitudinalController _longitudinalController;
private readonly GcpCommandAllocator _gcpAllocator;
private readonly GcpCommandExecutor _commandExecutor;
private readonly double _motionDirectionInBodyRadians;
private Trajectory2D _trajectory;
private double _terminalTravelDirection = 1.0;
@@ -112,7 +113,8 @@ namespace MultiWheelC.Control.Execution
double terminalApproachGainPerSecond = 0.8,
double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
double curvaturePreviewSeconds = 0.20,
double maximumCurvaturePreviewMeters = 0.12)
double maximumCurvaturePreviewMeters = 0.12,
double motionDirectionInBodyRadians = 0.0)
{
_stateProvider = stateProvider ??
throw new ArgumentNullException(
@@ -160,6 +162,9 @@ namespace MultiWheelC.Control.Execution
EnsureFiniteNonNegative(
maximumCurvaturePreviewMeters,
nameof(maximumCurvaturePreviewMeters));
NumericGuard.EnsureFinite(
motionDirectionInBodyRadians,
nameof(motionDirectionInBodyRadians));
if (terminalApproachDistanceMeters <=
finishDistanceMeters)
@@ -187,6 +192,9 @@ namespace MultiWheelC.Control.Execution
CurvaturePreviewSeconds = curvaturePreviewSeconds;
MaximumCurvaturePreviewMeters =
maximumCurvaturePreviewMeters;
_motionDirectionInBodyRadians =
AngleMath.NormalizeRadians(
motionDirectionInBodyRadians);
}
/// <summary>
@@ -484,7 +492,8 @@ namespace MultiWheelC.Control.Execution
projection,
controlReferenceSpeedMetersPerSecond,
feedforwardCurvaturePerMeter,
deltaTimeSeconds);
deltaTimeSeconds,
_motionDirectionInBodyRadians);
var lateralCommand =
_lateralController.Compute(context);
var commandSpeedMetersPerSecond =
@@ -617,9 +626,8 @@ namespace MultiWheelC.Control.Execution
var previewSpeedMetersPerSecond =
vehicleState.HasValidVelocityEstimate
? Math.Abs(
vehicleState.TwistInBody
.VxMetersPerSecond)
? CalculateActualLongitudinalSpeedMetersPerSecond(
vehicleState)
: Math.Abs(
controlReferenceSpeedMetersPerSecond);
@@ -931,13 +939,16 @@ namespace MultiWheelC.Control.Execution
}
/// <summary>
/// 计算车体坐标系实际纵向速度的绝对值，单位为m/s。
/// 计算车辆沿当前运动坐标系X轴实际速度的绝对值，单位为m/s。
/// </summary>
private static double CalculateActualLongitudinalSpeedMetersPerSecond(
private double CalculateActualLongitudinalSpeedMetersPerSecond(
VehicleState vehicleState)
{
return Math.Abs(
vehicleState.TwistInBody.VxMetersPerSecond);
Math.Cos(_motionDirectionInBodyRadians) *
vehicleState.TwistInBody.VxMetersPerSecond +
Math.Sin(_motionDirectionInBodyRadians) *
vehicleState.TwistInBody.VyMetersPerSecond);
}
/// <summary>
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+44 -1
View File
@@ -137,6 +137,18 @@ namespace MultiWheelC
protected virtual string ExperimentTrajectoryBaseName =>
"ProfiledStraight4m";
/// <summary>
/// 获取直线主运动方向相对车头的夹角，单位为rad。
/// </summary>
protected virtual double MotionDirectionInBodyRadians =>
0.0;
/// <summary>
/// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
/// </summary>
protected virtual bool ReturnWheelsForwardAfterCompletion =>
false;
/// <summary>
/// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
/// </summary>
@@ -190,7 +202,8 @@ namespace MultiWheelC
CruiseSpeedMetersPerSecond,
AccelerationMetersPerSecondSquared,
DecelerationMetersPerSecondSquared,
PointSpacingMeters);
PointSpacingMeters,
MotionDirectionInBodyRadians);
DrawTrajectory(trajectory);
@@ -226,6 +239,10 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
MotionDirectionInBodyRadians =
MotionDirectionInBodyRadians,
ReturnWheelsForwardAfterCompletion =
ReturnWheelsForwardAfterCompletion,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -467,6 +484,32 @@ namespace MultiWheelC
"ProfiledReverseStraight4m";
}
/// <summary>
/// 将舵轮准备到车体左前45°，以0.4m/s跟踪4m直线，停车后再恢复车头方向。
/// </summary>
[MovementTest(name = "新版控制器：45°蟹行4m直线轨迹跟踪")]
public sealed class NewControllerCrab45Straight4mTest
: NewControllerStraight4mTest
{
/// <summary>
/// 使用车体左前45°作为本次直线轨迹的固定运动方向。
/// </summary>
protected override double MotionDirectionInBodyRadians =>
Math.PI / 4.0;
/// <summary>
/// 蟹行轨迹正常完成后主动将四个舵轮恢复到车头方向。
/// </summary>
protected override bool ReturnWheelsForwardAfterCompletion =>
true;
/// <summary>
/// 将45°蟹行实验与普通前进和倒车实验的CSV名称明确区分。
/// </summary>
protected override string ExperimentTrajectoryBaseName =>
"ProfiledCrab45Straight4m";
}
/// <summary>
/// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
/// </summary>
MultiWheelC/Experiments/TestTrajectoryFactory.cs
+14 -5
View File
@@ -20,7 +20,8 @@ namespace MultiWheelC
double cruiseSpeedMetersPerSecond = 0.30,
double accelerationMetersPerSecondSquared = 0.20,
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
double pointSpacingMeters = 0.02,
double motionDirectionInBodyRadians = 0.0)
{
return CreateStraight(
startPoseInWorld,
@@ -28,7 +29,8 @@ namespace MultiWheelC
cruiseSpeedMetersPerSecond,
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared,
pointSpacingMeters);
pointSpacingMeters,
motionDirectionInBodyRadians);
}
/// <summary>
@@ -40,7 +42,8 @@ namespace MultiWheelC
double cruiseSpeedMetersPerSecond = 0.30,
double accelerationMetersPerSecondSquared = 0.20,
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
double pointSpacingMeters = 0.02,
double motionDirectionInBodyRadians = 0.0)
{
NumericGuard.EnsureFinite(
startPoseInWorld,
@@ -60,6 +63,9 @@ namespace MultiWheelC
NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
NumericGuard.EnsureFinite(
motionDirectionInBodyRadians,
nameof(motionDirectionInBodyRadians));
if (pointSpacingMeters > lengthMeters)
{
@@ -73,10 +79,13 @@ namespace MultiWheelC
pointSpacingMeters);
var points = new List<TrajectoryPoint>(
segmentCount + 1);
var worldMotionYawRadians =
startPoseInWorld.YawRadians +
motionDirectionInBodyRadians;
var directionX = travelDirection *
Math.Cos(startPoseInWorld.YawRadians);
Math.Cos(worldMotionYawRadians);
var directionY = travelDirection *
Math.Sin(startPoseInWorld.YawRadians);
Math.Sin(worldMotionYawRadians);
for (var index = 0;
index <= segmentCount;
MultiWheelC/Movements/PrepareWheelsForward.cs
+14 -4
View File
@@ -14,6 +14,11 @@ namespace MultiWheelC
/// </summary>
public class PrepareWheelsForward : MovementDefinition
{
/// <summary>
/// 获取或设置舵轮需要对准的车体方向，单位为rad；0表示车头方向。
/// </summary>
public double DirectionRadians;
/// <summary>
/// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。
/// </summary>
@@ -36,6 +41,10 @@ namespace MultiWheelC
/// </summary>
public override IEnumerable<bool> Get()
{
NumericGuard.EnsureFinite(
DirectionRadians,
nameof(DirectionRadians));
var config = PilotDefinition.Conf;
var toleranceDegrees =
ToleranceDegrees ??
@@ -75,10 +84,11 @@ namespace MultiWheelC
DateTime? alignedSince = null;
Completed = false;
if (!adapter.PrepareParallelDirection(0.0))
if (!adapter.PrepareParallelDirection(
DirectionRadians))
{
throw new InvalidOperationException(
"无法将所有舵轮下发到车体前向0°。");
"无法将所有舵轮下发到指定运动方向。");
}
try
@@ -87,7 +97,7 @@ namespace MultiWheelC
{
var aligned =
adapter.AreParallelWheelsAligned(
0.0,
DirectionRadians,
toleranceRadians);
if (aligned)
@@ -122,7 +132,7 @@ namespace MultiWheelC
}
finally
{
// 只清零驱动速度，保留已经下发的0°舵角。
// 只清零驱动速度，保留已经下发的目标舵角。
adapter.StopImmediately();
}
}
MultiWheelC/Movements/RotateInPlace.cs
+6 -9
View File
@@ -233,17 +233,14 @@ namespace MultiWheelC
var interval = now - lastCommandTime;
lastCommandTime = now;
// PID输出s为deg/s，Shared命令统一使用rad/s。
// adapter.Send最终调用普通安全版SendXYThSpeed。
// PID输出s为deg/s，Shared统一使用车体坐标系Twist2D和rad/s。
var omegaRadiansPerSecond =
(float)AngleMath.DegreesToRadians(s);
if (!adapter.Send(
new ChassisCommand(
PilotDefinition.Self.CarNum,
new Twist2D(
0.0,
0.0,
omegaRadiansPerSecond)),
if (!adapter.SendBodyTwist(
new Twist2D(
0.0,
0.0,
omegaRadiansPerSecond),
interval))
{
throw new InvalidOperationException(
MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+48 -6
View File
@@ -42,6 +42,16 @@ namespace MultiWheelC
/// </summary>
public Action<ParkingGeometricController> CycleObserver;
/// <summary>
/// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；0表示车头方向。
/// </summary>
public double MotionDirectionInBodyRadians;
/// <summary>
/// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
/// </summary>
public bool ReturnWheelsForwardAfterCompletion;
/// <summary>
/// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
/// </summary>
@@ -270,7 +280,11 @@ namespace MultiWheelC
}
var wheelPreparation =
new PrepareWheelsForward();
new PrepareWheelsForward
{
DirectionRadians =
MotionDirectionInBodyRadians
};
foreach (var keepRunning in wheelPreparation.Get())
{
if (!keepRunning)
@@ -284,15 +298,15 @@ namespace MultiWheelC
if (!wheelPreparation.Completed)
{
throw new InvalidOperationException(
"轨迹跟踪开始前舵轮未能稳定回到车头方向。");
"轨迹跟踪开始前舵轮未能稳定到达目标运动方向。");
}
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
// 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
adapter.ResetToBodyFrame();
adapter.ActivateMotionFrame(
MotionDirectionInBodyRadians);
var stateProvider =
StateProvider ??
@@ -333,7 +347,8 @@ namespace MultiWheelC
var commandExecutor =
new GcpCommandExecutor(
adapter,
maximumGcpAngleRateRadiansPerSecond);
maximumGcpAngleRateRadiansPerSecond,
MotionDirectionInBodyRadians);
Controller = new ParkingGeometricController(
stateProvider,
@@ -350,7 +365,8 @@ namespace MultiWheelC
terminalApproachGainPerSecond,
maximumTerminalApproachSpeedMetersPerSecond,
stanleyCurvaturePreviewSeconds,
stanleyMaximumCurvaturePreviewMeters);
stanleyMaximumCurvaturePreviewMeters,
MotionDirectionInBodyRadians);
var clock = Stopwatch.StartNew();
var previousCycleSeconds =
@@ -425,6 +441,28 @@ namespace MultiWheelC
Controller.Cancel();
}
if (ReturnWheelsForwardAfterCompletion)
{
var forwardPreparation =
new PrepareWheelsForward();
foreach (var keepRunning in
forwardPreparation.Get())
{
if (!keepRunning)
{
break;
}
yield return true;
}
if (!forwardPreparation.Completed)
{
throw new InvalidOperationException(
"轨迹完成后舵轮未能稳定回到车头方向。");
}
}
yield return false;
}
@@ -440,6 +478,10 @@ namespace MultiWheelC
"新版轨迹跟踪动作没有设置Trajectory。");
}
NumericGuard.EnsureFinite(
MotionDirectionInBodyRadians,
nameof(MotionDirectionInBodyRadians));
if (double.IsNaN(executionTimeoutSeconds) ||
double.IsInfinity(executionTimeoutSeconds) ||
executionTimeoutSeconds <= 0.0)
MultiWheelC/build/Clumsy/CommonUsage.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.pdb
BIN
View File
Binary file not shown.