舵机控制中的半正弦（S型）速度曲线及其在STM32上的应用

为什么需要S型速度曲线

只能通过PWM传目标角度位置来控制舵机，传的角度越大，舵机转动速度越快。

情况一：在两个相差α的角度目标位置之间，预计动作完成时间T，如果只是把目标位置划分为n个等距离的Δα，那在路径起点和终点的加速度会很大，其他部分理论上是匀速的，肉眼上看会有卡顿。

情况二：倘若把目标位置划分为n个不等长的Δα，Δα在起点最小（速度从0增大），逐渐增大，到当前位置在α/2的附近的时候Δα达到峰值（速度也达到峰值），之后再逐渐减小，直到终点Δα最小（速度减小到0)。这样路径起点和终点的加速度变化就没有情况一这么明显了。

那么该如何定量地划分Δα呢？容易发现正弦函数sin(pi/T*t)在[0, T]时的变化符合情况二中速度的变化，且其变化率是连续变化的。那么，如果使用[0, T]中的sin(pi/T*t)作为舵机运动的速度曲线，那舵机运动的加速度曲线也是平滑的。

位移：（1-cos（pi/T*t）)/2

STM32上的应用

单片机上需要实时控制，应该使用解析法推出当前时刻舵机当前的目标角度。

开始舵机控制时调用Servo_StartScurve使用一个结构体scurve_motion保存起始角度/终点角度/起始时刻/持续时间/划分步数等必要的计算量，在主函数非阻塞式调用Servo_UpdateScurve中用位移解析式更新目标位置值。

开始和更新Scurve函数

static Scurve_Motion_t scurve_motion = {0};
void Servo_StartScurve(Servo_Channel ch, float start, float end, uint32_t duration_ms)
{
        // 3.10 改 steps 50
    const int steps = 50;
    float sum_factors = 0.0f;
    
    // 预计算 sum_factors（半正弦）
    for (int i = 0; i < steps; i++) {
        float t_norm = (float)i / (steps - 1);
        sum_factors += sinf(M_PI * t_norm);
    }
    
    if (sum_factors < 1e-6f) return; // 防止除零
    
    float dt = duration_ms / 1000.0f / steps;
    float k = (end - start) / (sum_factors * dt);
    
    // 初始化运动控制器
    scurve_motion.active = 1;
    scurve_motion.channel = ch;
    scurve_motion.start_angle = start;
    scurve_motion.end_angle = end;
    scurve_motion.total_time_ms = duration_ms;
    scurve_motion.start_tick = HAL_GetTick();
    scurve_motion.k = k;
    scurve_motion.sum_factors = sum_factors;
    scurve_motion.steps = steps;
    
    // 立即设置起始角度（避免跳变）
    Servo_SetAngle(ch, start);
}
void Servo_UpdateScurve(void)
{
    if (!scurve_motion.active) return;
    
    uint32_t elapsed = HAL_GetTick() - scurve_motion.start_tick;
    
    if (elapsed >= scurve_motion.total_time_ms) {
        // 运动结束：设置最终角度，标记 inactive
        Servo_SetAngle(scurve_motion.channel, scurve_motion.end_angle);
        scurve_motion.active = 0;
        return;
    }
    
    // 计算当前时间对应的 step
    float t_norm = (float)elapsed / scurve_motion.total_time_ms; // [0, 1)
    int current_step = (int)(t_norm * scurve_motion.steps);
    if (current_step >= scurve_motion.steps) current_step = scurve_motion.steps - 1;
    
    // 计算当前速度因子（半正弦）
    float t_local = (float)current_step / (scurve_motion.steps - 1);
    float factor = sinf(M_PI * t_local);
    
    // 计算已走过的位移直接用解析积分（半正弦的积分是 (1 - cos(πt))/π）避免累积误差
    float progress = (1.0f - cosf(M_PI * t_norm)) / 2.0f; // S 曲线位置比例 [0,1]
    float current_angle = scurve_motion.start_angle + 
                         (scurve_motion.end_angle - scurve_motion.start_angle) * progress;
    
    Servo_SetAngle(scurve_motion.channel, current_angle);
}

主函数局部选段

if (HAL_GetTick() - last_scurve_update >= SCURVE_UPDATE_INTERVAL_MS) {
     Servo_UpdateScurve();      // ← 真正驱动平滑运动
     last_scurve_update = HAL_GetTick();
}