452c68bbeee899f3f674d62131317b16.png
双摆系统​zhuanlan.zhihu.com
82898dcdf1f501876c3b28db7e08782b.png

LQR控制器

LQR控制器是一种常用的控制器，相比较直接设置特征值的控制器来说有很多优点。

一般来说，我们需要从以下这几个维度来评估控制器：

  • 超调量
  • 使系统达到稳定的时间
  • 系统稳定后误差是否收敛

LQR控制器可以通过设置一个代价函数，将设计控制器转化为一个优化问题，通过这种方法可以直接影响输入输出误差以及控制量，从而达到最优控制。

该代价函数由两部分组成（实际上可以对不同要求通过自己设计代价方程来实现更复杂多样的控制器），第一部分为对输出的优化函数：

equation?tex=J_%7B1%7D+%3D+%5Cint_%7B0%7D%5E%7B%5Cinfty%7D%5Cleft%28+y+-+y_%7B%5Cinfty%7D+%5Cright%29%5E2dt%5C%5C+

可以看出，该函数为输出和设定值的差的积分，具有以下特性：

  • 输出震荡时间越长，误差越大，J值越大

通过状态方程将其变为：

equation?tex=J_%7B1%7D+%3D+%5Cint_%7B0%7D%5E%7B%5Cinfty%7D%5Cleft%28+x+-+x_%7B%5Cinfty%7D+%5Cright%29%5ETC%5ETC%28x+-+x_%7B%5Cinfty%7D%29dt%5C%5C+

在这里，我们可以设权重矩阵

equation?tex=Q%3DC%5ETC 。

第二部分为对输入的优化函数，可以实现对输入功率的设置要求:

equation?tex=J_%7B2%7D+%3D+%5Cint_%7B0%7D%5E%7B%5Cinfty%7Du%5ETRu+%5Cquad+dt%5C%5C+

对于系统来说LQR控制器即为解决如下问题：

equation?tex=+J%28u%5E%7B%5Cast%7D%2C+x_%7B0%7D%29+%3D+%5Cmin%5Climits_%7Bu%7D%5Cleft%28+J_%7B1%7D%28u%2Cx_%7B0%7D%29%2BJ_%7B2%7D%28u%2Cx_%7B0%7D%29%5Cright%29%5C%5C

该优化问题可以用Riccati微分方程求解，在此就不展开了。

Matlab中提供了求解该优化问题的命令:

Linear-Quadratic Regulator (LQR) design​www.mathworks.com

具体Matlab程序为：

%% A, B为系统状态空间矩阵
%% Q, R可以自己设计
%% 返回优化后的控制器系数，以及相应的极点
function [K, poleRK] = berechneLQR(A, B, Q, R)
K = [];
poleRK = [];
%% 系统可控
if rank(ctrb(A,B)) == max(size(A))
%% Q是否对称矩阵
    if Q == Q.'
%% Q是否正定
        if all(eig(Q)>0)
%% R是否正定
            if all(eig(R)>0)
%% 返回LQR控制器系数及极点
                [K,~,poleRK] =lqr(A,B,Q,R,[]);
            else
                disp('R 不是正定矩阵.');
            end
        else
            disp('Q 不是正定矩阵');
        end
        
    else
        disp('Q 不是对称矩阵.')
    end
else
    disp('系统不可控.');
end
end

Simulink

在之前的文章中，有介绍过用Fcn_Block实现双摆系统以及双摆系统的线性化。

  1. 首先，LQR控制器是对线性化系统的控制，因此需要选择静态工作点，然后将系统在该静态工作点线性化，得到系统的状态空间方程A, B, C, D。
  2. 然后选定Q阵和R阵，例如：Q =diag([10 1 1 1])；R = 1。通过上面的函数计算出控制器的系数K。
  3. 这次我们来使用S Function来实现双摆系统， S Function官网介绍链接为：
Use Level-2 MATLAB S-function in model​www.mathworks.com
  • 设置S Function的函数名，建立S-function模块与M文件形式的S-function之间的对应关系。
  • 填入S-function需要输入的外部参数的名称：本例子中Pendulum为系统各个参数，x0为初始值：

bebe4747aa6337841c0032f289677173.png
  • 设置双摆系统的各个参数：
function Pendulum = ladePendel()
    Pendulum.g = 9.81;
    Pendulum.m1 = 0.3;
    Pendulum.m2 = 0.3;
    Pendulum.l1 = 0.2;
    Pendulum.l2 = 0.2;
    Pendulum.Rp1 = 0;
    Pendulum.Rp2 = 0;
end
  • 编写M文件， S function：
function Sys_pendulum(block)
	setup(block);
end
% *************************************************************************
% 初始化
% *************************************************************************
function setup(block)
	% 输入输出数量
	block.NumInputPorts  = 1;
	block.NumOutputPorts = 1;
	% 输入
	block.InputPort(1).Dimensions	= 1;
	block.InputPort(1).DatatypeID	= 0;  % double
	block.InputPort(1).Complexity	= 'Real';
	block.InputPort(1).DirectFeedthrough = false;
	block.InputPort(1).SamplingMode = 'Sample';
	% 输出
	block.OutputPort(1).Dimensions	= 4;
	block.OutputPort(1).DatatypeID	= 0; % double
	block.OutputPort(1).Complexity	= 'Real';
	block.OutputPort(1).SamplingMode = 'Sample';    
	% 状态量数目
	block.NumContStates = 4;
	% 参数数目(双摆系统基本参数，初值）
	block.NumDialogPrms = 2;
	% 采样时间设置
	block.SampleTimes = [0 0];
	% 需要使用到的函数声明
	block.RegBlockMethod('InitializeConditions', @InitializeConditions);
	block.RegBlockMethod('Outputs', @Outputs);
	block.RegBlockMethod('Derivatives', @Derivatives);
	block.RegBlockMethod('Terminate', @Terminate);
end
% *************************************************************************
% 初值设置
% *************************************************************************
function InitializeConditions(block)
    x0 = block.DialogPrm(2).Data;
	block.ContStates.Data = x0;
end
% *************************************************************************
% 输出数据
% *************************************************************************
function Outputs(block)
	x = block.ContStates.Data;
	block.OutputPort(1).Data = x;
end
% *************************************************************************
% 导数计算
% *************************************************************************
function Derivatives(block)
    %双摆系统参数读取
    Pendulum = block.DialogPrm(1).Data;
	g =  Pendulum.g;
	m1 = Pendulum.m1;
	m2 = Pendulum.m2;
	l1 = Pendulum.l1;
	l2 = Pendulum.l2;
    %状态量
	x = block.ContStates.Data;
	phi1	= x(1);
	dphi1   = x(2);
	phi2    = x(3);
	dphi2	= x(4);
    %输入力矩
	M = block.InputPort(1).Data(1);
    %角加速度
    ddphi1 = -(6*(0 - 4*M*l2 ...
        + (3*g*l1*l2*m2*sin(phi1 - 2*phi2))/2 + (3*dphi1^2*l1^2*l2*m2*sin(2*phi1 - 2*phi2))/2 ...
        + 2*dphi2^2*l1*l2^2*m2*sin(phi1 - phi2) + 2*g*l1*l2*m1*sin(phi1) ...
        + (5*g*l1*l2*m2*sin(phi1))/2))/(l1^2*l2*(8*m1 + 15*m2 - 9*m2*cos(2*phi1 - 2*phi2)));
	ddphi2 = (6*(0 - 6*M*l2*m2*cos(phi1 - phi2) ...
        + 6*dphi1^2*l1^2*l2*m2^2*sin(phi1 - phi2) ...
        - 3*g*l1*l2*m2^2*sin(phi2) + (3*dphi2^2*l1*l2^2*m2^2*sin(2*phi1 - 2*phi2))/2 ...
        + 3*g*l1*l2*m2^2*sin(2*phi1 - phi2) ...
        + 2*dphi1^2*l1^2*l2*m1*m2*sin(phi1 - phi2) - (g*l1*l2*m1*m2*sin(phi2))/2 ...
        + (3*g*l1*l2*m1*m2*sin(2*phi1 - phi2))/2))/(l1*l2^2*m2*(8*m1 + 15*m2 - 9*m2*cos(2*phi1 - 2*phi2)));
	block.Derivatives.Data = [dphi1; ddphi1; dphi2; ddphi2 ];	
end
function Terminate(block)
end
  • 系统结构，M_AP为静态工作点时的输入，AP为静态工作点

3e2680af209db291925e3d05c94db620.png
  • 仿真结果：

观测同样初值，工作点，不同Q阵对控制器的影响：

AP= [pi/3 0 0 0];
x0=[-pi/3 0 -pi/2 0];
Q =diag([100 1 1 1]);
R = 1;

c3c27d5c89cc9a1d08d9a7cc8c00e286.png
红色为phi1 紫色为phi2

bb1c46de3b2e5b8ff397b9dbdc9c7abe.gif
AP= [pi/3 0 0 0];
x0=[-pi/3 0 -pi/2 0];
Q =diag([10 1 1 1]);
R = 1;

197a57a6d83caab1c9428ed8c6c644dd.png
红色为phi1 紫色为phi2

d380829779375bc3f903dc2cda5d2986.gif
AP= [pi/3 0 0 0];
x0=[-pi/3 0 -pi/2 0];
Q =diag([1 1 1 1]);
R = 1;

493ba46c15df0efd535e0875265861a4.png
红色为phi1 紫色为phi2

b8d3e9d16575e030022afe3c8aa723e4.gif
Logo
乐奇 Rokid 开放社区

更多推荐

cover

AR 智能眼镜并不是简单地在眼前放一块屏幕，而是把**摄像头、传感器、计算机视觉、空间定位、语音交互和云端服务**连接起来

avatar 乐奇 Rokid 开放社区
cover

让界面替我“摇“一次：一个AR祈愿智能体的AIUI实践

avatar 乐奇 Rokid 开放社区

智能眼镜市场全景解析：从技术演进到产业格局

本文系统梳理智能眼镜市场，涵盖产品形态、技术演进、市场规模、产业链结构及竞争格局。指出智能眼镜正从音频交互向AR显示与空间计算演进，受AI大模型、供应链成熟与生态延伸驱动，呈现分层发展态势。虽面临场景刚需不足、续航重量矛盾与隐私挑战，但B端应用先行、AI原生设计与标准完善将推动行业迈向规模化渗透，未来需关注核心技术突破与真实使用数据。

avatar 乐奇 Rokid 开放社区