问小白 wenxiaobai
资讯
历史
科技
环境与自然
成长
游戏
财经
文学与艺术
美食
健康
家居
文化
情感
汽车
三农
军事
旅行
运动
教育
生活
星座命理

倒立摆模型推导与LQR位置跟踪控制详解

创作时间:
作者:
@小白创作中心

倒立摆模型推导与LQR位置跟踪控制详解

引用
CSDN
1.
https://blog.csdn.net/weixin_55249340/article/details/140421091

倒立摆控制问题是一个经典的控制理论问题,广泛应用于机器人、航天器姿态控制等领域。本文将详细介绍倒立摆模型的推导过程,并使用LQR(线性二次调节器)方法实现位置跟踪控制。通过MATLAB/Simulink仿真,验证控制算法的有效性。

一 参数设定

倒立摆示意图如下:

其中各个符号如下:

符号
含义
符号
含义
$M$
小车质量
$m$
摆杆质量
$b_1$
小车移动阻尼
$b_2$
摆杆转动阻尼
$x$
小车位置(水平向右为正)
$\theta$
摆杆摆动的角度(顺时针转动为正)
-
-
$l$
转动关节到摆杆质心的长度
$F$
作用到小车的外力(水平向右为正)
$I$
摆杆绕质心的转动惯量
$N_{车}$
摆杆对小车的力水平分量
$P_{杆}$
小车对摆杆的力竖直分量
$N_{杆}$
小车对摆杆的力水平分量
$P_{车}$
摆杆对小车的力竖直分量

二 受力分析

1. 小车水平方向:

$$
F - N_{车} - b_1 \dot{x} = M \ddot{x}
$$

2. 摆杆水平方向:

$$
N_{杆} = m \frac{d^2(x + l \sin \theta)}{dt^2} = m (\ddot{x} + \ddot{\theta} l \cos \theta - \dot{\theta}^2 l \sin \theta)
$$

3. 摆杆竖直方向:

$$
P_{杆} - mg = m \frac{d^2(l \cos \theta)}{dt^2} = -m (\ddot{\theta} l \sin \theta + \dot{\theta}^2 l \cos \theta)
$$

4. 摆杆转动方向(对质心求矩):

$$
P_{杆} l \sin \theta - N_{杆} l \cos \theta - b_2 \dot{\theta} = I \ddot{\theta}
$$

5. 牛顿第三定律定律:

$$
N_{车} = N_{杆}, \quad P_{车} = P_{杆}
$$

三 方程求解

1. 联立方程(1)、(2)、(5),消去$N_{杆}$、$N_{车}$:

$$
(M + m) \ddot{x} + b_1 \dot{x} + m (\ddot{\theta} l \cos \theta - \dot{\theta}^2 l \sin \theta) = F
$$

2. 联立方程(2)、(3)、(4),消去$P_{杆}$、$N_{杆}$:

$$
(I + ml^2) \ddot{\theta} + b_2 \dot{\theta} + ml \ddot{x} \cos \theta = mgl \sin \theta
$$

3. 联立方程(7)、(8),整理得:

$$
\begin{align}
\ddot{\theta} &= \frac{(M+m)b_2 \dot{\theta} + ml [(F + ml \dot{\theta}^2 \sin \theta - b_1 \dot{x}) \cos \theta - (M+m)g \sin \theta]}{m^2 l^2 \cos^2 \theta - (I + ml^2)(M+m)} \
\ddot{x} &= \frac{(I + ml^2)(F - b_1 \dot{x}) + ml [ml (l \dot{\theta}^2 \sin \theta - g \cos \theta \sin \theta) + I \dot{\theta} \sin \theta + b_2 \dot{\theta} \cos \theta]}{(I + ml^2)(M+m) - m^2 l^2 \cos^2 \theta}
\end{align}
$$

注意:上述方程均采用MATLAB进行推导,正确与否还有待验证!!!

四 模型线性化

三 方程求解的方程结果可知,倒立摆系统是一个非线性系统

为了便于实现后续的控制,可在工作点($\theta = 0$)附近,进行线性化,具体过程如下:

当$\theta \approx 0$处,有:

$$
\begin{align}
\cos \theta &= 0 \
\sin \theta &= \theta \
\dot{\theta}^2 &= 0
\end{align}
$$

带入(10)到三 方程求解中的非线性模型(9)中,简单整理可得:

$$
\begin{align}
\ddot{\theta} &= \frac{3(M+m)g}{(4M+m)l} \theta - \frac{3(M+m)b_2}{(4M+m)ml^2} \dot{\theta} + \frac{3b_1}{(4M+m)l} \dot{x} - \frac{3}{(4M+m)l} F \
\ddot{x} &= -\frac{3mg}{4M+m} \theta + \frac{3b_2}{(4M+m)l} \dot{\theta} - \frac{4b_1}{4M+m} \dot{x} + \frac{4}{4M+m} F
\end{align}
$$

其中$I = \frac{ml^2}{3}$

取$X = [\theta \quad \dot{\theta} \quad x \quad \dot{x}]^T$为状态变量,倒立摆状态空间模型如下:

$$
\begin{align}
\dot{X} &= AX + Bu \
Y &= CX
\end{align}
$$

其中:

$$
A = \begin{bmatrix}
0 & 1 & 0 & 0 \
\frac{3(M+m)g}{(4M+m)l} & -\frac{3(M+m)b_2}{(4M+m)ml^2} & 0 & \frac{3b_1}{(4M+m)l} \
0 & 0 & 0 & 1 \
-\frac{3mg}{4M+m} & \frac{3b_2}{(4M+m)l} & 0 & -\frac{4b_1}{4M+m}
\end{bmatrix}, \quad
B = \begin{bmatrix}
0 \
-\frac{3}{(4M+m)l} \
0 \
\frac{4}{4M+m}
\end{bmatrix}, \quad
C = \begin{bmatrix}
1 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 1 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}
$$

五 采用LQR方法进行全状态反馈控制

1. LQR(Linear Quadratic Regulator)方法

针对线性系统,定义状态误差加权和控制输入加权为代价函数$J$:

$$
J = \int_{0}^{\infty} (E^T Q E + u^T R u) dt = \int_{0}^{\infty} [(X - Ref)^T Q (X - Ref) + u^T R u] dt
$$

其中$Q$,$R$为正定或者半正定矩阵,$Ref$为参考的状态

通过系列方法,最终求解黎卡提$Riccati$方程:

$$
A^T P + PA - P B R^{-1} B^T P + Q = 0
$$

得出$K = R^{-1} B^T P$

最终得出使得代价$J$最小的控制律为:

$$
u = -K (X - Ref)
$$

在MATLAB中可使用函数lqr(lqr函数介绍)直接得出$K$:

调用$lqr(A,B,Q,R)$即可求解$K$

关于lqr方法的推导过程可参考lqr推导过程

2. 倒立摆全状态反馈控制模型仿真

取模型中固有参数如下:

$$
\begin{align}
M &= 1.0\ kg \
m &= 0.5\ kg \
l &= 0.5\ m \
b_1 &= 0.3 \
b_2 &= 0.0
\end{align}
$$

按照四 模型线性化中的模型得:

$$
A = \begin{bmatrix}
0 & 1 & 0 & 0 \
19.6133 & 0 & 0 & 0.4 \
0 & 0 & 0 & 1 \
-3.2689 & 0 & 0 & -0.2667
\end{bmatrix}, \quad
B = \begin{bmatrix}
0 \
-1.3333 \
0 \
0.8889
\end{bmatrix}, \quad
C = \begin{bmatrix}
1 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 1 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}
$$

设置$Q$,$R$如下:

$$
Q = \begin{bmatrix}
20000 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 10000 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}, \quad
R = \begin{bmatrix}
0.001
\end{bmatrix}
$$

通过MATLAB中lqr函数计算出反馈增益$K$:

$$
K = [-7470.90\ -1559.47\ -3162.27\ -2193.39]
$$

在simulink中搭建控制仿真模型如下:

输入计算出的反馈增益$K$仿真结果如下:

MATLAB代码和simulink模型点击这里

欢迎大家一起交流!!!

Reference:

  1. LQR控制算法推导-连续与离散形式:https://blog.csdn.net/zjh2883/article/details/136167154
  2. MATLAB LQR函数:https://ww2.mathworks.cn/help/control/ref/lti.lqr.html
© 2023 北京元石科技有限公司 ◎ 京公网安备 11010802042949号