(1)
Q(\delta x,\delta u)\approx \frac{1}{2} \begin{bmatrix} 1 \\ \delta x \\ \delta u \end{bmatrix}^T \begin{bmatrix} 0 & Q_x^T & Q_u^T \\ Qx & Qxx & Qxu\\ Qu & Qux & Quu \end{bmatrix} \begin{bmatrix} 1 \\ \delta x \\ \delta u \end{bmatrix} \\ = \frac{1}{2} ( \delta x Q_x + \delta u Q_u + \\ Q_x^T \delta x + \delta x Q_{xx} \delta x + \delta u Q_{ux} \delta x + \\ Q_u^T \delta u + \delta x Q_{xu} \delta u + \delta u Q_{uu} \delta u )
(2)
\delta u^* = -Q_{uu}^{-1} Q_u -Q_{uu}^{-1} Q_{ux} \delta x = k + K \delta x
将 (2)式代入(1)式:
Q(\delta x,\delta u^*)\approx \frac{1}{2} ( \delta x Q_x + k Q_u +K Q_u \delta x + \\ Q_x^T \delta x + \delta x Q_{xx} \delta x + k Q_{ux} \delta x + K Q_{ux} \delta x \delta x + \\ Q_u^T k + Q_u^T K \delta x + \delta x Q_{xu} k + \delta x Q_{xu} K \delta x + \\ k Q_{uu} k + k Q_{uu} K \delta x + K \delta x Q_{uu} k + K \delta x Q_{uu} K \delta x )
分项整理得到:
Q(\delta x,\delta u^*)\approx \frac{1}{2} (k Q_u + Q_u^T k + k Q_{uu} k ) + \\ \frac{1}{2} \delta x (Q_x + K Q_u + Q_x^T + k Q_{ux} + Q_u^T K + Q_{xu} k + k Q_{uu} K + K Q_{uu} k + ) + \\ \frac{1}{2} \delta x \delta x (Q_{xx} + K Q_{ux} + Q_{xu} K + K Q_{uu} K )
第一项为:
第二项为:
\frac{1}{2} \delta x (Q_x + -Q_{uu}^{-1} Q_{ux} Q_u + Q_x^T -Q_{uu}^{-1} Q_u Q_{ux} - Q_u^T Q_{uu}^{-1} Q_{ux} -\\ Q_{xu} Q_{uu}^{-1} Q_u + Q_{uu}^{-1} Q_u Q_{uu} Q_{uu}^{-1} Q_{ux} + Q_{uu}^{-1} Q_{ux} Q_{uu} Q_{uu}^{-1} Q_u )\\ = \frac{1}{2} \delta x(Q_x + Q_x^T - Q_u^T Q_{uu}^{-1} Q_{ux} -Q_{xu} Q_{uu}^{-1} Q_u )
第三项为:
\frac{1}{2} \delta x \delta x (Q_{xx} -Q_{uu}^{-1}Q_{ux}Q_{ux} -Q_{xu}Q_{uu}^{-1}Q_{ux} + Q_{uu}^{-1}Q_{ux}Q_{uu}Q_{uu}^{-1}Q_{ux} ) \\ =\frac{1}{2} \delta x \delta x (Q_{xx}-Q_{xu}Q_{uu}^{-1}Q_{ux} )
链接:
墨之科技,版权所有 © Copyright 2017-2027
湘ICP备14012786号 邮箱:ai@inksci.com