嗨伙计们，我又来啦~ 想了这么长时间，终于准备着手了！今天跟大家一起分享下webots中是如何实现LQR控制的。 今天就不多啰嗦了，下面开始进入我要跟大家分享的内容。 还是老样子，我们先来介绍一下文章安排：第一部分，我们将在webots中进行仿真建模；第二部分，我们会对仿真模型进行物理建模，得到动力学方程；第三部分，跟大家一起回顾一下LQR控制是怎么一回事儿；最后我们将编程实现webots中的LQR控制案例仿真！ 想来想去，还是决定玩倒立摆吧，这么典型的入门案例，怎么可以放过呢！
1. webots建模
不知道大家还记不记得我的上两篇文章，今天的模型我还是准备在上次的模型上进行修改。 话不多说，直接上修改方案！ 在上次的基础上，我们新添加一个线性关节SliderJoint，并添加一个位置传感器PositionSensor和线性电机LinearMotor，将上次的单连杆节点剪切到SliderJoint的endPoint–children节点下！ 关于各个节点的关系图，如下图所示：
具体参数，见文末！
2. 系统动力学建模
假设关节旋转中心到质心距离为L，质心处转动惯量为I，忽略摩擦，通过输入力F使得倒立摆呈现平衡状态，如下图所示：
在水平方向上，有：
即：
对倒立摆旋转中心取矩，有：
在平衡位置处线性化，即有：
注意，线性化是局部的线性化！！！
令 u=F，经过线性化处理并忽略高阶微小量，得到控制方程组为：
取状态变量为：
其中，
则状态空间方程为：
将系统动力学方程，整理成状态空间方程如下：
设计控制器之前，我们首先要知道系统的可观可控性！ 具体就不赘述了，需要回顾的老铁建议再翻翻《现代控制工程》的内容。 这里我们直接使用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，则原来的状态空间方程变为：
因此，LQR是通过状态反馈增益系数K来改变矩阵Acl的特征值，进而去控制系统表现的！ 既然K这么关键，那应该怎么求K值呢？ 我们引入一个能量函数J，设计的K值应该使得二次型能量函数J取得最小值，即为优化问题求解，目标函数如下：
由此可见，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中采集系统的这4个状态。 在此demo中，线性关节的直线速度以及旋转关节的角速度，我们通过后向差分的方式来近似获取！ 而线性关节与旋转关节的传感器由于都是PositionSensor节点来实现的，因此两者数据获取方式相同，具体请查阅参考文档中对应语言的API. 设置R=1，Q矩阵如下：
计算得到，K=[ -31.6228 -26.4636 236.8730 67.2227] 仿真效果如下：
实际上，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的选取对系统响应的影响； 那我们今天就先讲到这儿，读万卷书也要行万里路，我是罗伯特祥，下次见！
