【运动控制】Apollo6.0的LpfCoefficients解析

【运动控制】Apollo6.0的LpfCoefficients解析

• LpfCoefficients解析
• • 1 digital_filter_coefficients.h
  • 2 digital_filter_coefficients.cc
  • 3 感谢

LpfCoefficients解析

知乎：【运动控制】Apollo6.0的LpfCoefficients解析

1 digital_filter_coefficients.h

#pragma once
#include <vector>
#include "cyber/common/log.h"

namespace apollo {
namespace common {

void LpfCoefficients(const double ts, const double cutoff_freq,
                     std::vector<double> *denominators,
                     std::vector<double> *numerators);

void LpFirstOrderCoefficients(const double ts, const double settling_time,
                              const double dead_time,
                              std::vector<double> *denominators,
                              std::vector<double> *numerators);

}  // namespace common
}  // namespace apollo

2 digital_filter_coefficients.cc

2.1 LpfCoefficients

Lpf：lowpass filter

从代码分析，采用的是二阶巴特沃斯低通滤波器，推导如下

传递函数如下
H ( S ) = ω N 2 s 2 + 2 ∗ 0.707 ∗ ω N ∗ s + ω N 2 H(S)=\frac{\omega_{N}^{2} }{s^2+2*0.707*\omega_{N}^{} *s+\omega_{N}^{2} } H(S)=s2+2∗0.707∗ωN​∗s+ωN2​ωN2​​
其中，阻尼比为0.707。

采用双线性变换，T为采样周期
s = 2 T 1 − z − 1 1 + z − 1 s=\frac {2} {T} \frac {1-z^{-1}} {1+z^{-1}} s=T2​1+z−11−z−1​
为了简便，设 α = ω N T 2 \alpha=\frac{\omega_{N}^{}T}{2} α=2ωN​T​，将上式带入传递函数，可以得到
H ( z ) = α 2 ( 1 + 2 z − 1 + z − 2 ) 1 + 2 α + α 2 1 + 2 ( α 2 − 1 ) 1 + 2 α + α 2 z − 1 + 1 − 2 α + α 2 1 + 2 α + α 2 z − 2 H(z)=\frac {\frac {\alpha^2 (1+2z^{-1}+z^{-2})} {1+\sqrt{2}\alpha+\alpha^2 } } { 1 + \frac{2(\alpha^2-1)} {1+\sqrt{2}\alpha+\alpha^2 } z^{-1} + \frac{1-\sqrt{2}\alpha+\alpha^2 } {1+\sqrt{2}\alpha+\alpha^2 } z^{-2} } H(z)=1+1+2 ​α+α22(α2−1)​z−1+1+2 ​α+α21−2 ​α+α2​z−21+2 ​α+α2α2(1+2z−1+z−2)​​

void LpfCoefficients(const double ts, const double cutoff_freq,
                     std::vector<double> *denominators,
                     std::vector<double> *numerators) {
  denominators->clear();
  numerators->clear();
  denominators->reserve(3);
  numerators->reserve(3);

  double wa = 2.0 * M_PI * cutoff_freq;  // Analog frequency in rad/s
  double alpha = wa * ts / 2.0;          // tan(Wd/2), Wd is discrete frequency
  double alpha_sqr = alpha * alpha;
  double tmp_term = std::sqrt(2.0) * alpha + alpha_sqr;
  double gain = alpha_sqr / (1.0 + tmp_term);

  denominators->push_back(1.0);
  denominators->push_back(2.0 * (alpha_sqr - 1.0) / (1.0 + tmp_term));
  denominators->push_back((1.0 - std::sqrt(2.0) * alpha + alpha_sqr) /
                          (1.0 + tmp_term));

  numerators->push_back(gain);
  numerators->push_back(2.0 * gain);
  numerators->push_back(gain);
}

2.2 LpFirstOrderCoefficients

1. 这里的滤波器主要由两个部分组成：

   采样系统 -为0阶保持器，传递函数如下所示：
   G 0 ( s ) = 1 − e − s T s G_{0}(s) = \frac{1- e^{-sT}}{s} G0​(s)=s1−e−sT​

   一阶滤波系统，传递函数如下所示：
   G 1 ( s ) = 1 s + a G_{1}(s) = \frac{1}{s+a} G1​(s)=s+a1​
   写成数字控制器的形式:
   G ( s ) = G 0 ( s ) G 1 ( s ) = ( 1 − e − s T ) ( 1 s ( s + a ) ) G_{}(s)= G_{0}(s)G_{1}(s) = (1 - e^{-sT})(\frac{1}{s(s+a)}) G​(s)=G0​(s)G1​(s)=(1−e−sT)(s(s+a)1​)

   代入离散变换公式得：
   G ( z ) = ( 1 − z − 1 ) ( z ( 1 − e − a T ) ( z − 1 ) ( z − e − a T ) ) G(z) = (1-z^{-1})(\frac{z(1-e^{-aT})}{(z-1)(z-e^{-aT})}) G(z)=(1−z−1)((z−1)(z−e−aT)z(1−e−aT)​)
   最终我们可以得到：
   G ( z ) = ( 1 − e − a T ) ( z − e − a T ) G(z) =\frac{(1-e^{-aT})}{(z-e^{-aT})} G(z)=(z−e−aT)(1−e−aT)​
   上述离散公式与apollo代码中的一致，其中T为采样时间，与采样频率相关；参数 a = 2 π f a = 2\pi f a=2πf, f f f为滤波器带宽，即为截止频率。

2. 代码相关

   代码里令 ω = 1 / s e t t l i n g t i m e \omega = 1/{settling time} ω=1/settlingtime ，按照上一步的公式推算猜测为估计的截止频率，单位为rad/s，不太明白是什么意思

void LpFirstOrderCoefficients(const double ts, const double settling_time,
                              const double dead_time,
                              std::vector<double> *denominators,
                              std::vector<double> *numerators) {
  // sanity check
  if (ts <= 0.0 || settling_time < 0.0 || dead_time < 0.0) {
    AERROR << "time cannot be negative";
    return;
  }

  const size_t k_d = static_cast<size_t>(dead_time / ts); // 死区
  double a_term;

  denominators->clear();
  numerators->clear();
  denominators->reserve(2);
  numerators->reserve(k_d + 1);  // size depends on dead-time
       
  if (settling_time == 0.0) {
    a_term = 0.0;
  } else {
    a_term = exp(-1 * ts / settling_time);
  }

  denominators->push_back(1.0);
  denominators->push_back(-a_term);
    
  numerators->insert(numerators->end(), k_d, 0.0); // 插入几阶的零
  numerators->push_back(1 - a_term);
}

3 感谢

感谢P&C组的张天宇同事对一阶低通滤波器的解析改进。