本發(fā)明涉及水下自主航行器(AutonomousUnderwaterVehicle，AUV)、水面無人艇(UnmannedSurfaceVehicle，USV)等欠驅(qū)動海洋水面或水下航行器的路徑跟蹤控制方法，尤其涉及一種多不確定性情況下的非線性路徑跟蹤控制方法。
背景技術(shù)：
：隨著水面或水下自主航行器在海洋研究和開發(fā)領(lǐng)域的應(yīng)用越來越廣泛，其高精度路徑跟蹤控制已經(jīng)成為當(dāng)前的研究熱點(diǎn)之一。路徑跟蹤是指控制航行器跟蹤一條滿足航向要求和性能約束，且與時間無關(guān)的期望軌跡。由于受到重量、可靠性、復(fù)雜性及效率等多方面的因素影響，目前大部分的海洋航行器屬于欠驅(qū)動系統(tǒng)，加上其本身具有高度的非線性、耦合性以及加速度不可積的非完整約束等特性，給運(yùn)動控制帶來了較大的挑戰(zhàn)。尤其在欠驅(qū)動航行器存在未建模動態(tài)、模型參數(shù)不確定性及風(fēng)浪流等多不確定性干擾時，傳統(tǒng)的控制方法往往不能保證路徑跟蹤控制的精度，有時甚至無法滿足實(shí)際作業(yè)的需要。技術(shù)實(shí)現(xiàn)要素：本發(fā)明的目的在于，在Serret-Frenet坐標(biāo)系下構(gòu)建欠驅(qū)動系統(tǒng)路徑跟蹤誤差模型和基于多不確定性的動力學(xué)模型，設(shè)計一種針對欠驅(qū)動海洋航行器的高精度非線性路徑跟蹤控制方法，用于消除模型參數(shù)不確定性、未建模動態(tài)及外部環(huán)境擾動等對路徑跟蹤的影響，實(shí)現(xiàn)對期望路徑的精確跟蹤控制。首先，本發(fā)明的非線性路徑跟蹤控制方法，將航行器側(cè)滑角的變化率看作一個不確定項(xiàng)dψ，建立欠驅(qū)動海洋航行器水平面內(nèi)包含多不確定性的路徑跟蹤誤差模型和動力學(xué)模型，表示為如下三個子系統(tǒng)：(1)位置子系統(tǒng)(2)姿態(tài)子系統(tǒng)(3)速度子系統(tǒng)其中，s為期望路徑上某點(diǎn)的橫坐標(biāo)；cc為期望路徑上該點(diǎn)處的曲率；(xe,ye,ψe)為航行器在Serret-Frenet坐標(biāo)系下的位置和姿態(tài)誤差；u、v和r分別為航行器質(zhì)心在慣性坐標(biāo)系下的縱向速度、橫向速度及航向角速度；vt為航行器的合成速度，m為航行器質(zhì)量，Iz為航行器繞z軸的轉(zhuǎn)動慣量；Xu,Xu|u|,Yv,Yv|v|,Nr,和Nr|r|為水動力參數(shù)；du,dv和dr為動力學(xué)模型集總不確定性，Δm,ΔIz,ΔXu,ΔXu|u|,ΔYv,ΔYv|v|,ΔNr,ΔNr|r|,為動力學(xué)模型參數(shù)不確定項(xiàng)，Δτu,Δτv和Δτr為外部環(huán)境擾動不確定項(xiàng)，Δi(i＝u,v,r)為其它未建模動態(tài)；τu為外部控制輸入力，τr為外部控制輸入力矩。本發(fā)明提供了一種高精度非線性路徑跟蹤控制方法，實(shí)現(xiàn)步驟如下：步驟一：設(shè)計期望視線角ψlos；步驟二：設(shè)計觀測器估計運(yùn)動學(xué)和動力學(xué)不確定性；對運(yùn)動學(xué)不確定性項(xiàng)dψ和動力學(xué)不確定性項(xiàng)du,dv和dr，通過觀測器進(jìn)行實(shí)時在線觀測，得到觀測值dψ，du,dv和dr；步驟三：設(shè)計基于觀測器的控制器；采用反步法(back-stepping)獲取虛擬向?qū)Э刂坡商摂M航向角速度控制律rd、輸入力控制律τu及輸入力矩控制律τr，并對步驟二觀測的不確定性dψ、du、dv和dr進(jìn)行實(shí)時補(bǔ)償；步驟四：采用跟蹤微分器對控制器進(jìn)行簡化，解決傳統(tǒng)back-stepping控制方法中的“計算膨脹”問題。所述的步驟三中，根據(jù)位置子系統(tǒng)，獲取虛擬向?qū)Э刂坡蔀椋浩渲?，k2＞0，為控制增益；根據(jù)姿態(tài)子系統(tǒng)，獲得虛擬航向角速度控制律rd和輸入力矩控制律τr為：其中，k1,k4＞0為控制增益；根據(jù)速度子系統(tǒng)，獲得輸入力控制律τu為：其中，k3＞0為控制增益，ud為恒定期望速度。所述的步驟四中得到簡化后的控制器可以表示為：其中，rc和為虛擬航向角速度控制律rd通過跟蹤微分器后得到的跟蹤信號和微分信號。本發(fā)明的優(yōu)點(diǎn)和積極效果在于：(1)本發(fā)明的非線性路徑跟蹤控制方法，將運(yùn)動學(xué)跟蹤誤差模型中的未知參數(shù)-航行器側(cè)滑角的變化率當(dāng)作運(yùn)動學(xué)不確定性，將動力學(xué)模型中的內(nèi)部參數(shù)不確定性、外部環(huán)境擾動及未建模動態(tài)等當(dāng)作動力學(xué)集總不確定性，并且采用觀測器對上述運(yùn)動學(xué)和動力學(xué)不確定性進(jìn)行實(shí)時在線觀測，實(shí)現(xiàn)對期望路徑的精確跟蹤控制；(2)本發(fā)明的非線性路徑跟蹤控制方法，采用back-stepping控制方法設(shè)計運(yùn)動學(xué)和動力學(xué)控制器，并且對觀測到的多不確定性實(shí)時補(bǔ)償；通過跟蹤微分器對控制器進(jìn)行簡化，使得控制器更適合移植到實(shí)時運(yùn)算平臺，使得本路徑跟蹤控制方法及實(shí)現(xiàn)的控制器更加適用于工程實(shí)踐；(3)本發(fā)明的非線性路徑跟蹤控制方法，不依賴于控制對象精確的數(shù)學(xué)模型，在多種不確定性的影響下可以保持名義模型同樣的性能，可以擴(kuò)展到海洋航行器在多不確定性下的三維空間路徑跟蹤控制中。附圖說明圖1是海洋航行器水平面內(nèi)的路徑跟蹤示意圖；圖2是本發(fā)明基于觀測器的非線性路徑跟蹤控制方法示意圖；圖3是本發(fā)明的非線性路徑跟蹤控制方法和傳統(tǒng)back-stepping控制器作用下路徑跟蹤對比圖；圖4是本發(fā)明的非線性路徑跟蹤控制方法和傳統(tǒng)back-stepping控制器作用下路徑跟蹤控制誤差對比圖；圖5是本發(fā)明的非線性路徑跟蹤控制方法和傳統(tǒng)back-stepping控制器作用下航行器速度對比圖。具體實(shí)施方式下面將結(jié)合附圖對本發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說明。本發(fā)明提供了一種基于觀測器的高精度非線性路徑跟蹤控制方法，以實(shí)現(xiàn)在多不確定性情況下的欠驅(qū)動海洋航行器高精度非線性路徑跟蹤。如圖1所示，海洋航行器坐標(biāo)系定義及水平面內(nèi)的路徑跟蹤示意圖。{I},{B}和{SF}分別為慣性坐標(biāo)系、載體坐標(biāo)系及Serret-Frenet曲線坐標(biāo)系。其中{I}和{B}坐標(biāo)系遵循標(biāo)準(zhǔn)規(guī)范，{SF}的坐標(biāo)原點(diǎn)為期望路徑上的任意一點(diǎn)，其橫軸XF沿路徑切線方向，縱軸YF為法線方向。下面在上述坐標(biāo)系下對多不確定性下的航行器水平面內(nèi)的路徑跟蹤控制系統(tǒng)進(jìn)行建模和分析。首先，海洋航行器在慣性坐標(biāo)系下的運(yùn)動學(xué)方程可以表示為：其中，x、y和r分別為航行器質(zhì)心Q在慣性坐標(biāo)系下的橫坐標(biāo)和縱坐標(biāo)，ψ為航行器航向角；u、v和r分別為航行器在慣性坐標(biāo)系下的縱向速度、橫向速度及航向角速度。字符上加點(diǎn)表示求導(dǎo)操作。其次，海洋航行器在Serret-Frenet曲線坐標(biāo)系路徑跟蹤誤差模型可以表示為：其中，s為期望路徑上某點(diǎn)P的橫坐標(biāo)，cc(s)為期望路徑上點(diǎn)P處的曲率，簡寫為cc；(xe,ye,ψe)為航行器在Serret-Frenet坐標(biāo)系下的位置和姿態(tài)誤差，xe和ye分別為航行器質(zhì)心Q在Serret-Frenet坐標(biāo)系下的橫坐標(biāo)和縱坐標(biāo)，姿態(tài)誤差ψe＝ψw-ψF，ψw＝ψ+β為航行器的航跡角，ψF為慣性坐標(biāo)系橫軸和Serret-Frenet坐標(biāo)系橫軸之間的夾角；β為航行器的側(cè)滑角，為側(cè)滑角的導(dǎo)數(shù)，也就是側(cè)滑角變化率；vt為航行器的合成速度，可以看出，上述模型中包含未知參數(shù)根據(jù)公式及動力學(xué)模型，取決于航行器縱向速度u、橫向速度v、航向角速度r及模型參數(shù)和不確定性du、dv。一種簡單的方法是通過測量加速度和獲得但是此方法會受到傳感器測量噪聲的影響。因此本發(fā)明將看作一個未知擾動進(jìn)行處理，即設(shè)置不確定性參數(shù)進(jìn)而運(yùn)動學(xué)模型的第三個方程轉(zhuǎn)化為：最后，欠驅(qū)動海洋航行器的包含多不確定性的動力學(xué)模型可以表示為：其中，m為航行器質(zhì)量；Iz為航行器繞z軸的轉(zhuǎn)動慣量，是指在{I}坐標(biāo)系下；τu為外部控制輸入力，調(diào)整海洋航行器的縱向速度；τr為外部控制輸入力矩，調(diào)整海洋航行器的航向角；Xu,Xu|u|，Yv,Yv|v|,Nr,和Nr|r|為水動力參數(shù)；du,dv和dr為動力學(xué)模型的總不確定性，Δm,ΔIz,ΔXu,ΔXu|u|,ΔYv,ΔYv|v|,ΔNr,ΔNr|r|,為動力學(xué)模型參數(shù)不確定項(xiàng)，Δτu,Δτv和Δτr為風(fēng)、浪、流等外部環(huán)境擾動不確定項(xiàng)，Δi(i＝u,v,r)為其它未建模動態(tài)。公式中|.|表示求取絕對值，例如|u|為速度的絕對值。為便于控制器設(shè)計，將上述模型轉(zhuǎn)化為以下三個子系統(tǒng)：(1)位置子系統(tǒng)(2)姿態(tài)子系統(tǒng)(3)速度子系統(tǒng)其中，姿態(tài)子系統(tǒng)中，將未知參數(shù)看作不確定性項(xiàng)dψ來進(jìn)行處理，是對現(xiàn)有系統(tǒng)的改進(jìn)，是本發(fā)明的創(chuàng)新點(diǎn)之一，在下面步驟二中設(shè)計觀測器對dψ進(jìn)行觀測。現(xiàn)有控制方法是通過動力學(xué)模型求解得到因此依賴于系統(tǒng)精確的數(shù)學(xué)模型。本發(fā)明提供的基于觀測器的高精度非線性路徑跟蹤控制方法包括四個步驟：根據(jù)視線角制導(dǎo)率計算視線角ψlos；設(shè)計觀測器估計運(yùn)動學(xué)和動力學(xué)不確定性；基于觀測器的控制器設(shè)計，包含設(shè)計虛擬向?qū)Э刂坡商摂M航向角速度控制律rd、輸入力控制律τu及輸入力矩控制律τr；采用跟蹤微分器對整個控制器進(jìn)行簡化。如圖2所示，對本發(fā)明的自適應(yīng)解耦控制方法的四個步驟進(jìn)行具體說明。步驟一：設(shè)計視線角制導(dǎo)率。為了降低算法的復(fù)雜度，本發(fā)明采用傳統(tǒng)的視線角制導(dǎo)率計算期望視線角，如下：其中，ψlos為期望視線角，Δ為視線距離。步驟二：運(yùn)動學(xué)和動力學(xué)不確定性觀測器設(shè)計。為了實(shí)時估計海洋航行器受到的內(nèi)外部擾動，可以采用擴(kuò)張狀態(tài)觀測器、二階滑模觀測器等多種觀測器，其中降階擴(kuò)展?fàn)顟B(tài)觀測器可以表示為：其中，dψ為運(yùn)動學(xué)不確定性觀測值，dr和du為動力學(xué)不確定性觀測值,ξi(i＝1,2,3,4)為觀測器輔助變量，ωi(i＝1,2,3,4)為觀測器增益。需要指出的是，動力學(xué)不確定項(xiàng)dv隱含在運(yùn)動學(xué)不確定性dψ中，即步驟三：基于觀測器的控制器設(shè)計。(1)根據(jù)位置子系統(tǒng)模型(5)，獲取的虛擬向?qū)Э刂坡煽梢员硎緸椋浩渲?，k2＞0為設(shè)計的控制增益。(2)進(jìn)行姿態(tài)子系統(tǒng)控制律設(shè)計。首先，進(jìn)行運(yùn)動學(xué)控制器設(shè)計。采用航向角速度r作為虛擬指令，為使得ψe＝ψlos，ψe的期望動態(tài)特性可以表示為：其中，是期望視線角的導(dǎo)數(shù)，k1為控制增益。將式(11)代入姿態(tài)子系統(tǒng)模型(6)可得虛擬向?qū)Э刂坡蓃d為：結(jié)合觀測器(9a)，虛擬向?qū)Э刂坡蓃d進(jìn)一步表示為：其次，采用back-stepping方法將上述設(shè)計的運(yùn)動學(xué)控制器反推至動力學(xué)。實(shí)際航向角速度r的期望動態(tài)特性可以表示為：其中，k4為控制增益，將式(14)代入動力學(xué)模型，結(jié)合觀測器(9b)可得輸入力矩控制律τr為：(3)進(jìn)行速度子系統(tǒng)控制律設(shè)計。縱向速度u的期望動態(tài)可以表示為：其中，k3為控制增益，ud為恒定期望速度，因此將式(16)代入動力學(xué)模型，結(jié)合觀測器(9c)可得輸入力控制律τu為：步驟四：采用跟蹤微分器對整個控制器進(jìn)行簡化。可以看出，步驟三設(shè)計的動力學(xué)控制器τr中包括表達(dá)式結(jié)合虛擬指令表達(dá)式(13)，可知動力學(xué)控制器非常復(fù)雜，且包含不確定性觀測值dψ，本發(fā)明采用線性或者非線性跟蹤微分器對運(yùn)動學(xué)虛擬控制指令rd進(jìn)行數(shù)值求導(dǎo)，更適合于實(shí)際工程應(yīng)用。中科院系統(tǒng)所韓京清研究員給出的非線性跟蹤微分器可以表示為：其中，h為采樣周期，q為加速因子，k為離散時間變量，fhan(·)函數(shù)的詳細(xì)表達(dá)式可以參照相關(guān)文獻(xiàn)(韓京清著,《自抗擾控制技術(shù)》[M].國防工業(yè)出版社,2008.p66-73)。本發(fā)明簡化后的控制器表示為：其中，rc和為虛擬參考指令rd通過跟蹤微分器后得到的跟蹤信號和微分信號。下面證明本發(fā)明方法的閉環(huán)系統(tǒng)穩(wěn)定性：定義閉環(huán)跟蹤誤差：E2＝u＝u-ud和E3＝[xe,ye]T。首先，證明誤差E1在本發(fā)明涉及控制器作用下的有界性，考慮如下的Lyapunov函數(shù)：沿方程(6)的軌跡，求公式(20)對時間的導(dǎo)數(shù)，可得:將方程(9a),(9b),(13)和(15)代入上式(21)，可得：其中，ε1和ε2為跟蹤微分器的跟蹤誤差，dψ、dr為觀測器的觀測誤差。進(jìn)而采用楊氏不等式，可得：其中，選擇可以保證矩陣K為正定矩陣。因此，閉環(huán)系統(tǒng)誤差E1可以表示為：其中，λmin(K)為正定矩陣K的最小特征值，λmax(G)為矩陣G的最大特征值。其次，證明閉環(huán)誤差E2在本發(fā)明控制方法作用下的有界性，考慮如下的Lyapunov函數(shù)：沿方程(7)的軌跡，求公式(25)對時間的導(dǎo)數(shù)，可得:設(shè)觀測器觀測誤差收斂，即du有界，因此當(dāng)k3足夠大時，能夠保證且使得閉環(huán)誤差滿足||E2||≤du/k3。最后，證明誤差E3在本發(fā)明控制方法作用下的有界性，考慮如下的Lyapunov函數(shù)：沿方程(5)的軌跡，求公式(27)對時間的導(dǎo)數(shù)，可得:設(shè)ye有界，且滿足|ye|≤y0，y0為一個正常數(shù)，閉環(huán)誤差E3可以表示為：其中，λmin(M)為矩陣M的最小特征值，λmax(N)為矩陣N的最大特征值。因此可以看出，閉環(huán)系統(tǒng)誤差取決于設(shè)計的控制參數(shù)、跟蹤微分器的跟蹤誤差和觀測器的觀測誤差。以上，閉環(huán)系統(tǒng)的穩(wěn)定性證明完畢。為了驗(yàn)證上述提出的控制器的有效性，在MATLAB/Simulink環(huán)境下建立海洋航行器水平面內(nèi)的路徑跟蹤控制系統(tǒng)的仿真模型，并對航行器在受多不確定性情況下的路徑跟蹤控制性能進(jìn)行驗(yàn)證。航行器模型參數(shù)如表1所示：表1航行器水動力參數(shù)初始值設(shè)置為：x(0)＝30m,y(0)＝-20m,u(0)＝0.1m/s,v(0)＝0m/s,r(0)＝0rad/s,ψ(0)＝π/2rad,及s(0)＝0m；期望速度ud＝1m/s；控制器參數(shù)k1＝1,k2＝0.1,k3＝1,k4＝10,ω1＝5,ω2＝20,ω3＝20,ω4＝20,h＝0.001s-1,r＝1000m/s2。假設(shè)期望路徑的參數(shù)化曲線方程可以表示為式(30)：其中，參數(shù)μ可以通過下式得到：期望路徑參數(shù)如表2所示。表2期望路徑參數(shù)路徑參數(shù)i＝0i＝1i＝2i＝3i＝4ai00.87-0.0210-51.5×10-6bi00.5-5×10-410-510-7假設(shè)所有的水動力參數(shù)在名義值的基礎(chǔ)上增加30％，航行器所受的外部環(huán)境擾動表示為：本發(fā)明的基于觀測器的非線性路徑跟蹤控制方法和傳統(tǒng)back-stepping控制方法作用下路徑跟蹤曲線圖如圖3所示。從圖3可以看出，本發(fā)明基于觀測器的非線性路徑跟蹤控制方法和傳統(tǒng)back-stepping控制方法都可以使得航行器漸進(jìn)收斂于期望路徑。但本發(fā)明方法精度更高，且沒有受到多不確定性的影響。兩種控制方法作用下，路徑跟蹤控制誤差曲線如圖4所示。由圖4可知，本發(fā)明方法使得路徑跟蹤誤差漸進(jìn)收斂于零附近，而傳統(tǒng)的back-stepping控制方法則明顯受到多不確定性的影響而性能變差。圖5為本發(fā)明的非線性路徑跟蹤控制方法和傳統(tǒng)back-stepping控制器作用下航行器速度對比曲線?？梢钥闯?，在本發(fā)明控制方法作用下，航行器縱向速度u(t)快速平穩(wěn)收斂于期望速度ud＝1m/s，而在傳統(tǒng)的back-stepping控制方法作用下，u(t)隨著航行器的不確定性在期望速度附近波動。另外，兩種控制方法作用下，航行器橫向速度v及航向角速度r均有界，但本發(fā)明方法更平滑。通過和傳統(tǒng)back-stepping控制器作用下的對比分析可知，在本發(fā)明的基于觀測器的非線性路徑跟蹤控制方法作用下，航行器的路徑跟蹤控制精度明顯提高，抗干擾能力顯著增強(qiáng)。當(dāng)前第1頁1 2 3