倒立摆模型及其LQR位置跟踪控制详解

倒立摆模型及其LQR位置跟踪控制详解

引用

CSDN

1.

https://blog.csdn.net/weixin_55249340/article/details/140421091

倒立摆模型是一种经典的控制理论研究对象，广泛应用于机器人、航空航天等领域。LQR（线性二次调节器）控制方法则是一种常用的最优控制策略，能够实现系统的稳定控制。本文将详细介绍倒立摆模型的推导过程及其使用LQR进行位置跟踪控制的方法。

一、参数设定

倒立摆示意图如下：

其中各个符号如下：

二、受力分析

1. 小车水平方向：

F - N_车 - b₁ x^· = M x^··

2. 摆杆水平方向：

N_杆 = m d²(x + l sin θ) / dt² = m (x^·· + θ^·· l cos θ - θ^·2 l sin θ)

3. 摆杆竖直方向：

P_杆 - mg = m d²(l cos θ) / dt² = -m (θ^·· l sin θ + θ^·2 l cos θ)

4. 摆杆转动方向（对质心求矩）：

P_杆 l sin θ - N_杆 l cos θ - b₂ θ^· = I θ^··

5. 牛顿第三定律定律：

N_车 = N_杆
P_车 = P_杆

三、方程求解

1. 联立方程(1)、(2)、(5)，消去N_杆、N_车：

(M + m) x^·· + b₁ x^· + m (θ^·· l cos θ - θ^·2 l sin θ) = F

2. 联立方程(2)、(3)、(4)，消去P_杆、N_杆：

(I + ml²) θ^·· + b₂ θ^· + ml x^·· cos θ = mgl sin θ

3. 联立方程(7)、(8)，整理得：

θ^·· = [(M+m)b₂θ^· + ml((F + mlθ^·2sinθ - b₁x^·)cosθ - (M+m)g sin(θ))] / [m²l²cos²θ - (I+ml²)(M+m)]

x^·· = [(I+ml²)(F-b₁x^·) + ml[ml(lθ^·2sinθ - gcosθsinθ) + Iθ^·sinθ + b₂θ^·cosθ]] / [(I+ml²)(M+m) - m²l²cos²θ]

注意：上述方程均采用MATLAB进行推导，正确与否还有待验证！

四、模型线性化

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

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

当θ ≈ 0处，有：

cosθ = 0
sinθ = θ
θ^·2 = 0

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

θ^·· = 3(M+m)g / [(4M+m)l]θ - 3(M+m)b₂ / [(4M+m)ml²]θ^· + 3b₁ / [(4M+m)l]x^· - 3 / [(4M+m)l]F

x^·· = -3mg / (4M+m)θ + 3b₂ / [(4M+m)l]θ^· - 4b₁ / (4M+m)x^· + 4 / (4M+m)F

其中I = ml² / 3

取X = [θ θ^· x x^·]^T为状态变量，倒立摆状态空间模型如下：

X^· = AX + Bu
Y = CX

其中：

A = [0 1 0 0
3(M+m)g / [(4M+m)l] -3(M+m)b₂ / [(4M+m)ml²] 0 3b₁ / [(4M+m)l]
0 0 0 1
-3mg / (4M+m) 3b₂ / [(4M+m)l] 0 -4b₁ / (4M+m)]

B = [0
-3 / [(4M+m)l]
0
4 / (4M+m)]

C = [1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1]

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

1. LQR(Linear Quadratic Regulator)方法

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

J = ∫₀^∞ (E^TQE + u^TRu) dt = ∫₀^∞ [(X-Ref)^TQ(X-Ref) + u^TRu] dt

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

通过系列方法，最终求解黎卡提Riccati方程：

A^TP + PA - PBR^-1B^TP + Q = 0

得出K = R^-1B^TP

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

u = -K(X-Ref)

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

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

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

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

取模型中固有参数如下：

M = 1.0 kg
m = 0.5 kg
l = 0.5 m
b₁ = 0.3
b₂ = 0.0

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

A = [0 1 0 0
19.6133 0 0 0.4
0 0 0 1
-3.2689 0 0 -0.2667]

B = [0
-1.3333
0
0.8889]

C = [1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1]

设置Q，R如下：

Q = [20000 0 0 0
0 1 0 0
0 0 10000 0
0 0 0 1]

R = [0.001]

通过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