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>