LQR工程实现（调研）

LQR控制器比PID要高级的一种算法，笔者找了一些资料，也做了一些笔记。如下：
离散LQR求解过程如下所示：

（一）流程需要求解黎卡提方程，即为第三步，关于黎卡提方程的离散求解伪代码如下（包含整个LQR步骤）：

LQR(Actual State x, Desired State xf, Q, R, A, B, dt):
        x_error = Actual State x – Desired State xf
        Initialize N =（ ##） # 通常将 N 设置为某个任意整数如 50。
        Set P = [Empty list of N + 1 elements]

        K = [Empty list of N elements]
        u = [Empty list of N elements]
        Let P[N] = Q
        For i = N, …, 1
             P[i-1] = Q + ATP[i]A – (ATP[i]B)(R + BTP[i]B)-1(BTP[i]A) # 离散时间代数里卡蒂方程 。
             diff   = abs(p[i-1]-p[i])  
             P[i] = P[i-1]
             if diff< tolerance 
                p=P[i];
                 break;
             end
        end
        K = -(R + BpB)-1BTpA # K 将保持最佳反馈增益值。 当乘以状态误差时，将生成最佳控制输入
        u = K @ x_error      # 最佳控制输入 u 的稳定值（例如 u = [线速度 v，角速度 ω]）
        Return u        #返回最优控制输入，这些输入将被发送到被控对象。
#=end=#

（二）笔者MATLAB实现求解离散黎卡提方程
根据上述的伪代码逻辑，一步一步实现离散黎卡提方程的求解，离散黎卡提方程具体可参考：https://en.wikipedia.org/wiki/Algebraic_Riccati_equation
（1）主函数

%Function：离散黎卡提方程求解
%Author：Chunyu Ju
%Date：2022-7-3 13:05
clear
%被控对象状态方程
A=[0 1 0
   0 0 1
   -7 -41 6];
B = [0
     0
     1];
C = [6 0 0];
Q = [1 0 1
    0   1 0
    0  0 1];
R=[1 0 0
    0 1 0
    0 0 0];
tolerance = 0.1;%迭代收敛条件
times =50;      %迭代次数
[res_P] = Riccati(Q,R,A,B,C,tolerance,times)%求解黎卡提方程

 A'.*res_P.*A-A'.*res_P.*B.*(R+B'.*res_P.*B)^(-1).*B'.*res_P.*A+Q-res_P  %结果验证

（2）求解黎卡提方程的迭代代码

function [res_p] = Riccati(Q,R,A,B,C,tolerance,times)
P=[];
P=Q;
%%res = [];
for i = 1:1:times
   
    P_next = A'.*P.*A-A'.*P.*B.*(R+B'.*P.*B)^(-1).*B'.*P.*A+Q;
    if abs(norm(P_next)-norm(P))<tolerance
        res_p = P_next; %存储结果
       %% flag =true;
        %%return res;
        break;
    end
    P=P_next;
end
end

后面如果应用到实际工程需要用c语言实现此算法
（三）C++代码工程实现

bool solveRiccatiIterationD(const MatrixXd& Ad,
                            const MatrixXd& Bd, const MatrixXd& Q,
                            const MatrixXd& R, MatrixXd& P,
                            const double& tolerance = 1.E-5,
                            const uint iter_max = 100000)
{
  P = Q; // initialize

  MatrixXd P_next;

  MatrixXd AdT = Ad.transpose();
  MatrixXd BdT = Bd.transpose();
  MatrixXd Rinv = R.inverse();

  double diff;
  for(uint i = 0; i < iter_max; ++i) {
    // -- discrete solver --
    P_next = AdT * P * Ad -
             AdT * P * Bd * (R + BdT * P * Bd).inverse() * BdT * P * Ad + Q;

    diff = fabs((P_next - P).maxCoeff());
    P = P_next;
    if(diff < tolerance) {
      std::cout << "iteration mumber = " << i << std::endl;
      return true;
    }
  }
  return false; // over iteration limit
}

https://en.wikipedia.org/wiki/Algebraic_Riccati_equation
https://blog.csdn.net/zghforever/article/details/104458789
https://zhuanlan.zhihu.com/p/363033191
https://zhuanlan.zhihu.com/p/437266419