Pixhawk(PX4)陀螺仪滤波与传感器温度补偿

小三体Sanity

已于 2023-05-21 19:16:09 修改

阅读量883

点赞数 1

分类专栏： PX4飞控文章标签：嵌入式硬件算法 c++

于 2023-05-21 13:34:04 首次发布

本文链接：https://blog.csdn.net/m0_37382815/article/details/130790914

版权

PX4飞控专栏收录该内容

1 篇文章 0 订阅

订阅专栏

首先打个小广告，一直很喜欢无人机，最近有空研究了一下Pixhawk(PX4)飞控，并把总结和心得写到公众号 "SantyNotebook" 里和大家分享，感兴趣的朋友可以一起来交流一下，如何从零手写无人机飞控~

本篇同步发在公众号推送中《陀螺仪滤波与传感器温度补偿》

我们知道IMU容易受到飞机机体振动影响，物理上通常在飞控安装位置设有减震垫；而IMU与气压计的测量结果会因为温度变化而导致漂移，物理上通常设置恒温装置来减小这种影响
在软件层面该如何做？这篇一起来看下频域滤波、温度补偿的原理以及它们在PX4上的实现

一陀螺仪滤波

滤波用于滤除高频噪声，保留低频信号，滤波处理缺陷是会造成信号滞后。行业内有时域和频域两种设计滤波器的方法。时域内设计的滤波器，不依赖于具体模型。频域滤波则更有针对性，但是计算开销较大

1 原理

1.1时域滤波

滤波器主要是对离散信号做卷积计算，时域内设计滤波器依赖于经验公式或观察输出波形调参

有限冲击响应数字滤波器（Finite Impulse Response Digital Filter，FIR）无限冲击响应数字滤波器（Infinite Impulse Response Digital Filter，IIR）表达形式如下

FIR

$y\left( n \right) = \sum\limits_{k = 0}^N {a\left( k \right)x\left( {n - k} \right)}$

IIR

$y\left( n \right) = \sum\limits_{k = 0}^N {a\left( k \right)x\left( {n - k} \right)} + \sum\limits_{j = 0}^P {b\left( j \right)y\left( {n - j} \right)}$

可见FIR是向前看几个时间点信号的平滑处理，IIR是除了历史信号，还结合输出反馈来做平滑

1.2 频域滤波

也可以在频域内设计滤波器参数

傅里叶原理表明：连续测量的时序或信号都可以表示为不同频率的正弦波信号无限叠加。于是，连续时间信号的傅里叶变换可以用于分析信号的频谱

$\begin{array}{l} F\left( {j\omega } \right) = \int_{ - \infty }^\infty {f\left( t \right){e^{ - j\omega t}}dt} \\ f\left( t \right) = \frac{1}{{2\pi }}\int_{ - \infty }^\infty {F\left( {j\omega } \right){e^{j\omega t}}d\omega } \end{array}$

其表达式在形式上可以推广到离散信号。相比而言，离散傅里叶变换（Discrete Fourier Transform, DFT）做频谱分析方法在工程上应用更为广泛，如图像处理领域等，因为它可以让计算机处理经过采集的离散信号

$\begin{array}{l} F\left( {{e^{j\theta }}} \right) = \sum\limits_{k = - \infty }^\infty {f\left( k \right){e^{ - j\theta k}}} \\ f\left( k \right) = \frac{1}{{2\pi }}\int_{2\pi } {F\left( {{e^{j\theta }}} \right){e^{j\theta k}}d\theta } \end{array}$

而FFT（Fast Fourier Transform 快速傅里叶变换）是DFT的快速实现算法，将计算复杂度从 $O\left( {{n^2}} \right) \to O\left( {n\log n} \right)$

从FFT推导过程来看，其本质是在较低的时间复杂度内将多项式系数表示法转化为点值表示法，而逆傅里叶变换iFFT则将点值表示法转回系数表示法

假设采样频率为Fs，采样点数为N。做FFT之后，某点表示的频率

${F_n} = \left( {n - 1} \right)*{F_s}/N$

该点a+bi的模值除以N/2就是对应该频率下的信号的幅度（对于直流信号是除以N）；该点的相位即是对应该频率下的信号的相位。相位可用表达式

$\varphi =arctan2\left( {b,a} \right)$

来得到

工程实现时，通常需要对周期性的时域信号进行截断，如果截断时间长度不是周期的整数倍，截取后信号会存在泄漏。此时需要加窗来使时域信号更好地满足FFT处理的周期性要求，减少泄漏。将原始截断后的信号与这个窗函数相乘，本质是对时域信号不等计权

汉宁窗是一种常用窗函数，其时域表达式为

$w\left( t \right) = \frac{1}{2}\left( {1 - \cos \frac{{2\pi t}}{T}} \right)$

有了频域特性，可以抑制特定频率、幅值的信号来实现降噪

若噪声频谱分布较窄，可选用陷波滤波器，若较宽，则选用带阻滤波器，一般陷波滤波器结合低通滤波器使用。相比普通高通、低通滤波，陷波滤波器精准抑制频带，使信号在某些频率点迅速衰减

PX4采用Notch陷波滤波器与滤波器串联降噪

连续系统二阶陷波滤波器传递函数

$G\left( s \right) = \frac{{{s^2} + \omega _c^2}}{{{s^2} + {\omega _{bw}}s + \omega _c^2}}$

其中 ${\omega _{bw}}$ 为陷波宽度，单位rad/s； ${\omega _c}$ 为陷波中央频率，单位rad/s

可以用双线性变换或零极点匹配方法离散化，PX4采用后者。为了方便读者理解代码，这里仅推导零极点匹配法

零极点表示下为

$G\left( s \right) = {K_s}\frac{{\left( {s - j{\omega _c}} \right)\left( {s + j\omega } \right)}}{{\left( {s - {p_1}} \right)\left( {s - {p_2}} \right)}}$

其中 ${K_s} = 1$ ； ${p_{1,2}} = \alpha \pm \beta$ 为陷波滤波器的极点，有

$\alpha = - \frac{{{\omega _{bw}}}}{2},\beta = \frac{{\sqrt {4\omega _c^2 - \omega _{bw}^2} }}{2}$

零极点匹配是指s域中的零极点对应到z域，第i个零极点对应关系为

${z_i} = {e^{{s_i}{T_s}}}$

于是有，z平面传函表达为

$G\left( z \right) = {K_z}\frac{{{z^2} - 2\cos \left( {{\omega _c}{T_s}} \right)z + 1}}{{{z^2} - 2r\cos \left( {\beta {T_s}} \right)z + {r^2}}}$

其中 $r = {e^{\alpha {T_s}}},{T_s}$ 为离散化采样时间，单位s

当s域为0时，z域为1，即

${\left. {G\left( s \right)} \right|_{s = 0}} = {\left. {G\left( z \right)} \right|_{z = 1}}$

代入可得

${K_z} = \frac{{1 - 2r\cos \left( {\beta {T_s}} \right) + {r^2}}}{{2 - 2r\cos \left( {{\omega _c}{T_s}} \right)}}$

于是有

$G\left( z \right) = \frac{{{b_0}{z^2} - {b_1}z + {b_2}}}{{{z^2} + {a_1}z + {a_2}}}$

在时域内的差分方程为

${y_n} = {b_0}{x_n} + {b_1}{x_{n - 1}} + {b_2}{x_{n - 2}} - {a_1}{y_{n - 1}} - {a_2}{y_{n - 2}}$

可以用来迭代新的样本

而另一种 $\alpha$ 低通滤波器，参数设计需要结合截止频率等，限于篇幅，内容从略

2 PX4滤波实现

2.1STM32对DSP支持

STM32上做离散信号处理，需要CortexM4硬件层面的支持。Cortex-M4支持了高级SIMD，MAC指令。此外，Cortex-M4F器件具有FPU（浮点单元），用于处理浮点计算

在软件层面，开源Arm Compiler 5实现了一套DSP函数库，用于离散信号处理
源码，手册

2.2实现流程

PX4正是借用了这部分代码完成FFT运算，流程如下

（1）初始化阶段浮点数转换成定点格式

for (int n = 0; n < _imu_gyro_fft_len; n++) {
    const float hanning_value = 0.5f * (1.f - cosf(2.f * M_PI_F * n / (_imu_gyro_fft_len - 1)));
    arm_float_to_q15(&hanning_value, &_hanning_window[n], 1);
}

（2）当陀螺仪数据有更新，执行滤波逻辑

void GyroFFT::Update(const hrt_abstime &timestamp_sample, int16_t *input[], uint8_t N) {
...
}

（3）加窗函数，乘法运算

arm_mult_q15(gyro_data_buffer[axis], _hanning_window, _fft_input_buffer, _imu_gyro_fft_len);

（4）执行RFFT逆快速傅里叶变换

arm_rfft_q15(&_rfft_q15, _fft_input_buffer, _fft_outupt_buffer);

（5）根据峰值确定噪声点，并用Quiin法插值估计噪声频率

void GyroFFT::FindPeaks(const hrt_abstime &timestamp_sample, int axis, q15_t *fft_outupt_buffer) {
...
}

（6）结合频域特性，设计滤波器，执行滤波迭代

// Apply dynamic notch filter from FFT
if (_dynamic_notch_fft_available) {
	for (int peak = MAX_NUM_FFT_PEAKS - 1; peak >= 0; peak--) {
		if (_dynamic_notch_filter_fft[axis][peak].getNotchFreq() > 0.f) {
			_dynamic_notch_filter_fft[axis][peak].applyArray(data, N);
		}
	}
}

这样，产出的信号，就可以用于上层应用啦

二温度补偿

1 原理

温度对IMU与气压计模块测量结果有明显影响。传感器工作时有预设的参考温度，当实际温度偏离时，测量结果会有偏移，通常用多项式来描述这种偏移关系

$\Delta b = \sum\limits_{i = 0}^N {{a_i}\Delta T}$

计算机中多项式运算实现，常采用Horner 算法（也称为秦九韶算法）来简化执行步骤，如下

$\begin{array}{l} \Delta b = \sum\limits_{i = 0}^N {{a_i}\Delta {T^i}} = {a_0} + {a_1}\Delta T + {a_2}\Delta {T^2} + ...\\ = {a_0} + {a_1}\left( {\Delta T + {a_2}\left( {\Delta T + {a_3}\left( {...} \right)} \right)} \right) \end{array}$

2 PX4实现

2.1 标定

升温，多项式拟合标定系数

if (_accel) {
	calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise, min_start_temp, max_start_temp);
	if (calibrators[num_calibrators]) {
		++num_calibrators;
	} else {
		PX4_ERR("alloc failed");
	}
}

2.2 更新

结合温度测量结果计算偏移量（以加计为例）

// calulate the offsets
float delta_temp_2 = delta_temp * delta_temp;
float delta_temp_3 = delta_temp_2 * delta_temp;

for (uint8_t i = 0; i < 3; i++) {
	offset[i] = coef.x0[i] + coef.x1[i] * delta_temp + coef.x2[i] * delta_temp_2 + coef.x3[i] * delta_temp_3;
}

从实现上来看，设计滤波器、传感器校准、温补系数校准过程都是用的原生数据，未经任何处理。而实际执行流程是原生传感器数据先滤波，再执行校正和温度补偿，最后用于上层应用