移动小车上双摆的LQR、LQG、LQI控制器和龙伯格观测器文件列表 LQG.m LQG_non_linear.m LQI.m LQR.m LQR_Non_linear.m Luenberger_observer.m Observer_non_linear.m最近蹲在实验室的工位上啃移动小车双摆的控制代码翻来覆去调了快两周终于把LQR、LQG、LQI和龙伯格观测器这几套东西跑通了——本来以为都是线性控制的老把戏结果踩的坑比实验室垃圾桶里的打印纸还多。先从最入门的LQR说起吧也就是线性二次调节器本质就是给状态反馈找最优的增益矩阵前提是你能拿到所有的状态量比如小车的位置x、速度dx/dt两个摆的角度θ1、θ2和角速度dθ1/dt、dθ2/dt。我最开始直接抄了网上的模板代码结果跑起来小车要么冲出去撞墙要么摆杆直接甩到天花板后来才发现是Q和R矩阵调得太离谱。给大家看一眼LQR.m里的核心片段% 我用sympy推完双摆小车的状态空间模型直接把A、B矩阵粘进来的 A [0 1 0 0 0; 0 -0.5 1.2 0.8 -0.3; 0 0 0 1 0; 0 -0.7 3.1 2.2 -1.5; 0 0 0 0 0]; B [0; 0.2; 0; 0.4; 0]; % Q是状态权重矩阵优先罚小车位置和摆杆角度 Q diag([100, 10, 50, 5, 20]); R 0.1; % R是控制输入权重别太小不然电机爆冲 [K,S,E] care(A,B,Q,R); % 闭环系统就是A-B*K直接仿真就能看到小车稳住摆杆这里踩过最大的坑就是Q和R的比例一开始我把摆的角速度权重设得比角度还高结果电机疯狂抖动像个得了帕金森的老头后来改成角度权重更高才稳。R设太小的话电机出力太大小车会猛地窜出去设太大又没力气把摆杆立起来调的时候建议先把Q固定慢慢磨R的数值。但是问题来了实际实验里你不可能把所有传感器都装全——比如摆的角速度很难直接测总不能每个摆都焊个角速度编码器吧这时候就轮到龙伯格观测器上场了。说白了就是用系统的输入和测量到的输出估算出剩下的状态量。贴一段Luenberger_observer.m的核心代码% 假设我们只能测小车位置和两个摆角所以C矩阵长这样 C [1 0 0 0 0; 0 0 1 0 0; 0 0 0 0 1]; % 用place算观测器增益L记得把观测器极点设得比控制器快3倍 L place(A,C,[-10 -11 -12 -13 -14]); % 观测器更新方程x_hat_dot A*x_hat B*u L*(y - C*x_hat) % 闭环反馈就把原来的K*x换成K*x_hat就行我最开始把极点设得和控制器一样慢结果观测器追不上真实状态的变化摆杆刚立起来就倒了后来把极点往左边移了移也就是带宽更高瞬间就稳了。记住一句话观测器要比控制器跑得快不然反馈的都是过期数据。移动小车上双摆的LQR、LQG、LQI控制器和龙伯格观测器文件列表 LQG.m LQG_non_linear.m LQI.m LQR.m LQR_Non_linear.m Luenberger_observer.m Observer_non_linear.m有了观测器之后就可以搞LQG了——其实就是把LQR和卡尔曼滤波线性情况下和龙伯格观测器是一回事结合起来既能处理过程噪声和测量噪声又不用全状态传感器。比如实际实验里电机有齿槽噪声编码器也有漂移纯LQR直接用的话会把噪声放大LQG就会先滤波再反馈。LQG.m的核心就是拼起滤波和控制两部分% Qk是过程噪声协方差对应模型误差 Qk diag([0.1, 0.05, 0.1, 0.05, 0.1]); % Rk是测量噪声协方差对应传感器精度 Rk diag([0.5, 0.3, 0.3]); % 用kalman函数算滤波增益 Kf kalman(A,C,Qk,Rk); % 闭环就是先用Kf算出观测状态x_hat再用K*x_hat算控制量当初我把Rk设得太小也就是太信任传感器数据了结果传感器一有噪声就炸锅摆杆晃得像个不倒翁后来把Rk调大也就是更相信模型反而稳了。后来才知道便宜的编码器就把Rk设大一点别硬刚噪声。接下来是LQI也就是带积分的线性二次调节器用来解决稳态误差的问题。比如我用纯LQR的时候小车总是停在0.48m的位置差个2厘米才到设定的0.5m查了半天发现是有恒定扰动比如地面摩擦力纯LQR消不掉。LQI的做法就是把误差的积分项当成一个新的状态加到原来的状态向量里然后照样套LQR的流程。给大家看一眼LQI.m里的增广部分% 把积分误差项塞进状态向量 x_aug [x; int_error]; A_aug [A, zeros(5,1); -C(1,:), 0]; % 用小车位置作为误差输入 B_aug [B; 0]; % 再对增广矩阵算LQR增益就行一开始我还不知道有标准LQI这个东西愣是手动加了个积分环节在控制器外面结果系统变得很不稳定后来才知道把积分项塞进状态向量里的写法靠谱多了。不过增广之后矩阵维度变高了调Q和R的时候更麻烦每次改一点都要重新算一遍Riccati方程不过结果是真的香加完之后小车直接精准停在设定位置连误差都没了。最后说一下那两个非线性的文件LQGnonlinear.m和Observernonlinear.m。一开始我用固定线性模型的时候只要摆角超过30度系统直接就崩了——毕竟双摆的非线性很强小角度下的线性模型根本扛不住大摆角。非线性观测器就是用雅克比矩阵在线更新A矩阵相当于实时调整模型参数比固定的龙伯格观测器好用多了。不过调参数的时候真的想死每次改完都要跑好多次仿真才能找到合适的增益不过最后跑出来的时候摆杆晃到45度都不会倒爽到了。最后把这几个文件的使用顺序整理一下新手可以先跑LQR.m熟悉一下系统模型然后加观测器用Luenberger_observer.m需要抗噪声就上LQG要消除稳态误差就用LQI要是遇到大摆角的非线性场景就换那两个非线性的文件。对了所有代码里的A、B、C矩阵都是根据我的小车参数调的大家用的时候记得改成自己的参数不然大概率会撞墙或者甩摆。要是有什么踩坑的经验也欢迎留言唠唠反正我已经被这玩意儿搞秃了半头头发了。