增加车队轨迹控制核心与轮组自转模式
This commit is contained in:
+24
-7
@@ -50,10 +50,10 @@
|
||||
|
||||
### 7. 默认状态采用Detour位姿与轮组平面速度组合
|
||||
|
||||
- 位姿和Omega来自经过校验/滤波的Detour。
|
||||
- 车体系Vx、Vy来自 `GetCarSpeed(true)`，分别使用同一时间常数低通滤波。
|
||||
- 轮组Vw同样滤波，但仅用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值，不替换控制输出中的Detour Omega。
|
||||
- 原因：位置依赖SLAM，控制平面速度优先使用响应更直接的电机/舵角反馈；轮组Vw对短时运动趋势有用，但动态精度尚不足以直接作为闭环角速度。
|
||||
- 位姿来自经过校验和任务坐标连续化处理的Detour。
|
||||
- 车体系 `Vx/Vy/Vw` 来自 `GetCarSpeed(true)`，使用同一时间常数低通滤波；轮组估计有效后，控制状态的 `Omega` 也使用轮组 `Vw`。首帧尚未建立采样时间基准时速度状态无效并对外置零。
|
||||
- 轮组 `Vw` 同时用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值。
|
||||
- 原因：位置仍依赖SLAM，控制速度优先使用响应更直接且不受Detour自转退化影响的电机/舵角反馈。
|
||||
- 依据：`ParkingVehicleStateProviderFactory`、`WheelFeedbackVehicleStateProvider`。
|
||||
|
||||
### 8. 横向控制可替换，纵向控制暂保持PID
|
||||
@@ -90,22 +90,39 @@
|
||||
- `l_step` 当前只作为诊断和后续健康分级依据，不单独决定状态有效性。
|
||||
- 依据：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`。
|
||||
|
||||
### 13. 车队布局采用不可变快照，刚体速度采用确定性分解
|
||||
### 13. 车队布局与状态采用不可变快照，刚体速度采用确定性分解
|
||||
|
||||
- `Shared/Fleet/FleetLayout.cs` 保存经车号唯一性和有限值校验的 `VehicleLayout` 快照，不提供搬运过程中的逐车修改入口。
|
||||
- `FleetKinematics.Decompose()` 已按平面刚体关系把 `FleetMotionCommand` 分解为各成员中心速度，并转换到各车真实车体系；不依赖通信、Detour、β、底盘限幅或QP。
|
||||
- `MultiWheelC.Tests/FleetKinematicsTests.cs` 已覆盖整体平移、绕车队中心旋转、绕成员车旋转和停止四个数学场景。
|
||||
- `MultiWheelC/Fleet/FleetState.cs` 保存车队虚拟中心的世界位姿、同一点的世界系/车队系速度、状态采样时间和速度有效性；`FleetPoseInWorld.Yaw` 定义车队 `+X` 方向，两份速度只是同一物理速度的不同坐标表达。
|
||||
- `FleetState.SampleTimestampSeconds` 采用主车/协调器生成聚合快照时的本机单调时间。成员本机时钟和Detour `tick` 的同步属于未来接收/状态估计层职责，不阻塞使用人工构造 `FleetState` 开发纯车队控制器。
|
||||
|
||||
### 14. 原地自转保留绝对航向与轮组相对角两种反馈模式
|
||||
|
||||
- `DetourAbsoluteHeading` 保留世界航向闭环和停车后的Detour位姿恢复，适用于必须对准绝对方向的任务。
|
||||
- `RelativeWheelOdometry` 通过 `TryGetWheelTwist()` 获取与Detour无关的滤波轮组 `Vw`，按轮组采样时间积分相对角度；活动自转不因Detour位置或航向退化中断。
|
||||
- 轮组相对模式是工程降级/测试模式，不等同于绝对定位：允许约±5°误差，不能消除轮胎打滑、轮径误差和积分漂移，也不能用于长期世界航向基准。
|
||||
- 依据：`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`、`RotationTests`。
|
||||
|
||||
### 15. 单车与车队轨迹跟踪通过组合共享纯控制核心
|
||||
|
||||
- `PathTrackingContext` 只携带受控刚体的车体系速度、速度有效性、轨迹投影和周期控制量，不依赖 `VehicleState` 或 `FleetState`。
|
||||
- `PathTrackingCore` 集中实现投影连续性、保护/终点策略、曲率预瞄、横纵向控制和GCP分配；`ParkingGeometricController` 与 `FleetController` 分别组合该核心，负责各自的状态适配和输出边界，不通过继承复制控制流程。
|
||||
- `FleetController` 第一版只闭环车队虚拟中心，并将GCP结果转换成车队原点处的 `FleetMotionCommand`；成员速度继续由 `FleetKinematics.Decompose()` 确定性分解。
|
||||
- 控制算法的可替换性继续由 `ILateralController`、`ILongitudinalController` 组合注入；状态源、通信、底盘发送和成员协调不进入纯核心。
|
||||
- `MultiWheelC.Tests` 已覆盖8个Stanley前进/倒车符号场景、6个车队控制周期场景和4个刚体分解场景；统一构建与打包通过。
|
||||
|
||||
## 已经确认但尚未实施
|
||||
|
||||
- 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。
|
||||
- 当前只完成车队布局模型和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、轨迹控制、通信和安全协调仍未接入业务流程。
|
||||
- 当前已完成车队布局模型、虚拟中心状态模型、虚拟中心轨迹控制和纯运动学分解，尚未形成可运行的多车链路：布局采集/激活、车队状态估计、通信、成员纠偏和安全协调仍未接入业务流程。
|
||||
- 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步读取成员位姿，选择车队参考系并创建新的不可变 `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`。该方案尚未实施。
|
||||
- 虚拟车队使用固定在车队坐标系中的对称前后GCP，把横向控制结果转换成车队原点 `FleetTwist`；这些点不是物理轮轴，也不直接参与单车四轮解算。横向控制器与GCP到Twist转换必须使用同一控制点半径；具体车辆级配置值和实车验证仍待完成。
|
||||
- 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员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`。
|
||||
|
||||
Reference in New Issue
Block a user