完善Detour状态估计与轨迹跟踪验证
This commit is contained in:
@@ -0,0 +1,129 @@
|
||||
# 系统架构
|
||||
|
||||
## 解决方案与依赖边界
|
||||
|
||||
`ParkingRobot.sln` 包含三个项目：
|
||||
|
||||
| 项目 | 目标框架 | 职责 |
|
||||
| --- | --- | --- |
|
||||
| `CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj` | `netstandard2.0` | 通用底盘、轮子模型、`SendMotion`/`SendXYThSpeed`和四轮几何解算 |
|
||||
| `MedullaAdapter/MedullaAdapter.csproj` | `net8.0` | Medulla M层插件、MCU/CAN/串口/IO、遥控、报警和硬件反馈 |
|
||||
| `MultiWheelC/MultiWheelC.csproj` | `netstandard2.0` | Clumsy C层插件、动作、控制器、轨迹、状态估计、实验记录 |
|
||||
|
||||
`Shared/` 没有独立项目：`MultiWheelC` 链接全部 `Shared/**/*.cs`，`MedullaAdapter` 只链接其需要的模型、数学、校验和底盘适配文件。`CommonUsage` 是独立底盘库，不反向依赖 `Shared`、M层或C层。来源：三个 `.csproj`。
|
||||
|
||||
```text
|
||||
Clumsy宿主
|
||||
└─ MultiWheelC
|
||||
├─ Trajectory / StateEstimation / Control / Movements
|
||||
├─ Shared
|
||||
└─ CommonUsage.dll
|
||||
|
||||
Medulla宿主
|
||||
└─ MedullaAdapter
|
||||
├─ Shared（链接的必要文件）
|
||||
├─ CommonUsage.dll
|
||||
└─ mcu_serial_bridge.dll → MCU → CAN / Serial / IO
|
||||
```
|
||||
|
||||
## 目录职责
|
||||
|
||||
| 路径 | 当前职责 |
|
||||
| --- | --- |
|
||||
| `MultiWheelC/Configuration/` | 停车控制、状态估计、原地自转和完成条件的车辆级运行参数 |
|
||||
| `MultiWheelC/Trajectory/` | 弧长参数化轨迹、插值、投影和进度窗口；`LegacyTrackAdapter.cs` 目前仅占位 |
|
||||
| `MultiWheelC/StateEstimation/` | Detour位姿校验/差分速度、轮组反馈速度组合与低通滤波 |
|
||||
| `MultiWheelC/Control/Abstractions/` | 横向、纵向控制器接口与周期输入/输出模型 |
|
||||
| `MultiWheelC/Control/Lateral/` | 当前默认 `StanleyLateralController` |
|
||||
| `MultiWheelC/Control/Longitudinal/` | 当前默认 `PidLongitudinalController` |
|
||||
| `MultiWheelC/Control/Allocation/` | 横纵结果组合、GCP限幅及GCP与刚体速度的转换 |
|
||||
| `MultiWheelC/Control/Execution/` | 单周期编排、终点策略、命令执行和耗时诊断 |
|
||||
| `MultiWheelC/Movements/` | 舵轮准备、轨迹跟踪、原地自转和组合动作计划 |
|
||||
| `MultiWheelC/Experiments/` | Clumsy宿主人工测试、测试轨迹工厂和CSV记录 |
|
||||
| `MultiWheelC/Old/` | 保留的旧实现；不能仅因仍参与编译就视为新版流程依赖 |
|
||||
| `Shared/` | M/C共享的SI数据模型、坐标变换、数值校验和底盘适配 |
|
||||
| `MedullaAdapter/` | 车型定义、LadderLogic、MCU桥、CAN/串口、遥控、诊断 |
|
||||
| `CommonUsage-MultiVehicleSync/commonusage/Chassis/` | 实际轮子模型、GCP/ICR求解、舵角/轮速分配和机械约束 |
|
||||
| `data_process/` | 离线实验数据处理；日常代码任务不扫描其中的实验日志 |
|
||||
| `参考文档/` | 参数样例和历史设计资料；不等同于运行时配置 |
|
||||
|
||||
## 入口与宿主生命周期
|
||||
|
||||
### C层
|
||||
|
||||
- `PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefinition>` 是Clumsy车型定义和M/C IO边界。
|
||||
- `PilotDefinition.Conf` 是动作读取运行配置的统一入口；`PilotConfig` 字段提供元数据和默认值。
|
||||
- `[MovementTest]` 类型由宿主发现并执行，例如 `NewControllerStraight4mTest`、`NewControllerReverseStraight4mTest`、`NewControllerCrab45Straight4mTest`、`TestRotateAngle` 和 `CompositeStopTurnGoTest`。
|
||||
- `MovementDefinition.Get()` 以 `IEnumerable<bool>` 形式协作执行：`true` 表示继续，结束动作时返回/产生 `false` 或退出枚举。具体宿主调度细节来自外部程序集，仓库内不可完全确认。
|
||||
|
||||
### M层
|
||||
|
||||
- `DiverCartDefinition : MultiWheelCartDefinition` 是Medulla车型定义。
|
||||
- `CommunicationInit()` 打开并配置MCU桥。
|
||||
- `[UseLadderLogic]` 注册 `AlarmRoutine`（50ms）、`MotorRoutine`（50ms）和 `MCURoutine`（20ms）；这些是声明的扫描间隔，不能直接等同于实测稳定周期。
|
||||
- `[UseManualController]` 注册 `Remote`。
|
||||
|
||||
## 新版轨迹跟踪调用链
|
||||
|
||||
```text
|
||||
MovementTest / MotionPlanExecutor
|
||||
→ TrajectoryTrackingMovement.Get()
|
||||
→ PrepareWheelsForward(DirectionRadians=β)
|
||||
→ MultiWheelChassisAdapter.ActivateMotionFrame(β)
|
||||
→ ParkingVehicleStateProviderFactory.Create()
|
||||
→ ParkingGeometricController.Start()/ExecuteCycle()
|
||||
→ IVehicleStateProvider.TryGetState()
|
||||
→ TrajectoryProjector.Project()
|
||||
→ ILateralController.Compute()
|
||||
→ ILongitudinalController.ComputeSpeedMetersPerSecond()
|
||||
→ GcpCommandAllocator.Allocate()
|
||||
→ GcpCommandExecutor.Execute()
|
||||
→ GcpKinematics.ToBodyTwist()
|
||||
→ MultiWheelChassisAdapter.SendBodyTwist()
|
||||
→ MultiWheelChassis.SendMotion()
|
||||
→ 四个真实舵轮角度和速度
|
||||
```
|
||||
|
||||
`TrajectoryTrackingMovement` 默认从 `PilotDefinition.Conf` 读取车辆级参数，同时保留少量动作级覆盖字段；横向控制器可通过 `LateralControllerFactory` 替换，纵向控制器当前固定创建为 `PidLongitudinalController`。
|
||||
|
||||
## 状态数据流
|
||||
|
||||
```text
|
||||
DetourInterface.getCartLocation()
|
||||
→ DetourVehicleStateProvider
|
||||
├─ 位姿单位转换、重复帧处理、跳变/预测残差校验
|
||||
└─ VelocityEstimator2D：Detour差分Vx/Vy/Omega
|
||||
|
||||
MultiWheelChassis.GetCarSpeed(true)
|
||||
→ WheelFeedbackVehicleStateProvider
|
||||
├─ Vx、Vy分别使用同一时间常数低通滤波
|
||||
├─ 覆盖Detour线速度
|
||||
└─ 保留Detour位姿和Omega
|
||||
|
||||
→ VehicleState（世界位姿、世界Twist、车体Twist）
|
||||
→ ParkingGeometricController
|
||||
```
|
||||
|
||||
## 运动坐标系与四轮解算数据流
|
||||
|
||||
`MultiWheelChassisAdapter` 对外只接收真实车体系 `Twist2D`。滚动动作开始前先固定运动方向β：
|
||||
|
||||
1. `ActivateMotionFrame(β)` 调用 `MultiWheelChassis.SetOriginBias(0,0,-β)`。
|
||||
2. 车体线速度通过 `R(-β)` 表达到运动坐标系。
|
||||
3. 运动系中以 `±ControlPointRadius` 作为虚拟前后GCP，计算GCP方向。
|
||||
4. `MultiWheelChassis.SendMotion` 由两个GCP方向求瞬时旋转中心ICR。
|
||||
5. 每个真实轮子的 `PhysicalPosition` 被旋转成运动系 `Position`，再由ICR分别求切线角和速度半径比例。
|
||||
6. `sTh - sw.ZeroDirection` 把运动系方向转成真实舵轮机械命令；距离和速度大小不因坐标旋转改变。
|
||||
|
||||
关键位置：`Shared/Chassis/MultiWheelChassisAdapter.cs`、`MultiWheelChassis.cs::SetOriginBias`、`SendMotion`、`CalculateAxes`。
|
||||
|
||||
## 动作组织
|
||||
|
||||
- `PrepareWheelsForward`：停车、下发任意固定方向β并等待稳定；名称保留“Forward”，但功能已支持非零方向。
|
||||
- `TrajectoryTrackingMovement`：准备运动系、创建控制器、周期执行、失败停车，可选完成后回正。
|
||||
- `MultiWheelRotateInPlace`：准备自转舵角、交接到XYTh解算、世界航向PID、完成后回正。
|
||||
- `MotionPlanExecutor`：开始运动前预检全部段，顺序执行 `TrackMotionPlanSegment` 和 `RotateInPlaceMotionPlanSegment`，共享状态源。
|
||||
|
||||
## 构建与打包
|
||||
|
||||
`build-and-package.ps1` 顺序构建 `CommonUsage`、M层和C层，将新 `CommonUsage.dll` 复制到 `ref/`，最后生成 `output/M` 和 `output/C`。两个部署包必须使用同一份 `CommonUsage.dll`。构建产物目录不作为知识来源，也不直接编辑。
|
||||
Reference in New Issue
Block a user