feat: 添加了自动超参数整定
This commit is contained in:
Horldsence
@@ -0,0 +1,42 @@
|
||||
[package]
|
||||
name = "chassis-tune"
|
||||
description = "Automated PID gain search for the chassis velocity loop: offline sweep/refine plus bench (hardware-in-the-loop) verification"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[features]
|
||||
default = []
|
||||
# 可选：用 plotters 输出 best.svg（默认关闭，避免 CI 依赖系统字体与更长编译）。
|
||||
plot = ["dep:plotters"]
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
chassis-bench = { workspace = true }
|
||||
chassis-config = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
dji-esc-sim = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
argmin = { workspace = true }
|
||||
argmin-math = { workspace = true }
|
||||
rand = { workspace = true }
|
||||
rayon = { workspace = true }
|
||||
indicatif = { workspace = true }
|
||||
csv = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
serde_json = { workspace = true }
|
||||
toml = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
anyhow = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
plotters = { workspace = true, optional = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,173 @@
|
||||
//! 评估后端：把"一组增益有多好"这件事抽象出来。
|
||||
//!
|
||||
//! - [`SimEvaluator`]：离线 harness + 被控对象集成（域随机化），CPU 密集、可并行；
|
||||
//! - [`BenchEvaluator`](crate::bench::BenchEvaluator)：真机台架（`chassis-bench`），单轮、串行、有安全包络。
|
||||
//!
|
||||
//! 搜索器只认识 [`Evaluator`] trait，因此"先仿真粗调、再上台架复验"是同一套
|
||||
//! 代码换一个后端，而不是两份实现。
|
||||
|
||||
use dji_esc_sim::{LoopLimits, PlantEnsemble, Scenario, simulate};
|
||||
use motor_core::VelocityGains;
|
||||
|
||||
use crate::cost::{Objective, Score, Verdict};
|
||||
|
||||
pub use crate::cost::Weights;
|
||||
|
||||
/// 一次评估的结果。
|
||||
pub type Evaluated = Score;
|
||||
|
||||
/// 被控对象评估后端。
|
||||
///
|
||||
/// 必须 `Send + Sync`：粗扫阶段由 `rayon` 并行调用。
|
||||
pub trait Evaluator: Send + Sync {
|
||||
/// 评估一组增益。
|
||||
fn evaluate(&self, gains: &VelocityGains) -> Evaluated;
|
||||
/// 一行描述（写进报告，便于复现）。
|
||||
fn describe(&self) -> String;
|
||||
}
|
||||
|
||||
/// 离线评估：在一组被控对象参数上重放同一条场景。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct SimEvaluator {
|
||||
/// 被控对象集成（域随机化）。
|
||||
pub ensemble: PlantEnsemble,
|
||||
/// 评价场景。
|
||||
pub scenario: Scenario,
|
||||
/// 限幅。
|
||||
pub limits: LoopLimits,
|
||||
/// 硬约束与权重。
|
||||
pub objective: Objective,
|
||||
/// 集成聚合权重：0 = 只看均值，1 = 只看最坏成员。
|
||||
pub robust_weight: f64,
|
||||
}
|
||||
|
||||
impl SimEvaluator {
|
||||
/// 组装。
|
||||
#[must_use]
|
||||
pub fn new(
|
||||
ensemble: PlantEnsemble,
|
||||
scenario: Scenario,
|
||||
limits: LoopLimits,
|
||||
objective: Objective,
|
||||
robust_weight: f64,
|
||||
) -> Self {
|
||||
Self { ensemble, scenario, limits, objective, robust_weight: robust_weight.clamp(0.0, 1.0) }
|
||||
}
|
||||
|
||||
/// 集成规模。
|
||||
#[must_use]
|
||||
pub fn ensemble_size(&self) -> usize {
|
||||
self.ensemble.len()
|
||||
}
|
||||
}
|
||||
|
||||
impl Evaluator for SimEvaluator {
|
||||
fn evaluate(&self, gains: &VelocityGains) -> Evaluated {
|
||||
let mut costs = Vec::with_capacity(self.ensemble.len());
|
||||
let mut nominal: Option<Score> = None;
|
||||
let mut worst_verdict = Verdict::Ok;
|
||||
|
||||
for (index, plant) in self.ensemble.iter().enumerate() {
|
||||
let score = match simulate(*gains, plant, &self.scenario, self.limits) {
|
||||
Ok(trace) => self.objective.score(&trace),
|
||||
Err(error) => Score::penalized(Objective::from_error(error)),
|
||||
};
|
||||
if index == 0 {
|
||||
nominal = Some(score);
|
||||
}
|
||||
if !score.verdict.is_ok() && worst_verdict.is_ok() {
|
||||
worst_verdict = score.verdict;
|
||||
}
|
||||
costs.push(score.cost);
|
||||
}
|
||||
|
||||
let count = costs.len() as f64;
|
||||
let mean = costs.iter().sum::<f64>() / count;
|
||||
let worst = costs.iter().copied().fold(f64::NEG_INFINITY, f64::max);
|
||||
let cost = (1.0 - self.robust_weight) * mean + self.robust_weight * worst;
|
||||
|
||||
// 判定取"名义成员优先，其次最差成员"：名义模型都没收敛时不必再看抖动。
|
||||
// 指标始终报名义成员的——报告要展示的是一条可复现的曲线。
|
||||
let nominal = nominal.unwrap_or_else(|| Score::penalized(Verdict::Rejected));
|
||||
let verdict = if nominal.verdict.is_ok() { worst_verdict } else { nominal.verdict };
|
||||
Score { cost, verdict, metrics: nominal.metrics }
|
||||
}
|
||||
|
||||
fn describe(&self) -> String {
|
||||
format!(
|
||||
"sim(ensemble={}, robust_weight={:.2}, scenario={:?}, target={} rpm, limit={} raw, {})",
|
||||
self.ensemble.len(),
|
||||
self.robust_weight,
|
||||
self.scenario.profile,
|
||||
self.scenario.target_rpm,
|
||||
self.limits.current_limit_raw,
|
||||
self.objective.weights.describe()
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::cost::Weights;
|
||||
use dji_esc_sim::{PlantParams, PlantTolerances};
|
||||
|
||||
fn evaluator(ensemble_size: usize, robust_weight: f64) -> SimEvaluator {
|
||||
let scenario = Scenario::step(1500.0, 2.0);
|
||||
let objective = Objective::new(Weights::default(), scenario, LoopLimits::gm3508());
|
||||
let ensemble = PlantEnsemble::randomized(
|
||||
PlantParams::gm3508_bench(),
|
||||
PlantTolerances::default(),
|
||||
ensemble_size,
|
||||
11,
|
||||
);
|
||||
SimEvaluator::new(ensemble, scenario, LoopLimits::gm3508(), objective, robust_weight)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn good_gains_beat_bad_gains_offline() {
|
||||
let evaluator = evaluator(4, 0.5);
|
||||
let good = evaluator.evaluate(&VelocityGains::new(4.0, 12.0, 0.0, 0.0));
|
||||
let bad = evaluator.evaluate(&VelocityGains::new(0.01, 0.0, 0.0, 0.0));
|
||||
assert!(good.cost < bad.cost, "good = {}, bad = {}", good.cost, bad.cost);
|
||||
assert_eq!(good.verdict, Verdict::Ok, "{good:?}");
|
||||
assert_eq!(bad.verdict, Verdict::NoSettle, "{bad:?}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_finite_gains_are_rejected_without_panicking() {
|
||||
let evaluator = evaluator(2, 0.0);
|
||||
let score = evaluator.evaluate(&VelocityGains::new(f32::NAN, 0.0, 0.0, 0.0));
|
||||
assert_eq!(score.verdict, Verdict::Rejected);
|
||||
assert!((score.cost - crate::cost::PENALTY).abs() < 1.0);
|
||||
assert!(score.metrics.is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn evaluation_is_deterministic() {
|
||||
let evaluator = evaluator(4, 0.5);
|
||||
let gains = VelocityGains::new(3.0, 9.0, 0.1, 0.5);
|
||||
let a = evaluator.evaluate(&gains);
|
||||
let b = evaluator.evaluate(&gains);
|
||||
assert_eq!(a.cost.to_bits(), b.cost.to_bits());
|
||||
assert_eq!(a.verdict, b.verdict);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn robust_weight_shifts_toward_the_worst_member() {
|
||||
let mean_only = evaluator(6, 0.0);
|
||||
let worst_only = evaluator(6, 1.0);
|
||||
let gains = VelocityGains::new(1.0, 6.0, 0.0, 0.0);
|
||||
let mean = mean_only.evaluate(&gains);
|
||||
let worst = worst_only.evaluate(&gains);
|
||||
assert!(worst.cost >= mean.cost, "worst = {}, mean = {}", worst.cost, mean.cost);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn describe_mentions_the_configuration() {
|
||||
let text = evaluator(3, 0.25).describe();
|
||||
assert!(text.contains("ensemble=3"), "{text}");
|
||||
assert!(text.contains("robust_weight=0.25"), "{text}");
|
||||
assert!(text.contains("1500 rpm"), "{text}");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,383 @@
|
||||
//! 真机（HIL）评估后端：在台架上逐次试验一组增益。
|
||||
//!
|
||||
//! 与 [`SimEvaluator`](crate::backend::SimEvaluator) 实现同一个 [`Evaluator`]，
|
||||
//! 因此 `sweep` / `validate` 的全部逻辑（粗扫、精修、报告）都不需要改动，
|
||||
//! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。
|
||||
//!
|
||||
//! # 安全设计
|
||||
//!
|
||||
//! 每次试验都：① 先确认目标电调反馈新鲜；② 逐拍重发设定值（命令租约）；
|
||||
//! ③ 每拍过一遍 [`SafetyEnvelope`]；④ 无论正常结束、触发包络还是 ctrl-c，
|
||||
//! 退出前一律 `disable()`；⑤ 试验之间插入静默期，让轮子真正停下来再开始下一组。
|
||||
//!
|
||||
//! 评估是**串行**的（真机只有一条总线）。调用方应把搜索放进单线程 rayon 池
|
||||
//! （见 `main`），否则大量线程会在互斥锁上排队、白白浪费时间。
|
||||
|
||||
use std::sync::Mutex;
|
||||
use std::time::Duration;
|
||||
|
||||
use anyhow::Context as _;
|
||||
use chassis_bench::{Abort, SafetyEnvelope, run_step_trial};
|
||||
use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use dji_esc_sim::{LoopLimits, Scenario, Trace};
|
||||
use motor_core::{Disable as _, MotorFeedback, VelocityGains};
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::backend::Evaluator;
|
||||
use crate::cost::{Objective, Score, Verdict};
|
||||
|
||||
/// 真机后端错误。
|
||||
#[derive(Debug, Error)]
|
||||
pub enum BenchError {
|
||||
/// 无法打开总线。
|
||||
#[error("cannot open CAN link: {0}")]
|
||||
Link(String),
|
||||
/// 电调总线启动失败。
|
||||
#[error("cannot start ESC bus: {0}")]
|
||||
Bus(String),
|
||||
/// 启动超时：目标电调没有反馈。
|
||||
#[error("esc {esc_id} has no fresh feedback after {waited_ms}ms (wiring / power / bitrate?)")]
|
||||
NoFeedback {
|
||||
/// 目标电调 ID。
|
||||
esc_id: u8,
|
||||
/// 等待时长（毫秒）。
|
||||
waited_ms: u64,
|
||||
},
|
||||
/// 搜索期间因安全事件中止且未要求继续。
|
||||
#[error("aborted by safety envelope: {reason}")]
|
||||
Aborted {
|
||||
/// 触发原因。
|
||||
reason: String,
|
||||
},
|
||||
}
|
||||
|
||||
/// 真机评估配置。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct BenchConfig {
|
||||
/// 电调配置（名称、ID、方向、减速比、限流）——增益由搜索覆写。
|
||||
pub esc: EscConfig,
|
||||
/// 总线运行参数。
|
||||
pub bus: BusOptions,
|
||||
/// 评估场景。
|
||||
pub scenario: Scenario,
|
||||
/// 安全包络。
|
||||
pub envelope: SafetyEnvelope,
|
||||
/// 硬约束与权重。
|
||||
pub objective: Objective,
|
||||
/// 试验之间的静默期（等轮子停稳）。
|
||||
pub between_trials: Duration,
|
||||
/// 命令租约续期节拍。
|
||||
pub lease_period: Duration,
|
||||
/// 启动时等待反馈的超时。
|
||||
pub startup_timeout: Duration,
|
||||
/// 安全检查触发后是否继续搜索（默认 false = 立即停）。
|
||||
pub continue_on_abort: bool,
|
||||
}
|
||||
|
||||
impl BenchConfig {
|
||||
/// 合理默认：1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。
|
||||
#[must_use]
|
||||
pub fn new(
|
||||
esc: EscConfig,
|
||||
scenario: Scenario,
|
||||
envelope: SafetyEnvelope,
|
||||
objective: Objective,
|
||||
) -> Self {
|
||||
Self {
|
||||
esc,
|
||||
bus: BusOptions::default(),
|
||||
scenario,
|
||||
envelope,
|
||||
objective,
|
||||
between_trials: Duration::from_millis(500),
|
||||
lease_period: Duration::from_millis(5),
|
||||
startup_timeout: Duration::from_secs(5),
|
||||
continue_on_abort: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 真机评估器（持有总线与运行时）。
|
||||
pub struct BenchEvaluator {
|
||||
session: Mutex<Session>,
|
||||
scenario: Scenario,
|
||||
envelope: SafetyEnvelope,
|
||||
objective: Objective,
|
||||
effort_limit_raw: i16,
|
||||
between_trials: Duration,
|
||||
lease_period: Duration,
|
||||
continue_on_abort: bool,
|
||||
/// 目前见过的最优轨迹（报告用）。
|
||||
best: Mutex<Option<(f64, Trace)>>,
|
||||
/// 中止记录（报告用）。
|
||||
aborts: Mutex<Vec<String>>,
|
||||
describe: String,
|
||||
}
|
||||
|
||||
struct Session {
|
||||
runtime: tokio::runtime::Runtime,
|
||||
bus: Option<EscBusHandle>,
|
||||
motor: EscMotor,
|
||||
}
|
||||
|
||||
impl BenchEvaluator {
|
||||
/// 打开链路、启动总线并等待目标电调反馈。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路 / 总线启动失败，或启动超时（电调无反馈）。
|
||||
pub fn open(link: can_link::CanLink, config: BenchConfig) -> Result<Self, BenchError> {
|
||||
let esc_id = config.esc.esc_id;
|
||||
let limits =
|
||||
LoopLimits { current_limit_raw: config.esc.current_limit, model: config.esc.model };
|
||||
let runtime = tokio::runtime::Builder::new_multi_thread()
|
||||
.worker_threads(2)
|
||||
.enable_all()
|
||||
.build()
|
||||
.map_err(|e| BenchError::Bus(e.to_string()))?;
|
||||
|
||||
let bus = EscBus::spawn(link, vec![config.esc.clone()], config.bus.clone())
|
||||
.map_err(|e| BenchError::Bus(e.to_string()))?;
|
||||
let motor = bus
|
||||
.motor(esc_id)
|
||||
.ok_or_else(|| BenchError::Bus(format!("esc {esc_id} missing from the bus")))?
|
||||
.clone();
|
||||
|
||||
let startup_timeout = config.startup_timeout;
|
||||
let ready = runtime.block_on(async {
|
||||
let deadline = tokio::time::Instant::now() + startup_timeout;
|
||||
loop {
|
||||
if motor.feedback_age().is_some_and(|age| age <= Duration::from_millis(100)) {
|
||||
return true;
|
||||
}
|
||||
if tokio::time::Instant::now() >= deadline {
|
||||
return false;
|
||||
}
|
||||
tokio::time::sleep(Duration::from_millis(20)).await;
|
||||
}
|
||||
});
|
||||
let evaluator = Self {
|
||||
session: Mutex::new(Session { runtime, bus: Some(bus), motor }),
|
||||
scenario: config.scenario,
|
||||
envelope: config.envelope,
|
||||
objective: config.objective,
|
||||
effort_limit_raw: limits.current_limit_raw,
|
||||
between_trials: config.between_trials,
|
||||
lease_period: config.lease_period,
|
||||
continue_on_abort: config.continue_on_abort,
|
||||
best: Mutex::new(None),
|
||||
aborts: Mutex::new(Vec::new()),
|
||||
describe: format!(
|
||||
"bench(esc={esc_id}, name={}, limit={} raw, target={} rpm, trial={:.1}s, gap={}ms)",
|
||||
config.esc.name,
|
||||
limits.current_limit_raw,
|
||||
config.scenario.target_rpm,
|
||||
config.scenario.total_s(),
|
||||
config.between_trials.as_millis()
|
||||
),
|
||||
};
|
||||
if !ready {
|
||||
// 启动失败也必须走完整停机路径：先 0 电流，再关总线。
|
||||
evaluator.shutdown();
|
||||
return Err(BenchError::NoFeedback {
|
||||
esc_id,
|
||||
waited_ms: startup_timeout.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
Ok(evaluator)
|
||||
}
|
||||
|
||||
/// 最优轨迹（若有）。
|
||||
#[must_use]
|
||||
pub fn best_trace(&self) -> Option<Trace> {
|
||||
self.best
|
||||
.lock()
|
||||
.unwrap_or_else(std::sync::PoisonError::into_inner)
|
||||
.as_ref()
|
||||
.map(|(_, trace)| trace.clone())
|
||||
}
|
||||
|
||||
/// 搜索期间的安全中止记录。
|
||||
#[must_use]
|
||||
pub fn aborts(&self) -> Vec<String> {
|
||||
self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).clone()
|
||||
}
|
||||
|
||||
/// 停机并关闭总线（**必须**在结束前调用）。
|
||||
pub fn shutdown(&self) {
|
||||
let mut session = self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
|
||||
let Some(bus) = session.bus.take() else {
|
||||
return;
|
||||
};
|
||||
for motor in bus.motors() {
|
||||
motor.disable();
|
||||
}
|
||||
session.runtime.block_on(async {
|
||||
tokio::time::sleep(Duration::from_millis(50)).await;
|
||||
bus.shutdown().await;
|
||||
});
|
||||
}
|
||||
|
||||
/// 一次真机试验（含试验间隔静默）。
|
||||
fn run_trial(&self, gains: &VelocityGains) -> Score {
|
||||
let mut session = self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
|
||||
let Session { runtime, motor, .. } = &mut *session;
|
||||
motor.set_velocity_gains(*gains);
|
||||
let scenario = self.scenario;
|
||||
let envelope = self.envelope.clone();
|
||||
let limit = self.effort_limit_raw;
|
||||
let lease = self.lease_period;
|
||||
let between = self.between_trials;
|
||||
|
||||
let outcome = runtime.block_on(async {
|
||||
let trial =
|
||||
run_step_trial(motor, &scenario, &envelope, limit, lease, std::future::pending())
|
||||
.await;
|
||||
// 试验间隔：让轮子真正停下来，再让下一次试验的起点一致。
|
||||
tokio::time::sleep(between).await;
|
||||
trial
|
||||
});
|
||||
|
||||
if let Some(reason) = outcome.abort {
|
||||
let verdict = verdict_for(&reason);
|
||||
let message = format!("{}: {reason}", self.describe);
|
||||
self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).push(message);
|
||||
return Score::penalized(verdict);
|
||||
}
|
||||
|
||||
let score = match crate::metrics::StepMetrics::from_trace(&outcome.trace, &self.scenario) {
|
||||
Some(metrics) => self.objective.score_metrics(metrics),
|
||||
None => Score::penalized(Verdict::Rejected),
|
||||
};
|
||||
// 记录目前最优的轨迹，供报告画图 / 复盘。
|
||||
let mut best = self.best.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
|
||||
if best.as_ref().is_none_or(|(cost, _)| score.cost < *cost) {
|
||||
*best = Some((score.cost, outcome.trace));
|
||||
}
|
||||
score
|
||||
}
|
||||
}
|
||||
|
||||
impl Drop for BenchEvaluator {
|
||||
fn drop(&mut self) {
|
||||
self.shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
impl Evaluator for BenchEvaluator {
|
||||
fn evaluate(&self, gains: &VelocityGains) -> Score {
|
||||
let score = self.run_trial(gains);
|
||||
if !score.verdict.is_ok() && !self.continue_on_abort {
|
||||
// 真机上出现安全事件时不"带伤继续搜索"：交给调用方在 sweep 之后
|
||||
// 检查 `aborts()` 并决定是否收工。这里只记录，不 panic。
|
||||
tracing::warn!(verdict = score.verdict.label(), "bench trial did not pass");
|
||||
}
|
||||
score
|
||||
}
|
||||
|
||||
fn describe(&self) -> String {
|
||||
self.describe.clone()
|
||||
}
|
||||
}
|
||||
|
||||
/// 安全事件 → 成本判定。
|
||||
///
|
||||
/// 过速 / 过温说明控制器失稳；掉线 / 超时 / 取消说明本次试验没做完，
|
||||
/// 两者的结果都不可用，但原因不同——报告要能区分。
|
||||
#[must_use]
|
||||
pub fn verdict_for(abort: &Abort) -> Verdict {
|
||||
match abort {
|
||||
Abort::Overspeed { .. } | Abort::OverTemperature { .. } => Verdict::Overspeed,
|
||||
Abort::FeedbackLost { .. } | Abort::Timeout { .. } | Abort::Cancelled => Verdict::Rejected,
|
||||
}
|
||||
}
|
||||
|
||||
/// 打开真机链路：给了 `interface` 走 SocketCAN，否则走 slcan 串口。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备 / 接口打开失败（路径、权限、接口未 up、平台不支持）时返回错误。
|
||||
pub fn open_link(
|
||||
device: &str,
|
||||
serial_baud: u32,
|
||||
can_speed: chassis_bench::CanSpeed,
|
||||
interface: Option<&str>,
|
||||
) -> Result<can_link::CanLink, BenchError> {
|
||||
let result = match interface {
|
||||
Some(interface) => chassis_bench::open_socketcan(interface),
|
||||
None => chassis_bench::open_slcan(
|
||||
device,
|
||||
chassis_bench::default_slcan_options(serial_baud, can_speed),
|
||||
),
|
||||
};
|
||||
result
|
||||
.with_context(|| match interface {
|
||||
Some(interface) => format!("打开 SocketCAN 接口 {interface} 失败（ip link / 权限？）"),
|
||||
None => format!("打开 slcan 设备 {device} 失败（路径 / 权限 / 占用？）"),
|
||||
})
|
||||
.map_err(|e| BenchError::Link(e.to_string()))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::cost::Weights;
|
||||
|
||||
fn config() -> BenchConfig {
|
||||
let mut esc = EscConfig::gm3508("bench", 1, 1.0);
|
||||
esc.current_limit = 1500;
|
||||
let scenario = Scenario::ramp_and_hold(500.0, 0.05, 0.15);
|
||||
let objective = Objective::new(
|
||||
Weights::default(),
|
||||
scenario,
|
||||
LoopLimits { current_limit_raw: esc.current_limit, model: esc.model },
|
||||
);
|
||||
let envelope = SafetyEnvelope::new()
|
||||
.with_overspeed(1200.0, Duration::from_millis(60))
|
||||
.with_temperature_max(70)
|
||||
.with_feedback_timeout(Duration::from_millis(200))
|
||||
.with_trial_timeout(Duration::from_secs(2));
|
||||
BenchConfig::new(esc, scenario, envelope, objective)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn config_defaults_are_safe() {
|
||||
let config = config();
|
||||
assert_eq!(config.bus.period, Duration::from_millis(1));
|
||||
assert_eq!(config.bus.command_timeout, Duration::from_millis(50));
|
||||
assert!(!config.continue_on_abort, "默认必须在安全事件后收工");
|
||||
assert!(config.between_trials >= Duration::from_millis(200));
|
||||
assert_eq!(config.lease_period, Duration::from_millis(5), "续租必须远密于 50ms 租约");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn every_abort_maps_to_a_definite_verdict() {
|
||||
let cases = [
|
||||
Abort::Overspeed { peak_rpm: 5000.0, limit_rpm: 1200.0, grace_ms: 60 },
|
||||
Abort::OverTemperature { temperature_c: 90, limit_c: 70 },
|
||||
Abort::FeedbackLost { esc_id: 1, age_ms: None, timeout_ms: 200 },
|
||||
Abort::Timeout { elapsed_ms: 6000 },
|
||||
Abort::Cancelled,
|
||||
];
|
||||
for abort in cases {
|
||||
let verdict = verdict_for(&abort);
|
||||
assert!(!verdict.is_ok(), "{abort:?} 不得被判为达标");
|
||||
}
|
||||
assert_eq!(
|
||||
verdict_for(&Abort::Overspeed { peak_rpm: 5000.0, limit_rpm: 1200.0, grace_ms: 60 }),
|
||||
Verdict::Overspeed
|
||||
);
|
||||
assert_eq!(verdict_for(&Abort::Cancelled), Verdict::Rejected);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn link_open_failures_are_errors() {
|
||||
// 不存在的设备：必须返回 Err 而不是 panic（真机前最常见的失败模式）。
|
||||
let result =
|
||||
open_link("/definitely/not/a/device", 2_000_000, chassis_bench::CanSpeed::M1, None);
|
||||
assert!(result.is_err(), "不存在的串口设备必须报错");
|
||||
let missing_interface =
|
||||
open_link("ignored", 2_000_000, chassis_bench::CanSpeed::M1, Some("can-not-real"));
|
||||
assert!(missing_interface.is_err(), "不存在的 CAN 接口必须报错");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,302 @@
|
||||
//! 成本函数：把指标折成一个标量，并把"不可接受"变成硬约束。
|
||||
//!
|
||||
//! 两个原则：
|
||||
//!
|
||||
//! 1. **硬约束不用权重表达**。过速 / 长时间饱和 / 不收敛不是"差一点"，
|
||||
//! 而是"不能用"——它们直接给罚值并记录判定，绝不允许被其他项的权重
|
||||
//! 抵消掉（否则搜索会主动挑一个会飞车的解）。
|
||||
//! 2. **各项归一化**。时间按轨迹时长归一、误差按 `|目标| × 时长` 归一、
|
||||
//! effort 按限幅归一，因此权重在不同场景 / 不同电机之间仍然可解释。
|
||||
|
||||
use dji_esc_sim::{LoopLimits, Scenario, SimError, Trace};
|
||||
|
||||
use crate::metrics::StepMetrics;
|
||||
|
||||
/// 罚值：远大于任何合法成本，但保持有限（避免 NaN 传染给优化器）。
|
||||
pub const PENALTY: f64 = 1.0e9;
|
||||
|
||||
/// 成本权重（各项已归一化到 O(1)）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Weights {
|
||||
/// 调节时间权重。
|
||||
pub settle: f64,
|
||||
/// 超调权重。
|
||||
pub overshoot: f64,
|
||||
/// 绝对误差积分（IAE）权重。
|
||||
pub iae: f64,
|
||||
/// effort（电流）权重。
|
||||
pub effort: f64,
|
||||
}
|
||||
|
||||
impl Default for Weights {
|
||||
fn default() -> Self {
|
||||
// 底盘速度环：跟得上、不过冲最重要；电流只作为次要惩罚
|
||||
// （省电流不是目的，避免烧电调才是）。
|
||||
Self { settle: 1.0, overshoot: 1.0, iae: 0.5, effort: 0.1 }
|
||||
}
|
||||
}
|
||||
|
||||
impl Weights {
|
||||
/// 人类可读描述（日志 / 报告用）。
|
||||
#[must_use]
|
||||
pub fn describe(&self) -> String {
|
||||
format!(
|
||||
"cost={:.2}·settle+{:.2}·overshoot+{:.2}·iae+{:.2}·effort",
|
||||
self.settle, self.overshoot, self.iae, self.effort
|
||||
)
|
||||
}
|
||||
|
||||
/// 权重是否全部有限且非负。
|
||||
#[must_use]
|
||||
pub fn is_valid(&self) -> bool {
|
||||
[self.settle, self.overshoot, self.iae, self.effort]
|
||||
.iter()
|
||||
.all(|w| w.is_finite() && *w >= 0.0)
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次评估的判定。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Verdict {
|
||||
/// 达标（成本 = 加权指标）。
|
||||
Ok,
|
||||
/// 增益含 NaN / inf，或场景退化（被拒绝，不进控制器）。
|
||||
Rejected,
|
||||
/// 数值发散。
|
||||
Diverged,
|
||||
/// 过速。
|
||||
Overspeed,
|
||||
/// 饱和占比过高（积分绕死 / 限幅太紧）。
|
||||
Saturated,
|
||||
/// 在场景内没有收敛。
|
||||
NoSettle,
|
||||
}
|
||||
|
||||
impl Verdict {
|
||||
/// 是否达标。
|
||||
#[must_use]
|
||||
pub fn is_ok(self) -> bool {
|
||||
matches!(self, Self::Ok)
|
||||
}
|
||||
|
||||
/// 简短标签（报告 / CSV 用）。
|
||||
#[must_use]
|
||||
pub fn label(self) -> &'static str {
|
||||
match self {
|
||||
Self::Ok => "ok",
|
||||
Self::Rejected => "rejected",
|
||||
Self::Diverged => "diverged",
|
||||
Self::Overspeed => "overspeed",
|
||||
Self::Saturated => "saturated",
|
||||
Self::NoSettle => "no-settle",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次评估的结果。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Score {
|
||||
/// 成本（越小越好）。
|
||||
pub cost: f64,
|
||||
/// 判定。
|
||||
pub verdict: Verdict,
|
||||
/// 指标（被拒绝时为 `None`）。
|
||||
pub metrics: Option<StepMetrics>,
|
||||
}
|
||||
|
||||
impl Score {
|
||||
/// 罚分结果。
|
||||
#[must_use]
|
||||
pub fn penalized(verdict: Verdict) -> Self {
|
||||
Self { cost: PENALTY, verdict, metrics: None }
|
||||
}
|
||||
}
|
||||
|
||||
/// 目标函数：场景 + 限幅 + 权重 + 硬约束。
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub struct Objective {
|
||||
/// 权重。
|
||||
pub weights: Weights,
|
||||
/// 评价场景。
|
||||
pub scenario: Scenario,
|
||||
/// 限幅与型号。
|
||||
pub limits: LoopLimits,
|
||||
/// 过速硬上限（rpm）。
|
||||
pub max_rpm: f32,
|
||||
/// 允许的饱和占比上限（0..1）。
|
||||
pub max_saturated_fraction: f32,
|
||||
}
|
||||
|
||||
impl Objective {
|
||||
/// 默认硬约束：过速上限 = 目标 × 3 + 500；饱和占比 ≤ 40%。
|
||||
#[must_use]
|
||||
pub fn new(weights: Weights, scenario: Scenario, limits: LoopLimits) -> Self {
|
||||
let max_rpm = scenario.target_rpm.abs() * 3.0 + 500.0;
|
||||
Self { weights, scenario, limits, max_rpm, max_saturated_fraction: 0.4 }
|
||||
}
|
||||
|
||||
/// 由仿真错误折算判定。
|
||||
#[must_use]
|
||||
pub fn from_error(error: SimError) -> Verdict {
|
||||
match error {
|
||||
SimError::NonFiniteGains | SimError::DegenerateScenario => Verdict::Rejected,
|
||||
SimError::Diverged { .. } => Verdict::Diverged,
|
||||
}
|
||||
}
|
||||
|
||||
/// 对一条轨迹评分。
|
||||
#[must_use]
|
||||
pub fn score(&self, trace: &Trace) -> Score {
|
||||
let Some(metrics) = StepMetrics::from_trace(trace, &self.scenario) else {
|
||||
return Score::penalized(Verdict::Rejected);
|
||||
};
|
||||
self.score_metrics(metrics)
|
||||
}
|
||||
|
||||
/// 对一组指标评分（真机后端直接复用，保证两条路径同一把尺子）。
|
||||
#[must_use]
|
||||
pub fn score_metrics(&self, metrics: StepMetrics) -> Score {
|
||||
if metrics.max_rpm > self.max_rpm {
|
||||
return Score { cost: PENALTY, verdict: Verdict::Overspeed, metrics: Some(metrics) };
|
||||
}
|
||||
if metrics.saturated_pct / 100.0 > self.max_saturated_fraction {
|
||||
return Score { cost: PENALTY, verdict: Verdict::Saturated, metrics: Some(metrics) };
|
||||
}
|
||||
if !metrics.settled {
|
||||
return Score { cost: PENALTY, verdict: Verdict::NoSettle, metrics: Some(metrics) };
|
||||
}
|
||||
Score { cost: self.weighted_cost(&metrics), verdict: Verdict::Ok, metrics: Some(metrics) }
|
||||
}
|
||||
|
||||
/// 归一化加权成本。
|
||||
#[must_use]
|
||||
pub fn weighted_cost(&self, metrics: &StepMetrics) -> f64 {
|
||||
let total_s = f64::from(self.scenario.total_s().max(1.0e-3));
|
||||
let target = f64::from(self.scenario.target_rpm.abs()).max(1.0e-3);
|
||||
let limit = f64::from(self.limits.current_limit_raw.max(1));
|
||||
|
||||
let settle = f64::from(metrics.settling_time_s) / total_s;
|
||||
let overshoot = f64::from(metrics.overshoot_pct) / 100.0;
|
||||
let iae = f64::from(metrics.iae) / (target * total_s);
|
||||
let effort = f64::from(metrics.rms_effort_raw) / limit;
|
||||
|
||||
self.weights.settle * settle
|
||||
+ self.weights.overshoot * overshoot
|
||||
+ self.weights.iae * iae
|
||||
+ self.weights.effort * effort
|
||||
}
|
||||
|
||||
/// 稳态误差是否满足验收阈值（报告 / 退出码用）。
|
||||
#[must_use]
|
||||
pub fn passes(
|
||||
&self,
|
||||
metrics: &StepMetrics,
|
||||
max_overshoot_pct: f32,
|
||||
max_ss_error_pct: f32,
|
||||
) -> bool {
|
||||
metrics.settled
|
||||
&& metrics.overshoot_pct <= max_overshoot_pct
|
||||
&& metrics.ss_error_pct <= max_ss_error_pct
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use dji_esc_sim::{PlantParams, simulate};
|
||||
use motor_core::VelocityGains;
|
||||
|
||||
const GOOD: VelocityGains = VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0);
|
||||
|
||||
fn objective(scenario: Scenario) -> Objective {
|
||||
Objective::new(Weights::default(), scenario, LoopLimits::gm3508())
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn good_gains_produce_a_finite_ok_cost() {
|
||||
let scenario = Scenario::step(1500.0, 1.0);
|
||||
let trace =
|
||||
simulate(GOOD, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()).unwrap();
|
||||
let score = objective(scenario).score(&trace);
|
||||
assert_eq!(score.verdict, Verdict::Ok);
|
||||
assert!(score.cost.is_finite() && score.cost < 10.0, "cost = {}", score.cost);
|
||||
assert!(score.metrics.is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_converging_gains_are_penalized_not_weighted() {
|
||||
// 纯比例、极低增益：到不了目标。
|
||||
let scenario = Scenario::step(4000.0, 1.0);
|
||||
let weak = VelocityGains::new(0.01, 0.0, 0.0, 0.0);
|
||||
let trace =
|
||||
simulate(weak, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()).unwrap();
|
||||
let score = objective(scenario).score(&trace);
|
||||
assert_eq!(score.verdict, Verdict::NoSettle);
|
||||
assert!((score.cost - PENALTY).abs() < 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn saturation_is_a_hard_constraint() {
|
||||
let scenario = Scenario::step(9000.0, 1.0);
|
||||
// 增益极大 + 限流很小 → 长时间饱和。
|
||||
let limits = LoopLimits { current_limit_raw: 800, model: dji_esc::EscModel::C620 };
|
||||
let trace = simulate(
|
||||
VelocityGains::new(500.0, 0.0, 0.0, 0.0),
|
||||
&PlantParams::gm3508(),
|
||||
&scenario,
|
||||
limits,
|
||||
)
|
||||
.unwrap();
|
||||
let score = Objective::new(Weights::default(), scenario, limits).score(&trace);
|
||||
assert!(
|
||||
matches!(score.verdict, Verdict::Saturated | Verdict::NoSettle),
|
||||
"{:?}",
|
||||
score.verdict
|
||||
);
|
||||
assert!(score.cost >= PENALTY);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn verdict_labels_are_stable() {
|
||||
assert_eq!(Verdict::Ok.label(), "ok");
|
||||
assert_eq!(Verdict::NoSettle.label(), "no-settle");
|
||||
assert!(Verdict::Ok.is_ok());
|
||||
assert!(!Verdict::Overspeed.is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn weights_must_be_finite_and_non_negative() {
|
||||
assert!(Weights::default().is_valid());
|
||||
let bad = Weights { settle: -1.0, ..Weights::default() };
|
||||
assert!(!bad.is_valid());
|
||||
let nan = Weights { iae: f64::NAN, ..Weights::default() };
|
||||
assert!(!nan.is_valid());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn weighted_cost_decreases_when_the_response_improves() {
|
||||
let scenario = Scenario::step(1500.0, 1.0);
|
||||
let objective = objective(scenario);
|
||||
let good = simulate(GOOD, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()).unwrap();
|
||||
let sluggish = simulate(
|
||||
VelocityGains::new(0.6, 1.0, 0.0, 0.0),
|
||||
&PlantParams::gm3508(),
|
||||
&scenario,
|
||||
LoopLimits::gm3508(),
|
||||
)
|
||||
.unwrap();
|
||||
let good_metrics = StepMetrics::from_trace(&good, &scenario).unwrap();
|
||||
let sluggish_metrics = StepMetrics::from_trace(&sluggish, &scenario).unwrap();
|
||||
assert!(
|
||||
objective.weighted_cost(&good_metrics) < objective.weighted_cost(&sluggish_metrics),
|
||||
"好的响应必须成本更低"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sim_errors_map_to_verdicts() {
|
||||
assert_eq!(Objective::from_error(SimError::NonFiniteGains), Verdict::Rejected);
|
||||
assert_eq!(Objective::from_error(SimError::DegenerateScenario), Verdict::Rejected);
|
||||
assert_eq!(Objective::from_error(SimError::Diverged { step: 3 }), Verdict::Diverged);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,572 @@
|
||||
//! 增益参数空间：轴定义、采样与**坐标空间**映射。
|
||||
//!
|
||||
//! # 为什么要有"坐标空间"
|
||||
//!
|
||||
//! 增益的合法取值跨越数量级（`ki` 可能在 0.1 与 100 之间），线性空间的
|
||||
//! 单纯形步长对数量级不敏感；同时 `kp/ki/kd` 必须为正。把每个轴映射到
|
||||
//! **坐标空间**（对数轴取 `ln`，线性轴恒等）后：
|
||||
//!
|
||||
//! - 无约束优化器不会走出正增益区域；
|
||||
//! - 步长在不同数量级上等价（±0.1 的坐标步长 = ×1.1 的比例步长）；
|
||||
//! - `LogOrZero` 轴在**粗扫**阶段可以精确取 0（`ff = 0` 是台架常态），
|
||||
//! 在**精修**阶段退化为对数轴（零是不可导的拐点，本地搜索收益为负）。
|
||||
|
||||
use motor_core::VelocityGains;
|
||||
use rand::Rng;
|
||||
use rand::seq::SliceRandom as _;
|
||||
use thiserror::Error;
|
||||
|
||||
/// 轴数量：`kp`、`ki`、`kd`、`feedforward`。
|
||||
pub const AXIS_COUNT: usize = 4;
|
||||
|
||||
/// 轴的取值范围映射方式。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub enum Scale {
|
||||
/// 线性均匀。
|
||||
Linear,
|
||||
/// 对数均匀（要求 `min > 0`）。
|
||||
Log,
|
||||
/// 以概率 `p_zero` 取精确 0，否则对数均匀（用于 `ki` / `kd` / `ff`）。
|
||||
LogOrZero {
|
||||
/// 取 0 的概率。
|
||||
p_zero: f64,
|
||||
},
|
||||
}
|
||||
|
||||
impl Scale {
|
||||
/// 是否是对数族（`Log` / `LogOrZero`）。
|
||||
#[must_use]
|
||||
pub fn is_log(self) -> bool {
|
||||
matches!(self, Self::Log | Self::LogOrZero { .. })
|
||||
}
|
||||
|
||||
/// 从文本解析（CLI 用）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 未知名字时返回 [`GainsError::UnknownScale`]。
|
||||
pub fn parse(text: &str) -> Result<Self, GainsError> {
|
||||
match text {
|
||||
"linear" => Ok(Self::Linear),
|
||||
"log" => Ok(Self::Log),
|
||||
"log0" | "log-or-zero" => Ok(Self::LogOrZero { p_zero: 0.25 }),
|
||||
other => Err(GainsError::UnknownScale(other.to_owned())),
|
||||
}
|
||||
}
|
||||
|
||||
/// 文本名。
|
||||
#[must_use]
|
||||
pub fn name(self) -> &'static str {
|
||||
match self {
|
||||
Self::Linear => "linear",
|
||||
Self::Log => "log",
|
||||
Self::LogOrZero { .. } => "log0",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 参数空间定义错误。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
||||
pub enum GainsError {
|
||||
/// 轴区间非法。
|
||||
#[error("invalid range for {axis}: {reason}")]
|
||||
InvalidRange {
|
||||
/// 轴名。
|
||||
axis: String,
|
||||
/// 原因。
|
||||
reason: String,
|
||||
},
|
||||
/// 未知的映射方式名。
|
||||
#[error("unknown scale '{0}' (expected linear|log|log0)")]
|
||||
UnknownScale(String),
|
||||
/// 区间文本无法解析（期望 `MIN..MAX`）。
|
||||
#[error("cannot parse range '{0}' (expected MIN..MAX)")]
|
||||
BadRange(String),
|
||||
/// 只给了对数轴 0 下界。
|
||||
#[error("axis {0} uses a log scale but min must be > 0")]
|
||||
NonPositiveLogMin(String),
|
||||
}
|
||||
|
||||
/// 单条增益轴。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct GainAxis {
|
||||
/// 轴名（`kp` / `ki` / `kd` / `feedforward`）。
|
||||
pub name: &'static str,
|
||||
/// 下界。
|
||||
pub min: f64,
|
||||
/// 上界。
|
||||
pub max: f64,
|
||||
/// 取值映射。
|
||||
pub scale: Scale,
|
||||
}
|
||||
|
||||
impl GainAxis {
|
||||
/// 构造。
|
||||
#[must_use]
|
||||
pub const fn new(name: &'static str, min: f64, max: f64, scale: Scale) -> Self {
|
||||
Self { name, min, max, scale }
|
||||
}
|
||||
|
||||
/// 校验。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 非有限 / `min > max` / 对数轴下界 ≤ 0 时返回错误。
|
||||
pub fn validate(&self) -> Result<(), GainsError> {
|
||||
let axis = self.name.to_owned();
|
||||
if !(self.min.is_finite() && self.max.is_finite()) {
|
||||
return Err(GainsError::InvalidRange { axis, reason: "bounds must be finite".into() });
|
||||
}
|
||||
if self.min > self.max {
|
||||
return Err(GainsError::InvalidRange {
|
||||
axis,
|
||||
reason: format!("min {} > max {}", self.min, self.max),
|
||||
});
|
||||
}
|
||||
if self.scale.is_log() && self.min <= 0.0 {
|
||||
return Err(GainsError::NonPositiveLogMin(axis));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 在区间内采样（含 `LogOrZero` 的零分支）。
|
||||
pub fn sample(&self, rng: &mut impl Rng) -> f64 {
|
||||
match self.scale {
|
||||
Scale::Linear => rng.random_range(self.min..=self.max),
|
||||
Scale::Log => self.log_sample(rng),
|
||||
Scale::LogOrZero { p_zero } => {
|
||||
if rng.random_bool(p_zero.clamp(0.0, 1.0)) {
|
||||
0.0
|
||||
} else {
|
||||
self.log_sample(rng)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn log_sample(&self, rng: &mut impl Rng) -> f64 {
|
||||
let lo = self.min.max(f64::MIN_POSITIVE).ln();
|
||||
let hi = self.max.ln();
|
||||
if hi <= lo {
|
||||
return self.min;
|
||||
}
|
||||
rng.random_range(lo..=hi).exp()
|
||||
}
|
||||
|
||||
/// 夹到区间内。
|
||||
#[must_use]
|
||||
pub fn clamp(&self, value: f64) -> f64 {
|
||||
let clamped = value.clamp(self.min, self.max);
|
||||
if clamped.is_finite() { clamped } else { self.min }
|
||||
}
|
||||
|
||||
/// 参数 → 坐标。
|
||||
#[must_use]
|
||||
pub fn encode(&self, value: f64) -> f64 {
|
||||
if self.scale.is_log() {
|
||||
self.clamp(value).max(f64::MIN_POSITIVE).ln()
|
||||
} else {
|
||||
self.clamp(value)
|
||||
}
|
||||
}
|
||||
|
||||
/// 坐标 → 参数（先夹到坐标边界，再取指数）。
|
||||
#[must_use]
|
||||
pub fn decode(&self, coord: f64) -> f64 {
|
||||
let (lo, hi) = self.coord_bounds();
|
||||
let coord = if coord.is_finite() { coord.clamp(lo, hi) } else { lo };
|
||||
if self.scale.is_log() { coord.exp() } else { coord }
|
||||
}
|
||||
|
||||
/// 坐标空间的边界。
|
||||
#[must_use]
|
||||
pub fn coord_bounds(&self) -> (f64, f64) {
|
||||
if self.scale.is_log() {
|
||||
(self.min.max(f64::MIN_POSITIVE).ln(), self.max.ln())
|
||||
} else {
|
||||
(self.min, self.max)
|
||||
}
|
||||
}
|
||||
|
||||
/// 人类可读描述。
|
||||
#[must_use]
|
||||
pub fn describe(&self) -> String {
|
||||
format!("{} ∈ [{}..{}] ({})", self.name, self.min, self.max, self.scale.name())
|
||||
}
|
||||
}
|
||||
|
||||
/// 四轴参数空间。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct GainSpace {
|
||||
/// 比例增益（raw / rpm）。
|
||||
pub kp: GainAxis,
|
||||
/// 积分增益（raw / (rpm·s)）。
|
||||
pub ki: GainAxis,
|
||||
/// 微分增益。
|
||||
pub kd: GainAxis,
|
||||
/// 速度前馈（raw / rpm）。
|
||||
pub feedforward: GainAxis,
|
||||
}
|
||||
|
||||
impl GainSpace {
|
||||
/// 组装。
|
||||
#[must_use]
|
||||
pub const fn new(kp: GainAxis, ki: GainAxis, kd: GainAxis, feedforward: GainAxis) -> Self {
|
||||
Self { kp, ki, kd, feedforward }
|
||||
}
|
||||
|
||||
/// 默认空间：覆盖空载台架（`kp≈2, ki≈10, ff=0`）与带载（`ff≈1.77`）两侧。
|
||||
#[must_use]
|
||||
pub const fn default_gm3508() -> Self {
|
||||
Self {
|
||||
kp: GainAxis::new("kp", 0.5, 30.0, Scale::Log),
|
||||
ki: GainAxis::new("ki", 0.1, 100.0, Scale::LogOrZero { p_zero: 0.2 }),
|
||||
kd: GainAxis::new("kd", 0.01, 3.0, Scale::LogOrZero { p_zero: 0.5 }),
|
||||
feedforward: GainAxis::new("feedforward", 0.01, 2.5, Scale::LogOrZero { p_zero: 0.2 }),
|
||||
}
|
||||
}
|
||||
|
||||
/// 校验全部轴。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 任一轴非法时返回错误。
|
||||
pub fn validate(&self) -> Result<(), GainsError> {
|
||||
for axis in self.axes() {
|
||||
axis.validate()?;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 四轴（顺序固定 `kp, ki, kd, feedforward`）。
|
||||
#[must_use]
|
||||
pub const fn axes(&self) -> [GainAxis; AXIS_COUNT] {
|
||||
[self.kp, self.ki, self.kd, self.feedforward]
|
||||
}
|
||||
|
||||
/// 随机采样一个候选增益。
|
||||
pub fn sample(&self, rng: &mut impl Rng) -> VelocityGains {
|
||||
let [kp, ki, kd, ff] = self.axes().map(|axis| axis.sample(rng));
|
||||
VelocityGains::new(kp as f32, ki as f32, kd as f32, ff as f32)
|
||||
}
|
||||
|
||||
/// 分层（Latin Hypercube）采样：每个轴把 `[0,1)` 等分成 `count` 层，
|
||||
/// 每层恰好落一个样本，再逐轴独立打乱。
|
||||
///
|
||||
/// 相比独立随机采样，分层采样在四维空间里不会留下大片空洞，粗扫阶段
|
||||
/// 用更少的评估次数覆盖更大的空间。
|
||||
pub fn stratified(&self, count: usize, rng: &mut impl Rng) -> Vec<VelocityGains> {
|
||||
let count = count.max(1);
|
||||
let mut columns: Vec<Vec<f64>> = Vec::with_capacity(AXIS_COUNT);
|
||||
for axis in self.axes() {
|
||||
let (lo, hi) = axis.coord_bounds();
|
||||
let mut column = Vec::with_capacity(count);
|
||||
for layer in 0..count {
|
||||
let unit = (layer as f64 + rng.random_range(0.0..1.0)) / count as f64;
|
||||
column.push(lo + unit * (hi - lo));
|
||||
}
|
||||
column.shuffle(rng);
|
||||
columns.push(column);
|
||||
}
|
||||
(0..count)
|
||||
.map(|index| {
|
||||
self.from_coords([
|
||||
columns[0][index],
|
||||
columns[1][index],
|
||||
columns[2][index],
|
||||
columns[3][index],
|
||||
])
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// 把任意（可能越界 / 非有限）增益夹进空间。
|
||||
#[must_use]
|
||||
pub fn clamp_gains(&self, gains: VelocityGains) -> VelocityGains {
|
||||
self.from_values([gains.kp, gains.ki, gains.kd, gains.feedforward].map(f64::from))
|
||||
}
|
||||
|
||||
/// 由四个参数值构造（先夹界，再保证有限）。
|
||||
#[must_use]
|
||||
pub fn from_values(&self, values: [f64; AXIS_COUNT]) -> VelocityGains {
|
||||
let axes = self.axes();
|
||||
let v: [f64; AXIS_COUNT] = std::array::from_fn(|i| axes[i].clamp(values[i]));
|
||||
VelocityGains::new(v[0] as f32, v[1] as f32, v[2] as f32, v[3] as f32)
|
||||
}
|
||||
|
||||
/// 参数 → 坐标。
|
||||
#[must_use]
|
||||
pub fn to_coords(&self, gains: &VelocityGains) -> [f64; AXIS_COUNT] {
|
||||
let axes = self.axes();
|
||||
let values = [gains.kp, gains.ki, gains.kd, gains.feedforward].map(f64::from);
|
||||
std::array::from_fn(|i| axes[i].encode(values[i]))
|
||||
}
|
||||
|
||||
/// 坐标 → 参数。
|
||||
#[must_use]
|
||||
pub fn from_coords(&self, coords: [f64; AXIS_COUNT]) -> VelocityGains {
|
||||
let axes = self.axes();
|
||||
let v: [f64; AXIS_COUNT] = std::array::from_fn(|i| axes[i].decode(coords[i]));
|
||||
VelocityGains::new(v[0] as f32, v[1] as f32, v[2] as f32, v[3] as f32)
|
||||
}
|
||||
|
||||
/// 坐标空间的边界（下界数组, 上界数组）。
|
||||
#[must_use]
|
||||
pub fn coord_bounds(&self) -> ([f64; AXIS_COUNT], [f64; AXIS_COUNT]) {
|
||||
let axes = self.axes();
|
||||
let pairs: [(f64, f64); AXIS_COUNT] = std::array::from_fn(|i| axes[i].coord_bounds());
|
||||
let lower = std::array::from_fn(|i| pairs[i].0);
|
||||
let upper = std::array::from_fn(|i| pairs[i].1);
|
||||
(lower, upper)
|
||||
}
|
||||
|
||||
/// 最优解是否贴在某个轴的边界上（提示是否该放宽搜索范围）。
|
||||
///
|
||||
/// 贴边界通常意味着"约束而不是数据在决定结果"——继续在这个盒子里搜索
|
||||
/// 不会更好，报告里要明确说出来，而不是把一个边界值当成最优解交付。
|
||||
#[must_use]
|
||||
pub fn boundary_hits(&self, gains: &VelocityGains) -> Vec<String> {
|
||||
let values = [gains.kp, gains.ki, gains.kd, gains.feedforward];
|
||||
let mut hits = Vec::new();
|
||||
for (axis, value) in self.axes().iter().zip(values) {
|
||||
let value = f64::from(value);
|
||||
let tolerance = ((axis.max - axis.min).abs() * 0.01).max(1.0e-9);
|
||||
if (value - axis.max).abs() <= tolerance {
|
||||
hits.push(format!(
|
||||
"{} 落在上界 {}（若要继续提升，放宽 --{}）",
|
||||
axis.name, axis.max, axis.name
|
||||
));
|
||||
} else if (value - axis.min).abs() <= tolerance {
|
||||
hits.push(format!(
|
||||
"{} 落在下界 {}（若要继续压缩，放宽 --{}）",
|
||||
axis.name, axis.min, axis.name
|
||||
));
|
||||
}
|
||||
}
|
||||
hits
|
||||
}
|
||||
|
||||
/// 人类可读描述（报告用）。
|
||||
#[must_use]
|
||||
pub fn describe(&self) -> String {
|
||||
self.axes().iter().map(GainAxis::describe).collect::<Vec<_>>().join("; ")
|
||||
}
|
||||
}
|
||||
|
||||
/// 解析 `MIN..MAX`（`..=` 视为等价）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 无法解析或区间非法时返回错误。
|
||||
pub fn parse_axis(name: &'static str, spec: &str, scale: Scale) -> Result<GainAxis, GainsError> {
|
||||
let spec = spec.trim();
|
||||
let bad = || GainsError::BadRange(spec.to_owned());
|
||||
// `MIN..MAX` 与 `MIN..=MAX` 视为等价；单个数值表示固定该轴。
|
||||
let (min_text, max_text) = match spec.split_once("..=").or_else(|| spec.split_once("..")) {
|
||||
Some((lo, hi)) => (lo.trim(), hi.trim()),
|
||||
None => (spec, spec),
|
||||
};
|
||||
if min_text.is_empty() || max_text.is_empty() {
|
||||
return Err(bad());
|
||||
}
|
||||
let min: f64 = min_text.parse().map_err(|_| bad())?;
|
||||
let max: f64 = max_text.parse().map_err(|_| bad())?;
|
||||
let axis = GainAxis::new(name, min, max, scale);
|
||||
axis.validate()?;
|
||||
Ok(axis)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use rand::SeedableRng as _;
|
||||
use rand::rngs::StdRng;
|
||||
|
||||
fn rng(seed: u64) -> StdRng {
|
||||
StdRng::seed_from_u64(seed)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn clamp_keeps_values_inside_the_space() {
|
||||
let space = GainSpace::default_gm3508();
|
||||
let wild = VelocityGains::new(1.0e9, -5.0, 100.0, f32::NAN);
|
||||
let clamped = space.clamp_gains(wild);
|
||||
assert!(clamped.kp <= 30.0 && clamped.kp >= 0.5);
|
||||
assert!(clamped.ki >= 0.1);
|
||||
assert!(clamped.kd <= 3.0);
|
||||
assert!(clamped.feedforward.is_finite());
|
||||
assert!(clamped.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sampling_stays_inside_bounds_and_is_deterministic() {
|
||||
let space = GainSpace::default_gm3508();
|
||||
let mut a = rng(7);
|
||||
let mut b = rng(7);
|
||||
for _ in 0..500 {
|
||||
let g1 = space.sample(&mut a);
|
||||
let g2 = space.sample(&mut b);
|
||||
assert_eq!(g1, g2, "同 seed 必须一致");
|
||||
assert!((0.5..=30.0).contains(&g1.kp));
|
||||
assert!((0.0..=100.0).contains(&g1.ki));
|
||||
assert!((0.0..=3.0).contains(&g1.kd));
|
||||
assert!((0.0..=2.5).contains(&g1.feedforward));
|
||||
assert!(g1.is_finite());
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn log_axis_samples_across_orders_of_magnitude() {
|
||||
let axis = GainAxis::new("kp", 0.1, 100.0, Scale::Log);
|
||||
let mut rng = rng(3);
|
||||
let mut below_one = 0;
|
||||
let mut above_ten = 0;
|
||||
for _ in 0..2000 {
|
||||
let value = axis.sample(&mut rng);
|
||||
assert!((0.1..=100.0).contains(&value));
|
||||
if value < 1.0 {
|
||||
below_one += 1;
|
||||
}
|
||||
if value > 10.0 {
|
||||
above_ten += 1;
|
||||
}
|
||||
}
|
||||
// 对数均匀：三个数量级应大致各占 1/3。
|
||||
assert!(below_one > 400 && below_one < 900, "below_one = {below_one}");
|
||||
assert!(above_ten > 400 && above_ten < 900, "above_ten = {above_ten}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn log_or_zero_hits_exact_zero() {
|
||||
let axis = GainAxis::new("feedforward", 0.01, 2.5, Scale::LogOrZero { p_zero: 0.5 });
|
||||
let mut rng = rng(11);
|
||||
let mut zeros = 0;
|
||||
for _ in 0..2000 {
|
||||
if axis.sample(&mut rng) == 0.0 {
|
||||
zeros += 1;
|
||||
}
|
||||
}
|
||||
assert!((800..1200).contains(&zeros), "zeros = {zeros}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn coord_round_trip_is_stable() {
|
||||
let space = GainSpace::default_gm3508();
|
||||
let mut rng = rng(5);
|
||||
let mut saw_zero = false;
|
||||
for _ in 0..200 {
|
||||
let gains = space.sample(&mut rng);
|
||||
let back = space.from_coords(space.to_coords(&gains));
|
||||
// 坐标空间对**非零**取值是双射（只允许 f32 舍入误差）；
|
||||
// 0 在对数轴里没有原像，往返会落到该轴下界——这是 LogOrZero 的定义。
|
||||
for (axis, (sampled, restored)) in space.axes().iter().zip([
|
||||
(gains.kp, back.kp),
|
||||
(gains.ki, back.ki),
|
||||
(gains.kd, back.kd),
|
||||
(gains.feedforward, back.feedforward),
|
||||
]) {
|
||||
if sampled == 0.0 {
|
||||
saw_zero = true;
|
||||
assert!(
|
||||
(restored - axis.min as f32).abs() <= 1e-6 * axis.min as f32,
|
||||
"{} 的零值往返应落到下界 {}，得到 {restored}",
|
||||
axis.name,
|
||||
axis.min
|
||||
);
|
||||
} else {
|
||||
let tolerance = 1e-3 * sampled.max(1.0);
|
||||
assert!(
|
||||
(restored - sampled).abs() <= tolerance,
|
||||
"{} 往返误差过大：{sampled} → {restored}",
|
||||
axis.name
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
assert!(saw_zero, "LogOrZero 轴必须真的采到过 0");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decode_clamps_out_of_range_coordinates() {
|
||||
let space = GainSpace::default_gm3508();
|
||||
let below = space.from_coords([-1.0e9, -1.0e9, -1.0e9, -1.0e9]);
|
||||
assert!(below.kp >= 0.5);
|
||||
let above = space.from_coords([1.0e9; AXIS_COUNT]);
|
||||
assert!(above.kp <= 30.0);
|
||||
assert!(above.feedforward.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn stratified_covers_every_layer_of_every_axis() {
|
||||
let space = GainSpace::default_gm3508();
|
||||
let count = 32;
|
||||
let samples = space.stratified(count, &mut rng(9));
|
||||
assert_eq!(samples.len(), count);
|
||||
// 每个轴在坐标空间被等分：把样本坐标排序后，相邻差应大致等于层宽。
|
||||
let (lower, upper) = space.coord_bounds();
|
||||
let span = upper[0] - lower[0];
|
||||
let mut coords: Vec<f64> = samples.iter().map(|g| space.to_coords(g)[0]).collect();
|
||||
coords.sort_by(f64::total_cmp);
|
||||
let layer = span / count as f64;
|
||||
for window in coords.windows(2) {
|
||||
let gap = window[1] - window[0];
|
||||
assert!(gap < layer * 3.0, "gap {gap} too large for layer {layer}");
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn stratified_handles_large_counts_without_truncation() {
|
||||
let space = GainSpace::default_gm3508();
|
||||
let samples = space.stratified(96, &mut rng(4));
|
||||
assert_eq!(samples.len(), 96);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn parse_axis_accepts_ranges_and_single_values() {
|
||||
let axis = parse_axis("kp", "0.5..20", Scale::Log).unwrap();
|
||||
assert!((axis.min - 0.5).abs() < 1e-12);
|
||||
assert!((axis.max - 20.0).abs() < 1e-12);
|
||||
|
||||
let open = parse_axis("ki", "0..=50", Scale::Linear).unwrap();
|
||||
assert!((open.max - 50.0).abs() < 1e-12);
|
||||
|
||||
let fixed = parse_axis("kd", "0.25", Scale::Linear).unwrap();
|
||||
assert!((fixed.min - 0.25).abs() < 1e-12 && (fixed.max - 0.25).abs() < 1e-12);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn parse_axis_rejects_garbage() {
|
||||
assert!(matches!(parse_axis("kp", "abc", Scale::Log), Err(GainsError::BadRange(_))));
|
||||
assert!(matches!(parse_axis("kp", "", Scale::Log), Err(GainsError::BadRange(_))));
|
||||
assert!(matches!(
|
||||
parse_axis("kp", "-1..5", Scale::Log),
|
||||
Err(GainsError::NonPositiveLogMin(_))
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn validate_rejects_inverted_and_non_finite_bounds() {
|
||||
let bad = GainAxis::new("kp", 10.0, 1.0, Scale::Linear);
|
||||
assert!(bad.validate().is_err());
|
||||
let nan = GainAxis::new("kp", f64::NAN, 1.0, Scale::Linear);
|
||||
assert!(nan.validate().is_err());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn validate_accepts_the_default_space() {
|
||||
assert!(GainSpace::default_gm3508().validate().is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn boundary_hits_flag_pinned_axes_only() {
|
||||
let space = GainSpace::default_gm3508();
|
||||
let pinned = VelocityGains::new(30.0, 0.1, 1.0, 1.0);
|
||||
let hits = space.boundary_hits(&pinned);
|
||||
assert_eq!(hits.len(), 2, "{hits:?}");
|
||||
assert!(hits.iter().any(|hit| hit.contains("kp")), "{hits:?}");
|
||||
assert!(hits.iter().any(|hit| hit.contains("ki")), "{hits:?}");
|
||||
|
||||
let inside = VelocityGains::new(5.0, 10.0, 0.5, 1.0);
|
||||
assert!(space.boundary_hits(&inside).is_empty());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
//! 底盘速度环 **PID 自动整定**：参数空间搜索 + 指标评估 + 报告。
|
||||
//!
|
||||
//! # 一条命令背后发生了什么
|
||||
//!
|
||||
//! 1. [`gains`] 定义搜索空间（四轴、对数刻度、可精确取 0 的前馈 / 微分）；
|
||||
//! 2. [`search`] 先做种子化分层粗扫（`rayon` 并行），再以 top-k 为起点做
|
||||
//! 多起点 `argmin` 局部精修；
|
||||
//! 3. 每次评估由 [`backend`] 执行：离线 harness 在**被控对象集成**上重放场景，
|
||||
//! 或（真机后端）在台架上跑一次受安全包络约束的试验；
|
||||
//! 4. [`metrics`] / [`cost`] 把轨迹折成加权成本，硬约束（过速 / 长时饱和 /
|
||||
//! 不收敛）直接给罚值，不参与权重折衷；
|
||||
//! 5. [`report`] 落盘 CSV / TOML / JSON / Markdown（可选 SVG）。
|
||||
//!
|
||||
//! # 超参数从哪来
|
||||
//!
|
||||
//! "搜什么、怎么打分、什么算达标"由 [`preset`] 定义，优先级为
|
||||
//! **内置默认 < 预设文件（`config/tune.toml`）< 显式命令行参数**。
|
||||
//! 一次评审过的预设可以长期复用，不再靠命令行回忆当时那组数。
|
||||
//!
|
||||
//! # 可复现
|
||||
//!
|
||||
//! 随机性只来自 `--seed`：同一 seed + 同一参数空间 + 同一后端 → 逐位一致的结果。
|
||||
//! 这是 CI 里做冒烟回归的前提。预设只是把参数空间 / 权重 / 场景固化下来，
|
||||
//! 因此"同一个预设 + 同一个 seed"在任何机器上都应当复现同一结果。
|
||||
//!
|
||||
//! # 安全
|
||||
//!
|
||||
//! 真机后端（`--backend bench`）会**真的驱动电机**。安全由三层保证：
|
||||
//! 搜索前的强制确认标志、每次试验的 `chassis-bench` 安全包络、以及任何退出
|
||||
//! 路径都先归零电流。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod backend;
|
||||
pub mod bench;
|
||||
pub mod cost;
|
||||
pub mod gains;
|
||||
pub mod metrics;
|
||||
#[cfg(feature = "plot")]
|
||||
pub mod plot;
|
||||
pub mod preset;
|
||||
pub mod report;
|
||||
pub mod search;
|
||||
|
||||
pub use backend::{Evaluated, Evaluator, SimEvaluator};
|
||||
pub use bench::{BenchConfig, BenchError, BenchEvaluator};
|
||||
pub use cost::{Objective, PENALTY, Score, Verdict, Weights};
|
||||
pub use gains::{AXIS_COUNT, GainAxis, GainSpace, GainsError, Scale, parse_axis};
|
||||
pub use metrics::StepMetrics;
|
||||
pub use preset::{
|
||||
Acceptance, AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec,
|
||||
DEFAULT_PRESET_PATH, PRESET_VERSION, PlantSpec, Preset, PresetError, PresetFile, ProfileSpec,
|
||||
RangeSpec, ScaleSpec, ScenarioSpec, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning,
|
||||
};
|
||||
pub use report::{ReportContext, ReportError, write_reports};
|
||||
pub use search::{
|
||||
Phase, Ranked, SearchConfig, SearchError, SolverKind, Trial, evaluate_once, sweep,
|
||||
};
|
||||
|
||||
/// 被整定的物理量类型（来自 `motor-core`，与驱动层、台架共用同一个定义）。
|
||||
pub use motor_core::VelocityGains;
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,265 @@
|
||||
//! 步响应指标：把一条 [`Trace`] 折成可比较、可解释的标量。
|
||||
//!
|
||||
//! 指标口径对**所有后端**（离线 harness、真机台架）一致——整定结果能不能
|
||||
//! 从仿真迁移到实车，取决于两条路径是否用同一把尺子量。
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
|
||||
/// 稳态判据的相对带宽（±2% of target）。
|
||||
pub const SETTLING_BAND: f32 = 0.02;
|
||||
/// 稳态窗口占轨迹总时长的比例（从 75% 处开始）。
|
||||
///
|
||||
/// 与场景自带的 `steady_start_s()` 取**更晚**者：阶跃场景没有显式斜坡，
|
||||
/// 若不设下限就会把整条上升段也算进"稳态"，任何真实响应都会被判成未收敛。
|
||||
pub const STEADY_FRACTION: f32 = 0.75;
|
||||
/// 指标最少需要的采样数（样本太少时一切统计都无意义）。
|
||||
pub const MIN_SAMPLES: usize = 8;
|
||||
|
||||
/// 一条步响应轨迹的指标集合。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct StepMetrics {
|
||||
/// 上升时间 10% → 90%（秒）；未到达时等于轨迹总时长。
|
||||
pub rise_time_s: f32,
|
||||
/// 超调量（%，相对目标；负值截为 0）。
|
||||
pub overshoot_pct: f32,
|
||||
/// 调节时间：最后一次离开 ±2% 带的时刻（秒）。
|
||||
pub settling_time_s: f32,
|
||||
/// 是否在稳态窗口内保持 ±2% 带（即真正收敛）。
|
||||
pub settled: bool,
|
||||
/// 稳态误差（%，相对目标）。
|
||||
pub ss_error_pct: f32,
|
||||
/// 绝对误差积分 ∫|e|dt（归一化前的原始值）。
|
||||
pub iae: f32,
|
||||
/// 误差平方积分 ∫e²dt。
|
||||
pub ise: f32,
|
||||
/// 时间加权绝对误差积分 ∫t|e|dt。
|
||||
pub itae: f32,
|
||||
/// 峰值 effort（raw，绝对值）。
|
||||
pub peak_effort_raw: f32,
|
||||
/// effort 均方根（raw）。
|
||||
pub rms_effort_raw: f32,
|
||||
/// effort 符号翻转次数（抖振）。
|
||||
pub effort_reversals: usize,
|
||||
/// 饱和采样占比（%）。
|
||||
pub saturated_pct: f32,
|
||||
/// 全过程最大转速（rpm，绝对值）。
|
||||
pub max_rpm: f32,
|
||||
/// 稳态窗口内的平均转速（rpm）。
|
||||
pub steady_rpm: f32,
|
||||
}
|
||||
|
||||
impl StepMetrics {
|
||||
/// 从轨迹计算指标；样本不足或目标转速为 0 时返回 `None`。
|
||||
#[must_use]
|
||||
pub fn from_trace(trace: &Trace, scenario: &Scenario) -> Option<Self> {
|
||||
let target = scenario.target_rpm;
|
||||
if trace.len() < MIN_SAMPLES || trace.len() != trace.t_s.len() {
|
||||
return None;
|
||||
}
|
||||
if !target.is_finite() || target.abs() < f32::EPSILON {
|
||||
return None;
|
||||
}
|
||||
let sign = if target < 0.0 { -1.0 } else { 1.0 };
|
||||
let magnitude = target.abs();
|
||||
|
||||
// 上升时间：10% / 90% 两个阈值首次被跨过的时刻。
|
||||
let t10 = trace
|
||||
.rpm
|
||||
.iter()
|
||||
.zip(&trace.t_s)
|
||||
.find(|(rpm, _)| **rpm * sign >= 0.1 * magnitude)
|
||||
.map(|(_, &t)| t);
|
||||
let t90 = trace
|
||||
.rpm
|
||||
.iter()
|
||||
.zip(&trace.t_s)
|
||||
.find(|(rpm, _)| **rpm * sign >= 0.9 * magnitude)
|
||||
.map(|(_, &t)| t);
|
||||
let total_s = trace.t_s.last().copied().unwrap_or(0.0);
|
||||
let rise_time_s = match (t10, t90) {
|
||||
(Some(t10), Some(t90)) if t90 >= t10 => t90 - t10,
|
||||
// 没有跨过 90%：用剩余时长表达"到不了"，让成本函数自然惩罚它。
|
||||
_ => total_s,
|
||||
};
|
||||
|
||||
// 超调：朝着目标方向的过冲。
|
||||
let peak_directed =
|
||||
trace.rpm.iter().map(|rpm| *rpm * sign).fold(f32::NEG_INFINITY, f32::max);
|
||||
let overshoot_pct = ((peak_directed - magnitude) / magnitude * 100.0).max(0.0);
|
||||
|
||||
// 稳态窗口：场景给出的起点与"最后 25% 时长"取更晚者。
|
||||
let steady_start = (total_s * STEADY_FRACTION).max(scenario.steady_start_s());
|
||||
let steady_end = scenario.steady_end_s().max(steady_start);
|
||||
let window = trace.window(steady_start, steady_end);
|
||||
let steady_rpm = window.mean_rpm();
|
||||
let ss_error_pct = ((steady_rpm - target).abs() / magnitude) * 100.0;
|
||||
|
||||
// 调节时间：最后一次离开 ±band 的时刻；此后一直留在带内才算收敛。
|
||||
let band = SETTLING_BAND * magnitude;
|
||||
let mut settling_time_s = 0.0f32;
|
||||
for (rpm, &t) in trace.rpm.iter().zip(&trace.t_s) {
|
||||
if (*rpm - target).abs() > band {
|
||||
settling_time_s = t;
|
||||
}
|
||||
}
|
||||
// 收敛 = 最后一次离开带宽的时刻已经进入稳态窗口（此后一直留在带内）。
|
||||
let settled = settling_time_s <= steady_start && !window.is_empty();
|
||||
|
||||
// 误差积分（全过程，按 dt 累加）。
|
||||
let mut abs_error_area = 0.0f32;
|
||||
let mut squared_error_area = 0.0f32;
|
||||
let mut time_weighted_area = 0.0f32;
|
||||
for index in 1..trace.len() {
|
||||
let dt = (trace.t_s[index] - trace.t_s[index - 1]).max(0.0);
|
||||
let error = trace.setpoint_rpm[index] - trace.rpm[index];
|
||||
abs_error_area += error.abs() * dt;
|
||||
squared_error_area += error * error * dt;
|
||||
time_weighted_area += trace.t_s[index] * error.abs() * dt;
|
||||
}
|
||||
|
||||
let peak_effort_raw =
|
||||
trace.effort_raw.iter().map(|&raw| f32::from(raw).abs()).fold(0.0f32, f32::max);
|
||||
let rms_effort_raw = if trace.effort_raw.is_empty() {
|
||||
0.0
|
||||
} else {
|
||||
(trace.effort_raw.iter().map(|&raw| f32::from(raw) * f32::from(raw)).sum::<f32>()
|
||||
/ trace.effort_raw.len() as f32)
|
||||
.sqrt()
|
||||
};
|
||||
|
||||
Some(Self {
|
||||
rise_time_s,
|
||||
overshoot_pct,
|
||||
settling_time_s,
|
||||
settled,
|
||||
ss_error_pct,
|
||||
iae: abs_error_area,
|
||||
ise: squared_error_area,
|
||||
itae: time_weighted_area,
|
||||
peak_effort_raw,
|
||||
rms_effort_raw,
|
||||
effort_reversals: trace.effort_reversals(),
|
||||
saturated_pct: trace.saturated_fraction() * 100.0,
|
||||
max_rpm: trace.peak_rpm(),
|
||||
steady_rpm,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn scenario() -> Scenario {
|
||||
Scenario::step(1000.0, 1.0)
|
||||
}
|
||||
|
||||
/// 合成一条轨迹：一阶响应 + 可选超调 + 可选末端偏差。
|
||||
fn synthetic(alpha: f32, overshoot: f32, final_offset: f32) -> Trace {
|
||||
let scenario = scenario();
|
||||
let dt = scenario.dt_s;
|
||||
let steps = scenario.samples();
|
||||
let mut trace = Trace::with_capacity(steps);
|
||||
let mut rpm = 0.0f32;
|
||||
for step in 0..steps {
|
||||
let t = step as f32 * dt;
|
||||
let target = scenario.setpoint_at(t);
|
||||
rpm += (target - rpm) * alpha;
|
||||
let value = if t > 0.3 {
|
||||
rpm + overshoot * 1000.0 * (-(t - 0.3) * 20.0).exp() + final_offset
|
||||
} else {
|
||||
rpm
|
||||
};
|
||||
trace.push(t, target, value, 0, false);
|
||||
}
|
||||
trace
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn first_order_response_has_no_overshoot_and_settles() {
|
||||
let trace = synthetic(0.02, 0.0, 0.0);
|
||||
let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap();
|
||||
assert!(metrics.overshoot_pct < 0.5, "overshoot = {}", metrics.overshoot_pct);
|
||||
assert!(metrics.settled, "一阶响应必须收敛");
|
||||
assert!(metrics.rise_time_s > 0.0 && metrics.rise_time_s < 1.0);
|
||||
assert!(metrics.ss_error_pct < 1.0, "ss = {}", metrics.ss_error_pct);
|
||||
assert!(metrics.iae > 0.0 && metrics.itae > 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn overshoot_is_measured_against_the_target() {
|
||||
let trace = synthetic(0.05, 0.2, 0.0);
|
||||
let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap();
|
||||
assert!(
|
||||
(metrics.overshoot_pct - 20.0).abs() < 3.0,
|
||||
"overshoot = {}",
|
||||
metrics.overshoot_pct
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn steady_state_offset_shows_up() {
|
||||
let trace = synthetic(0.02, 0.0, 50.0);
|
||||
let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap();
|
||||
assert!((metrics.ss_error_pct - 5.0).abs() < 1.0, "ss = {}", metrics.ss_error_pct);
|
||||
// 5% 的末端偏差超出 ±2% 带 → 不算收敛。
|
||||
assert!(!metrics.settled);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_converging_response_is_flagged() {
|
||||
// 误差始终不减（alpha 很小），且稳态窗口内仍在带外。
|
||||
let trace = synthetic(0.0005, 0.0, 0.0);
|
||||
let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap();
|
||||
assert!(!metrics.settled, "未收敛必须被标记");
|
||||
assert!(metrics.ss_error_pct > 2.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn effort_statistics_and_saturation_are_reported() {
|
||||
let mut trace = synthetic(0.02, 0.0, 0.0);
|
||||
for (index, raw) in trace.effort_raw.iter_mut().enumerate() {
|
||||
*raw = if index % 2 == 0 { 500 } else { -500 };
|
||||
}
|
||||
trace.saturated.fill(true);
|
||||
let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap();
|
||||
assert!((metrics.peak_effort_raw - 500.0).abs() < 1e-3);
|
||||
assert!((metrics.rms_effort_raw - 500.0).abs() < 1e-3);
|
||||
assert_eq!(metrics.effort_reversals, trace.len() - 1);
|
||||
assert!((metrics.saturated_pct - 100.0).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn negative_targets_are_handled_symmetrically() {
|
||||
let scenario = Scenario::step(-1000.0, 1.0);
|
||||
let steps = scenario.samples();
|
||||
let mut trace = Trace::with_capacity(steps);
|
||||
let mut rpm = 0.0f32;
|
||||
for step in 0..steps {
|
||||
let t = step as f32 * scenario.dt_s;
|
||||
let target = scenario.setpoint_at(t);
|
||||
rpm += (target - rpm) * 0.02;
|
||||
trace.push(t, target, rpm, 0, false);
|
||||
}
|
||||
let metrics = StepMetrics::from_trace(&trace, &scenario).unwrap();
|
||||
assert!(metrics.settled);
|
||||
assert!(metrics.ss_error_pct < 1.0);
|
||||
assert!(metrics.overshoot_pct < 1.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn degenerate_inputs_are_rejected() {
|
||||
let short = Trace {
|
||||
t_s: vec![0.0, 0.001],
|
||||
setpoint_rpm: vec![1.0, 1.0],
|
||||
rpm: vec![0.0, 0.1],
|
||||
effort_raw: vec![0, 0],
|
||||
saturated: vec![false, false],
|
||||
};
|
||||
assert!(StepMetrics::from_trace(&short, &scenario()).is_none());
|
||||
|
||||
let zero_target = Scenario::step(0.0, 1.0);
|
||||
let trace = synthetic(0.02, 0.0, 0.0);
|
||||
assert!(StepMetrics::from_trace(&trace, &zero_target).is_none());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,129 @@
|
||||
//! 可选（`--features plot`）：把最优步响应画成 SVG。
|
||||
//!
|
||||
//! 为什么是 SVG 而不是 PNG：矢量图在任何缩放级别都能看清起动段的细节，
|
||||
//! 且不需要位图编码器（依赖更小）。默认关闭该 feature——绘图不是整定的
|
||||
//! 必要条件，CSV 已经给了全部数据。
|
||||
|
||||
use std::path::Path;
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
use plotters::backend::SVGBackend;
|
||||
use plotters::chart::ChartBuilder;
|
||||
use plotters::drawing::IntoDrawingArea as _;
|
||||
use plotters::series::LineSeries;
|
||||
use plotters::style::{BLACK, BLUE, Color as _, RED, WHITE};
|
||||
|
||||
use crate::report::ReportError;
|
||||
|
||||
/// 写出步响应三联图：转速跟随、误差、电流。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 绘图或写文件失败时返回 [`ReportError::Plot`]。
|
||||
pub fn write_step_svg(path: &Path, trace: &Trace, scenario: &Scenario) -> Result<(), ReportError> {
|
||||
let root = SVGBackend::new(path, (1000, 900)).into_drawing_area();
|
||||
root.fill(&WHITE).map_err(plot_err)?;
|
||||
let (upper, lower) = root.split_vertically(600);
|
||||
let (middle, bottom) = lower.split_vertically(150);
|
||||
|
||||
let total = trace.t_s.last().copied().unwrap_or(1.0).max(1.0e-3);
|
||||
let peak = trace.rpm.iter().copied().fold(scenario.target_rpm, f32::max).max(1.0);
|
||||
let effort_peak =
|
||||
trace.effort_raw.iter().map(|&raw| f32::from(raw).abs()).fold(1.0f32, f32::max);
|
||||
|
||||
// 转速跟随。
|
||||
let mut chart = ChartBuilder::on(&upper)
|
||||
.caption("velocity step response (rpm)", ("sans-serif", 24))
|
||||
.margin(12)
|
||||
.x_label_area_size(36)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, 0.0f32..peak * 1.1)
|
||||
.map_err(plot_err)?;
|
||||
chart.configure_mesh().x_desc("t (s)").y_desc("rpm").draw().map_err(plot_err)?;
|
||||
chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().copied().zip(trace.setpoint_rpm.iter().copied()),
|
||||
&BLACK,
|
||||
))
|
||||
.map_err(plot_err)?
|
||||
.label("setpoint")
|
||||
.legend(|(x, y)| plotters::element::PathElement::new([(x, y), (x + 18, y)], BLUE));
|
||||
chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().copied().zip(trace.rpm.iter().copied()),
|
||||
&BLUE,
|
||||
))
|
||||
.map_err(plot_err)?
|
||||
.label("measured")
|
||||
.legend(|(x, y)| plotters::element::PathElement::new([(x, y), (x + 18, y)], RED));
|
||||
chart.configure_series_labels().background_style(WHITE.mix(0.8)).draw().map_err(plot_err)?;
|
||||
|
||||
// 误差。
|
||||
let mut error_chart = ChartBuilder::on(&middle)
|
||||
.caption("error (rpm)", ("sans-serif", 16))
|
||||
.margin(8)
|
||||
.x_label_area_size(28)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, -peak..peak)
|
||||
.map_err(plot_err)?;
|
||||
error_chart.configure_mesh().draw().map_err(plot_err)?;
|
||||
error_chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace
|
||||
.t_s
|
||||
.iter()
|
||||
.zip(trace.setpoint_rpm.iter().zip(trace.rpm.iter()))
|
||||
.map(|(&t, (&sp, &rpm))| (t, sp - rpm)),
|
||||
&RED,
|
||||
))
|
||||
.map_err(plot_err)?;
|
||||
|
||||
// 电流。
|
||||
let mut effort_chart = ChartBuilder::on(&bottom)
|
||||
.caption("effort (raw)", ("sans-serif", 16))
|
||||
.margin(8)
|
||||
.x_label_area_size(28)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, -effort_peak..effort_peak)
|
||||
.map_err(plot_err)?;
|
||||
effort_chart.configure_mesh().draw().map_err(plot_err)?;
|
||||
effort_chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().zip(trace.effort_raw.iter()).map(|(&t, &raw)| (t, f32::from(raw))),
|
||||
&BLUE,
|
||||
))
|
||||
.map_err(plot_err)?;
|
||||
|
||||
root.present().map_err(plot_err)?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn plot_err<E: std::fmt::Debug>(error: E) -> ReportError {
|
||||
ReportError::Plot(format!("{error:?}"))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use dji_esc_sim::{LoopLimits, PlantParams, SetpointProfile, simulate};
|
||||
use motor_core::VelocityGains;
|
||||
|
||||
#[test]
|
||||
fn writes_a_non_empty_svg() {
|
||||
let scenario =
|
||||
Scenario::ramp_and_hold(1200.0, 0.2, 0.8).with_profile(SetpointProfile::RampAndHold);
|
||||
let trace = simulate(
|
||||
VelocityGains::new(4.0, 12.0, 0.0, 0.3),
|
||||
&PlantParams::gm3508(),
|
||||
&scenario,
|
||||
LoopLimits::gm3508(),
|
||||
)
|
||||
.unwrap();
|
||||
let path = std::env::temp_dir().join("chassis-tune-plot.svg");
|
||||
write_step_svg(&path, &trace, &scenario).unwrap();
|
||||
let text = std::fs::read_to_string(&path).unwrap();
|
||||
assert!(text.starts_with("<?xml") || text.contains("<svg"), "not an svg");
|
||||
assert!(text.len() > 1000, "svg looks empty");
|
||||
let _ = std::fs::remove_file(&path);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,713 @@
|
||||
//! 报告与产物写出：CSV / TOML / JSON / Markdown（可选 SVG）。
|
||||
//!
|
||||
//! 产物布局（`--out-dir`）：
|
||||
//!
|
||||
//! | 文件 | 内容 |
|
||||
//! | --- | --- |
|
||||
//! | `trials.csv` | 全部候选：增益 + 指标 + 成本 + 判定（可用任意表格软件复盘） |
|
||||
//! | `best.toml` | 可直接粘贴进 `chassis.toml` 的 `[[motor]]` 片段 |
|
||||
//! | `best-trace.csv` | 最优增益的完整步响应轨迹 |
|
||||
//! | `report.md` | 人类可读总结：设置、Top-N、结论与注意事项 |
|
||||
//! | `summary.json` | 机器可读结果（CI / 后续工具消费） |
|
||||
//! | `best.svg` | 步响应三联图（仅 `--features plot`） |
|
||||
|
||||
use std::fmt::Write as _;
|
||||
use std::path::{Path, PathBuf};
|
||||
|
||||
use dji_esc_sim::{LoopLimits, Scenario, Trace};
|
||||
use serde::Serialize;
|
||||
|
||||
use crate::gains::GainSpace;
|
||||
use crate::metrics::StepMetrics;
|
||||
use crate::search::{Ranked, SearchConfig, Trial, verdict_hint};
|
||||
|
||||
/// 报告写出错误。
|
||||
#[derive(Debug, thiserror::Error)]
|
||||
pub enum ReportError {
|
||||
/// 目录创建或文件写入失败。
|
||||
#[error("io error at {path}: {source}")]
|
||||
Io {
|
||||
/// 出错路径。
|
||||
path: PathBuf,
|
||||
/// 底层错误。
|
||||
source: std::io::Error,
|
||||
},
|
||||
/// CSV 写出失败。
|
||||
#[error("csv error: {0}")]
|
||||
Csv(#[from] csv::Error),
|
||||
/// JSON 写出失败。
|
||||
#[error("json error: {0}")]
|
||||
Json(#[from] serde_json::Error),
|
||||
/// TOML 写出失败。
|
||||
#[error("toml error: {0}")]
|
||||
Toml(#[from] toml::ser::Error),
|
||||
/// 没有可用的最优结果。
|
||||
#[error("no feasible trial to report")]
|
||||
NoBest,
|
||||
/// SVG 绘图失败（仅 `--features plot`）。
|
||||
#[error("plot error: {0}")]
|
||||
Plot(String),
|
||||
}
|
||||
|
||||
fn io(path: impl Into<PathBuf>, source: std::io::Error) -> ReportError {
|
||||
ReportError::Io { path: path.into(), source }
|
||||
}
|
||||
|
||||
/// 生成产物所需的上下文。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct ReportContext {
|
||||
/// 输出目录。
|
||||
pub out_dir: PathBuf,
|
||||
/// 本次搜索使用的参数空间。
|
||||
pub space: GainSpace,
|
||||
/// 搜索配置。
|
||||
pub config: SearchConfig,
|
||||
/// 场景。
|
||||
pub scenario: Scenario,
|
||||
/// 限幅。
|
||||
pub limits: LoopLimits,
|
||||
/// 后端描述（`sim(...)` / `bench(...)`）。
|
||||
pub backend: String,
|
||||
/// 目标轮名（写回 TOML 用）。
|
||||
pub motor_name: String,
|
||||
/// 验收阈值：最大超调（%）。
|
||||
pub max_overshoot_pct: f32,
|
||||
/// 验收阈值：最大稳态误差（%）。
|
||||
pub max_ss_error_pct: f32,
|
||||
/// 最优增益的轨迹（有则写 `best-trace.csv`）。
|
||||
pub best_trace: Option<Trace>,
|
||||
}
|
||||
|
||||
/// 写出全部产物，返回写出的文件列表。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 目录创建、文件写入或序列化失败时返回 [`ReportError`]。
|
||||
pub fn write_reports(ctx: &ReportContext, ranked: &Ranked) -> Result<Vec<PathBuf>, ReportError> {
|
||||
std::fs::create_dir_all(&ctx.out_dir).map_err(|e| io(&ctx.out_dir, e))?;
|
||||
let mut written = Vec::new();
|
||||
|
||||
let trials_path = ctx.out_dir.join("trials.csv");
|
||||
write_trials_csv(&trials_path, &ranked.trials)?;
|
||||
written.push(trials_path);
|
||||
|
||||
let best_path = ctx.out_dir.join("best.toml");
|
||||
write_best_toml(&best_path, ctx, &ranked.best)?;
|
||||
written.push(best_path);
|
||||
|
||||
if let Some(trace) = &ctx.best_trace {
|
||||
let trace_path = ctx.out_dir.join("best-trace.csv");
|
||||
write_trace_csv(&trace_path, trace)?;
|
||||
written.push(trace_path);
|
||||
}
|
||||
|
||||
let report_path = ctx.out_dir.join("report.md");
|
||||
write_markdown(&report_path, ctx, ranked)?;
|
||||
written.push(report_path);
|
||||
|
||||
let summary_path = ctx.out_dir.join("summary.json");
|
||||
write_summary(&summary_path, ctx, ranked)?;
|
||||
written.push(summary_path);
|
||||
|
||||
#[cfg(feature = "plot")]
|
||||
if let Some(trace) = &ctx.best_trace {
|
||||
let svg_path = ctx.out_dir.join("best.svg");
|
||||
crate::plot::write_step_svg(&svg_path, trace, &ctx.scenario)?;
|
||||
written.push(svg_path);
|
||||
}
|
||||
|
||||
Ok(written)
|
||||
}
|
||||
|
||||
/// `trials.csv` 的一行。
|
||||
#[derive(Debug, Serialize)]
|
||||
struct TrialRow {
|
||||
index: u64,
|
||||
phase: &'static str,
|
||||
kp: f32,
|
||||
ki: f32,
|
||||
kd: f32,
|
||||
feedforward: f32,
|
||||
cost: f64,
|
||||
verdict: &'static str,
|
||||
settled: bool,
|
||||
rise_time_s: f32,
|
||||
overshoot_pct: f32,
|
||||
settling_time_s: f32,
|
||||
ss_error_pct: f32,
|
||||
steady_rpm: f32,
|
||||
iae: f32,
|
||||
peak_effort_raw: f32,
|
||||
rms_effort_raw: f32,
|
||||
saturated_pct: f32,
|
||||
max_rpm: f32,
|
||||
}
|
||||
|
||||
impl TrialRow {
|
||||
fn from_trial(trial: &Trial) -> Self {
|
||||
let metrics = trial.score.metrics.unwrap_or_default();
|
||||
Self {
|
||||
index: trial.index,
|
||||
phase: trial.phase.label(),
|
||||
kp: trial.gains.kp,
|
||||
ki: trial.gains.ki,
|
||||
kd: trial.gains.kd,
|
||||
feedforward: trial.gains.feedforward,
|
||||
cost: trial.score.cost,
|
||||
verdict: trial.score.verdict.label(),
|
||||
settled: metrics.settled,
|
||||
rise_time_s: metrics.rise_time_s,
|
||||
overshoot_pct: metrics.overshoot_pct,
|
||||
settling_time_s: metrics.settling_time_s,
|
||||
ss_error_pct: metrics.ss_error_pct,
|
||||
steady_rpm: metrics.steady_rpm,
|
||||
iae: metrics.iae,
|
||||
peak_effort_raw: metrics.peak_effort_raw,
|
||||
rms_effort_raw: metrics.rms_effort_raw,
|
||||
saturated_pct: metrics.saturated_pct,
|
||||
max_rpm: metrics.max_rpm,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn write_trials_csv(path: &Path, trials: &[Trial]) -> Result<(), ReportError> {
|
||||
let mut writer = csv::Writer::from_path(path)?;
|
||||
for trial in trials {
|
||||
writer.serialize(TrialRow::from_trial(trial))?;
|
||||
}
|
||||
writer.flush().map_err(|e| io(path, e))?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 轨迹 CSV 的一行。
|
||||
#[derive(Debug, Serialize)]
|
||||
struct TraceRow {
|
||||
t_s: f32,
|
||||
setpoint_rpm: f32,
|
||||
rpm: f32,
|
||||
effort_raw: i16,
|
||||
saturated: bool,
|
||||
}
|
||||
|
||||
fn write_trace_csv(path: &Path, trace: &Trace) -> Result<(), ReportError> {
|
||||
let mut writer = csv::Writer::from_path(path)?;
|
||||
for index in 0..trace.len() {
|
||||
writer.serialize(TraceRow {
|
||||
t_s: trace.t_s[index],
|
||||
setpoint_rpm: trace.setpoint_rpm[index],
|
||||
rpm: trace.rpm[index],
|
||||
effort_raw: trace.effort_raw[index],
|
||||
saturated: trace.saturated[index],
|
||||
})?;
|
||||
}
|
||||
writer.flush().map_err(|e| io(path, e))?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 可粘贴进既有 `[[motor]]` 段落的增益补丁。
|
||||
#[derive(Debug, Serialize)]
|
||||
struct GainPatch {
|
||||
kp: f32,
|
||||
ki: f32,
|
||||
kd: f32,
|
||||
feedforward: f32,
|
||||
}
|
||||
|
||||
fn write_best_toml(path: &Path, ctx: &ReportContext, best: &Trial) -> Result<(), ReportError> {
|
||||
let gains_patch = GainPatch {
|
||||
kp: best.gains.kp,
|
||||
ki: best.gains.ki,
|
||||
kd: best.gains.kd,
|
||||
feedforward: best.gains.feedforward,
|
||||
};
|
||||
let mut text = String::new();
|
||||
writeln!(text, "# 由 chassis-tune 生成（seed={}, backend={}）", ctx.config.seed, ctx.backend)
|
||||
.ok();
|
||||
writeln!(
|
||||
text,
|
||||
"# 轮子：{}｜场景：{}｜限流 {} raw",
|
||||
ctx.motor_name,
|
||||
crate::search::scenario_summary(&ctx.scenario),
|
||||
ctx.limits.current_limit_raw
|
||||
)
|
||||
.ok();
|
||||
writeln!(text, "# 参数空间：{}", ctx.space.describe()).ok();
|
||||
text.push_str("# 把下面四行粘进 chassis.toml 里对应的 [[motor]] 段落即可。\n");
|
||||
text.push_str(&toml::to_string(&gains_patch)?);
|
||||
std::fs::write(path, text).map_err(|e| io(path, e))?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// `summary.json` 的结构。
|
||||
#[derive(Debug, Serialize)]
|
||||
struct Summary<'a> {
|
||||
backend: &'a str,
|
||||
seed: u64,
|
||||
samples: usize,
|
||||
top_k: usize,
|
||||
solver: &'static str,
|
||||
evals: u64,
|
||||
ok_trials: usize,
|
||||
rejected_trials: usize,
|
||||
scenario: String,
|
||||
gain_space: String,
|
||||
limits: LimitSummary,
|
||||
thresholds: Thresholds,
|
||||
best: BestSummary,
|
||||
top: Vec<BestSummary>,
|
||||
notes: Vec<String>,
|
||||
}
|
||||
|
||||
#[derive(Debug, Serialize)]
|
||||
struct LimitSummary {
|
||||
current_limit_raw: i16,
|
||||
}
|
||||
|
||||
#[derive(Debug, Serialize)]
|
||||
struct Thresholds {
|
||||
max_overshoot_pct: f32,
|
||||
max_ss_error_pct: f32,
|
||||
}
|
||||
|
||||
#[derive(Debug, Serialize)]
|
||||
struct BestSummary {
|
||||
index: u64,
|
||||
phase: &'static str,
|
||||
kp: f32,
|
||||
ki: f32,
|
||||
kd: f32,
|
||||
feedforward: f32,
|
||||
cost: f64,
|
||||
verdict: &'static str,
|
||||
metrics: Option<MetricsSummary>,
|
||||
}
|
||||
|
||||
#[derive(Debug, Serialize)]
|
||||
struct MetricsSummary {
|
||||
rise_time_s: f32,
|
||||
overshoot_pct: f32,
|
||||
settling_time_s: f32,
|
||||
ss_error_pct: f32,
|
||||
steady_rpm: f32,
|
||||
iae: f32,
|
||||
rms_effort_raw: f32,
|
||||
saturated_pct: f32,
|
||||
max_rpm: f32,
|
||||
}
|
||||
|
||||
impl MetricsSummary {
|
||||
fn from_metrics(metrics: &StepMetrics) -> Self {
|
||||
Self {
|
||||
rise_time_s: metrics.rise_time_s,
|
||||
overshoot_pct: metrics.overshoot_pct,
|
||||
settling_time_s: metrics.settling_time_s,
|
||||
ss_error_pct: metrics.ss_error_pct,
|
||||
steady_rpm: metrics.steady_rpm,
|
||||
iae: metrics.iae,
|
||||
rms_effort_raw: metrics.rms_effort_raw,
|
||||
saturated_pct: metrics.saturated_pct,
|
||||
max_rpm: metrics.max_rpm,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn best_summary(trial: &Trial) -> BestSummary {
|
||||
BestSummary {
|
||||
index: trial.index,
|
||||
phase: trial.phase.label(),
|
||||
kp: trial.gains.kp,
|
||||
ki: trial.gains.ki,
|
||||
kd: trial.gains.kd,
|
||||
feedforward: trial.gains.feedforward,
|
||||
cost: trial.score.cost,
|
||||
verdict: trial.score.verdict.label(),
|
||||
metrics: trial.score.metrics.as_ref().map(MetricsSummary::from_metrics),
|
||||
}
|
||||
}
|
||||
|
||||
fn write_summary(path: &Path, ctx: &ReportContext, ranked: &Ranked) -> Result<(), ReportError> {
|
||||
let mut notes = Vec::new();
|
||||
if !ranked.best.score.verdict.is_ok() {
|
||||
notes.push(format!(
|
||||
"没有任何候选达标：最优判定为 {}（{}）",
|
||||
ranked.best.score.verdict.label(),
|
||||
verdict_hint(ranked.best.score.verdict)
|
||||
));
|
||||
}
|
||||
if let Some(metrics) = ranked.best.score.metrics {
|
||||
if metrics.overshoot_pct > ctx.max_overshoot_pct {
|
||||
notes.push(format!(
|
||||
"最优解超调 {:.1}% 超过阈值 {:.1}%",
|
||||
metrics.overshoot_pct, ctx.max_overshoot_pct
|
||||
));
|
||||
}
|
||||
if metrics.ss_error_pct > ctx.max_ss_error_pct {
|
||||
notes.push(format!(
|
||||
"最优解稳态误差 {:.1}% 超过阈值 {:.1}%",
|
||||
metrics.ss_error_pct, ctx.max_ss_error_pct
|
||||
));
|
||||
}
|
||||
if metrics.saturated_pct > 5.0 {
|
||||
notes.push(format!("最优解饱和占比 {:.1}%，限流可能偏紧", metrics.saturated_pct));
|
||||
}
|
||||
}
|
||||
// 贴边界的警告排在"通用提醒"之前：它是本次搜索最具体的一条结论。
|
||||
notes.extend(ctx.space.boundary_hits(&ranked.best.gains));
|
||||
notes.push(
|
||||
"仿真整定依赖被控对象模型；上真机前先用 `validate` 在台架上复验（--backend bench）。"
|
||||
.into(),
|
||||
);
|
||||
|
||||
let summary = Summary {
|
||||
backend: &ctx.backend,
|
||||
seed: ctx.config.seed,
|
||||
samples: ctx.config.samples,
|
||||
top_k: ctx.config.top_k,
|
||||
solver: ctx.config.solver.name(),
|
||||
evals: ranked.evals,
|
||||
ok_trials: ranked.ok_count(),
|
||||
rejected_trials: ranked.rejected,
|
||||
scenario: crate::search::scenario_summary(&ctx.scenario),
|
||||
gain_space: ctx.space.describe(),
|
||||
limits: LimitSummary { current_limit_raw: ctx.limits.current_limit_raw },
|
||||
thresholds: Thresholds {
|
||||
max_overshoot_pct: ctx.max_overshoot_pct,
|
||||
max_ss_error_pct: ctx.max_ss_error_pct,
|
||||
},
|
||||
best: best_summary(&ranked.best),
|
||||
top: ranked.top(10).iter().map(best_summary).collect(),
|
||||
notes,
|
||||
};
|
||||
let text = serde_json::to_string_pretty(&summary)?;
|
||||
std::fs::write(path, text).map_err(|e| io(path, e))?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn write_markdown(path: &Path, ctx: &ReportContext, ranked: &Ranked) -> Result<(), ReportError> {
|
||||
let mut text = String::new();
|
||||
let best = &ranked.best;
|
||||
|
||||
writeln!(text, "# 速度环自动整定报告\n").ok();
|
||||
write_markdown_conclusion(&mut text, ctx, best);
|
||||
write_markdown_settings(&mut text, ctx, ranked);
|
||||
write_markdown_top(&mut text, ranked);
|
||||
write_markdown_artifacts(&mut text);
|
||||
write_markdown_notes(&mut text, ctx, best);
|
||||
|
||||
std::fs::write(path, text).map_err(|e| io(path, e))?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 结论段：最优增益、指标、验收、无解时的原因。
|
||||
fn write_markdown_conclusion(text: &mut String, ctx: &ReportContext, best: &Trial) {
|
||||
writeln!(text, "## 结论\n").ok();
|
||||
writeln!(
|
||||
text,
|
||||
"**kp = {:.3} ki = {:.3} kd = {:.3} feedforward = {:.3}**（成本 {:.4}，判定 {}）\n",
|
||||
best.gains.kp,
|
||||
best.gains.ki,
|
||||
best.gains.kd,
|
||||
best.gains.feedforward,
|
||||
best.cost(),
|
||||
best.score.verdict.label()
|
||||
)
|
||||
.ok();
|
||||
if let Some(metrics) = best.score.metrics {
|
||||
writeln!(
|
||||
text,
|
||||
"- 上升时间 {:.0} ms｜超调 {:.1}%｜调节时间 {:.0} ms｜稳态误差 {:.2}%",
|
||||
metrics.rise_time_s * 1000.0,
|
||||
metrics.overshoot_pct,
|
||||
metrics.settling_time_s * 1000.0,
|
||||
metrics.ss_error_pct
|
||||
)
|
||||
.ok();
|
||||
writeln!(
|
||||
text,
|
||||
"- 峰值电流 {:.0} raw｜RMS 电流 {:.0} raw｜饱和 {:.1}%｜峰值转速 {:.0} rpm",
|
||||
metrics.peak_effort_raw, metrics.rms_effort_raw, metrics.saturated_pct, metrics.max_rpm
|
||||
)
|
||||
.ok();
|
||||
writeln!(
|
||||
text,
|
||||
"- 验收：{}（阈值 超调 ≤ {:.1}%，稳态误差 ≤ {:.1}%）\n",
|
||||
if ctx.best_passes(&metrics) { "✓ 通过" } else { "✗ 未通过" },
|
||||
ctx.max_overshoot_pct,
|
||||
ctx.max_ss_error_pct
|
||||
)
|
||||
.ok();
|
||||
}
|
||||
if !best.score.verdict.is_ok() {
|
||||
writeln!(
|
||||
text,
|
||||
"> ⚠ 没有任何候选达标（最优：{}）。{}\n",
|
||||
best.score.verdict.label(),
|
||||
verdict_hint(best.score.verdict)
|
||||
)
|
||||
.ok();
|
||||
}
|
||||
}
|
||||
|
||||
/// 设置段：复现本次搜索所需的全部输入。
|
||||
fn write_markdown_settings(text: &mut String, ctx: &ReportContext, ranked: &Ranked) {
|
||||
writeln!(text, "## 设置\n").ok();
|
||||
writeln!(text, "| 项 | 值 |").ok();
|
||||
writeln!(text, "| --- | --- |").ok();
|
||||
writeln!(text, "| 后端 | `{}` |", ctx.backend).ok();
|
||||
writeln!(text, "| 场景 | `{}` |", crate::search::scenario_summary(&ctx.scenario)).ok();
|
||||
writeln!(text, "| 参数空间 | `{}` |", ctx.space.describe()).ok();
|
||||
writeln!(text, "| 限流 | {} raw |", ctx.limits.current_limit_raw).ok();
|
||||
writeln!(text, "| 种子 | {} |", ctx.config.seed).ok();
|
||||
writeln!(
|
||||
text,
|
||||
"| 搜索 | samples={}, top_k={}, solver={}, max_iters={} |",
|
||||
ctx.config.samples,
|
||||
ctx.config.top_k,
|
||||
ctx.config.solver.name(),
|
||||
ctx.config.max_iters
|
||||
)
|
||||
.ok();
|
||||
writeln!(
|
||||
text,
|
||||
"| 评估 | {} 次（达标 {} / 共 {}） |",
|
||||
ranked.evals,
|
||||
ranked.ok_count(),
|
||||
ranked.trials.len()
|
||||
)
|
||||
.ok();
|
||||
writeln!(text, "| 目标轮 | `{}` |\n", ctx.motor_name).ok();
|
||||
}
|
||||
|
||||
/// Top-10 表格。
|
||||
fn write_markdown_top(text: &mut String, ranked: &Ranked) {
|
||||
writeln!(text, "## Top 10\n").ok();
|
||||
writeln!(
|
||||
text,
|
||||
"| # | 阶段 | kp | ki | kd | ff | 成本 | 判定 | 超调 % | 稳态误差 % | 调节 ms |"
|
||||
)
|
||||
.ok();
|
||||
writeln!(text, "| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- |").ok();
|
||||
for (rank, trial) in ranked.top(10).iter().enumerate() {
|
||||
let metrics = trial.score.metrics.unwrap_or_default();
|
||||
writeln!(
|
||||
text,
|
||||
"| {} | {} | {:.3} | {:.3} | {:.3} | {:.3} | {:.4} | {} | {:.1} | {:.2} | {:.0} |",
|
||||
rank + 1,
|
||||
trial.phase.label(),
|
||||
trial.gains.kp,
|
||||
trial.gains.ki,
|
||||
trial.gains.kd,
|
||||
trial.gains.feedforward,
|
||||
trial.cost(),
|
||||
trial.score.verdict.label(),
|
||||
metrics.overshoot_pct,
|
||||
metrics.ss_error_pct,
|
||||
metrics.settling_time_s * 1000.0
|
||||
)
|
||||
.ok();
|
||||
}
|
||||
writeln!(text).ok();
|
||||
}
|
||||
|
||||
/// 产物清单。
|
||||
fn write_markdown_artifacts(text: &mut String) {
|
||||
writeln!(text, "## 产物\n").ok();
|
||||
writeln!(text, "- `trials.csv`：全部候选（含未达标者与判定原因）").ok();
|
||||
writeln!(text, "- `best.toml`：可粘贴的 `[[motor]]` 增益片段").ok();
|
||||
writeln!(text, "- `best-trace.csv`：最优增益的完整轨迹").ok();
|
||||
writeln!(text, "- `hil-validation.csv`：真机重复性复验（仅 `--backend bench`）").ok();
|
||||
writeln!(text, "- `summary.json`：机器可读结果\n").ok();
|
||||
}
|
||||
|
||||
/// 注意事项：把"仿真整定不能替代实车复验"写进报告，而不是只写在 README 里。
|
||||
fn write_markdown_notes(text: &mut String, ctx: &ReportContext, best: &Trial) {
|
||||
let hits = ctx.space.boundary_hits(&best.gains);
|
||||
writeln!(text, "## 注意事项\n").ok();
|
||||
writeln!(text, "1. 仿真整定只保证在被控对象模型内最优；模型参数（摩擦 / 延迟）是量级估计，")
|
||||
.ok();
|
||||
writeln!(
|
||||
text,
|
||||
" 上真机前请用同一 `--scenario` 跑 `chassis-tune sweep --backend bench` 复验。"
|
||||
)
|
||||
.ok();
|
||||
writeln!(
|
||||
text,
|
||||
"2. 台架搜索会真的驱动轮子：必须架空或整车固定，且加 `--i-know-the-wheel-spins`。"
|
||||
)
|
||||
.ok();
|
||||
writeln!(
|
||||
text,
|
||||
"3. `current_limit` 属于安全约束，不参与搜索；它会钳住 PID 输出，也决定饱和判定。"
|
||||
)
|
||||
.ok();
|
||||
writeln!(text, "4. 复现本次结果：同一 `--seed` + 同一参数空间 → 逐位一致。").ok();
|
||||
for hit in &hits {
|
||||
writeln!(text, "- ⚠ 搜索落在参数空间边界：{hit}").ok();
|
||||
}
|
||||
writeln!(
|
||||
text,
|
||||
"5. 本模型不含测量噪声：仿真里更高的增益不会被噪声惩罚，因此最优解可能偏激进——\n 台架复验（`--backend bench`）是把它拉回现实的唯一手段。"
|
||||
)
|
||||
.ok();
|
||||
}
|
||||
|
||||
impl ReportContext {
|
||||
/// 一组指标是否满足验收阈值。
|
||||
#[must_use]
|
||||
pub fn best_passes(&self, metrics: &StepMetrics) -> bool {
|
||||
metrics.settled
|
||||
&& metrics.overshoot_pct <= self.max_overshoot_pct
|
||||
&& metrics.ss_error_pct <= self.max_ss_error_pct
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::backend::{Evaluator, SimEvaluator};
|
||||
use crate::cost::{Objective, Weights};
|
||||
use crate::gains::GainSpace;
|
||||
use crate::search::{SearchConfig, sweep};
|
||||
use dji_esc_sim::{LoopLimits, PlantEnsemble, PlantParams};
|
||||
|
||||
fn ranked(name: &str) -> (GainSpace, ReportContext, Ranked) {
|
||||
let space = GainSpace::default_gm3508();
|
||||
let scenario = Scenario::step(1500.0, 1.0);
|
||||
let limits = LoopLimits::gm3508();
|
||||
let objective = Objective::new(Weights::default(), scenario, limits);
|
||||
let ensemble = PlantEnsemble::homogeneous(PlantParams::gm3508(), 1);
|
||||
let evaluator = SimEvaluator::new(ensemble, scenario, limits, objective, 0.0);
|
||||
let config = SearchConfig {
|
||||
seed: 3,
|
||||
samples: 48,
|
||||
top_k: 2,
|
||||
max_iters: 40,
|
||||
max_evals: 900,
|
||||
..SearchConfig::default()
|
||||
};
|
||||
let ranked = sweep(&space, &config, &evaluator, None).unwrap();
|
||||
let trace =
|
||||
dji_esc_sim::simulate(ranked.best.gains, &PlantParams::gm3508(), &scenario, limits)
|
||||
.unwrap();
|
||||
let ctx = ReportContext {
|
||||
out_dir: std::env::temp_dir().join(format!("chassis-tune-report-{name}")),
|
||||
space,
|
||||
config,
|
||||
scenario,
|
||||
limits,
|
||||
backend: evaluator.describe(),
|
||||
motor_name: "front_left".into(),
|
||||
max_overshoot_pct: 20.0,
|
||||
max_ss_error_pct: 5.0,
|
||||
best_trace: Some(trace),
|
||||
};
|
||||
(space, ctx, ranked)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn writes_every_artifact() {
|
||||
let (_, ctx, ranked) = ranked("writes_every_artifact");
|
||||
let _ = std::fs::remove_dir_all(&ctx.out_dir);
|
||||
let written = write_reports(&ctx, &ranked).unwrap();
|
||||
let names: Vec<String> = written
|
||||
.iter()
|
||||
.map(|p| p.file_name().unwrap_or_default().to_string_lossy().into_owned())
|
||||
.collect();
|
||||
for expected in ["trials.csv", "best.toml", "best-trace.csv", "report.md", "summary.json"] {
|
||||
assert!(names.iter().any(|n| n == expected), "missing {expected}: {names:?}");
|
||||
}
|
||||
for path in &written {
|
||||
assert!(path.exists(), "{} must exist", path.display());
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn trials_csv_has_a_stable_header_and_one_row_per_trial() {
|
||||
let (_, ctx, ranked) = ranked("trials_csv_has_a_stable_header_and_one_row_per_trial");
|
||||
write_reports(&ctx, &ranked).unwrap();
|
||||
let text = std::fs::read_to_string(ctx.out_dir.join("trials.csv")).unwrap();
|
||||
let mut lines = text.lines();
|
||||
let header = lines.next().unwrap();
|
||||
for column in ["index", "phase", "kp", "ki", "kd", "feedforward", "cost", "verdict"] {
|
||||
assert!(header.contains(column), "header missing {column}: {header}");
|
||||
}
|
||||
assert_eq!(lines.count(), ranked.trials.len());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn best_toml_parses_back_through_the_config_schema() {
|
||||
let (_, ctx, ranked) = ranked("best_toml_parses_back_through_the_config_schema");
|
||||
write_reports(&ctx, &ranked).unwrap();
|
||||
let text = std::fs::read_to_string(ctx.out_dir.join("best.toml")).unwrap();
|
||||
// 片段本体是合法 TOML：与其它段落拼起来即可被 chassis-config 读回。
|
||||
let patch = text
|
||||
.lines()
|
||||
.filter(|line| !line.trim_start().starts_with('#'))
|
||||
.collect::<Vec<_>>()
|
||||
.join("\n");
|
||||
let full = format!(
|
||||
"[can]\ninterface = \"can0\"\n[mecanum]\nhalf_length = 0.2\nhalf_width = 0.25\nwheel_radius = 0.0762\n\n[[motor]]\nname = \"front_left\"\nesc_id = 1\ndirection = 1\ngear_ratio = 19.203\n{patch}"
|
||||
);
|
||||
let mut motors = Vec::new();
|
||||
for index in 1..4 {
|
||||
motors.push(format!(
|
||||
"[[motor]]\nname = \"w{index}\"\nesc_id = {}\ndirection = 1\ngear_ratio = 19.203\n",
|
||||
index + 1
|
||||
));
|
||||
}
|
||||
let full = format!("{full}\n{}", motors.join("\n"));
|
||||
let config = chassis_config::Config::from_toml(&full).expect("best.toml must be readable");
|
||||
let gains = config.motors[0].velocity_gains();
|
||||
assert_eq!(gains, ranked.best.gains);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn summary_json_is_machine_readable() {
|
||||
let (_, ctx, ranked) = ranked("summary_json_is_machine_readable");
|
||||
write_reports(&ctx, &ranked).unwrap();
|
||||
let text = std::fs::read_to_string(ctx.out_dir.join("summary.json")).unwrap();
|
||||
let value: serde_json::Value = serde_json::from_str(&text).unwrap();
|
||||
assert_eq!(value["seed"], serde_json::json!(3));
|
||||
assert_eq!(value["samples"], serde_json::json!(48));
|
||||
assert!(value["best"]["kp"].as_f64().unwrap() > 0.0);
|
||||
assert!(value["top"].as_array().unwrap().len() <= 10);
|
||||
assert!(!value["notes"].as_array().unwrap().is_empty());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn markdown_mentions_the_gains_and_thresholds() {
|
||||
let (_, ctx, ranked) = ranked("markdown_mentions_the_gains_and_thresholds");
|
||||
write_reports(&ctx, &ranked).unwrap();
|
||||
let text = std::fs::read_to_string(ctx.out_dir.join("report.md")).unwrap();
|
||||
assert!(text.contains("kp ="), "{text}");
|
||||
assert!(text.contains("Top 10"), "{text}");
|
||||
assert!(text.contains("front_left"), "{text}");
|
||||
assert!(text.contains("注意事项"), "{text}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn best_trace_csv_matches_the_trace_length() {
|
||||
let (_, ctx, ranked) = ranked("best_trace_csv_matches_the_trace_length");
|
||||
write_reports(&ctx, &ranked).unwrap();
|
||||
let text = std::fs::read_to_string(ctx.out_dir.join("best-trace.csv")).unwrap();
|
||||
let rows = text.lines().count();
|
||||
let expected = ctx.best_trace.as_ref().unwrap().len() + 1; // + 表头
|
||||
assert_eq!(rows, expected);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn best_passes_respects_the_thresholds() {
|
||||
let (_, ctx, _) = ranked("t");
|
||||
let passing = StepMetrics {
|
||||
settled: true,
|
||||
overshoot_pct: 10.0,
|
||||
ss_error_pct: 1.0,
|
||||
..StepMetrics::default()
|
||||
};
|
||||
assert!(ctx.best_passes(&passing));
|
||||
let overshooting = StepMetrics { overshoot_pct: 50.0, ..passing };
|
||||
assert!(!ctx.best_passes(&overshooting));
|
||||
let unsettled = StepMetrics { settled: false, ..passing };
|
||||
assert!(!ctx.best_passes(&unsettled));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,628 @@
|
||||
//! 两阶段参数空间搜索：种子化分层粗扫 → `argmin` 多起点局部精修。
|
||||
//!
|
||||
//! # 为什么是两阶段
|
||||
//!
|
||||
//! 成本函数有一大片"罚值平原"（不收敛 / 饱和的候选成本都等于 `PENALTY`），
|
||||
//! 纯局部搜索会陷在平原上；而纯网格搜索在四个数量级跨度上需要天文数字的
|
||||
//! 评估次数。两阶段各取所长：
|
||||
//!
|
||||
//! 1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数把
|
||||
//! 空间铺满，拿到若干个互不相邻的可达解（top-k）；
|
||||
//! 2. **精修**：以每个可达解为起点做多起点 `argmin` 局部搜索（Nelder-Mead
|
||||
//! 或 PSO），在**坐标空间**（对数刻度）里收敛到局部最优。
|
||||
//!
|
||||
//! # 可复现性
|
||||
//!
|
||||
//! 每个候选的随机性只来自 `(种子, 候选下标)` 派生的独立 RNG，结果与线程
|
||||
//! 调度无关；粗扫结果按下标排序后再取 top-k，因此同 seed 两次运行逐位一致。
|
||||
|
||||
use std::sync::atomic::{AtomicU64, Ordering};
|
||||
|
||||
use argmin::core::{CostFunction, Error as ArgminError, Executor};
|
||||
use argmin::solver::neldermead::NelderMead;
|
||||
use argmin::solver::particleswarm::ParticleSwarm;
|
||||
use dji_esc_sim::Scenario;
|
||||
use indicatif::ProgressBar;
|
||||
use motor_core::VelocityGains;
|
||||
use rand::SeedableRng as _;
|
||||
use rand::rngs::StdRng;
|
||||
use rayon::prelude::*;
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::backend::Evaluator;
|
||||
use crate::cost::{Score, Verdict};
|
||||
use crate::gains::{AXIS_COUNT, GainSpace};
|
||||
|
||||
/// 搜索阶段的错误。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
||||
pub enum SearchError {
|
||||
/// 参数空间非法。
|
||||
#[error("invalid gain space: {0}")]
|
||||
InvalidSpace(String),
|
||||
/// 粗扫样本数为 0。
|
||||
#[error("samples must be >= 1")]
|
||||
NoSamples,
|
||||
}
|
||||
|
||||
/// 候选来自哪个阶段。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Phase {
|
||||
/// 分层粗扫。
|
||||
Coarse,
|
||||
/// 局部精修。
|
||||
Refine,
|
||||
/// 用户指定 / 基线点。
|
||||
Seed,
|
||||
}
|
||||
|
||||
impl Phase {
|
||||
/// CSV / 报告标签。
|
||||
#[must_use]
|
||||
pub fn label(self) -> &'static str {
|
||||
match self {
|
||||
Self::Coarse => "coarse",
|
||||
Self::Refine => "refine",
|
||||
Self::Seed => "seed",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次被评估的候选。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Trial {
|
||||
/// 评估序号（按评估顺序递增，报告可复现）。
|
||||
pub index: u64,
|
||||
/// 所属阶段。
|
||||
pub phase: Phase,
|
||||
/// 增益。
|
||||
pub gains: VelocityGains,
|
||||
/// 评估结果。
|
||||
pub score: Score,
|
||||
}
|
||||
|
||||
impl Trial {
|
||||
/// 成本。
|
||||
#[must_use]
|
||||
pub fn cost(&self) -> f64 {
|
||||
self.score.cost
|
||||
}
|
||||
}
|
||||
|
||||
/// 局部精修求解器。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||||
pub enum SolverKind {
|
||||
/// Nelder-Mead（无导数、对罚值平原鲁棒），默认。
|
||||
#[default]
|
||||
NelderMead,
|
||||
/// 粒子群（更全局，评估次数更高）。
|
||||
Pso,
|
||||
}
|
||||
|
||||
impl SolverKind {
|
||||
/// 文本名。
|
||||
#[must_use]
|
||||
pub fn name(self) -> &'static str {
|
||||
match self {
|
||||
Self::NelderMead => "nelder-mead",
|
||||
Self::Pso => "pso",
|
||||
}
|
||||
}
|
||||
|
||||
/// 从文本解析。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 未知名字时返回错误。
|
||||
pub fn parse(text: &str) -> Result<Self, SearchError> {
|
||||
match text {
|
||||
"nelder-mead" | "neldermead" | "nm" => Ok(Self::NelderMead),
|
||||
"pso" | "particle-swarm" => Ok(Self::Pso),
|
||||
other => Err(SearchError::InvalidSpace(format!(
|
||||
"unknown solver '{other}' (expected nelder-mead|pso)"
|
||||
))),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 搜索配置。
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub struct SearchConfig {
|
||||
/// 种子（决定分层采样与多起点顺序）。
|
||||
pub seed: u64,
|
||||
/// 粗扫样本数。
|
||||
pub samples: usize,
|
||||
/// 进入精修的种子数量。
|
||||
pub top_k: usize,
|
||||
/// 精修求解器。
|
||||
pub solver: SolverKind,
|
||||
/// 每个起点的最大迭代数。
|
||||
pub max_iters: usize,
|
||||
/// PSO 粒子数（`SolverKind::Pso` 时生效）。
|
||||
pub particles: usize,
|
||||
/// 评估预算上限（含粗扫）。
|
||||
pub max_evals: u64,
|
||||
}
|
||||
|
||||
impl Default for SearchConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
seed: 20_260_101,
|
||||
samples: 512,
|
||||
top_k: 8,
|
||||
solver: SolverKind::NelderMead,
|
||||
max_iters: 200,
|
||||
particles: 24,
|
||||
max_evals: 4_000,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl SearchConfig {
|
||||
/// 校验。
|
||||
///
|
||||
/// 预算小于样本数**不是**错误：[`sweep`] 会把预算下限抬到 `samples`
|
||||
/// （粗扫必须能把铺满空间的样本跑完），因此只有"没有样本"才是硬错误。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 样本数为 0 时返回 [`SearchError::NoSamples`]。
|
||||
pub fn validate(&self) -> Result<(), SearchError> {
|
||||
if self.samples == 0 {
|
||||
return Err(SearchError::NoSamples);
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
/// 搜索结果（按成本升序）。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct Ranked {
|
||||
/// 全部候选（升序）。
|
||||
pub trials: Vec<Trial>,
|
||||
/// 最优候选。
|
||||
pub best: Trial,
|
||||
/// 实际评估次数。
|
||||
pub evals: u64,
|
||||
/// 被硬约束拦下的候选数。
|
||||
pub rejected: usize,
|
||||
}
|
||||
|
||||
impl Ranked {
|
||||
/// 前 `n` 名。
|
||||
#[must_use]
|
||||
pub fn top(&self, n: usize) -> &[Trial] {
|
||||
let n = n.min(self.trials.len());
|
||||
&self.trials[..n]
|
||||
}
|
||||
|
||||
/// 达到 `Verdict::Ok` 的候选数。
|
||||
#[must_use]
|
||||
pub fn ok_count(&self) -> usize {
|
||||
self.trials.iter().filter(|t| t.score.verdict.is_ok()).count()
|
||||
}
|
||||
}
|
||||
|
||||
/// 共享的评估预算与计数器。
|
||||
struct Budget {
|
||||
used: AtomicU64,
|
||||
limit: u64,
|
||||
}
|
||||
|
||||
impl Budget {
|
||||
fn new(limit: u64) -> Self {
|
||||
Self { used: AtomicU64::new(0), limit }
|
||||
}
|
||||
|
||||
/// 取一个评估配额；返回 `None` 表示预算耗尽。
|
||||
///
|
||||
/// 用 CAS 而不是 `fetch_add`：预算耗尽后优化器仍会持续询问，若仍自增，
|
||||
/// `spent()` 就会把"被拒绝的询问"也算成评估次数，报告里的数字便失去意义。
|
||||
fn take(&self) -> Option<u64> {
|
||||
let mut current = self.used.load(Ordering::Relaxed);
|
||||
loop {
|
||||
if current >= self.limit {
|
||||
return None;
|
||||
}
|
||||
match self.used.compare_exchange_weak(
|
||||
current,
|
||||
current + 1,
|
||||
Ordering::Relaxed,
|
||||
Ordering::Relaxed,
|
||||
) {
|
||||
Ok(_) => return Some(current),
|
||||
Err(observed) => current = observed,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn spent(&self) -> u64 {
|
||||
self.used.load(Ordering::Relaxed)
|
||||
}
|
||||
}
|
||||
|
||||
/// 按预算评估一组增益（预算耗尽时直接给罚值，不再花时间算）。
|
||||
fn evaluate_with_budget(
|
||||
budget: &Budget,
|
||||
evaluator: &dyn Evaluator,
|
||||
gains: &VelocityGains,
|
||||
) -> Score {
|
||||
if budget.take().is_some() {
|
||||
evaluator.evaluate(gains)
|
||||
} else {
|
||||
Score::penalized(Verdict::Rejected)
|
||||
}
|
||||
}
|
||||
|
||||
/// 记录本次精修见过的最优解。
|
||||
///
|
||||
/// 不依赖 `argmin` 的结果访问器（跨版本不稳定），而是让成本函数自己记账：
|
||||
/// 谁算出过最低成本，谁就是这次精修的结果。
|
||||
#[derive(Default)]
|
||||
struct BestTracker {
|
||||
best: std::sync::Mutex<Option<(f64, [f64; AXIS_COUNT])>>,
|
||||
}
|
||||
|
||||
impl BestTracker {
|
||||
fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
fn record(&self, cost: f64, coords: [f64; AXIS_COUNT]) {
|
||||
let mut best = self.best.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
|
||||
if best.is_none_or(|(current, _)| cost < current) {
|
||||
*best = Some((cost, coords));
|
||||
}
|
||||
}
|
||||
|
||||
fn take(&self) -> Option<(f64, [f64; AXIS_COUNT])> {
|
||||
*self.best.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
|
||||
}
|
||||
}
|
||||
|
||||
/// 把 [`Evaluator`] 适配成 `argmin` 的成本函数（坐标空间参数）。
|
||||
struct CoordProblem<'a> {
|
||||
evaluator: &'a dyn Evaluator,
|
||||
space: &'a GainSpace,
|
||||
budget: &'a Budget,
|
||||
tracker: &'a BestTracker,
|
||||
}
|
||||
|
||||
impl CostFunction for CoordProblem<'_> {
|
||||
type Param = Vec<f64>;
|
||||
type Output = f64;
|
||||
|
||||
fn cost(&self, param: &Self::Param) -> Result<f64, ArgminError> {
|
||||
let coords: [f64; AXIS_COUNT] = match param.as_slice() {
|
||||
[a, b, c, d] => [*a, *b, *c, *d],
|
||||
_ => return Ok(crate::cost::PENALTY),
|
||||
};
|
||||
let gains = self.space.from_coords(coords);
|
||||
let cost = match self.budget.take() {
|
||||
Some(_) => self.evaluator.evaluate(&gains).cost,
|
||||
None => crate::cost::PENALTY,
|
||||
};
|
||||
if cost.is_finite() {
|
||||
self.tracker.record(cost, coords);
|
||||
}
|
||||
Ok(cost)
|
||||
}
|
||||
}
|
||||
|
||||
/// 以 `start` 为起点做一次局部精修，返回见过的最优增益。
|
||||
fn refine(
|
||||
space: &GainSpace,
|
||||
evaluator: &dyn Evaluator,
|
||||
budget: &Budget,
|
||||
config: &SearchConfig,
|
||||
start: VelocityGains,
|
||||
seed: u64,
|
||||
) -> VelocityGains {
|
||||
let (lower, upper) = space.coord_bounds();
|
||||
let tracker = BestTracker::new();
|
||||
let problem = CoordProblem { evaluator, space, budget, tracker: &tracker };
|
||||
let start_coords = space.to_coords(&start);
|
||||
|
||||
// 两种求解器的 `OptimizationResult` 类型不同，这里只保留"成功 / 失败"。
|
||||
let outcome: Result<(), ArgminError> = match config.solver {
|
||||
SolverKind::NelderMead => {
|
||||
// n 维需要 n+1 个初始顶点：起点 + 每轴一小步（轴跨度的 5%）。
|
||||
let mut simplex: Vec<Vec<f64>> = Vec::with_capacity(AXIS_COUNT + 1);
|
||||
simplex.push(start_coords.to_vec());
|
||||
for axis in 0..AXIS_COUNT {
|
||||
let mut vertex = start_coords;
|
||||
let span = (upper[axis] - lower[axis]).abs();
|
||||
vertex[axis] = (vertex[axis] + 0.05 * span.max(1.0e-3)).min(upper[axis]);
|
||||
simplex.push(vertex.to_vec());
|
||||
}
|
||||
let solver: NelderMead<Vec<f64>, f64> = NelderMead::new(simplex);
|
||||
Executor::new(problem, solver)
|
||||
.configure(|state| state.max_iters(config.max_iters as u64))
|
||||
.run()
|
||||
.map(|_| ())
|
||||
}
|
||||
SolverKind::Pso => {
|
||||
let solver: ParticleSwarm<Vec<f64>, f64, StdRng> =
|
||||
ParticleSwarm::new((lower.to_vec(), upper.to_vec()), config.particles.max(2))
|
||||
.with_rng_generator(StdRng::seed_from_u64(seed));
|
||||
Executor::new(problem, solver)
|
||||
.configure(|state| state.max_iters(config.max_iters as u64))
|
||||
.run()
|
||||
.map(|_| ())
|
||||
}
|
||||
};
|
||||
// 求解器失败（预算耗尽 / 数值问题）不算错：退化到起点，搜索继续。
|
||||
if let Err(error) = outcome {
|
||||
tracing::debug!(%error, "local refinement stopped early");
|
||||
}
|
||||
tracker.take().map_or(start, |(_, coords)| space.from_coords(coords))
|
||||
}
|
||||
|
||||
/// 执行两阶段搜索。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 参数空间或配置非法时返回 [`SearchError`]。
|
||||
pub fn sweep(
|
||||
space: &GainSpace,
|
||||
config: &SearchConfig,
|
||||
evaluator: &dyn Evaluator,
|
||||
progress: Option<&ProgressBar>,
|
||||
) -> Result<Ranked, SearchError> {
|
||||
space.validate().map_err(|e| SearchError::InvalidSpace(e.to_string()))?;
|
||||
config.validate()?;
|
||||
|
||||
let budget = Budget::new(config.max_evals.max(config.samples as u64));
|
||||
|
||||
// ---------------- 阶段 1：分层粗扫（并行） ----------------
|
||||
let mut rng = StdRng::seed_from_u64(config.seed);
|
||||
let candidates = space.stratified(config.samples, &mut rng);
|
||||
let mut trials: Vec<Trial> = candidates
|
||||
.into_par_iter()
|
||||
.enumerate()
|
||||
.map(|(index, gains)| Trial {
|
||||
index: index as u64,
|
||||
phase: Phase::Coarse,
|
||||
gains,
|
||||
score: evaluate_with_budget(&budget, evaluator, &gains),
|
||||
})
|
||||
.collect();
|
||||
if let Some(bar) = progress {
|
||||
bar.inc(trials.len() as u64);
|
||||
}
|
||||
|
||||
// 排序后再取 top-k：与线程调度无关，同 seed 必定选出同一批种子。
|
||||
trials.sort_by(|a, b| a.cost().total_cmp(&b.cost()).then(a.index.cmp(&b.index)));
|
||||
let seeds: Vec<Trial> = trials.iter().take(config.top_k.max(1)).copied().collect();
|
||||
|
||||
// ---------------- 阶段 2：多起点精修（并行） ----------------
|
||||
let coarse_count = trials.len() as u64;
|
||||
let refined: Vec<Trial> = seeds
|
||||
.par_iter()
|
||||
.enumerate()
|
||||
.map(|(slot, seed_trial)| {
|
||||
let seed = config.seed.wrapping_add((slot as u64 + 1).wrapping_mul(0x9E37_79B9));
|
||||
let gains = refine(space, evaluator, &budget, config, seed_trial.gains, seed);
|
||||
let score = evaluate_with_budget(&budget, evaluator, &gains);
|
||||
Trial { index: coarse_count + slot as u64, phase: Phase::Refine, gains, score }
|
||||
})
|
||||
.collect();
|
||||
if let Some(bar) = progress {
|
||||
bar.inc(refined.len() as u64);
|
||||
}
|
||||
trials.extend(refined);
|
||||
|
||||
trials.sort_by(|a, b| a.cost().total_cmp(&b.cost()).then(a.index.cmp(&b.index)));
|
||||
let evals = budget.spent();
|
||||
let rejected = trials.iter().filter(|t| !t.score.verdict.is_ok()).count();
|
||||
let best = trials.first().copied().ok_or(SearchError::NoSamples)?;
|
||||
Ok(Ranked { trials, best, evals, rejected })
|
||||
}
|
||||
|
||||
/// 只评估一组增益（`validate` 子命令）。
|
||||
#[must_use]
|
||||
pub fn evaluate_once(evaluator: &dyn Evaluator, gains: &VelocityGains) -> Trial {
|
||||
Trial { index: 0, phase: Phase::Seed, gains: *gains, score: evaluator.evaluate(gains) }
|
||||
}
|
||||
|
||||
/// 一次评估都未达标时的提示（报告用）。
|
||||
#[must_use]
|
||||
pub fn verdict_hint(verdict: Verdict) -> &'static str {
|
||||
match verdict {
|
||||
Verdict::Ok => "达标",
|
||||
Verdict::Rejected => "参数非法或场景退化",
|
||||
Verdict::Diverged => "数值发散：增益过大",
|
||||
Verdict::Overspeed => "过速：增益过大或前馈过高",
|
||||
Verdict::Saturated => "长时间饱和：限流太紧或积分过大",
|
||||
Verdict::NoSettle => "未收敛：增益过小或积分不足（空载台架考虑 ff=0）",
|
||||
}
|
||||
}
|
||||
|
||||
/// 报告用的场景摘要。
|
||||
#[must_use]
|
||||
pub fn scenario_summary(scenario: &Scenario) -> String {
|
||||
format!(
|
||||
"{:?} target={:.0} rpm ramp={:.0}ms hold={:.0}ms dt={:.1}ms",
|
||||
scenario.profile,
|
||||
scenario.target_rpm,
|
||||
scenario.ramp_s * 1000.0,
|
||||
scenario.hold_s * 1000.0,
|
||||
scenario.dt_s * 1000.0
|
||||
)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::backend::SimEvaluator;
|
||||
use crate::cost::{Objective, Weights};
|
||||
use dji_esc_sim::{LoopLimits, PlantEnsemble, PlantParams, PlantTolerances};
|
||||
|
||||
fn space() -> GainSpace {
|
||||
GainSpace::default_gm3508()
|
||||
}
|
||||
|
||||
fn evaluator(plant: PlantParams, ensemble: usize, robust_weight: f64) -> SimEvaluator {
|
||||
let scenario = Scenario::step(1500.0, 2.0);
|
||||
let limits = LoopLimits::gm3508();
|
||||
let objective = Objective::new(Weights::default(), scenario, limits);
|
||||
let ensemble = PlantEnsemble::randomized(plant, PlantTolerances::default(), ensemble, 7);
|
||||
SimEvaluator::new(ensemble, scenario, limits, objective, robust_weight)
|
||||
}
|
||||
|
||||
fn small_config(seed: u64) -> SearchConfig {
|
||||
SearchConfig {
|
||||
seed,
|
||||
samples: 64,
|
||||
top_k: 3,
|
||||
max_iters: 60,
|
||||
max_evals: 2_000,
|
||||
..SearchConfig::default()
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sweep_finds_a_usable_configuration() {
|
||||
let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0);
|
||||
let ranked = sweep(&space(), &small_config(1), &evaluator, None).unwrap();
|
||||
let best = ranked.best;
|
||||
assert_eq!(best.score.verdict, Verdict::Ok, "{best:?}");
|
||||
let metrics = best.score.metrics.unwrap();
|
||||
assert!(metrics.ss_error_pct < 5.0, "ss = {}", metrics.ss_error_pct);
|
||||
assert!(metrics.overshoot_pct < 30.0, "overshoot = {}", metrics.overshoot_pct);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn sweep_beats_the_documented_hand_tuned_baseline() {
|
||||
let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0);
|
||||
// README 记录的空载台架起步整定：kp=2, ki=10, kd=0, ff=0。
|
||||
let baseline = evaluator.evaluate(&VelocityGains::new(2.0, 10.0, 0.0, 0.0));
|
||||
let ranked = sweep(&space(), &small_config(17), &evaluator, None).unwrap();
|
||||
assert!(
|
||||
ranked.best.cost() < baseline.cost,
|
||||
"search {} must beat baseline {}",
|
||||
ranked.best.cost(),
|
||||
baseline.cost
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn results_are_reproducible_for_a_fixed_seed() {
|
||||
let evaluator = evaluator(PlantParams::gm3508_bench(), 4, 0.5);
|
||||
let a = sweep(&space(), &small_config(99), &evaluator, None).unwrap();
|
||||
let b = sweep(&space(), &small_config(99), &evaluator, None).unwrap();
|
||||
assert_eq!(a.best.gains, b.best.gains, "同 seed 必须给出同一组增益");
|
||||
assert_eq!(a.best.cost().to_bits(), b.best.cost().to_bits());
|
||||
assert_eq!(a.trials.len(), b.trials.len());
|
||||
for (x, y) in a.trials.iter().zip(&b.trials) {
|
||||
assert_eq!(x.gains, y.gains);
|
||||
assert_eq!(x.cost().to_bits(), y.cost().to_bits());
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn different_seeds_explore_differently() {
|
||||
let evaluator = evaluator(PlantParams::gm3508_bench(), 2, 0.5);
|
||||
let a = sweep(&space(), &small_config(1), &evaluator, None).unwrap();
|
||||
let b = sweep(&space(), &small_config(2), &evaluator, None).unwrap();
|
||||
let same_phase1: Vec<_> =
|
||||
a.trials.iter().filter(|t| t.phase == Phase::Coarse).map(|t| t.gains).collect();
|
||||
let other: Vec<_> =
|
||||
b.trials.iter().filter(|t| t.phase == Phase::Coarse).map(|t| t.gains).collect();
|
||||
assert_ne!(same_phase1, other);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn refined_trials_are_not_worse_than_their_seed() {
|
||||
let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0);
|
||||
let ranked = sweep(&space(), &small_config(5), &evaluator, None).unwrap();
|
||||
let coarse_best = ranked
|
||||
.trials
|
||||
.iter()
|
||||
.filter(|t| t.phase == Phase::Coarse)
|
||||
.map(Trial::cost)
|
||||
.fold(f64::INFINITY, f64::min);
|
||||
assert!(
|
||||
ranked.best.cost() <= coarse_best + 1e-9,
|
||||
"best {} vs coarse best {coarse_best}",
|
||||
ranked.best.cost()
|
||||
);
|
||||
assert!(ranked.trials.iter().any(|t| t.phase == Phase::Refine));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn pso_solver_also_improves_or_holds() {
|
||||
let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0);
|
||||
let mut config = small_config(3);
|
||||
config.solver = SolverKind::Pso;
|
||||
config.particles = 8;
|
||||
config.max_iters = 12;
|
||||
let ranked = sweep(&space(), &config, &evaluator, None).unwrap();
|
||||
assert_eq!(ranked.best.score.verdict, Verdict::Ok, "{:?}", ranked.best);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn budget_bounds_the_number_of_evaluations() {
|
||||
let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0);
|
||||
let mut config = small_config(4);
|
||||
config.max_evals = 80;
|
||||
let ranked = sweep(&space(), &config, &evaluator, None).unwrap();
|
||||
assert!(ranked.evals <= 84, "evals = {}", ranked.evals);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn invalid_space_is_rejected() {
|
||||
let mut bad = space();
|
||||
bad.kp.min = -1.0;
|
||||
let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0);
|
||||
assert!(matches!(
|
||||
sweep(&bad, &small_config(1), &evaluator, None),
|
||||
Err(SearchError::InvalidSpace(_))
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_samples_is_rejected() {
|
||||
let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0);
|
||||
let mut config = small_config(1);
|
||||
config.samples = 0;
|
||||
assert_eq!(sweep(&space(), &config, &evaluator, None).err(), Some(SearchError::NoSamples));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn solver_kind_parsing_round_trips() {
|
||||
assert_eq!(SolverKind::parse("nelder-mead").unwrap(), SolverKind::NelderMead);
|
||||
assert_eq!(SolverKind::parse("pso").unwrap(), SolverKind::Pso);
|
||||
assert!(SolverKind::parse("simulated-annealing").is_err());
|
||||
assert_eq!(SolverKind::default().name(), "nelder-mead");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn evaluate_once_reports_the_same_verdict() {
|
||||
let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0);
|
||||
let trial = evaluate_once(&evaluator, &VelocityGains::new(4.0, 12.0, 0.0, 0.0));
|
||||
assert_eq!(trial.phase, Phase::Seed);
|
||||
assert_eq!(trial.score.verdict, Verdict::Ok);
|
||||
assert!(trial.score.metrics.is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn verdict_hints_cover_every_verdict() {
|
||||
for verdict in [
|
||||
Verdict::Ok,
|
||||
Verdict::Rejected,
|
||||
Verdict::Diverged,
|
||||
Verdict::Overspeed,
|
||||
Verdict::Saturated,
|
||||
Verdict::NoSettle,
|
||||
] {
|
||||
assert!(!verdict_hint(verdict).is_empty());
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn scenario_summary_mentions_the_key_numbers() {
|
||||
let text = scenario_summary(&Scenario::step(1200.0, 1.0));
|
||||
assert!(text.contains("1200 rpm"), "{text}");
|
||||
assert!(text.contains("1000ms"), "{text}");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user