168 lines
4.9 KiB
C++
168 lines
4.9 KiB
C++
//
|
||
// Created by lgv on 2026/3/2.
|
||
//
|
||
/**
|
||
* @file s_curve.h
|
||
* @brief 严格点到点位置 S 曲线轨迹生成器
|
||
*
|
||
* 实现标准 7 段式位置 S 曲线速度剖面：
|
||
* 1. 加加速度上升段（加速度逐渐增大）
|
||
* 2. 匀加速段
|
||
* 3. 加加速度下降段（加速度逐渐降到 0）
|
||
* 4. 匀速巡航段
|
||
* 5. 加加速度下降段（减速度逐渐增大）
|
||
* 6. 匀减速段
|
||
* 7. 加加速度上升段（减速度逐渐降到 0）
|
||
*
|
||
* 本类只负责严格点到点 profile 规划与采样，不负责实时目标跟踪。
|
||
*/
|
||
|
||
#pragma once
|
||
|
||
#include <algorithm>
|
||
#include <cmath>
|
||
#include <vector>
|
||
|
||
|
||
namespace cmvr
|
||
{
|
||
|
||
/**
|
||
* @brief 单次点到点运动的 S 曲线剖面参数
|
||
*/
|
||
struct SCurveProfile
|
||
{
|
||
// 7 个阶段的持续时间
|
||
double t1; // 加加速度上升时间（加速阶段）
|
||
double t2; // 匀加速时间
|
||
double t3; // 加加速度下降时间（加速阶段结束）
|
||
double t4; // 匀速时间（巡航）
|
||
double t5; // 加加速度下降时间（减速阶段）
|
||
double t6; // 匀减速时间
|
||
double t7; // 加加速度上升时间（减速阶段结束）
|
||
|
||
double total_time;
|
||
|
||
// 运动约束
|
||
double j_max; // 最大加加速度 (rad/s³)
|
||
double a_max; // 最大加速度 (rad/s²)
|
||
double v_max; // 最大速度 (rad/s)
|
||
|
||
// 运动参数
|
||
double distance; // 运动总距离
|
||
double direction; // +1 或 -1
|
||
double v_cruise; // 实际达到的巡航速度
|
||
double a_limit; // 实际达到的加速度上限
|
||
|
||
// 初始条件
|
||
double p0; // 初始位置
|
||
double v0; // 初始速度
|
||
double a0; // 初始加速度
|
||
double vf; // 终止速度
|
||
|
||
SCurveProfile()
|
||
: t1(0), t2(0), t3(0), t4(0), t5(0), t6(0), t7(0)
|
||
, total_time(0)
|
||
, j_max(50.0), a_max(10.0), v_max(3.0)
|
||
, distance(0), direction(1.0), v_cruise(0), a_limit(0)
|
||
, p0(0), v0(0), a0(0), vf(0)
|
||
{}
|
||
};
|
||
|
||
/**
|
||
* @brief 严格点到点位置 S 曲线规划器
|
||
*/
|
||
class SCurve
|
||
{
|
||
public:
|
||
/**
|
||
* @brief 构造函数（带运动约束）
|
||
* @param max_velocity 最大速度 (rad/s)
|
||
* @param max_acceleration 最大加速度 (rad/s²)
|
||
* @param max_jerk 最大加加速度 (rad/s³)
|
||
*/
|
||
SCurve(double max_velocity = 3.0,
|
||
double max_acceleration = 10.0,
|
||
double max_jerk = 50.0);
|
||
|
||
/**
|
||
* @brief 设置运动约束
|
||
*/
|
||
void setConstraints(double max_velocity, double max_acceleration, double max_jerk);
|
||
|
||
/**
|
||
* @brief 获取当前运动约束
|
||
*/
|
||
void getConstraints(double& max_velocity, double& max_acceleration, double& max_jerk) const;
|
||
|
||
/**
|
||
* @brief 计算点到点运动的完整 S 曲线剖面
|
||
* @param start_position 起始位置
|
||
* @param end_position 终止位置
|
||
* @param start_velocity 起始速度（默认 0）
|
||
* @param end_velocity 终止速度（默认 0）
|
||
* @return 计算得到的剖面参数
|
||
*/
|
||
SCurveProfile calculateProfile(double start_position, double end_position,
|
||
double start_velocity = 0.0, double end_velocity = 0.0);
|
||
|
||
/**
|
||
* @brief 给定剖面下，获取时刻 t 的位置
|
||
* @param profile S 曲线剖面
|
||
* @param t 从起点开始的时间
|
||
* @return 时刻 t 的位置
|
||
*/
|
||
double getPositionAtTime(const SCurveProfile& profile, double t) const;
|
||
|
||
/**
|
||
* @brief 给定剖面下，获取时刻 t 的速度
|
||
* @param profile S 曲线剖面
|
||
* @param t 从起点开始的时间
|
||
* @return 时刻 t 的速度
|
||
*/
|
||
double getVelocityAtTime(const SCurveProfile& profile, double t) const;
|
||
|
||
/**
|
||
* @brief 给定剖面下，获取时刻 t 的加速度
|
||
* @param profile S 曲线剖面
|
||
* @param t 从起点开始的时间
|
||
* @return 时刻 t 的加速度
|
||
*/
|
||
double getAccelerationAtTime(const SCurveProfile& profile, double t) const;
|
||
|
||
/**
|
||
* @brief 给定剖面下，获取时刻 t 的加加速度
|
||
* @param profile S 曲线剖面
|
||
* @param t 从起点开始的时间
|
||
* @return 时刻 t 的加加速度
|
||
*/
|
||
double getJerkAtTime(const SCurveProfile& profile, double t) const;
|
||
|
||
/**
|
||
* @brief 按固定时间间隔生成轨迹点
|
||
* @param profile S 曲线剖面
|
||
* @param dt 轨迹点时间间隔
|
||
* @param positions 输出：每个时间点的位置
|
||
* @param velocities 输出：每个时间点的速度
|
||
* @param accelerations 输出：每个时间点的加速度
|
||
*/
|
||
void generateTrajectory(const SCurveProfile& profile, double dt,
|
||
std::vector<double>& positions,
|
||
std::vector<double>& velocities,
|
||
std::vector<double>& accelerations) const;
|
||
|
||
private:
|
||
double max_velocity_;
|
||
double max_acceleration_;
|
||
double max_jerk_;
|
||
|
||
static constexpr double EPSILON = 1e-9;
|
||
static constexpr double VELOCITY_THRESHOLD = 1e-6;
|
||
|
||
double computeSegmentJerk(const SCurveProfile& profile, double t) const;
|
||
void calculateShortProfile(SCurveProfile& profile) const;
|
||
void calculateLongProfile(SCurveProfile& profile) const;
|
||
};
|
||
|
||
} // namespace cmvr
|