轨迹跟踪 LQR算法

### 轨迹跟踪中LQR算法的应用和实现

#### 基于LQR最优控制算法的轨迹跟踪原理
线性二次型调节器（Linear Quadratic Regulator, LQR）是一种用于设计控制器的方法，特别适用于多输入多输出系统的稳定性和性能优化。对于轨迹跟踪问题，LQR可以通过最小化一个代价函数来找到最佳控制律，该代价函数通常由状态误差和控制努力组成[^1]。

#### 构建车辆动力学模型
为了应用LQR进行轨迹跟踪，首先需要构建精确描述被控对象行为的动力学方程。这涉及到定义车辆的状态变量（如位置、速度）、输入量（如转向角速率、加速度），以及它们之间的关系表达式。这些构成了系统矩阵A和B，在此基础上才能求解Riccati方程得到反馈增益K。

#### 参数调优与仿真验证
选择合适的权重矩阵Q和R是影响最终控制系统表现的关键因素之一。其中Q反映了对不同状态分量的关注程度；而R则决定了施加到执行机构上的力或扭矩的重要性。合理设定这两个参数能够使得实际行驶路径尽可能贴近期望路线的同时保持平稳过渡而不至于过度振荡。此外，还需考虑如何使所选参数具备一定普适性以便适应更多场景下的需求[^2]。

#### MATLAB代码示例
下面给出一段简单的MATLAB脚本作为演示，展示了如何利用内置lqr()函数快速搭建起一个基本版的LQR控制器来进行直线车道跟随：

function LQR_Example()
    % 定义时间步长dt=0.1s; tspan=[0:dt:10]; n=length(tspan);
    
    % 初始化状态向量x(初始偏移设为零)
    x=zeros(n,2); u=zeros(n,1);

    % 设定目标参考点y_ref=5;
    
    % 配置系数矩阵A,B,C,D (假设简化后的二阶单入单出系统)
    A=[0 1;-k/m 0];
    B=[0;1/m];
    C=[1 0];
    D=0;

    % 计算LQR增益K
    Q=diag([q_pos q_vel]); R=r_input;
    K=lqr(A,B,Q,R);

    for i=2:n
        e=y_ref-x(i-1,1); v=-K*(e*[1;0]+C*x(i-1,:).');
        
        dx=A*x(i-1,:)'+B*v;
        x(i,:)=x(i-1,:)+dx*dt;
        u(i)=v;
    end
    
    figure();
    subplot(2,1,1), plot(tspan,x(:,1)), title('Position Error'), ylabel('Error(m)');
    subplot(2,1,2), stairs(tspan,u), title('Control Input'), xlabel('Time(s)'),ylabel('Input(N)');
end