• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

webots玩转控制论之LQR控制器

人工智能 罗伯特祥 2695次浏览 0个评论

嗨伙计们,我又来啦~   想了这么长时间,终于准备着手了!今天跟大家一起分享下webots中是如何实现LQR控制的。   今天就不多啰嗦了,下面开始进入我要跟大家分享的内容。   还是老样子,我们先来介绍一下文章安排:第一部分,我们将在webots中进行仿真建模;第二部分,我们会对仿真模型进行物理建模,得到动力学方程;第三部分,跟大家一起回顾一下LQR控制是怎么一回事儿;最后我们将编程实现webots中的LQR控制案例仿真!   想来想去,还是决定玩倒立摆吧,这么典型的入门案例,怎么可以放过呢!  
webots玩转控制论之LQR控制器  

1. webots建模

  不知道大家还记不记得我的上两篇文章,今天的模型我还是准备在上次的模型上进行修改。   话不多说,直接上修改方案!   在上次的基础上,我们新添加一个线性关节SliderJoint,并添加一个位置传感器PositionSensor和线性电机LinearMotor,将上次的单连杆节点剪切到SliderJointendPointchildren节点下!   关于各个节点的关系图,如下图所示:  
webots玩转控制论之LQR控制器   具体参数,见文末!  

2. 系统动力学建模

假设关节旋转中心到质心距离为L,质心处转动惯量为I,忽略摩擦,通过输入力F使得倒立摆呈现平衡状态,如下图所示:  
webots玩转控制论之LQR控制器   在水平方向上,有:
webots玩转控制论之LQR控制器   即:
webots玩转控制论之LQR控制器   对倒立摆旋转中心取矩,有:  
webots玩转控制论之LQR控制器   在平衡位置处线性化,即有:  
webots玩转控制论之LQR控制器  

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

  令 u=F,经过线性化处理并忽略高阶微小量,得到控制方程组为:  
webots玩转控制论之LQR控制器   取状态变量为:  
webots玩转控制论之LQR控制器   其中,  
webots玩转控制论之LQR控制器   则状态空间方程为:  
webots玩转控制论之LQR控制器   将系统动力学方程,整理成状态空间方程如下:  
webots玩转控制论之LQR控制器  
webots玩转控制论之LQR控制器   设计控制器之前,我们首先要知道系统的可观可控性!   具体就不赘述了,需要回顾的老铁建议再翻翻《现代控制工程》的内容。   这里我们直接使用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,则原来的状态空间方程变为:  
webots玩转控制论之LQR控制器   因此,LQR是通过状态反馈增益系数K来改变矩阵Acl的特征值,进而去控制系统表现的!   既然K这么关键,那应该怎么求K值呢?   我们引入一个能量函数J,设计的K值应该使得二次型能量函数J取得最小值,即为优化问题求解,目标函数如下:  
webots玩转控制论之LQR控制器   由此可见,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个状态变量,即:  
webots玩转控制论之LQR控制器   因此,我们要在webots中采集系统的这4个状态。   在此demo中,线性关节的直线速度以及旋转关节的角速度,我们通过后向差分的方式来近似获取!   而线性关节与旋转关节的传感器由于都是PositionSensor节点来实现的,因此两者数据获取方式相同,具体请查阅参考文档中对应语言的API.   设置R=1,Q矩阵如下:  
webots玩转控制论之LQR控制器   计算得到,K=[ -31.6228  -26.4636  236.8730   67.2227]   仿真效果如下:  
webots玩转控制论之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的选取对系统响应的影响;   那我们今天就先讲到这儿,读万卷书也要行万里路,我是罗伯特祥,下次见!  


webots玩转控制论之LQR控制器  


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明webots玩转控制论之LQR控制器
喜欢 (0)

您必须 登录 才能发表评论!

加载中……