// // lqrd: 连续系统的离散 LQR 设计MATLAB lqrd 的纯 C 实现// 零第三方依赖可直接编译// g -O2 -stdc11 lqrd.cpp -o lqrd// ./lqrd// #includeiostream#includevector#includecmath#includestdexcept#includeiomanip#includealgorithm// // 极简矩阵类仅实现本算法所需功能// classMatrix{public:introws,cols;std::vectordoubledata;Matrix():rows(0),cols(0){}Matrix(intr,intc):rows(r),cols(c),data(r*c,0.0){}Matrix(intr,intc,doublev):rows(r),cols(c),data(r*c,v){}doubleoperator()(inti,intj){returndata[i*colsj];}constdoubleoperator()(inti,intj)const{returndata[i*colsj];}staticMatrixIdentity(intn){MatrixI(n,n);for(inti0;in;i)I(i,i)1.0;returnI;}staticMatrixZero(intr,intc){returnMatrix(r,c,0.0);}Matrixtranspose()const{MatrixT(cols,rows);for(inti0;irows;i)for(intj0;jcols;j)T(j,i)(*this)(i,j);returnT;}Matrixoperator*(constMatrixB)const{if(cols!B.rows)throwstd::invalid_argument(Matrix mult dimension mismatch);MatrixC(rows,B.cols);for(inti0;irows;i)for(intk0;kcols;k){doublea(*this)(i,k);if(a0.0)continue;for(intj0;jB.cols;j)C(i,j)a*B(k,j);}returnC;}Matrixoperator(constMatrixB)const{MatrixC(rows,cols);for(size_t i0;idata.size();i)C.data[i]data[i]B.data[i];returnC;}Matrixoperator-(constMatrixB)const{MatrixC(rows,cols);for(size_t i0;idata.size();i)C.data[i]data[i]-B.data[i];returnC;}Matrixoperator*(doubles)const{MatrixC(rows,cols);for(size_t i0;idata.size();i)C.data[i]data[i]*s;returnC;}doublenorm()const{doubles0.0;for(doublev:data)sv*v;returnstd::sqrt(s);}// ---- 高斯-约当消元求逆带部分主元----Matrixinverse()const{if(rows!cols)throwstd::invalid_argument(inverse: non-square matrix);constintnrows;Matrix A*this;Matrix IIdentity(n);for(intcol0;coln;col){intpivcol;doublemaxvstd::fabs(A(col,col));for(intrcol1;rn;r){doublevstd::fabs(A(r,col));if(vmaxv){maxvv;pivr;}}if(maxv1e-14)throwstd::runtime_error(inverse: singular matrix);if(piv!col){for(intj0;jn;j){std::swap(A(col,j),A(piv,j));std::swap(I(col,j),I(piv,j));}}doubledA(col,col);for(intj0;jn;j){A(col,j)/d;I(col,j)/d;}for(intr0;rn;r){if(rcol)continue;doublefA(r,col);if(f0.0)continue;for(intj0;jn;j){A(r,j)-f*A(col,j);I(r,j)-f*I(col,j);}}}returnI;}// ---- 矩阵指数缩放-平方 泰勒级数 ----// exp(A) ( exp(A/2^s) )^(2^s)使 ‖A/2^s‖ ≤ 0.5Matrixexp()const{if(rows!cols)throwstd::invalid_argument(exp: non-square matrix);constintnrows;doublenrmnorm();ints0;while(nrm0.5){nrm*0.5;s;}constdoublescalestd::pow(2.0,-s);Matrix A(*this)*scale;Matrix resultIdentity(n);Matrix termIdentity(n);for(intk1;k20;k){termterm*A;for(size_t i0;iterm.data.size();i)term.data[i]/k;resultresultterm;if(term.norm()1e-18)break;}for(inti0;is;i)resultresult*result;returnresult;}voidblockSet(intr0,intc0,constMatrixB){for(inti0;iB.rows;i)for(intj0;jB.cols;j)(*this)(r0i,c0j)B(i,j);}MatrixblockGet(intr0,intc0,intr,intc)const{MatrixB(r,c);for(inti0;ir;i)for(intj0;jc;j)B(i,j)(*this)(r0i,c0j);returnB;}};// // 核心算法lqrd含交叉项 N// voidlqrd(constMatrixA,constMatrixB,constMatrixQ,constMatrixR,constMatrixN,doubleTs,MatrixKd,MatrixS){constintnA.rows;constintmB.cols;if(A.cols!n||B.rows!n)throwstd::invalid_argument(A/B 维度不匹配);if(Q.rows!n||Q.cols!n)throwstd::invalid_argument(Q 维度不匹配);if(R.rows!m||R.cols!m)throwstd::invalid_argument(R 维度不匹配);if(N.rows!n||N.cols!m)throwstd::invalid_argument(N 维度不匹配);// 1) Van Loan 扩展矩阵van Loan, IEEE TAC 23(3), 1978// K [A B; 0 0] (nm 维), W [Q N; N R] (nm 维)// H [-K W; 0 K] (2(nm) 维)// exp(H·Ts) [ exp(-K·Ts) X ;// 0 exp(K·Ts) ]// 其中 X exp(-K·Ts) · ∫₀^Ts exp(Kτ) W exp(Kτ) dτ// 故 [Qd Nd; Nd Rd] exp(-K·Ts)^{-1} · X// Ad, Bd 由 exp(K·Ts) [Ad Bd; 0 I] 提取精确 ZOH 离散化constintdnm;Matrix KMatrix::Zero(d,d);K.blockSet(0,0,A);K.blockSet(0,n,B);Matrix WMatrix::Zero(d,d);W.blockSet(0,0,Q);W.blockSet(0,n,N);W.blockSet(n,0,N.transpose());W.blockSet(n,n,R);Matrix HMatrix::Zero(2*d,2*d);Matrix KtK.transpose();for(inti0;id;i)for(intj0;jd;j)H(i,j)-Kt(i,j);H.blockSet(0,d,W);H.blockSet(d,d,K);// 2) 矩阵指数 e^(H·Ts)Matrix E(H*Ts).exp();// 3) 提取离散量Matrix E11E.blockGet(0,0,d,d);Matrix XE.blockGet(0,d,d,d);Matrix E22E.blockGet(d,d,d,d);Matrix AdE22.blockGet(0,0,n,n);Matrix BdE22.blockGet(0,n,n,m);Matrix QdNdRdE11.inverse()*X;Matrix QdQdNdRd.blockGet(0,0,n,n);Matrix NdQdNdRd.blockGet(0,n,n,m);Matrix RdQdNdRd.blockGet(n,n,m,m);// 4) 迭代求解离散 Riccati 方程Matrix Rd_invRd.inverse();Matrix A2Ad-Bd*Rd_inv*Nd.transpose();Matrix Q2Qd-Nd*Rd_inv*Nd.transpose();SQ2;constdoubletol1e-12;constintmax_iter10000;for(intk0;kmax_iter;k){Matrix S_nextA2.transpose()*S*A2-A2.transpose()*S*Bd*(Bd.transpose()*S*BdRd).inverse()*Bd.transpose()*S*A2Q2;doublediff(S_next-S).norm()/(S_next.norm()1e-16);SS_next;if(difftol)break;}// 5) 计算增益 KdKd(Bd.transpose()*S*BdRd).inverse()*(Bd.transpose()*S*AdNd.transpose());}// 便捷重载N 0voidlqrd(constMatrixA,constMatrixB,constMatrixQ,constMatrixR,doubleTs,MatrixKd,MatrixS){Matrix NMatrix::Zero(A.rows,B.cols);lqrd(A,B,Q,R,N,Ts,Kd,S);}// // 示例主程序// intmain(){// 双积分器ẍ uMatrixA(2,2);A(0,0)0;A(0,1)1;A(1,0)0;A(1,1)0;MatrixB(2,1);B(0,0)0;B(1,0)1;Matrix QMatrix::Identity(2);Matrix RMatrix::Identity(1);constdoubleTs0.01;Matrix Kd,S;lqrd(A,B,Q,R,Ts,Kd,S);std::coutstd::fixedstd::setprecision(4);std::cout离散 LQR 增益 Kd \n;for(inti0;iKd.rows;i){for(intj0;jKd.cols;j)std::cout Kd(i,j) ;std::cout\n;}std::cout\nRiccati 解 S \n;for(inti0;iS.rows;i){for(intj0;jS.cols;j)std::cout S(i,j) ;std::cout\n;}return0;}