多旋翼姿态解算之高低通滤波

本文方法来自于全权老师的《多旋翼飞行器设计与控制》一书

首先我们来看怎样通过机载传感器得到机体的姿态角；

姿态角变化率$\dot{\theta}, \dot{\phi}, \dot{\psi}$θ˙,ϕ˙​,ψ˙​与机体角速度$^{b} \omega$bω之间的关系如下：

$\left[ \begin{array}{c}{\dot{\phi}} \\ {\dot{\theta}} \\ {\dot{\psi}}\end{array}\right]=\left[ \begin{array}{ccc}{1} & {\tan \theta \sin \phi} & {\tan \theta \cos \phi} \\ {0} & {\cos \phi} & {-\sin \phi} \\ {0} & {\sin \phi / \cos \theta} & {\cos \phi / \cos \theta}\end{array}\right] \left[ \begin{array}{c}{\omega_{x_{b}}} \\ {\omega_{b_{b}}} \\ {\omega_{b_{b}}}\end{array}\right]$⎣⎡​ϕ˙​θ˙ψ˙​​⎦⎤​=⎣⎡​100​tanθsinϕcosϕsinϕ/cosθ​tanθcosϕ−sinϕcosϕ/cosθ​⎦⎤​⎣⎡​ωxb​​ωbb​​ωbb​​​⎦⎤​

在多旋翼飞行过程中，俯仰角和滚转角一般很小，那么上式可以近似为：

$\left[ \begin{array}{c}{\dot{\phi}} \\ {\dot{\theta}} \\ {\dot{\psi}}\end{array}\right] \approx \left[ \begin{array}{c}{\omega_{x_{b}}} \\ {\omega_{y_{b}}} \\ {\omega_{z_{b}}}\end{array}\right]$⎣⎡​ϕ˙​θ˙ψ˙​​⎦⎤​≈⎣⎡​ωxb​​ωyb​​ωzb​​​⎦⎤​

姿态角还可以通过加速度计和磁力计的测量值得到；

加速度测量的是比力，满足下列形式

$\left[ \begin{array}{l}{a_{x_{b}}} \\ {a_{y_{b}}}\end{array}\right]=\left[ \begin{array}{c}{\dot{v}_{x_{b}}+g \sin \theta} \\ {\dot{v}_{y_{b}}-g \cos \theta \sin \phi}\end{array}\right]$[axb​​ayb​​​]=[v˙xb​​+gsinθv˙yb​​−gcosθsinϕ​]

我们假设多旋翼飞行器工作在低速状态下，上式可近似为：

$\left\{\begin{array}{l}{a_{x_{b}} \approx g \sin \theta} \\ {a_{y_b} \approx-g \cos \theta \sin \phi}\end{array}\right.${axb​​≈gsinθayb​​≈−gcosθsinϕ​

三轴加速度计固连在多旋翼机体上，其坐标轴与机体坐标轴一致。因此，俯仰角和滚转角观测量可以由加速度计测量值近似得到：(得到的是姿态角的低频分量，因为低速运动的假设，运动模态集中在低频段)

$\left\{\begin{array}{c}{\theta_{\mathrm{m}}=\arcsin \left( \begin{array}{c} \frac{a_{x_{\mathrm{b}} \mathrm{m}}}{g}\end{array}\right)} \\ {\phi_{\mathrm{m}}=-\arcsin \left( \begin{array}{c}\frac {a_{y_{\mathrm{b}} \mathrm{m}}} {g \cos \theta_{\mathrm{m}}}\end{array}\right)}\end{array}\right.$⎩⎨⎧​θm​=arcsin(gaxb​m​​​)ϕm​=−arcsin(gcosθm​ayb​m​​​)​

其中

$^{\mathrm{b}}{\mathrm{a}_m}=\left[ \begin{array}{lll}{a_{x_{\mathrm{b}}m}} & {a_{y_{\mathrm{b}}m} } & {a_{z_{\mathrm{b}} \mathrm{m}}}\end{array}\right]^{\mathrm{T}}$bam​=[axb​m​​ayb​m​​azb​m​​]T

偏航角测量原理：

假设磁力计测量值为

$\mathbf{b}_{\mathbf{m}_{\mathrm{m}}}=\left[ \begin{array}{lll}{m_{x_{\mathrm{b}}}} & {m_{y_{\mathrm{b}}}} & {m_{z_{\mathrm{b}}}}\end{array}\right]^T$bmm​​=[mxb​​​myb​​​mzb​​​]T。考虑到磁力计可能不是水平放置，所以需要利用俯仰角与滚转角将磁力计的测量值投影到水平面。

$\left\{\begin{array}{l}{m_{x_{c}}=m_{x_{b}} \cos \theta_{m}+m_{y_{b}} \sin \phi_{m} \sin \theta_{m}+m_{z_{b}} \cos \phi_{m} \sin \theta_{m}} \\ {m_{y_{c}}=m_{y_{b}} \cos \phi_{m}-m_{z_{b}} \sin \phi_{m}}\end{array}\right.${mxc​​=mxb​​cosθm​+myb​​sinϕm​sinθm​+mzb​​cosϕm​sinθm​myc​​=myb​​cosϕm​−mzb​​sinϕm​​

其中$m_{x_{\mathrm{c}}}, {m}_{y_{\mathrm{e}}} \in \mathbb{R}$mxc​​,mye​​∈R表示磁力计读数在水平面内的投影。定义$\psi_{\operatorname{mag}} \in[-\pi, \pi]$ψmag​∈[−π,π]，则有：

$\psi_{\mathrm{mag}}=\arctan 2\left(m_{y_{\mathrm{c}}}, m_{x_{\mathrm{e}}}\right)$ψmag​=arctan2(myc​​,mxe​​)

三轴陀螺仪的动态响应快，测量精度高，但求解姿态角时需要对角速度积分，会产生累积误差；三轴加速度计和三轴磁力计可以得到稳定的姿态角，无累积误差，但是动态响应差，测量噪声大。

高低通滤波(即线性互补滤波器)的基本思想是利用各自的特征得到更稳定的姿态角

下面以俯仰角为例来介绍高低通滤波器的一般设计方法：

俯仰角$\theta \in \mathbb{R}$θ∈R的拉氏变换表示为：

$\theta(s)=\frac{1}{\tau s+1} \theta(s)+\frac{\tau s}{\tau s+1} \theta(s)$θ(s)=τs+11​θ(s)+τs+1τs​θ(s)

其中，$\frac{1}{\tau s+1}$τs+11​表示低通滤波器的传递函数，$\tau \in \mathbb{R}_{+}$τ∈R+​表示时间常数，$\frac{\tau s}{\tau s+1}$τs+1τs​表示高通滤波器的传递函数。

考虑加速度计得到的俯仰角无漂移，但噪声大，可将俯仰角测量值建模为如下形式：

$\theta_{\mathrm{m}}=\theta+n_{\theta}$θm​=θ+nθ​

其中$n_{\theta} \in \mathbb{R}$nθ​∈R表示高频噪声，$\theta$θ表示俯仰角真值。考虑到角速度积分得到的俯仰角噪声小但漂移大，可以建模为：

$\frac{\omega_{y_{0} m}(s)}{s}=\theta(s)+c \frac{1}{s}$sωy0​m​(s)​=θ(s)+cs1​

其中$\omega_{y_{b} m}(s) / s$ωyb​m​(s)/s表示对角速度$\omega_{y_{b} m}$ωyb​m​进行积分得到的俯仰角的拉氏变换，$c/s$c/s表示常值漂移的拉氏变换，$\omega_{y_{b} m}$ωyb​m​是陀螺仪的测量值。因此针对俯仰角，高低通滤波器的标准形式为：

$\hat{\theta}(s)=\frac{1}{\tau s+1} \theta_{\mathrm{m}}(s)+\frac{\tau s}{\tau s+1}\left(\frac{1}{s} \omega_{y_{\mathrm{b}} \mathrm{m}}(s)\right)$θ^(s)=τs+11​θm​(s)+τs+1τs​(s1​ωyb​m​(s))

接下来详细说明高低通滤波能够精确估计姿态角的原理：

上式可以写为如下形式：

$\hat{\theta}(s)=\theta(s)+\left(\frac{1}{\tau s+1} n_{\theta}(s)+\frac{\tau s}{\tau s+1} c \frac{1}{s}\right)$θ^(s)=θ(s)+(τs+11​nθ​(s)+τs+1τs​cs1​)

可以看出高频噪声$n_{\theta}$nθ​通过低通滤波器$\frac{1}{\tau s+1}$τs+11​后基本衰减为0，低频信号$c/s$c/s通过高通滤波器$\frac{\tau s}{\tau s+1}$τs+1τs​后也基本衰减为0.因此可以认为：

$\hat{\theta}(s)\approx \theta(s)$θ^(s)≈θ(s)

在整个过程中，低通滤波器将$\theta_{\mathrm{m}}$θm​无漂移的优势保留了下来，高通滤波器将$\omega_{y_{b} m}(s) / s$ωyb​m​(s)/s噪声小的优势保留了下来。

接下来看高低通滤波具体的算法实现：

我们使用一阶向后差分法将滤波器转换为离散形式，即使用$s=\left(1-z^{-1}\right) / T_{\mathrm{s}}$s=(1−z−1)/Ts​，其中$T_{\mathrm{s}} \in \mathbb{R}_{+}$Ts​∈R+​表示滤波器的采样周期。这时，滤波器的形式变为如下：

$\hat{\theta}(z)=\frac{1}{\tau \frac{1-z^{-1}}{T_{\mathrm{s}}}+1} \theta_{\mathrm{m}}(z)+\frac{\tau}{\tau \frac{1-z^{-1}}{T_{\mathrm{s}}}+1} \omega_{\mathrm{ym}(z)}$θ^(z)=τTs​1−z−1​+11​θm​(z)+τTs​1−z−1​+1τ​ωym(z)​

将上式转化为离散时间差分方程的形式：

$\hat{\theta}(k)=\frac{\tau}{\tau+T_{\mathrm{s}}}\left(\hat{\theta}(k-1)+T_{\mathrm{s}} \omega_{\mathrm{y}_{\mathrm{b}} \mathrm{m}}(k)\right)+\frac{T_{\mathrm{s}}}{\tau+T_{\mathrm{s}}} \theta_{\mathrm{m}}(k)$θ^(k)=τ+Ts​τ​(θ^(k−1)+Ts​ωyb​m​(k))+τ+Ts​Ts​​θm​(k)

令$\alpha = \frac{\tau}{\tau+T_{\mathrm{s}}}$α=τ+Ts​τ​，上式可以写成;

$\hat{\theta}(k)=\alpha \left(\hat{\theta}(k-1)+T_{\mathrm{s}} \omega_{y_{\mathrm{b}} m}(k)\right)+(1-\alpha) \theta_{\mathrm{m}}(k)$θ^(k)=α(θ^(k−1)+Ts​ωyb​m​(k))+(1−α)θm​(k)

下面使用传感器实测的数据来验证高低通滤波的实际效果(低通滤波器截止频率设置为$50Hz$50Hz，传感器采样频率为$200Hz$200Hz)

可以看到陀螺仪直接积分得到的俯仰角直接发散掉了，而通过加速度计直接计算得到的俯仰角含有大量噪声，高低通滤波器输出的俯仰角则融合了上述两种方法各自的长处，精确得到了俯仰角的测量值。