一种倒立摆的无迹卡尔曼滤波控制方法

技术领域

本发明属于倒立摆控制技术领域，具体涉及一种倒立摆的无迹卡尔曼滤波控制方法。

背景技术

倒立摆系统的最初分析研究始于二十世纪五十年代，是一个比较复杂的不稳定、多变量、带有非线性和强耦合特性的高阶机械系统。倒立摆系统存在严重的不确定性，一方面是系统的参数的不确定性，一方面是系统受到不确定因素的干扰。近些年来，国内外不少专家、学者一直将它视为典型的研究对象，提出了很多控制方案，对倒立摆系统的稳定性和镇定问题进行了大量研究，都在试图寻找不同的控制方法实现对倒立摆的控制，以便检查或说明该方法的严重非线性和绝对不稳定系统的控制能力，其控制方法在军工、航天、机器人领域和一般工业过程中都有着广泛的用途，如精密仪器的加工、机器人行走过程的平衡控制、火箭发射中的垂直度控制、导弹拦截控制、航空对接控制、卫星飞行中的姿态控制等方面均涉及到倒置问题。因此，从控制这个角度上讲，对倒立摆的研究在理论和方法论上均有着深远意义。倒立摆系统是一个典型的自不稳定系统，其中摆作为一个典型的振动和运动问题，可以抽象为许多问题来研究。随着非线性科学的发展，以前的采用线性化方法来描述非线性的性质，固然无可非议，但这种方法很有局限性，非线性的一些本质特征往往不是用线性的方法所能体现的。非线性是造成混乱、无序或混沌的核心因素，造成混乱、无序或混沌并不意味着需要复杂的原因，简单的非线性就会产生非常的混乱、无序或混沌。在倒立摆系统中含有极其丰富和复杂的动力学行为，如分叉、分形和混沌动力学。

现有技术中针对倒立摆的控制有古典、现代及各种智能控制理论，诸如LQR控制、模糊控制、遗传算法控制、滑模变结构控制等，但各种控制实现过程中难免受到系统噪声、量测噪声、外界干扰和建模误差的影响，导致设计出的控制器抗干扰能力有限。近年来，也有诸如使用KF、EKF等方法设计的控制律，尽管抗干扰能力有所增强，但是KF需要线性化计算状态转移矩阵，会带来模型误差。EKF对非线性系统做了一阶线性化近似，然后再对系统进行卡尔曼滤波处理，进而实现了将卡尔曼滤波理论扩展到非线性系统领域的目的。但EKF方法只是取了非线性系统Taylor级数展开的一阶部分进行线性化近似，因此难免会产生近似误差，这在具有较高精度要求的环境中是不适用的。

发明内容

为解决上述问题，本发明公开了一种倒立摆的无迹卡尔曼滤波控制方法，解决了现有倒立摆控制算法响应时间、控制精度、鲁棒性略差，自抗扰能力有限的问题。

具体方案如下：

一种倒立摆的无迹卡尔曼滤波控制方法，其特征在于，包括如下步骤：首先对倒立摆原始状态分布的均值和方差做初始化处理，然后确定一个采样规则，再在原状态分布中按照所选规则确定一些Sigma点，使所确定的Sigma点与原状态分布的均值和协方差相等，将这些Sigma点经非线性系统函数做非线性变换得到一系列对应点集，最后再用这些变换得到的点集确定出非线性变换后状态均值和协方差。

基于状态方差阵对角相似分解采样策略的无迹变换基本步骤：已知非线性状态变换y＝f(x)，其中状态向量x为n维随机向量，假设系统状态均值为

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

式中，D＝diag(ζ

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

式中，

针对倒立摆的非线性连续模型进行离散化处理，选取零均值高斯白噪声作为系统过程噪声W(k)和量测噪声V(k)，已知倒立摆增广后状态向量和观测向量分别为x

式中，f

假设倒立摆模型中的过程噪声W(k)和量测噪声V(k)分别具有协方差阵Q和R，在不同时刻k，对于倒立摆的模型增广状态向量x

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

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

(3)计算一步预测均值及协方差矩阵，首先通过式(3.2)计算加权因子

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

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

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

式中，

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

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

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

本发明的有益效果是：本发明提供的基于无迹卡尔曼滤波的倒立摆控制方法、装置及系统，采用无迹卡尔曼滤波算法，与传统倒立摆控制算法相比，在控制器中采用无迹卡尔曼滤波，用UKF直接对非线性系统进行状态估计，无须对系统进行线性化处理，具有更理想的控制效果和抗干扰性。采用状态方差阵对角相似分解策略，避免了UKF算法无法收敛，计算负荷大、状态方差阵无法保持正定等问题。摆杆运动姿态预测更加准确，减小了倒立摆惯性与干扰影响，保持摆杆稳定。有效减小惯性与干扰影响，整个装置具有较高的可靠性和鲁棒性，结构简单，成本低廉。

附图说明

图1为倒立摆古典控制原理图。

图2为倒立摆古典控制简化后的二阶系统框图。

图3为倒立摆现代控制原理。

图4为本发明基于无迹卡尔曼滤波预测倒立摆状态的流程图。

图5为XZ-ⅡA型旋转式倒立摆系统的总体结构图。

图6为XZ-ⅡA型旋转式倒立摆系统的组成框图。

图7为动力学模型图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图1-3所示，现有技术中针对倒立摆的古典控制：旋臂由转轴处的直流力矩电机驱动，可绕转轴在垂直于电机转轴的铅直平面内转动；旋臂和摆杆之间由电位器的活动转轴相连，摆杆可绕转轴在垂直于转轴的铅直平面内转动；由电位器测量得到的2个角位移信号(旋臂与铅直线的夹角，摆杆和旋臂之间的相对角度)，作为系统的2个输出量被送入DSP控制器；由角位移的差分可得到角速度信号，然后根据PID调节算法调节PWM控制信号，并转化为电压信号提供给驱动电路，以驱动直流力矩电机的运动，通过电机带动旋臂的转动来控制摆杆的运动。现代控制(基于状态反馈的控制)：首先判断系统为可控，可观系统，然后根据状态反馈确定反馈控制律，利用极点配置的方法求反馈矩阵K，使系统闭环稳定。智能控制：基于蚁群算法、遗传算法、神经网络等智能优化算法设计控制律，使倒立摆系统闭环稳定。

如图4所示，本发明提供了一种倒立摆的无迹卡尔曼滤波控制方法，包括如下步骤：首先对倒立摆原始状态分布的均值和方差做初始化处理，然后确定一个采样规则，再在原状态分布中按照所选规则确定一些Sigma点，使所确定的Sigma点与原状态分布的均值和协方差相等，将这些Sigma点经非线性系统函数做非线性变换得到一系列对应点集，最后再用这些变换得到的点集确定出非线性变换后状态均值和协方差。

基于状态方差阵对角相似分解采样策略的无迹变换基本步骤：已知非线性状态变换y＝f(x)，其中状态向量x为n维随机向量，假设系统状态均值为

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

式中，D＝diag(ζ

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

式中，

针对倒立摆的非线性连续模型进行离散化处理，选取零均值高斯白噪声作为系统过程噪声W(k)和量测噪声V(k)，已知倒立摆增广后状态向量和观测向量分别为x

式中，f

假设倒立摆模型中的过程噪声W(k)和量测噪声V(k)分别具有协方差阵Q和R，在不同时刻k，对于倒立摆的模型增广状态向量x

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

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

(3)计算一步预测均值及协方差矩阵，首先通过式(3.2)计算加权因子

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

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

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

式中，

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

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

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

实施例

XZ-ⅡA型旋转式倒立摆系统，是一个典型的机电一体化系统，采用运动控制器和伺服电机进行实时运动控制。XZ-ⅡA型旋转式倒立摆系统采用直流力矩电机直接驱动和内置DSP芯片控制。可以脱离计算机直接运行，也可以通过串口通讯用计算机控制。图5是XZ-ⅡA型旋转式倒立摆系统的总体结构图。图6是XZ-ⅡA型旋转式倒立摆系统的组成框图。系统包括计算机、DSP控制器、驱动机构、倒立摆本体和位置检测元件几大部分，组成了一个闭环系统。

系统采用TMS320F240 DSP控制器为核心器件，能够独立执行实时控制算法，也可以通过RS-232C串行总线与计算机通讯，进行在线控制算法调试。它的工作原理是由电位器测量得到的2个角位移信号(旋臂与铅直线的夹角，摆杆和旋臂之间的相对角度)，作为系统的2个输出量被送入计算机。计算机根据一定的控制算法，计算出控制量，并转化为相应的电压信号提供给驱动电路，以驱动直流力矩电机的运动，通过电机带动旋臂的转动来控制摆杆的倒立和保持平衡。

倒立摆系统理论分析忽略各种摩擦等影响后，可抽象成图7所示的动力学模型。其中系统的主要机械参数及变量如下表所示。

其中，旋臂绕轴转动的转动惯量J1以及相应的摩擦力矩系数f1，摆杆绕轴转动的转动惯量J2以及相应的摩擦力矩系数f2。通过测量和计算J1＝0.004Kg·m

令θ1→0，θ2→0，则有线性化模型为：

令

系统的状态空间方程如下：

因为rank[B,AB,A2B,A3B]＝4，rank[C；CA；CA2；CA3]＝4，系统是完全可控和完全可观测的。

设计相应倒立摆UKF状态预测算法主要步骤如下：

(1)UKF参数及状态量初始化

首先通过公式(3.2)初始化相应的状态量和观测量加权因子

式中，

(2)计算Sigma点和时间更新

UKF算法通过一个确定的样本组来完成随机状态变量的非线性传递。设已知k时刻倒立摆的非线性系统外部输入信号为δ

(3)观测值更新

完成Sigma点的计算以及状态量的时间更新后，就需要对倒立摆的观测量进行更新。设在时刻k，Sigma点的系统观测预测值为z

选取系统的状态量

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。