[发明专利]一种倒立摆的无迹卡尔曼滤波控制方法

权利要求书

1.一种倒立摆的无迹卡尔曼滤波控制方法，其特征在于，包括如下步骤：

首先对倒立摆原始状态分布的均值和方差做初始化处理，然后确定一个采样规则，再在原状态分布中按照所选规则确定一些Sigma点，使所确定的Sigma点与原状态分布的均值和协方差相等，接着将这些Sigma点经非线性系统函数做非线性变换得到一系列对应点集，最后再用这些变换得到的点集确定出非线性变换后状态均值和协方差。

2.根据权利要求1所述的一种倒立摆的无迹卡尔曼滤波控制方法，其特征在于，基于状态方差阵对角相似分解采样策略的无迹变换基本步骤如下：

已知非线性状态变换y＝f(x)，其中状态向量x为n维随机向量，假设系统状态均值为方差为P，则UT变换得到Sigma点和相应权值的步骤如下：

(1)通过下式求得2n+1个采样点，n为系统模型状态的维数；

式中，D＝diag(ζ₁,ζ₂,ζ₃,...,ζ_n)，ζ_i(i＝1,2,...,n)为P的特征值组成的对角矩阵，(P_m)_i表示矩阵的第i列，f_eig函数的作用是求矩阵P的全部特征值，构成对角阵D，并求P的特征向量构成H的列向量；

(2)求解模型状态Sigma点相应的加权因子；

式中，为初始采样点的状态均值加权因子，为初始采样点的协方差加权因子，和为第i个采样点对应的状态均值和协方差加权因子；β≥0是一个用来合并系统方程中高阶项动差的非负权系数，β的存在可以把系统高阶项的影响包括在内，其中，对于高斯分布的情形β＝2最优；α为决定先验均值附近Sigma点分布广度的主要刻度参数，其取值范围为10^-4≤α≤1，可通过取不同α值实现对采样点分布状态的控制；κ是一个没有具体取值和界限的待选参数，但对于对称采样来说，其取值保证矩阵(n+λ)P是一个半正定矩阵，取κ＝0或3-n；λ＝α²(n+κ)-n是一个用来降低算法总的预测误差的缩放比例参数。

3.根据权利要求1所述的一种倒立摆的无迹卡尔曼滤波控制方法，其特征在于，针对倒立摆的非线性连续模型进行离散化处理，选取零均值高斯白噪声作为系统过程噪声W(k)和量测噪声V(k)，已知倒立摆增广后状态向量和观测向量分别为x_e和z，则离散化后的倒立摆非线性辨识系统模型可由式(3.3)和(3.4)描述，

式中，f_e表示倒立摆模型状态增广后的非线性过程函数，h_e表示倒立摆的观测函数；

假设倒立摆模型中的过程噪声W(k)和量测噪声V(k)分别具有协方差阵Q和R；在不同时刻k，对于倒立摆的模型增广状态向量x_e的无迹卡尔曼滤波算法基本实现步骤如下：

(1)通过下式确定倒立摆模型状态采样点，即Sigma点集；

(2)对所获取的采样点通过非线性变换函数进行一步预测；

(3)计算一步预测均值及协方差矩阵，首先通过式(3.2)计算加权因子和然后对采样点的一步预测值进行加权求和即可得到倒立摆状态一步预测的均值和协方差；

(4)对第(3)步得到的倒立摆状态一步预测均值进行UT变换，得到新的采样点；

(5)进行测量值更新，即对上一步获取的采样点集做量测变换，进而得到倒立摆状态的观测预测值；

(6)计算观测预测值均值及协方差，将上一步获取的倒立摆采样点观测预测值经过加权因子进行加权后求取对应均值及协方差；

式中，是倒立摆系统模型的观测方差；是倒立摆系统模型的状态观测协方差；

(7)利用上一步求出的协方差矩阵计算相应卡尔曼增益矩阵；

(8)更新系统状态量和协方差；

至此，完成对倒立摆模型状态均值和协方差的一次更新。