webots玩转控制论之LQR控制器

108
0
2020年11月16日 10时57分

嗨伙计们,我又来啦~

 

想了这么长时间,终于准备着手了!今天跟大家一起分享下webots中是如何实现LQR控制的。

 

今天就不多啰嗦了,下面开始进入我要跟大家分享的内容。

 

还是老样子,我们先来介绍一下文章安排:第一部分,我们将在webots中进行仿真建模;第二部分,我们会对仿真模型进行物理建模,得到动力学方程;第三部分,跟大家一起回顾一下LQR控制是怎么一回事儿;最后我们将编程实现webots中的LQR控制案例仿真!

 

想来想去,还是决定玩倒立摆吧,这么典型的入门案例,怎么可以放过呢!

 

图1-捂嘴

 

1. webots建模

 

不知道大家还记不记得我的上两篇文章,今天的模型我还是准备在上次的模型上进行修改。

 

话不多说,直接上修改方案!

 

在上次的基础上,我们新添加一个线性关节SliderJoint,并添加一个位置传感器PositionSensor和线性电机LinearMotor,将上次的单连杆节点剪切到SliderJointendPointchildren节点下!

 

关于各个节点的关系图,如下图所示:

 

封面

 

具体参数,见文末!

 

2. 系统动力学建模

假设关节旋转中心到质心距离为L,质心处转动惯量为I,忽略摩擦,通过输入力F使得倒立摆呈现平衡状态,如下图所示:

 

图2-受力分析

 

在水平方向上,有:

1

 

即:

2

 

对倒立摆旋转中心取矩,有:

 

3

 

在平衡位置处线性化,即有:

 

4

 

 注意,线性化是局部的线性化!!!

 

令 u=F,经过线性化处理并忽略高阶微小量,得到控制方程组为:

 

5

 

取状态变量为:

 

6

 

其中,

 

7

 

则状态空间方程为:

 

8

 

将系统动力学方程,整理成状态空间方程如下:

 

9

 

10

 

设计控制器之前,我们首先要知道系统的可观可控性!

 

具体就不赘述了,需要回顾的老铁建议再翻翻《现代控制工程》的内容。

 

这里我们直接使用MATLAB来判断:

 

% 系统参数
m=2;
g=9.81;
I=1;
L=0.5;
% 状态空间模型
A = [0 1 0 0;
0 0 -m*g*L^2/I 0;
0 0 0 1;
0 0 m*g*L/I 0]
B = [0;
(I-m*L^2)/(I*m);
0;
L/I]
C = [1 0 0 0;
0 0 1 0]
D=[0;
0]
sys_ss = ss(A,B,C,D)
% 可控可观性判断
cont = ctrb(sys_ss)
if rank(cont)==4
disp("该系统可控!")
else
disp("该系统不可控!")
end
obser = obsv(sys_ss)
if rank(obser)==4
disp("该系统可观!")
else
disp("该系统不可观!")
end

 

由以上计算可知,该系统可控可观!

 

3. LQR控制器的回顾

 

在设计控制器之前,我们首先明确控制目标:θ=0

 

在一阶倒立摆模型里,我们通过输入外力F来使得倒立摆平衡,即输入为力,输出为倒立摆转角!

 

现在再来看LQR控制器的设计,首先LQR是linear quadratic regulator的缩写,也就是线性二次型调节器。LQR的设计,最关键的一步是求状态反馈增益系数K!

 

u=-kx,则原来的状态空间方程变为:

 

11

 

因此,LQR是通过状态反馈增益系数K来改变矩阵Acl的特征值,进而去控制系统表现的!

 

既然K这么关键,那应该怎么求K值呢?

 

我们引入一个能量函数J,设计的K值应该使得二次型能量函数J取得最小值,即为优化问题求解,目标函数如下:

 

12

 

由此可见,K值由Q和R矩阵确定.

 

那Q、R矩阵又该如何取值呢?

 

假设R不变,Q值越大,要想J值较小,那么x应该更小,这就意味着Acl矩阵的特征值位于S平面的左侧很远处,因此x能很快的收敛到0;假设Q不变,R值越大,那么输入值u对J的影响会越大,要想使得J较小,那么u应该更小,也就是说R越大,状态衰减会越慢!

 

注意:与轨迹跟踪不同的是,LQR是调整状态为0,而轨迹跟踪是调整误差为0!

 

LQR控制器我们就回顾这么多,这里我们直接使用MATLAB工具箱函数lqr来求解K值!

 

接下来,我们来看下在webots中的仿真情况^_^

 

4. webots中的仿真

 

在该系统中,我们有4个状态变量,即:

 

13

 

因此,我们要在webots中采集系统的这4个状态。

 

在此demo中,线性关节的直线速度以及旋转关节的角速度,我们通过后向差分的方式来近似获取!

 

而线性关节与旋转关节的传感器由于都是PositionSensor节点来实现的,因此两者数据获取方式相同,具体请查阅参考文档中对应语言的API.

 

设置R=1,Q矩阵如下:

 

14

 

计算得到,K=[ -31.6228  -26.4636  236.8730   67.2227]

 

仿真效果如下:

 

图3-lqr仿真

 

实际上,webots的例程中,自带了倒立摆的仿真案例,感兴趣的小伙伴可以在[文件Open Sample World]中搜索pendulum仔细研究一下demo中所用的控制方法以及各节点结构。

 

5. Demo程序

整体代码如下:

% MATLAB controller for Webots
% File: lqr_controller.m
desktop;
keyboard;
%% system model
% 系统参数
m=2;
g=9.81;
I=1;
L=0.5;
% 状态空间模型
A = [0 1 0 0;
0 0 -m*g*L^2/I 0;
0 0 0 1;
0 0 m*g*L/I 0]
B = [0;
(I+m*L^2)/(I*m);
0;
L/I]
C = [1 0 0 0;
0 0 1 0]
% LQR
Q = C'*C;
Q(1,1) = 1000;
Q(3,3) = 100;
R = 1;
K = lqr(A,B,Q,R) %状态反馈控制增益矩阵
TIME_STEP = wb_robot_get_basic_time_step();
linear_motor = wb_robot_get_device('lm');
rotational_motor = wb_robot_get_device('rm');
linear_position_sensor = wb_robot_get_device('lps');
wb_position_sensor_enable(linear_position_sensor, TIME_STEP);
position_sensor = wb_robot_get_device('rps');
wb_position_sensor_enable(position_sensor, TIME_STEP);
wb_motor_set_force(linear_motor, 0);
wb_motor_set_torque(rotational_motor, 0);
%% control loop
time = wb_robot_get_time();
x = wb_position_sensor_get_value(linear_position_sensor);
q = wb_position_sensor_get_value(position_sensor);
% main loop:
while wb_robot_step(TIME_STEP) ~= -1
last_time = time;
last_x = x;
last_q = q;
time = wb_robot_get_time();
x = wb_position_sensor_get_value(linear_position_sensor)
q = wb_position_sensor_get_value(position_sensor)
dx = (x - last_x)/(time - last_time)
dq = (q - last_q)/(time - last_time)
X = [x;dx;q;dq];
u = -K*X
wb_motor_set_force(linear_motor, u);
end

小结

最后小结一下,今天的内容其实比较老套,算是现代控制理论的基础入门教程吧,需要注意的有以下几个点:

– (1)要明确控制目标是什么,输入什么,输出什么;

– (2)状态反馈增益系数K值的求解过程;

– (3)Q、R的选取对系统响应的影响;

 

那我们今天就先讲到这儿,读万卷书也要行万里路,我是罗伯特祥,下次见!

 


图5-参数

 

发表评论

后才能评论