一种便捷的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;
}