【基础技术】一种简易实现的T型滤波器

一种便捷的T型滤波器：

• 用在速度滤波，则可以保证加速度为梯型；
• 设计思路：
• 最大速度绝对值为从当前s_now运行值目标s_star所允许的最大速度；
• 目标速度取匀速最大速度与速度限制值的较小者；
• 在加速度阶段，平滑跟踪此速度，在减速阶段，直接取值此速度；

头文件

#ifndef _TRAMP_H_
#define _TRAMP_H_

#include <math.h>

class Tramp
{
private:
    /* data */
    float s_star_;
    float s_now_;
    float v_now_;
    float v_max_;
    float a_max_;
    float get_sign(float x)
    {
        if (x > 0.0) return 1.0;
        else if (x < 0.0) return -1.0;
        return 0.0;
    }
public:
    Tramp(float v_max=1.2, float a_max=5.0);
    ~Tramp();
    float update(float s_star, float dt);
    void reset(void);

    float moni_s_now;
    float moni_v_now;
    float moni_acc_now;
    float moni_vref;
    float moni_dv;
    float moni_dt_ms;
};
#endif

源文件

#include "Tramp.h"
#include <stdio.h>

Tramp::Tramp(float v_max, float a_max)
:v_max_(v_max),a_max_(a_max),
s_now_(0),v_now_(0)
{    
}

Tramp::~Tramp()
{
}


// ----------------------------------------------------------
/* 一种便捷的T型滤波器：
 * 用在速度滤波，则可以保证加速度为梯型；
 *  设计思路：最大速度绝对值为从当前s_now运行值目标s_star所允许的最大速度；
 *  目标速度取匀速最大速度与速度限制值的较小者；
 *  在加速度阶段，平滑跟踪此速度，在减速阶段，直接取值此速度；
 */
// ----------------------------------------------------------
float Tramp::update(float s_star, float dt)
{
    s_star_ = s_star;
    float ds = s_star_ - s_now_;
    float v_limit = get_sign(ds)*sqrt(2*fabsf(ds)*a_max_);
    float v_ref = 0.0;
    if (v_limit > 0)
    {
        v_ref = (v_limit < v_max_) ? v_limit : v_max_;
    }
    else
    {
        v_ref = (v_limit > -v_max_) ? v_limit : -v_max_;
    }
    
    float dv = v_ref - v_now_;
    float v_step = 0.0;
    if (dv > 0)
    {
        v_step = (dv < a_max_*dt) ? dv : a_max_*dt;
    }
    else
    {
        v_step = (dv > -a_max_*dt) ? dv : -a_max_*dt;
    }

    float v_new = v_now_ + v_step;
    v_new = (fabsf(v_new) < fabsf(v_ref)) ? v_new : v_ref;

    s_now_ += 0.5*(v_now_ + v_new)*dt;
    v_now_ = v_new;
    
    
    // printf("v_limit = %f, v_ref = %f v_now_ = %f\n", v_limit, v_ref, v_now_);
    moni_s_now = s_now_;
    moni_v_now = v_now_;
    moni_acc_now = v_step/dt;
    moni_vref = v_limit;//v_ref;
    moni_dv = dv;
    moni_dt_ms = dt*1000.0;

    return s_now_;
}

void Tramp::reset(void)
{    
    s_now_ = 0.0;
    v_now_ = 0.0;
}