增加车队轨迹控制核心与轮组自转模式
This commit is contained in:
@@ -1 +1,201 @@
|
||||
// 输出FleetMotionCommand
|
||||
using System;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
using MultiWheelC.Control.Allocation;
|
||||
using MultiWheelC.Control.Execution;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
// 表示车队中心单周期轨迹控制的计算结果，不包含通信发送结果。
|
||||
public enum FleetControlCycleResult
|
||||
{
|
||||
Inactive = 0,
|
||||
CommandGenerated = 1,
|
||||
Completed = 2,
|
||||
Faulted = 3
|
||||
}
|
||||
|
||||
// 将公共轨迹核心生成的GCP命令转换为车队坐标系下的刚体速度命令。
|
||||
public sealed class FleetController
|
||||
{
|
||||
private readonly PathTrackingCore _trackingCore;
|
||||
|
||||
// virtualControlPointRadiusMeters必须与横向控制器采用的虚拟车队GCP半径一致。
|
||||
public FleetController(
|
||||
ILateralController lateralController,
|
||||
ILongitudinalController longitudinalController,
|
||||
GcpCommandAllocator gcpAllocator,
|
||||
double virtualControlPointRadiusMeters,
|
||||
double finishDistanceMeters = 0.04,
|
||||
double finishSpeedMetersPerSecond = 0.02,
|
||||
double finishHeadingToleranceRadians =
|
||||
3.0 * Math.PI / 180.0,
|
||||
double maximumDistanceToTrajectoryMeters = 0.30,
|
||||
double terminalBrakingPreviewMeters = 0.02,
|
||||
double terminalApproachDistanceMeters = 0.10,
|
||||
double terminalApproachGainPerSecond = 0.8,
|
||||
double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
|
||||
double curvaturePreviewSeconds = 0.20,
|
||||
double maximumCurvaturePreviewMeters = 0.12)
|
||||
{
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
virtualControlPointRadiusMeters,
|
||||
nameof(virtualControlPointRadiusMeters));
|
||||
|
||||
VirtualControlPointRadiusMeters =
|
||||
virtualControlPointRadiusMeters;
|
||||
_trackingCore = new PathTrackingCore(
|
||||
lateralController,
|
||||
longitudinalController,
|
||||
gcpAllocator,
|
||||
finishDistanceMeters,
|
||||
finishSpeedMetersPerSecond,
|
||||
finishHeadingToleranceRadians,
|
||||
maximumDistanceToTrajectoryMeters,
|
||||
terminalBrakingPreviewMeters,
|
||||
terminalApproachDistanceMeters,
|
||||
terminalApproachGainPerSecond,
|
||||
maximumTerminalApproachSpeedMetersPerSecond,
|
||||
curvaturePreviewSeconds,
|
||||
maximumCurvaturePreviewMeters);
|
||||
}
|
||||
|
||||
// 虚拟车队中心到前、后GCP的距离，单位为m。
|
||||
public double VirtualControlPointRadiusMeters { get; }
|
||||
|
||||
public double FinishDistanceMeters =>
|
||||
_trackingCore.FinishDistanceMeters;
|
||||
|
||||
public double FinishSpeedMetersPerSecond =>
|
||||
_trackingCore.FinishSpeedMetersPerSecond;
|
||||
|
||||
public double FinishHeadingToleranceRadians =>
|
||||
_trackingCore.FinishHeadingToleranceRadians;
|
||||
|
||||
public double MaximumDistanceToTrajectoryMeters =>
|
||||
_trackingCore.MaximumDistanceToTrajectoryMeters;
|
||||
|
||||
public double TerminalBrakingPreviewMeters =>
|
||||
_trackingCore.TerminalBrakingPreviewMeters;
|
||||
|
||||
public double TerminalApproachDistanceMeters =>
|
||||
_trackingCore.TerminalApproachDistanceMeters;
|
||||
|
||||
public double TerminalApproachGainPerSecond =>
|
||||
_trackingCore.TerminalApproachGainPerSecond;
|
||||
|
||||
public double MaximumTerminalApproachSpeedMetersPerSecond =>
|
||||
_trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;
|
||||
|
||||
public double CurvaturePreviewSeconds =>
|
||||
_trackingCore.CurvaturePreviewSeconds;
|
||||
|
||||
public double MaximumCurvaturePreviewMeters =>
|
||||
_trackingCore.MaximumCurvaturePreviewMeters;
|
||||
|
||||
public bool IsActive => _trackingCore.IsActive;
|
||||
|
||||
public bool IsCompleted => _trackingCore.IsCompleted;
|
||||
|
||||
public string LastFailureReason =>
|
||||
_trackingCore.LastFailureReason;
|
||||
|
||||
public Exception LastException =>
|
||||
_trackingCore.LastException;
|
||||
|
||||
public FleetState? LastFleetState { get; private set; }
|
||||
|
||||
public TrajectoryProjection? LastProjection =>
|
||||
_trackingCore.LastProjection;
|
||||
|
||||
public GcpMotionCommand? LastGcpCommand =>
|
||||
_trackingCore.LastRequestedCommand;
|
||||
|
||||
public FleetMotionCommand? LastCommand { get; private set; }
|
||||
|
||||
// 重置公共核心，并从轨迹起点开始跟踪虚拟车队中心。
|
||||
public void Start(Trajectory2D trajectory)
|
||||
{
|
||||
_trackingCore.Start(trajectory);
|
||||
LastFleetState = null;
|
||||
LastCommand = null;
|
||||
}
|
||||
|
||||
// 计算一周期车队中心命令；非运行状态、完成或故障时返回停车命令。
|
||||
public FleetControlCycleResult ComputeCommand(
|
||||
FleetState fleetState,
|
||||
double deltaTimeSeconds,
|
||||
out FleetMotionCommand command)
|
||||
{
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
deltaTimeSeconds,
|
||||
nameof(deltaTimeSeconds));
|
||||
|
||||
command = FleetMotionCommand.Stop();
|
||||
|
||||
if (!_trackingCore.IsActive)
|
||||
{
|
||||
return FleetControlCycleResult.Inactive;
|
||||
}
|
||||
|
||||
LastFleetState = fleetState;
|
||||
var output = _trackingCore.Compute(
|
||||
fleetState.FleetPoseInWorld,
|
||||
fleetState.TwistAtFleetOriginInFleet,
|
||||
fleetState.HasValidVelocityEstimate,
|
||||
deltaTimeSeconds);
|
||||
|
||||
if (output.Result ==
|
||||
PathTrackingCycleResult.Completed)
|
||||
{
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.Completed;
|
||||
}
|
||||
|
||||
if (output.Result ==
|
||||
PathTrackingCycleResult.Faulted)
|
||||
{
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.Faulted;
|
||||
}
|
||||
|
||||
if (output.Result !=
|
||||
PathTrackingCycleResult.CommandGenerated ||
|
||||
!output.Command.HasValue)
|
||||
{
|
||||
return FleetControlCycleResult.Inactive;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var twistAtFleetOrigin =
|
||||
GcpKinematics.ToBodyTwist(
|
||||
output.Command.Value,
|
||||
VirtualControlPointRadiusMeters);
|
||||
command = new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
twistAtFleetOrigin);
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.CommandGenerated;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_trackingCore.Fail(
|
||||
"车队中心GCP命令转换异常：" +
|
||||
exception.Message,
|
||||
exception);
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.Faulted;
|
||||
}
|
||||
}
|
||||
|
||||
// 取消当前轨迹并使后续周期只生成停车命令。
|
||||
public void Cancel()
|
||||
{
|
||||
_trackingCore.Cancel();
|
||||
LastFleetState = null;
|
||||
LastCommand = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
// 一次经过校验的虚拟车队原点状态快照。
|
||||
public readonly struct FleetState
|
||||
{
|
||||
public FleetState(
|
||||
double sampleTimestampSeconds,
|
||||
Pose2D fleetPoseInWorld,
|
||||
Twist2D twistAtFleetOriginInWorld,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
NumericGuard.EnsureFinite(
|
||||
fleetPoseInWorld,
|
||||
nameof(fleetPoseInWorld));
|
||||
NumericGuard.EnsureFinite(
|
||||
twistAtFleetOriginInWorld,
|
||||
nameof(twistAtFleetOriginInWorld));
|
||||
|
||||
SampleTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
FleetPoseInWorld = new Pose2D(
|
||||
fleetPoseInWorld.XMeters,
|
||||
fleetPoseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
fleetPoseInWorld.YawRadians));
|
||||
HasValidVelocityEstimate =
|
||||
hasValidVelocityEstimate;
|
||||
|
||||
// 位姿有效但速度尚未初始化时显式置零，避免控制器误用输入值。
|
||||
TwistAtFleetOriginInWorld =
|
||||
hasValidVelocityEstimate
|
||||
? twistAtFleetOriginInWorld
|
||||
: Twist2D.Zero;
|
||||
|
||||
var worldPoseInFleet =
|
||||
FrameTransform2D.Inverse(
|
||||
FleetPoseInWorld);
|
||||
TwistAtFleetOriginInFleet =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
worldPoseInFleet,
|
||||
TwistAtFleetOriginInWorld);
|
||||
}
|
||||
|
||||
// 状态源单调时钟中的采样时刻，单位为s。
|
||||
public double SampleTimestampSeconds { get; }
|
||||
|
||||
// 车队坐标系原点在世界坐标系中的实际位姿。
|
||||
public Pose2D FleetPoseInWorld { get; }
|
||||
|
||||
// 车队原点处的实际刚体速度，在世界坐标系中表达。
|
||||
public Twist2D TwistAtFleetOriginInWorld { get; }
|
||||
|
||||
// 同一刚体速度在车队坐标系中表达，供车队控制器使用。
|
||||
public Twist2D TwistAtFleetOriginInFleet { get; }
|
||||
|
||||
// 速度是否已经初始化并可用于闭环控制。
|
||||
public bool HasValidVelocityEstimate { get; }
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user