卡尔曼滤波在一阶倒立摆中的应用
一、一阶倒立摆系统的建模
一阶倒立摆系统的结构示意图如图1所示
本系统内部各相关参数定义如下：
M 小车质量
m 摆杆质量
b 小车摩擦系数
l 摆杆转动轴心到杆质心的长度
I 摆杆惯量
F 加在小车上的力
x 小车位置
? 摆杆与垂直向上方向的夹角 图一 一阶段倒立摆系统的组成
? 摆杆与垂直向下方向的夹角（考虑到摆杆初始位置为竖直向下）
应用Newton经典力学对系统进行受力分析，图二所示：
图二 系统的受力分析
小车水平方向所受的合力，可以得到以下方程：
??F?bx??N M?x由摆杆水平方向的受力进行分析可以得到下面等式：
d2N?m2(x?lsin?)
dt??cos??ml??2sin? ???ml?即：N?mx把这个等式代入上式中，就得到系统的第一个运动方程：
??cos??ml??2sin??F （1-1） ??bx??ml?(M?m)?x为了推出系统的第二个运动方程，我们对摆杆垂直方向上的合力进行分析，
可以得到下面方程：
d2P?mg??m2(lcos?)
dt??sin??ml??2cos? 即：P?mg?ml?力矩平衡方程如下：
?? ?Plsin??Nlcos??I?注意：此方程中力矩的方向，由于?????,cos???cos?,sin???sin?，故等式前面有负号。
合并这两个方程，约去P和N，得到第二个运动方程：
???mglsin???ml??cos? （1-2） (I?ml2)?x设?????，当摆杆与垂直向上方向之间的夹角?与1（单位是弧度）相比
d?2)?0。dt这里用u来代表被控对象的输入力F，线性化后得到该系统数学模型的微分方程表达式：
很小，即 ???1 时，则可以进行近似处理：cos???1，sin????，(2?????mgl??ml?x?I?ml? (1-3) ??????bx??ml??ux?(M?m)???
由现代控制理论原理可知，控制系统的状态空间方程可写成如下形式：
??AX?BuX
Y?CX?Du??解方程，可以得到： ?,?方程组（1-3）对?x??x??x?2222?(I?ml)bmgl(I?ml)?????xx???u222?I(M?m)?MmlI(M?m)?MmlI(M?m)?Mml? ?????????mlbmgl(M?m)ml????x???u??222?I(M?m)?MmlI(M?m)?MmlI(M?m)?Mml?整理后得到系统状态空间方程：
?0????x?????0x???????0?????????????0?
1?(I?ml2)bI(M?m)?Mml20?mlbI(M?m)?Mml20mgl2I(M?m)?Mml20mgl(M?m)I(M?m)?Mml220?0??2x?????I?ml0??x??2??I(M?m)?Mml????u??1?????0?ml??????0??????I(M?m)?Mml2??????x?????x??1000??x???0?u Y????????????0010?????0??????????为上式就是一阶倒立摆小车系统的状态空间方程，式中：x为小车的位移；x?为摆杆的角速度；u为输入；Y为输出。 小车的速度；?为摆杆的角度；?假设:
M 小车质量
0.5 Kg
m 摆杆质量
0.2 Kg
b 小车摩擦系数 0.1 N/m/sec
l 摆杆转动轴心到杆质心的长度 0.3 m I 摆杆惯量
T 采样时间
0.006 kg*m*m
0.005s
利用Matlab,我们可以求出
A =
0 1.0000 0 0 0 -0.1818 2.6727 0 0 0 0 1.0000 0 -0.4545 31.1818 0
B = 0 1.8182 0 4.5455 C =
1 0 0 0 0 0 1 0 D = 0 0
而系统的能控矩阵的秩rank[BABA2BA3B]?4。
系统的能观矩阵的秩rank[CCACA2CA3]?4。故系统是能控能观的。
取采样时间为T?0.1s，在Matlab下将状态方程离散化，得：
?1.0000 0.0991 0.0136 0.0005?? 0 0.9818 0.2789 0.0136?? Ad??? 0 ?0.0023 1.1598 0.1053??? 0 ?0.0474 3.2764 1.1598???0.0091??0.1822?
? Bd??
?0.0232???0.4743???1 0 0 0? Cd????0 0 1 0?此时，最优控制原理，可以求得：（参考最优控制作业）
K???54.7723,?30.1561,87.6758,17.4019?
以上过程并未考虑噪声影响，而噪声是实际存在的，因此，可采用卡尔曼滤波器消除噪声影响。
假设受干扰系统由下述的状态方程描述：
??Ax?Bu?Gwx
y?Cx?v其中，G??0 ?0.25 0 0?为系统噪声输入矩阵。假设系统噪声w和测量噪声v为均值为0，方差已知，且相互独立。
此时，卡尔曼滤波器为：
T????A?LC?x??Bu?Lyx??Cx?y AP?PA?T?1PTCRC?P
其中，L?PCTR?1,P可以由求解下述Raccati方程得到：
TGQ0? GR和Q为系统噪声和测量噪声的方差，式中，在这里，我们选择Q?R?1。
利用Matlab当中的kalman函数，我们可以求得
L?[?390. 5 ? T1172. 6 9. 2 这时，系统应由三部分组成，即受控系统，最优控制系统，卡尔曼滤
波器。此时的合成系统为：
???A ?BK?x????? A?L?C???LC?x
???x???????Bx??K??BN?barr?B?Nbar
?x?y?[C 0?]????x在Matlab下对合成系统进行仿真，可以得到：
由图可知，系统具有较小的超调量，较快的响应时间，且利用卡尔曼
滤波器以后，控制输出非常平滑。