0.本系列目的

理解运用LQR

参考教程

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

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

1 理解

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

2 运用

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

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

  • 创建步骤
  1. 创建两个零件,分别代表滑块杆子
  2. 新建装配体将两个零件按照适当配合进行装配,如图
    在这里插入图片描述

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

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

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

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

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

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

  1. 如果需要导出urdf模型,你需要额外设置每个关节的轴和运动类型【参考链接】
       如果需要导出xml 模型,可以直接导出,不需要进行额外设置【参考链接】

  2. 按照上述导出xml模型的参考链接,修改模型的重力方向各零件质量并添加观测值(位移 x x x θ \theta θ,速度 v v v ω \omega ω

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


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

模型介绍

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

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

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

模型推导

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

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

模型推导方法

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

化简方法

完成公式推导后,我们有了以下两个式子【对应工具篇的(6) (7)】:
{ m g l sin ⁡ θ − m l cos ⁡ θ ⋅ x ¨ = ( J + m l 2 ) ⋅ θ ¨ − m l sin ⁡ θ ⋅ θ ˙ 2 + m l cos ⁡ θ ⋅ θ ¨ + ( M + m ) ⋅ x ¨ = F \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. mglsinθmlcosθx¨=(J+ml2)θ¨mlsinθθ˙2+mlcosθθ¨+(M+m)x¨=F

进行化简,在平衡点附近,有
θ ˙ ≈ 0 cos ⁡ θ ≈ 1 sin ⁡ θ ≈ θ \dot{\theta} \approx 0 \quad \cos \theta \approx 1 \quad \sin \theta \approx \theta θ˙0cosθ1sinθθ

化简后,两方程如下:(输入 u = F u=F u=F
{ ( J + m l 2 ) θ ¨ − m g l θ = − m l x ¨ ( M + m ) x ¨ + m l θ ¨ = u \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. {(J+ml2)θ¨mg=mlx¨(M+m)x¨+mlθ¨=u

取:
x = [ x 1 x 2 x 3 x 4 ] = [ θ θ ˙ x x ˙ ] , u = F 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 x= x1x2x3x4 = θθ˙xx˙ ,u=F
根据上面的两个方程,化简二元一次方程,可得 x ¨ , θ ¨ \ddot x,\ddot \theta x¨,θ¨
{ x ˙ 1 = x 2 x ˙ 2 = m g l ( M + m ) J ( M + m ) + M m l 2 x 1 + − m l J ( M + m ) + M m l 2 u x ˙ 3 = x 4 x ˙ 4 = − m 2 g l 2 J ( M + m ) + M m l 2 x 1 + J + m l 2 J ( M + m ) + M m l 2 u \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. x˙1=x2x˙2=J(M+m)+Mml2mgl(M+m)x1+J(M+m)+Mml2mlux˙3=x4x˙4=J(M+m)+Mml2m2gl2x1+J(M+m)+Mml2J+ml2u

结论

因此状态方程中 A 、 B 、 C 、 D A、B、C、D ABCD四个矩阵的表达式可求出
A = [ 0 1 0 0 − m g l ( M + m ) J ( M + m ) + M m l 2 0 0 0 0 0 0 1 − m 2 g l 2 J ( M + m ) + M m l 2 0 0 0 ] 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] A= 0J(M+m)+Mml2mgl(M+m)0J(M+m)+Mml2m2gl2100000000010

B = [ 0 − m l J ( M + m ) + M m l 2 0 J + m l 2 J ( M + m ) + M m l 2 ] 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] B= 0J(M+m)+Mml2ml0J(M+m)+Mml2J+ml2

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

D = 0 D=0 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=1 kg M=1kg,摆杆质量 m = 5 k g m=5kg m=5kg,仿真时间设置为 20 s 20s 20s,修改重力方向,添加观测值(位移 x x x θ \theta θ,速度 v v v ω \omega ω
完成后效果如图
在这里插入图片描述
运行后输出结果如图【位移的单位是10^-13,目前可忽略】
在这里插入图片描述
这里看到theta的值大约是 π \pi π,因此要在模型中减去一个常数 π \pi π【因为初始情况下theta应该是0】

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

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

在这里插入图片描述

 
 

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

设置扰动及输入

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

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

    如图所示
    在这里插入图片描述

    设置扰动的方法补充说明

    事实上,这种扰动的设置方法并不科学,因为这种方法是直接加在关节上的,这份力矩还是会对系统产生作用【是内力不是外力,我们要模拟的是外力】
    举个例子
    比如把开关拨到常数档,然后给一个较小的常数(如:0.5)
    设置好K值这些后,再进行模拟,就会出现一些很反常的现象,如:
    在这里插入图片描述
    这是因为给了摆杆一个力矩为0.5的转矩,与滑块的外力平衡了

  • 移动副
    输入为力【Force】,而动量设置为自动计算。
    这里的输入根据LQR计算,将输入线先拉远一点,到时候要将里面封装成一个模块

    如图所示
    在这里插入图片描述

封装模块并实现LQR控制

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

  • 封装模块
    在四个输出处另外设置四个output
    【注:此图及后面的图还没对 θ \theta θ ω \omega ω进行系数矫正,是因为截图是还没注意到这个错误】
    在这里插入图片描述

    然后将 除了一个input和四个output的全部模块 封装成一个子系统
    在这里插入图片描述

  • 填入LQR的K值
    根据四个自变量的顺序,调整K的顺序,满足 u = K x u =K x u=Kx

    删掉外面的四个output模块和一个input模块,加入一个Mux模块Gain模块,将K值填入第一个Gain模块的增益上,并把乘法形式改成矩阵 K*u
    在这里插入图片描述
    再加一个反馈模块,用来强调 u = K x u=Kx u=Kx;通过ctrl+r改变Gain模块方向,最后效果如图
    在这里插入图片描述

运行程序

出于不知道什么原因,matlab中lqr求出的K值不能直接用在模块中,应该在后两位乘上 − 1 -1 1,(或者前两位乘上 − 1 -1 1,并在gain处乘 − 1 -1 1
笔者认为,跟建模时自己将 θ \theta θ设置成顺时针为正有关系,但不确定这个猜想是否正确

若不这么设置,就会出现以下情况:

在这里插入图片描述
【如图,越来越跟不上,因为后两维 x , x ˙ x,\dot x x,x˙成正反馈了,越跑越大】

能控,但控得不多((

调整好后,效果如下【笔者换了个模型,看得能直观些】
在这里插入图片描述
在这里插入图片描述在这里插入图片描述

模型位移等值不太稳定的原因是,该模型中一直在持续给干扰,如果把这个干扰改成阶跃的,那么曲线会更加好看
在这里插入图片描述
在这里插入图片描述 在这里插入图片描述

后续操作

接下来能做的操作其实不多了:

  • 修改仿真时间【没啥用】
  • 调参:根据模型控制情况调整扰动的强度【修改左上角随机干扰的系数】
  • 调参:双击Scope观察几个变量的变化情况,根据变化情况修改LQR中的QR权重矩阵,得到新的K
  • 改进扰动方式【重中之重,现在的现象是“反直觉”的】
  • 了解LQR怎么设置稳态误差,从而实现倒立摆稳定时x的位置可控

到此,matlab搭建物理仿真模型的过程几乎完全结束了,上面标红的两个部分笔者还没找到合适的解决方法,求大佬指点!

项目代码

https://gitee.com/hitszwowow/pendulum-dynamics-system


如果觉得有用的话点个赞和收藏呗🥺

------------分割线-------------
我的习惯是最新的一篇文章会加一个“仅关注可见”,方便我了解真实“阅读量”。
(如果觉得被骗关注了可以取关哈哈哈

Logo

开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!

更多推荐