增加车队布局与刚体速度分配基础
This commit is contained in:
@@ -0,0 +1 @@
|
||||
// M层负责串口读写、校验、收发状态
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,139 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetKinematicsTests
|
||||
{
|
||||
private const double Tolerance = 1e-9;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
var layout = CreateTailToTailLayout();
|
||||
|
||||
VerifyTranslation(layout);
|
||||
VerifyRotationAroundFleetCenter(layout);
|
||||
VerifyRotationAroundFirstVehicle(layout);
|
||||
VerifyStop(layout);
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetKinematics刚体速度分配测试通过。共4个场景。");
|
||||
}
|
||||
|
||||
private static FleetLayout CreateTailToTailLayout()
|
||||
{
|
||||
return new FleetLayout(
|
||||
new[]
|
||||
{
|
||||
new VehicleLayout(
|
||||
1,
|
||||
new Pose2D(1.0, 0.0, 0.0)),
|
||||
new VehicleLayout(
|
||||
2,
|
||||
new Pose2D(-1.0, 0.0, Math.PI))
|
||||
});
|
||||
}
|
||||
|
||||
private static void VerifyTranslation(FleetLayout layout)
|
||||
{
|
||||
var commands = FleetKinematics.Decompose(
|
||||
layout,
|
||||
new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
new Twist2D(0.4, 0.0, 0.0)));
|
||||
|
||||
AssertTwist(Find(commands, 1), 0.4, 0.0, 0.0);
|
||||
AssertTwist(Find(commands, 2), -0.4, 0.0, 0.0);
|
||||
}
|
||||
|
||||
private static void VerifyRotationAroundFleetCenter(
|
||||
FleetLayout layout)
|
||||
{
|
||||
var commands = FleetKinematics.Decompose(
|
||||
layout,
|
||||
FleetMotionCommand.RotateAround(
|
||||
Point2D.Zero,
|
||||
0.2));
|
||||
|
||||
AssertTwist(Find(commands, 1), 0.0, 0.2, 0.2);
|
||||
AssertTwist(Find(commands, 2), 0.0, 0.2, 0.2);
|
||||
}
|
||||
|
||||
private static void VerifyRotationAroundFirstVehicle(
|
||||
FleetLayout layout)
|
||||
{
|
||||
var commands = FleetKinematics.Decompose(
|
||||
layout,
|
||||
FleetMotionCommand.RotateAround(
|
||||
new Point2D(1.0, 0.0),
|
||||
0.2));
|
||||
|
||||
AssertTwist(Find(commands, 1), 0.0, 0.0, 0.2);
|
||||
AssertTwist(Find(commands, 2), 0.0, 0.4, 0.2);
|
||||
}
|
||||
|
||||
private static void VerifyStop(FleetLayout layout)
|
||||
{
|
||||
var commands = FleetKinematics.Decompose(
|
||||
layout,
|
||||
FleetMotionCommand.Stop());
|
||||
|
||||
AssertTwist(Find(commands, 1), 0.0, 0.0, 0.0);
|
||||
AssertTwist(Find(commands, 2), 0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
private static FleetMemberCommand Find(
|
||||
IReadOnlyList<FleetMemberCommand> commands,
|
||||
int vehicleId)
|
||||
{
|
||||
for (var index = 0; index < commands.Count; index++)
|
||||
{
|
||||
if (commands[index].VehicleId == vehicleId)
|
||||
{
|
||||
return commands[index];
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
$"没有找到车辆{vehicleId}的分配命令。");
|
||||
}
|
||||
|
||||
private static void AssertTwist(
|
||||
FleetMemberCommand command,
|
||||
double expectedVx,
|
||||
double expectedVy,
|
||||
double expectedOmega)
|
||||
{
|
||||
AssertNear(
|
||||
command.TwistInVehicleBody.VxMetersPerSecond,
|
||||
expectedVx,
|
||||
command.VehicleId,
|
||||
"Vx");
|
||||
AssertNear(
|
||||
command.TwistInVehicleBody.VyMetersPerSecond,
|
||||
expectedVy,
|
||||
command.VehicleId,
|
||||
"Vy");
|
||||
AssertNear(
|
||||
command.TwistInVehicleBody.OmegaRadiansPerSecond,
|
||||
expectedOmega,
|
||||
command.VehicleId,
|
||||
"Omega");
|
||||
}
|
||||
|
||||
private static void AssertNear(
|
||||
double actual,
|
||||
double expected,
|
||||
int vehicleId,
|
||||
string valueName)
|
||||
{
|
||||
if (Math.Abs(actual - expected) > Tolerance)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"车辆{vehicleId}的{valueName}错误：" +
|
||||
$"actual={actual:F9}, expected={expected:F9}。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -36,6 +36,7 @@ namespace MultiWheelC.Tests
|
||||
|
||||
Console.WriteLine(
|
||||
"Stanley前进/倒车横向符号测试通过。共8个场景。");
|
||||
FleetKinematicsTests.Run();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
// 输出FleetMotionCommand
|
||||
@@ -0,0 +1 @@
|
||||
// 只在主车激活，完成车队轨迹控制和命令分配
|
||||
@@ -0,0 +1 @@
|
||||
// 主车、从车都有，负责执行分配给本车的命令
|
||||
@@ -0,0 +1 @@
|
||||
// 检查通信超时、成员故障、定位状态、夹臂状态和相对误差
|
||||
@@ -0,0 +1 @@
|
||||
// 主车汇总各车状态、时间对齐、计算车队中心和相对布局误差；输出FleetState
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1,76 @@
|
||||
// 把车队整体速度分解为每辆车的局部速度
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
// 纯数学地把车队参考点速度分解成各成员车体系中的刚体速度。
|
||||
public static class FleetKinematics
|
||||
{
|
||||
public static IReadOnlyList<FleetMemberCommand> Decompose(
|
||||
FleetLayout layout,
|
||||
FleetMotionCommand command)
|
||||
{
|
||||
if (layout == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(layout));
|
||||
}
|
||||
|
||||
EnsureCommandIsFinite(command);
|
||||
|
||||
var commands =
|
||||
new FleetMemberCommand[layout.VehicleCount];
|
||||
var referencePoint = command.ReferencePointInFleet;
|
||||
var referenceTwist = command.TwistAtReferencePoint;
|
||||
|
||||
for (var index = 0;
|
||||
index < layout.VehicleCount;
|
||||
index++)
|
||||
{
|
||||
var vehicle = layout.Vehicles[index];
|
||||
var offsetXMeters =
|
||||
vehicle.PoseInFleet.XMeters -
|
||||
referencePoint.XMeters;
|
||||
var offsetYMeters =
|
||||
vehicle.PoseInFleet.YMeters -
|
||||
referencePoint.YMeters;
|
||||
|
||||
var twistAtVehicleInFleet = new Twist2D(
|
||||
referenceTwist.VxMetersPerSecond -
|
||||
referenceTwist.OmegaRadiansPerSecond *
|
||||
offsetYMeters,
|
||||
referenceTwist.VyMetersPerSecond +
|
||||
referenceTwist.OmegaRadiansPerSecond *
|
||||
offsetXMeters,
|
||||
referenceTwist.OmegaRadiansPerSecond);
|
||||
|
||||
var fleetPoseInVehicle =
|
||||
FrameTransform2D.Inverse(
|
||||
vehicle.PoseInFleet);
|
||||
var twistInVehicleBody =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
fleetPoseInVehicle,
|
||||
twistAtVehicleInFleet);
|
||||
|
||||
commands[index] = new FleetMemberCommand(
|
||||
vehicle.VehicleId,
|
||||
twistInVehicleBody);
|
||||
}
|
||||
|
||||
return Array.AsReadOnly(commands);
|
||||
}
|
||||
|
||||
private static void EnsureCommandIsFinite(
|
||||
FleetMotionCommand command)
|
||||
{
|
||||
NumericGuard.EnsureFinite(
|
||||
command.ReferencePointInFleet.XMeters,
|
||||
nameof(command.ReferencePointInFleet));
|
||||
NumericGuard.EnsureFinite(
|
||||
command.ReferencePointInFleet.YMeters,
|
||||
nameof(command.ReferencePointInFleet));
|
||||
NumericGuard.EnsureFinite(
|
||||
command.TwistAtReferencePoint,
|
||||
nameof(command.TwistAtReferencePoint));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
// 编队固定布局快照，车队坐标系原点就是编队参考中心。
|
||||
public sealed class FleetLayout
|
||||
{
|
||||
private readonly ReadOnlyCollection<VehicleLayout> _vehicles;
|
||||
|
||||
public FleetLayout(IReadOnlyList<VehicleLayout> vehicles)
|
||||
{
|
||||
if (vehicles == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(vehicles));
|
||||
}
|
||||
|
||||
if (vehicles.Count == 0)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"编队布局至少需要包含一辆车。",
|
||||
nameof(vehicles));
|
||||
}
|
||||
|
||||
var snapshot = new VehicleLayout[vehicles.Count];
|
||||
var vehicleIds = new HashSet<int>();
|
||||
|
||||
for (var index = 0; index < vehicles.Count; index++)
|
||||
{
|
||||
var vehicle = vehicles[index];
|
||||
if (vehicle.VehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicles),
|
||||
$"第{index}辆车的编号必须大于零。");
|
||||
}
|
||||
|
||||
if (!vehicleIds.Add(vehicle.VehicleId))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"编队布局包含重复车号{vehicle.VehicleId}。",
|
||||
nameof(vehicles));
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
vehicle.PoseInFleet,
|
||||
$"{nameof(vehicles)}[{index}].{nameof(VehicleLayout.PoseInFleet)}");
|
||||
snapshot[index] = vehicle;
|
||||
}
|
||||
|
||||
_vehicles = Array.AsReadOnly(snapshot);
|
||||
}
|
||||
|
||||
public IReadOnlyList<VehicleLayout> Vehicles => _vehicles;
|
||||
|
||||
public int VehicleCount => _vehicles.Count;
|
||||
|
||||
public bool TryGetVehicle(
|
||||
int vehicleId,
|
||||
out VehicleLayout vehicle)
|
||||
{
|
||||
for (var index = 0; index < _vehicles.Count; index++)
|
||||
{
|
||||
if (_vehicles[index].VehicleId == vehicleId)
|
||||
{
|
||||
vehicle = _vehicles[index];
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
vehicle = default;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
// 单车车体系在车队坐标系中的固定位姿。
|
||||
public readonly struct VehicleLayout
|
||||
{
|
||||
public VehicleLayout(int vehicleId, Pose2D poseInFleet)
|
||||
{
|
||||
VehicleId = vehicleId;
|
||||
PoseInFleet = poseInFleet;
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public Pose2D PoseInFleet { get; }
|
||||
}
|
||||
|
||||
// 车队参考点及该点在车队坐标系中表达的刚体速度。
|
||||
public readonly struct FleetMotionCommand
|
||||
{
|
||||
public FleetMotionCommand(
|
||||
Point2D referencePointInFleet,
|
||||
Twist2D twistAtReferencePoint)
|
||||
{
|
||||
ReferencePointInFleet = referencePointInFleet;
|
||||
TwistAtReferencePoint = twistAtReferencePoint;
|
||||
}
|
||||
|
||||
public Point2D ReferencePointInFleet { get; }
|
||||
|
||||
public Twist2D TwistAtReferencePoint { get; }
|
||||
|
||||
public static FleetMotionCommand RotateAround(
|
||||
Point2D rotationCenterInFleet,
|
||||
double omegaRadiansPerSecond)
|
||||
{
|
||||
return new FleetMotionCommand(
|
||||
rotationCenterInFleet,
|
||||
new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
omegaRadiansPerSecond));
|
||||
}
|
||||
|
||||
public static FleetMotionCommand Stop()
|
||||
{
|
||||
return new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
Twist2D.Zero);
|
||||
}
|
||||
}
|
||||
|
||||
// 分配给指定车辆、在该车车体系中表达的刚体速度。
|
||||
public readonly struct FleetMemberCommand
|
||||
{
|
||||
public FleetMemberCommand(
|
||||
int vehicleId,
|
||||
Twist2D twistInVehicleBody)
|
||||
{
|
||||
VehicleId = vehicleId;
|
||||
TwistInVehicleBody = twistInVehicleBody;
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public Twist2D TwistInVehicleBody { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
// 只定义无线报文的数据结构、版本和字段
|
||||
@@ -1,15 +1,7 @@
|
||||
// 纯数据层：只描述坐标、速度和命令
|
||||
// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
|
||||
// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
|
||||
// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
|
||||
// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
|
||||
|
||||
// 共享的二维运动模型，不包含车辆路由或底盘执行策略。
|
||||
// 使用SI单位；车体系X向前、Y向左、逆时针为正，XxxInYyy表示Xxx在Yyy坐标系中。
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 二维坐标点，X、Y单位均为米。
|
||||
/// </summary>
|
||||
// 二维坐标点，单位为m。
|
||||
public readonly struct Point2D
|
||||
{
|
||||
public Point2D(double xMeters, double yMeters)
|
||||
@@ -25,11 +17,7 @@ namespace MyParking.Shared
|
||||
public static Point2D Zero => new Point2D(0.0, 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 二维局部坐标系在父坐标系中的位姿。
|
||||
/// 位置单位为米，朝向单位为弧度，逆时针为正。
|
||||
/// 具体父子关系由变量名称说明，例如RadarPoseInBody。
|
||||
/// </summary>
|
||||
// 二维位姿，位置单位为m，航向单位为rad。
|
||||
public readonly struct Pose2D
|
||||
{
|
||||
public Pose2D(
|
||||
@@ -55,11 +43,7 @@ namespace MyParking.Shared
|
||||
new Pose2D(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 二维刚体速度。
|
||||
/// 线速度单位为m/s，角速度单位为rad/s。
|
||||
/// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
|
||||
/// </summary>
|
||||
// 二维刚体速度，线速度单位为m/s，角速度单位为rad/s。
|
||||
public readonly struct Twist2D
|
||||
{
|
||||
public Twist2D(
|
||||
@@ -81,74 +65,4 @@ namespace MyParking.Shared
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -2,23 +2,17 @@ using System;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 统一检查跨层数值和二维位姿是否由满足基本范围要求的有限值组成。
|
||||
/// </summary>
|
||||
// 统一拒绝跨层数据中的NaN、无穷大和不符合正负范围的数值。
|
||||
public static class NumericGuard
|
||||
{
|
||||
/// <summary>
|
||||
/// 判断指定浮点数是否既不是NaN也不是无穷大。
|
||||
/// </summary>
|
||||
// 判断浮点数是否为有限值。
|
||||
public static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 确保指定浮点数不是NaN或无穷大。
|
||||
/// </summary>
|
||||
// 拒绝NaN和无穷大。
|
||||
public static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
@@ -31,9 +25,7 @@ namespace MyParking.Shared
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 确保指定浮点数是非负有限值。
|
||||
/// </summary>
|
||||
// 拒绝负数、NaN和无穷大。
|
||||
public static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
@@ -48,9 +40,7 @@ namespace MyParking.Shared
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 确保指定浮点数是正有限值。
|
||||
/// </summary>
|
||||
// 拒绝零、负数、NaN和无穷大。
|
||||
public static void EnsureFinitePositive(
|
||||
double value,
|
||||
string parameterName)
|
||||
@@ -65,9 +55,7 @@ namespace MyParking.Shared
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 确保二维位姿的位置和航向均为有限值。
|
||||
/// </summary>
|
||||
// 同时检查二维位姿的位置和航向。
|
||||
public static void EnsureFinite(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
@@ -82,9 +70,7 @@ namespace MyParking.Shared
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 确保二维刚体速度的两个线速度分量和角速度均为有限值。
|
||||
/// </summary>
|
||||
// 同时检查二维刚体速度的两个线速度分量和角速度。
|
||||
public static void EnsureFinite(
|
||||
Twist2D twist,
|
||||
string parameterName)
|
||||
|
||||
+13
-3
@@ -90,17 +90,26 @@
|
||||
- `l_step` 当前只作为诊断和后续健康分级依据，不单独决定状态有效性。
|
||||
- 依据：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`。
|
||||
|
||||
### 13. 车队布局采用不可变快照，刚体速度采用确定性分解
|
||||
|
||||
- `Shared/Fleet/FleetLayout.cs` 保存经车号唯一性和有限值校验的 `VehicleLayout` 快照，不提供搬运过程中的逐车修改入口。
|
||||
- `FleetKinematics.Decompose()` 已按平面刚体关系把 `FleetMotionCommand` 分解为各成员中心速度，并转换到各车真实车体系；不依赖通信、Detour、β、底盘限幅或QP。
|
||||
- `MultiWheelC.Tests/FleetKinematicsTests.cs` 已覆盖整体平移、绕车队中心旋转、绕成员车旋转和停止四个数学场景。
|
||||
|
||||
## 已经确认但尚未实施
|
||||
|
||||
- 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。
|
||||
- 当前代码没有可以确认的完整多车分配方案：`Shared/Fleet/FleetKinematics.cs` 仍只有职责注释，`VehicleLayout` 和 `FleetMotionCommand` 目前只有模型定义、没有业务调用；不得把这些类型当成已经实现的功能。
|
||||
- 当前只完成车队布局模型和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、轨迹控制、通信和安全协调仍未接入业务流程。
|
||||
- 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步读取成员位姿，选择车队参考系并创建新的不可变 `FleetLayout`，再由上层协调器原子激活。共同搬运期间的相对位姿变化属于状态误差，不能通过修改 `FleetLayout` 吸收；松开车辆后清除激活布局。具体采集和激活接口尚未实施。
|
||||
- 多车共同搬运不能只闭环车队中心：整体位姿误差与成员相对布局误差必须分开估计和约束，否则成员误差可能相互抵消而使平均中心看似正确。
|
||||
- 计划采用分层职责：车队控制器产生参考点 `FleetTwist`，分配层依据成员 `VehicleLayout` 计算每车真实车体系 `BodyTwist`，单车层继续负责β变换、GCP和本车四轮解算。
|
||||
- 第一版采用确定性的虚拟刚体速度分配，不先引入QP/HQP：若成员在车队系中的固定布局为位置 `(x_i,y_i)`、朝向 `theta_i`，则成员中心在车队系中的速度为 `(Vx-omega*y_i, Vy+omega*x_i, omega)`，再通过 `R(-theta_i)` 转到本车体系后交给 `SendBodyTwist()`。QP/HQP只在需要同时调整车队参考速度、处理成员能力差异、松弛约束或严格任务优先级时再引入。
|
||||
- “按状态最差车辆协调速度”采用车队共同可行性和统一缩放表达：普通能力受限时由所有成员约束确定共同速度比例；任一成员报警、通信超时、状态不可用或刚体误差越界时整队停车。时间戳、心跳、命令有效期和本地超时停车属于第一版安全契约，延迟预测补偿可以后续增加。
|
||||
- 虚拟车队可使用固定在车队坐标系中的前后GCP作为控制几何，例如位于 `±L_F`；这些点只用于把横向控制结果转换成车队参考点 `FleetTwist`，不是物理轮轴，也不直接参与单车四轮解算。前后GCP方向仍随控制输出动态变化，且横向控制器与GCP到Twist转换必须使用同一 `L_F`。该方案尚未实施。
|
||||
- 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员Detour位姿用于融合车队整体位姿和检查相对布局，单车轮速/舵角用于确认命令执行偏差，电机电流、扭矩或力传感信息用于负载与内力监控。相对位姿接近目标并不能证明没有内力，因此不能只依靠刚性连接或位姿误差判断负载均衡。
|
||||
- 每车β应由车队分配层基于整个运动段的速度方向、机械舵角和可行性集中选择；只允许在全车停车时准备和激活。所有成员确认舵轮到位后通过车队级同步屏障统一释放非零运动命令。
|
||||
- 第一版不把每车β作为复杂优化变量：车队动作先明确主要滚动方向 `beta_fleet`（如正常0°、斜行45°、横移90°），成员按固定布局朝向换算 `beta_i = beta_fleet - theta_i`，并利用180°等效和有符号速度选择方便的本地表示。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`，也不会让各车命令数值相同；仅允许在全车停车时准备和激活，全部成员舵轮到位后通过同步屏障释放非零命令。只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。
|
||||
- 旧版参考项目采用固定双车布局：各车由 `carWorld ∘ layout⁻¹` 反推车队中心，再对位置和圆周航向求平均；路径控制器以该虚拟中心跟踪轨迹。同时它可按 `fleetTarget ∘ layout_i` 生成每车理想位姿，并叠加Detour布局纠偏和邻车两腿检测纠偏，因此并非只控制平均中心。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotDefinition.cs`、`ChassisController.cs`。
|
||||
- 旧版 `SetOriginBias(layoutX, layoutY, layoutTh)` 是把各车真实轮子统一表达在车队虚拟坐标系中，属于固定编队布局变换；蟹行通过共同 `frontTh/rearTh` 表达。它与新版“每车按运动段独立选择纯旋转β”的职责不同，虽然两者可产生相同的实际轮子姿态和车辆运动。
|
||||
- 旧版 `SetOriginBias(layoutX, layoutY, layoutTh)` 是把各车真实轮子统一表达在车队虚拟坐标系中，属于固定编队布局变换。旧版联动显式区分常规、蟹行和绕车队中心旋转三类模式；蟹行角可由动作或遥控给出任意值（`FleetCrabWalk` 默认45°），并在运动前以零速度对齐舵轮、运行时使用180°等效和轮速反号，但没有根据整段轨迹和每车约束自主求解β的统一规划过程。给定简单蟹行动作时，它与新版固定β可能产生相同的实际轮子姿态和车辆运动。
|
||||
- 旧版自动 `FleetCurveWalk`、`FleetCrabWalk` 会先以零速度下发初始GCP角，等待成员新鲜、布局正确、命令可行、舵轮到位和从车应用新序列后才开始运动；原地旋转通过 `RotateWheelsAligned` 和 `FleetMotionReleased` 做整队释放。普通手动入口仍有 `SendMotion` 本车舵轮未对齐时速度置零的门控，但不保证与自动动作相同的车队级同步屏障。
|
||||
- 旧版的全局与局部定位用途不同：自动模式的车队中心估计仍依赖Detour，与局部POS纠偏开关无关；邻车两腿检测只能提供相对间距和姿态信息，不能单独确定世界坐标中的车队中心。
|
||||
- 旧版单车几何控制器和原地自转直接读取 `DetourInterface.getCartLocation()`，未见当前新版的源时间对齐、跳变候选或任务坐标连续化；旧版宿主参考代码另有基于 `l_step` 和数据新鲜度减速/暂停并触发重定位的 `RelocalizationManager`。这只能说明旧版通过宿主安全状态机管理明显失效，不能据此认定Detour位姿天然连续；实车部署是否使用相同宿主版本和配置仍待确认。来源：`mdcstoolbox/Clumsy/MotionControllers/AbstractGeometricController.cs`、`原版停车机器人/parkingrobot/ClumsyPilot/Movements.cs`、`mdcstoolbox/Clumsy/HighLevelSecurity/RelocalizationManager.cs`。
|
||||
@@ -110,6 +119,7 @@
|
||||
## 待评估
|
||||
|
||||
- 多车共同搬运时的车队参考点与固定GCP距离、任意成员布局、加权/异常值鲁棒的车队位姿估计、队形误差闭环，以及 `FleetMotionCommand → 每车Twist2D` 的具体分配与限幅算法。
|
||||
- 确定性刚体分配验证完成后，再评估集中式单步QP/HQP：安全和刚体可行性应作为硬约束或更高层级，相对布局、中心跟踪、平滑与能耗依次降低优先级；严格HQP不能仅靠单个加权QP的大权重近似。短时域MPC及舵轮动态延迟预测属于更后续阶段。
|
||||
- 负载共享和内力监控可用信号、阈值、降级与停车策略；当前项目尚未建立可确认的力/扭矩闭环。
|
||||
- 正式轨迹规划层与 `Trajectory2D` 的接入格式；当前 `TestTrajectoryFactory` 仅用于实验。
|
||||
- 是否长期保留旧版GCP/`SendMotion`后端，或增加经过充分验证的“车体Twist直接到各轮”的新后端。
|
||||
|
||||
+6
-2
@@ -22,11 +22,15 @@
|
||||
| `Point2D` | 二维位置或向量，单位m |
|
||||
| `Pose2D` | 二维位置和朝向，单位m/rad |
|
||||
| `Twist2D` | 同一点处的 `Vx`、`Vy`、`Omega`，单位m/s、rad/s |
|
||||
| `VehicleLayout` | 单车车体系在车队系中的位姿；当前仅数据模型 |
|
||||
| `FleetMotionCommand` | 车队参考点及该点速度；当前尚无车队分配执行器 |
|
||||
| `VehicleLayout` | 单车车体系在车队系中的固定目标位姿 |
|
||||
| `FleetLayout` | 不可变的成员布局快照，构造时校验成员数量、车号唯一性和位姿有限值 |
|
||||
| `FleetMotionCommand` | 车队参考点及该点在车队系中表达的刚体速度 |
|
||||
| `FleetMemberCommand` | 指定车辆及其真实车体系中表达的成员中心速度 |
|
||||
|
||||
坐标变换集中在 `FrameTransform2D`；有限值检查集中在 `NumericGuard`；角度处理集中在 `AngleMath`。
|
||||
|
||||
`FleetKinematics.Decompose()` 依据 `v_i = v_ref + omega × (r_i-r_ref)` 生成每车命令，再按 `VehicleLayout.PoseInFleet` 的朝向把线速度从车队系转换到成员车体系；所有刚性连接成员的 `Omega` 保持相同。
|
||||
|
||||
## 轨迹契约
|
||||
|
||||
文件：`MultiWheelC/Trajectory/`。
|
||||
|
||||
+8
-4
@@ -1,6 +1,6 @@
|
||||
# 当前进展
|
||||
|
||||
更新日期：2026-08-19。这里只保存当前状态，不作为完整开发历史。
|
||||
更新日期：2026-08-20。这里只保存当前状态，不作为完整开发历史。
|
||||
|
||||
## 已完成/已接入
|
||||
|
||||
@@ -19,6 +19,7 @@
|
||||
- C层轨迹CSV已补充原始/滤波轮组 `Vx/Vy/Vw`、轮组与预测时刻、Detour帧间隔、实际/允许创新、候选原因、估计器状态和不可用原因。
|
||||
- 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。
|
||||
- `MultiWheelC.Tests` 已提供不依赖实车的Stanley前进/倒车横向符号回归，8个场景通过；该项目不进入正式解决方案和打包脚本。
|
||||
- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；不依赖实车的4个刚体分解场景已通过。
|
||||
- 本次建立工作区/项目AGENTS导航和 `docs/` 按需知识库。
|
||||
|
||||
以上表示代码入口存在，不表示全部实车工况已经验收。
|
||||
@@ -29,13 +30,14 @@
|
||||
- 暂时冻结普通运动跳变阈值和30°/s、40°/s²自转参数，等待Detour接口语义后再决定是否实施自转结束后的条件化仅位置连续化或调整航向恢复策略。
|
||||
- Detour对接最小问题已整理到 `docs/detour-information-checklist.md`，不要求取得源码。
|
||||
- 验证非零β运动系，当前已有45°蟹行直线入口；曲线蟹行仍需设计实验。
|
||||
- 在单车和停车流程稳定后研究多车共同搬运的刚体模型与成员命令分配。
|
||||
- 多车共同搬运已完成布局模型和纯刚体速度分解；下一阶段是夹紧后布局采集/原子激活、车队状态与控制、通信和安全协调，再把成员命令接入各车 `SendBodyTwist()`。QP/HQP不作为第一版前置条件。
|
||||
- β在多车中定位为单车执行坐标系而非车队核心优化量：常规、斜行和横移动作先确定车队主要滚动方向，各车按布局朝向换算本地β，停车预对齐并经车队同步屏障统一释放。轨迹级β搜索只作为机械余量或复杂方向变化下的后续增强。
|
||||
|
||||
## 阻塞/待确认
|
||||
|
||||
- Clumsy/Medulla正式宿主、插件部署和配置持久化说明未纳入仓库。
|
||||
- 完整停车业务流程和验收指标未确认。
|
||||
- 多车通信协议、车队参考点、车辆布局来源和故障降级策略未确认。
|
||||
- 多车通信协议、统一时间轴、车队参考点选取、夹紧后布局采集方法、刚体误差阈值和故障降级策略未确认；已确认运行布局应在夹紧后生成不可变快照并由上层整体激活。
|
||||
- 最新实车实验数据对控制周期与舵轮滞后的结论尚未沉淀为可复核结果。
|
||||
- Detour `l_step` 精确定义、显式重定位/坐标重置状态和部署端MDCS恢复策略待确认；当前不能把 `l_step<4` 当作唯一有效性条件。
|
||||
- Detour `getCartLocation()` 的位姿坐标系、`tick` 采样/解算/发布语义、是否已有独立连续里程计或定位状态接口，以及部署版本和实际里程计/SLAM配置待确认。
|
||||
@@ -48,4 +50,6 @@
|
||||
2. 在Detour信息返回前不继续全局放宽状态估计边界，也不提高自转角速度/角加速度；现有偶发定位不可用保持安全停车。
|
||||
3. 若工程上必须先降低位置型自转失败率，再单独设计仅在纯自转结束、轮组无平移且航向有效时启用的位置连续化；不得扩展到普通轨迹运动，也不得吸收航向偏移。
|
||||
4. 为轨迹插值/投影、坐标变换、状态跳变候选和终点策略继续补充不依赖宿主的数学回归测试。
|
||||
5. 单车稳定后，再设计 `FleetMotionCommand → 每车Twist2D` 的车队运动学，不直接启用 `#if false` 旧多车代码。
|
||||
5. 设计车队建立生命周期：预设布局引导就位，夹紧静止后同步采集成员位姿，创建并原子激活新的 `FleetLayout`，松开后清除；车队参考系和采样同步方法需先明确。
|
||||
6. 继续补充 `FleetKinematics` 的圆弧、任意布局和倒车数学场景，再接入车队状态与控制流程，不直接启用 `#if false` 旧多车代码。
|
||||
7. 在接入通信前定义车队状态时间对齐、心跳/命令有效期、共同速度缩放、成员故障整队停车和舵轮准备同步屏障。
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user