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
MedullaAdapter/DiverCartDefinition.cs
+30 -24
View File
@@ -283,8 +283,6 @@ namespace MedullaAdapter
Math.Sign(x);
var steeringDegrees =
-normalizedSteering * MaxManualTheta;
var frontTh = steeringDegrees;
var rearTh = -steeringDegrees;
ManualMode = (int)mode;
switch (mode)
@@ -292,16 +290,24 @@ namespace MedullaAdapter
case ManualControlMode.Normal:
// 普通模式统一使用车体速度命令：
// X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
// SendBodyCommand(
// vx: speed,
// vy: 0.0,
// omegaRadiansPerSecond: omega,
// interval);
Chassis.SendMotion(
var normalOmegaRadiansPerSecond =
speed *
Math.Tan(
AngleMath.DegreesToRadians(
steeringDegrees)) /
adapter.ControlPointRadiusMeters;
if (!adapter.SendBodyTwist(
new Twist2D(
speed,
frontTh,
rearTh,
interval);
0.0,
normalOmegaRadiansPerSecond),
interval))
{
adapter.StopImmediately();
Console.WriteLine(
"Normal SendMotion decomposition failed: " +
adapter.LastFailureReason);
}
break;
case ManualControlMode.Crab:
// 舵轮机械范围为[-120°,120°]。
@@ -332,13 +338,15 @@ namespace MedullaAdapter
// 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
// 复用与普通模式相同的SendMotion前后控制点解算。
if (!adapter.SendVirtualAckermannMotion(
motionDirectionRadians:
Math.PI / 2.0,
speedMetersPerSecond:
var crabOmegaRadiansPerSecond =
speed *
Math.Tan(crabSteeringRadians) /
adapter.ControlPointRadiusMeters;
if (!adapter.SendBodyTwist(
new Twist2D(
0.0,
speed,
steeringRadians:
crabSteeringRadians,
crabOmegaRadiansPerSecond),
interval))
{
adapter.StopImmediately();
@@ -380,13 +388,11 @@ namespace MedullaAdapter
// 普通安全版SendXYThSpeed只下发角速度，
// 四轮实际舵角未到位时不会开放驱动速度。
if (!adapter.Send(
new ChassisCommand(
CarNum,
new Twist2D(
0.0,
0.0,
spinOmegaRadiansPerSecond)),
if (!adapter.SendBodyTwist(
new Twist2D(
0.0,
0.0,
spinOmegaRadiansPerSecond),
interval))
{
adapter.StopImmediately();
MedullaAdapter/MedullaAdapter.csproj
+2 -2
View File
@@ -42,8 +42,8 @@
</ItemGroup>
<ItemGroup>
<Compile Include="..\Shared\Models\ChassisCommand.cs"
Link="Shared\Models\ChassisCommand.cs" />
<Compile Include="..\Shared\Models\MotionModels.cs"
Link="Shared\Models\MotionModels.cs" />
<Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
Link="Shared\Mathematics\FrameTransform2D.cs" />
MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
BIN
View File
Binary file not shown.
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.
Shared/Chassis/MultiWheelChassisAdapter.cs
+163 -78
View File
@@ -12,7 +12,9 @@ namespace MyParking.Shared
{
#region 辅助内容
private const float BiasTolerance = 0.001f;
private const double MotionDeadband = 1e-6;
private readonly MultiWheelChassis _chassis;
private double _activeMotionDirectionRadians;
/// <summary>
/// 当前适配器对应的车辆编号。
/// </summary>
@@ -31,10 +33,22 @@ namespace MyParking.Shared
/// <summary>
/// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
/// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
/// 对称四舵轮底盘中，它通常等于物理轮距的一半。
/// </summary>
public double HalfTrackWidthMeters { get; }
/// <summary>
/// 获取旧版SendMotion使用的对称前后GCP半径，单位为m。
/// </summary>
public double ControlPointRadiusMeters =>
_chassis.ControlPointRadius / 1000.0;
/// <summary>
/// 获取当前已经准备并激活的滚动运动系X轴在真实车体系中的方向，单位为rad。
/// </summary>
public double ActiveMotionDirectionRadians =>
_activeMotionDirectionRadians;
/// <summary>
/// Width of the steering-alignment speed gate, in degrees.
/// </summary>
@@ -168,6 +182,7 @@ namespace MyParking.Shared
/// <summary>
/// 激活指定运动方向对应的SendMotion坐标系。
/// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
/// 调用方必须先停车，并确认舵轮已经按该方向完成预对齐。
/// </summary>
public void ActivateMotionFrame(
double motionDirectionRadians)
@@ -176,10 +191,13 @@ namespace MyParking.Shared
motionDirectionRadians,
nameof(motionDirectionRadians));
var normalizedDirectionRadians =
AngleMath.NormalizeRadians(
motionDirectionRadians);
var biasDegrees =
ConvertRadiansToSingleDegrees(
-AngleMath.NormalizeRadians(
motionDirectionRadians),
-normalizedDirectionRadians,
nameof(motionDirectionRadians));
var currentBias =
_chassis.GetOriginBias();
@@ -194,6 +212,8 @@ namespace MyParking.Shared
biasDegrees)) <=
BiasTolerance)
{
SetActiveMotionDirection(
normalizedDirectionRadians);
return;
}
@@ -201,6 +221,19 @@ namespace MyParking.Shared
x: 0.0f,
y: 0.0f,
th: biasDegrees);
SetActiveMotionDirection(
normalizedDirectionRadians);
}
/// <summary>
/// 缓存当前运动坐标系方向，供控制周期内转换车体速度。
/// </summary>
private void SetActiveMotionDirection(
double motionDirectionRadians)
{
_activeMotionDirectionRadians =
AngleMath.NormalizeRadians(
motionDirectionRadians);
}
public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
{
@@ -251,94 +284,146 @@ namespace MyParking.Shared
if (MaximumWheelRadiusMeters <= 0.0 ||
HalfWheelBaseMeters <= 0.0 ||
HalfTrackWidthMeters <= 0.0)
HalfTrackWidthMeters <= 0.0 ||
ControlPointRadiusMeters <= 0.0)
{
throw new InvalidOperationException(
"Wheel positions cannot produce valid chassis dimensions.");
"Wheel positions and ControlPointRadius must produce valid chassis dimensions.");
}
var initialBias = _chassis.GetOriginBias();
SetActiveMotionDirection(
-AngleMath.DegreesToRadians(
initialBias.Z));
// 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。
_chassis.PreferMinimumSteeringTravel = true;
}
/// <summary>
/// 将车体坐标系速度命令发送给多舵轮底盘。
/// 将车体坐标系刚体速度统一转换为滚动SendMotion、原地自转或停车命令。
/// 非零平移命令使用调用方在运动段开始前已经准备并激活的β运动坐标系。
/// </summary>
public bool Send(ChassisCommand command, TimeSpan? interval = null)
{
if (command.VehicleId != VehicleId)
{
throw new InvalidOperationException(
$"命令车辆编号{command.VehicleId}与适配器车辆编号" +
$"{VehicleId}不一致。");
}
ValidateTwist(command.BodyTwist);
// 防止其他旧逻辑再次调用DirectionAngle或
// SetOriginBias改变底盘坐标语义。
EnsureBodyFrameIsActive();
var vxMetersPerSecond =
(float)command.BodyTwist.VxMetersPerSecond;
var vyMetersPerSecond =
(float)command.BodyTwist.VyMetersPerSecond;
var omegaDegreesPerSecond =
ConvertRadiansToSingleDegrees(
command.BodyTwist.OmegaRadiansPerSecond,
nameof(command.BodyTwist.OmegaRadiansPerSecond));
var success = _chassis.SendXYThSpeed(
vxMetersPerSecond,
vyMetersPerSecond,
omegaDegreesPerSecond,
interval,
enableDifferentialSteerFeedforward: true);
if (!success)
{
// 防止分解失败后继续执行上一条运动命令。
_chassis.PredefinedDriveStop();
}
return success;
}
/// <summary>
/// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
/// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
/// </summary>
public bool SendVirtualAckermannMotion(
double motionDirectionRadians,
double speedMetersPerSecond,
double steeringRadians,
public bool SendBodyTwist(
Twist2D bodyTwist,
TimeSpan? interval = null)
{
NumericGuard.EnsureFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
EnsureRepresentableAsSingle(
speedMetersPerSecond,
nameof(speedMetersPerSecond));
NumericGuard.EnsureFinite(
steeringRadians,
nameof(steeringRadians));
EnsureMotionFrameIsActive(
motionDirectionRadians);
ValidateTwist(bodyTwist);
if (Math.Abs(steeringRadians) >=
Math.PI / 2.0)
var linearSpeedMetersPerSecond =
Math.Sqrt(
bodyTwist.VxMetersPerSecond *
bodyTwist.VxMetersPerSecond +
bodyTwist.VyMetersPerSecond *
bodyTwist.VyMetersPerSecond);
if (linearSpeedMetersPerSecond <= MotionDeadband)
{
throw new ArgumentOutOfRangeException(
nameof(steeringRadians),
"虚拟阿克曼转向角必须位于正负90度以内。");
if (Math.Abs(
bodyTwist.OmegaRadiansPerSecond) <=
MotionDeadband)
{
StopImmediately();
return true;
}
return SendPureRotation(
bodyTwist.OmegaRadiansPerSecond,
interval);
}
var steeringDegrees =
return SendRollingTwistInActiveMotionFrame(
bodyTwist,
linearSpeedMetersPerSecond,
interval);
}
/// <summary>
/// 将车体刚体速度转换到已准备的运动坐标系，并生成该坐标系中的前后GCP方向。
/// </summary>
private bool SendRollingTwistInActiveMotionFrame(
Twist2D bodyTwist,
double linearSpeedMetersPerSecond,
TimeSpan? interval)
{
EnsureMotionFrameIsActive(
_activeMotionDirectionRadians);
var bodyPoseInMotionFrame =
new Pose2D(
0.0,
0.0,
-_activeMotionDirectionRadians);
var motionTwist =
FrameTransform2D.TransformTwistAtSamePoint(
bodyPoseInMotionFrame,
bodyTwist);
var motionVxMetersPerSecond =
motionTwist.VxMetersPerSecond;
var motionVyMetersPerSecond =
motionTwist.VyMetersPerSecond;
if (Math.Abs(motionVxMetersPerSecond) <=
MotionDeadband)
{
StopImmediately();
throw new InvalidOperationException(
"当前车体速度几乎垂直于已经准备的运动坐标系，" +
"无法由方向型前后GCP稳定表示。请停车后按目标主运动方向重新准备并激活β。");
}
var travelDirection =
Math.Sign(
motionVxMetersPerSecond);
var signedCenterSpeedMetersPerSecond =
travelDirection *
linearSpeedMetersPerSecond;
var frontVelocityYMetersPerSecond =
motionVyMetersPerSecond +
bodyTwist.OmegaRadiansPerSecond *
ControlPointRadiusMeters;
var rearVelocityYMetersPerSecond =
motionVyMetersPerSecond -
bodyTwist.OmegaRadiansPerSecond *
ControlPointRadiusMeters;
var directedVxMetersPerSecond =
travelDirection *
motionVxMetersPerSecond;
var frontAngleRadians =
Math.Atan2(
travelDirection *
frontVelocityYMetersPerSecond,
directedVxMetersPerSecond);
var rearAngleRadians =
Math.Atan2(
travelDirection *
rearVelocityYMetersPerSecond,
directedVxMetersPerSecond);
return SendGcpMotionInActiveFrame(
signedCenterSpeedMetersPerSecond,
frontAngleRadians,
rearAngleRadians,
interval);
}
/// <summary>
/// 将已经完成自转舵轮准备的纯角速度命令交给XYTh底盘解算。
/// </summary>
private bool SendPureRotation(
double omegaRadiansPerSecond,
TimeSpan? interval)
{
EnsureBodyFrameIsActive();
var success = _chassis.SendXYThSpeed(
0.0f,
0.0f,
ConvertRadiansToSingleDegrees(
steeringRadians,
nameof(steeringRadians));
var success =
_chassis.SendMotion(
(float)speedMetersPerSecond,
steeringDegrees,
-steeringDegrees,
interval);
omegaRadiansPerSecond,
nameof(omegaRadiansPerSecond)),
interval,
enableDifferentialSteerFeedforward: true);
if (!success)
{
@@ -347,11 +432,10 @@ namespace MyParking.Shared
return success;
}
/// <summary>
/// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。
/// 在当前已激活的运动坐标系中将有符号速度和前后GCP角度发送给旧版SendMotion。
/// </summary>
public bool SendGcpMotion(
private bool SendGcpMotionInActiveFrame(
double speedMetersPerSecond,
double frontAngleRadians,
double rearAngleRadians,
@@ -366,7 +450,8 @@ namespace MyParking.Shared
NumericGuard.EnsureFinite(
rearAngleRadians,
nameof(rearAngleRadians));
EnsureBodyFrameIsActive();
EnsureMotionFrameIsActive(
_activeMotionDirectionRadians);
if (Math.Abs(frontAngleRadians) >=
Math.PI / 2.0 ||
Shared/Models/ChassisCommand.cs
-153
View File
@@ -1,153 +0,0 @@
// 纯数据层：只描述坐标、速度和命令
// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
namespace MyParking.Shared
{
/// <summary>
/// 二维坐标点，X、Y单位均为米。
/// </summary>
public readonly struct Point2D
{
public Point2D(double xMeters, double yMeters)
{
XMeters = xMeters;
YMeters = yMeters;
}
public double XMeters { get; }
public double YMeters { get; }
public static Point2D Zero => new Point2D(0.0, 0.0);
}
/// <summary>
/// 二维局部坐标系在父坐标系中的位姿。
/// 位置单位为米，朝向单位为弧度，逆时针为正。
/// 具体父子关系由变量名称说明，例如RadarPoseInBody。
/// </summary>
public readonly struct Pose2D
{
public Pose2D(
double xMeters,
double yMeters,
double yawRadians)
{
XMeters = xMeters;
YMeters = yMeters;
YawRadians = yawRadians;
}
public double XMeters { get; }
public double YMeters { get; }
public double YawRadians { get; }
public Point2D Position =>
new Point2D(XMeters, YMeters);
public static Pose2D Identity =>
new Pose2D(0.0, 0.0, 0.0);
}
/// <summary>
/// 二维刚体速度。
/// 线速度单位为m/s，角速度单位为rad/s。
/// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
/// </summary>
public readonly struct Twist2D
{
public Twist2D(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond)
{
VxMetersPerSecond = vxMetersPerSecond;
VyMetersPerSecond = vyMetersPerSecond;
OmegaRadiansPerSecond = omegaRadiansPerSecond;
}
public double VxMetersPerSecond { get; }
public double VyMetersPerSecond { get; }
public double OmegaRadiansPerSecond { get; }
public static Twist2D Zero =>
new Twist2D(0.0, 0.0, 0.0);
}
/// <summary>
/// 单辆车的车体坐标系在车队坐标系中的位姿。
/// </summary>
public readonly struct VehicleLayout
{
public VehicleLayout(
int vehicleId,
Pose2D poseInFleet)
{
VehicleId = vehicleId;
PoseInFleet = poseInFleet;
}
public int VehicleId { get; }
public Pose2D PoseInFleet { get; }
}
/// <summary>
/// 车队整体运动命令，速度分量均在车队坐标系中表达。
/// </summary>
public readonly struct FleetMotionCommand
{
public FleetMotionCommand(
Point2D referencePointInFleet,
Twist2D twistAtReferencePoint)
{
ReferencePointInFleet = referencePointInFleet;
TwistAtReferencePoint = twistAtReferencePoint;
}
/// <summary>
/// 速度命令对应的参考点，也可作为自定义旋转中心。
/// </summary>
public Point2D ReferencePointInFleet { get; }
/// <summary>
/// 参考点处的车队速度。
/// </summary>
public Twist2D TwistAtReferencePoint { get; }
/// <summary>
/// 创建绕指定中心原地旋转的车队命令。
/// </summary>
public static FleetMotionCommand RotateAround(
Point2D rotationCenterInFleet,
double omegaRadiansPerSecond)
{
return new FleetMotionCommand(
rotationCenterInFleet,
new Twist2D(
0.0,
0.0,
omegaRadiansPerSecond));
}
/// <summary>
/// 创建车队停止命令。
/// </summary>
public static FleetMotionCommand Stop()
{
return new FleetMotionCommand(
Point2D.Zero,
Twist2D.Zero);
}
}
}
output/C/CommonUsage.dll
BIN
View File
Binary file not shown.
output/C/MultiWheelC.dll
BIN
View File
Binary file not shown.
output/C/MultiWheelC.pdb
BIN
View File
Binary file not shown.
output/M/CommonUsage.dll
BIN
View File
Binary file not shown.
output/M/MedullaAdapter.dll
BIN
View File
Binary file not shown.
output/M/MedullaAdapter.pdb
BIN
View File
Binary file not shown.
ref/CommonUsage.dll
BIN
View File
Binary file not shown.