776 lines
29 KiB
Rust
776 lines
29 KiB
Rust
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||
|
||
pub mod scenario {
|
||
//! 场景定义：设定值曲线 + 采样节拍。
|
||
//!
|
||
//! 场景与具体被控对象解耦：同一场景可在 [`crate::PlantMotor`]（离线）、
|
||
//! `CanLink` 上的 `EscBus` 闭环（在线）与真机台架（`chassis-bench`）上重放，
|
||
//! 因此整定结果可以跨后端比较。
|
||
|
||
/// 设定值曲线。
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||
pub enum SetpointProfile {
|
||
/// 阶跃：`t = 0` 直接给目标（忽略 `ramp_s`）。
|
||
Step,
|
||
/// 斜坡 + 保持：`ramp_s` 内线性升到目标，随后保持 `hold_s`。
|
||
RampAndHold,
|
||
/// 阶梯：在 `ramp_s + hold_s` 内均分 `levels` 级，逐级跳变到目标。
|
||
Staircase {
|
||
/// 阶梯数（≥ 1）。
|
||
levels: u32,
|
||
},
|
||
/// 升 → 保持 → 降 → 保持（回零），总时长 `2 × (ramp_s + hold_s)`。
|
||
UpDown,
|
||
}
|
||
|
||
/// 一次试验的设定值与时间规格。
|
||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||
pub struct Scenario {
|
||
/// 控制 / 采样节拍（秒）。
|
||
pub dt_s: f32,
|
||
/// 斜坡时间（秒）；`0` 表示瞬时。
|
||
pub ramp_s: f32,
|
||
/// 保持时间（秒）。
|
||
pub hold_s: f32,
|
||
/// 目标转子转速（rpm）。
|
||
pub target_rpm: f32,
|
||
/// 设定值曲线。
|
||
pub profile: SetpointProfile,
|
||
}
|
||
|
||
impl Scenario {
|
||
/// 默认控制节拍（1 kHz，与 C620 命令周期一致）。
|
||
pub const DEFAULT_DT_S: f32 = 0.001;
|
||
|
||
/// 阶跃 + 保持。
|
||
#[must_use]
|
||
pub const fn step(target_rpm: f32, hold_s: f32) -> Self {
|
||
Self {
|
||
dt_s: Self::DEFAULT_DT_S,
|
||
ramp_s: 0.0,
|
||
hold_s,
|
||
target_rpm,
|
||
profile: SetpointProfile::Step,
|
||
}
|
||
}
|
||
|
||
/// 斜坡 + 保持（台架默认整定场景）。
|
||
#[must_use]
|
||
pub const fn ramp_and_hold(target_rpm: f32, ramp_s: f32, hold_s: f32) -> Self {
|
||
Self {
|
||
dt_s: Self::DEFAULT_DT_S,
|
||
ramp_s,
|
||
hold_s,
|
||
target_rpm,
|
||
profile: SetpointProfile::RampAndHold,
|
||
}
|
||
}
|
||
|
||
/// 换一个采样节拍。
|
||
#[must_use]
|
||
pub const fn with_dt_s(mut self, dt_s: f32) -> Self {
|
||
self.dt_s = dt_s;
|
||
self
|
||
}
|
||
|
||
/// 换一条设定值曲线。
|
||
#[must_use]
|
||
pub const fn with_profile(mut self, profile: SetpointProfile) -> Self {
|
||
self.profile = profile;
|
||
self
|
||
}
|
||
|
||
/// 场景总时长（秒）。
|
||
#[must_use]
|
||
pub fn total_s(&self) -> f32 {
|
||
match self.profile {
|
||
SetpointProfile::Step => self.hold_s,
|
||
SetpointProfile::RampAndHold | SetpointProfile::Staircase { .. } => {
|
||
self.ramp_s + self.hold_s
|
||
}
|
||
SetpointProfile::UpDown => 2.0 * (self.ramp_s + self.hold_s),
|
||
}
|
||
}
|
||
|
||
/// 场景采样点数（`dt_s` 非法时返回 1，由调用方做参数校验）。
|
||
#[must_use]
|
||
pub fn samples(&self) -> usize {
|
||
let total = self.total_s();
|
||
let dt_ok = self.dt_s.is_finite() && self.dt_s > 0.0;
|
||
let total_ok = total.is_finite() && total > 0.0;
|
||
if !dt_ok || !total_ok {
|
||
return 1;
|
||
}
|
||
((total / self.dt_s).round() as usize).max(1)
|
||
}
|
||
|
||
/// `t_s` 时刻的设定值（rpm）。
|
||
#[must_use]
|
||
pub fn setpoint_at(&self, t_s: f32) -> f32 {
|
||
let target = self.target_rpm;
|
||
let ramp = self.ramp_s;
|
||
match self.profile {
|
||
SetpointProfile::Step => target,
|
||
SetpointProfile::RampAndHold => {
|
||
if ramp > 0.0 && t_s < ramp {
|
||
target * (t_s / ramp)
|
||
} else {
|
||
target
|
||
}
|
||
}
|
||
SetpointProfile::Staircase { levels } => {
|
||
let levels = levels.max(1);
|
||
let total = ramp + self.hold_s;
|
||
if total <= 0.0 {
|
||
return target;
|
||
}
|
||
let rung_s = total / levels as f32;
|
||
let index = (t_s / rung_s).floor().max(0.0) as u32;
|
||
target * ((index.min(levels - 1) + 1) as f32 / levels as f32)
|
||
}
|
||
SetpointProfile::UpDown => {
|
||
let up_end = ramp;
|
||
let hold_end = up_end + self.hold_s;
|
||
let down_end = hold_end + ramp;
|
||
if t_s < up_end {
|
||
if ramp > 0.0 { target * (t_s / ramp) } else { target }
|
||
} else if t_s < hold_end {
|
||
target
|
||
} else if t_s < down_end {
|
||
let progress = (t_s - hold_end) / ramp;
|
||
target * (1.0 - progress)
|
||
} else {
|
||
0.0
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// 稳态评价窗口的起点（秒）：斜坡 / 阶梯结束、进入最后一段保持时。
|
||
#[must_use]
|
||
pub fn steady_start_s(&self) -> f32 {
|
||
match self.profile {
|
||
SetpointProfile::Step => 0.0,
|
||
SetpointProfile::RampAndHold => self.ramp_s,
|
||
SetpointProfile::Staircase { levels } => {
|
||
let level_s = self.level_s(levels);
|
||
((levels.max(1) - 1) as f32) * level_s
|
||
}
|
||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||
}
|
||
}
|
||
|
||
/// 稳态评价窗口的终点（秒）。
|
||
#[must_use]
|
||
pub fn steady_end_s(&self) -> f32 {
|
||
match self.profile {
|
||
// 升降场景只评价上升段后的保持段。
|
||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||
_ => self.total_s(),
|
||
}
|
||
}
|
||
|
||
/// 阶梯的单级时长（秒）。
|
||
fn level_s(&self, levels: u32) -> f32 {
|
||
let levels = levels.max(1);
|
||
let total = self.ramp_s + self.hold_s;
|
||
if total > 0.0 { total / levels as f32 } else { 0.0 }
|
||
}
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
/// 浮点断言辅助：避免 `float_cmp`，同时给出可读的失败信息。
|
||
#[track_caller]
|
||
fn assert_close(actual: f32, expected: f32) {
|
||
assert!((actual - expected).abs() < 1e-3, "actual {actual}, expected {expected}");
|
||
}
|
||
|
||
#[test]
|
||
fn step_is_immediate_and_lasts_hold() {
|
||
let s = Scenario::step(1000.0, 0.5);
|
||
assert!((s.total_s() - 0.5).abs() < 1e-6);
|
||
assert_close(s.setpoint_at(0.0), 1000.0);
|
||
assert_close(s.setpoint_at(0.4), 1000.0);
|
||
assert_eq!(s.samples(), 500);
|
||
}
|
||
|
||
#[test]
|
||
fn ramp_is_linear_then_holds() {
|
||
let s = Scenario::ramp_and_hold(1000.0, 0.2, 1.0);
|
||
assert_close(s.setpoint_at(0.0), 0.0);
|
||
assert!((s.setpoint_at(0.1) - 500.0).abs() < 1e-3);
|
||
assert_close(s.setpoint_at(0.2), 1000.0);
|
||
assert_close(s.setpoint_at(1.1), 1000.0);
|
||
assert!((s.total_s() - 1.2).abs() < 1e-6);
|
||
}
|
||
|
||
#[test]
|
||
fn staircase_rises_in_equal_levels() {
|
||
let s = Scenario::ramp_and_hold(1000.0, 0.0, 1.0)
|
||
.with_profile(SetpointProfile::Staircase { levels: 4 });
|
||
assert!((s.setpoint_at(0.0) - 250.0).abs() < 1e-3);
|
||
assert!((s.setpoint_at(0.3) - 500.0).abs() < 1e-3);
|
||
assert!((s.setpoint_at(0.6) - 750.0).abs() < 1e-3);
|
||
assert!((s.setpoint_at(0.99) - 1000.0).abs() < 1e-3);
|
||
// 越界采样被夹在最后一级。
|
||
assert_close(s.setpoint_at(5.0), 1000.0);
|
||
}
|
||
|
||
#[test]
|
||
fn up_down_returns_to_zero() {
|
||
let s = Scenario::ramp_and_hold(600.0, 0.1, 0.2).with_profile(SetpointProfile::UpDown);
|
||
assert!((s.total_s() - 0.6).abs() < 1e-6);
|
||
assert!((s.setpoint_at(0.05) - 300.0).abs() < 1e-3);
|
||
assert_close(s.setpoint_at(0.2), 600.0);
|
||
assert!((s.setpoint_at(0.35) - 300.0).abs() < 1e-3);
|
||
assert_close(s.setpoint_at(0.59), 0.0);
|
||
}
|
||
|
||
#[test]
|
||
fn degenerate_scenario_reports_one_sample() {
|
||
let s = Scenario::step(1.0, 0.0);
|
||
assert_eq!(s.samples(), 1);
|
||
let s = Scenario::step(1.0, 1.0).with_dt_s(0.0);
|
||
assert_eq!(s.samples(), 1);
|
||
}
|
||
}
|
||
}
|
||
|
||
pub use scenario::*;
|
||
|
||
pub mod trace {
|
||
//! 试验轨迹：一次场景重放的全部采样。
|
||
|
||
/// 一条完整试验轨迹（各列等长，索引即采样序号）。
|
||
#[derive(Debug, Clone, Default, PartialEq)]
|
||
pub struct Trace {
|
||
/// 采样时刻（秒）。
|
||
pub t_s: Vec<f32>,
|
||
/// 该时刻的设定值（rpm）。
|
||
pub setpoint_rpm: Vec<f32>,
|
||
/// 该时刻的实测转速（rpm，物理方向）。
|
||
pub rpm: Vec<f32>,
|
||
/// 该时刻下发的 effort（raw，已限幅）。
|
||
pub effort_raw: Vec<i16>,
|
||
/// 该时刻控制器输出是否贴着限幅（饱和）。
|
||
pub saturated: Vec<bool>,
|
||
}
|
||
|
||
impl Trace {
|
||
/// 预留容量的空轨迹。
|
||
#[must_use]
|
||
pub fn with_capacity(capacity: usize) -> Self {
|
||
Self {
|
||
t_s: Vec::with_capacity(capacity),
|
||
setpoint_rpm: Vec::with_capacity(capacity),
|
||
rpm: Vec::with_capacity(capacity),
|
||
effort_raw: Vec::with_capacity(capacity),
|
||
saturated: Vec::with_capacity(capacity),
|
||
}
|
||
}
|
||
|
||
/// 追加一个采样。
|
||
pub fn push(
|
||
&mut self,
|
||
t_s: f32,
|
||
setpoint_rpm: f32,
|
||
rpm: f32,
|
||
effort_raw: i16,
|
||
saturated: bool,
|
||
) {
|
||
self.t_s.push(t_s);
|
||
self.setpoint_rpm.push(setpoint_rpm);
|
||
self.rpm.push(rpm);
|
||
self.effort_raw.push(effort_raw);
|
||
self.saturated.push(saturated);
|
||
}
|
||
|
||
/// 采样数。
|
||
#[must_use]
|
||
pub fn len(&self) -> usize {
|
||
self.rpm.len()
|
||
}
|
||
|
||
/// 是否为空。
|
||
#[must_use]
|
||
pub fn is_empty(&self) -> bool {
|
||
self.rpm.is_empty()
|
||
}
|
||
|
||
/// 最后一段采样（稳态窗口用；越界时返回空切片）。
|
||
#[must_use]
|
||
pub fn window(&self, from_s: f32, to_s: f32) -> Window<'_> {
|
||
let start = self.t_s.partition_point(|&t| t < from_s);
|
||
let end = self.t_s.partition_point(|&t| t <= to_s);
|
||
let end = end.max(start);
|
||
Window { trace: self, range: start..end }
|
||
}
|
||
|
||
/// 全轨迹最大转速（rpm）。
|
||
#[must_use]
|
||
pub fn peak_rpm(&self) -> f32 {
|
||
self.rpm.iter().copied().fold(0.0f32, |acc, v| acc.max(v.abs()))
|
||
}
|
||
|
||
/// 全轨迹最小 / 最大转速（rpm）。
|
||
#[must_use]
|
||
pub fn rpm_range(&self) -> (f32, f32) {
|
||
let min = self.rpm.iter().copied().fold(f32::INFINITY, f32::min);
|
||
let max = self.rpm.iter().copied().fold(f32::NEG_INFINITY, f32::max);
|
||
if self.rpm.is_empty() { (0.0, 0.0) } else { (min, max) }
|
||
}
|
||
|
||
/// 饱和采样占比（0..=1）。
|
||
#[must_use]
|
||
pub fn saturated_fraction(&self) -> f32 {
|
||
if self.saturated.is_empty() {
|
||
return 0.0;
|
||
}
|
||
let count = self.saturated.iter().filter(|&&s| s).count();
|
||
count as f32 / self.saturated.len() as f32
|
||
}
|
||
|
||
/// effort 符号翻转次数（振荡 / 抖振指标）。
|
||
#[must_use]
|
||
pub fn effort_reversals(&self) -> usize {
|
||
let mut reversals = 0usize;
|
||
let mut last_sign = 0i16;
|
||
for &raw in &self.effort_raw {
|
||
let sign = raw.signum();
|
||
if sign != 0 {
|
||
if last_sign != 0 && sign != last_sign {
|
||
reversals += 1;
|
||
}
|
||
last_sign = sign;
|
||
}
|
||
}
|
||
reversals
|
||
}
|
||
|
||
/// 全部列长度一致（不变量自检）。
|
||
#[must_use]
|
||
pub fn is_consistent(&self) -> bool {
|
||
self.t_s.len() == self.rpm.len()
|
||
&& self.setpoint_rpm.len() == self.rpm.len()
|
||
&& self.effort_raw.len() == self.rpm.len()
|
||
&& self.saturated.len() == self.rpm.len()
|
||
}
|
||
}
|
||
|
||
/// 轨迹上的一个时间窗口视图。
|
||
#[derive(Debug, Clone)]
|
||
pub struct Window<'a> {
|
||
trace: &'a Trace,
|
||
range: std::ops::Range<usize>,
|
||
}
|
||
|
||
impl Window<'_> {
|
||
/// 窗口内采样数。
|
||
#[must_use]
|
||
pub fn len(&self) -> usize {
|
||
self.range.len()
|
||
}
|
||
|
||
/// 窗口是否为空。
|
||
#[must_use]
|
||
pub fn is_empty(&self) -> bool {
|
||
self.range.is_empty()
|
||
}
|
||
|
||
/// 窗口内的实测转速。
|
||
#[must_use]
|
||
pub fn rpm(&self) -> &[f32] {
|
||
&self.trace.rpm[self.range.clone()]
|
||
}
|
||
|
||
/// 窗口内的设定值。
|
||
#[must_use]
|
||
pub fn setpoint_rpm(&self) -> &[f32] {
|
||
&self.trace.setpoint_rpm[self.range.clone()]
|
||
}
|
||
|
||
/// 窗口内的 effort。
|
||
#[must_use]
|
||
pub fn effort_raw(&self) -> &[i16] {
|
||
&self.trace.effort_raw[self.range.clone()]
|
||
}
|
||
|
||
/// 窗口内的时刻。
|
||
#[must_use]
|
||
pub fn t_s(&self) -> &[f32] {
|
||
&self.trace.t_s[self.range.clone()]
|
||
}
|
||
|
||
/// 窗口内转速均值。
|
||
#[must_use]
|
||
pub fn mean_rpm(&self) -> f32 {
|
||
let values = self.rpm();
|
||
if values.is_empty() {
|
||
return 0.0;
|
||
}
|
||
values.iter().sum::<f32>() / values.len() as f32
|
||
}
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
fn sample_trace() -> Trace {
|
||
let mut trace = Trace::with_capacity(8);
|
||
for i in 0..8 {
|
||
let t = i as f32 * 0.1;
|
||
trace.push(t, 100.0, t * 100.0, if i % 2 == 0 { 10 } else { -10 }, i > 5);
|
||
}
|
||
trace
|
||
}
|
||
|
||
#[test]
|
||
fn push_keeps_columns_aligned() {
|
||
let trace = sample_trace();
|
||
assert_eq!(trace.len(), 8);
|
||
assert!(trace.is_consistent());
|
||
}
|
||
|
||
#[test]
|
||
fn window_selects_half_open_range() {
|
||
let trace = sample_trace();
|
||
let window = trace.window(0.2, 0.4);
|
||
assert_eq!(window.len(), 3); // t = 0.2, 0.3, 0.4
|
||
assert_eq!(window.t_s(), &[0.2, 0.3, 0.4]);
|
||
assert!((window.mean_rpm() - 30.0).abs() < 1e-3);
|
||
}
|
||
|
||
#[test]
|
||
fn window_out_of_range_is_empty() {
|
||
let trace = sample_trace();
|
||
let window = trace.window(5.0, 6.0);
|
||
assert!(window.is_empty());
|
||
assert!(window.mean_rpm().abs() < f32::EPSILON);
|
||
}
|
||
|
||
#[test]
|
||
fn peak_and_range_are_absolute() {
|
||
let trace = sample_trace();
|
||
assert!((trace.peak_rpm() - 70.0).abs() < 1e-3);
|
||
assert_eq!(trace.rpm_range(), (0.0, 70.0));
|
||
}
|
||
|
||
#[test]
|
||
fn saturated_fraction_counts_flags() {
|
||
let trace = sample_trace();
|
||
assert!((trace.saturated_fraction() - 0.25).abs() < 1e-6);
|
||
}
|
||
|
||
#[test]
|
||
fn effort_reversals_counts_sign_changes() {
|
||
let trace = sample_trace();
|
||
// +,−,+,−,+,−,+,−：每次翻转都计数（首个样本建立方向）。
|
||
assert_eq!(trace.effort_reversals(), 7);
|
||
}
|
||
}
|
||
}
|
||
|
||
pub use trace::*;
|
||
|
||
pub mod metrics {
|
||
//! 步响应指标：把一条 [`Trace`] 折成可比较、可解释的标量。
|
||
//!
|
||
//! 指标口径对**所有后端**（离线 harness、真机台架）一致——整定结果能不能
|
||
//! 从仿真迁移到实车，取决于两条路径是否用同一把尺子量。
|
||
|
||
use crate::{Scenario, Trace};
|
||
|
||
/// 稳态判据的相对带宽（±5% of target）。
|
||
///
|
||
/// C620 的转速反馈是整数 rpm，空载低速时测量抖动本身就会超过 ±2%；
|
||
/// 真机台架若沿用 ±2% 会把几乎每一组正常 PID 都误判为 `no-settle`。
|
||
pub const SETTLING_BAND: f32 = 0.05;
|
||
/// 判定“基本收敛”时使用的较宽相对带宽（±10% of target）。
|
||
///
|
||
/// 调节时间仍按 ±5% 计算；这里只回答“稳态阶段是否大体跟住”，避免真机反馈
|
||
/// 的少量量化尖峰把整组可用增益判成未收敛。
|
||
pub const SETTLED_BAND: f32 = 0.10;
|
||
/// 稳态窗口占轨迹总时长的比例（从 75% 处开始）。
|
||
///
|
||
/// 与场景自带的 `steady_start_s()` 取**更晚**者：阶跃场景没有显式斜坡，
|
||
/// 若不设下限就会把整条上升段也算进"稳态"，任何真实响应都会被判成未收敛。
|
||
pub const STEADY_FRACTION: f32 = 0.75;
|
||
/// 稳态窗口允许的离带采样占比。
|
||
///
|
||
/// 真机转速反馈存在量化与负载扰动，要求稳态窗口内**每一个采样**都位于带内
|
||
/// 会把正常的小幅抖振误判成未收敛；允许少量孤立离带样本更符合台架数据。
|
||
pub const MAX_STEADY_EXCURSION_FRACTION: f32 = 0.10;
|
||
/// 指标最少需要的采样数（样本太少时一切统计都无意义）。
|
||
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,
|
||
/// 调节时间：最后一次离开 ±5% 带的时刻（秒）。
|
||
pub settling_time_s: f32,
|
||
/// 是否在稳态窗口内基本保持 ±10% 带（允许少量测量抖动离带）。
|
||
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_band = SETTLED_BAND * magnitude;
|
||
let out_of_band =
|
||
window.rpm().iter().filter(|rpm| (**rpm - target).abs() > settled_band).count();
|
||
let excursion_fraction =
|
||
if window.is_empty() { 1.0 } else { out_of_band as f32 / window.len() as f32 };
|
||
let settled = excursion_fraction <= MAX_STEADY_EXCURSION_FRACTION;
|
||
|
||
// 误差积分（全过程，按 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, 60.0);
|
||
let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap();
|
||
assert!((metrics.ss_error_pct - 6.0).abs() < 1.0, "ss = {}", metrics.ss_error_pct);
|
||
}
|
||
|
||
#[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 isolated_feedback_excursions_do_not_reject_a_settled_response() {
|
||
let mut trace = synthetic(0.02, 0.0, 0.0);
|
||
let target = scenario().target_rpm;
|
||
let index = trace.t_s.iter().position(|t_s| *t_s > 0.75).unwrap();
|
||
// 稳态窗口内制造一个孤立测量尖峰。
|
||
trace.rpm[index] = target * 1.12;
|
||
|
||
let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap();
|
||
assert!(metrics.settled, "少量孤立反馈尖峰不应否决稳定响应");
|
||
}
|
||
|
||
#[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());
|
||
}
|
||
}
|
||
}
|
||
|
||
pub use metrics::*;
|