【控制】二階 UGV 的無窮時間狀态輸入性能最優解算

結合生活中常見實際情況可知，無人車的運作空間一般為二維平面。是以，假設無人車的動力學模型如下：

p ˙ i = v i v ˙ i = u i \begin{aligned} \dot{p}_i = v_i \\ \dot{v}_i = u_i \\ \end{aligned} p˙​i​=vi​v˙i​=ui​​

這裡， p i ∈ r 2 p_i\in\mathbb{r}^2 pi​∈r2 表示ugv的位置狀态， v i ∈ r 2 v_i\in\mathbb{r}^2 vi​∈r2 表示ugv的速度狀态， u i ∈ r 2 u_i\in\mathbb{r}^2 ui​∈r2 表示ugv的控制輸入。

也可寫成狀态空間方程的形式：

x ˙ i = a x i + b u i \dot{x}_i = a x_i + b u_i x˙i​=axi​+bui​

其中， x i = [ p i , v i ] t x_i = [p_i, v_i]^t xi​=[pi​,vi​]t， a = [ 0 1 0 0 ] a=\left[\begin{matrix} 0 & 1 \\ 0 & 0 \\ \end{matrix}\right] a=[00​10​]， b = [ 0 1 ] b=\left[\begin{matrix} 0 \\ 1 \\ \end{matrix}\right] b=[01​]。

性能名額為：

j = t f + 1 2 ∫ 0 t f u 2 ( t ) d t j = t_f + \frac{1}{2} \int_{0}^{t_f} u^2(t) dt j=tf​+21​∫0tf​​u2(t)dt

初始狀态（橫截條件）為：

p ( 0 ) = 10 p ( t f ) = 0 v ( 0 ) = 1 v ( t f ) = 0 \begin{aligned} &p(0) = 10 &p(t_f) = 0 \\ &v(0) = 1 &v(t_f) = 0 \\ \end{aligned} ​p(0)=10v(0)=1​p(tf​)=0v(tf​)=0​

建構系統hamilton函數

h = 1 2 u 2 ( t ) + λ 1 v + λ 2 u ( t ) h = \frac{1}{2} u^2(t) + \lambda_1 v + \lambda_2 u(t) h=21​u2(t)+λ1​v+λ2​u(t)

正則方程

p ˙ ( t ) = ∂ h ∂ λ 1 = v v ˙ ( t ) = ∂ h ∂ λ 2 = u λ ˙ 1 = − ∂ h ∂ p = 0 ⇒ λ 1 = a λ ˙ 2 = − ∂ h ∂ v = − λ 1 ⇒ λ 2 = − a t + b \begin{aligned} &\dot{p}(t) = \frac{\partial h}{\partial \lambda_1} = v \\ &\dot{v}(t) = \frac{\partial h}{\partial \lambda_2} = u \\ &\dot{\lambda}_1 = -\frac{\partial h}{\partial p} = 0 \rightarrow \lambda_1 = a\\ &\dot{\lambda}_2 = -\frac{\partial h}{\partial v} = -\lambda_1 \rightarrow \lambda_2 = -a t + b \\ \end{aligned} ​p˙​(t)=∂λ1​∂h​=vv˙(t)=∂λ2​∂h​=uλ˙1​=−∂p∂h​=0⇒λ1​=aλ˙2​=−∂v∂h​=−λ1​⇒λ2​=−at+b​

因為 u u u 是無限制的，是以有極值條件

∂ h ∂ u = u + λ 2 = 0 \frac{\partial h}{\partial u} = u + \lambda_2 = 0 ∂u∂h​=u+λ2​=0

得出來 u u u 的表達式

u = − λ 2 = a t − b u = - \lambda_2 = at - b u=−λ2​=at−b

由 u u u 的表達式做積分可得

v = 1 2 a t 2 − b t + c p = 1 6 a t 3 − 1 2 b t 2 + c t + d \begin{aligned} v &= \frac{1}{2} a t^2 - b t + c \\ p &= \frac{1}{6} a t^3 - \frac{1}{2} b t^2 + c t + d \\ \end{aligned} vp​=21​at2−bt+c=61​at3−21​bt2+ct+d​

結合橫截條件中的初态條件 p ( 0 ) = 10 , v ( 0 ) = 1 p(0) = 10, v(0) = 1 p(0)=10,v(0)=1 可得

c = 1 d = 10 \begin{aligned} c &= 1 \\ d &= 10 \\ \end{aligned} cd​=1=10​

由hamilton函數在最優軌線末端應滿足的條件 h ( t f ) = − ∂ φ ∂ t f h(t_f) = -\frac{\partial \varphi}{\partial t_f} h(tf​)=−∂tf​∂φ​ 有

h ( t f ) = 1 2 u 2 ( t ) + λ 1 v + λ 2 u ( t ) = 1 2 ( a t f − b ) 2 + a ( 1 2 a t f 2 − b t f + c ) + ( − a t f + b ) ( a t f − b ) = − ∂ φ ∂ t f = − 1 \begin{aligned} h(t_f) &= \frac{1}{2} u^2(t) + \lambda_1 v + \lambda_2 u(t) \\ &= \frac{1}{2} (at_f-b)^2 + a(\frac{1}{2}a t_f^2 - b t_f + c) + (-at_f + b)(at_f-b) \\ &= -\frac{\partial \varphi}{\partial t_f} \\ &= -1 \\ \end{aligned} h(tf​)​=21​u2(t)+λ1​v+λ2​u(t)=21​(atf​−b)2+a(21​atf2​−btf​+c)+(−atf​+b)(atf​−b)=−∂tf​∂φ​=−1​

再結合橫街條件中的末态條件 p ( t f ) = 0 , v ( t f ) = 0 p(t_f) = 0, v(t_f) = 0 p(tf​)=0,v(tf​)=0，現在已經三個方程三個未知量，解方程即可

{ 1 2 ( a t f − b ) 2 + a ( 1 2 a t f 2 − b t f + c ) + ( − a t f + b ) ( a t f − b ) = − 1 1 2 a t f 2 − b t f + c = 0 1 6 a t f 3 − 1 2 b t f 2 + c t f + d = 0 \left\{\begin{aligned} &\frac{1}{2} (at_f-b)^2 + a(\frac{1}{2}a t_f^2 - b t_f + c) + (-at_f + b)(at_f-b) = -1 \\ &\frac{1}{2} a t_f^2 - b t_f + c = 0 \\ &\frac{1}{6} a t_f^3 - \frac{1}{2} b t_f^2 + c t_f + d = 0 \\ \end{aligned}\right. ⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧​​21​(atf​−b)2+a(21​atf2​−btf​+c)+(−atf​+b)(atf​−b)=−121​atf2​−btf​+c=061​atf3​−21​btf2​+ctf​+d=0​

利用matlab解算方程組

解的最後結果為

{ a = 0.4276 b = 1.6897 t f = 7.2589 \left\{\begin{aligned} &a = 0.4276 \\ &b = 1.6897 \\ &t_f = 7.2589 \\ \end{aligned}\right. ⎩⎪⎨⎪⎧​​a=0.4276b=1.6897tf​=7.2589​

是以，代入解算出來的參數，那麼有最優

{ t f ∗ = 7.2589 u ∗ ( t ) = 0.4276 t − 1.6897 v ∗ ( t ) = 0.2138 t 2 − 1.6897 t + 1 p ∗ ( t ) = 0.0713 t 3 − 0.8448 t 2 + t + 10 j ∗ ( t ) = t f ∗ + 1 2 ∫ 0 t f ∗ u ∗ 2 ( t ) d t \left\{\begin{aligned} &t_f^* = 7.2589 \\ &u^*(t) = 0.4276 t - 1.6897 \\ &v^*(t) = 0.2138 t^2 - 1.6897 t + 1 \\ &p^*(t) = 0.0713 t^3 - 0.8448 t^2 + t + 10 \\ &j^*(t) = t_f^* + \frac{1}{2} \int_{0}^{t_f^*} u^{*2}(t) dt \\ \end{aligned}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​​tf∗​=7.2589u∗(t)=0.4276t−1.6897v∗(t)=0.2138t2−1.6897t+1p∗(t)=0.0713t3−0.8448t2+t+10j∗(t)=tf∗​+21​∫0tf∗​​u∗2(t)dt​