基于Kalman滤波的一阶倒立摆控制

人形灭睡机

已于 2022-06-12 18:02:46 修改

阅读量1.5k

点赞数 5

文章标签： python

于 2022-04-21 17:21:29 首次发布

本文链接：https://blog.csdn.net/qq_56297952/article/details/124283021

版权

0、简介

0.1 主要步骤：

1、建立倒立摆的运动方程；

2、运动方程线性化；

3、系统状态空间建模；

4、对运动方程离散化；

5、根据离散化后的模型设计Kalman滤波器和相应的控制器。

0.2 Kalman滤波器构成

0.3 一阶倒立摆模型示意图

1、一阶倒立摆系统的运动方程：

$\left\{ \begin{array}{l} \left( m+m_0 \right) \ddot{x}+ml\cos \theta \ddot{\theta}=F+ml\dot{\theta}^2\sin \theta\\ ml\cos \theta \ddot{x}+\left( ml^2+J \right) \ddot{\theta}=mgl\sin \theta\\ \end{array} \right.$

m——摆杆质量（kg）；

m0——滑块的质量（kg）；

g——重力加速度（m/s^2）；

J——摆杆转动惯量（kg x m2），数值为1/3 x ml^2;

l——摆杆半长（m）。

2、运动方程线性化

由于倒立摆稳定的工作点附近的摆角 $\theta$ 为小量，故可将运动方程在平衡点 $\theta=0$ 附近进行线性化，即：

$\left\{ \begin{array}{l} \dot{\theta}^2\approx 0\\ \sin \theta \approx \theta\\ \cos \theta \approx 1\\ \end{array} \right.$

得到的线性化后的数学模型的微分方程为 $^{\left[ 1 \right]}$ ：

$\left\{ \begin{array}{l} \left( m+m_0 \right) \ddot{x}+ml\ddot{\theta}=F\\ ml\ddot{x}+\left( ml^2+J \right) \ddot{\theta}=mgl\theta\\ \end{array} \right.$

3、状态空间建模

取 $\left[ x,\ \theta ,\ \dot{x},\ \dot{\theta} \right] ^T$ 为状态量，F为输入， $\left[ x,\ \theta ,\ \dot{x},\ \dot{\theta} \right] ^T$ 为输出，列写系统的状态空间方程：

状态方程：

$\left[ \begin{array}{c} \dot{x}\\ \dot{\theta}\\ \ddot{x}\\ \ddot{\theta}\\ \end{array} \right] =\left( \begin{matrix} 0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& -\frac{3mg}{4m_0+m}& 0& 0\\ 0& \frac{3\left( m_0+m \right) g}{\left( 4m_0+m \right) l}& 0& 0\\ \end{matrix} \right) \left[ \begin{array}{c} x\\ \theta\\ \dot{x}\\ \dot{\theta}\\ \end{array} \right] +\left[ \begin{array}{c} 0\\ 0\\ \frac{4}{4m_0+m}\\ -\frac{3}{\left( 4m_0+m \right) l}\\ \end{array} \right] F$

输出方程：

$y=\left[ \begin{array}{c} x\\ \theta\\ \dot{x}\\ \dot{\theta}\\ \end{array} \right] =\left[ \begin{matrix} 1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\ \end{matrix} \right] \left[ \begin{array}{c} x\\ \theta\\ \dot{x}\\ \dot{\theta}\\ \end{array} \right]$

上述状态空间表达式可以简写为：

$\left\{ \begin{array}{l} \dot{X}=AX+Bu\\ Y=CX\\ \end{array} \right.$

4、模型离散化

这里采用了一种最简单的离散化方法：

当两次状态的时间间隔 $\varDelta t$ 足够小时，状态方程可写为：

$\left( X_k-X_{k-1} \right) /\varDelta t=AX_{k-1}+Bu_{k-1}$

进一步化简得到：

$X_k=\left( I_{4\times 4}+\varDelta tA \right) X_{k-1}+\varDelta tBu_{k-1}$

输出方程为：

Y_k=CX_k

取 $\bar{A}=\left( I_{4\times 4}+\varDelta tA \right) ,\ \bar{B}=\varDelta tB$ ，将离散化后的状态空间方程写成符合卡尔曼滤波器的一般表达形式：，将离散化后的状态空间方程写成符合卡尔曼滤波器的一般表达形式：

$\left\{ \begin{array}{l} X_k=\bar{A}X_{k-1}+\bar{B}u_{k-1}+w_{k-1}\\ Z_k=HX_k+v_k\\ \end{array} \right.$

其中， $w_{k-1}$ 为系统噪声， v_k 为系统测量噪声。

至此，我们把原问题转化成了卡尔曼滤波可用的形式。

5、根据离散化后的模型设计Kalman滤波器和相应的控制器

Kalman滤波在这里起到了跟踪信号的作用，具体来说是为了评价测量值和预测值可信度更高，根据判断输出Kalman滤波器的估计值。所以还需要有一个控制器，判断跟踪的信号是否达到要求，如果没有，那么输出控制信号进行调节。

假设系统噪声和系统测量噪声均为白噪声（这种情况下Kalman滤波器可以说是最优滤波器），此外考虑我们的系统很健康，相比于测量噪声，系统噪声是个小量。所以我们这里算是简化问题为：利用卡尔曼滤波器克服测量白噪声的影响。

5.1 选择控制器

本程序选择了PD控制器，对于离散系统，PD控制器非常好设计。

$u\left( k \right) =K_p\left( e\left( k \right) +K_d\left( e\left( k \right) -e\left( k-1 \right) \right) \right)$

e(k)为k时刻的误差，在本系统中e(k)=theta；Kp为比例控制系数，Kd为微分控制系数；u(k)为应输入的控制量，在本系统中u(k)=F，对应于下一次循环的u(k-1)。

5.2 选择初值

这里初值和系统常数的选择均是参考了论文[1]，具体为:

m0	m	g	l
0.6	0.105	9.8	0.28

5.3 完整程序(Python)

import random
import numpy as np
import matplotlib.pyplot as plt

# super parameters
A = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [0, -1.2323, 0, 0], [0, 29.5509, 0, 0]], dtype=np.float64)
B = np.array([0, 0, 1.5968, -4.2772], dtype=np.float64).reshape(4, 1)
H = np.identity(4, dtype=np.float64)

d_T = 0.1
noise_Z = np.array([0, random.gauss(0, 3), 0, 0]).reshape(4, 1)

A_ = (H + d_T * A)
B_ = d_T * B

R = np.array([[0, 0, 0, 0], [0, 5, 0, 0], [0, 0, 0, 0], [0, 0, 0, 20]], dtype=np.float64)
Q = np.zeros(4, dtype=np.float64).reshape(4, 1)

X_0 = np.array([0, 45, 0, 0], dtype=np.float64).reshape(4, 1)
U_0 = 0
Z_0 = X_0 + noise_Z
P_0 = np.array([[1000, 0, 0, 0], [0, 1000, 0, 0], [0, 0, 1000, 0], [0, 0, 0, 1000]], dtype=np.float64)
E_0 = 0
E_1 = 0

EPOCH = 50
count = 0

ERROR = []
EPOCH_NUM = []

print("System Parameters:")
print("=====================================")
print("    m0  |   m   |   g   |   l   |   ")
print(" -----------------------------------")
print("    0.6 | 0.105 |  9.8  | 0.28  |")
print("=====================================")


print("***********Program is Running...**************")

class PID_Control:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
    def PD_Control(self, Err0, Err1):
        control = self.Kp * (Err1 + self.Kd * (Err1 - Err0))
        return control

pid = PID_Control(0.05, 0.2, 0.1)

for epoch in range(EPOCH):

    if epoch == 0:
        X = X_0
        U = U_0
        P = P_0
        Z = Z_0
    X_ = np.dot(A_, X) + B_ * U
    P_ = np.dot(np.dot(A_, P), A_.T) + Q
    Kk = np.dot(P_, np.linalg.inv(P_ + R))
    X = X_ + np.dot(Kk, Z - X_)
    P = np.dot(H - Kk, P_)
#  upgrade Z, U
    E_1 = X[1, 0]

    U = pid.PD_Control(E_0, E_1)
    Z = X + noise_Z

    E_0 = E_1
    ERROR.append(E_0)
    EPOCH_NUM.append(epoch)
    print("EPOCH= ", epoch, "\t| Error = ", E_0, "\t| Absolute Error=", abs(E_0))
    if abs(E_0) < 1e-04:
        count += 1
        if count == 10:
            break
print("****************Over!******************")
print("The error is:", abs(E_0), '\n')
print(EPOCH_NUM)
print(ERROR)

plt.axhline(0, linestyle='dotted', color='g')
plt.plot(EPOCH_NUM, ERROR, color='r')
plt.scatter(EPOCH_NUM, ERROR, color='b')
plt.show()

5.4 输出结果

横轴为时间t，纵轴为角度theta，选取了三个不同的初始偏转角度：

6、参考文献

[1]林乐天. 基于卡尔曼滤波器的一阶倒立摆控制研究[D]. 黑龙江:哈尔滨工业大学,2010. DOI:10.7666/d.D268331.

[2]于蕾,方蒽,纪雯.一阶倒立摆系统建模与仿真研究[J].电子世界,2021(15):25-26.DOI:10.19353/j.cnki.dzsj.2021.15.011.

人形灭睡机

关注

5
点赞
踩
28

收藏

觉得还不错? 一键收藏
1
评论
基于Kalman滤波的一阶倒立摆控制

Language Python Filter 普通Kalman滤波器 Kalman滤波器一阶倒立摆模型示意图主要步骤：1、建立倒立摆的运动方程（线性）；2、对运动方程离散化；3、根据离散化后的模型设计Kalman滤波器和相应的控制器。......
复制链接

扫一扫