LQR的理解与运用第二期——一阶倒立摆在matlab上的LQR实现

学书才浅

已于 2024-01-04 17:05:57 修改

阅读量2.8k

点赞数 24

分类专栏：机器人学? 文章标签： matlab 开发语言图像处理机器人

于 2023-03-04 11:55:02 首次发布

本文链接：https://blog.csdn.net/weixin_51772802/article/details/128901047

版权

机器人学? 专栏收录该内容

7 篇文章 8 订阅

订阅专栏

0.本系列目的

理解与运用LQR

参考教程

matlab动力学建模与simscape验证
https://www.bilibili.com/video/BV1h44y1m7ca/

笔者是跟着B站上这个教程做的，收获颇丰。如果你和笔者一样，之前从未接触过simscape，那么up的讲解一定会让你对simscape的使用有了初步了解。 【强烈推荐观看】

1 理解

见上一期LQR的理解与运用第一期——理解篇

2 运用

在solidworks上创建一阶倒立摆模型并导出

所需建立的模型如下图所示
在这里插入图片描述

创建步骤

创建两个零件，分别代表滑块和杆子
新建装配体将两个零件按照适当配合进行装配，如图

后面在matlab里可以单独设置每个零件的质量，因此不用太在意滑块和杆子的尺寸 参考这篇

这里一共进行了四处配合，分别限制了滑块y，z方向移动，以及令摆杆边线与滑块突起处重合【即同轴心】，以及限制了滑块的滑动范围（如图所示滑块长度为400mm；并限制滑块的移动范围在0~40000mm之间，并把滑块放到了20000mm处）
在这里插入图片描述

这样整个机构就只有滑块的运动副和摆杆的转动副两个自由度了

但是后来笔者发现这么做这个插件并不会识别到滑块的运动副，而是会认为滑块是固定的，因此删去了第四个配合

倒立摆模型中，初始状态应该保证杆子竖直向上，如上图

为了实现竖直效果，可以先加一个配合，使摆杆的侧面与滑块的侧面平行，然后调整滑块位置，最后删除该配合

如果需要导出urdf模型，你需要额外设置每个关节的轴和运动类型【参考链接】
如果需要导出xml 模型，可以直接导出，不需要进行额外设置【参考链接】
按照上述导出xml模型的参考链接，修改模型的重力方向，各零件质量并添加观测值（位移 $x$ ， $\theta$ ，速度 $v$ ， $\omega$ ）

最好同时修改零件的颜色，不然模型颜色和背景色一致，将很难分辨
也可以在matlab中导入模型后再设置

一阶倒立摆的模型及物理公式推导

模型介绍

本篇使用的一阶倒立摆模型具有两个自由度，一个是绕轴枢转动的转动副，一个是滑块的移动副；
且该模型仅具有一个驱动，即移动副所对应轴，驱动作用效果用作用力F体现

该模型仅有两个稳定点，分别在 $\theta=0°$ 和 $\theta=180°$ 时。

但是在后面仿真时，当摆杆处以恒定驱动作为扰动时， $\theta$ 的稳定值会变化

模型推导

采用以下模型
在这里插入图片描述
变量声明

物理量	符号
滑块质量	$M$
摆杆质量	$m$
摆杆转动轴心到杆质心长度【半杆长】	$l$
摆杆在转轴处转动惯量	$J$
小车受到外力	$F$
小车位置	$x$
摆杆与竖直方向夹角【本文顺时针为正】	$\theta$

模型推导方法

看【工具篇】拉格朗日动力学建模及系统设置初值求变量，用牛顿-欧拉公式或者拉格朗日动力学公式推导

化简方法

完成公式推导后，我们有了以下两个式子【对应工具篇的(6) (7)】：
$\left\{\begin{array}{l} {m g l \sin \theta}-{m l \cos\theta} \cdot\ddot{x}=(J+{m l^2 }) \cdot\ddot{\theta} \\\\ -ml\sin\theta\cdot \dot{\theta}^{2}+m l \cos\theta\cdot\ddot{\theta}+(M + m)\cdot{\ddot{x}}=F \end{array}\right.$

进行化简，在平衡点附近，有
$\dot{\theta} \approx 0 \quad \cos \theta \approx 1 \quad \sin \theta \approx \theta$

化简后，两方程如下：（输入 $u = F$ ）
$\left\{\begin{array}{l} \left(J+m l^{2}\right) \ddot{\theta}-m g l \theta=-m l \ddot{x} \\ (M+m) \ddot{x}+m l \ddot{\theta}=u \end{array}\right.$

取：
$x=\left[\begin{array}{l} x_{1} \\ x_{2} \\ x_{3} \\ x_{4} \end{array}\right]=\left[\begin{array}{c} \theta \\ \dot{\theta} \\ x \\ \dot{x} \end{array}\right],u=F$
根据上面的两个方程，化简二元一次方程，可得 $\ddot x,\ddot \theta$
$\left\{\begin{array}{l} \dot{x}_{1}=x_{2} \\ \dot{x}_{2}=\frac{m g l(M+m)}{J(M+m)+M m l^{2}} x_{1}+\frac{-m l}{J(M+m)+M m l^{2}} u \\ \dot{x}_{3}=x_{4} \\ \dot{x}_{4}=\frac{-m^{2} g l^{2}}{J(M+m)+M m l^{2}} x_{1}+\frac{J+m l^{2}}{J(M+m)+M m l^{2}} u \end{array}\right.$

结论

因此状态方程中 $A 、 B 、 C 、 D$ 四个矩阵的表达式可求出
$A=\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ \frac{-m g l ( M + m )}{J(M+m)+M m l^{2}} & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ \frac{-m^{2} g l^{2}}{J(M+m)+M m l^{2}} & 0 & 0 & 0 \end{array}\right]$

$B=\left[\begin{array}{l} 0\\ \frac{- m l}{J(M+m)+M m l^{2}} \\ 0 \\ \frac{J+m l^{2}}{J(M+m)+M m l^{2}} \end{array}\right]$

$C=\left[\begin{array}{llll} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}\right]$

$D = 0$

根据模型求LQR的K值

g = 9.80665;
m = 5;  % 摆杆质量
M = 1;  % 滑块质量
l = 0.18;  % 半杆长
J = 1/3 * m * (2 * l)^2;

% ------------------------------------
A21 = m*g*l*(M+m)/(J*(M+m)+M*m*l^2)
A41 = -m^2*g*l^2/(J*(M+m)+M*m*l^2)
A = [0  1  0  0;
    A21 0  0  0;
     0  0  0  1;
    A41 0  0  0];
% [theta, dtheta, x, dx]

B2 = -m*l/(J*(M+m)+M*m*l^2);
B4 = (J+m*l^2)/(J*(M+m)+M*m*l^2);
B = [0;B2;0;B4];

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

D=[0;0];
% ------------------------------------
%% sys = ss(A,B,C,D)
eig(A)
%% Qc=ctrb(A,B)
%% rank(Qc)
Qb=obsv(A,C)
rank(Qb)
% ------------------------------------
Q = [100 0 0 0
     0 1 0 0
     0 0 10 0
     0 0 0 1];
R = 0.1;
K = lqr(A,B,Q,R)
% ------------------------------------
Ac=A-B*K;
x0 = [0;0;0;0]; %初始状态
t = 0:0.05:20;
u = zeros(size(t));
[y,x]=lsim(Ac,B,C,D,u,t,x0);
plot(t,y);

matlab仿真的实现流程与步骤

准备步骤

这一部分操作如果看不懂可以参考上文中提到的打开xml的链接。

正确导入xml模型

这里令滑块质量 $M = 1 k g$ ,摆杆质量 $m = 5 k g$ ，仿真时间设置为 $20 s$ ，修改重力方向，添加观测值（位移 $x$ ， $\theta$ ，速度 $v$ ， $\omega$ ）
完成后效果如图
在这里插入图片描述
运行后输出结果如图【位移的单位是10^-13，目前可忽略】

这里看到theta的值大约是 $\pi$ ，因此要在模型中减去一个常数 $\pi$ 【因为初始情况下theta应该是0】

此外，应该保证 $\theta$ 和 $x$ 的正方向和我们建模时的正方向相同，上面建模时我们将顺时针方向的 $\theta$ 视为正值，而matlab默认以逆时针为正，因此需要对 $\theta$ 乘负号； $x$ 的方向一致

此处两个值的正负建议读者自行验证，否则到时候LQR算出来的K值，还需要修改其中两个维度的正负号才能符合模型

在这里插入图片描述

此外本文模型杆子质心到转轴距离 $l = 0.18 m$ 【注意是杆长的一半！】

设置扰动及输入

为转动轴和移动副设置输入【actuator】

转动副
输入为转矩【Torque】，而动量设置为自动计算。
这里的输入不可控，因此设置为0，或者设置成白噪声，以模拟干扰

如图所示

设置扰动的方法补充说明

事实上，这种扰动的设置方法并不科学，因为这种方法是直接加在关节上的，这份力矩还是会对系统产生作用【是内力不是外力，我们要模拟的是外力】
举个例子
比如把开关拨到常数档，然后给一个较小的常数（如：0.5）
设置好K值这些后，再进行模拟，就会出现一些很反常的现象，如：

这是因为给了摆杆一个力矩为0.5的转矩，与滑块的外力平衡了
移动副
输入为力【Force】，而动量设置为自动计算。
这里的输入根据LQR计算，将输入线先拉远一点，到时候要将里面封装成一个模块

如图所示

封装模块并实现LQR控制

LQR控制指的就是输入满足 $u = K x$ 过程的控制，详见下文

封装模块
在四个输出处另外设置四个output
【注：此图及后面的图还没对 $\theta$ 和 $\omega$ 进行系数矫正，是因为截图是还没注意到这个错误】

然后将除了一个input和四个output的全部模块 封装成一个子系统
填入LQR的K值
根据四个自变量的顺序，调整K的顺序，满足 $u = K x$

删掉外面的四个output模块和一个input模块，加入一个Mux模块和Gain模块，将K值填入第一个Gain模块的增益上，并把乘法形式改成矩阵 K*u

再加一个反馈模块，用来强调 $u = K x$ ；通过ctrl+r改变Gain模块方向，最后效果如图