目录
低成本无人机状态估计与控制
第一章：无人机概论
介绍了无人机机器发展简史，论述了无人机的重要性，以及相较于传统有人机，无人机所具有的优势。
第二章：无人机的运动方程
讨论了坐标系、刚体运动方程的推导，无人机运动线性方程，以及线性系统状态空间的表现形式。
第三章：无人机导航系统
介绍了惯性导航、大气数据和卫星无线电导航系统、多普勒高度表和磁传感器。研究了无人机系统中，这些系统的测量故障及故障建模。
第四章：无人机动力学估计
给出了无人机最优线性卡尔曼滤波器（OKF）状态估计，研究了OKF的稳定性和自适应卡尔曼滤波的必要性。
最优线性卡尔曼滤波器
书上所说的最优卡尔曼滤波器（OKF），其实就是卡尔曼滤波器，其使用的滤波器增益值使得后验状态估计值误差数值的平方最小。
书上的公式也与普通卡尔曼滤波公式一样，只是其中多定义了新息与新息协方差（为之后卡尔曼滤波的自适应服务）。
状态预测 x ~ k / k − 1 = F k x ^ k − 1 / k − 1 + B k u k − 1 协方差预测 P k / k − 1 = F k P k − 1 / k − 1 F k T + Q k − 1 新息 e ~ k = z k − H k x ~ k / k − 1 新息协方差 P Δ k = ( H k P k / k − 1 H k T + R k ) 最优卡尔曼增益 K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 状态估计 x ^ k / k = x ~ k / k − 1 + K k e ~ k 协方差估计 P k / k = ( I − K k H k ) P k / k − 1 \begin{array}{|c|c|} \hline \text { 状态预测 } & \tilde{\boldsymbol{x}}_{k / k-1}=\boldsymbol{F}_k \hat{\boldsymbol{x}}_{k-1 / k-1}+\boldsymbol{B}_k \boldsymbol{u}_{k-1} \\ \hline \text { 协方差预测 } & \boldsymbol{P}_{k / k-1}=\boldsymbol{F}_k \boldsymbol{P}_{k-1 / k-1} \boldsymbol{F}_k^{\mathrm{T}}+\boldsymbol{Q}_{k-1} \\ \hline \text { 新息 } & \tilde{\boldsymbol{e}}_k=\boldsymbol{z}_k-\boldsymbol{H}_k \tilde{\boldsymbol{x}}_{k / k-1} \\ \hline \text { 新息协方差 } & \boldsymbol{P}_{\Delta k}=\left(\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}+\boldsymbol{R}_k\right) \\ \hline \text { 最优卡尔曼增益 } & \boldsymbol{K}_k=\boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}\left(\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}+\boldsymbol{R}_k\right)^{-1} \\ \hline \text { 状态估计 } & \hat{\boldsymbol{x}}_{k / k}=\tilde{\boldsymbol{x}}_{k / k-1}+K_k \tilde{\boldsymbol{e}}_k \\ \hline \text { 协方差估计 } & \boldsymbol{P}_{k / k}=\left(\boldsymbol{I}-\boldsymbol{K}_k \boldsymbol{H}_k\right) \boldsymbol{P}_{k / k-1} \\ \hline \end{array} 状态预测 协方差预测 新息 新息协方差 最优卡尔曼增益 状态估计 协方差估计 x~k/k−1=Fkx^k−1/k−1+Bkuk−1Pk/k−1=FkPk−1/k−1FkT+Qk−1e~k=zk−Hkx~k/k−1PΔk=(HkPk/k−1HkT+Rk)Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1x^k/k=x~k/k−1+Kke~kPk/k=(I−KkHk)Pk/k−1
卡尔曼滤波器稳定性
对于状态估计公式进行 z z z变换，求得其特征多项式为 z I − ( F k − K k H k F k ) \mathrm{z} \boldsymbol{I}-\left(\boldsymbol{F}_k-\boldsymbol{K}_k \boldsymbol{H}_k \boldsymbol{F}_k\right) zI−(Fk−KkHkFk)，根据其根在 z z z平面的位置判定其是否稳定。
用于 UAV 状态估计的 OKF
要估计的状态为 n=9 的向量：
x = [ Δ u Δ w Δ q Δ θ Δ h Δ β Δ p Δ r Δ ϕ ] \mathbf{x}=\left[\begin{array}{lllllllll} \Delta u & \Delta w & \Delta q & \Delta \theta & \Delta h & \Delta \beta & \Delta p & \Delta r & \Delta \phi \end{array}\right] x=[ΔuΔwΔqΔθΔhΔβΔpΔrΔϕ]
此时控制输入量为： u = [ Δ δ e Δ δ T Δ δ a Δ δ r ] T u=\left[\begin{array}{llll}\Delta \delta_{\mathrm{e}} & \Delta \delta_{\mathrm{T}} & \Delta \delta_{\mathrm{a}} & \Delta \delta_{\mathrm{r}}\end{array}\right]^{\mathrm{T}} u=[ΔδeΔδTΔδaΔδr]T
式中 Δ u \Delta u Δu和 Δ w \Delta w Δw分别为在 x x x轴和 z z z轴的速度分量的扰动； Δ h \Delta h Δh为平面高度的扰动； Δ p \Delta p Δp、 Δ q \Delta q Δq、 Δ r \Delta r Δr分别为绕 x x x、 y y y和 z z z轴的角速度扰动； Δ θ \Delta \theta Δθ、 Δ ϕ \Delta \phi Δϕ、 Δ β \Delta \beta Δβ分别为俯仰、滚转和侧滑角的扰动； Δ δ e \Delta \delta_{\mathrm{e}} Δδe、 Δ δ a \Delta \delta_{\mathrm{a}} Δδa、 Δ δ r \Delta \delta_{\mathrm{r}} Δδr分别为升降舵、副翼和方向舵偏转的扰动； Δ δ T \Delta \delta_{\mathrm{T}} ΔδT为推力的变化。
然后根据状态空间中的组合动力学形式，引入如下的 UAV 过程和观测模型：
x k = F k x k − 1 + B k u k − 1 + G k w k z k = H k x k + v k \begin{gathered}\boldsymbol{x}_k=\boldsymbol{F}_k \boldsymbol{x}_{k-1}+\boldsymbol{B}_k \boldsymbol{u}_{k-1}+\boldsymbol{G}_k \boldsymbol{w}_k \\ \boldsymbol{z}_k=H_k \boldsymbol{x}_k+\boldsymbol{v}_k\end{gathered} xk=Fkxk−1+Bkuk−1+Gkwkzk=Hkxk+vk
式中 F k \boldsymbol{F}_k Fk为系统的动态矩阵； B k \boldsymbol{B}_k Bk为控制分配矩阵； z k \boldsymbol{z}_k zk为测量向量； G k \boldsymbol{G}_k Gk为系统噪声的转换矩阵； H k \boldsymbol{H}_k Hk为测量矩阵，这里是一个9*9的单位矩阵； w k \boldsymbol{w}_k wk、 v k \boldsymbol{v}_k vk分别为系统过程和测量高斯白噪声：
E ( w k w j T ) = Q k δ k j E ( v k v j T ) = R k δ k j E ( w k v j T ) = 0 \begin{gathered}E\left(\boldsymbol{w}_k \boldsymbol{w}_j^{\mathrm{T}}\right)=\boldsymbol{Q}_k \boldsymbol{\delta}_{k j} \\ E\left(\boldsymbol{v}_k \boldsymbol{v}_j^{\mathrm{T}}\right)=\boldsymbol{R}_k \boldsymbol{\delta}_{k j} \\ E\left(\boldsymbol{w}_k \boldsymbol{v}_j^{\mathrm{T}}\right)=0\end{gathered} E(wkwjT)=QkδkjE(vkvjT)=RkδkjE(wkvjT)=0
式中 Q k \boldsymbol{Q}_k Qk为过程噪声协方差矩阵； R k \boldsymbol{R}_k Rk为测量噪声协方差矩阵； δ k j \boldsymbol{\delta}_{k j} δkj为 Kronecker 函数。而后利用卡尔曼滤波的更新公式进行状态估计。
卡尔曼滤波器自适应
设计自适应的一些方法：
- 多模态自适应估计（MMAE）：使用多组并行的，使用多组不同参数的卡尔曼滤波器来并行计算，得到对当前系统的状态估计，再分别计算出对量测量的估计，与真实的量测量相减得到残差，据此得到与实际模型的相似程度，利用假设检验算法计算出各个模型的条件概率，用来衡量各个卡尔曼滤波器的状态估计值的正确性，对各个状态估计值取概率加权平均值,从而形成对实际系统的混合状态估计。
[1]范双菲,赵方方,李夏菁等.基于SINS/CNS组合导航系统的多模型自适应估计算法[J].深空探测学报,2014,1(04):
- 通过新息或者残差序列的分析确定卡尔曼滤波器未知的噪声协方差。
-
基于新息的自适应估计：新息定义为 e ~ k = z k − H k x ~ k / k − 1 \tilde{\boldsymbol{e}}_k=\boldsymbol{z}_k-\boldsymbol{H}_k \tilde{\boldsymbol{x}}_{k / k-1} e~k=zk−Hkx~k/k−1，通过研究新息序列确定噪声的先验特征是否与其真实特征相符合。新息的序列为：
C ^ e k = 1 N ∑ j = k − N + 1 k e ~ j e ~ j T \hat{\boldsymbol{C}}_{e_k}=\frac{1}{N} \sum_{j=k-N+1}^k \tilde{\boldsymbol{e}}_j \tilde{\boldsymbol{e}}_j^{\mathrm{T}} C^ek=N1j=k−N+1∑ke~je~jT
式中： N N N为“滑动窗口的维度”。PS：指的是考虑的连续数据点数量。
在基于新息的方法中，之前预测的矩阵 R R R和 Q Q Q使用实时测量量进行更新。滤波器中的统计矩阵使用下式进行更新：R ^ k = C ^ e k − H k P k / k − 1 H k T Q ^ k = K k C ^ e k K k T \begin{gathered} \hat{\boldsymbol{R}}_k=\hat{\boldsymbol{C}}_{e_k}-\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}} \\ \hat{\boldsymbol{Q}}_k=\boldsymbol{K}_k \hat{\boldsymbol{C}}_{\boldsymbol{e}_k} \boldsymbol{K}_k^{\mathrm{T}} \end{gathered} R^k=C^ek−HkPk/k−1HkTQ^k=KkC^ekKkT
-
基于残差的自适应估计：直接更新受残差序列变化影响的测量协方差矩阵和/或系统噪声。
残差序列可以表示为v k = z k − H x ^ k / k \boldsymbol{v}_k=\boldsymbol{z}_k-\boldsymbol{H} \hat{\boldsymbol{x}}_{k / k} vk=zk−Hx^k/k
式中： N N N为“滑动窗口的维度”。
矩阵 R R R估计算法为R ^ k = C ^ v k + H k P k / k H k T \hat{\boldsymbol{R}}_k=\hat{\boldsymbol{C}}_{v_k}+\boldsymbol{H}_k \boldsymbol{P}_{k / k} \boldsymbol{H}_k^{\mathrm{T}} R^k=C^vk+HkPk/kHkT
矩阵 Q Q Q的估计算法为
Q ^ k = ( 1 N j ∑ k − N + 1 k Δ x j Δ x j T ) + P k / k − F P k − 1 / k − 1 F T \hat{\boldsymbol{Q}}_k=\left(\frac{1}{N_j} \sum_{k-N+1}^k \Delta \boldsymbol{x}_j \Delta \boldsymbol{x}_j^{\mathrm{T}}\right)+\boldsymbol{P}_{k / k}-\boldsymbol{F} \boldsymbol{P}_{k-1 / k-1} \boldsymbol{F}^{\mathrm{T}} Q^k=(Nj1k−N+1∑kΔxjΔxjT)+Pk/k−FPk−1/k−1FT
式中 Δ x k \Delta \boldsymbol{x}_k Δxk为状态修正序列（滤波器估计和外推估计之间的差）。
Δ x k = x ^ k / k − x ^ k / k − 1 \Delta x_k=\hat{x}_{k / k}-\hat{x}_{k / k-1} Δxk=x^k/k−x^k/k−1
不足：* 在使用基于残差或者基于新息方法对未知系统和测量噪声协方差矩阵进行估计时，新息和残差矩阵被使用了N个循环。这消耗了巨大的计算资源，使得确定一个合适的滑动窗口维度变得非常重要。
- 为了能够使用基于残差或新息的估计方法，测量的量值、类型和分布必须前后一致。否则，系统和测量噪声的协方差矩阵不能使用残差或新息矩阵进行估计。
- 基于新息的估计器可能会引起负协方差矩阵。这种情况在式 R ^ k = C ^ e k − H k P k / k − 1 H k T \hat{\boldsymbol{R}}_k=\hat{\boldsymbol{C}}_{e_k}-\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}} R^k=C^ek−HkPk/k−1HkT后一项比前一项大的时候出现。
-
- 通过概率方法进行噪声估计：周期性使用对未知噪声协方差的估计更新滤波器算法中的噪声结构。
- 通过迭代处理相同的值
PS：新息：观测值减去先验估计值的观测值
残差：观测值减去后验估计值（即最优估计值）的观测值
第五章：传感器故障时的无人机动力学估计
为解决估计系统任意故障（比如测量通道的反常的测量值、突然的漂移或者步间的变化等）导致测量值不可靠，滤波器会给出不正确的结果，并随时间发散的问题，要构建具备足够鲁棒性的滤波器。本章使用一个 MNSF（测量噪声比例因子）的 RKF（鲁棒卡尔曼滤波器）和使用多个 MNSF 的 RKF 算法进行对比，并将其应用到 UAV 的状态估计过程。
-
具有一个测量噪声比例因子的RKF：
RKF的基础是比较新息序列协方差的真实值和理论值。当测量系统运行条件和综合滤波器使用模型不一致时，卡尔曼滤波器增益随新息序列协方差矩阵不同而改变。这些情况下，新息序列协方差矩阵根据以下式子改变：P e k = H k P k / k − 1 H k T + S k R k (1) \boldsymbol{P}_{e k}=\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}+\boldsymbol{S}_k \boldsymbol{R}_k\tag{1} Pek=HkPk/k−1HkT+SkRk(1)
同时,卡尔曼增益变为：
K k = P k / k − 1 H k T ( H k P k / k − 1 H k T + S k R k ) − 1 (2) \boldsymbol{K}_k=\boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}\left(\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}+\boldsymbol{S}_k \boldsymbol{R}_k\right)^{-1}\tag{2} Kk=Pk/k−1HkT(HkPk/k−1HkT+SkRk)−1(2)
式中： S k \boldsymbol{S}_k Sk为 SMNSF（单个测量噪声比例因子）。
当由于测量系统运行条件发生显著变化，导致预测的观测量 H k x ~ k / k − 1 \boldsymbol{H}_k \tilde{\boldsymbol{x}}_{k / k-1} Hkx~k/k−1与实际的观测量 y k \boldsymbol{y}_k yk显著不同时，卡尔曼增益发生变化，即当滤波器误差真实值超出理论误差：tr { e ~ k e ~ k T } ⩾ tr { H k P k / k − 1 H k T + R k } (3) \operatorname{tr}\left\{\tilde{\boldsymbol{e}}_k \tilde{\boldsymbol{e}}_k^{\mathrm{T}}\right\} \geqslant \operatorname{tr}\left\{\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}+\boldsymbol{R}_k\right\}\tag{3} tr{e~ke~kT}⩾tr{HkPk/k−1HkT+Rk}(3)
滤波器必须鲁棒地运行。
当滤波器误差真实值超出理论误差时将式（1）代入式（3）得到tr { e ~ k e ~ k T } ⩾ tr { H k P k / k − 1 H k T } + S k tr { R k } \operatorname{tr}\left\{\tilde{\boldsymbol{e}}_k \tilde{\boldsymbol{e}}_k^{\mathrm{T}}\right\} \geqslant \operatorname{tr}\left\{\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}\right\}+\boldsymbol{S}_k \operatorname{tr}\left\{\boldsymbol{R}_k\right\} tr{e~ke~kT}⩾tr{HkPk/k−1HkT}+Sktr{Rk}
根据等式 tr { e ~ k e ~ k T } = e ~ k T e ~ k \operatorname{tr}\left\{\tilde{\boldsymbol{e}}_k \tilde{\boldsymbol{e}}_k^{\mathrm{T}}\right\}=\tilde{\boldsymbol{e}}_k^{\mathrm{T}} \tilde{\boldsymbol{e}}_k tr{e~ke~kT}=e~kTe~k， S k \boldsymbol{S}_k Sk可以写为
S k = e ~ k T e ~ k − tr { H k P k / k − 1 H k T } tr { R k } S_k=\frac{\tilde{\boldsymbol{e}}_k^{\mathrm{T}} \tilde{\boldsymbol{e}}_k-\operatorname{tr}\left\{\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}\right\}}{\operatorname{tr}\left\{\boldsymbol{R}_k\right\}} Sk=tr{Rk}e~kTe~k−tr{HkPk/k−1HkT}
即当系统故障，满足式（3）中的条件，比例因子 S k \boldsymbol{S}_k Sk会增加，使得卡尔曼增益减小，从而降低故障新息序列的影响。
正常情况下使用KF运行，故障情况下使用RKF运行，这个过程使用一种统计信息进行控制。引入两个假设：- γ 0 \gamma_0 γ0：系统正常运行。
- γ 1 \gamma_1 γ1：估计系统存在故障。
为了检测故障，定义如下一个统计函数：
β k = e ~ k T [ H k P k / k − 1 H k T + R k ] − 1 e ~ k \boldsymbol{\beta}_k=\tilde{\boldsymbol{e}}_k^{\mathrm{T}}\left[H_k P_{k / k-1} H_k^{\mathrm{T}}+R_k\right]^{-1} \tilde{\boldsymbol{e}}_k βk=e~kT[HkPk/k−1HkT+Rk]−1e~k
统计函数是一个具有 M M M自由度的 χ 2 \chi^2 χ2分布（卡方发布），这里 M M M为新息向量的维数。假如将重要性水平 α \alpha α选为
γ 0 : β k ⩽ χ α , M 2 ( ∀ k ) γ 1 : β k > χ α , M 2 ( ∃ k ) \begin{array}{ll} \gamma_0: \beta_k \leqslant \chi_{\alpha, M}^2 & (\forall k) \\ \gamma_1: \beta_k>\chi_{\alpha, M}^2 & (\exists k) \end{array} γ0:βk⩽χα,M2γ1:βk>χα,M2(∀k)(∃k)
可以找到门限值 χ α , M 2 \chi_{\alpha, M}^2 χα,M2。因此，当假设 γ 1 \gamma_1 γ1正确， β k \beta_k βk的统计值将比门限值 χ α , M 2 \chi_{\alpha, M}^2 χα,M2更大，比如：
P { χ 2 > χ α , M 2 } = α ( 0 < α < 1 ) P\left\{\chi^2>\chi_{\alpha, M}^2\right\}=\alpha \quad(0<\alpha<1) P{χ2>χα,M2}=α(0<α<1)
-
具有多个测量噪声比例因子的RKF：
对于具有多个变量的复杂系统，需要使用修正测量噪声协方差矩阵相关项的多个测量噪声比例因子构建矩阵，从而修正卡尔曼增益。
将比例矩阵 S k \boldsymbol{S}_k Sk添加到算法中1 μ ∑ j = k − μ + 1 k e ~ j e ~ j T = H k P k / k − 1 H k T + S k R k \frac{1}{\mu} \sum_{j=k-\mu+1}^k \tilde{\boldsymbol{e}}_j \tilde{\boldsymbol{e}}_j^{\mathrm{T}}=\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}+\boldsymbol{S}_k \boldsymbol{R}_k μ1j=k−μ+1∑ke~je~jT=HkPk/k−1HkT+SkRk
得到：
S k = ( 1 μ ∑ j = k − μ + 1 k e ~ j e ~ j T − H k P k / k − 1 H k T ) + R k − 1 \boldsymbol{S}_k=\left(\frac{1}{\mu} \sum_{j=k-\mu+1}^k \tilde{\boldsymbol{e}}_j \tilde{\boldsymbol{e}}_j^{\mathrm{T}}-\boldsymbol{H}_k \boldsymbol{P}_{k / k-1} \boldsymbol{H}_k^{\mathrm{T}}\right)+\boldsymbol{R}_k^{-1} Sk= μ1j=k−μ+1∑ke~je~jT−HkPk/k−1HkT +Rk−1
式中： μ \mu μ为滑动窗口的宽度。
当正常运行时，比例矩阵为单位矩阵， S k = I \boldsymbol{S}_k=I Sk=I。由于 μ \mu μ为有限数字，且使用计算机计算，其中隐含诸如估计误差和取整误差。得到的矩阵 S k \boldsymbol{S}_k Sk可能不是对角阵，也可能是对角元素为负或者小于 1 的值。为避免这样的情形，按照如下的规则组成比例矩阵：S ∗ = diag ( s 1 ∗ , s 2 ∗ , ⋯ , s n ∗ ) s i ∗ = max { 1 , S i i } ( i = 1 , n ) \begin{gathered}\boldsymbol{S}^*=\operatorname{diag}\left(s_1^*, s_2^*, \cdots, s_n^*\right) \\s_i^*=\max \left\{1, S_{i i}\right\} \quad(i=1, n)\end{gathered} S∗=diag(s1∗,s2∗,⋯,sn∗)si∗=max{1,Sii}(i=1,n)
鲁棒自适应方法的比较
四种测量故障场景中的比较：
- 瞬时异常测量值
- 连续测量偏差
- 测量噪声增量
- 零输出故障
传感器/执行器故障时的无人机动力学估计
本章研究了一种稳定条件下，传感器/执行器故障时
R
R
R和
Q
Q
Q自适应 RAKF。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
