{"id":429,"date":"2025-01-21T17:45:12","date_gmt":"2025-01-21T09:45:12","guid":{"rendered":"http:\/\/106.12.148.75\/?p=429"},"modified":"2025-03-14T18:29:04","modified_gmt":"2025-03-14T10:29:04","slug":"teset","status":"publish","type":"post","link":"https:\/\/jamilblog.top\/?p=429","title":{"rendered":"\u4ecelqr\u5230ilqr\u518d\u5230cilqr"},"content":{"rendered":"\n

\u6700\u8fd1\u5de5\u7a0b\u4e0a\u8c03\u8bd5lqr\u76f8\u5173\u7684\u7b97\u6cd5\u6709\u4e9b\u5fc3\u70e6\uff0c\u60f3\u518d\u4ed4\u7ec6\u63a8\u5bfc\u4e00\u904d\u9759\u9759\u5fc3\uff0c\u5728\u7f51\u4e0a\u641c\u7d22\u4e86\u4e00\u5708\u6559\u7a0b\u603b\u89c9\u5f97\u5dee\u70b9\u610f\u601d\uff0c\u51b3\u5b9a\u81ea\u5df1\u8be6\u7ec6\u7684\u5199\u4e00\u904d\u4ecelqr\u7684\u901a\u7528\u89e3\u6cd5\u4e00\u76f4\u5230cilqr\u7684\u6c42\u89e3\uff0c\u6587\u7ae0\u4f1a\u4fdd\u7559\u6240\u6709\u51e0\u4e4e\u6240\u6709\u7684\u63a8\u5bfc\u6b65\u9aa4\uff0c\u751a\u81f3\u4e0d\u4f1a\u7701\u7565\u79fb\u9879\u3001\u77e9\u9635\u8f6c\u7f6e\u7b49\u6b65\u9aa4\u4ee5\u9632\u8bfb\u8005\u8ddf\u4e22\uff0c\u6240\u4ee5\u6587\u7ae0\u4f1a\u663e\u5f97\u6bd4\u8f83\u5197\u957f\uff0c\u4f46\u8ddf\u7740\u62ff\u7b14\u63a8\u5bfc\u4e00\u904d\u76f8\u4fe1\u603b\u4f1a\u6709\u6536\u83b7\u3002<\/p>\n\n\n\n

LQR\u63a8\u5bfc<\/h1>\n\n\n\n

\u5bf9\u4e8eLQR\u6765\u8bf4\u6211\u4eec\u8981\u89e3\u51b3\u7684\u662f\u4e00\u4e2a\u6709\u9650\u65f6\u57df\u4e0b\u7684\u6700\u4f18\u95ee\u9898\uff1a<\/p>\n\n\n\n

$$\\begin{matrix}
\\min_{u}{x_N^TQ_Nx_x + \\sum_{t = 1}^{N-1} x_t^TQ_tx_t + u_t^TR_tu_t}\\\\
st. x_{t+1} = A_tx_t + B_tu_t
\\end{matrix}$$<\/p>\n\n\n\n

\u4e3a\u4e86\u6c42\u89e3\u8fd9\u4e2a\u6700\u4f18\u95ee\u9898\uff0c\u6211\u4eec\u6784\u5efa\u4ef7\u503c\u51fd\u6570:<\/p>\n\n\n\n

$$V_t(x_t) = \\min_{u}{x_t^TQ_tx_t + u_t^TR_tu_t + V_{t+1}(x_{t+1})}$$<\/p>\n\n\n\n

\u8fd9\u91cc\u7684\\(V_t(x_t) \\)\u6240\u4ee3\u8868\u7684\u5177\u4f53\u542b\u4e49\u4e3a \\(t \\)\u65f6\u523b\u4e00\u76f4\u5230\u7ec8\u7aef\u65f6\u523b\uff0c\u5728\u4f7f\u7528\u6700\u4f18\u89e3\u7684\u60c5\u51b5\u4e0b\u4ee3\u4ef7\u51fd\u6570\u7684\u6700\u5c0f\u503c\uff0c\u8fd9\u4e2a\u6982\u5ff5\u5f88\u7ed5\u4f46\u5f88\u597d\u7406\u89e3\uff0c\\( V_t \\)\u7684\u4e24\u4e2a\u90e8\u5206\u4e00\u4e2a\u662f\u5f53\u524d\u65f6\u523b\u7684\u4ee3\u4ef7\uff0c\u4e00\u4e2a\u662f\u4e0b\u4e2a\u65f6\u523b\u7684\u4ee3\u4ef7\uff0c\u4e0d\u96be\u770b\u51fa\u8fd9\u662f\u4e00\u4e2a\u9012\u5f52\u51fd\u6570\uff0c\u53ef\u4ee5\u79fb\u690d\u5ef6\u4f38\u5230\u7ec8\u7aef\u65f6\u523b\\( N \\)\u3002\u8fd9\u4e2a\u4ee3\u4ef7\u51fd\u6570\u7684\u8bbe\u8ba1\u5728\u6700\u4f18\u63a7\u5236\u548c\u5f3a\u5316\u5b66\u4e60\u4e2d\u90fd\u5f88\u5e38\u89c1\u3002\u6211\u4eec\u7684\u76ee\u6807\u5c31\u662f\u6c42\u89e3\u51fa\u8fd9\u4e2a\u6700\u4f18\u7684\u63a7\u5236\u5e8f\u5217 \\( u^* = {u^*_1, u^*_2, ..., u^*_N} \\)\u3002\u5728lqr\u4e2d\uff0c\u63a7\u5236\u91cf\u88ab\u8bbe\u8ba1\u4e3a\u5168\u72b6\u6001\u53cd\u9988\uff0c\u4e5f\u5c31\u662f<\/p>\n\n\n\n

$$ u^*_i = -K_ix_i $$<\/p>\n\n\n\n

\u8fd9\u91cc\u6211\u4eec\u4e0d\u8ba8\u8bba\u8d1d\u5c14\u66fc\u65b9\u7a0b\u8fd9\u4e9b\u6982\u5ff5\u6027\u7684\u4e1c\u897f\uff0c\u53ea\u4ece\u6c42\u89e3\u7684\u601d\u8def\u6765\u8bf4\uff0c\u53ef\u4ee5\u53d1\u73b0\u6bcf\u4e00\u4e2a\u65f6\u523b\u7684\u4ee3\u4ef7\u51fd\u6570\u90fd\u8981\u4f9d\u8d56\u4e0b\u4e00\u65f6\u523b\u7684\u503c\uff0c\u8fd9\u6837\u7684\u5199\u6cd5\u65e0\u6cd5\u663e\u793a\u7684\u7ed9\u51fa\u4ee3\u4ef7\u51fd\u6570\u548c\u8f93\u5165u\u7684\u5173\u7cfb\uff0c\u6240\u4ee5\u6211\u4eec\u7684\u7b2c\u4e00\u4e2a\u4efb\u52a1\u662f\u8981\u6c42\u89e3\u51fa\u8fd9\u4e2a\u4ee3\u4ef7\u51fd\u6570\u548c\u6700\u4f18\u8f93\u5165 \\( u^* \\)\u7684\u663e\u793a\u8868\u8fbe\u3002<\/p>\n\n\n\n

\u4ece \\( V_t \\)\u7684\u5f62\u5f0f\u4e0d\u80fd\u770b\u51fa\u4f3c\u4e4e\u53ea\u6709\u7ec8\u7aef\u65f6\u523b\u7684\u4ee3\u4ef7\u4e0d\u9700\u8981\u672a\u6765\u7684\u4fe1\u606f\u5f88\u5bb9\u6613\u5c31\u53ef\u4ee5\u5199\u51fa\u4ed6\u7684\u663e\u5f0f\u8868\u8fbe\uff1a<\/p>\n\n\n\n

$$V_N = x_N^TQ_Nx_N$$<\/p>\n\n\n\n

\u8fd9\u91cc\u6211\u4eec\u7ed9\u51fa\u4e00\u4e2a\u63a8\u6f14\u6027\u7684\u7ed3\u8bba\uff0c\u4efb\u610f\u65f6\u523b\u7684\u4ee3\u4ef7\u51fd\u6570\u90fd\u53ef\u4ee5\u5199\u6210\u4ee5\u4e0b\u5f62\u5f0f\uff1a<\/p>\n\n\n\n

$$ V_t = x_t^TP_tx_t $$<\/p>\n\n\n\n

\u4e3a\u4ec0\u4e48\u5f97\u51fa\u8fd9\u6837\u7684\u7ed3\u8bba\uff1f\u56e0\u4e3a\u4ee3\u4ef7\u51fd\u6570\u662f\u4ece\u7ec8\u7aef\u4ee3\u4ef7\u63a8\u6f14\u56de\u53bb\u7684\uff0c\u5f53\\(V_N\\)\u662f\u4e8c\u6b21\u578b\u65f6\uff0c\\(V_{N-1} = x_{N-1}^TQ_{N-1}x_{N-1} + (kx_{N-1})^TR_{N-1} (kx_{N-1}) +x_N^TQ_Nx_N \\) \u3002\u663e\u7136\u8fd9\u662f\u72b6\u6001\u7684\u4e00\u7cfb\u5217\u7ebf\u6027\u53d8\u6362\uff0c\u4e00\u5b9a\u53ef\u4ee5\u6574\u7406\u4e3a\u4e8c\u6b21\u578b\u5f62\u5f0f\uff0c\u540c\u7406\u8fd9\u6837\u4e00\u76f4\u9012\u63a8\u56de\u53bb\u6bcf\u4e00\u4e2a\u65f6\u523b\u90fd\u53ef\u4ee5\u8fd9\u6837\u8868\u8fbe\u3002<\/p>\n\n\n\n

\u6709\u4e86\u8fd9\u6837\u7684\u7ed3\u8bba\u6211\u4eec\u5c31\u53ef\u4ee5\u8fdb\u4e00\u6b65\u7684\u63a8\u5bfc\u4ee3\u4ef7\u51fd\u6570\u7684\u9012\u63a8\u516c\u5f0f\uff0c\u76ee\u6807\u662f\u8ba9\u9012\u63a8\u516c\u5f0f\u5bf9\\(u_t\\)\u6c42\u5bfc\u7b49\u4e8e0\u65f6\uff08\u4e5f\u5c31\u662f\u7406\u8bba\u4e0a\u7684\u6700\u4f18\u89e3\uff09\u53ea\u4e0e\\(P_t+1\\)\u548c\\(x_t\\)\u6709\u5173\uff0c\u8fd9\u6837\u6211\u4eec\u5c31\u53ef\u4ee5\u901a\u8fc7\u7ec8\u7aef\u7684\\(P_N = Q_N\\)\u6765\u4e00\u6b65\u4e00\u6b65\u9012\u63a8\u56de\u53bb\u6c42\u5f97\u6bcf\u4e2a\u65f6\u523b\u7684\u6700\u4f18\u89e3\u3002<\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n
LaTeX<\/span><\/path><\/path><\/svg><\/span>
\\<\/span>begin<\/span>{<\/span>aligned<\/span>}<\/span>V<\/span>_<\/span>t<\/span>(<\/span>x<\/span>_<\/span>t<\/span>)<\/span>&<\/span>= x<\/span>_<\/span>t<\/span>^<\/span>T Q<\/span>_<\/span>t x<\/span>_<\/span>t  <\/span>+<\/span> u<\/span>_<\/span>t<\/span>^<\/span>T R<\/span>_<\/span>t u<\/span>_<\/span>t  <\/span>+<\/span> <\/span>(<\/span>A<\/span>_<\/span>t x<\/span>_<\/span>t <\/span>+<\/span> B<\/span>_<\/span>t u<\/span>_<\/span>t<\/span>)^<\/span>T P<\/span>_{<\/span>t<\/span>+<\/span>1<\/span>}<\/span> <\/span>(<\/span>A<\/span>_<\/span>t x<\/span>_<\/span>t <\/span>+<\/span> B<\/span>_<\/span>t u<\/span>_<\/span>t<\/span>)<\/span>\\\\<\/span>[<\/span>6<\/span>pt<\/span>]<\/span>&<\/span>= x<\/span>_<\/span>t<\/span>^<\/span>T Q<\/span>_<\/span>t x<\/span>_<\/span>t  <\/span>+<\/span> u<\/span>_<\/span>t<\/span>^<\/span>T R<\/span>_<\/span>t u<\/span>_<\/span>t <\/span>\\\\<\/span>&<\/span>\\<\/span>quad <\/span>+\\<\/span>,<\/span> x<\/span>_<\/span>t<\/span>^<\/span>T A<\/span>_<\/span>t<\/span>^<\/span>T P<\/span>_{<\/span>t<\/span>+<\/span>1<\/span>}<\/span> A<\/span>_<\/span>t x<\/span>_<\/span>t  <\/span>+<\/span> u<\/span>_<\/span>t<\/span>^<\/span>T B<\/span>_<\/span>t<\/span>^<\/span>T P<\/span>_{<\/span>t<\/span>+<\/span>1<\/span>}<\/span> B<\/span>_<\/span>t u<\/span>_<\/span>t <\/span>+<\/span> <\/span>2<\/span>\\<\/span>,<\/span>x<\/span>_<\/span>t<\/span>^<\/span>T A<\/span>_<\/span>t<\/span>^<\/span>T P<\/span>_{<\/span>t<\/span>+<\/span>1<\/span>}<\/span> B<\/span>_<\/span>t<\/span>\\<\/span>,<\/span>u<\/span>_<\/span>t <\/span>\\\\<\/span>[<\/span>6<\/span>pt<\/span>]<\/span>&<\/span>= x<\/span>_<\/span>t<\/span>^<\/span>T <\/span>\\bigl(<\/span>Q<\/span>_<\/span>t <\/span>+<\/span> A<\/span>_<\/span>t<\/span>^<\/span>T P<\/span>_{<\/span>t<\/span>+<\/span>1<\/span>}<\/span> A<\/span>_<\/span>t<\/span>\\bigr)\\<\/span>,<\/span>x<\/span>_<\/span>t  <\/span>+<\/span> u<\/span>_<\/span>t<\/span>^<\/span>T <\/span>\\bigl(<\/span>R<\/span>_<\/span>t <\/span>+<\/span> B<\/span>_<\/span>t<\/span>^<\/span>T P<\/span>_{<\/span>t<\/span>+<\/span>1<\/span>}<\/span> B<\/span>_<\/span>t<\/span>\\bigr)\\<\/span>,<\/span>u<\/span>_<\/span>t  <\/span>+<\/span> <\/span>2<\/span>\\<\/span>,<\/span>x<\/span>_<\/span>t<\/span>^<\/span>T<\/span>\\bigl(<\/span>A<\/span>_<\/span>t<\/span>^<\/span>T P<\/span>_{<\/span>t<\/span>+<\/span>1<\/span>}<\/span> B<\/span>_<\/span>t<\/span>\\bigr)\\<\/span>,<\/span>u<\/span>_<\/span>t <\/span>\\<\/span>,<\/span>.<\/span>\\<\/span>end<\/span>{<\/span>aligned<\/span>}<\/span><\/span><\/code><\/pre><\/div>\n\n\n\n
\u901a\u8fc7\u4e00\u7cfb\u5217\u7684\u5e26\u5165\u548c\u53d8\u6362\uff0c\u6211\u4eec\u53d1\u73b0\u4ee3\u4ef7\u51fd\u6570\u53d8\u6210\u4e86\\(u_t\\)\u7684\u663e\u5f0f\u8868\u8fbe\uff0c\u4f3c\u4e4e\u53ea\u6709\\(P_t\\)\u662f\u4e2a\u672a\u77e5\u91cf\uff0c\u4f46\u50cf\u4e4b\u524d\u8bf4\u7684\uff0c\u6211\u4eec\u7684\u4ee3\u4ef7\u51fd\u6570\u662f\u4ece\u7ec8\u7aef\u5f80\u56de\u9012\u5f52\u7684\uff0c\u6211\u4eec\u59cb\u7ec8\u77e5\u9053t+1\u65f6\u523b\u7684\\(P_{t+1}\\)\u3002\u90a3\u4e48\u600e\u4e48\u624d\u80fd\u627e\u5230\u6700\u4f18\u7684\u8f93\u5165\u5462\uff1f\u5bf9\u4e8e\u8fd9\u6837\u4e00\u4e2a\u4e8c\u6b21\u578b\u7684\u884c\u9a76\uff0c\u5f53\u4ee3\u4ef7\u51fd\u6570\u5bf9\u8f93\u5165\u6c42\u5bfc\u7b49\u4e8e0\u65f6\u4e3a\u6700\u4f18\u89e3\uff0c\u4e5f\u5c31\u662f<\/p>\n\n\n\n
$$ \\frac{\\partial V_t(x_t)}{\\partial u_t} = 0 $$<\/p>\n\n\n\n
\u5c06\u4e4b\u524d\u5f97\u51fa\u7684\\(V_t(x_t)\\)\u4ee3\u5165\u53ef\u5f97<\/p>\n\n\n\n
$$ 2(R_{t}+B_{t}^TP_{t+1}B_t)u_t +2B_t^TP_{t+1}A_tx_t = 0$$<\/p>\n\n\n\n
$$ (R_t + B_t^TP_{t+1}B_t)u_t = -B_t^TP_{t+1}A_tx_t $$<\/p>\n\n\n\n
$$ u_t = -{(R_t + B_t^TP_{t+1}B_t)}^{-1}B_t^TP_{t+1}A_tx_t $$<\/p>\n\n\n\n
\u73b0\u5728\u6211\u4eec\u63a8\u5bfc\u51fa\u4e86\u6bcf\u4e00\u6b65\u7684\u6700\u4f18\u63a7\u5236\u5f8b\u7684\u8868\u8fbe\u5f0f<\/p>\n\n\n\n
$$ k_t = {(R_t + B_t^TP_{t+1}B_t)}^{-1}B_t^TP_{t+1}A_t $$<\/p>\n\n\n\n
\u663e\u7136\u6211\u4eec\u73b0\u5728\u53ef\u4ee5\u5728\u5f53\u524d\u65f6\u95f4\u65f6\u523bt\u5229\u7528\u4e0b\u4e00\u65f6\u523b\u7684\\(P_{t+1}\\)l\u6765\u8ba1\u7b97\u5f53\u524d\u65f6\u523b\u7684\u6700\u4f18\u63a7\u5236\u5f8b\u4e86\uff0c\u4f46\u662f\u6211\u4eec\u8be5\u5982\u4f55\u901a\u8fc7\\(P_{t+1}\\)\u6765\u5f97\u5230\\(P_{t}\\)\u4ee5\u8ba1\u7b97\\(u^*_{t-1}\\)\u5462\uff1f\u6240\u4ee5\u6211\u4eec\u4e0d\u5f97\u4e0d\u5199\u51fa\\(P_{t}\\)\u7684\u8fed\u4ee3\u8868\u8fbe\u5f0f\u3002\u5c06\u8fd9\u4e2a\u6700\u4f18\u63a7\u5236\u5f8b\uff0c\u4e5f\u662f\u5c31\\(u^*_t = -kx_t\\)\u5e26\u5165\u56de\\(V_{t}(x_t)\\)\u7684\u8868\u8fbe\u5f0f\u4e2d\u53ef\u4ee5\u5f97\u5230\uff1a<\/p>\n\n\n\n
$$ P_t = Q_t + A_t^TP_{t+1}A_t - A^TP_{t+1}B_t(R_t+B^TP_{t+1}B)^{-1}B_t^TP_{t+1}A_t$$ <\/p>\n\n\n\n
\u8fd9\u4e2a\u65b9\u7a0b\u5c31\u662f\u5927\u540d\u9f0e\u9f0e\u7684\u674e\u5361\u8fea\u65b9\u7a0b\uff0c\u56e0\u4e3a\u662f\u8ba8\u8bba\u6709\u9650\u65f6\u57df\u4e0b\u7684\u6536\u655b\u95ee\u9898\uff0c\u6240\u4ee5\u5b83\u662f\u7531\u6700\u7ec8\u72b6\u6001N\u4e00\u6b65\u4e00\u6b65\u63a8\u6f14\u56de\u6765\u7684\uff0c\u5728\u6b64\u6211\u4eec\u53ef\u4ee5\u5f88\u7b80\u5355\u7684\u53d1\u6563\u5230\u65e0\u9650\u65f6\u57df\u7684lqr\u6539\u5982\u4f55\u6c42\u89e3\u8fd9\u4e2aP\u5462\uff1f<\/p>\n\n\n\n
\u5bf9\u4e8e\u65e0\u9650\u65f6\u57df\u7684lqr\u6765\u8bf4\uff0c\u6211\u4eec\u5c31\u65e0\u6cd5\u4ece\u7ec8\u7aef\u72b6\u6001\u6765\u5012\u63a8\u5bfcP\u4e86\uff0c\u6211\u4eec\u9700\u8981\u6c42\u89e3\u4e00\u4e2aP\u7684\u7a33\u6001\u89e3\uff0c\u4e5f\u5c31\u662f\u5f53\u65f6\u95f4\u8d8b\u5411\u4e8e\u65e0\u7a77\u65f6\u4ecd\u80fd\u6ee1\u8db3\u4e0a\u8ff0\u674e\u5361\u8fea\u65b9\u7a0b\u7684\u89e3\uff1a<\/p>\n\n\n\n
$$ P = Q + A^TPA - A^TPB(R+B^TPB)^{-1}B^TPA$$ <\/p>\n\n\n\n
\u6240\u4ee5\u5bf9\u4e8e\u65e0\u9650\u65f6\u57dflqr\u6765\u8bf4\u9700\u8981\u6c42\u51fa\u674e\u5361\u8fea\u65b9\u7a0b\u7684\u7a33\u5b9a\u89e3\uff0c\u4e5f\u53ef\u4ee5\u6ee1\u8db3\u6211\u4eec\u524d\u9762\u7684\u8981\u6c42\uff0c\u90a3\u4e48\u5982\u4f55\u8ba1\u7b97\u8fd9\u4e2a\u7a33\u5b9a\u89e3\uff0c\u4f9d\u65e7\u9700\u8981\u7528\u5230\u6709\u9650\u65f6\u57df\u7684\u89e3\u6cd5\uff0c\u53ea\u4e0d\u8fc7\u8fd9\u6b21\u662f\u4ece\u4e00\u4e2a\u5047\u8bbe\u7684\u521d\u503c\uff08\u5047\u8bbe\u7684\u65e0\u7a77\u65f6\u95f4\u7684\u7ec8\u7aef\u4ee3\u4ef7\u77e9\u9635\uff09\u5f00\u59cb\u5411\u524d\u8fed\u4ee3\uff0c\u7531\u4e8e\u6ca1\u6709\u4e86\u65f6\u57df\u7684\u9650\u5236\uff0c\u901a\u5e38\u6211\u4eec\u5c06\u4e24\u6b21p\u77e9\u9635\u7684\u53d8\u5316\u91cf\u4f5c\u4e3a\u8fed\u4ee3\u7ec8\u6b62\u7684\u6761\u4ef6\u3002<\/p>\n\n\n\n
iLQR\u63a8\u5bfc<\/h1>\n\n\n\n
\u6709\u4e86LQR\u7684\u57fa\u7840\uff0c\u6211\u4eec\u5df2\u7ecf\u53ef\u4ee5\u89e3\u51b3\u5927\u90e8\u5206\u7684\u95ee\u9898\uff0c\u4f46\u8fd9\u5bf9\u4e8e\u8f68\u8ff9\u89c4\u5212\u6765\u8bf4\u8fdc\u8fdc\u4e0d\u591f\uff0c\u56e0\u4e3a\u666e\u901a\u7684LQR\u53d7\u5236\u4e0e\u88ab\u63a7\u5bf9\u8c61\u4e3a\u7ebf\u6027\u6a21\u578b\uff0c\u800c\u5728\u81ea\u52a8\u9a7e\u9a76\u4e2d\u65e0\u8bba\u662f\u89c4\u5212\u8fd8\u662f\u63a7\u5236\uff0c\u65e0\u8bba\u662f\u8fd0\u52a8\u5b66\u6a21\u578b\u8fd8\u662f\u52a8\u529b\u5b66\u6a21\u578b\u90fd\u5f88\u96be\u7528\u7ebf\u6027\u6a21\u578b\u53bb\u98d9\u5230\uff0c\u6240\u4ee5ilqr\u5e94\u8fd0\u800c\u751f\uff0c\u8ba9\u6211\u4eec\u91cd\u65b0\u5b9a\u4e49\u6211\u4eec\u8981\u6c42\u89e3\u7684\u95ee\u9898\uff1a<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
LaTeX<\/span><\/path><\/path><\/svg><\/span>
$$<\/span>\\<\/span>begin<\/span>{<\/span>array<\/span>}{<\/span>l<\/span>}<\/span><\/span>\n\\<\/span>min <\/span>{\\<\/span>substack<\/span>{<\/span>u<\/span>{<\/span>0<\/span>}<\/span>, <\/span>\\<\/span>ldots, u<\/span>_{<\/span>N<\/span>-<\/span>1<\/span>}<\/span> <\/span>\\<\/span><\/span>\nx<\/span>_{<\/span>0<\/span>}<\/span>, <\/span>\\<\/span>ldots, x<\/span>_{<\/span>N<\/span>}<\/span> <\/span>\\<\/span><\/span>\nN<\/span>-<\/span>1<\/span>}}<\/span> J=<\/span>\\<\/span>frac<\/span>{<\/span>1<\/span>}{<\/span>2<\/span>}<\/span> x<\/span>_{<\/span>N<\/span>}^{<\/span>T<\/span>}<\/span> Q<\/span>_{<\/span>N<\/span>}<\/span> x<\/span>_{<\/span>N<\/span>}+<\/span>x<\/span>_{<\/span>N<\/span>}^{<\/span>T<\/span>}<\/span> p<\/span>_{<\/span>N<\/span>}+<\/span>q<\/span>_{<\/span>N<\/span>}<\/span> <\/span>\\<\/span><\/span>\n+\\<\/span>sum<\/span>_{<\/span>k=<\/span>0<\/span>}^{<\/span>N<\/span>-<\/span>1<\/span>}\\left(\\<\/span>frac<\/span>{<\/span>1<\/span>}{<\/span>2<\/span>}<\/span> x<\/span>_{<\/span>k<\/span>}^{<\/span>T<\/span>}<\/span> Q x<\/span>_{<\/span>k<\/span>}+<\/span>x<\/span>_{<\/span>k<\/span>}^{<\/span>T<\/span>}<\/span> p<\/span>+\\<\/span>frac<\/span>{<\/span>1<\/span>}{<\/span>2<\/span>}<\/span> u<\/span>_{<\/span>k<\/span>}^{<\/span>T<\/span>}<\/span> R u<\/span>_{<\/span>k<\/span>}+<\/span>u<\/span>_{<\/span>k<\/span>}^{<\/span>T<\/span>}<\/span> r<\/span>+<\/span>q<\/span>\\right)<\/span> <\/span>\\<\/span><\/span>\n\\<\/span>text <\/span>{<\/span> s.t. <\/span>}<\/span> <\/span>\\<\/span>quad x<\/span>_{<\/span>k<\/span>+<\/span>1<\/span>}<\/span>=f<\/span>\\left(<\/span>x<\/span>_{<\/span>k<\/span>}<\/span>, u<\/span>_{<\/span>k<\/span>}\\right)<\/span>, <\/span>\\<\/span>quad k=<\/span>0<\/span>,<\/span>1<\/span>, <\/span>\\<\/span>ldots, N<\/span>-<\/span>1<\/span> <\/span>\\<\/span><\/span>\nx<\/span>_{<\/span>0<\/span>}<\/span>=x<\/span>^{\\<\/span>text <\/span>{<\/span>start <\/span>}}<\/span><\/span>\n\\<\/span>end<\/span>{<\/span>array<\/span>}<\/span>$$<\/span><\/span><\/code><\/pre><\/div>\n\n\n\n
\u73b0\u5728\u8fd9\u4e2a\u6700\u4f18\u95ee\u9898\u4e2d\u7684\u7b49\u5f0f\u7ea6\u675f\uff0c\u4e5f\u5c31\u662f\u6211\u4eec\u7684\u7cfb\u7edf\u65b9\u7a0b\u53d8\u4e3a\u4e86\u975e\u7ebf\u6027\u65b9\u7a0b\uff0c\u8981\u5bf9\u8fd9\u6837\u7684lqr\u6c42\u89e3\u5c31\u5fc5\u987b\u5148\u5bf9\u8fd9\u4e2a\u7cfb\u7edf\u65b9\u7a0b\u8fdb\u884c\u7ebf\u6027\u5316\uff1a<\/p>\n\n\n\n
$$ \\delta x_{t+1} = A_t\\delta x_t + B_t\\delta u_t $$<\/p>\n\n\n\n
\u4e00\u65e6\u5c3d\u5fc3\u7ebf\u6027\u5316\u52bf\u5fc5\u4f1a\u5f15\u5165\u7ebf\u6027\u5316\u7684\u8bef\u5dee\uff0c\u5f53\u72b6\u6001\u504f\u79bb\u4e00\u9636\u6cf0\u52d2\u5c55\u5f00\u7684\u5c55\u5f00\u70b9\u8d8a\u8fdc\u65f6\u8bef\u5dee\u5c31\u4f1a\u8d8a\u5927\uff0c\u800c\u5982\u679c\u53ea\u4f7f\u7528\u7ebf\u6027\u5316\u7684\u6a21\u578b+LQR\u6c42\u89e3\uff0c\u5982\u679c\u521d\u89e3\u91cc\u6700\u4f18\u89e3\u8f83\u8fdc\u5f97\u5230\u7684\u6548\u679c\u5c31\u4f1a\u5f88\u5dee\u3002\u4e0d\u96be\u53d1\u73b0\u7ecf\u8fc7\u4e00\u9636\u6cf0\u52d2\u5c55\u5f00\u540e\uff0c\u65e0\u8bba\u662f\u72b6\u6001\u91cf\u8fd8\u662f\u8f93\u5165\u91cf\u90fd\u53d8\u6210\u4e86\u76f8\u5bf9\u4e8e\u5c55\u5f00\u70b9\u7684delta\u91cf\uff0c\u8fd9\u4e5f\u662filqr\u7684\u6574\u4f53\u6c42\u89e3\u601d\u8def\uff0c\u6bcf\u6b21\u8fed\u4ee3\u90fd\u5bf9\u4e0a\u4e00\u6b21\u7684\u89e3\u8fdb\u884cdelta\u91cf\u7684\u201c\u6270\u52a8\u201d\u4f7f\u5176\u9010\u6e10\u6536\u655b\u81f3\u6700\u4f18\u503c\u3002<\/p>\n\n\n\n
\u73b0\u5728\u6211\u4eec\u5c06\u539f\u672c\u975e\u7ebf\u6027\u7684\u7cfb\u7edf\u8f6c\u6362\u4e3a\u4e86\u4e00\u4e2a\u6807\u51c6\u7684LQR\u95ee\u9898\uff1a<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
\u901a\u8fc7\u6c42\u89e3\u8fd9\u4e2a\u6807\u51c6LQR\u95ee\u9898\u53ef\u4ee5\u6c42\u5f97\u6bcf\u6b21\u201c\u6270\u52a8\u201d\u7684delta u\u5e76\u901a\u8fc7\u6bcf\u4e00\u6b65\u8fdb\u884cbackward\u66f4\u65b0\u7cfb\u7edf\u72b6\u6001\u3002<\/p>\n\n\n\n
\u5e26\u6709\u7ebf\u6027\u9879\u7684LQR\u95ee\u9898<\/h2>\n\n\n\n
\u5728\u8fd9\u91cc\u8fd8\u6709\u4e00\u4e2a\u5c0f\u95ee\u9898\uff0c\u660e\u663e\u8fd9\u4e2aLQR\u95ee\u9898\u76f8\u6bd4\u4e8e\u7b2c\u4e00\u5c0f\u8282\u63d0\u5230\u7684LQR\u95ee\u9898\u591a\u4e86\u4e00\u4e2a\u4e00\u6b21\u9879\uff08\u7ebf\u6027\u9879\uff09\uff0c\u8fd9\u8be5\u5982\u4f55\u6c42\u89e3\uff1f\u5176\u5b9e\u603b\u4f53\u7684\u601d\u8def\u662f\u4e0d\u53d8\u7684\uff0c\u800c\u6211\u4eec\u6700\u7ec8\u7684\u8f93\u51fa\u5f62\u5f0f\u4e3a\\(u^* = -Kx + w\\)\uff0c\u5176\u4e2d-Kx\u4e3a\u53cd\u9988\u9879\uff0c\u8fd9\u4e00\u9879\u4e0e\u4e4b\u524d\u7684\u6c42\u89e3\u65e0\u8bef\u800cw\u4e3a\u524d\u9988\u9879\u3002\u6c42\u89e3\u8fc7\u7a0b\u548c\u4e4b\u524d\u4e00\u81f4\uff0c\u5728\u8fd9\u91cc\u6211\u5927\u81f4\u7ed9\u51fa\u6c42\u89e3\u7684\u8fc7\u7a0b\uff1a<\/p>\n\n\n\n
\u4ee3\u4ef7\u51fd\u6570\u4e3a\uff1a<\/p>\n\n\n\n
$$J = \\sum_{k=0}^{N-1} \\left( x_k^\\top Q x_k + u_k^\\top R u_k + c_k^\\top x_k + d_k^\\top u_k \\right) + x_N^\\top Q_f x_N + c_N^\\top x_N $$<\/p>\n\n\n\n
\u5047\u8bbe \\(\\)( V_{k+1}(x_{k+1}) [\\latex]\u5177\u6709\u4e8c\u6b21\u5f62\u5f0f\uff1a<\/p>\n\n\n\n
$$ V_{k+1}(x_{k+1}) = x_{k+1}^\\top P_{k+1} x_{k+1} + s_{k+1}^\\top x_{k+1} + q_{k+1} $$<\/p>\n\n\n\n
\u5176\u4e2d\uff1a<\/p>\n\n\n\n
\\(\\)P_{k+1} \\(\\) \u662fRiccati\u77e9\u9635\uff08\u5904\u7406\u4e8c\u6b21\u9879\uff09\uff0c<\/p>\n\n\n\n
\\(\\) s_{k+1} \\(\\) \u662f\u7ebf\u6027\u9879\u7684\u7cfb\u6570\u5411\u91cf\uff08\u5904\u7406\u4e00\u6b21\u9879\uff09\uff0c<\/p>\n\n\n\n
\\(\\)q_{k+1} \\(\\)\u662f\u5e38\u6570\u9879\uff08\u6700\u7ec8\u4e0d\u5f71\u54cd\u63a7\u5236\u5f8b\uff09\u3002<\/p>\n\n\n\n
\u5c06 \\(\\) V_{k+1}(x_{k+1}) \\(\\)\u4ee3\u5165\u8d1d\u5c14\u66fc\u65b9\u7a0b\uff1a<\/p>\n\n\n\n
$$V_k(x_k) = \\min_{u_k} \\Big[ x_k^\\top \\left( Q + A^\\top P_{k+1} A \\right) x_k + u_k^\\top \\left( R + B^\\top P_{k+1} B \\right) u_k \\\\+ u_k^\\top \\left( 2 B^\\top P_{k+1} A x_k + d_k + B^\\top s_{k+1} \\right) \\\\+ x_k^\\top \\left( c_k + A^\\top s_{k+1} \\right) + \\text{\u5e38\u6570\u9879} \\Big]$$<\/p>\n\n\n\n
\u4f7fV\u5bf9\u6700\u4f18\u89e3\u6c42\u5bfc\u5f970\u4e3a\uff1a<\/p>\n\n\n\n
$$ \\frac{\\partial V_k}{\\partial u_k} = 2 \\left( R + B^\\top P_{k+1} B \\right) u_k + 2 B^\\top P_{k+1} A x_k + d_k + B^\\top s_{k+1} = 0$$<\/p>\n\n\n\n
$$ u_k^* = - \\underbrace{\\left( R + B^\\top P_{k+1} B \\right)^{-1} B^\\top P_{k+1} A}_{K_k} x_k - \\frac{1}{2} \\underbrace{\\left( R + B^\\top P_{k+1} B \\right)^{-1} \\left( d_k + B^\\top s_{k+1} \\right)}_{w_k}$$<\/p>\n\n\n\n
CILQR\u63a8\u5bfc<\/h1>\n\n\n\n
\u73b0\u5728\u5bf9\u4e8e\u6211\u4eec\u53ea\u5269\u4e0b\u6700\u540e\u4e00\u4e2a\u95ee\u9898\uff0c\u8fd9\u4e2a\u6c42\u89e3\u65b9\u6cd5\u8fd8\u662f\u5904\u7406\u4e0d\u4e86\u7ea6\u675f\uff0c\u8fd9\u8ba9\u4eba\u5f88\u8111\u74dc\u5b50\u75bc\u56e0\u4e3a\u5728\u81ea\u52a8\u9a7e\u9a76\u4e2d\u6240\u9047\u5230\u7684\u7ea6\u675f\u5b9e\u5728\u592a\u591a\u4e86\uff08\u969c\u788d\u7269\u3001\u8f66\u8f86\u7269\u7406\u7ea6\u675f\u3001\u4ea4\u901a\u6cd5\u89c4\u7ea6\u675f\uff09\uff0c\u8fd9\u4e5f\u5c31\u5f15\u5165\u4e86CILQR\u7b97\u6cd5\uff0c\u9996\u5148\u5bf9\u95ee\u9898\u8fdb\u884c\u5efa\u6a21\uff1a<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
LaTeX<\/span><\/path><\/path><\/svg><\/span>
\\<\/span>begin<\/span>{<\/span>align*<\/span>}<\/span><\/span>\n\\<\/span>min<\/span>_{<\/span>u<\/span>_<\/span>0<\/span>, ..., u<\/span>_{<\/span>N<\/span>-<\/span>1<\/span>}}<\/span> <\/span>\\<\/span>quad <\/span>&<\/span> J = c<\/span>_<\/span>N<\/span>(<\/span>x<\/span>_<\/span>N<\/span>)<\/span> <\/span>+<\/span> <\/span>\\<\/span>sum<\/span>_{<\/span>k=<\/span>0<\/span>}^{<\/span>N<\/span>-<\/span>1<\/span>}<\/span> <\/span>\\left(<\/span> c<\/span>_<\/span>k<\/span>^<\/span>u<\/span>(<\/span>x<\/span>_<\/span>k<\/span>)<\/span> <\/span>+<\/span> C<\/span>_<\/span>k<\/span>^<\/span>u <\/span>(<\/span>u<\/span>_<\/span>k<\/span>)<\/span> <\/span>\\right)<\/span> <\/span>\\\\<\/span><\/span>\n\\<\/span>text<\/span>{<\/span>s.t.<\/span>}<\/span> <\/span>\\<\/span>quad <\/span>&<\/span> x<\/span>_{<\/span>k<\/span>+<\/span>1<\/span>}<\/span> = f<\/span>(<\/span>x<\/span>_<\/span>k, u<\/span>_<\/span>k<\/span>)<\/span>, <\/span>\\<\/span>quad k = <\/span>0<\/span>, <\/span>1<\/span>, ..., N<\/span>-<\/span>1<\/span> <\/span>\\\\<\/span><\/span>\n&<\/span> x<\/span>_<\/span>0<\/span> = x<\/span>_{\\<\/span>text<\/span>{<\/span>start<\/span>}}<\/span> <\/span>\\\\<\/span><\/span>\n&<\/span> f<\/span>_<\/span>k<\/span>^<\/span>x<\/span>(<\/span>x<\/span>_<\/span>k<\/span>)<\/span> <\/span>\\<\/span>leq <\/span>0<\/span>, <\/span>\\<\/span>quad k = <\/span>0<\/span>, <\/span>1<\/span>, ..., N <\/span>\\\\<\/span><\/span>\n&<\/span> f<\/span>_<\/span>k<\/span>^<\/span>u<\/span>(<\/span>u<\/span>_<\/span>k<\/span>)<\/span> <\/span>\\<\/span>leq <\/span>0<\/span>, <\/span>\\<\/span>quad k = <\/span>0<\/span>, <\/span>1<\/span>, ..., N<\/span>-<\/span>1<\/span><\/span>\n\\<\/span>end<\/span>{<\/span>align*<\/span>}<\/span><\/span>\n<\/span><\/code><\/pre><\/div>\n\n\n\n
\u8fd9\u662f\u4e00\u4e2a\u901a\u7528\u7684planning\u95ee\u9898\uff0c\u5176\u4e2d\u5f15\u5165\u4e86\u975e\u7ebf\u6027\u7684cost\u3001constrain\u4ee5\u53ca\u7cfb\u7edf\u6a21\u578b\uff08\u7b49\u5f0f\u7ea6\u675f\uff09\uff0c\u597d\u7684\u6709\u4e86ilqr\u7684\u6c42\u89e3\u601d\u8def\u4ee5\u540e\u8fd9\u4e9b\u90fd\u4e0d\u518d\u662f\u56f0\u6270\u6211\u4eec\u7684\u95ee\u9898\uff0c\u7531\u4e8e\u591a\u6b21\u8fed\u4ee3\u53ef\u4ee5\u4f7f\u7cfb\u7edf\u9010\u6e10\u8d8b\u8fd1\u4e8e\u6700\u4f18\u89e3\uff0c\u90a3\u4e48\u6211\u4eec\u4e5f\u5c31\u4e0d\u5728\u4e4e\u7ebf\u6027\u5316\u5bf9\u8fd9\u4e9b\u8ba8\u538c\u7684\u975e\u7ebf\u6027\u95ee\u9898\u7684\u5f71\u54cd\u4e86\uff0c\u5c06\u975e\u7ebf\u6027\u7684cost\u3001constrain\u7ebf\u6027\u5316\uff0c\u5176\u4e2d\u5bf9\u7cfb\u7edf\u65b9\u7a0b\u7684\u7ebf\u6027\u5316\u540c\u4e0a\u8fd9\u91cc\u4e0d\u518d\u8d58\u8ff0\u3002<\/p>\n\n\n\n
\u5c06\u975e\u7ebf\u6027cost\u5728\\(\\)x = x_k\\(\\)\u5904\u8fdb\u884c\u4e8c\u9636\u6cf0\u52d2\u5c55\u5f00\uff1a<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
LaTeX<\/span><\/path><\/path><\/svg><\/span>
{c}_{k}^{x}<\/span>\\<\/span>left<\/span>( {{x}_{k} + <\/span>\\<\/span>delta<\/span> {x}_{k}}<\/span>\\<\/span>right<\/span>)  <\/span>\\<\/span>approx<\/span>  {<\/span>\\<\/span>left<\/span>( <\/span>\\<\/span>delta<\/span> {x}_{k}<\/span>\\<\/span>right<\/span>) }^{T}{<\/span>\\<\/span>nabla<\/span> }^{2}{c}_{k}^{x}<\/span>\\<\/span>left<\/span>( {x}_{k}<\/span>\\<\/span>right<\/span>) <\/span>\\<\/span>delta<\/span> {x}_{k} + {<\/span>\\<\/span>left<\/span>( <\/span>\\<\/span>delta<\/span> {x}_{k}<\/span>\\<\/span>right<\/span>) }^{T}<\/span>\\<\/span>nabla<\/span> {c}_{k}^{x}<\/span>\\<\/span>left<\/span>( {x}_{k}<\/span>\\<\/span>right<\/span>)  + {c}_{k}^{x}<\/span>\\<\/span>left<\/span>( {x}_{k}<\/span>\\<\/span>right<\/span>)<\/span><\/span><\/code><\/pre><\/div>\n\n\n\n
\u5bf9\u4e0econstrain\u6211\u4eec\u9700\u8981\u5148\u5f15\u5165\u7f5a\u51fd\u6570(\u969c\u788d\u51fd\u6570\uff09\u7684\u6982\u5ff5\uff0c\u5176\u6838\u5fc3\u601d\u8def\u5728\u4e8e\u901a\u8fc7\u6781\u5927\u7684cost\u60e9\u7f5a\u6765\u4ee3\u66ff\u4e0d\u7b49\u5f0f\u7ea6\u675f<\/p>\n\n\n\n
$$b_k^x = q_1\\exp (q_2f_k^x)$$<\/p>\n\n\n\n
\u5c06constrain\u7684\u975e\u7ebf\u6027\u51fd\u6570\u5e26\u5165\u7f5a\u51fd\u6570\u4e4b\u540e\u5c06\u5176\u4e5f\u8fdb\u884c\u7ebf\u6027\u5316\uff1a<\/p>\n\n\n\n
$$b_k^x \\left( x_k + \\delta x_k \\right) \\approx \\delta x_k^T \\nabla^2 b_k^x \\left( x_k \\right) \\delta x_k + \\delta x_k^T \\nabla b_k^x \\left( x_k \\right) + b_k^x \\left( x_k \\right)$$<\/p>\n\n\n\n
\u8fd9\u6837\u6211\u4eec\u5728\u8bbe\u8ba1cost\u4ee5\u53caconstrain\u65f6\u53ea\u9700\u8981\u7ed9\u51fa\u5176\u55e8\u68ee\u548c\u96c5\u514b\u6bd4\u77e9\u9635\u5373\u53ef\u3002<\/p>\n\n\n\n
<\/p>\n","protected":false},"excerpt":{"rendered":"
\u6700\u8fd1\u5de5\u7a0b\u4e0a\u8c03\u8bd5lqr\u76f8\u5173\u7684\u7b97\u6cd5\u6709\u4e9b\u5fc3\u70e6\uff0c\u60f3\u518d\u4ed4\u7ec6\u63a8\u5bfc\u4e00\u904d\u9759\u9759\u5fc3\uff0c\u5728\u7f51\u4e0a\u641c\u7d22\u4e86\u4e00\u5708\u6559\u7a0b\u603b\u89c9\u5f97\u5dee\u70b9\u610f\u601d\uff0c\u51b3\u5b9a\u81ea\u5df1\u8be6\u7ec6\u7684\u5199\u4e00\u904d\u4ecelqr\u7684 …<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":true,"template":"","format":"standard","meta":{"emotion":"","emotion_color":"","title_style":"","license":"","footnotes":""},"categories":[19,2,3],"tags":[33,38,35,34,44,36,37],"class_list":{"0":"post-429","1":"post","2":"type-post","3":"status-publish","4":"format-standard","5":"hentry","6":"category-19","7":"category-control","8":"category-planning","9":"tag-cilqr","11":"tag-ilqr","12":"tag-lqr","13":"tag-44","14":"tag-36","15":"tag-37"},"_links":{"self":[{"href":"https:\/\/jamilblog.top\/index.php?rest_route=\/wp\/v2\/posts\/429","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/jamilblog.top\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/jamilblog.top\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/jamilblog.top\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/jamilblog.top\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=429"}],"version-history":[{"count":78,"href":"https:\/\/jamilblog.top\/index.php?rest_route=\/wp\/v2\/posts\/429\/revisions"}],"predecessor-version":[{"id":581,"href":"https:\/\/jamilblog.top\/index.php?rest_route=\/wp\/v2\/posts\/429\/revisions\/581"}],"wp:attachment":[{"href":"https:\/\/jamilblog.top\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=429"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/jamilblog.top\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=429"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/jamilblog.top\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=429"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}