159 lines
21 KiB
Markdown
159 lines
21 KiB
Markdown
# 技术决策
|
||
|
||
只记录长期有效的方案状态；实验临时参数和失败尝试不在此保存。
|
||
|
||
## 已经实施
|
||
|
||
### 1. 分离 CommonUsage、Shared、M层和C层
|
||
|
||
- `CommonUsage` 保持独立通用底盘库，不反向依赖项目控制层。
|
||
- `Shared` 只保存M/C共享模型、数学、校验和底盘适配，不建立独立程序集。
|
||
- `MedullaAdapter` 管硬件，`MultiWheelC` 管动作和控制。
|
||
- 原因：避免硬件通信、控制算法和旧底盘库相互反向耦合。
|
||
- 依据：三个 `.csproj`、`AGENTS.md`。
|
||
|
||
### 2. Shared统一SI单位和真实车体系命令
|
||
|
||
- Shared使用m、m/s、rad、rad/s，车体系X前Y左、逆时针为正。
|
||
- 对外底盘命令统一为 `Twist2D`；旧版mm/deg接口只在 `MultiWheelChassisAdapter` 边界转换。
|
||
- 原因：减少不同层之间的单位和符号歧义，为后续单车/车队刚体速度分配保留统一模型。
|
||
|
||
### 3. 显式β运动坐标系，不根据速度分量猜测模式
|
||
|
||
- 切换流程固定为停车、预对齐舵轮、`ActivateMotionFrame(β)`，运行阶段统一调用 `SendBodyTwist()`。
|
||
- β=0表示车头方向，β=90°表示车体左侧作为虚拟前向；支持任意固定β。
|
||
- 实际四轮始终使用真实布局参与解算，β只改变表达坐标系。
|
||
- β不是改变目标刚体运动的额外自由度；它用于为同一 `BodyTwist` 选择本运动段更易满足舵角、最小转舵和非奇异条件的主要滚动方向。
|
||
- β不增加车辆物理运动能力，也不改变同一刚体速度最终要求的轮子滚动轴线；当前主要价值是让真实车体系 `BodyTwist` 能稳定接入方向型旧 `SendMotion`，并使虚拟GCP角远离±90°表达奇异区。若以后改为 `BodyTwist` 直接到四轮的完整逆解，β可以退化为底盘内部的等效解选择，不应成为车队控制器的核心概念。
|
||
- β可参与运动前的候选方向规划，但不能代替真实舵轮限位检查：±120°保护必须对完整四轮解算结果逐轮检查，并同时考虑“舵角±180°、轮速反向”的等效解。
|
||
- 限制：运动中不切换β；纯自转要求回到真实车体系并采用专用舵轮准备。
|
||
- 依据：`PrepareWheelsForward`、`TrajectoryTrackingMovement`、`MultiWheelChassisAdapter`。
|
||
|
||
### 4. 保留前后GCP作为旧底盘几何边界
|
||
|
||
- 控制层输出前后虚拟GCP方向；`MultiWheelChassis.SendMotion` 由两条法线求ICR，再计算每个真实轮子的切线方向和速度比例。
|
||
- GCP是刚体控制点，不是物理轮轴中心；`ControlPointRadius` 必须在前馈、运动学和旧解算中保持一致。
|
||
- 原因：复用已经存在的机械限位、等效舵角、速度斜坡和差速舵轮分配逻辑。
|
||
|
||
### 5. 轨迹采用弧长参数化而非时间参数化
|
||
|
||
- 轨迹保存点序、累计弧长、车体中心位姿、曲率和有符号参考速度。
|
||
- 实际周期 `deltaTime` 只进入控制器、滤波器和命令变化率，不作为轨迹索引。
|
||
- 原因：执行进度由车辆空间位置决定，调度周期波动不会直接跳过时间采样点。
|
||
|
||
### 6. 正负参考速度表达前进和倒车
|
||
|
||
- 轨迹Yaw始终是车头方向；正速度前进、负速度倒车。
|
||
- Stanley曲率前馈、横向/航向修正和终点行驶方向显式考虑速度符号。
|
||
- 同一测试曲线要求速度同号，不在滚动中直接切换前进/倒车。
|
||
- 依据：`TrajectoryPoint`、`TestTrajectoryFactory`、`StanleyLateralController`。
|
||
|
||
### 7. 默认状态采用Detour位姿与轮组平面速度组合
|
||
|
||
- 位姿来自经过校验和任务坐标连续化处理的Detour。
|
||
- 车体系 `Vx/Vy/Vw` 来自 `GetCarSpeed(true)`，使用同一时间常数低通滤波；轮组估计有效后，控制状态的 `Omega` 也使用轮组 `Vw`。首帧尚未建立采样时间基准时速度状态无效并对外置零。
|
||
- 轮组 `Vw` 同时用于Detour跳变期间的短时位姿预测、运动合理性和动态航向创新阈值。
|
||
- 原因：位置仍依赖SLAM，控制速度优先使用响应更直接且不受Detour自转退化影响的电机/舵角反馈。
|
||
- 依据：`ParkingVehicleStateProviderFactory`、`WheelFeedbackVehicleStateProvider`。
|
||
|
||
### 8. 横向控制可替换，纵向控制暂保持PID
|
||
|
||
- `ILateralController` 是Stanley/LQR/MPC等的稳定扩展点；`TrajectoryTrackingMovement.LateralControllerFactory` 可注入实现。
|
||
- 默认横向为Stanley，默认纵向为 `PidLongitudinalController`。
|
||
- 当前没有单独的控制器工厂文件，避免为手动替换增加不必要结构。
|
||
|
||
### 9. 低速和终点采用单向收敛
|
||
|
||
- 起步区域使用小幅释放速度；终点前读取更低速度并进入单向低速逼近。
|
||
- 到达或越过终点后只停车，不生成反向修正速度。
|
||
- 完成同时检查剩余弧长、终点欧氏距离、车头航向和β方向实际速度。
|
||
- 原因：停车机器人终点附近反复前后修正风险高，优先保证运动方向稳定和可预测。
|
||
- 依据：`ParkingGeometricController.ResolveTerminalApproachSpeed()`、`HasReachedEnd()`。
|
||
|
||
### 10. 对已观察的舵轮响应加入有限前馈与预瞄
|
||
|
||
- M层差速转舵角速度前馈已接入，当前默认增益0.9、速度上限0.03m/s；实现位于 `MotorRoutine.CalculateDiffSteerRateFeedforward()`。
|
||
- Stanley曲率前馈预瞄已接入，当前车辆默认时间0.15s、最大距离0.12m。
|
||
- 两者均是可配置补偿，不替代底层PID、真实周期和机械响应验证。
|
||
|
||
### 11. 车辆级参数集中到 PilotConfig，动作只做必要覆盖
|
||
|
||
- 停车控制参数集中在 `Configuration/PilotConfig.ParkingControl.cs`，动作默认读取 `PilotDefinition.Conf`。
|
||
- 组合运动段可以覆盖自身完成条件；实验轨迹速度描述实验本身，车辆级最大命令速度仍负责最终限制。
|
||
|
||
### 12. Detour阶跃采用任务坐标连续化与安全停车分级处理
|
||
|
||
- Detour原始世界观测与当前任务控制位姿通过 `controlFromDetour` 隔离；疑似阶跃先进入候选确认，确认期间使用轮组 `Vx/Vy/Vw` 短时预测。
|
||
- 仅对幅值受限且连续确认的小坐标偏移自动更新变换；大幅、不一致或超过确认窗口的变化使状态不可用，由动作安全停车。
|
||
- 完整轨迹状态继续要求位置和航向均可靠；原地自转只依赖独立验证的航向，位置单独异常不再终止正在进行的自转，航向异常仍触发安全停止。
|
||
- 近似原地自转期间禁止自动吸收坐标偏移，避免把旋转定位劣化写入任务坐标系；目标航向到达并停止驱动后，才在既有幅值和确认窗口内处理位置恢复，未恢复时禁止进入下一运动段。
|
||
- `l_step` 当前只作为诊断和后续健康分级依据，不单独决定状态有效性。
|
||
- 依据：`DetourVehicleStateProvider`、`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`。
|
||
|
||
### 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` 开发纯车队控制器。
|
||
- 当前无法直接获得被搬运车辆中心，因此 `FleetLayoutCapture.Capture()` 已确定用成员车体中心X/Y的算术平均定义车队原点，用指定主车Yaw定义车队朝向；主车缺失时拒绝采集，不做隐式降级。
|
||
- 布局采集只返回采集时刻的 `FleetPoseInWorld` 与不可变 `FleetLayout`，不读取Detour/通信，也不负责夹紧、静止、时间对齐或原子激活。6个布局采集数学场景已通过。
|
||
|
||
### 14. 原地自转保留绝对航向与轮组相对角两种反馈模式
|
||
|
||
- `DetourAbsoluteHeading` 保留世界航向闭环和停车后的Detour位姿恢复，适用于必须对准绝对方向的任务。
|
||
- `RelativeWheelOdometry` 通过 `TryGetWheelTwist()` 获取与Detour无关的滤波轮组 `Vw`，按轮组采样时间积分相对角度；活动自转不因Detour位置或航向退化中断。
|
||
- 轮组相对模式是工程降级/测试模式，不等同于绝对定位：允许约±5°误差，不能消除轮胎打滑、轮径误差和积分漂移，也不能用于长期世界航向基准。
|
||
- 依据：`WheelFeedbackVehicleStateProvider`、`MultiWheelRotateInPlace`、`RotationTests`。
|
||
|
||
### 15. 单车与车队轨迹跟踪通过组合共享纯控制核心
|
||
|
||
- `PathTrackingContext` 只携带受控刚体的车体系速度、速度有效性、轨迹投影和周期控制量，不依赖 `VehicleState` 或 `FleetState`。
|
||
- `PathTrackingCore` 集中实现投影连续性、保护/终点策略、曲率预瞄、横纵向控制和GCP分配；`ParkingGeometricController` 与 `FleetController` 分别组合该核心，负责各自的状态适配和输出边界，不通过继承复制控制流程。
|
||
- `FleetController` 第一版只闭环车队虚拟中心，并支持固定 `β_fleet`：实际纵向速度沿β投影，GCP结果从运动坐标系旋转到车队坐标系后形成车队原点处的 `FleetMotionCommand`；成员速度继续由 `FleetKinematics.Decompose()` 确定性分解。
|
||
- `FleetStateEstimator` 已根据固定 `FleetLayout` 和时间对齐目标时刻反算、融合车队中心，并输出每车 `FleetMemberLayoutError`；当前第一版先做候选中心两两一致性检查，再对X/Y等权平均、对Yaw做圆周平均。正负候选偏差的公共分量进入车队中心，相对分量仍保留在成员布局误差中，不会因平均而消失。成员样本的实际采集和跨电脑时间处理仍属于外层接收链。
|
||
- `FleetCoordinator` 已串联状态估计、虚拟中心控制、布局误差警告区间内的统一速度缩放、刚体分解和 `FleetMemberCommandCorrector` 小范围纠偏；`FleetRuntime` 已把周期结果转换为本车直接执行和远端成员命令。
|
||
- `FleetPreparationCoordinator` 已按 `beta_i = beta_fleet - theta_i` 生成成员准备目标，并通过任务号和全员Ready形成统一激活屏障；`FleetMemberAgent` 负责本车舵轮准备、激活、命令校验和底盘执行；`FleetRuntime` 将二者接入同一固定布局滚动任务生命周期。
|
||
- 控制算法的可替换性继续由 `ILateralController`、`ILongitudinalController` 组合注入；状态源、通信、底盘发送和成员协调不进入纯核心。
|
||
- `MultiWheelC.Tests` 已覆盖8个Stanley前进/倒车符号场景、8个车队控制周期场景、6个布局采集场景和4个刚体分解场景；统一构建与打包通过。
|
||
|
||
### 16. 车队通信保持最小值契约，失联停车采用主车与本车双层保护
|
||
|
||
- `Shared/Fleet/FleetProtocol.cs` 只保存 `FleetCommand`、`FleetMemberReport`、枚举和协议常量，不绑定停车机器人控制器、串口实现或复杂消息类层次；无线字节帧和校验留在具体传输实现。
|
||
- `IFleetTransport` 是运行层唯一传输边界；当前 `InMemoryFleetTransport` 只在测试项目中模拟定向命令、广播和成员报告，后续无线实现替换该接口而不改车队算法。
|
||
- 主车 `FleetSafetySupervisor` 依据主车本地接收时间、成员状态和故障锁存整队停车决定；每辆车的 `FleetMemberAgent` 依据本机单调时间和命令有效期独立看门狗停车。`FleetRuntime` 已执行本车停止并向可达成员下发停止，两层保护仍不能互相替代。
|
||
- 同一 `FleetRuntime` 部署到所有车辆，角色由显式 `selfVehicleId` 与 `leaderVehicleId` 决定，不把固定车号硬编码为主车；主车也是成员，本车命令直接执行，不要求传输层回环。
|
||
|
||
## 已经确认但尚未实施
|
||
|
||
- 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。
|
||
- 转舵系统辨识应对四个轮组分别使用相同激励，输入采用实际差速转舵命令（`TotalDiff`或左右轮实际发送命令之差），输出采用实际舵角；若只用目标角到实际角，会把当前PID包含在闭环模型中，更换PID后该模型不能继续代表转舵机构。控制阶段优先保留四个独立PID实例和内部状态、共用一套参数，并依据四个健康轮组中最不利的动态设计稳定裕量；只有硬件健康且公共参数仍无法兼顾时，才评估配置化的逐轮小幅校准，不在代码中硬编码某个轮位特例。
|
||
- 当前多车的固定布局滚动任务已形成内存通信可运行链路。仍缺少C层正式动作入口、布局原子激活、成员状态实际采集、跨机时间换算、公共世界坐标验证和无线传输实现。
|
||
- 单车 `MultiWheelC/StateEstimation` 继续负责Detour重复帧、跳变候选、轮速短时预测和任务坐标连续化；车队层不复制这套原始定位处理，只消费经过本车校验的成员状态，并负责跨车时间对齐、固定布局反算、成员一致性检查和中心融合。成员状态进入融合前仍必须确认处于同一公共坐标系；各车独立的 `_controlFromDetour` 连续化变换是否会造成跨车基准差异，属于通信/状态接收契约必须验证的事项。
|
||
- 第一版保留当前保守的等权车队中心融合；任务生命周期和内存零速运行链已经贯通。公共坐标系与主车时间轴语义确认并取得静止/低速双车日志后，再按数据增加车队历史预测、逐成员创新门控、健康降级和Huber等鲁棒加权；不在缺少Detour协方差时提前实现协方差加权或Covariance Intersection。
|
||
- 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步取得同一世界坐标系下的成员位姿，调用 `FleetLayoutCapture` 创建新的不可变布局，再由上层协调器原子激活。共同搬运期间的相对位姿变化属于状态误差，不能通过修改 `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，把横向控制结果转换成车队原点 `FleetTwist`；这些点不是物理轮轴，也不直接参与单车四轮解算。横向控制器与GCP到Twist转换必须使用同一控制点半径；具体车辆级配置值和实车验证仍待完成。
|
||
- 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员Detour位姿用于融合车队整体位姿和检查相对布局，单车轮速/舵角用于确认命令执行偏差，电机电流、扭矩或力传感信息用于负载与内力监控。相对位姿接近目标并不能证明没有内力，因此不能只依靠刚性连接或位姿误差判断负载均衡。
|
||
- 第一版不把每车β作为复杂优化变量：`FleetController` 支持固定车队主要滚动方向 `beta_fleet`，`FleetPreparationCoordinator` 按布局换算成员 `beta_i` 并采用180°等效轴，`FleetRuntime` 已将停车预对齐、全员Ready和统一激活接入运行链。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`；只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。
|
||
- 旧版参考项目采用固定双车布局：各车由 `carWorld ∘ layout⁻¹` 反推车队中心，再对位置和圆周航向求平均；路径控制器以该虚拟中心跟踪轨迹。同时它可按 `fleetTarget ∘ layout_i` 生成每车理想位姿，并叠加Detour布局纠偏和邻车两腿检测纠偏，因此并非只控制平均中心。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotDefinition.cs`、`ChassisController.cs`。
|
||
- 旧版 `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`。
|
||
- 旧版的 `MultiVehicleUseDetect` 与 `MultiVehicleSyncUseDetour` 是彼此独立且默认关闭的开关，并非强制二选一。两者同时开启时，常规/蟹行的 `SendMotion()` 会直接将检测补偿与POS补偿逐分量相加，没有互斥或状态级融合；这不等于每次运行必然冲突，但存在重复修正、相互对抗和放大噪声的结构风险。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotConfig.cs`、`PilotDefinition.cs`。
|
||
- 旧版方案只作为设计参考，不直接移植：`GetLayoutPoseForCar()` 将成员写死为两侧镜像布局，车队几何通过 `SetOriginBias(layout)` 和 `ControlPointRadius=distance/2` 进入各车底盘解算；当两类局部纠偏均关闭时，成员一致性仍可能依赖共同开环命令和被搬运物的机械约束。
|
||
|
||
## 待评估
|
||
|
||
- 多车共同搬运时的车队参考点与固定GCP距离、逐成员能力/命令可行性限幅，以及带时间预测和异常成员隔离的鲁棒车队位姿融合；当前任意成员布局、确定性刚体分解和小范围队形误差闭环已有第一版实现。
|
||
- 确定性刚体分配验证完成后，再评估集中式单步QP/HQP：安全和刚体可行性应作为硬约束或更高层级，相对布局、中心跟踪、平滑与能耗依次降低优先级；严格HQP不能仅靠单个加权QP的大权重近似。短时域MPC及舵轮动态延迟预测属于更后续阶段。
|
||
- 负载共享和内力监控可用信号、阈值、降级与停车策略；当前项目尚未建立可确认的力/扭矩闭环。
|
||
- 正式轨迹规划层与 `Trajectory2D` 的接入格式；当前 `TestTrajectoryFactory` 仅用于实验。
|
||
- 是否长期保留旧版GCP/`SendMotion`后端，或增加经过充分验证的“车体Twist直接到各轮”的新后端。
|
||
- 蟹行遥控中按半轮距/半轴距缩放舵角的映射是否符合统一曲率语义；这是遥控手感策略，不应与坐标变换混为一谈。
|